目录
1.简述
2.头文件
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:
//判断mlpLoopKeyFrameQueue是否非空
bool CheckNewKeyFrames();
//获取候选闭环关键帧放入mvpEnoughConsistentCandidates
bool DetectLoop();
/**
*
* 1. 候选帧和当前关键帧通过Bow加速描述子的匹配,剔除特征点匹配数少的闭环候选帧
* 2. 利用RANSAC粗略地计算出当前帧与闭环帧的Sim3,选出较好的那个sim3,确定闭环帧
* 2. 根据确定的闭环帧和对应的Sim3,对3D点进行投影找到更多匹配,通过优化的方法计算更精确的Sim3。
* 3. 将闭环帧以及闭环帧相连的关键帧的MapPoints与当前帧的点进行匹配(当前帧---闭环帧+相连关键帧)
*/
bool ComputeSim3();
/**针对CorrectedPosesMap里的关键帧,mvpLoopMapPoints投影到这个关键帧上与其特征点并进行匹配。
* 如果匹配成功的特征点本身就有mappoint,就用mvpLoopMapPoints里匹配的点替换,替换下来的mappoint则销毁
* @param CorrectedPosesMap 表示和当前帧在covisibility相连接的keyframe及其修正的位姿
*/
void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap);
/**
* @brief 闭环
*
* 1. 通过求解的Sim3以及相对姿态关系,调整与当前帧相连的关键帧位姿以及这些关键帧观测到的MapPoints的位置(相连关键帧---当前帧)
* 2. 将闭环帧以及闭环帧相连的关键帧的MapPoints和与当前帧相连的关键帧的点进行匹配(相连关键帧+当前帧---闭环帧+相连关键帧)
* 3. 通过MapPoints的匹配关系更新这些帧之间的连接关系,即更新covisibility graph
* 4. 对Essential Graph(Pose Graph)进行优化,MapPoints的位置则根据优化后的位姿做相对应的调整
* 5. 创建线程进行全局Bundle Adjustment
*/
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;
//找到的和mpCurrentKF形成闭环检测的关键帧
KeyFrame* mpMatchedKF;
std::vector<ConsistentGroup> mvConsistentGroups;
//由DetectLoop()得到的候选关键帧
std::vector<KeyFrame*> mvpEnoughConsistentCandidates;
//将mpMatchedKF闭环关键帧相连的关键帧全部取出来放入vpLoopConnectedKFs
std::vector<KeyFrame*> mvpCurrentConnectedKFs;
//将mvpLoopMapPoints投影到当前关键帧mpCurrentKF进行投影得到的匹配
std::vector<MapPoint*> mvpCurrentMatchedPoints;
// 将vpLoopConnectedKFs的MapPoints取出来放入mvpLoopMapPoints
std::vector<MapPoint*> mvpLoopMapPoints;
//表示通过ComputeSim3()算的当前帧mpCurrentKF到世界坐标系的变换
cv::Mat mScw;
//表示通过ComputeSim3()算的当前帧mpCurrentKF到世界坐标系的Sim3变换
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;
};
3.源文件
3.1. 类函数Run()
3.1.1. 代码
void LoopClosing::Run()
{
mbFinished =false;
while(1)
{
// Check if there are keyframes in the queue
//如果有新的keyframe插入到闭环检测序列(在localmapping::run()结尾处插入)
if(CheckNewKeyFrames())
{
// Detect loop candidates and check covisibility consistency
//检测是否有闭环候选关键帧
if(DetectLoop())
{
// Compute similarity transformation [sR|t]
// In the stereo/RGBD case s=1
//计算候选关键帧的与当前帧的sim3并且返回是否形成闭环的判断
//并在候选帧中找出闭环帧
//并计算出当前帧和闭环帧的sim3
if(ComputeSim3())
{
// Perform loop fusion and pose graph optimization
CorrectLoop();
}
}
}
ResetIfRequested();
if(CheckFinish())
break;
usleep(5000);
}
SetFinish();
}
3.2. 类函数DetectLoop()
3.2.1.代码
bool LoopClosing::DetectLoop()
{
//先将要处理的闭环检测队列的关键帧弹出来一个
{
unique_lock<mutex> lock(mMutexLoopQueue);
mpCurrentKF = mlpLoopKeyFrameQueue.front();
mlpLoopKeyFrameQueue.pop_front();
// Avoid that a keyframe can be erased while it is being process by this thread
mpCurrentKF->SetNotErase();
}
//If the map contains less than 10 KF or less than 10 KF have passed from last loop detection
// 如果距离上次闭环没多久(小于10帧),或者map中关键帧总共还没有10帧,则不进行闭环检测
if(mpCurrentKF->mnId<mLastLoopKFid+10)
{
mpKeyFrameDB->add(mpCurrentKF);
mpCurrentKF->SetErase();
return false;
}
// Compute reference BoW similarity score
// This is the lowest score to a connected keyframe in the covisibility graph
// We will impose loop candidates to have a higher similarity than this
//遍历所有共视关键帧,计算当前关键帧与每个共视关键的bow相似度得分,计算minScore
//返回Covisibility graph中与此节点连接的节点(即关键帧),总的来说这一步是为了计算阈值 minScore
const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
float minScore = 1;
for(size_t i=0; i<vpConnectedKeyFrames.size(); i++)
{
KeyFrame* pKF = vpConnectedKeyFrames[i];
if(pKF->isBad())
continue;
const DBoW2::BowVector &BowVec = pKF->mBowVec;
float score = mpORBVocabulary->score(CurrentBowVec, BowVec);
if(score<minScore)
minScore = score;
}
// Query the database imposing the minimum score
//在最低相似度 minScore的要求下,获得闭环检测的候选帧集合
vector<KeyFrame*<