LocalMapping.h

#pragma once
#include "OrbSLAM/Headers/KeyFrame.h"
#include "OrbSLAM/Headers/Atlas.h"
#include "OrbSLAM/Headers/LoopClosing.h"
#include "OrbSLAM/Headers/Tracking.h"
#include "OrbSLAM/Headers/KeyFrameDatabase.h"
 
#include <mutex>
namespace NDEVR
{
 
    class OrbSLAM;
    class Tracking;
    class LoopClosing;
    class Atlas;
    class InfoPipe;

    class LocalMapping
    {
    public:
        LocalMapping(OrbSLAM* pSys, Atlas* pAtlas, bool bMonocular, bool bInertial, LogPtr log);

        void setCameraSettings(bool bMonocular, bool bInertial);
        void SetLoopCloser(LoopClosing* pLoopCloser);

        void SetTracker(Tracking* pTracker);
        fltp04 inertialPercent() const;
        void Run();

        void InsertKeyFrame(KeyFrame* pKF);
        void EmptyQueue();

        void RequestStop();
        void RequestReset();
        void RequestResetActiveMap();
        bool Stop();
        void Release();
        bool isStopped();
        bool stopRequested();
        bool AcceptKeyFrames();
        void SetAcceptKeyFrames(bool flag);
        bool SetNotStop(bool flag);
        Buffer<std::pair<Eigen::Vector3f, Vector<2, uint04>>> FindSharedPoints(KeyFrame* a, KeyFrame* b);
        void InterruptBA();

        void RequestFinish();
        bool isFinished();
 
        uint04 KeyframesInQueue()
        {
            std::unique_lock<std::mutex> lock(mMutexNewKFs);
            return cast<uint04>(mlNewKeyFrames.size());
        }
 
        bool IsInitializing();
        double GetCurrKFTime();
        KeyFrame* currentKeyFrame();
 
        std::mutex mMutexImuInit;
 
        Eigen::MatrixX<g_type> mcovInertial;
        Eigen::Matrix3<g_type> m_gyro_rotation;
        Eigen::Vector3<g_type> m_gyro_bias;
        Eigen::Vector3<g_type> mba;
        fltp04 m_scale = 1.0f;
 
        unsigned int mInitSect;
        unsigned int mIdxInit;
        fltp08 m_initial_timestamp;
 
        // For debugging (erase in normal mode)
 
        bool mbNotBA1 = true;
        bool mbNotBA2 = true;
        bool mbBadImu = false;
 
        // not consider far points (clouds)
        bool mbFarPoints;
        float mThFarPoints;
    protected:
        void processIMUKeyFrame();
        bool CheckNewKeyFrames();
        void processNextKeyFrame();
        void CreateNewMapPoints();
        void handleNewKeyFrame();
        void MapPointCulling();
        void SearchInNeighbors();
        void KeyFrameCulling();
 
        Optimizer m_optimizer;
        OrbSLAM* mpSystem;
        ORBmatcher m_matcher;
        bool mbMonocular = false;
        bool mbInertial = false;
 
        void ResetIfRequested();
        
        bool CheckFinish();
        void SetFinish();
 
    private:
        Buffer<Vector<2, uint04>> m_matched_indices;
        Atlas* mpAtlas = nullptr;
 
        LoopClosing* mpLoopCloser = nullptr;
        Tracking* mpTracker = nullptr;
 
        Buffer<KeyFrame*> mlNewKeyFrames;
 
        KeyFrame* m_current_keyframe = nullptr;
 
        Buffer<MapPoint*> mlpRecentAddedMapPoints;
 
        std::mutex mMutexNewKFs;
 
        
 
        bool mbAcceptKeyFrames = true;
 
        void InitializeIMU(float priorG = 1e2, float priorA = 1e6, bool bFirst = false);
        void ScaleRefinement();
 
        
 
        fltp08 mTinit;
        LogPtr m_log;
 
        bool bInitializing = false;
        bool mbAbortBA = false;
        bool mbStopped = false;
        bool mbStopRequested = false;
        bool mbNotStop = false;
        bool mbResetRequested = false;
        bool mbResetRequestedActiveMap = false;
        bool mbFinishRequested = false;
        bool mbFinished = true;
 
    };
 
}