前言 
闭环矫正是闭环中最重要的一个环节，前面是检测闭环，这里用检测的闭环关系对所有关键帧的位姿和地图点进行矫正。
其中包含两个重要的操作分别是： 位姿传播和矫正以及地图点坐标传播和矫正。
1.位姿的传播和矫正
mpCurrentKF:表示当前的关键帧。
pKFi:当前关键帧的某个共视关键帧。
mg2oScw:从世界坐标系到当前关键帧的Sim(3) 变换。它经过了g2o 优化， 我们认为这个是最新的、准确的Sim(3) 变换，需要用它来传播和矫正其他旧的、不准确的关键帧。
Tic:从mpCurrentKF 到pKFi 的SE(3) 位姿变换。
g2oSic: 从mpCurrentKF到pKFi 的Sim(3) 位姿变换。因为它们距离非常近，尺度不会发生明显的变化，所以在g2oSic中尺度因子s=1, 此时g2oSic和位姿变换Tic其实是一样的。
g2oCorrectedSiw:pKFi经过Sim(3) 位姿传播和矫正过的世界坐标系下的Sim(3) 变换。
g2oSiw:pKFi未经过Sim(3) 位姿传播和矫正的世界坐标系下的Sim(3) 变换。其中尺度因子s = 1 。
最后的目的是用mg2oScw传播和矫正pKFi, 这样就能将闭环检测中费力计算的mg2oScw 作用在和当前关键帧连接的关键帧上，即利用图中的红色曲线加上黄色曲线来表示蓝色曲线。
2.地图点坐标传播和矫正
mpCurrentKF:表示当前的关键帧。
pKFi:当前关键帧的某个共视关键帧。
g2oCorrectedSiw:pKFi经过Sim(3) 位姿传播和矫正过的世界坐标系下的Sim(3) 变换。
g2oCorrectedSwi:g2oCorrectedSiw的逆变换。
g2oSiw:pKFi未经过Sim(3) 位姿传播和矫正的世界坐标系下的Sim(3) 变换。
eigP3Dw: pKFi的某个有效的世界坐标系下的地图点坐标。
eigCorrectedP3Dw: eigP3Dw经过位姿g2oCorrectedSiw矫正后的世界坐
标系下的地图点坐标，也是我们最终要得到的坐标。
最后的目的是先用g2oSiw将未矫正的、不准确的世界坐标系下的地图点eigP3Dw变换到pKFi的相机坐标系下，然后利用传播过的、准确的位姿g2oCorrectedSwi将其再变换到世界坐标系下，就得到了eigCorrectedP3Dw即，从世界坐标系到pKFi的带箭头的绿色曲线，再通过带箭头的蓝色曲线回到世界坐标系。
1.函数声明
void LoopClosing::CorrectLoop()2.函数定义
具体流程如下：
1.结束局部建图线程、全局BA, 为闭环矫正做准备。
防止在回环矫正时局部地图线程中InsertKeyFrame函数插入新的关键帧
2.因为之前在闭环检测、计算Sim(3) 时改变了该关键帧的地图点，所以需要根据共视关系更新当前帧与其他关键帧之间的连接关系。
3. 通过前面计算的当前关键帧的Sim(3) 变换进行位姿传播，矫正与当前帧相连的关键帧的位姿和它们的地图点。
4. 检查当前关键帧的地图点与闭环匹配关键帧的地图点是否存在冲突，对冲突的地图点进行替换或填补。
5. 调用SearchAndFuse(CorrectedSim3)函数将闭环相连关键帧组中的所有地图点投影到当前关键帧组中，进行匹配、融合，新增或替换当前关键帧组中关键帧的地图点。
6. 更新当前关键帧的两级共视关键帧的连接关系，得到前面经过地图点融合而新建立的共视连接关系（在更新的基础上去除之前的连接剩下的就是新新建立的关系）。
7. 进行本质图优化，优化本质图中所有关键帧的位姿。
8. 新建一个线程用于全局BA 优化，优化所有的关键帧和地图点。
/*
闭环矫正
1. 通过求解的Sim3以及相对姿态关系，调整与当前帧相连的关键帧位姿以及这些关键帧观测到的地图点位置（相连关键帧---当前帧）
2. 将闭环帧以及闭环帧相连的关键帧的地图点和与当前帧相连的关键帧的点进行匹配（当前帧+相连关键帧---闭环帧+相连关键帧）
3. 通过MapPoints的匹配关系更新这些帧之间的连接关系，即更新covisibility graph
4. 对Essential Graph（Pose Graph）进行优化，MapPoints的位置则根据优化后的位姿做相对应的调整
5. 创建线程进行全局Bundle Adjustment
*/
void LoopClosing::CorrectLoop()
{
cout << "Loop detected!" << endl;
// Send a stop signal to Local Mapping
// Avoid new keyframes are inserted while correcting the loop
// Step 0：结束局部地图线程、全局BA，为闭环矫正做准备
// 请求局部地图停止，防止在回环矫正时局部地图线程中InsertKeyFrame函数插入新的关键帧
mpLocalMapper->RequestStop();
if(isRunningGBA())
{
// 如果有全局BA在运行，终止掉，迎接新的全局BA
unique_lock<mutex> lock(mMutexGBA);
mbStopGBA = true;
// 记录全局BA次数
mnFullBAIdx++;
if(mpThreadGBA)
{
// 停止全局BA线程
mpThreadGBA->detach();
delete mpThreadGBA;
}
}
// Wait until Local Mapping has effectively stopped
// 一直等到局部地图线程结束再继续
while(!mpLocalMapper->isStopped())
{
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
// Ensure current keyframe is updated
// Step 1：根据共视关系更新当前关键帧与其它关键帧之间的连接关系
// 因为之前闭环检测、计算Sim3中改变了该关键帧的地图点，所以需要更新
mpCurrentKF->UpdateConnections();
// Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation
// Step 2：通过位姿传播，得到Sim3优化后，与当前帧相连的关键帧的位姿，以及它们的地图点
// 取出当前关键帧及其共视关键帧，称为“当前关键帧组”
mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
mvpCurrentConnectedKFs.push_back(mpCurrentKF);
// CorrectedSim3：存放闭环g2o优化后当前关键帧的共视关键帧的世界坐标系下Sim3 变换
// NonCorrectedSim3：存放没有矫正的当前关键帧的共视关键帧的世界坐标系下Sim3 变换
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：通过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;
}
// 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;
// 取出经过位姿传播后的Sim3变换
g2o::Sim3 g2oCorrectedSiw = mit->second;
g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
// 取出未经过位姿传播的Sim3变换
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) 内部做了相似变换 s*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();
}
// 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);
// Make sure connections are updated
// Step 2.4：根据共视关系更新当前帧与其它关键帧之间的连接
// 地图点的位置改变了,可能会引起共视关系\权值的改变
pKFi->UpdateConnections();
}
// Start Loop Fusion
// Update matched map points and replace if duplicated
// 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();
}
}
}
}
// Project MapPoints observed in the neighborhood of the loop keyframe
// into the current keyframe and neighbors using corrected poses.
// Fuse duplications.
// Step 4：将闭环相连关键帧组mvpLoopMapPoints 投影到当前关键帧组中，进行匹配，融合，新增或替换当前关键帧组中KF的地图点
// 因为 闭环相连关键帧组mvpLoopMapPoints 在地图中时间比较久经历了多次优化，认为是准确的
// 而当前关键帧组中的关键帧的地图点是最近新计算的，可能有累积误差
// CorrectedSim3：存放矫正后当前关键帧的共视关键帧，及其世界坐标系下Sim3 变换
SearchAndFuse(CorrectedSim3);//同样是在LoopClosing.cc中定义。
// After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop
// Step 5：更新当前关键帧组之间的两级共视相连关系，得到因闭环时地图点融合而新得到的连接关系
// LoopConnections：存储因为闭环地图点调整而新生成的连接关系
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：进行本质图优化，优化本质图中所有关键帧的位姿和地图点
// LoopConnections是形成闭环后新生成的连接关系，不包括步骤7中当前帧与闭环匹配帧之间的连接关系
Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
// Add loop edge
// Step 7：添加当前帧与闭环匹配帧之间的边（这个连接关系不优化）
// 它在下一次的本质图优化里面使用
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;
}
结束语
以上就是我学习到的内容，如果对您有帮助请多多支持我，如果哪里有问题欢迎大家在评论区积极讨论，我看到会及时回复。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
