BA是SLAM中重要的优化手段，本文详细解释了这种方法的原理，以及采用g2o图优化的应用示例。

1. 投影模型与BA优化函数

BA(Bundle Adjustment)，又称光束法平差（平差就是抹平误差）。BA的本质是一个优化模型，其目的是最小化重投影误差。

所谓重投影误差就是二次投影与一次投影之间产生的误差。

第一次投影指的就是相机在拍照的时候三维空间点投影到图像上第一次投影在相机平面产生了特征点$p_1$，我们可以计算出$P$的坐标位置。

之后相机进行了运动，通过一些方法我们得到这个运动的数值，进而得到了它的位姿。由于我们现在知道相机的位姿（计算估计得来）和$P$的世界坐标，因此可以计算$P$在第二幅图下的投影，这就是所谓的第二次投影。

此时在相机平面产生了特征点$p_2$，而通过特征匹配我们能够知道特征点$p_2’$的真实位置，两者会产生误差，这就是所谓的重投影误差。换句话说，重投影误差是指的真实三维空间点在图像平面上的投影（也就是图像上的像素点）和重投影（其实是用我们的计算值得到的虚拟的像素点）的差值。

给定$N$个两张图中完成匹配的点，记作：

已知相机的内参矩阵为$K$，求解相机的运动$R,t$，注意字符$z$的上标表示第几个点。则：

根据投影关系：

采用最小二乘法优化：

简化形式，已知观测方程为$z=h(x,y)$其中$x$表示位姿，$y$表示路标。观测误差就可以表示为：

$z$表示一次投影得到的特征点位置，$h(\xi,p)$表示二次投影的结果，$h$就是投影函数（这里用李代数表示，$p$表示三维点）。如果把所有观测结果考虑进来，给误差添加一个下标：$z_{ij}$表示位姿$\xi_i$处观测路标$p_i$产生的数据，最后就得到了需要优化的函数：

2. 求解BA

2.1 迭代求解

这不是一个线性函数，所以可以通过第六章的非线性优化的方法来求解。根据非线性优化的思想，我们应该从某个的初始值开始，不断地寻找下降方向 $\Delta x$ 来找到目标函数的最优解，即不断地求解增量方程中的增量$\Delta x$。

首先需要把所有自变量定义成待优化变量：

相应的，增量方程中的$\Delta x$则是对整体自变量的增量。在这个意义下，当我们给自变量一个增量时，目标函数变为：

其中$F{ij}$表示整个代价函数在当前状态下对相机姿态的偏导数，$E{ij}$表示该函数对路标点位置的偏导。把相机的位姿变量和空间点变量放在一起：

最后待求式子就可以被化简为：

通过GN或者LM方法，最后可以得到增量线性方程：

由于我们把变量分为了位姿和空间点两种，所以雅克比矩阵还需要被分块：

以GN为例，H矩阵表示为：

因为考虑了所有的优化变量，这 个线性方程的维度将非常大，包含了所有的相机位姿和路标点。如果直接对 H 求逆来计算增量方程，由于矩阵求逆是复杂度为 O(n3) 的操作 ，这是非常消耗计算资源的。 幸运地是，这里的 H 矩阵是有一定的特殊结构的。利用这个特殊结构，我们可以加速求解过程。

2.3 稀疏性与边缘化

先举个例子，如下图相机$C1$能够观测到路标$P{1,2,3,4}$而相机$C2$能观测到路标$P{3,4,5,6}$

该场景下的BA优化函数为:

此时$x=[\xi_1,\xi_2,p_1,…,p_6]^T$可知：

由于这里描述的是$C_1$观测到$P_1$所以只有两项不为0：

用图案表示如下：

$H$矩阵和$C,P$关系图之间，除了对角线元素以外，有明显的关联性：

img

现在考虑更一般的情况，假如我们有 m 个相机位姿，n 个路标点。由于通常路标数量远远会比相机多，于是有 n ≫ m。由上面推理可知，实际当中的 H 矩阵会像下图所示的那样。它的左上角块显得非常小，而右下角的对角块占据了大量地方。除此之外，非对角部分则分布着散乱的观测数据。这就是镐形矩阵（箭头矩阵）。

img

因此线性方程$H\Delta x=g$又可以表示为如下形式：

对于这个方程直接代入消元，先求$\Delta x_c$然后求$\Delta x_p$，得到的结果为：

由于$C$是一个对角块矩阵，对角矩阵的逆矩阵非常好求（对角线上的元素不能为0）：

因此边缘化的主要计算量在于求解$\Delta x_c$ 。从概率角度来看，我们称这一步为边缘化，是因为我们实际上把求 $(\Delta x_c,\Delta x_p)$的问题，转化成先求$\Delta x_c$，再求 $\Delta x_p$的过程。这一步相当于做了条件概率展开：

结果是求出了关于$x_c$ 的边缘分布，故称边缘化。在上边讲的边缘化过程中，我们实际把所有的路标点都给边缘化了。

2.3 鲁棒核函数

在前面的 BA 问题中，我们最小化误差项的二范数平方和，作为目标函数。当出现误匹配时，误差$e$就会很大，如果我们再使用平方，这个误差就会更大，算法将试图调整这条边所连接的节点的估计值，使它们顺应这条边的无理要求。由于这个边的误差真的很大，往往会抹平了其他正确边的影响，使优化算法专注于调整一个错误的值。

出现这种问题的原因是，当误差很大时，二范数增长得太快了。于是就有了核函数的存在。核函数保证每条边的误差不会太大以至于掩盖掉其他的边。这种核函数称之为鲁棒核函数。

最常用的鲁棒核函数有Huber核：

当误差 $e$ 大于某个阈值 $δ$ 后，函数增长由二次形式变成了一次形式，相当于限制了梯度的最大值。同时，Huber 核函数又是光滑的，可以很方便地求导。

img

3. 图优化

3.1 BA中图的构建思路

在图优化中，顶点是待优化的量，边是误差。

考虑重投影误差公式：

这里有两种思路：

  1. 假设得到的所有世界坐标是真实可靠地，换言之，第一项没有重投影误差。那么第一个式子默认为0，我们只需要调整第二项中的$R，t$让整个函数达到极小值(局部最优解)。
  2. 假设得到的世界坐标有误差，那么我们就需要将两个式子结合起来共同优化，优化的对象就变为了：$N$个特征点的世界坐标+变换矩阵，一共有$3N+12$个参数

如果我们考虑第二种情况（既优化位姿又优化路标点），那么图就变成：

3.2 使用g2o优化

现在我们明确了顶点是位姿+路标点，边是重投影误差，如果使用g2o进行优化，我们需要完成：

  • 设置顶点和边
  • 构建优化图
  • 增添顶点和边
  • 执行优化

然而在g2o里面专门为我们提供了位姿、路标、重投影误差模型，所以不需要自己设置顶点和边。

（1）构建图优化

//矩阵块：位姿6个维度，路标3个维度
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6, 3>> Block;
// Step1 选择一个线性方程求解器
std::unique_ptr<Block::LinearSolverType> linearSolver(new g2o::LinearSolverCholmod<Block::PoseMatrixType>());
// Step2 选择一个稀疏矩阵块求解器
std::unique_ptr<Block> solver_ptr(new Block(std::move(linearSolver)));
// Step3 选择一个梯度下降方法，从GN、LM、DogLeg中选
g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(std::move(solver_ptr));

g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm(solver);
optimizer.setVerbose(true);

（2）加顶点和边

这里采用了g2o提供的模板创建VertexSE3Expmap顶点和VertexSBAPointXYZ边

特征点采用了归一化的方式，相机模型得到的$XYZ$值设为特征点初值。第一个位姿设为单位pose，第二个位姿设为和第一个一样，并固定第一个位姿。

for (int i = 0; i < 2; i++) {
    g2o::VertexSE3Expmap *v = new g2o::VertexSE3Expmap();
    v->setId(i);
    if (i == 0) {
        v->setFixed(true);
    }
    v->setEstimate(g2o::SE3Quat());
    optimizer.addVertex(v);
}

//添加特征点作为节点
for (size_t i = 0; i < pts1.size(); i++) {
    g2o::VertexSBAPointXYZ *v = new g2o::VertexSBAPointXYZ();
    v->setId(2 + i);
    //深度未知，设为1，利用相机模型，相当于是在求归一化相机坐标
    double z = 1;
    double x = (pts1[i].x - cx) * z / fx;
    double y = (pts1[i].y - cy) * z / fy;
    v->setMarginalized(true);
    v->setEstimate(Eigen::Vector3d(x, y, z));
    optimizer.addVertex(v);
}
//////////////////////////////////////
//添加第一帧中的边
vector<g2o::EdgeProjectXYZ2UV *> edges;
for (size_t i = 0; i < pts1.size(); i++) {
    g2o::EdgeProjectXYZ2UV *edge = new g2o::EdgeProjectXYZ2UV();

    edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(i + 2)));
    edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(0)));

    edge->setMeasurement(Eigen::Vector2d(pts1[i].x, pts1[i].y));
    edge->setInformation(Eigen::Matrix2d::Identity());
    edge->setParameterId(0, 0);
    edge->setRobustKernel(new g2o::RobustKernelHuber());

    optimizer.addEdge(edge);
    edges.push_back(edge);
}

//添加第二帧中的边
for (size_t i = 0; i < pts2.size(); i++) {
    g2o::EdgeProjectXYZ2UV *edge = new g2o::EdgeProjectXYZ2UV();

    edge->setVertex(0, dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(i + 2)));
    edge->setVertex(1, dynamic_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(1)));

    edge->setMeasurement(Eigen::Vector2d(pts2[i].x, pts2[i].y));
    edge->setInformation(Eigen::Matrix2d::Identity());
    edge->setParameterId(0, 0);
    edge->setRobustKernel(new g2o::RobustKernelHuber());
    optimizer.addEdge(edge);
    edges.push_back(edge);
}

（3）执行优化

cout << "开始优化" << endl;
chrono::steady_clock::time_point t1 = chrono::steady_clock::now();//计时
optimizer.initializeOptimization();
optimizer.optimize(10);
chrono::steady_clock::time_point t2 = chrono::steady_clock::now();//结束计时
cout << "优化完毕" << endl;

chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
cout << "solve time cost = " << time_used.count() << " seconds. " << endl;

//输出变换矩阵T
g2o::VertexSE3Expmap *v = dynamic_cast<g2o::VertexSE3Expmap *>(optimizer.vertex(1));
Eigen::Isometry3d pose = v->estimate();
cout << "Pose=" << endl << pose.matrix() << endl;

//优化后所有特征点的位置
for (size_t i = 0; i < pts1.size(); i++) {
    g2o::VertexSBAPointXYZ *v = dynamic_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(i + 2));
    cout << "vertex id:" << i + 2 << ",pos=";
    Eigen::Vector3d pos = v->estimate();
    cout << pos(0) << "," << pos(1) << "," << pos(2) << endl;

（4）结果分析

对于这两张图而言，可以看到相机只做了Y方向的运动，并没有发生旋转。

通过EPNP计算的结果如下，可以看出$R$非常接近单位矩阵（说明没有发生旋转），Y方向位移明显，其他方向位移不明显，我们将EPNP计算的结果试做标准结果，然后比对BA得到的结果。

Epnp R:
[0.9999996742857543, 0.0006346775733399234, -0.0004986108331204287;
 -0.0006349112234142204, 0.9999996886585097, -0.0004685836340065393;
 0.0004983132783585473, 0.0004689000549962402, 0.9999997659082801]
t:
[-0.02433141053497313;
 -0.9995135415375248;
 -0.01951058032180258]

通过BA计算的结果如下，位移量整体变小了约1/3，这是由于我们选取的坐标系不同导致。值得注意的是，未优化路标得到的位移量和上面的标准位移量不成很好的比例，而优化路标后的位移量与上面的比例大约为3.15，符合的很好。由此，我们认为优化路标是有必要的。

#未优化路标
Pose=
    0.999996  -0.00290451  0.000505124    -0.011841
  0.00290592     0.999992  -0.00281963    -0.313892
-0.000496931   0.00282108     0.999996  -0.00914159
           0            0            0            1
#优化路标
Pose=
           1  0.000308113 -0.000653462  -0.00758165
-0.000308225            1 -0.000171787    -0.317904
 0.000653409  0.000171988            1   -0.0061968
           0            0            0            1