/**
* 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