全局优化
按说在上一节中，我们完成了基图的图优化之后，后端的工作就结束了。但作者又在最后开启了一个线程来进行全局的 BA 优化。 不太清楚作者的本意是啥，我猜作者是在想，回环不是想有就能有的，逮着一个机会就可劲儿的提高系统精度。反正在没有回环产生的日子里，后端优化闲着也是闲着，要真是在全局优化过程中再次产生了回环， 我停下来重新弄也来得及。
本文中，我们就来看看这最后一个优化都干了些什么事情。按照老套路，先总体了解一下业务，再详细分析具体完成优化的函数实现。 我个人并不怎么喜欢这段代码，总觉得它潜在的风险挺多的。
1. 全局优化的整体过程
闭环修正的时候，我们就看到 LoopClosing 的成员函数 CorrectLoop 在最后的时候，创建了线程 mpThreadGBA， 以当前关键帧的 id 为输入参数，调用 RunGlobalBundleAdjustment 完成最后的全局优化。
mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);
下面是这个函数的代码片段，它有一个传参 nLoopKF，是当前关键帧的 id。 这里忍不住想吐槽一下这个命名，要不是专门看了一下传入的实参，我还一直以为它是闭环帧的id。 在函数的一开始输出了一行日志之后，用局部变量 idx 记录下 mnFullBAIdx，并调用函数 Optimizer::GlobalBundleAdjustemnt 完成闭环优化。再之后就是优化结束，更新地图了。 整个过程十分直接。
void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF) {
cout && "Starting Global Bundle Adjustment" && endl;
int idx = mnFullBAIdx;
Optimizer::GlobalBundleAdjustemnt(mpMap,10,&mbStopGBA,nLoopKF,false);
这个 mnFullBAIdx 本身是个布尔变量，在这里非得当个整型使。刚开始的时候，我寻思着， 这个 ORB-SLAM2 的代码稍微有点老，它的依赖现在都有了很多更新，干脆都迁移到新版本上好了。先用 C++17 编译，就因为 mnFullBAIdx 非要当个整型使，搞得我编不过去。 我想，既然你想当整型，那我就让你痛痛快快地，当个整形好了。嘁哧咔嚓，我把头文件给改了， 一顿骚操作，编译通过了。一运行，嘿，没跑起来！退一步，我又给改回来了，用 C++14 编译了一遍把这个问题给回避了。
后来，我研究了一下这个 mnFullBAIdx，合着它在第一次执行 ++ 运算之后就一直是 1 了，之后就再也没有变过。所以直接改成整型，它运行的逻辑就发生了重大的变化。 最直接的影响，就是下面第 6 行的这个判定条件，在布尔变量的逻辑下，只要发生过一次闭环，之后这个条件就不管用了。而在整型变量的逻辑下，mnFullBAIdx++ 和这个判定条件是在两个线程中进行的， 就有一定的概率，导致该条件生效，直接退出闭环优化。
{
unique_lock<mutex> lock(mMutexGBA);
if (idx != mnFullBAIdx) return;
成员变量 mbStopGBA 用于标记是否需要终止全局优化。若该值为 true，那么上面第4行的全局优化可能还没有结束就被打断了。此时意味着检测到新的闭环了，这个算了一半的结果就被抛弃了。 因为接下来要更新地图，所以在第 9 行中通过接口 RequestStop 通知局部地图管理器先暂停一下，第10、11行等局部地图管理器停下来之后再进行更新。
if (!mbStopGBA) {
mpLocalMapper->RequestStop();
while (!mpLocalMapper->isStopped() && !mpLocalMapper->isFinished())
usleep(1000);
对地图对象的互斥量 mMutexMapUpdate 加锁。并从地图对象的成员 mvpKeyFrameOrigins 中记录的初始化关键帧开始，通过广度优先搜索遍历整个生成树，更新各关键帧的位姿。 局部容器 lpKFtoCheck 用于记录已扩展待检查的关键帧对象。
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
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 (auto 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();
}
接下来，在一个 for 循环中完成地图点的 3D 坐标的更新。
const vector<MapPoint*> vpMPs = mpMap->GetAllMapPoints();
for(size_t i = 0; i < vpMPs.size(); i++) {
MapPoint* pMP = vpMPs[i];
if (pMP->isBad()) continue;
if (pMP->mnBAGlobalForKF == nLoopKF) {
// If optimized by Global BA, just update
pMP->SetWorldPos(pMP->mPosGBA);
} else {
// Update according to the correction of its reference keyframe
KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();
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);
}
}
最后，通过接口 InformNewBigChange 通知地图对象完成了全局闭环，并告知局部地图管理器，可以接着更新局部地图了。
mpMap->InformNewBigChange();
mpLocalMapper->Release();
cout << "Map updated!" << endl;
}
mbFinishedGBA = true;
mbRunningGBA = false;
} }
2. 全局 BA 优化的实现
下面是函数 Optimizer::GlobalBundleAdjustemnt 的实现，这个函数只是一个壳子，它从传入的地图对象中获取了所有的关键帧和地图点之后，就调用了另一个函数 Optimizer::BundleAdjustment。 参数 nIterations 限定了最大的迭代次数，布尔型的指针 pbStopFlag 是一个用来提前结束优化的变量，nLoopKF 是触发闭环的关键帧，bRobust 表示优化的过程中是否需要使用鲁棒核函数。
void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag,
const unsigned long nLoopKF, const bool bRobust) {
vector vpKFs = pMap->GetAllKeyFrames();
vector vpMP = pMap->GetAllMapPoints();
BundleAdjustment(vpKFs,vpMP,nIterations,pbStopFlag, nLoopKF, bRobust);
}
下面是函数 Optimizer::BundleAdjustment 的代码片段。该函数一开始定义了一个局部的容器 vbNotIncludedMP 用于记录那些不参与优化的地图点。
接着用我们熟悉的配方[ 1, 2]构建了 g2o 的优化器和求解器对象。
void Optimizer::BundleAdjustment(const vector<KeyFrame *> & vpKFs, const vector<MapPoint *> & vpMP,
int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust) {
vector<bool> vbNotIncludedMP;
vbNotIncludedMP.resize(vpMP.size());
g2o::SparseOptimizer optimizer;
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
如果输入参数 pbStopFlag 不是空指针，说明调用的时候指定了用来提前结束优化的变量，这里需要通过接口 setForceStopFlag 告知 optimizer。
if (pbStopFlag)
optimizer.setForceStopFlag(pbStopFlag);
接下来遍历所有的关键帧，构建节点。局部变量 maxKFid 用于记录关键帧中最大的 id。
long unsigned int maxKFid = 0;
for(size_t i = 0; i < vpKFs.size(); i++) {
KeyFrame* pKF = vpKFs[i];
if (pKF->isBad())
continue;
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
vSE3->setId(pKF->mnId);
vSE3->setFixed(pKF->mnId==0);
optimizer.addVertex(vSE3);
if(pKF->mnId > maxKFid)
maxKFid = pKF->mnId;
}
下面的代码片段，在一个 for 循环中遍历所有的地图点构建节点，以及描述关键帧与地图点几何约束的连边。局部变量 thHuber2D 和 thHuber3D 分别是自由度为2和3的卡方分布95%置信度阈值， 用于配置鲁棒核函数。有时可能受到地图点融合等多方面的影响吧，导致一个地图点被所有的关键帧抛弃了， 此时就会在第90行，根据局部变量 nEdges 将它从优化器中移除。
const float thHuber2D = sqrt(5.99);
const float thHuber3D = sqrt(7.815);
for (size_t i = 0; i < vpMP.size(); i++) {
MapPoint* pMP = vpMP[i];
if (pMP->isBad()) continue;
g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
const int id = pMP->mnId + maxKFid + 1;
vPoint->setId(id);
vPoint->setMarginalized(true);
optimizer.addVertex(vPoint);
const map<KeyFrame*,size_t> observations = pMP->GetObservations();
int nEdges = 0;
for(auto mit = observations.begin(); mit != observations.end(); mit++) {
KeyFrame* pKF = mit->first;
if (pKF->isBad() || pKF->mnId > maxKFid)
continue;
nEdges++;
const cv::KeyPoint & kpUn = pKF->mvKeysUn[mit->second];
if (pKF->mvuRight[mit->second] < 0) {
Eigen::Matrix<double,2,1> obs;
obs << kpUn.pt.x, kpUn.pt.y;
g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
e->setMeasurement(obs);
const float & invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity() * invSigma2);
if(bRobust) {
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuber2D);
}
e->fx = pKF->fx;
e->fy = pKF->fy;
e->cx = pKF->cx;
e->cy = pKF->cy;
optimizer.addEdge(e);
} else {
Eigen::Matrix<double,3,1> obs;
const float kp_ur = pKF->mvuRight[mit->second];
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
e->setMeasurement(obs);
const float & invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
Eigen::Matrix3d Info = Eigen::Matrix3d::Identity() * invSigma2;
e->setInformation(Info);
if(bRobust) {
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuber3D);
}
e->fx = pKF->fx;
e->fy = pKF->fy;
e->cx = pKF->cx;
e->cy = pKF->cy;
e->bf = pKF->mbf;
optimizer.addEdge(e);
}
}
if (nEdges == 0) {
optimizer.removeVertex(vPoint);
vbNotIncludedMP[i]=true;
} else {
vbNotIncludedMP[i]=false;
} }
上述的 for 循环运行结束之后，就完成了图优化问题的构建。下面两行代码完成具体优化迭代。
optimizer.initializeOptimization();
optimizer.optimize(nIterations);
最后，从优化器中取出关键帧的位姿和地图点的空间坐标，更新地图对象。
for (size_t i = 0; i < vpKFs.size(); i++) {
KeyFrame* pKF = vpKFs[i];
if(pKF->isBad())
continue;
g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
g2o::SE3Quat SE3quat = vSE3->estimate();
if (nLoopKF == 0) {
pKF->SetPose(Converter::toCvMat(SE3quat));
} else {
pKF->mTcwGBA.create(4,4,CV_32F);
Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);
pKF->mnBAGlobalForKF = nLoopKF;
} }
for (size_t i = 0; i < vpMP.size(); i++) {
if(vbNotIncludedMP[i]) continue;
MapPoint* pMP = vpMP[i];
if(pMP->isBad())
continue;
g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
if (nLoopKF == 0) {
pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
pMP->UpdateNormalAndDepth();
} else {
pMP->mPosGBA.create(3,1,CV_32F);
Converter::toCvMat(vPoint->estimate()).copyTo(pMP->mPosGBA);
pMP->mnBAGlobalForKF = nLoopKF;
} }
}
3. 完
本文中我们研究了 ORB-SLAM2 中最后一个优化。并没有什么具体的知识点，其核心的 BA 图优化， 我们已经在前文[1, 2, 3]中多次见到，套路应该很熟了。