227 lines
6.5 KiB
C++
227 lines
6.5 KiB
C++
/**
|
||
* This file is part of ORB-SLAM3
|
||
*
|
||
* Copyright (C) 2017-2020 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 <http://www.gnu.org/licenses/>.
|
||
*/
|
||
|
||
|
||
#ifndef LOOPCLOSING_H
|
||
#define LOOPCLOSING_H
|
||
|
||
#include "KeyFrame.h"
|
||
#include "LocalMapping.h"
|
||
#include "Atlas.h"
|
||
#include "ORBVocabulary.h"
|
||
#include "Tracking.h"
|
||
#include "Config.h"
|
||
|
||
#include "KeyFrameDatabase.h"
|
||
|
||
#include <boost/algorithm/string.hpp>
|
||
#include <thread>
|
||
#include <mutex>
|
||
#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<set<KeyFrame*>,int> ConsistentGroup;
|
||
typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
|
||
Eigen::aligned_allocator<std::pair<KeyFrame* const, g2o::Sim3> > > KeyFrameAndPose;
|
||
|
||
public:
|
||
|
||
LoopClosing(Atlas* pAtlas, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale);
|
||
|
||
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<std::mutex> lock(mMutexGBA);
|
||
return mbRunningGBA;
|
||
}
|
||
bool isFinishedGBA(){
|
||
unique_lock<std::mutex> lock(mMutexGBA);
|
||
return mbFinishedGBA;
|
||
}
|
||
|
||
void RequestFinish();
|
||
|
||
bool isFinished();
|
||
|
||
Viewer* mpViewer;
|
||
|
||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||
|
||
#ifdef REGISTER_TIMES
|
||
double timeDetectBoW;
|
||
|
||
std::vector<double> vTimeBoW_ms;
|
||
std::vector<double> vTimeSE3_ms;
|
||
std::vector<double> vTimePRTotal_ms;
|
||
|
||
std::vector<double> vTimeLoopFusion_ms;
|
||
std::vector<double> vTimeLoopEssent_ms;
|
||
std::vector<double> vTimeLoopTotal_ms;
|
||
|
||
std::vector<double> vTimeMergeFusion_ms;
|
||
std::vector<double> vTimeMergeBA_ms;
|
||
std::vector<double> vTimeMergeTotal_ms;
|
||
|
||
std::vector<double> vTimeFullGBA_ms;
|
||
std::vector<double> vTimeMapUpdate_ms;
|
||
std::vector<double> vTimeGBATotal_ms;
|
||
#endif
|
||
|
||
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<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs);
|
||
bool DetectCommonRegionsFromBoW(std::vector<KeyFrame*> &vpBowCand, KeyFrame* &pMatchedKF, KeyFrame* &pLastCurrentKF, g2o::Sim3 &g2oScw,
|
||
int &nNumCoincidences, std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs);
|
||
bool DetectCommonRegionsFromLastKF(KeyFrame* pCurrentKF, KeyFrame* pMatchedKF, g2o::Sim3 &gScw, int &nNumProjMatches,
|
||
std::vector<MapPoint*> &vpMPs, std::vector<MapPoint*> &vpMatchedMPs);
|
||
int FindMatchesByProjection(KeyFrame* pCurrentKF, KeyFrame* pMatchedKFw, g2o::Sim3 &g2oScw,
|
||
set<MapPoint*> &spMatchedMPinOrigin, vector<MapPoint*> &vpMapPoints,
|
||
vector<MapPoint*> &vpMatchedMapPoints);
|
||
|
||
|
||
void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap, vector<MapPoint*> &vpMapPoints);
|
||
void SearchAndFuse(const vector<KeyFrame*> &vConectedKFs, vector<MapPoint*> &vpMapPoints);
|
||
|
||
void CorrectLoop();
|
||
|
||
void MergeLocal();
|
||
void MergeLocal2();
|
||
|
||
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<KeyFrame*> mlpLoopKeyFrameQueue;
|
||
|
||
std::mutex mMutexLoopQueue;
|
||
|
||
// Loop detector parameters
|
||
float mnCovisibilityConsistencyTh;
|
||
|
||
// Loop detector variables
|
||
KeyFrame* mpCurrentKF;
|
||
KeyFrame* mpLastCurrentKF;
|
||
KeyFrame* mpMatchedKF;
|
||
std::vector<ConsistentGroup> mvConsistentGroups;
|
||
std::vector<KeyFrame*> mvpEnoughConsistentCandidates;
|
||
std::vector<KeyFrame*> mvpCurrentConnectedKFs;
|
||
std::vector<MapPoint*> mvpCurrentMatchedPoints;
|
||
std::vector<MapPoint*> mvpLoopMapPoints;
|
||
cv::Mat mScw;
|
||
g2o::Sim3 mg2oScw;
|
||
|
||
//-------
|
||
Map* mpLastMap;
|
||
|
||
bool mbLoopDetected;
|
||
int mnLoopNumCoincidences;
|
||
int mnLoopNumNotFound;
|
||
KeyFrame* mpLoopLastCurrentKF;
|
||
g2o::Sim3;
|
||
g2o::Sim3 mg2oLoopScw;
|
||
KeyFrame* mpLoopMatchedKF;
|
||
std::vector<MapPoint*> mvpLoopMPs;
|
||
std::vector<MapPoint*> mvpLoopMatchedMPs;
|
||
bool mbMergeDetected;
|
||
int mnMergeNumCoincidences;
|
||
int mnMergeNumNotFound;
|
||
KeyFrame* mpMergeLastCurrentKF;
|
||
g2o::Sim3 mg2oMergeSlw;
|
||
g2o::Sim3 mg2oMergeSmw;
|
||
g2o::Sim3 mg2oMergeScw;
|
||
KeyFrame* mpMergeMatchedKF;
|
||
std::vector<MapPoint*> mvpMergeMPs;
|
||
std::vector<MapPoint*> mvpMergeMatchedMPs;
|
||
std::vector<KeyFrame*> 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值为什么用mn,疑似bug,后面会用到这个变量自加,但IDE提示bool类型不能自加
|
||
bool mnFullBAIdx;
|
||
|
||
|
||
|
||
vector<double> vdPR_CurrentTime;
|
||
vector<double> vdPR_MatchedTime;
|
||
vector<int> vnPR_TypeRecogn;
|
||
};
|
||
|
||
} //namespace ORB_SLAM
|
||
|
||
#endif // LOOPCLOSING_H
|