LoopClosing.h

#pragma once
#include "OrbSLAM/Headers/Optimizer.h"
#include "OrbSLAM/Headers/KeyFrame.h"
#include "OrbSLAM/Headers/Atlas.h"
#include "OrbSLAM/Headers/ORBVocabulary.h"
#include "OrbSLAM/Headers/Tracking.h"
#include "OrbSLAM/Headers/KeyFrameDatabase.h"
#include "GraphOptimization/Headers/types_seven_dof_expmap.h"
 
#include <thread>
#include <mutex>
 
namespace NDEVR
{
 
    class Tracking;
    class LocalMapping;
    class KeyFrameDatabase;
    class Map;
    class InfoPipe;

    class LoopClosing
    {
    public:
 
        typedef std::pair<Set<KeyFrame*>, int> ConsistentGroup;         
        typedef std::map<KeyFrame*, Sim3, std::less<KeyFrame*>,
            Eigen::aligned_allocator<std::pair<KeyFrame* const, Sim3>>> KeyFrameAndPose; 
 
    public:

        LoopClosing(Atlas* pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc, const bool bFixScale, const bool bActiveLC, LogPtr log);
 
        void setParams(const bool bFixScale, const bool bActiveLC);
        void SetTracker(Tracking* pTracker);
 
        void SetLocalMapper(LocalMapping* pLocalMapper);
 
        // Main function
        void Run();
 
        void InsertKeyFrame(KeyFrame* pKF);
 
        void RequestReset();
        void RequestResetActiveMap(Map* pMap);
 
        // This function will run in a separate thread
        void RunGlobalBundleAdjustment(Map* pActiveMap, uint04 nLoopKF);
 
        bool isRunningGBA() {
            std::unique_lock<std::mutex> lock(mMutexGBA);
            return mbRunningGBA;
        }
        bool isFinishedGBA() {
            std::unique_lock<std::mutex> lock(mMutexGBA);
            return mbFinishedGBA;
        }
 
        void RequestFinish();
 
        bool isFinished();
 
        Viewer* mpViewer;
 
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
    protected:
 
        bool CheckNewKeyFrames();
 
 
        //Methods to implement the new place recognition algorithm
        bool NewDetectCommonRegions();
        bool DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, Sim3& gScw, uint04& nNumProjMatches,
            Buffer<MapPoint*>& vpMPs, Buffer<MapPoint*>& vpMatchedMPs);
        bool DetectCommonRegionsFromBoW(Buffer<KeyFrame*>& vpBowCand, KeyFrame*& pMatchedKF, KeyFrame*& pLastCurrentKF, Sim3& ParkeScw,
            int& nNumCoincidences, Buffer<MapPoint*>& vpMPs, Buffer<MapPoint*>& vpMatchedMPs);
        uint04 FindMatchesByProjection(KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, Sim3& ParkeScw, Buffer<MapPoint*>& vpMapPoints,
            Buffer<MapPoint*>& vpMatchedMapPoints);
 
 
        void SearchAndFuse(const KeyFrameAndPose& CorrectedPosesMap, Buffer<MapPoint*>& vpMapPoints);
        void SearchAndFuse(const Buffer<KeyFrame*>& vConectedKFs, Buffer<MapPoint*>& vpMapPoints);
 
        void CorrectLoop();
 
        void MergeLocal();
        void MergeLocal2();
 
        void ResetIfRequested();
        Buffer<KeyFrame*> m_covisible_frames;
        ORBmatcher m_orb_matcher;
        ORBmatcher m_bow_matcher;
        bool mbResetRequested = false;
        bool mbResetActiveMapRequested = false;
        Map* mpMapToReset = nullptr;
        std::mutex mMutexReset;
 
        bool CheckFinish();
        void SetFinish();
        bool mbFinishRequested = false;
        bool mbFinished = true;
        std::mutex mMutexFinish;
 
        Atlas* mpAtlas;
        Tracking* mpTracker;
 
        KeyFrameDatabase* mpKeyFrameDB;
        ORBVocabulary* mpORBVocabulary;
 
        LocalMapping* mpLocalMapper;
 
        std::list<KeyFrame*> mlpLoopKeyFrameQueue;
 
        std::mutex mMutexLoopQueue;
 
        // Loop detector parameters
        float mnCovisibilityConsistencyTh = 3.0f;
 
        // Loop detector variables
        KeyFrame* mpCurrentKF = nullptr;
        KeyFrame* mpLastCurrentKF = nullptr;
        KeyFrame* mpMatchedKF = nullptr;
        Buffer<ConsistentGroup> mvConsistentGroups;
        Buffer<KeyFrame*> mvpEnoughConsistentCandidates;
        Buffer<KeyFrame*> mvpCurrentConnectedKFs;
        Buffer<MapPoint*> mvpCurrentMatchedPoints;
        Buffer<MapPoint*> mvpLoopMapPoints;
        cv::Mat mScw;
        Sim3 mParkeScw;
 
        //-------
        Map* mpLastMap = nullptr;
 
        bool mbLoopDetected = false;
        int mnLoopNumCoincidences = 0;
        int mnLoopNumNotFound = 0;
        KeyFrame* mpLoopLastCurrentKF = nullptr;
        Sim3 mParkeLoopSlw;
        Sim3 mParkeLoopScw;
        KeyFrame* mpLoopMatchedKF = nullptr;
        Buffer<MapPoint*> mvpLoopMPs;
        Buffer<MapPoint*> mvpLoopMatchedMPs;
        bool mbMergeDetected = false;
        int mnMergeNumCoincidences = 0;
        int mnMergeNumNotFound = 0;
        KeyFrame* mpMergeLastCurrentKF = nullptr;
        Sim3 mParkeMergeSlw;
        Sim3 mParkeMergeSmw;
        Sim3 mParkeMergeScw;
        KeyFrame* mpMergeMatchedKF;
        Buffer<MapPoint*> mvpMergeMPs;
        Buffer<MapPoint*> mvpMergeMatchedMPs;
        Buffer<KeyFrame*> mvpMergeConnectedKFs;
 
        Sim3 mSold_new;
        // Variables related to Global Bundle Adjustment
        bool mbRunningGBA = false;
        bool mbFinishedGBA = true;
        bool mbStopGBA = false;
        std::mutex mMutexGBA;
        std::thread* mpThreadGBA = nullptr;
 
        // Fix scale in the stereo/RGB-t_dim case
        bool mbFixScale;
 
 
        int mnFullBAIdx = 0;
 
        //DEBUG
        int mnNumCorrection = 0;
        int mnCorrectionGBA = 0;
 
 
        // To (de)activate LC
        bool mbActiveLC = true;
        LogPtr m_log;
    private:
        Optimizer m_optimizer;
    };
 
}