System.h

#pragma once
#include "DLLInfo.h"
#include <thread>
#include <opencv2/core/core.hpp>
 
#include "OrbSLAM/Headers/Tracking.h"
#include "OrbSLAM/Headers/Atlas.h"
#include "OrbSLAM/Headers/LocalMapping.h"
#include "OrbSLAM/Headers/LoopClosing.h"
#include "OrbSLAM/Headers/KeyFrameDatabase.h"
#include "OrbSLAM/Headers/ORBVocabulary.h"
#include "OrbSLAM/Headers/Viewer.h"
#include "OrbSLAM/Headers/SLAMParameters.h"
#include <NDEVR/IntegratedRotation.h>
namespace NDEVR
{
    class Viewer;
    class FrameDrawer;
    class MapDrawer;
    class Atlas;
    class Tracking;
    class LocalMapping;
    class LoopClosing;
    class Model;
    class Settings;
    class DesignObjectLookup;
    class KeyPointMap;
    class KeyFrameMap;
    class InfoPipe;
    class ORBSLAM_API OrbSLAM
    {
    public:
        const KeyFrame* lastKeyFrame() const;
        bool needsMapReset() const;
        OrbSLAM(const SLAMParameters& params, LogPtr log);
        OrbSLAM(const File& vocab, const SLAMParameters& params, LogPtr log);

        void setParams(const SLAMParameters& params);
        void init(const File& vocab, const SLAMParameters& params);
        Sophus::SE3f TrackStereo(OrbTrackingInfo& info);

        Sophus::SE3f TrackRGBD(OrbTrackingInfo& info);

        Sophus::SE3f TrackMonocular(OrbTrackingInfo& info);

        void updateInfo(const Matrix<fltp08>& global_offset, DesignObjectLookup* lookup, const Model& frame_model, const Matrix<fltp08>& frame_model_transform, bool show_keyframes, bool show_map_points);

        void ActivateLocalizationMode();
        void DeactivateLocalizationMode();

        bool MapChanged();

        void reset();
        void ResetActiveMap();

        void Shutdown();
        bool isShutDown();

        Buffer<KeyFrame*> getAllKeyFrames() const;
        Buffer<Sophus::SE3f> getAllKeyFramesAfter(uint04 index) const;
        Tracking::eTrackingState GetTrackingState();
        uint04 CurrentMapID() const;
        Buffer<MapPoint*> GetTrackedMapPoints();

        double GetTimeFromIMUInit();
        void ChangeDataset();
 
    private:
 
        // Input sensor
        Sensor m_sensor;
 
        // ORB vocabulary used for place recognition and feature matching.
        ORBVocabulary* m_vocabulary = nullptr;
 
        // KeyFrame database for place recognition (relocalization and loop detection).
        KeyFrameDatabase* m_key_frame_database = nullptr;
 
        // Map structure that stores the pointers to all KeyFrames and MapPoints.
        //Map* mpMap;
        Atlas* m_atlas = nullptr;
 
        // Tracker. It receives a frame and computes the associated camera pose.
        // It also decides when to insert a new keyframe, create some new MapPoints and
        // performs relocalization if tracking fails.
        Tracking* m_tracker = nullptr;
 
        // Local Mapper. It manages the local map and performs local bundle adjustment.
        LocalMapping* m_local_mapper = nullptr;
 
        // Loop Closer. It searches loops with every new keyframe. If there is a loop it performs
        // a pose graph optimization and full bundle adjustment (in a new thread) afterwards.
        LoopClosing* m_loop_closer = nullptr;
 
        KeyPointMap* m_orb_point_map = nullptr;
        KeyFrameMap* m_orb_frame_map = nullptr;
        // OrbSLAM threads: Local Mapping, Loop Closing, Viewer.
        // The Tracking thread "lives" in the main execution thread that creates the OrbSLAM object.
        std::thread* mptLocalMapping = nullptr;
        std::thread* mptLoopClosing = nullptr;
 
        // reset flag
        std::mutex mMutexReset;
        bool mbReset = false;
        bool mbResetActiveMap = false;
 
        // Change mode flags
        std::mutex mMutexMode;
        bool mbActivateLocalizationMode = false;
        bool mbDeactivateLocalizationMode = false;
        // Shutdown flag
        bool mbShutDown = false;
        // Tracking state
        Tracking::eTrackingState mTrackingState = Tracking::eTrackingState::NOT_INITIALIZED;
        Buffer<MapPoint*> mTrackedMapPoints;
        std::mutex m_mutex_state;
        LogPtr m_log;
        fltp08 m_time_offset = Constant<fltp08>::Invalid;
        int m_last_big_change_idx = 0;
    };

    static inline Matrix<fltp04> Convert(Eigen::Matrix4f mat)
    {
        Matrix<fltp04> transform;
        for (uint01 i = 0; i < 4; i++)
        {
            for (uint01 x = 0; x < 4; x++)
            {
                transform[i][x] = mat.row(x).col(i).value();// at<fltp04>(x, i);
            }
        }
        return transform;
    }
}