/** * This file is part of ORB-SLAM3 * * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. * * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public * License as published by the Free Software Foundation, either version 3 of the License, or * (at your option) any later version. * * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License along with ORB-SLAM3. * If not, see . */ #ifndef LOOPCLOSING_H #define LOOPCLOSING_H #include "KeyFrame.h" #include "LocalMapping.h" #include "Atlas.h" #include "ORBVocabulary.h" #include "Tracking.h" #include "KeyFrameDatabase.h" #include #include #include #include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h" namespace ORB_SLAM3 { class Tracking; class LocalMapping; class KeyFrameDatabase; class Map; class LoopClosing { public: typedef pair,int> ConsistentGroup; typedef map, Eigen::aligned_allocator > > KeyFrameAndPose; public: LoopClosing(Atlas* pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,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, unsigned long nLoopKF); bool isRunningGBA(){ unique_lock lock(mMutexGBA); return mbRunningGBA; } bool isFinishedGBA(){ unique_lock lock(mMutexGBA); return mbFinishedGBA; } void RequestFinish(); bool isFinished(); Viewer* mpViewer; #ifdef REGISTER_TIMES vector vdDataQuery_ms; vector vdEstSim3_ms; vector vdPRTotal_ms; vector vdMergeMaps_ms; vector vdWeldingBA_ms; vector vdMergeOptEss_ms; vector vdMergeTotal_ms; vector vnMergeKFs; vector vnMergeMPs; int nMerges; vector vdLoopFusion_ms; vector vdLoopOptEss_ms; vector vdLoopTotal_ms; vector vnLoopKFs; int nLoop; vector vdGBA_ms; vector vdUpdateMap_ms; vector vdFGBATotal_ms; vector vnGBAKFs; vector vnGBAMPs; int nFGBA_exec; int nFGBA_abort; #endif EIGEN_MAKE_ALIGNED_OPERATOR_NEW protected: bool CheckNewKeyFrames(); //Methods to implement the new place recognition algorithm bool NewDetectCommonRegions(); bool DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches, std::vector &vpMPs, std::vector &vpMatchedMPs); bool DetectCommonRegionsFromBoW(std::vector &vpBowCand, KeyFrame* &pMatchedKF, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw, int &nNumCoincidences, std::vector &vpMPs, std::vector &vpMatchedMPs); bool DetectCommonRegionsFromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches, std::vector &vpMPs, std::vector &vpMatchedMPs); int FindMatchesByProjection(KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, g2o::Sim3 &g2oScw, set &spMatchedMPinOrigin, vector &vpMapPoints, vector &vpMatchedMapPoints); void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap, vector &vpMapPoints); void SearchAndFuse(const vector &vConectedKFs, vector &vpMapPoints); void CorrectLoop(); void MergeLocal(); void MergeLocal2(); void CheckObservations(set &spKFsMap1, set &spKFsMap2); void ResetIfRequested(); bool mbResetRequested; bool mbResetActiveMapRequested; Map* mpMapToReset; std::mutex mMutexReset; bool CheckFinish(); void SetFinish(); bool mbFinishRequested; bool mbFinished; std::mutex mMutexFinish; Atlas* mpAtlas; Tracking* mpTracker; KeyFrameDatabase* mpKeyFrameDB; ORBVocabulary* mpORBVocabulary; LocalMapping *mpLocalMapper; std::list mlpLoopKeyFrameQueue; std::mutex mMutexLoopQueue; // Loop detector parameters float mnCovisibilityConsistencyTh; // Loop detector variables KeyFrame* mpCurrentKF; KeyFrame* mpLastCurrentKF; KeyFrame* mpMatchedKF; std::vector mvConsistentGroups; std::vector mvpEnoughConsistentCandidates; std::vector mvpCurrentConnectedKFs; std::vector mvpCurrentMatchedPoints; std::vector mvpLoopMapPoints; cv::Mat mScw; g2o::Sim3 mg2oScw; //------- Map* mpLastMap; bool mbLoopDetected; int mnLoopNumCoincidences; int mnLoopNumNotFound; KeyFrame* mpLoopLastCurrentKF; g2o::Sim3 mg2oLoopSlw; g2o::Sim3 mg2oLoopScw; KeyFrame* mpLoopMatchedKF; std::vector mvpLoopMPs; std::vector mvpLoopMatchedMPs; bool mbMergeDetected; int mnMergeNumCoincidences; int mnMergeNumNotFound; KeyFrame* mpMergeLastCurrentKF; g2o::Sim3 mg2oMergeSlw; g2o::Sim3 mg2oMergeSmw; g2o::Sim3 mg2oMergeScw; KeyFrame* mpMergeMatchedKF; std::vector mvpMergeMPs; std::vector mvpMergeMatchedMPs; std::vector mvpMergeConnectedKFs; g2o::Sim3 mSold_new; //------- long unsigned int mLastLoopKFid; // Variables related to Global Bundle Adjustment bool mbRunningGBA; bool mbFinishedGBA; bool mbStopGBA; std::mutex mMutexGBA; std::thread* mpThreadGBA; // Fix scale in the stereo/RGB-D case bool mbFixScale; bool mnFullBAIdx; vector vdPR_CurrentTime; vector vdPR_MatchedTime; vector vnPR_TypeRecogn; //DEBUG string mstrFolderSubTraj; int mnNumCorrection; int mnCorrectionGBA; // To (de)activate LC bool mbActiveLC = true; #ifdef REGISTER_LOOP string mstrFolderLoop; #endif }; } //namespace ORB_SLAM #endif // LOOPCLOSING_H