局部BA优化
在 ORB-SLAM2 中，TRACKING 线程是系统的前端里程计，粗略的估计相机的位姿，强调计算的实时性。LOCAL MAPPING 和 LOOP ClOSING 两个线程则是系统的后台优化器， 通过优化算法对相机的历史位姿进行调整，提高定位和建图的精度。只是 LOCAL MAPPING 线程进行的的局部BA优化，LOOP CLOSING 线程进行的完整的BA优化。
在完成新关键帧的插入、地图点的筛选， 以及新地图点生成之后，LOCAL MAPPING 根据插入的新关键帧及其邻接关键帧， 以及出现在这些关键帧中的地图点，构建了一个局部的位姿图，通过 g2o 库进行BA优化。由于局部BA只对一个局部的地图进行优化，而且在优化之前，并没有进行闭环检测，所以其计算复杂度并不高， 在好一点的机器上甚至可以达到实时的计算效率。
本文中，我们先介绍 BA 算法的基本思想，再结合代码详细分析 ORB-SLAM2 中的局部 BA 的实现过程。
1. BA 算法的基本思想
BA 的全称是 Bundle Adjustment，很多人将它翻译成光束平差法。当初第一次看到这个算法名字的时候，有点不明所以，每个字我都认识，但是放到一起也猜不出来是个啥。 再加上网络上各种教程上讲的什么图优化、雅可比矩阵等看起来高深的名词，让当时初学的我头疼了很久，后来发现这个算法的本质就是个最小二乘。整个 BA 算法可以分为构建优化问题和求解优化问题两个部分。
构建优化问题的过程，是根据相机位姿和地图点之间的几何和代数关系，写出各个观测量的误差函数的过程。同一个地图点可能出现在多个关键帧中，生成每个关键帧的时候， 前端里程计都会给它一个初始的估计值。根据相机的内参和关键帧的位姿估计，可以建立地图点的空间坐标与像素坐标之间的映射关系。理想情况下，根据这一映射关系，将地图点重新投影到关键帧上， 应当得到与对应的特征点一致的像素坐标。但现实总是有差距的，通常将两个像素坐标的差值称为重投影误差。取误差项的平方作为损失函数，我们把所有的损失函数加起来， 就得到了光束平差法的优化目标——寻找合适的地图点坐标和关键帧位姿，使得所有损失函数的和最小。
实际上，我们构建的是一个非线性最小二乘问题。求解这类问题，一般都是通过迭代的方法逐渐逼近一个局部极值。常见的方法有梯度下降法、高斯牛顿法、LM法等。 梯度下降法计算复杂度较小，但是收敛速度慢，而且如果迭代步长过大容易出现在极值附近反复波动的现象。高斯牛顿伐收敛速度就比较快，但是存在奇异矩阵求逆的问题，算法的鲁棒性较差。 因为优化的状态变量很多，所以遇到奇异矩阵的概率是很大的。LM法(Levenberg-Marquardt)是高斯牛顿的一个改进方法，解决了奇异矩阵的问题，是较为常用的一种求解方法。
因为有g2o, ceres 等优化算法库的存在，我们在具体实现 BA 算法的时候，很少亲自去写高斯牛顿或者LM算法，而是直接调用库的求解器来完成。所以，本文不再分析如何求解优化问题， 未来将有数值分析的专题介绍各种优化求解算法。
为了方便描述该非线性最小二乘问题，人们开发出了位姿图的工具，根据位姿图进行优化求解的过程，常被称为图优化。 如下图所示，位姿图可以看做是一种二部图。图中的三角形可以理解为地图点，圆圈则是关键帧位姿。同一个地图点可能出现在多个关键帧中，我们用一条边连接这种观测关系，就建立了一个约束， 可以用重投影误差来表示边的权重。关键帧之间有相对的位姿关系，串联起来就是机器人的运动轨迹。
 |
优化算法库已经为我们提供了较为灵活的变成框架来描述位姿图。从位姿图到优化目标函数，再到最终的局部极值，都有求解器帮我们完成了。 下面我们结合代码，来看一下 ORB-SLAM2 是如何利用 g2o 完成局部 BA 优化的。
2. 局部 BA 的实现过程
在局部建图的总体过程中，我们看到 LocalMapping 调用函数 Optimizer::LocalBundleAdjustment() 完成了局部 BA 优化。 下面是调用该函数的代码片段，它有三个输入参数。其中，mpCurrentKeyFrame 是新插入的关键帧；mbAbortBA 是一个控制变量可以用来提前终止 BA 优化，在 LocalMapping 中每当有新的关键帧插入时， 都会将该变量置为 true；mpMap 是 LocalMapping 主要维护的数据对象，记录着所有的地图点和关键帧。
// void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame, &mbAbortBA, mpMap);
函数 LocalBundleAdjustment 代码写的很长，大体上有如下几个部分构成：
- 构建局部地图，主要是在填充三个容器。lLocalKeyFrames 中记录了当前关键帧及其在共视图中的一级邻接关键帧 ，lLocalMapPoints 中记录了 lLocalKeyFrames 中出现的所有有效地图点， lFixedCameras 中记录了当前关键帧在共视图中的二级邻接关键帧。
- 构建优化问题，主要是选择优化器，构建位姿图中的顶点和边。把 lLocalKeyFrames 中涉及的关键帧构建成可变顶点，lFixedCameras 中记录的关键帧构建成固定顶点， 为 lLocalKeyFrames 中的每个地图点及观测到它的关键帧构建一个约束边。
- 求解优化问题，通过从粗略到精细的两阶段优化后得到较为可信的优化结果，标记下那些误差较大的地图点准备删除。
- 更新地图信息，删除误差较大的地图点，更新 lLocalKeyFrames 中各个关键帧的位姿，更新剩下的地图点空间坐标和法向量。
std::list<KeyFrame*> lLocalKeyFrames;
lLocalKeyFrames.push_back(pKF);
pKF->mnBALocalForKF = pKF->mnId;
const std::vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
for(int i = 0, iend = vNeighKFs.size(); i < iend; i++) {
KeyFrame* pKFi = vNeighKFs[i];
pKFi->mnBALocalForKF = pKF->mnId;
if(!pKFi->isBad())
lLocalKeyFrames.push_back(pKFi);
}
2.1 构建局部地图
如右侧的代码片段所示，函数 LocalBundleAdjustment 构建了一个局部的容器 lLocalKeyFrames，用于记录当前关键帧及其在共视图中的一级邻接关键帧。
其中2,3行将当前的关键帧放置到容器中，并修改其成员变量 mnBALocalForKF 记录下当前帧。在后续的代码中我们会看到，这个 mnBALocalForKF 用于标记关键帧是否参与局部 BA 优化。
第5行通过关键帧的成员函数 GetVectorCovisibleKeyFrames 我们获取了当前关键帧的一级邻接。在接下来的 6 到 11 的 for 循环中，标记了各个一级邻接的成员变量 mnBALocalForKF， 并判定关键帧是否有效，将之放置到局部的容器 lLocalKeyFrames 中。
std::list<MapPoint*> lLocalMapPoints;
for (auto lit = lLocalKeyFrames.begin(); lit != lLocalKeyFrames.end(); lit++) {
vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();
for (auto vit = vpMPs.begin(), vend = vpMPs.end(); vit!=vend; vit++) {
MapPoint* pMP = *vit;
if (pMP && !pMP->isBad() && pMP->mnBALocalForKF != pKF->mnId) {
lLocalMapPoints.push_back(pMP);
pMP->mnBALocalForKF = pKF->mnId;
} } }
左侧是填充局部容器 lLocalMapPoints 的代码片段。为了排版，我对这段代码做了等价的修改。
第2行在一个 for 循环中遍历 lLocalKeyFrames，通过关键帧的成员函数 GetMapPointMatches 获取各个关键帧中成功匹配的地图点。
在第4行的 for 循环中遍历所有的地图点。第6行判定地图点有效，并且没有被添加到容器 lLocalMapPoints 中。则添加之，并用其成员变量 mnBALocalForKF 记录下当前关键帧的 ID。
list<KeyFrame*> lFixedCameras;
for (auto lit=lLocalMapPoints.begin(); lit != lLocalMapPoints.end(); lit++) {
map<KeyFrame*, size_t> observations = (*lit)->GetObservations();
for (auto mit=observations.begin(); mit!=observations.end(); mit++) {
KeyFrame* pKFi = mit->first;
if(pKFi->mnBALocalForKF != pKF->mnId && pKFi->mnBAFixedForKF != pKF->mnId) {
pKFi->mnBAFixedForKF = pKF->mnId;
if (!pKFi->isBad())
lFixedCameras.push_back(pKFi);
} } }
在右侧的代码片段中构建了用于记录当前帧的所有二级邻接关键帧的局部容器 lFixedCameras。
在第2行的 for 循环中遍历所有的地图点，在第3行中，通过地图点的成员函数 GetObservations 获取所有观测到该地图点的关键帧。
在第4行的 for 循环中，检查关键帧是否已经设置了 mnBALocalForKF 或者 mnBAFixedForKF。 若没有则用其成员变量 mnBAFixedForKF 记录下当前帧的 ID。同时判定关键帧有效，则将其被添加到局部容器 lFixedCameras 中。
lLocalKeyFrames、lFixedCameras、lLocalMapPoints这三个容器构成了一个局部的地图。下面将根据这三个容器构建一个位姿图，描述非线性最小二乘的优化问题。
2.2 构建优化问题
与 TRACKING 线程通过函数Optimizer::PoseOptimization估计相机位姿时的套路类似，
首先构造了一个 g2o 的优化器和求解器对象。下面代码中第3行的 BlockSolver_6_3 的 6 是指待优化的相机位姿有6个自由度，3 是指特征点的空间坐标是三维的。
第5行指定使用 LM 算法进行求解。g2o 是一个通用的图优化器，主要还是用于 SLAM 或者求解 BA(Bundle Adjustment) 问题。
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);
接下来定义了一个局部变量 maxKFid 用于记录参与局部 BA 的最大关键帧，感觉没啥用。
unsigned long maxKFid = 0;
在 g2o 中，把系统的状态量描述成一个个节点(Vertex)，状态量之间的约束，则被抽象成节点之间的边(Edge)，进而把优化问题用一个图的结构来表示。 在这里，各关键帧的位姿、地图点的空间坐标都是系统的状态量。各个关键帧中观测到的地图点的重投影误差则是状态量之间的约束。
系统的状态量中，有变量和常量之分。变量就是那些在优化迭代过程中，可以调节的量，最小化损失函数。当前帧及其一级邻接关键帧的位姿， 以及出现在这些关键帧中的地图点的空间坐标，就是这里的变量。当前帧的二级邻接关键帧的位姿在局部 BA 的过程中是固定不变的，也就是所谓的常量。
|
|
上面的左侧代码为当前帧及其以及邻接关键帧构建了可变的节点对象，右侧的代码则为二级邻接关键帧构建了固定的节点对象。两者的不同之处在于第7行的 setFixed 配置。 需要注意的是，在整个 ORB-SLAM2 的坐标系统是以第一个关键帧为参考建立的，对应的 mnId 为 0，该关键帧的位姿始终是一个\(4\times 4\)的单位矩阵。 因此左侧代码的第7行才会判定关键帧的索引 mnId 是否为 0。
然后计算了最多的约束数量 nExpectedSize，并依此构建了边的容器分配内存。为了提高算法的鲁棒性，局部BA优化采用 Huber 核函数进行回归估计，降低异常点的负面作用， 在下面代码片段的第3,4行中针对单目和双目地图点定义了两个huber参数。
const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();
// 省略 vpEdgesMono 等容器的构造和内存分配相关语句
const float thHuberMono = sqrt(5.991);
const float thHuberStereo = sqrt(7.815);
在接下来的 for 循环中，为每个地图点构建了一个 VertexSBAPointXYZ 类型的节点，记录了各个地图点的空间坐标。
for (auto lit = lLocalMapPoints.begin(); lit != lLocalMapPoints.end(); lit++) {
MapPoint* pMP = *lit;
g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
int id = pMP->mnId+maxKFid+1;
vPoint->setId(id);
vPoint->setMarginalized(true);
optimizer.addVertex(vPoint);
紧接着遍历观测到各地图点的关键帧，获取关键帧中与地图点匹配的特征点的像素坐标。这里省略双目和深度相机相关的代码， 详细放到本部分最后一篇文章中介绍。
const map<KeyFrame*,size_t> observations = pMP->GetObservations();
for (auto mit=observations.begin(); mit != observations.end(); mit++) {
KeyFrame* pKFi = mit->first;
if (!pKFi->isBad()) {
const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];
// 这里省略双目相关的代码，详细放到本部分最后一篇文章中介绍
Eigen::Matrix obs;
obs << kpUn.pt.x, kpUn.pt.y;
在下面的代码片段中，为每个观测值构建一条边，约束地图点坐标和关键帧的位姿。下面的第2,3行代码，记录了边的两个端点分别是地图点和关键帧。 第4行设定边的观测值为对应特征点的像素坐标。第5,6行为每条边赋予了一个信息矩阵，这是一个类似权重的东西，在高斯分布下可以认为是协方差矩阵的逆。 该值越大，说明不确定性越小，在优化时的作用就越大。
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(pKFi->mnId)));
e->setMeasurement(obs);
const float & invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
接下来就是指定 Huber 核函数、记录相机内参、将新建的边添加到优化器和一些容器中。遍历完局部地图中所有的地图点和关键帧之后，就完成了优化问题的构建工作。接下来就要进行求解了。
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(thHuberMono);
e->fx = pKFi-^gt;fx;
e->fy = pKFi-^gt;fy;
e->cx = pKFi-^gt;cx;
e->cy = pKFi-^gt;cy;
optimizer.addEdge(e);
vpEdgesMono.push_back(e);
vpEdgeKFMono.push_back(pKFi);
vpMapPointEdgeMono.push_back(pMP);
2.3 求解优化问题
在 g2o 的框架下，求解优化问题没有什么好说的，只需要像下面这样先后调用两个函数，先完成初始化工作，再迭代求解。
optimizer.initializeOptimization();
optimizer.optimize(5);
bool bDoMore= true;
if(pbStopFlag && *pbStopFlag)
bDoMore = false;
if(bDoMore) {
for(size_t i=0; i < vpEdgesMono.size(); i++) {
g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
MapPoint* pMP = vpMapPointEdgeMono[i];
if(pMP->isBad())
continue;
if(e->chi2() > 5.991 || !e->isDepthPositive())
e->setLevel(1);
e->setRobustKernel(0);
}
// 省略双目和深度相机相关代码
optimizer.initializeOptimization(0);
optimizer.optimize(10);
}
这里的特殊之处在于，把迭代过程分成了两个阶段。先用上面的代码片段，进行一次较粗的优化。再根据标识 pbStopFlag，进行第二阶段的优化。 这个 pbStopFlag 是函数 LocalBundleAdjustment 的一个指针参数，它指向的是 LocalMapping 的 mbAbortBA。
右侧的代码片段中省略了关于双目和深度相机的代码，它们的总体逻辑与单目相机没有本质区别。如果需要进行第二阶段的优化，那么就遍历优化后的边。 在第10行中，通过一个根据卡方统计量设定的阈值 5.991 剔除掉那些误差较大的边，同时还剔除掉那些深度值为负的边。
第11行的 setLevel(1) 是在完成边的剔除操作，它需要和 initializeOptimization(0) 配合使用。我们通过对象 optimizer 的接口完成优化求解的初始化操作， 其输入参数 0 表示只使用 level 为 0 的边进行优化，初次构造边的时候，level 的默认值就是 0。这里通过 setLevel 将之设置为 1，就不会在下次迭代中参与优化。
完成了边的筛选工作之后，在第15,16行，再次调用 initializeOptimization 和 optimize 完成进一步的迭代优化。 完成迭代之后，还会再次遍历所有的边，记录下那些误差较大，或者深度值为负的边，准备在后续更新地图的时候将相应的约束删除。
vector<pair<KeyFrame*,MapPoint*>> vToErase;
vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());
for(size_t i=0; i < vpEdgesMono.size(); i++) {
g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
MapPoint* pMP = vpMapPointEdgeMono[i];
if (pMP->isBad())
continue;
if (e->chi2() > 5.991 || !e->isDepthPositive()) {
KeyFrame* pKFi = vpEdgeKFMono[i];
vToErase.push_back(make_pair(pKFi,pMP));
}
}
// 省略双目和深度相机相关的代码
2.4 更新地图信息
最后，对地图对象的互斥量成员加锁，准备更新地图信息。
unique_lock<mutex> lock(pMap->mMutexMapUpdate);
先根据刚刚记录的待删除的边，将对应的观测从地图中剔除掉。主要是在下面代码片段的第5,6行中，分别调用关键帧和地图点的 Erase 接口移除观测信息。
if (!vToErase.empty()) {
for(size_t i = 0; i < vToErase.size();i++) {
KeyFrame* pKFi = vToErase[i].first;
MapPoint* pMPi = vToErase[i].second;
pKFi->EraseMapPointMatch(pMPi);
pMPi->EraseObservation(pKFi);
} }
接着，通过g2o的接口获取优化后的关键帧位姿，通过关键帧的 SetPose 接口完成更新。
for (auto lit = lLocalKeyFrames.begin(); lit != lLocalKeyFrames.end(); lit++) {
KeyFrame* pKF = *lit;
g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
g2o::SE3Quat SE3quat = vSE3->estimate();
pKF->SetPose(Converter::toCvMat(SE3quat));
}
类似的，还需要更新地图点的空间坐标。
for (auto lit = lLocalMapPoints.begin(); lit != lLocalMapPoints.end(); lit++) {
MapPoint* pMP = *lit;
g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
pMP->UpdateNormalAndDepth();
}
3. 完
本文中，我根据自己的理解和心路历程讨论了 BA 算法的基本思想。其本质是把地图的优化问题描述成非线性最小二乘问题，在通过迭代求解的方法完成。 我们还详细分析了函数 Optimizer::LocalBundleAdjustment 的代码，将整个局部 BA 优化过程拆分成了构建局部地图、构建优化问题、求解优化问题、更新地图信息四个部分。