152 lines
3.5 KiB
C++
152 lines
3.5 KiB
C++
/**
|
|
* This file is part of ORB-SLAM2.
|
|
*
|
|
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
|
|
* For more information see <https://github.com/raulmur/ORB_SLAM2>
|
|
*
|
|
* ORB-SLAM2 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-SLAM2 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-SLAM2. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#ifndef LOOPCLOSING_H
|
|
#define LOOPCLOSING_H
|
|
|
|
#include "KeyFrame.h"
|
|
#include "LocalMapping.h"
|
|
#include "Map.h"
|
|
#include "ORBVocabulary.h"
|
|
#include "Tracking.h"
|
|
|
|
#include "KeyFrameDatabase.h"
|
|
|
|
#include <thread>
|
|
#include <mutex>
|
|
#include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
|
|
|
|
namespace ORB_SLAM2
|
|
{
|
|
|
|
class Tracking;
|
|
class LocalMapping;
|
|
class KeyFrameDatabase;
|
|
|
|
|
|
class LoopClosing
|
|
{
|
|
public:
|
|
|
|
typedef pair<set<KeyFrame*>,int> ConsistentGroup;
|
|
typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
|
|
Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose;
|
|
|
|
public:
|
|
|
|
LoopClosing(Map* pMap, 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();
|
|
|
|
// This function will run in a separate thread
|
|
void RunGlobalBundleAdjustment(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();
|
|
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
|
|
|
protected:
|
|
|
|
bool CheckNewKeyFrames();
|
|
|
|
bool DetectLoop();
|
|
|
|
bool ComputeSim3();
|
|
|
|
void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap);
|
|
|
|
void CorrectLoop();
|
|
|
|
void ResetIfRequested();
|
|
bool mbResetRequested;
|
|
std::mutex mMutexReset;
|
|
|
|
bool CheckFinish();
|
|
void SetFinish();
|
|
bool mbFinishRequested;
|
|
bool mbFinished;
|
|
std::mutex mMutexFinish;
|
|
|
|
Map* mpMap;
|
|
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* 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;
|
|
|
|
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;
|
|
};
|
|
|
|
} //namespace ORB_SLAM
|
|
|
|
#endif // LOOPCLOSING_H
|