文章目录
算法原理解析
LoopClosing线程是对当前新到来的关键帧进行闭环检测，看看之前的关键帧中哪个和当前关键帧形成闭环；另一个是纠正闭环，就是在检测到闭环的时候要将当前关键帧和闭环关键帧的位姿拉到一起进行融合
一、闭环检测DetectLoop
1.从关键帧列表中获取列表头的关键帧，也就是获取最新插入到LoopClosing线程的关键帧，并判断当前关键帧是否距离上次关键帧闭环检测的帧之间大于10帧，大于10帧才进行后边的闭环检测，否则将新的关键帧插入到关键帧数据库中。
2.获取当前关键帧和与其共视的所有关键帧词袋向量比较的最小分值minScore；
3.在关键帧数据库中查找和当前关键帧存在闭环的候选关键帧(DetectLoopCandidates)。步骤如下：
1）获取当前关键帧在共视图中和他连接的关键帧，并遍历查找这些关键帧当中的词向量都在哪些关键帧中出现过，查到的共享词向量的关键帧存放在lKFsSharingWords当中；
2）遍历lKFsSharingWords，统计候选帧中与pKF具有共同单词最多的单词数maxCommonWords；
3）再次遍历lKFsSharingWords，得到和当前关键帧共享单词数大于0.8×maxCommonWords，并且和当前关键帧的BoW得分大于minScore的关键帧，存入lScoreAndMatch当中；
4）遍历lScoreAndMatch当中的关键帧，对其中每个关键帧计算和其共视关系最好的10个关键帧中的最高得分的关键帧和他们的累计分值bestAccScore，存入lAccScoreAndMatch当中；
5）遍历lAccScoreAndMatch，对其中每个关键帧判断其得分大于0.75×bestAccScore的话，将该关键帧存入闭环候选帧spAlreadyAddedKF当中返回；
4.判断连续性：一个关键帧和其所有的相连关键帧一起构成了一个关键帧组，判断连续性的根据就是判断当前关键帧组当中的关键帧是否在上一次的关键帧组中出现过足够的帧数。
1）遍历闭环候选帧，将每一个候选帧和其所有相连的关键帧放在一个set集合当中，构成了当前的候选连续性关键帧组；
2）遍历上一次的连续性关键帧组，判断当前的候选连续性关键帧组中的每一个关键帧是否在上一次的连续性关键帧组中出现过。
如果出现过，则将该关键帧的连续性个数在上一次的基础上加1存储在nCurrentConsistency中，当nCurrentConsistency大于阈值3的时候并且还没有足够的连续性则认为当前候选关键帧具有足够的连续性，将其加入到mvpEnoughConsistentCandidates当中。
3）mvpEnoughConsistentCandidates中有加入的关键帧，则认为具有连续性。
此时mvpEnoughConsistentCandidates就是检测所得的闭环候选关键帧。
5.用当前的连续性共视关键帧组更新上一次的连续性共视关键帧组（mvConsistentGroups = vCurrentConsistentGroups;）。
6.将当前关键帧加入到关键帧数据库当中。
二、计算相似变换ComputeSim3
sim3的引入：对于单目相机来说，具有尺度不确定性。如果在单目SLAM当中使用欧式变换SE3表示位姿，由于尺度不确定性和累积漂移的存在，整个运动过程中的尺度会发生变化，而SE3体现不出尺度的变化，所以此时就需要借用相似变换将尺度因子表达出来。对于空间中的一个点p，要投影到相机坐标系当中，相似变换表达如下：
{p}’=\begin{bmatrix} sR & t\ 0^T& 1 \end{bmatrix}p=sRp+t.
相似变换的尺度s作用在p的3个坐标上，相当于对p进行了一次缩放。相似变换对矩阵乘法构成群，称为相似变换群sim3。
以上引入描述来自高博视觉slam十四讲当中。另外sim3计算也可以参考笔者之前的一篇博文：ORB-SLAM2代码阅读笔记（十）：sim3求解
ComputeSim3流程如下：
1.遍历候选闭环关键帧中每个关键帧，对每个候选关键帧和当前关键帧之间求解sim3。
1）这里先对每个候选关键帧和当前关键帧进行BOW搜索匹配，只有当匹配的特征点数>=20的时候才为该候选帧构建sim3求解器，否则剔除掉该候选帧；
2）对1）中为每个匹配成功的候选帧进行sim3迭代优化求解，根据求解出的s,R,t再次对当前关键帧和候选关键帧进行匹配(函数为SearchBySim3)；
3）根据sim3求解结果构造当前关键帧到候选关键帧的sim3变换（这里的尺度s是sim3求解出来的）gScm，并遍历2）中的匹配点进行SIM3优化；
如果优化后的inlier数>=20，则该候选帧就是匹配关键帧，并计算当前帧位姿经过sim3修正后的位姿（这里构造闭环帧的sim3位姿gSmw时，尺度s为1）mg2oScw = gScm*gSmw，至此也就计算出了当前关键帧的位姿；
如果优化后的inlier数<20，则继续遍历下一个候选关键帧进行sim3变换求解；
2.获取当前关键帧的匹配闭环关键帧及其相连关键帧对应的地图点。将这些地图点通过上面优化得到的Sim3(位姿mScw) 变换投影到当前关键帧进行匹配SearchByProjection：
如果匹配点数>=40，则判定当前关键帧和闭环候选关键帧存在闭环关系；
如果匹配点数<40，则判定当前关键这暖和闭环候选帧之间没有闭环关系；
注意，在LoopClosing::ComputeSim3()函数中一共进行了下面三次特征点匹配：
1.第一次匹配
int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
这里主要是通过SearchByBow搜索当前关键帧mpCurrentKF中和闭环候选帧pKF匹配的地图点，BoW通过将单词聚类到树结构node的方法，这样可以加快搜索匹配速度，vvpMapPointMatches[i]用于存储当前关键帧和候选关键帧之间匹配的地图点。
2.第二次匹配
matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
使用sim3求出来的s,R,t通过SearchBySim3得到更多匹配。
3.第三次匹配
matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
这里得到的当前关键帧中匹配上闭环关键帧共视地图点(mvpCurrentMatchedPoints)将用于后面CorrectLoop时当时关键帧地图点的冲突融合，
到这里不仅确保了当前关键帧与闭环帧之间匹配度高， 而且保证了闭环帧的共视图中的地图点和当前帧的特征点匹配度更高,说明该闭环帧是正确的。
三、闭环校正CorrectLoop
1.暂停LocalMapping线程，防止其在闭环校正阶段向LoopClosing线程中插入新的关键帧；
2.停止全局BA优化线程运行；
3.更新当前关键帧与其共视的其他关键帧之间的连接关系UpdateConnections;
4.得到当前关键帧和与其连接的共视关键帧组，并通过当前关键帧的sim3位姿计算出与其相连接的共视关键帧的闭环优化后的sim3位姿；
5.利用4中所得的调整过的和当前关键帧相连的所有关键帧的sim3位姿来更新每个关键帧相对应的地图点的3D坐标和深度等信息；
6.根据关键帧的SIM3位姿构造出欧式变换SE3，也就是关键帧的位姿，并更新关键帧的位姿和连接关系；
7.更新当前关键帧当中的地图点，或者将经过sim3位姿纠正过的地图点加入到当前关键帧当中。
前置理解事项：
第一次匹配：通过BoW比较进行匹配；
第二次匹配：在第一次匹配的基础上，根据匹配点进行sim3优化求解后，再根据sim3所求的旋转R、平移t和尺度s再次进行当前关键帧和闭环候选帧之间的匹配SearchBySim3，并再次进行优化；
第三次匹配：使用第二次匹配完优化后的当前关键帧和闭环候选帧之间的SIM3变换再次进行匹配matcher.SearchByProjection；
通过以上三次匹配后，所得的匹配点是质量最高的，毕竟每次匹配和优化后都会剔除一些outlier。在闭环校正当中使用这些匹配点进行地图点融合，也就是用这些地图点替换掉当前关键帧。
用投影匹配上的经过SIM3计算纠正过位姿的地图点更新当前关键帧（也就是存在闭环的关键帧）中对应的地图点；
如果当前关键帧中没有与之对应的地图点，则将这些经过SIM3计算纠正过位姿的地图点加入当前关键帧当中，并更新该地图点的观测帧和描述子；
8.地图点进行融合SearchAndFuse：
这里是使用闭环匹配关键帧和其共视关键帧当中的地图点来更新当前关键帧的共视关键帧中相匹配的地图点。
当前关键帧的共视关键帧中没有的地图点，则直接进行添加；有的则直接用闭环匹配关键帧和其共视关键帧当中对应的地图点进行更新。
这样就完成了地图点的融合。
9.优化EseentialGraph：Optimizer::OptimizeEssentialGraph
EseentialGraph其实就是当前地图中所有的关键帧、所有的地图点组成的图，那么这里其实就是一个大的图优化。
只不过在这里边优化的时候要区分当前关键帧、和当前关键帧形成的闭环关键帧、当前关键帧的共视关键帧、和闭环关键帧有共视关系的关键帧。
如果关键帧的位姿在sim3求解的时候已经求出来了，则使用sim3求解所得的位姿。图的顶点都是关键帧，边分为正常边(normal edge)和闭环边(loop edge),
用优化所得的关键帧的sim3变换恢复出SE3，并对关键帧中相应的地图点更新其3D坐标和深度信息。
10.将当前关键帧添加到匹配的闭环关键帧的闭环边中。
将与当前关键帧形成闭环的关键帧添加到当前关键帧的闭环边中；
11.启动一个线程进行全局优化RunGlobalBundleAdjustment。
毋庸置疑的是，随着相机的运动，我们计算的相机位姿，三角化得到的点云位置，都是有误差的，即使我们使用局部的或全局的BA去优化，仍然会存在累积误差。而消除误差最有效的办法是发现闭环，并根据闭环对所有结果进行优化。闭环是一个比BA更加强烈、更加准确的约束，所有如果能够检测到闭环，并对其优化，就可以让结果更加准确。
闭环条件检测
首先我们计算关键帧Ki和在Covisibility Graph中与其相连的关键帧之间的词袋(BOW)之间的相似度。本文中，作者离线训练了大量的基于ORB描述的词袋，在程序运行时加载进去。这里的词袋作为对该关键帧的描述，将闭环检测转变为一个类似于模式识别的问题。当相机再次来到之前到过的场景时，就会因为看到相同的景物，而得到类似的词袋描述，从而检测到闭环。这里的接口是
// Detect loop candidates and check covisibility consistency
if(DetectLoop())
跟进函数接口，可以很清晰地看到检测闭环的过程，这里作者使用了DBOW2库。
单目SLAM系统存在一个问题，就是尺度的模糊性。
针对一个三维点P点来说，我们在单目拍摄的两幅图中可以匹配到pL和pR，但是无法确定其在三角化里的具体位置，所以存在尺度模糊。而对于立体相机或RGBD相机，这里的尺度就可以唯一确定。所以，单目SLAM是一个Sim3相似变换群，有7个自由度，即1个尺度因子s、3个旋转角度、3个位移，即[sR|t]。显然，立体相机和RGBD相机中s=1。所以单目相机在优化时，是在相似变换群优化，本文是在RANSAC框架下进行优化。这个接口为：
// Compute similarity transformation [sR|t]
// In the stereo/RGBD case s=1
if(ComputeSim3())
闭环矫正的第一步是融合重复的点云，并且在Covisibility Graph中插入新的边以连接闭环。首先当前帧的位姿会根据相似变换而被矫正，同时所有与其相连的关键帧也会被矫正。所有的被闭环处的关键帧观察到的地图点会通过映射在一个小范围里，然后去搜索它的近邻匹配。这样就可以对所有匹配的点云进行更加有效的数据融合，并更新关键帧位姿，以及在图中的边。这部分接口为：
// Perform loop fusion and pose graph optimization
CorrectLoop();
Essential Graph优化
为了有效地完成闭环，本文使用本质图（Essential Graph）去优化位姿图。这样就可以将闭环的误差分散到整个图中。这部分主要是在CorrectLoop()中的后部分：
// Optimize graph
Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
// Add loop edge
mpMatchedKF->AddLoopEdge(mpCurrentKF);
mpCurrentKF->AddLoopEdge(mpMatchedKF);
源码解读LoopClosing.cc
#include "LoopClosing.h"
#include "Sim3Solver.h"
#include "Converter.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"
#include "Optimizer.h"
#include "ORBmatcher.h"
#include<mutex>
#include<thread>
//回环检测线程
namespace ORB_SLAM2
{
// /// 自定义数据类型, ConsistentGroup.first对应每个“连续组”中的关键帧，ConsistentGroup.second为每个“连续组”的序号
// typedef pair<set<KeyFrame*>,int> ConsistentGroup;
// /// 存储关键帧对象和位姿的键值对,这里是map的完整构造函数
// typedef map<KeyFrame*, //键
// g2o::Sim3, //值
// std::less<KeyFrame*>, //排序算法
// Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > // 指定分配器,和内存空间开辟有关. 为了能够使用Eigen库中的SSE和AVX指令集加速,需要将传统STL容器中的数据进行对齐处理
// > KeyFrameAndPose;
// 构造函数
/**
* @brief 构造函数
* @param[in] pMap 地图指针
* @param[in] pDB 词袋数据库
* @param[in] pVoc 词典
* @param[in] bFixScale 表示sim3中的尺度是否要计算,对于双目和RGBD情况尺度是固定的,s=1,bFixScale=true;而单目下尺度是不确定的,此时bFixScale=false,sim
* 3中的s需要被计算
* 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;
/// 一个队列, 其中存储了参与到回环检测的关键帧 (当然这些关键帧也有可能因为各种原因被设置成为bad,这样虽然这个关键帧还是存储在这里但是实际上已经不再实质性地参与到回环检测的过程中去了)
std::list<KeyFrame*> mlpLoopKeyFrameQueue;
/// 操作参与到回环检测队列中的关键帧时,使用的互斥量
std::mutex mMutexLoopQueue;
// Loop detector parameters
/// 连续性阈值,构造函数中将其设置成为了3
float mnCovisibilityConsistencyTh;
// Loop detector variables
/// 当前关键帧,其实称之为"当前正在处理的关键帧"更加合适
KeyFrame* mpCurrentKF;
// 最终检测出来的,和当前关键帧形成闭环的闭环关键帧
KeyFrame* mpMatchedKF;
/// 上一次执行的时候产生的连续组s
std::vector<ConsistentGroup> mvConsistentGroups;
/// 从上面的关键帧中进行筛选之后得到的具有足够的"连续性"的关键帧 -- 这个其实也是相当于更高层级的、更加优质的闭环候选帧
std::vector<KeyFrame*> mvpEnoughConsistentCandidates;
/// 和当前关键帧相连的关键帧形成的"当前关键帧组"
std::vector<KeyFrame*> mvpCurrentConnectedKFs;
/// 下面的变量中存储的地图点在"当前关键帧"中成功地找到了匹配点的地图点的集合
std::vector<MapPoint*> mvpCurrentMatchedPoints;
/// 闭环关键帧上的所有相连关键帧的地图点
std::vector<MapPoint*> mvpLoopMapPoints;
// 下面的变量的cv::Mat格式版本
cv::Mat mScw;
// 当得到了当前关键帧的闭环关键帧以后,计算出来的从世界坐标系到当前帧的sim3变换
g2o::Sim3 mg2oScw;
/// 上一次闭环帧的id
long unsigned int mLastLoopKFid;
// Variables related to Global Bundle Adjustment
/// 全局BA线程是否在进行
bool mbRunningGBA;
/// 全局BA线程在收到停止请求之后是否停止的比标志 //? 可是直接使用上面变量的逆不就可以表示了吗? //? 表示全局BA工作是否正常结束?
bool mbFinishedGBA;
/// 由当前线程调用,请求停止当前正在进行的全局BA
bool mbStopGBA;
/// 在对和全局线程标志量有关的操作的时候使用的互斥量
std::mutex mMutexGBA;
/// 全局BA线程句柄
std::thread* mpThreadGBA;
// Fix scale in the stereo/RGB-D case
/// 如果是在双目或者是RGBD输入的情况下,就要固定尺度,这个变量就是是否要固定尺度的标志
*bool mbFixScale;
// 已经进行了的全局BA次数(包含中途被打断的)
//bool mnFullBAIdx;
*/
LoopClosing::LoopClosing(Map *pMap, KeyFrameDatabase *pDB, ORBVocabulary *pVoc, const bool bFixScale):
mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap),
mpKeyFrameDB(pDB), mpORBVocabulary(pVoc), mpMatchedKF(NULL), mLastLoopKFid(0), mbRunningGBA(false), mbFinishedGBA(true),
mbStopGBA(false), mpThreadGBA(NULL), mbFixScale(bFixScale), mnFullBAIdx(0)
{
// 连续性阈值
mnCovisibilityConsistencyTh = 3;
}
// 设置追踪线程句柄
/** @brief 设置追踪线程的句柄
* @param[in] pTracker 追踪线程的句柄 */
void LoopClosing::SetTracker(Tracking *pTracker)
{
mpTracker=pTracker;
}
// 设置局部建图线程的句柄
/** @brief 设置局部建图线程的句柄
* @param[in] pLocalMapper */
void LoopClosing::SetLocalMapper(LocalMapping *pLocalMapper)
{
mpLocalMapper=pLocalMapper;
}
// 线程主函数
// Main function
/** @brief 回环检测线程主函数 */
void LoopClosing::Run()
{
mbFinished =false;
// 线程主循环
while(1)
{
// Check if there are keyframes in the queue
// Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的
// 在LocalMapping中通过 InsertKeyFrame 将关键帧插入闭环检测队列mlpLoopKeyFrameQueue
// 闭环检测队列mlpLoopKeyFrameQueue中的关键帧不为空
if(CheckNewKeyFrames())
{
// Detect loop candidates and check covisibility consistency
if(DetectLoop())
{
// Compute similarity transformation [sR|t]
// In the stereo/RGBD case s=1
if(ComputeSim3())
{
// Perform loop fusion and pose graph optimization
CorrectLoop();
}
}
}
// 查看是否有外部线程请求复位当前线程
ResetIfRequested();
// 查看外部线程是否有终止当前线程的请求,如果有的话就跳出这个线程的主函数的主循环
if(CheckFinish())
break;
//usleep(5000);
std::this_thread::sleep_for(std::chrono::milliseconds(5));
}
// 运行到这里说明有外部线程请求终止当前线程,在这个函数中执行终止当前线程的一些操作
SetFinish();
}
// 将某个关键帧加入到回环检测的过程中,由局部建图线程调用
/** @brief 将某个关键帧加入到回环检测的过程中,由局部建图线程调用
* @param[in] pKF */
void LoopClosing::InsertKeyFrame(KeyFrame *pKF)
{
unique_lock<mutex> lock(mMutexLoopQueue);
// NOTICE 这里第0个关键帧不能够参与到回环检测的过程中,因为第0关键帧定义了整个地图的世界坐标系
if(pKF->mnId!=0)
mlpLoopKeyFrameQueue.push_back(pKF);
}
/*
* 查看列表中是否有等待被插入的关键帧
* @return 如果存在，返回true
*/
/** @brief 查看列表中是否有等待被插入的关键帧
* @return true 如果有
* @return false 没有 */
bool LoopClosing::CheckNewKeyFrames()
{
unique_lock<mutex> lock(mMutexLoopQueue);
return(!mlpLoopKeyFrameQueue.empty());
}
/** @brief 检测回环,如果有的话就返回真 */
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
// STEP 1：如果距离上次闭环没多久（小于10帧），或者map中关键帧总共还没有10帧，则不进行闭环检测
// 后者的体现是当mLastLoopKFid为0的时候
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
// VIII-A
// STEP 2：遍历所有共视关键帧，计算当前关键帧与每个共视关键的bow相似度得分，并得到最低得分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
// STEP 3：在所有关键帧中找出闭环备选帧
// 说白了就是认为和当前关键帧具有回环关系的关键帧,不应该低于当前关键帧的相邻关键帧的最低的相似度
// 得到的这些关键帧,和当前关键帧具有较多的共视单词,并且相似度评分都挺高
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
// If there are no loop candidates, just add new keyframe and return false
if(vpCandidateKFs.empty())
{
mpKeyFrameDB->add(mpCurrentKF);
mvConsistentGroups.clear();
mpCurrentKF->SetErase();
return false; // 没有检测到回环
}
// 接下来就属于检测到回环啦
// For each loop candidate check consistency with previous loop candidates
// Each candidate expands a covisibility group (keyframes connected to the loop candidate in the covisibility graph)
// A group is consistent with a previous group if they share at least a keyframe
// We must detect a consistent loop in several consecutive keyframes to accept it
// STEP 4：在候选帧中检测具有连续性的候选帧
// 1、每个候选帧将与自己相连的关键帧构成一个“子候选组spCandidateGroup”， vpCandidateKFs-->spCandidateGroup
// 2、检测“子候选组”中每一个关键帧是否存在于“连续组”，如果存在 nCurrentConsistency++，则将该“子候选组”放入“当前连续组vCurrentConsistentGroups”
// 3、如果nCurrentConsistency大于等于3，那么该”子候选组“代表的候选帧过关，进入mvpEnoughConsistentCandidates
// 相关的概念说明:
// 组(group): 对于某个关键帧, 其和其具有共视关系的关键帧组成了一个"组";
// 子候选组(CandidateGroup): 对于某个候选的回环关键帧, 其和其具有共视关系的关键帧组成的一个"组";
// 连续(Consistent): 不同的组之间如果共同拥有一个及以上的关键帧,那么称这两个组之间具有连续关系
// 连续性(Consistency):称之为连续长度可能更合适,表示累计的连续的链的长度:A--B 为1, A--B--C--D 为3等;具体反映在数据类型 ConsistentGroup.second上
// 连续组(Consistent group): mvConsistentGroups存储了上次执行回环检测时, 新的被检测出来的具有连续性的多个组的集合.由于组之间的连续关系是个网状结构,因此可能存在
// 一个组因为和不同的连续组链都具有连续关系,而被添加两次的情况(当然连续性度量是不相同的)
// 连续组链:我guoqing自造的称呼,类似于菊花链A--B--C--D这样形成了一条连续组链.对于这个例子中,由于可能E,F都和D有连续关系,因此连续组链会产生分叉;为了简化计算,连续组中将只会保存
// 最后形成连续关系的连续组们(见下面的连续组的更新)
// 子连续组: 上面的连续组中的一个组
// 连续组的初始值: 在遍历某个候选帧的过程中,如果该子候选组没有能够和任何一个上次的子连续组产生连续关系,那么就将添加自己组为连续组,并且连续性为0(相当于新开了一个连续链)
// 连续组的更新: 当前次回环检测过程中,所有被检测到和之前的连续组链有连续的关系的组,都将在对应的连续组链后面+1,这些子候选组(可能有重复,见上)都将会成为新的连续组;
// 换而言之连续组mvConsistentGroups中只保存连续组链中末尾的组
mvpEnoughConsistentCandidates.clear();// 最终筛选后得到的闭环帧
// ConsistentGroup数据类型为pair<set<KeyFrame*>,int>
// ConsistentGroup.first对应每个“连续组”中的关键帧，ConsistentGroup.second为每个“连续组”的序号
// 这个下标是每个"子连续组"的下标,bool表示当前的候选组中是否有和该组相同的一个关键帧
vector<ConsistentGroup> vCurrentConsistentGroups;
vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false);
// 遍历刚才得到的每一个候选关键帧
for(size_t i=0, iend=vpCandidateKFs.size(); i<iend; i++)
{
KeyFrame* pCandidateKF = vpCandidateKFs[i];
// 将自己以及与自己相连的关键帧构成一个“子候选组”
set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
spCandidateGroup.insert(pCandidateKF);
bool bEnoughConsistent = false;
bool bConsistentForSomeGroup = false;
// 遍历之前的“子连续组s”
for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++)
{
// 取出一个之前的子连续组
set<KeyFrame*> sPreviousGroup = mvConsistentGroups[iG].first;
// 遍历每个“子候选组”，检测候选组中每一个关键帧在“子连续组”中是否存在
// 如果有一帧共同存在于“子候选组”与之前的“子连续组”，那么“子候选组”与该“子连续组”连续
bool bConsistent = false;
for(set<KeyFrame*>::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++)
{
if(sPreviousGroup.count(*sit))
{
bConsistent=true;// 该“子候选组”与该“子连续组”相连
bConsistentForSomeGroup=true;// 该“子候选组”至少与一个”子连续组“相连
break;
}
}
if(bConsistent)
{
// 和当前的候选组发生"连续"关系的子连续组的"已连续id"
int nPreviousConsistency = mvConsistentGroups[iG].second;
int nCurrentConsistency = nPreviousConsistency + 1;
// 如果当前遍历到的"子连续组"还没有和"子候选组有相同的关键帧的记录,那么就+
if(!vbConsistentGroup[iG])// 这里作者原本意思是不是应该是vbConsistentGroup[i]而不是vbConsistentGroup[iG]呢？（wubo???）
//! 应该就是iG,vbConsistentGroup声明时候的大小和mvConsistentGroups是相同的,而在遍历mvConsistentGroups中的每个元素的时候使用的下标是iG
//! 而i是在遍历vpCandidateKFs也就是经过KeyFrameDatabase得到的一堆初始的闭环候选帧的时候才使用的 --- guoqing
{
// 将该“子候选组”的该关键帧打上连续编号加入到“当前连续组”
ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
vCurrentConsistentGroups.push_back(cg);
vbConsistentGroup[iG]=true; //this avoid to include the same group more than once
}
// 如果已经连续得足够多了,那么当前的这个候选关键帧是足够靠谱的
if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent)
{
// 在遍历每个候选关键帧的过程中的、遍历之前的“子连续组”过程中,如果遍历到某个"子连续组"中符合要求的关键帧,就把当前的候选关键帧也添加进去
mvpEnoughConsistentCandidates.push_back(pCandidateKF);
bEnoughConsistent=true; //this avoid to insert the same candidate more than once
}
//这里是不是缺一个break来提高效率呢？(wubo???)
// 师兄说的对 (guoqing)
}
}
// If the group is not consistent with any previous group insert with consistency counter set to zero
// 如果该“子候选组”的所有关键帧都不存在于“子连续组”，那么 vCurrentConsistentGroups 将为空，
// 于是就把“子候选组”全部拷贝到 vCurrentConsistentGroups， 并最终用于更新mvConsistentGroups，计数器设为0，重新开始
if(!bConsistentForSomeGroup)
{
// 这个地方是不是最好clear一下 vCurrentConsistentGroups 呢？(wubo???)
//! 师兄不可以啊! 因为上面的if条件满足的时候,只能够说明当前遍历到的这个候选关键帧形成的子候选组和所有的子连续组没有连续关系,所以当前子候选组是一个新出现的连续组链;
//! 但是这不能够代表其他的候选关键帧的子候选组和那些子连续组们没有连续关系!所以 vCurrentConsistentGroups 不能清空
//! 如果清空了的话,那么只要候选关键帧中有一帧比较孤立的,和当前已有的所有连续组都不产生共视关系,那么完蛋了,在遍历当前候选关键帧之前的所有连续关系
//! 都会被清空 ----guoqing
ConsistentGroup cg = make_pair(spCandidateGroup,0);
vCurrentConsistentGroups.push_back(cg);
}
}// 遍历得到的初级的候选关键帧
// Update Covisibility Consistent Groups
// 当前的变上一次的
mvConsistentGroups = vCurrentConsistentGroups;
// Add Current Keyframe to database
mpKeyFrameDB->add(mpCurrentKF);
// 如果不过关
if(mvpEnoughConsistentCandidates.empty())
{
mpCurrentKF->SetErase();
return false;
}
else // 过关
{
return true;
}
// 多余的代码,执行不到
mpCurrentKF->SetErase();
return false;
}
/**
* @brief 计算当前帧与闭环帧的Sim3变换等
* @details \n
* 1. 通过Bow加速描述子的匹配，利用RANSAC粗略地计算出当前帧与闭环帧的Sim3（当前帧---闭环帧） \n
* 2. 根据估计的Sim3，对3D点进行投影找到更多匹配，通过优化的方法计算更精确的Sim3（当前帧---闭环帧） \n
* 3. 将闭环帧以及闭环帧相连的关键帧的MapPoints与当前帧的点进行匹配（当前帧---闭环帧+相连关键帧） \n
* \n
* 注意以上匹配的结果均都存在成员变量mvpCurrentMatchedPoints中，实际的更新步骤见CorrectLoop()步骤3：Start Loop Fusion \n
* 对于双目或者是RGBD输入的情况,计算得到的尺度=1
*/
bool LoopClosing::ComputeSim3()
{
// 说明：计算当前帧与闭环帧的Sim3变换等
// 1. 通过Bow加速描述子的匹配，利用RANSAC粗略地计算出当前帧与闭环帧的Sim3（当前帧---闭环帧） \n
// 2. 根据估计的Sim3，对3D点进行投影找到更多匹配，通过优化的方法计算更精确的Sim3（当前帧---闭环帧） \n
// 3. 将闭环帧以及闭环帧相连的关键帧的MapPoints与当前帧的点进行匹配（当前帧---闭环帧+相连关键帧） \n
// 注意以上匹配的结果均都存在成员变量mvpCurrentMatchedPoints中，实际的更新步骤见CorrectLoop()步骤3：Start Loop Fusion \n
// 对于双目或者是RGBD输入的情况,计算得到的尺度=1
// Input:
// do :
// 1. 遍历闭环候选帧集，筛选出与当前帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver
// 2. 对每一个候选帧进行 RANSAC 迭代匹配，直到有一个候选帧匹配成功，或者全部失败
// 3. 取出闭环匹配上关键帧的相连关键帧，得到它们的MapPoints放入 mvpLoopMapPoints
// 4. 将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配
// 5. 判断当前帧与检测出的所有闭环关键帧是否有足够多的MapPoints匹配
// 6. 清空mvpEnoughConsistentCandidates
// output:
// Step 0. 准备工作
// For each consistent loop candidate we try to compute a Sim3
const int nInitialCandidates = mvpEnoughConsistentCandidates.size();
// We compute first ORB matches for each candidate
// If enough matches are found, we setup a Sim3Solver
ORBmatcher matcher(0.75,true);
// 用 vector 存储每一个候选帧的Sim3Solver求解器
vector<Sim3Solver*> vpSim3Solvers;
vpSim3Solvers.resize(nInitialCandidates);
// 用 vector 存储每个候选帧的匹配地图点信息
vector<vector<MapPoint*> > vvpMapPointMatches;
vvpMapPointMatches.resize(nInitialCandidates);
// 用 vector 存储每个候选帧应该被放弃(True）或者 保留(False)
vector<bool> vbDiscarded;
vbDiscarded.resize(nInitialCandidates);
// 完成 Step 1.0 的匹配后，被保留的候选帧数量
int nCandidates=0;
// Step 1. 遍历闭环候选帧集，筛选出与当前帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver
for(int i=0; i<nInitialCandidates; i++)
{
// Step 1.1 从筛选的闭环候选帧中取出一帧关键帧pKF
KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
// 避免在LocalMapping中KeyFrameCulling函数将此关键帧作为冗余帧剔除
pKF->SetNotErase();
// 如果候选帧质量不高，直接PASS
if(pKF->isBad())
{
vbDiscarded[i] = true;
continue;
}
// Step 1.2 将当前帧 mpCurrentKF 与闭环候选关键帧pKF匹配
// 通过bow加速得到 mpCurrentKF 与 pKF 之间的匹配特征点
// vvpMapPointMatches 是匹配特征点对应的 MapPoints,本质上来自于候选闭环帧
int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
// 匹配的特征点数太少，该候选帧剔除
if(nmatches<20)
{
vbDiscarded[i] = true;
continue;
}
else
{
// Step 1.3 为保留的候选帧构造Sim3求解器
// 如果 mbFixScale 为 true，则是6DoFf优化（双目 RGBD）
// 如果是false，则是7DoF优化（单目）
Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
// Sim3Solver Ransac 过程至少20个inliers 300次迭代
pSolver->SetRansacParameters(0.99,20,300);
vpSim3Solvers[i] = pSolver;
}
// 保留的候选帧数量
nCandidates++;
}
// 用于标记是否有一个候选帧通过Sim3Solver的求解与优化
bool bMatch = false;
// Step 2.0 对每一个候选帧进行 RANSAC 迭代匹配，直到有一个候选帧匹配成功，或者全部失败
while(nCandidates>0 && !bMatch)
{
// 遍历每一个候选帧
for(int i=0; i<nInitialCandidates; i++)
{
if(vbDiscarded[i])
continue;
KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
// 内点（Inliers）标志
// 即标记经过RANSAC sim3 求解后,vvpMapPointMatches中的哪些作为内点
vector<bool> vbInliers;
// 内点（Inliers）数量
int nInliers;
// 是否到达了最优解
bool bNoMore;
// Step 2.1 取出从 Step 1.3 中为当前候选帧构建的 Sim3Solver 并开始迭代
Sim3Solver* pSolver = vpSim3Solvers[i];
// 最多迭代5次，返回的Scm是候选帧pKF到当前帧mpCurrentKF的Sim3变换（T12）
cv::Mat Scm = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
// If Ransac reachs max. iterations discard keyframe
// 经过n次循环，每次迭代5次，总共迭代 n*5 次
// 总迭代次数达到最大限制还没有求出合格的Sim3变换，该候选帧剔除
if(bNoMore)
{
vbDiscarded[i]=true;
nCandidates--;
}
// If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences
if(!Scm.empty())
{
vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));
for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
{
// 保存inlier的MapPoint
if(vbInliers[j])
vpMapPointMatches[j]=vvpMapPointMatches[i][j];
}
// Step 2.2 通过步骤2.1求取的Sim3变换引导关键帧匹配弥补步骤1中的漏匹配
// 候选帧pKF到当前帧mpCurrentKF的R（R12），t（t12），变换尺度s（s12）
cv::Mat R = pSolver->GetEstimatedRotation();
cv::Mat t = pSolver->GetEstimatedTranslation();
const float s = pSolver->GetEstimatedScale();
// 查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数，之前使用SearchByBoW进行特征点匹配时会有漏匹配）
// 通过Sim3变换，确定pKF1的特征点在pKF2中的大致区域，同理，确定pKF2的特征点在pKF1中的大致区域
// 在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新匹配vpMapPointMatches
matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
// Step 2.3 Sim3优化，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
// OpenCV的Mat矩阵转成Eigen的Matrix类型
g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
// 如果mbFixScale为true，则是6DoFf优化（双目 RGBD），如果是false，则是7DoF优化（单目）
// 优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScm
const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
// 如果优化成功，则停止 ransacs（即while循环）并继续下一步
if(nInliers>=20)
{
// 为True时将不再进入 while循环
bMatch = true;
// mpMatchedKF就是最终闭环检测出来与当前帧形成闭环的关键帧
mpMatchedKF = pKF;
// 得到从世界坐标系到该候选帧的Sim3变换，Scale=1
g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);
// 得到g2o优化后从世界坐标系到当前帧的Sim3变换
mg2oScw = gScm*gSmw;
mScw = Converter::toCvMat(mg2oScw);
mvpCurrentMatchedPoints = vpMapPointMatches;
// 只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
break;
}
}
}
}
// 退出上面while循环的原因有两种,一种是求解到了bMatch置位后出的,另外一种是nCandidates耗尽为0
// 没有一个闭环匹配候选帧通过Sim3的求解与优化
if(!bMatch)
{
// 清空mvpEnoughConsistentCandidates
for(int i=0; i<nInitialCandidates; i++)
// 卧槽,这么严格吗?这些关键帧以后都不会在再参加回环检测过程了
mvpEnoughConsistentCandidates[i]->SetErase();
// 当前关键帧也是,将不会再参加回环检测的过程了
mpCurrentKF->SetErase();
return false;
}
// Step 3.0：取出闭环匹配上关键帧的相连关键帧，得到它们的MapPoints放入 mvpLoopMapPoints
// 注意是匹配上的那个关键帧：mpMatchedKF
// 将mpMatchedKF相连的关键帧全部取出来放入 vpLoopConnectedKFs
// 将vpLoopConnectedKFs的MapPoints取出来放入mvpLoopMapPoints
vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
// 包含闭环匹配关键帧本身,形成一个组
vpLoopConnectedKFs.push_back(mpMatchedKF);
mvpLoopMapPoints.clear();
// 遍历这个组中的每一个关键帧
for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
{
KeyFrame* pKF = *vit;
vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
// 遍历其中一个关键帧的所有地图点
for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
{
MapPoint* pMP = vpMapPoints[i];
if(pMP)
{
if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId)
{
mvpLoopMapPoints.push_back(pMP);
// 标记该MapPoint被mpCurrentKF闭环时观测到并添加，避免重复添加
pMP->mnLoopPointForKF=mpCurrentKF->mnId;
}
}
}
}
// Find more matches projecting with the computed Sim3
// Step 4.0：将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配
// 根据投影查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数）
// 根据Sim3变换，将每个mvpLoopMapPoints投影到mpCurrentKF上，并根据尺度确定一个搜索区域，
// 根据该MapPoint的描述子与该区域内的特征点进行匹配，如果匹配误差小于TH_LOW即匹配成功，更新mvpCurrentMatchedPoints
// mvpCurrentMatchedPoints将用于SearchAndFuse中检测当前帧MapPoints与匹配的MapPoints是否存在冲突
// 搜索范围系数为10
matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
// If enough matches accept Loop
// Step 5.0 判断当前帧与检测出的所有闭环关键帧是否有足够多的MapPoints匹配
int nTotalMatches = 0;
for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
{
if(mvpCurrentMatchedPoints[i])
nTotalMatches++;
}
// Step 6.0 清空mvpEnoughConsistentCandidates
if(nTotalMatches>=40)
{
// 如果当前回环可靠,那么就取消 候选的优质连续关键帧中的所有帧 参与回环检测的资格,除了当前的回环关键帧
for(int i=0; i<nInitialCandidates; i++)
if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
mvpEnoughConsistentCandidates[i]->SetErase();
return true;
}
else // 同,只不过这个时候回环帧不靠谱.也清除
{
for(int i=0; i<nInitialCandidates; i++)
mvpEnoughConsistentCandidates[i]->SetErase();
mpCurrentKF->SetErase();
return false;
}
}
/**
* @brief 闭环纠正
* @detials \n
* 1. 通过求解的Sim3以及相对姿态关系，调整与当前帧相连的关键帧位姿以及这些关键帧观测到的MapPoints的位置（相连关键帧---当前帧） \n
* 2. 将闭环帧以及闭环帧相连的关键帧的MapPoints和与当前帧相连的关键帧的点进行匹配（相连关键帧+当前帧---闭环帧+相连关键帧） \n
* 3. 通过MapPoints的匹配关系更新这些帧之间的连接关系，即更新covisibility graph \n
* 4. 对Essential Graph（Pose Graph）进行优化，MapPoints的位置则根据优化后的位姿做相对应的调整 \n
* 5. 创建线程进行全局Bundle Adjustment
*/
void LoopClosing::CorrectLoop()
{
cout << "Loop detected!" << endl;
// STEP 0：请求局部地图停止，防止在回环矫正时局部地图线程中InsertKeyFrame函数插入新的关键帧
// STEP 1：根据共视关系更新当前帧与其它关键帧之间的连接
// STEP 2：通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的MapPoints
// STEP 3：检查当前帧的MapPoints与闭环匹配帧的MapPoints是否存在冲突，对冲突的MapPoints进行替换或填补
// STEP 4：通过将闭环时相连关键帧的mvpLoopMapPoints投影到这些关键帧中，进行MapPoints检查与替换
// STEP 5：更新当前关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系
// STEP 6：进行EssentialGraph优化，LoopConnections是形成闭环后新生成的连接关系，不包括步骤7中当前帧与闭环匹配帧之间的连接关系
// STEP 7：添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）
// STEP 8：新建一个线程用于全局BA优化
// Send a stop signal to Local Mapping
// Avoid new keyframes are inserted while correcting the loop
// STEP 0：请求局部地图停止，防止在回环矫正时局部地图线程中InsertKeyFrame函数插入新的关键帧
mpLocalMapper->RequestStop();
// 终止全局BA线程
// 在回环纠正的时候调用,查看当前是否已经有一个全局优化的线程在进行
// bool isRunningGBA(){
// unique_lock<std::mutex> lock(mMutexGBA);
// return mbRunningGBA;
// }
if(isRunningGBA())
{
// 这个标志位仅用于控制输出提示，可忽略
unique_lock<mutex> lock(mMutexGBA);
mbStopGBA = true;
mnFullBAIdx++;
if(mpThreadGBA)
{
// 停止全局BA线程
mpThreadGBA->detach();
delete mpThreadGBA;
}
}
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped())
{
//usleep(1000);
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
// Ensure current keyframe is updated
// STEP 1：根据共视关系更新当前帧与其它关键帧之间的连接
// 猜测这里还要更新的原因应该是,当前处理的这个关键帧应该不是当前时刻最新插入的关键帧,它可能和后面新来的一些关键帧也产生了共视关系,虽然
// 在那些关键帧中也在这个中添加了更新的操作但是,有些图(本征图?)没法被他们更新吧,还是得这个关键帧自己来
mpCurrentKF->UpdateConnections();
// Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation
// STEP 2：通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的MapPoints
// 当前帧与世界坐标系之间的Sim变换在ComputeSim3函数中已经确定并优化，
// 通过相对位姿关系，可以确定这些相连的关键帧与世界坐标系之间的Sim3变换
// 取出与当前帧相连的关键帧，包括当前关键帧 -- 获取当前关键帧组
mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
mvpCurrentConnectedKFs.push_back(mpCurrentKF);
KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
// 先将mpCurrentKF的Sim3变换存入，固定不动
CorrectedSim3[mpCurrentKF]=mg2oScw;
cv::Mat Twc = mpCurrentKF->GetPoseInverse();
// 对地图点操作的临界区
{
// Get Map Mutex
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
// STEP 2.1：通过位姿传播，得到Sim3调整后其它与当前帧相连关键帧的位姿（只是得到，还没有修正）
// 遍历"当前关键帧组""
for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
{
KeyFrame* pKFi = *vit;
cv::Mat Tiw = pKFi->GetPose();
// currentKF在前面已经添加
if(pKFi!=mpCurrentKF)
{
// 得到当前帧到pKFi帧的相对变换
cv::Mat Tic = Tiw*Twc;
cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);
cv::Mat tic = Tic.rowRange(0,3).col(3);
g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
// 当前帧的位姿固定不动，其它的关键帧根据相对关系得到Sim3调整的位姿
g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;
// Pose corrected with the Sim3 of the loop closure
// 得到闭环g2o优化后各个关键帧的位姿
CorrectedSim3[pKFi]=g2oCorrectedSiw;
}
cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);
cv::Mat tiw = Tiw.rowRange(0,3).col(3);
g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);
// Pose without correction
NonCorrectedSim3[pKFi]=g2oSiw;
}
// Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop
// STEP 2.2：得到调整相连帧位姿后，修正这些关键帧的MapPoints
// 遍历当前关键帧组中的每一个关键帧
for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++)
{
KeyFrame* pKFi = mit->first;
g2o::Sim3 g2oCorrectedSiw = mit->second;
g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi];
vector<MapPoint*> vpMPsi = pKFi->GetMapPointMatches();
// 遍历这个关键帧中的每一个地图点
for(size_t iMP=0, endMPi = vpMPsi.size(); iMP<endMPi; iMP++)
{
MapPoint* pMPi = vpMPsi[iMP];
if(!pMPi)
continue;
if(pMPi->isBad())
continue;
if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId) // 防止重复修正
continue;
// 修正过程开始,本质上也是基于当前关键帧的优化后的位姿展开的
// Project with non-corrected pose and project back with corrected pose
// 将该未校正的eigP3Dw先从世界坐标系映射到未校正的pKFi相机坐标系，然后再反映射到校正后的世界坐标系下
cv::Mat P3Dw = pMPi->GetWorldPos();
Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));
cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
pMPi->SetWorldPos(cvCorrectedP3Dw);
pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
pMPi->mnCorrectedReference = pKFi->mnId;
// 勿忘更新
pMPi->UpdateNormalAndDepth();
}
// Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
// STEP 2.3：将Sim3转换为SE3，根据更新的Sim3，更新关键帧的位姿
// 其实是现在已经有了更新后的关键帧组中关键帧的位姿,但是在上面的操作时只是暂时存储到了 KeyFrameAndPose 类型的变量中,还没有写回到关键帧对象中
Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix(); //调用toRotationMatrix来去除尺度信息的
Eigen::Vector3d eigt = g2oCorrectedSiw.translation(); //目测是其中还包含有尺度信息,还得去除
double s = g2oCorrectedSiw.scale();
eigt *=(1./s); //[R t/s;0 1]
cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);
pKFi->SetPose(correctedTiw);
// Make sure connections are updated
// STEP 2.4：根据共视关系更新当前帧与其它关键帧之间的连接
// 地图点的位置改变了,可能会引起共视关系\权值的改变
pKFi->UpdateConnections();
}
// Start Loop Fusion
// Update matched map points and replace if duplicated
// STEP 3：检查当前帧的MapPoints与闭环匹配帧的MapPoints是否存在冲突，对冲突的MapPoints进行替换或填补
// mvpCurrentMatchedPoints 是刚才当前关键帧和闭环关键帧组的所有地图点进行投影得到的匹配点
for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
{
if(mvpCurrentMatchedPoints[i])
{
MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];
//下标越界问题应该不存在的吧,存储地图点的数组应该是和特征点的数目长度是相同的,即使是mvpCurrentMatchedPoints中也不会有超过特征点个数的地图点啊
MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i);
if(pCurMP)
// 如果有重复的MapPoint（当前帧和匹配帧各有一个），则用匹配帧的代替现有的
// 因为现有的帧往往已经有累计的误差了
pCurMP->Replace(pLoopMP);
else// 如果当前帧没有该MapPoint，则直接添加
{
mpCurrentKF->AddMapPoint(pLoopMP,i);
pLoopMP->AddObservation(mpCurrentKF,i);
pLoopMP->ComputeDistinctiveDescriptors();
}
}
}
}
// Project MapPoints observed in the neighborhood of the loop keyframe
// into the current keyframe and neighbors using corrected poses.
// Fuse duplications.
// STEP 4：通过将闭环时相连关键帧的mvpLoopMapPoints投影到这些关键帧中，进行MapPoints检查与替换
// 当前线程中的函数
SearchAndFuse(CorrectedSim3);
// After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop
// STEP 5：更新当前关键帧之间的共视相连关系，得到因闭环时MapPoints融合而新得到的连接关系
// 这个变量中将会存储那些因为闭环关系的形成,而新形成的链接关系
map<KeyFrame*, set<KeyFrame*> > LoopConnections;
// STEP 5.1：遍历当前帧相连关键帧（一级相连）
for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
{
KeyFrame* pKFi = *vit;
// STEP 5.2：得到与当前帧相连关键帧的相连关键帧（二级相连）
vector<KeyFrame*> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();
// Update connections. Detect new links.
// STEP 5.3：更新一级相连关键帧的连接关系(会把当前关键帧添加进去,因为地图点已经更新和替换了)
pKFi->UpdateConnections();
// STEP 5.4：取出该帧更新后的连接关系
LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames();
// STEP 5.5：从连接关系中去除闭环之前的二级连接关系，剩下的连接就是由闭环得到的连接关系
for(vector<KeyFrame*>::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++)
{
LoopConnections[pKFi].erase(*vit_prev);
}
// STEP 5.6：从连接关系中去除闭环之前的一级连接关系，剩下的连接就是由闭环得到的连接关系
for(vector<KeyFrame*>::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++)
{
LoopConnections[pKFi].erase(*vit2);
}
}
// Optimize graph
// STEP 6：进行EssentialGraph优化，LoopConnections是形成闭环后新生成的连接关系，不包括步骤7中当前帧与闭环匹配帧之间的连接关系
Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
// Add loop edge
// STEP 7：添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）
// 这两句话应该放在OptimizeEssentialGraph之前，因为OptimizeEssentialGraph的步骤4.2中有优化
mpMatchedKF->AddLoopEdge(mpCurrentKF);
mpCurrentKF->AddLoopEdge(mpMatchedKF);
// Launch a new thread to perform Global Bundle Adjustment
// STEP 8：新建一个线程用于全局BA优化
// OptimizeEssentialGraph只是优化了一些主要关键帧的位姿，这里进行全局BA可以全局优化所有位姿和MapPoints
mbRunningGBA = true;
mbFinishedGBA = false;
mbStopGBA = false;
mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);
// Loop closed. Release Local Mapping.
mpLocalMapper->Release();
cout << "Loop Closed!" << endl;
mLastLoopKFid = mpCurrentKF->mnId;
}
// 通过将闭环时相连关键帧的MapPoints投影到当前关键帧组中的这些关键帧中
// 进行MapPoints检查与替换(将回环帧中的地图点代替当前关键帧组中关键帧中的地图点)
// 因为回环关键帧处的时间比较久远,而当前关键帧组中的关键帧的地图点会有累计的误差啊
/**
* @brief 通过将闭环时相连关键帧的MapPoints投影到这些关键帧中，进行MapPoints检查与替换
* @param[in] CorrectedPosesMap 关联的当前帧组中的关键帧和相应的纠正后的位姿
*/
void LoopClosing::SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap)
{
// 定义ORB匹配器
ORBmatcher matcher(0.8);
// 遍历闭环相连的关键帧
for(KeyFrameAndPose::const_iterator mit=CorrectedPosesMap.begin(), mend=CorrectedPosesMap.end(); mit!=mend;mit++)
{
KeyFrame* pKF = mit->first;
g2o::Sim3 g2oScw = mit->second;
cv::Mat cvScw = Converter::toCvMat(g2oScw);
// 将闭环相连帧的MapPoints坐标变换到pKF帧坐标系，然后投影，检查冲突并融合
vector<MapPoint*> vpReplacePoints(mvpLoopMapPoints.size(),static_cast<MapPoint*>(NULL));
// vpReplacePoints中存储的将会是这个关键帧中的地图点（也就是需要替换掉的新的地图点）,原地图点的id则对应这个变量的下标
// 搜索区域系数为4
matcher.Fuse(pKF,cvScw,mvpLoopMapPoints,4,vpReplacePoints);
// Get Map Mutex
// 之所以不在前面的 Fuse 函数中进行地图点融合更新的原因是需要对地图加锁,而这里的设计中matcher中并不保存地图的指针
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
const int nLP = mvpLoopMapPoints.size();
// 遍历闭环帧组的所有的地图点
for(int i=0; i<nLP;i++)
{
MapPoint* pRep = vpReplacePoints[i];
if(pRep)
{
// 用mvpLoopMapPoints替换掉之前的
pRep->Replace(mvpLoopMapPoints[i]);
}
}
}
}
// 由外部线程调用,请求复位当前线程
/** @brief 由外部线程调用,请求复位当前线程.在回环检测复位完成之前,该函数将一直保持堵塞状态 */
void LoopClosing::RequestReset()
{
// 标志置位
{
unique_lock<mutex> lock(mMutexReset);
mbResetRequested = true;
}
// 堵塞,直到回环检测线程复位完成
while(1)
{
{
unique_lock<mutex> lock2(mMutexReset);
if(!mbResetRequested)
break;
}
//usleep(5000);
std::this_thread::sleep_for(std::chrono::milliseconds(5));
}
}
// 当前线程调用,检查是否有外部线程请求复位当前线程,如果有的话就复位回环检测线程
void LoopClosing::ResetIfRequested()
{
unique_lock<mutex> lock(mMutexReset);
// 如果有来自于外部的线程的复位请求,那么就复位当前线程
if(mbResetRequested)
{
mlpLoopKeyFrameQueue.clear(); // 清空参与和进行回环检测的关键帧队列
mLastLoopKFid=0; // 上一次没有和任何关键帧形成闭环关系
mbResetRequested=false; // 复位请求标志复位
}
}
// 全局BA线程,这个是这个线程的主函数; 输入的函数参数看上去是闭环关键帧,但是在调用的时候给的其实是当前关键帧的id
// This function will run in a separate thread
/**
* @brief 全局BA线程,这个函数是这个线程的主函数
* @param[in] nLoopKF 看名字是闭环关键帧,但是实际上给的是当前关键帧的ID
*/
void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)
{
cout << "Starting Global Bundle Adjustment" << endl;
// 记录当前迭代id,用来检查全局BA过程是否是因为意外结束的
int idx = mnFullBAIdx;
// 牛逼,直接调优化器中的函数了
// 10是迭代次数
// mbStopGBA直接传引用过去了,这样当有外部请求的时候这个优化函数能够及时相应并且结束掉
//? 提问:进行完这个过程后我们能够获得哪些信息?
// 目测是能够得到全部关键帧优化后的位姿,以及部分地图点优化之后的位姿
Optimizer::GlobalBundleAdjustemnt(mpMap, // 地图点对象
10, // 迭代次数
&mbStopGBA, // 外界控制 GBA 停止的标志
nLoopKF, // 形成了闭环的当前关键帧的id
false); // 不使用鲁棒核函数
// Update all MapPoints and KeyFrames
// Local Mapping was active during BA, that means that there might be new keyframes
// not included in the Global BA and they are not consistent with the updated map.
// We need to propagate the correction through the spanning tree
{
unique_lock<mutex> lock(mMutexGBA);
// 如果全局BA过程是因为意外结束的,那么后面的内容就都不用管了
if(idx!=mnFullBAIdx)
return;
// 如果没有中断当前次BA的请求
// 这里和上面那句话的功能还有些不同,因为如果一次全局优化被中断,往往意味又要重新开启一个新的全局BA;为了中断当前正在执行的优化过程mbStopGBA将会被置位,同时会有一定的时间
// 使得该线程进行响应;而在开启一个新的全局优化进程之前 mbStopGBA 将会被置为False
// 因此,如果被强行中断的线程退出时已经有新的线程启动了,mbStopGBA=false,为了避免进行后面的程序,所以有了上面的程序;
// 而如果被强行终端的线程退出时新的线程还没有启动,那么上面的条件就不起作用了(虽然概率很小,前面的程序中mbStopGBA置位后很快mnFullBAIdx就++了,保险起见),所以这里要再判断一次
if(!mbStopGBA)
{
cout << "Global Bundle Adjustment finished" << endl;
cout << "Updating map ..." << endl;
mpLocalMapper->RequestStop();
// Wait until Local Mapping has effectively stopped
while(!mpLocalMapper->isStopped() && !mpLocalMapper->isFinished())
{
//usleep(1000);
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
// Get Map Mutex
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
// Correct keyframes starting at map first keyframe
// 看上去是一个向量，但是要知道这个变量中其实只保存了第一个关键帧
list<KeyFrame*> lpKFtoCheck(mpMap->mvpKeyFrameOrigins.begin(),mpMap->mvpKeyFrameOrigins.end());
// 遍历全局地图中的所有关键帧
while(!lpKFtoCheck.empty())
{
KeyFrame* pKF = lpKFtoCheck.front();
const set<KeyFrame*> sChilds = pKF->GetChilds();
cv::Mat Twc = pKF->GetPoseInverse();
// 遍历当前关键帧的子关键帧
for(set<KeyFrame*>::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++)
{
KeyFrame* pChild = *sit;
// 避免重复设置
if(pChild->mnBAGlobalForKF!=nLoopKF)
{
// (对于坐标系中的点的话)从父关键帧到当前子关键帧的位姿变换
cv::Mat Tchildc = pChild->GetPose()*Twc;
// （对于坐标系中的点）再利用优化后的父关键帧的位姿，转换到世界坐标系下 -- //? 算是得到了这个子关键帧的优化后的位姿啦？
// 这种最小生成树中除了根节点，其他的节点都会作为其他关键帧的子节点，这样做可以使得最终所有的关键帧都得到了优化
pChild->mTcwGBA = Tchildc*pKF->mTcwGBA;//*Tcorc*pKF->mTcwGBA;
pChild->mnBAGlobalForKF=nLoopKF;
}
lpKFtoCheck.push_back(pChild);
}
// 更新当前关键帧的位姿
pKF->mTcwBefGBA = pKF->GetPose();
pKF->SetPose(pKF->mTcwGBA);
// 从列表中移除
lpKFtoCheck.pop_front();
}
// Correct MapPoints
const vector<MapPoint*> vpMPs = mpMap->GetAllMapPoints();
// 遍历每一个地图点
for(size_t i=0; i<vpMPs.size(); i++)
{
MapPoint* pMP = vpMPs[i];
if(pMP->isBad())
continue;
// NOTICE 并不是所有的地图点都会直接参与到全局BA优化中,但是大部分的地图点需要根据全局BA优化后的结果来重新纠正自己的位姿
// 如果这个地图点直接参与到了全局BA优化的过程,那么就直接重新设置器位姿即可
if(pMP->mnBAGlobalForKF==nLoopKF)
{
// If optimized by Global BA, just update
pMP->SetWorldPos(pMP->mPosGBA);
}
else // 如故这个地图点并没有直接参与到全局BA优化的过程中,那么就使用器参考关键帧的新位姿来优化自己的位姿
{
// Update according to the correction of its reference keyframe
KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();
// 说明这个关键帧，在前面的过程中也没有因为“当前关键帧”得到全局BA优化
//? 可是,为什么会出现这种情况呢? 难道是因为这个地图点的参考关键帧设置成为了bad?
if(pRefKF->mnBAGlobalForKF!=nLoopKF)
continue;
// Map to non-corrected camera
cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0,3).colRange(0,3);
cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0,3).col(3);
// 转换到其参考关键帧相机坐标系下的坐标
cv::Mat Xc = Rcw*pMP->GetWorldPos()+tcw;
// Backproject using corrected camera
// 然后使用已经纠正过的参考关键帧的位姿,再将该地图点变换到世界坐标系下
cv::Mat Twc = pRefKF->GetPoseInverse();
cv::Mat Rwc = Twc.rowRange(0,3).colRange(0,3);
cv::Mat twc = Twc.rowRange(0,3).col(3);
pMP->SetWorldPos(Rwc*Xc+twc);
}
}
// 释放,使得LocalMapping线程重新开始工作
mpLocalMapper->Release();
cout << "Map updated!" << endl;
}
mbFinishedGBA = true;
mbRunningGBA = false;
} // 更新(几乎)所有的关键帧和地图点
}
// 由外部线程调用,请求终止当前线程
void LoopClosing::RequestFinish()
{
unique_lock<mutex> lock(mMutexFinish);
mbFinishRequested = true;
}
// 当前线程调用,查看是否有外部线程请求当前线程
bool LoopClosing::CheckFinish()
{
unique_lock<mutex> lock(mMutexFinish);
return mbFinishRequested;
}
// 有当前线程调用,执行完成该函数之后线程主函数退出,线程销毁
void LoopClosing::SetFinish()
{
unique_lock<mutex> lock(mMutexFinish);
mbFinished = true;
}
// 由外部线程调用,判断当前回环检测线程是否已经正确终止了
bool LoopClosing::isFinished()
{
unique_lock<mutex> lock(mMutexFinish);
return mbFinished;
}
} //namespace ORB_SLAM