ORB_SLAM2回环检测
词典是特征点的描述子的集合，属于同一类特征的特征点的描述子组成单词。
在局部建图线程中，处理完一个关键帧后，会将其放入回环检测线程
在使用关键帧数据库搜索候选关键帧组（DetectLoopCandidates）的时候，没有考虑关键帧的连续性。因此在DetectLoop()函数中检测三个连续的闭环候选关键帧，均与当前帧有较高的相似度。
考虑到单目相机的尺度漂移，计算当前帧与候选闭环帧的Sim3变换，而不是T。（1）使用词袋加速算法，找到与当前帧具有足够多的相同单词的候选关键帧集，并初步计算Sim3变换。（2）将候选闭环帧中的路标点投影到当前帧中，通过优化的方法进一步计算Sim3。（3）将闭环帧及其共视帧的路标点投影至当前帧中进行匹配。（4）判断所选闭环帧是否可靠（（3）步中获得的匹配数目大于40）。
闭环校正。计算了当前帧和闭环帧的Sim3变换后，去更新当前帧及其共视帧的位姿，以及路标点的坐标。其中涉及了地图点的融合，共视图的更新，本质图的优化。随后建立了一个全局BA优化线程，去优化所有的关键帧位姿及路标点坐标。
回环检测是指：找到与当前帧4相似的关键帧1，这样的话就可以根据1直接估计4的位姿，而不是1–2--3–4。减少了误差传递。对于当前帧4的共视帧6，可以1—4---6来获得，而不是1–2--3–4--6（也就是回环校正，不过在回环校正的时候，还会去校正路标点坐标）。
void LocalMapping::Run()
............................................
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
检测回环
获得满足连续条件的闭环关键帧，插入到容器mvpEnoughConsistentCandidates中。
虽然使用关键帧数据库，可以获得当前帧的候选闭环关键帧：
vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
但这里检测出的关键帧不满足连续性，这里筛选出那些具有连续性的关键帧。产生闭环的关键帧应具有特点：连续的三个关键帧，且三个关键帧间的相似度评分很高。
bool LoopClosing::DetectLoop()
遍历当前帧的共视关键帧
遍历当前帧的共视关键帧（>15个路标点），使用DBow计算两帧间的词袋相似度。
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;
}
寻找候选的闭环关键帧
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
筛选获得闭环帧
（1）组(group): 对于某个关键帧, 其和其具有共视关系的关键帧组成了一个"组";
（2）子候选组(CandidateGroup): 对于某个候选的回环关键帧, 其和其具有共视关系的关键帧组成的一个"组";
（3）连续(Consistent): 不同的组之间如果共同拥有一个及以上的关键帧,那么称这两个组之间具有连续关系；
（4）连续性(Consistency):称之为连续长度可能更合适,表示累计的连续的链的长度:A–B 为1, A–B--C–D 为3等;具体反映在数据类型 ConsistentGroup.second上；
（5）连续组(Consistent group): mvConsistentGroups存储了上次执行回环检测时, 新的被检测出来的具有连续性的多个组的集合.由于组之间的连续关系是个网状结构,因此可能存在 一个组因为和不同的连续组链都具有连续关系,而被添加两次的情况(当然连续性度量是不相同的)；
（6）连续组链:自造的称呼,类似于菊花链A–B--C–D这样形成了一条连续组链.对于这个例子中,由于可能E,F都和D有连续关系,因此连续组链会产生分叉;为了简化计算,连续组中将只会保存最后形成连续关系的连续组们。(见下面的连续组的更新)
（7）子连续组: 上面的连续组中的一个组；
（8）连续组的初始值: 在遍历某个候选帧的过程中,如果该子候选组没有能够和任何一个上次的子连续组产生连续关系,那么就将添加自己组为连续组,并且连续性为0；
（9）连续组的更新: 当前次回环检测过程中,所有被检测到和之前的连续组链有连续的关系的组,都将在对应的连续组链后面+1,这些子候选组(可能有重复,见上)都将会成为新的连续组; 换而言之连续组mvConsistentGroups中只保存连续组链中末尾的组
（1）ConsistentGroup：连续组遍历，类型为pair
（2）spCandidateGroup：每个闭环关键帧的共视关键帧集。
遍历每一个候选闭环关键帧
（1）找到当前候选闭环关键帧的共视关键帧（构成一个子候选组）
set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
（2）遍历上一次闭环检测到的子连续组
for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++)
（3） 遍历子候选组，判断子候选组中的关键帧是否存在于子连续组中，如果存在，结束循环
for(set<KeyFrame*>::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++)
{
if(sPreviousGroup.count(*sit))
{
// 如果存在，该“子候选组”与该“子连续组”相连
bConsistent=true;
// 该“子候选组”至少与一个”子连续组“相连，跳出循环
bConsistentForSomeGroup=true;
break;
}
}
（4）将该子候选组插入到子连续组中，如果子连续组的长度满足要求，则将子候选关键帧插入候选闭环容器中
（5）如果子候选组中的关键帧在所有的子连续组中都找不到，则创建一个新的子连续组，插入到连续组中
if(!bConsistentForSomeGroup)
{
ConsistentGroup cg = make_pair(spCandidateGroup,0);
vCurrentConsistentGroups.push_back(cg);
}
计算Sim3变换，获得闭环帧
（1）通过Bow加速描述子的匹配，筛选出与当前帧的匹配特征点数大于20的候选帧集合，利用RANSAC粗略地计算出当前帧与闭环帧的Sim3（当前帧—闭环帧）
（2）根据估计的Sim3，将每个候选帧中的路标点投影到当前帧中找到更多匹配，通过优化的方法计算更精确的Sim3（当前帧—闭环帧）。有一个帧成功了，就结束此次的循环。
（3）找到候选帧的共视关键帧，找到所有的路标点，投影到当前帧中进行匹配（当前帧–闭环帧+共视帧）（不进行优化）。
（4）判断候选帧是否可靠（如果（3）步匹配上的路标点的数量大于40，则闭环帧可靠）。
计算当前关键帧和上一步中的闭环候选关键帧的Sim3变换。Sim3变换多了一个尺度变换。
计算SIm3，而不是T的原因就是存在尺度漂移。
/**
* @brief 计算当前关键帧和上一步闭环候选帧的Sim3变换
* 1. 遍历闭环候选帧集，筛选出与当前帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver
* 2. 对每一个候选帧进行 Sim3Solver 迭代匹配，直到有一个候选帧匹配成功，或者全部失败
* 3. 取出闭环匹配上关键帧的相连关键帧，得到它们的MapPoints放入 mvpLoopMapPoints
* 4. 将闭环匹配上关键帧以及相连关键帧的MapPoints投影到当前关键帧进行投影匹配
* 5. 判断当前帧与检测出的所有闭环关键帧是否有足够多的MapPoints匹配
* 6. 清空mvpEnoughConsistentCandidates
* @return true 只要有一个候选关键帧通过Sim3的求解与优化，就返回true
* @return false 所有候选关键帧与当前关键帧都没有有效Sim3变换
*/
bool LoopClosing::ComputeSim3()
注意以上匹配的结果均都存在成员变量mvpCurrentMatchedPoints中，实际的更新步骤见CorrectLoop()步骤3
对于双目或者是RGBD输入的情况,计算得到的尺度=1
遍历上一步中的候选关键帧
如果候选关键帧在局部建图线程中被删除的话，就直接跳过。
// Step 1. 遍历闭环候选帧集，初步筛选出与当前关键帧的匹配特征点数大于20的候选帧集合，并为每一个候选帧构造一个Sim3Solver
for(int i=0; i<nInitialCandidates; i++)
{
// Step 1.1 从筛选的闭环候选帧中取出一帧有效关键帧pKF
KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
// 避免在LocalMapping中KeyFrameCulling函数将此关键帧作为冗余帧剔除
pKF->SetNotErase();
// 如果候选帧质量不高，直接PASS
// 在局部建图线程中，如果一个关键帧被删除，则isBad为true
if(pKF->isBad())
{
vbDiscarded[i] = true;
continue;
}
通过词袋算法获得当前帧与候选帧间匹配上的特征点数量
// Step 1.2 将当前帧 mpCurrentKF 与闭环候选关键帧pKF匹配
// 通过bow加速得到 mpCurrentKF 与 pKF 之间的匹配特征点
// vvpMapPointMatches 是匹配特征点对应的地图点,本质上来自于候选闭环帧
int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
// 粗筛：匹配的特征点数太少，该候选帧剔除
if(nmatches<20)
{
vbDiscarded[i] = true;
continue;
}
else
{
// Step 1.3 为保留的候选帧构造Sim3求解器
// 如果 mbFixScale（是否固定尺度） 为 true，则是6 自由度优化（双目 RGBD）
// 如果是false，则是7 自由度优化（单目）
Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
// Sim3Solver Ransac 过程置信度0.99，至少20个inliers 最多300次迭代
pSolver->SetRansacParameters(0.99,20,300);
vpSim3Solvers[i] = pSolver;
}
// 保留的候选帧数量
nCandidates++;
对每个候选关键帧求解Sim3变换，成功的关键帧进行SearchBySim3
（1）遍历每个候选帧
for(int i=0; i<nInitialCandidates; i++)
（2）迭代计算每个候选帧的Sim3变换，如果达到迭代次数上限后，还没有合格的结果，就删除当前候选帧
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
// 总迭代次数达到最大限制还没有求出合格的Sim3变换，该候选帧剔除
if(bNoMore)
{
vbDiscarded[i]=true;
nCandidates--;
}
（3）如果计算出了Sim3变换，就将当前帧mpCurrentKF和候选关键帧pKF中的路标点相互投影匹配，以匹配上更多的路标点。随后根据匹配关系进行优化。只要有一个优化成功了，就直接结果while循环
if(!Scm.empty())
{
// 取出经过Sim3Solver 后匹配点中的内点集合
vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));
for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
{
// 保存内点
if(vbInliers[j])
vpMapPointMatches[j]=vvpMapPointMatches[i][j];
}
// Step 2.2 通过上面求取的Sim3变换引导关键帧匹配，弥补Step 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中的匹配
// 只有互相都成功匹配的才认为是可靠的匹配
matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
// Step 2.3 用新的匹配来优化 Sim3，只要有一个候选帧通过Sim3的求解与优化，就跳出停止对其它候选帧的判断
// OpenCV的Mat矩阵转成Eigen的Matrix类型
// gScm：候选关键帧到当前帧的Sim3变换
g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
// 如果mbFixScale为true，则是6 自由度优化（双目 RGBD），如果是false，则是7 自由度优化（单目）
// 优化mpCurrentKF与pKF对应的MapPoints间的Sim3，得到优化后的量gScm
const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
// 如果优化成功，则停止while循环遍历闭环候选
if(nInliers>=20)
{
// 为True时将不再进入 while循环
bMatch = true;
// mpMatchedKF就是最终闭环检测出来与当前帧形成闭环的关键帧
mpMatchedKF = pKF;
// gSmw：从世界坐标系 w 到该候选帧 m 的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;
}
}
如果所有候选关键帧经过SIM3变换均失败，则返回false
// 退出上面while循环的原因有两种,一种是求解到了bMatch置位后出的,另外一种是nCandidates耗尽为0
if(!bMatch)
{
// 如果没有一个闭环匹配候选帧通过Sim3的求解与优化
// 清空mvpEnoughConsistentCandidates，这些候选关键帧以后都不会在再参加回环检测过程了
for(int i=0; i<nInitialCandidates; i++)
mvpEnoughConsistentCandidates[i]->SetErase();
// 当前关键帧也将不会再参加回环检测了
mpCurrentKF->SetErase();
// Sim3 计算失败，退出了
return false;
}
取出与当前帧闭环匹配上的关键帧及其共视关键帧，以及这些共视关键帧的地图点
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)
{
// mnLoopPointForKF 用于标记，避免重复添加
if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId)
{
mvpLoopMapPoints.push_back(pMP);
// 标记一下
pMP->mnLoopPointForKF=mpCurrentKF->mnId;
}
}
}
}
将闭环关键帧及其共视关键帧的所有地图点投影到当前关键帧进行投影匹配
matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
判断闭环是否可靠
统计当前帧与候选闭环及其共视帧中匹配上的路标点的数量，小于40的话，认为不可靠。
// Step 4：将闭环关键帧及其共视关键帧的所有地图点投影到当前关键帧进行投影匹配
// 根据投影查找更多的匹配（成功的闭环匹配需要满足足够多的匹配特征点数）
// 根据Sim3变换，将每个mvpLoopMapPoints投影到mpCurrentKF上，搜索新的匹配对
// mvpCurrentMatchedPoints是前面经过SearchBySim3得到的已经匹配的点对，这里就忽略不再匹配了
// 搜索范围系数为10
matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
// If enough matches accept Loop
// Step 5: 统计当前帧与检测出的所有闭环关键帧的匹配地图点数目，超过40个说明成功闭环，否则失败
int nTotalMatches = 0;
for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
{
if(mvpCurrentMatchedPoints[i])
nTotalMatches++;
}
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;
}
闭环校正
闭环关键帧的位姿会根据Sim3进行校正，同时与其相连的关键帧的位姿即路标点的位置也会被校正。
Sim3校正对旋转矩阵没有影响，只会影响平移矩阵。
（1）在上一帧计算当前帧和闭环帧的Sim3位姿变换时，建立了闭环帧及其共视帧的路标点与当前帧的联系，因此先更新共视图。
（2）根据计算的当前帧和闭环帧的Sim3变换，去更新当前帧及其共视帧的位姿，以及路标点的坐标。
（3）因为闭环帧已经经过了多次优化，认为是精确的，因此建立闭环帧及其共视帧的路标点与当前帧及其共视帧的联系，进行路标点的匹配、融合。
（4）优化本质图（只优化位姿）
（5）建立一个全局BA优化线程
结束局部地图线程、全局BA，为闭环校正做准备
mpLocalMapper->RequestStop();
if(isRunningGBA())
{
// 如果有全局BA在运行，终止掉，迎接新的全局BA
unique_lock<mutex> lock(mMutexGBA);
mbStopGBA = true;
// 记录全局BA次数
mnFullBAIdx++;
if(mpThreadGBA)
{
// 停止全局BA线程
mpThreadGBA->detach();
delete mpThreadGBA;
}
}
更新共视图
在闭环检测、计算Sim3的过程中，建立了当前帧的特征点和其闭环帧、闭环帧的共视帧的路标点间的联系，因此这里需要更新一个共视图。
// Step 1：根据共视关系更新当前关键帧与其它关键帧之间的连接关系
// 因为之前闭环检测、计算Sim3中改变了该关键帧的地图点，所以需要更新
mpCurrentKF->UpdateConnections();
更新当前帧、与其相连的关键帧的位姿。
当前帧与世界坐标系之间的Sim变换在ComputeSim3函数中已经确定并优化，通过相对位姿关系，可以确定与当前帧相连的关键帧与世界坐标系之间的Sim3变换
（1）通过mg2oScw（认为是准的）来进行位姿传播，得到当前关键帧的共视关键帧的世界坐标系下Sim3 位姿.遍历"当前关键帧组"（当前帧+共视帧）
// Step 2.1：通过mg2oScw（认为是准的）来进行位姿传播，得到当前关键帧的共视关键帧的世界坐标系下Sim3 位姿（还没有修正）
// 遍历"当前关键帧组""
for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
{
KeyFrame* pKFi = *vit;
cv::Mat Tiw = pKFi->GetPose();
if(pKFi!=mpCurrentKF) //跳过当前关键帧，因为当前关键帧的位姿已经在前面优化过了，在这里是参考基准
{
// 得到当前关键帧 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);
// g2oSic：当前关键帧 mpCurrentKF 到其共视关键帧 pKFi 的Sim3 相对变换
// 这里是non-correct, 所以scale=1.0
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优化后当前关键帧的共视关键帧的Sim3 位姿
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
// 存放没有矫正的当前关键帧的共视关键帧的Sim3变换
NonCorrectedSim3[pKFi]=g2oSiw;
}
（2）校正当前帧的共视关键帧的路标点坐标
路标点世界坐标------（未校正的T）--------路标点相机坐标-------（校正的Sim3）--------路标点世界坐标
保持路标点和帧间的相对位置不变。
要记得更新地图点的平均观测方向和观测范围
// Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop
// Step 2.2：得到矫正的当前关键帧的共视关键帧位姿后，修正这些关键帧的地图点
// 遍历待矫正的共视关键帧
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);
// map(P) 内部做了变换 R*P +t
// 下面变换是：eigP3Dw： world →g2oSiw→ i →g2oCorrectedSwi→ world
Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));
cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
pMPi->SetWorldPos(cvCorrectedP3Dw);
// 记录矫正该地图点的关键帧id，防止重复
pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
// 记录该地图点所在的关键帧id
pMPi->mnCorrectedReference = pKFi->mnId;
// 因为地图点更新了，需要更新其平均观测方向以及观测距离范围
pMPi->UpdateNormalAndDepth();
}
（3）将共视关键帧的Sim3转换为SE3，根据更新的Sim3，更新关键帧的位姿
// Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
// Step 2.3：将共视关键帧的Sim3转换为SE3，根据更新的Sim3，更新关键帧的位姿
// 其实是现在已经有了更新后的关键帧组中关键帧的位姿,但是在上面的操作时只是暂时存储到了 KeyFrameAndPose 类型的变量中,还没有写回到关键帧对象中
// 调用toRotationMatrix 可以自动归一化旋转矩阵
Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
double s = g2oCorrectedSiw.scale();
// 平移向量中包含有尺度信息，还需要用尺度归一化
eigt *=(1./s);
cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);
// 设置矫正后的新的pose
pKFi->SetPose(correctedTiw);
更新当前帧路标点
更新当前帧中的路标点，应为在ComputeSim3()函数获取闭环帧的时候，将闭环帧及其共视帧的路标点和当前帧的特征点进行了匹配
// Step 3：检查当前帧的地图点与经过闭环匹配后该帧的地图点是否存在冲突，对冲突的进行替换或填补
// mvpCurrentMatchedPoints 是当前关键帧和闭环关键帧组的所有地图点进行投影得到的匹配点
for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
{
if(mvpCurrentMatchedPoints[i])
{
//取出同一个索引对应的两种地图点，决定是否要替换
// 匹配投影得到的地图点
MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];
// 原来的地图点
MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i);
if(pCurMP)
// 如果有重复的MapPoint，则用匹配的地图点代替现有的
// 因为匹配的地图点是经过一系列操作后比较精确的，现有的地图点很可能有累计误差
pCurMP->Replace(pLoopMP);
else
{
// 如果当前帧没有该MapPoint，则直接添加
mpCurrentKF->AddMapPoint(pLoopMP,i);
pLoopMP->AddObservation(mpCurrentKF,i);
pLoopMP->ComputeDistinctiveDescriptors();
}
}
}
}
路标点融合
在局部建图的时候，已经获得了当前帧及其共视帧所联系的路标点。这里又已知闭环帧及其共视帧的路标点。闭环帧出现在当前帧之前，进行了多次优化，因此其路标点准确。所以将闭环帧及其共视帧的路标点投影到当前帧组中进行匹配，融合。
SearchAndFuse(CorrectedSim3);
更新当前帧的连接关系
前面进行了路标点的融合，这里要更改一下连接关系。
map
KeyFrame：当前帧及其共视关键帧中的一帧
set
优化共视图
Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
添加当前帧与闭环匹配帧之间的边
mpMatchedKF->AddLoopEdge(mpCurrentKF);
mpCurrentKF->AddLoopEdge(mpMatchedKF);
建立一个线程进行全局BA优化
mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);
全局BA优化线程
优化所有的关键帧及路标点
void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)