闭环修正
在上一节中，我们计算了当前帧与候选闭环帧之间的相似变换 Sim3，并根据Sim3的几何约束进一步的进行地图点匹配， 最终选出了一个闭环关键帧。现在需要做的事情就是，根据选出的闭环关键帧，以及估计的相似变换关系，修正当前帧的位姿，消除系统的累积误差。 这主要涉及到地图点融合和基于 Essential Graph 的闭环优化。
在本文中，我们先总体查看一下整个闭环修正的流程，再详细解析地图点融合和 EssentialGraph 优化的代码。
1. 总体过程
闭环探测器 LoopClosing 在其成员函数 CorrectLoop 中完成闭环修正的工作。总体上，其运行过程可以分为几个部分：
- 准备工作：通知 LOCAL_MAPPING 线程在闭环修正期间不要插入新的关键帧；终止当前正在进行的全局 BA 优化；
- 传播关键帧位姿修正量：根据闭环帧的位姿以及闭环帧到当前帧的相似变换，计算修正后的当前帧位姿，并传播给邻接关键帧；
- 更新地图点位置：根据当前帧共视组的修正位姿，计算它们观测到的地图点的世界坐标；
- 更新共视图的连接关系：更新当前帧共视组中地图点的可视关系，融合重复的地图点，建立闭环约束；
- 基于 Essential Graph 的优化；
- 开启全局优化的线程。
void LoopClosing::CorrectLoop() {
// 省略准备工作相关代码
mpCurrentKF->UpdateConnections();
mvpCurrentConnectedKFs=mpCurrentKF->GetVectorCovisibleKeyFrames();
mvpCurrentConnectedKFs.push_back(mpCurrentKF);
在右侧的代码片段中，我们省略了准备工作相关的代码。通过关键帧的接口函数 UpdateConnections 可以更新关键帧的地图点、邻接关键帧及其连接权重。 连接权重是指两个关键帧共视的地图点数量。通过接口 GetVectorCovisibleKeyFrames 可以获取按照权重排序的邻接关键帧，再将当前帧自身添加进来，就得到一个当前帧共视组， 用成员容器 mvpCurrentConnectedKFs 保存。下面的代码片段构建了两个局部的 map 容器 CorrectedSim3 和 NonCorrectedSim3，分别用于保存当前帧共视组中各个关键帧修正后和未修正的相似变换。 类型 KeyFrameAndPose 是在 头文件中定义的 std::map 容器。 与我们常用的 std::map 方式不同，这里给出了四个类型参数，依次描述了映射表的键、值的数据类型，键的比较函数，以及针对 Eigen 字节对齐的内存申请方法。 第10行，用局部变量 Twc 记录下了从当前帧到世界的位姿关系。
// typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
// Eigen::aligned_allocator<std::pair<KeyFrame *const, g2o::Sim3>>> KeyFrameAndPose;
KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
CorrectedSim3[mpCurrentKF]=mg2oScw;
cv::Mat Twc = mpCurrentKF->GetPoseInverse();
下面的代码片段，在一个局部的语句块中，通过 for 循环遍历了当前帧共视组中的所有关键帧。在遍历之前，先对地图对象的互斥量 mMutexMapUpdate 加锁，防止在修正过程中修改了地图信息。 第15行的局部变量 Tiw 是世界到关键帧的位姿关系，在第17行将它左乘到 Twc 就得到了当前帧到该关键帧的位姿关系， 再在第21行乘上上一节中得到的当前帧在世界的修正位姿，就得到了目标关键帧修正后的位姿。
{
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
for(auto vit=mvpCurrentConnectedKFs.begin(); vit != mvpCurrentConnectedKFs.end(); vit++) {
KeyFrame* pKFi = *vit;
cv::Mat Tiw = pKFi->GetPose();
if(pKFi!=mpCurrentKF) {
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);
g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;
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);
NonCorrectedSim3[pKFi]=g2oSiw;
}
接下来在下面的两级 for 循环中，遍历当前帧共视组观测到的所有地图点，根据关键帧的修正位姿计算地图点的三维坐标。 其中第 40 行的判定条件和 49 行对地图点的成员变量 mnCorrectedByKF 的赋值，主要是为了防止重复修改地图点的坐标，因为同一个地图点可能被多个关键帧观测到。 第43-45行就是用来计算地图点坐标的，并在47到51行完成地图点的更新。在53-61行中完成了关键帧的更新。
for(auto mit = CorrectedSim3.begin(); mit != CorrectedSim3.end(); 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; iMP < vpMPsi.size(); iMP++) {
MapPoint* pMPi = vpMPsi[iMP];
if (!pMPi || pMPi->isBad())
continue;
if(pMPi->mnCorrectedByKF == mpCurrentKF->mnId)
continue;
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();
}
Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().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);
pKFi->UpdateConnections();
}
在进行地图点融合之前，先在一个 for 循环中，把上一节中与闭环帧及其邻接关键帧匹配的地图点添加到当前帧中(如69-71行所示)。 如果当前帧已经有地图点，则用闭环匹配的地图点替换(如67行所示)。之所以要替换，是因为当前帧累积误差的影响，其地图点的位置估计不准确，而闭环匹配的地图点可能经历了多轮优化，精度较高。
for(size_t i=0; i < mvpCurrentMatchedPoints.size(); i++) {
if(mvpCurrentMatchedPoints[i]) {
MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];
MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i);
if(pCurMP)
pCurMP->Replace(pLoopMP);
else {
mpCurrentKF->AddMapPoint(pLoopMP,i);
pLoopMP->AddObservation(mpCurrentKF,i);
pLoopMP->ComputeDistinctiveDescriptors();
} } } }
SearchAndFuse(CorrectedSim3);
此时，已经在共视图中建立了闭环帧与当前帧的邻接关系。调用成员函数 SearchAndFuse 完成地图点的匹配融合之后，共视图的连接关系将发生较大变化。 在下面的 for 循环中更新当前帧共视组中各个关键帧的链接，同时将因为闭环新增的链接保存到局部的容器 LoopConnections 中。
map<KeyFrame*, set<KeyFrame*>> LoopConnections;
for(auto vit = mvpCurrentConnectedKFs.begin(); vit != mvpCurrentConnectedKFs.end(); vit++) {
KeyFrame* pKFi = *vit;
vector<KeyFrame*> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();
pKFi->UpdateConnections();
LoopConnections[pKFi] = pKFi->GetConnectedKeyFrames();
for(auto vit_prev=vpPreviousNeighbors.begin(); vit_prev != vpPreviousNeighbors.end(); vit_prev++)
LoopConnections[pKFi].erase(*vit_prev);
for(auto vit2=mvpCurrentConnectedKFs.begin(); vit2 != mvpCurrentConnectedKFs.end(); vit2++)
LoopConnections[pKFi].erase(*vit2);
}
上面代码片段的基本逻辑就是，在更新关键帧的邻接关系之前，先用局部容器 vpPreviousNeighbors 记录下原来的连边关系。 调用接口 UpdateConnections 之后，获取新的连边关系，然后在81-84行中，通过两个二级for循环从中剔除掉 vpPreviousNeighbors 和 mvpCurrentConnectedKFs 中的连边关系， 剩下的就是因为闭环新增的连边。
现在，现在我们已经建立了闭环关系，可以调用优化函数 Optimizer::OptimizeEssentialGraph 进行基于 Essential Graph 的优化。最后，开启全局 BA 线程。
Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF,
NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
mpMap->InformNewBigChange();
mpMatchedKF->AddLoopEdge(mpCurrentKF);
mpCurrentKF->AddLoopEdge(mpMatchedKF);
mbRunningGBA = true;
mbFinishedGBA = false;
mbStopGBA = false;
mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);
mpLocalMapper->Release();
mLastLoopKFid = mpCurrentKF-&t;mnId;
}
2. 融合地图点
在上一小节中，我们看到函数 CorrectLoop() 通过调用 SearchAndFuse() 完成地图点的匹配搜索和融合。下面是该函数的实现，它有一个传参， 输入的是当前帧共视组以及其中各个关键帧修正后的位姿，CorrectedSim。
void LoopClosing::SearchAndFuse(const KeyFrameAndPose & CorrectedPosesMap) {
ORBmatcher matcher(0.8);
for(auto mit = CorrectedPosesMap.begin(); mit != CorrectedPosesMap.end(); mit++) {
KeyFrame* pKF = mit->first;
g2o::Sim3 g2oScw = mit->second;
cv::Mat cvScw = Converter::toCvMat(g2oScw);
vector<MapPoint*> vpReplacePoints(mvpLoopMapPoints.size(), static_cast<MapPoint*>(NULL));
matcher.Fuse(pKF,cvScw,mvpLoopMapPoints,4,vpReplacePoints);
unique_lock lock(mpMap->mMutexMapUpdate);
const int nLP = mvpLoopMapPoints.size();
for(int i = 0; i < nLP;i++) {
MapPoint* pRep = vpReplacePoints[i];
if(pRep)
pRep->Replace(mvpLoopMapPoints[i]);
}
} }
SearchAndFuse() 在一个 for 循环中遍历共视组中各个关键帧。出于多线程的考虑，作者在第7行构建了一个局部的容器 vpReplacePoints 用于记录需要替换的融合地图点， 第8行调用 ORBmatcher 的 Fuse() 接口完成具体的地图点融合工作之后，对地图对象的互斥量 mMutexMapUpdate 加锁，然后完成替换，使融合地图点生效。下面我们来看一下 ORBmatcher::Fuse() 的实现。
int ORBmatcher::Fuse(KeyFrame *pKF, cv::Mat Scw,
const vector<MapPoint*> &vpPoints,
float th,
vector<MapPoint*> &vpReplacePoint) {
右侧是该函数的代码片段，它有 5 个参数。其中，pKF 对应的传参是 CorrectedSim 中的各个关键帧指针，Scw 则是修正后的关键帧相似变换矩阵， vpPoints 传入的是上一节中记录的闭环帧及其邻接关键帧中地图点 mvpLoopMapPoints。 阈值 th 用于控制搜索范围， 类似的套路我们曾在匀速运动模型估计相机位姿中见过。 最后的参数 vpReplacePoint 是该函数的输出，它与 mvpLoopMapPoints 一一对应，记录的是融合后的地图点。函数的一开始先进行了一些准备工作，定义了一些局部变量。 左侧的代码片段从关键帧中获取了相机的内参，右侧的代码片段则从输入参数 Scw 中分解出了相似变换中的尺度因子 scw、旋转矩阵 Rcw、平移向量 tcw，并推算出相机的世界坐标 Ow。
|
|
接下来，在一个 for 循环中遍历输入参数 vpPoints 中记录的所有闭环地图点。下面代码片段中定义的局部容器 spAlreadyFound 记录了考察关键帧中已有的地图点，nFused 用于统计融合的地图点数量。
const set<MapPoint*> spAlreadyFound = pKF->GetMapPoints();
int nFused=0; const int nPoints = vpPoints.size();
for (int iMP = 0; iMP < nPoints; iMP++) {
MapPoint* pMP = vpPoints[iMP];
if (pMP->isBad() || spAlreadyFound.count(pMP))
continue;
cv::Mat p3Dw = pMP->GetWorldPos();
cv::Mat p3Dc = Rcw*p3Dw+tcw;
if (p3Dc.at<float>(2) < 0.0f)
continue;
const float invz = 1.0 / p3Dc.at<float>(2);
const float x = p3Dc.at<float>(0) * invz;
const float y = p3Dc.at<float>(1) * invz;
const float u = fx * x + cx;
const float v = fy * y + cy;
if (!pKF->IsInImage(u,v))
continue;
如左侧的代码片段所示，对于每一个地图点，我们在第 18 行中判定，如果地图点是否被标记为失效，或者考察关键帧已经观测到该地图点了，就不需要在该关键帧下进行融合了。
我们在其中的第 20 行获取地图点的世界坐标 p3Dw，然后在第 21 行转换到考察关键帧的坐标系下得到 p3Dc。 如果投影到相机坐标系下后，z轴的坐标为负数，说明该地图点在相机背后，不可能被考察关键帧观测到。遇到这种情况，就在第22行中跳过了。
相机坐标系下的 z 轴实际就是地图点在考察关键帧中的深度值。接下来的第24-28行中，根据深度值以及相机的内参，将 p3Dc 转换到像素坐标系下。 通过关键帧的接口 IsInImage() 判定地图点是否会出现在图像中，并跳过那些不可能被观测到的地图点。
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
cv::Mat PO = p3Dw-Ow;
const float dist3D = cv::norm(PO);
if (dist3D < minDistance || dist3D > maxDistance)
continue;
ORB-SLAM2 通过图像金字塔计算特征点，不同层级的金字塔对应不同的图像尺度。所以对于每个地图点，都有一个适用的尺度范围，左侧的代码中通过接口 GetMaxDistanceInvariance 和 GetMinDistanceInvariance 获取尺度的适用范围。第33,34行中计算了地图点到相机的距离，并在35，36行中剔除掉那些不在尺度适用范围的地图点。
cv::Mat Pn = pMP->GetNormal();
if (PO.dot(Pn) < 0.5 * dist3D) continue;
通过相机到地图点的矢量 PO 与地图点法向量的点乘以得到两个矢量夹角的余弦值，而\(\cos 60° = 0.5\)。所以左侧代码是在判定关键帧观测到地图点的视角不超过 60°。
int nPredictedLevel = pMP->PredictScale(dist3D,pKF);
const float radius = th*pKF->mvScaleFactors[nPredictedLevel];
const vector<size_t> vIndices = pKF->GetFeaturesInArea(u,v,radius);
if(vIndices.empty())
continue;
通过对地图点的层层筛选之后，根据输入参数 th 以及从地图点接口 PredictScale 获取的预测尺度，计算得到一个搜索半径 radius。 关键帧的接口 GetFeaturesInArea 将以投影像素坐标 u,v 为中心像素半径小于 radius 的所有特征点到容器 vIndices 中。
接下来，在一个 for 循环中遍历 vIndices 中记录的特征点，统计其中的最佳匹配。下面的代码片段中，局部变量 dMP 是当前正在考察的地图点描述子。 bestDist 和 bestIdx 分别记录了最佳匹配点的描述子与 dMP 的距离，以及最佳匹配点在考察关键帧中的索引。
const cv::Mat dMP = pMP->GetDescriptor();
int bestDist = 256; int bestIdx = -1;
for(auto vit = vIndices.begin(); vit != vIndices.end(); vit++) {
const size_t idx = *vit;
const cv::KeyPoint &kp = pKF->mvKeysUn[idx];
const int &kpLevel= kp.octave;
if (kpLevel < nPredictedLevel-1 || kpLevel > nPredictedLevel)
continue;
if (pKF->mvuRight[idx] >=0) {
// 针对双目和深度相机的处理
const float &kpx = kp.pt.x;
const float &kpy = kp.pt.y;
const float &kpr = pKF->mvuRight[idx];
const float ex = u-kpx;
const float ey = v-kpy;
const float er = ur-kpr;
const float e2 = ex*ex+ey*ey+er*er;
if (e2*pKF->mvInvLevelSigma2[kpLevel] > 7.8)
continue;
} else {
// 针对单目相机的处理
const float &kpx = kp.pt.x;
const float &kpy = kp.pt.y;
const float ex = u-kpx;
const float ey = v-kpy;
const float e2 = ex*ex+ey*ey;
if (e2*pKF->mvInvLevelSigma2[kpLevel] > 5.99)
continue;
}
const cv::Mat &dKF = pKF->mDescriptors.row(idx);
const int dist = DescriptorDistance(dMP,dKF);
if (dist < bestDist) {
bestDist = dist;
bestIdx = idx;
}
}
最后，我们再次检查最佳匹配的距离，如果小于指定阈值(50)，则认定关键帧中第 bestIdx 特征点与地图点实际上是同一个，执行融合操作。如果最佳匹配的特征点已经被分配了地图点， 就将当前考察的地图点记录到输出容器 vpReplacePoint 中。若对应特征点尚未分配地图点，则直接分配给考察关键帧。
if (bestDist <= TH_LOW) {
MapPoint* pMPinKF = pKF->GetMapPoint(bestIdx);
if (pMPinKF) {
if (!pMPinKF->isBad())
vpReplacePoint[iMP] = pMPinKF;
} else {
pMP->AddObservation(pKF,bestIdx);
pKF->AddMapPoint(pMP,bestIdx);
}
nFused++;
} }
return nFused;
}
3. 基于 Essential Graph 的优化
基图(EssentialGraph)是ORB-SLAM2中比共视图更为稀疏的描述关键帧链接关系的图，它只为那些共视地图点数量超过 100 个的关键帧建立共视关系。如果一个关键帧很差劲， 跟谁都没有100个共视点，就只记录下共视点最多的那个共视关系。下面是完成具有优化任务的函数 Optimizer::OptimizeEssentialGraph 的实现，它的输入参数有点多:
- pMap: 输入的是 LoopClosing 的成员变量 mpMap，全局的地图对象；
- pLoopKF: 输入的是 LoopClosing 的成员变量 mpMatchedKF，闭环关键帧；
- pCurKF: 输入的是 LoopClosing 的成员变量 mpCurrentKF，当前关键帧；
- NonCorrectedSim3: 输入的是函数 CorrectLoop() 中定义的局部变量 NonCorrectedSim3，当前帧共视组及其修正之前的相似变换；
- CorrectedSim3: 输入的是函数 CorrectLoop() 中定义的局部变量 CorrectedSim3，当前帧共视组及其修正之后的相似变换；
- LoopConnections: 输入的是函数 CorrectLoop() 中定义的局部容器 LoopConnections，记录了各个关键帧因为闭环新增的邻接关系；
- bFixScale: 表示是否需要优化尺度，对于双目和深度相机而言为 false，对于单目相机则是 true。
void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
const LoopClosing::KeyFrameAndPose &CorrectedSim3,
const map<KeyFrame *, set<KeyFrame *>> &LoopConnections, const bool &bFixScale)
跟其他位姿图优化过程[ 1, 2]一样，基图的位姿优化也是通过 g2o 完成的，首先构造优化器和求解器对象。 所不同的是这里需要估计的是有7个自由度的相似变换，所以其求解器类型是 BlockSolver_7_3，其中 7 是指待优化的相似变换有7个自由度，3 则是观测量特征点的空间坐标的三个自由度。
g2o::SparseOptimizer optimizer;
optimizer.setVerbose(false);
g2o::BlockSolver_7_3::LinearSolverType * linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_7_3::PoseMatrixType>();
g2o::BlockSolver_7_3 * solver_ptr= new g2o::BlockSolver_7_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
solver->setUserLambdaInit(1e-16);
optimizer.setAlgorithm(solver);
紧接着从地图对象中获取所有的关键帧和地图点对象保存到局部容器 vpKFs 和 vpMaps 中。容器 vScw 用于记录闭环修正之后、优化之前的相似变换关系。 vCorrectedSwc 则用于记录本次优化之后各个关键帧的相似变换。局部容器 vpVertices 则记录了根据这些相似变换构建的 Vertex。minFeat 表示在基图中两个关键帧之间最少的共视地图点。
const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
const vector<MapPoint*> vpMPs = pMap->GetAllMapPoints();
const unsigned int nMaxKFid = pMap->GetMaxKFid();
vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3>> vScw(nMaxKFid+1);
vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3>> vCorrectedSwc(nMaxKFid+1);
vector<g2o::VertexSim3Expmap*> vpVertices(nMaxKFid+1);
const int minFeat = 100;
接下来，在一个for循环中构建所有关键帧的相似变换对象 VSim3，如果在地图点融合的过程中调整过关键帧的位姿，那么就优先使用调整后的位姿， 否则以原有的位姿变换矩阵构建一个尺度为1的相似变换。将该相似变换结果作为优化初值添加到优化问题对象 optimizer 中。 需要强调一点的是，在下面的代码片段的第 33 行中，把闭环帧的相似变换标记为固定值，不参与优化。
for (size_t i=0; i < vpKFs.size(); i++) {
for (size_t i=0; i < vpKFs.size(); i++) {
KeyFrame* pKF = vpKFs[i];
if (pKF->isBad()) continue;
g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();
if (it != CorrectedSim3.end()) {
// 优先使用调整后的相似变换
vScw[nIDi] = it->second;
VSim3->setEstimate(it->second);
} else {
Eigen::Matrix<double,3,3> Rcw = Converter::toMatrix3d(pKF->GetRotation());
Eigen::Matrix<double,3,1> tcw = Converter::toVector3d(pKF->GetTranslation());
g2o::Sim3 Siw(Rcw,tcw,1.0);
vScw[nIDi] = Siw;
VSim3->setEstimate(Siw);
}
if (pKF == pLoopKF) VSim3->setFixed(true);
VSim3->setId(nIDi);
VSim3->setMarginalized(false);
VSim3->_fix_scale = bFixScale;
optimizer.addVertex(VSim3);
vpVertices[nIDi]=VSim3;
}
然后构建闭环边。在下面的代码片段中，构建了一个局部容器 sInsertedEdges 用于记录添加的闭环边。每个闭环边都是一个二元组，以连接的端点中 ID 较小者为二元组的第一个元素。 matLambda 是一个 7x7 的单位矩阵，他将在下面的 for 循环中作为信息矩阵的初值赋给各个连边的 information。
set<pair<long unsigned int,long unsigned int>> sInsertedEdges;
const Eigen::Matrix<double,7,7> matLambda = Eigen::Matrix<double,7,7>::Identity();
for (auto mit = LoopConnections.begin(); mit != LoopConnections.end(); mit++) {
KeyFrame* pKF = mit->first;
const long unsigned int nIDi = pKF->mnId;
const set<KeyFrame*> &spConnections = mit->second;
const g2o::Sim3 Siw = vScw[nIDi];
const g2o::Sim3 Swi = Siw.inverse();
for(auto sit=spConnections.begin(); sit != spConnections.end(); sit++) {
const long unsigned int nIDj = (*sit)->mnId;
if ((nIDi != pCurKF->mnId || nIDj != pLoopKF->mnId) && pKF->GetWeight(*sit) < minFeat)
continue;
const g2o::Sim3 Sjw = vScw[nIDj];
const g2o::Sim3 Sji = Sjw * Swi;
g2o::EdgeSim3* e = new g2o::EdgeSim3();
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
e->setMeasurement(Sji);
e->information() = matLambda;
optimizer.addEdge(e);
sInsertedEdges.insert(make_pair(min(nIDi,nIDj),max(nIDi,nIDj)));
}
}
紧接着根据关键帧的共视关系建立其它连边。主要包含三类：生成树描述的边、历史的闭环边、共视关系的边。在下面的 for 循环中，遍历所有的关键帧，用局部变量 Swi 记录各个关键帧在世界坐标系下的逆变换， 方便后续计算各个连边所描述的帧间相似变换关系。
for (size_t i = 0; i < vpKFs.size(); i++) {
KeyFrame* pKF = vpKFs[i]; const int nIDi = pKF->mnId; g2o::Sim3 Swi;
LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF);
if (iti != NonCorrectedSim3.end())
Swi = (iti->second).inverse();
else
Swi = vScw[nIDi].inverse();
KeyFrame* pParentKF = pKF->GetParent();
if(pParentKF) {
int nIDj = pParentKF->mnId;
g2o::Sim3 Sjw;
LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF);
if (itj != NonCorrectedSim3.end())
Sjw = itj->second;
else
Sjw = vScw[nIDj];
g2o::Sim3 Sji = Sjw * Swi;
g2o::EdgeSim3* e = new g2o::EdgeSim3();
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
e->setMeasurement(Sji);
e->information() = matLambda;
optimizer.addEdge(e);
}
const set<KeyFrame*> sLoopEdges = pKF->GetLoopEdges();
for(auto sit=sLoopEdges.begin(); sit != sLoopEdges.end(); sit++) {
KeyFrame* pLKF = *sit;
if (pLKF->mnId < pKF->mnId) {
g2o::Sim3 Slw;
LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF);
if (itl!=NonCorrectedSim3.end())
Slw = itl->second;
else
Slw = vScw[pLKF->mnId];
g2o::Sim3 Sli = Slw * Swi;
g2o::EdgeSim3* el = new g2o::EdgeSim3();
el->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pLKF->mnId)));
el->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
el->setMeasurement(Sli);
el->information() = matLambda;
optimizer.addEdge(el);
}
}
const vector<KeyFrame*> vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat);
for(auto vit=vpConnectedKFs.begin(); vit != vpConnectedKFs.end(); vit++) {
KeyFrame* pKFn = *vit;
if(pKFn && pKFn != pParentKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn)) {
if (!pKFn->isBad() && pKFn->mnId < pKF->mnId) {
if(sInsertedEdges.count(make_pair(min(pKF->mnId, pKFn->mnId), max(pKF->mnId, pKFn->mnId))))
continue;
g2o::Sim3 Snw;
LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn);
if (itn != NonCorrectedSim3.end())
Snw = itn->second;
else
Snw = vScw[pKFn->mnId];
g2o::Sim3 Sni = Snw * Swi;
g2o::EdgeSim3* en = new g2o::EdgeSim3();
en->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFn->mnId)));
en->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
en->setMeasurement(Sni);
en->information() = matLambda;
optimizer.addEdge(en);
}
}
}
}
从上述构建连边的代码中，我们可以看到基图的图优化相比于之前的优化套路[ 1, 2]而言，有另一个不同之处，就是它以帧间关系作为观测量，地图点的位置并没有参与优化。 紧接着就是通过优化器，进行最多20轮的迭代。
optimizer.initializeOptimization();
optimizer.optimize(20);
优化结束之后，就对地图对象加锁，完成地图更新。
unique_lock<mutex> lock(pMap->mMutexMapUpdate);
// SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]
for (size_t i = 0; i < vpKFs.size(); i++) {
KeyFrame* pKFi = vpKFs[i];
const int nIDi = pKFi->mnId;
g2o::VertexSim3Expmap* VSim3 = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(nIDi));
g2o::Sim3 CorrectedSiw = VSim3->estimate();
vCorrectedSwc[nIDi] = CorrectedSiw.inverse();
Eigen::Matrix3d eigR = CorrectedSiw.rotation().toRotationMatrix();
Eigen::Vector3d eigt = CorrectedSiw.translation();
double s = CorrectedSiw.scale();
eigt *=(1./s); //[R t/s;0 1]
cv::Mat Tiw = Converter::toCvSE3(eigR,eigt);
pKFi->SetPose(Tiw);
}
值得关注的是最后这个for循环中，根据各个地图点的参考关键帧的位姿给出了各个地图点优化后的 3D 坐标。
// Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose
for (size_t i=0; i < vpMPs.size(); i++) {
MapPoint* pMP = vpMPs[i];
if(pMP->isBad()) continue;
int nIDr;
if (pMP->mnCorrectedByKF == pCurKF->mnId) {
nIDr = pMP->mnCorrectedReference;
} else {
KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();
nIDr = pRefKF->mnId;
}
g2o::Sim3 Srw = vScw[nIDr];
g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr];
cv::Mat P3Dw = pMP->GetWorldPos();
Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
Eigen::Matrix<double,3,1> eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw));
cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
pMP->SetWorldPos(cvCorrectedP3Dw);
pMP->UpdateNormalAndDepth();
} }
4. 完
实际上，闭环修正的核心工作内容只有三个：① 计算当前帧及其一级邻接关键帧的修正相似变换；② 对当前帧及其一级邻接中观测到的地图点进行融合，建立闭环关系；③ 完成基图的图优化。
地图点融合没有什么特别之处，无非是按照估计的相似变换，将地图点投影到像素坐标上，在一个很小的范围内进行特征点匹配，如果成功匹配上了，则认为闭环检测的地图点与考察的关键帧特征点是同一个点。
基于基图的图优化，有两点需要注意：其一，优化的相机参数有7个自由度，包含了相机的旋转平移以及尺度。其二，地图点的坐标并没有参与到优化过程中，而是根据参考关键帧推算更新的。