爽一把手写Bundle Adjustment

今天是传说中的5.20，同时也是本人最近特别钟爱的神剧《权力的游戏》大结局的日子，作为一名伪技术宅，这么重大的日子当然要写一篇文来纪念一下。所以就有了这篇手写Bundle Adjustment的记录文。

Bundle Adjustment（后面简写成BA）在视觉SLAM里面的作用不用多说，自然是非常重要的。BA主要求解的是一个关于相机位姿以及路标点的空间坐标的大规模最小二乘优化问题，每一个相机位姿都与多个路标点关联，而每一个路标点也同时和多个相机位姿相关联，由此形成复杂的网状的相互约束，通过调整相机位姿和路标点的位置估计，使得所有路标点在所有相机中的重投影误差最小。下面我们就来亲自动手尝试一下求解Bundle Adjustment。相关的理论可以参考《视觉SLAM十四讲》第六章和第十章，这里仅简单略过。

1 Levenberg-Marquardt优化

考虑简单的最小二乘优化问题：

$\min_{\textbf{x}}{\frac{1}{2}\left \| f(\textbf{x}) \right \|_2^2}$

将 $f(\textbf{x})$ 进行一阶泰勒展开：

$f(\textbf{x})\approx f(\textbf{x})+J(\textbf{x})\Delta \textbf{x}$

同时通过拉格朗日乘子添加对增量的约束，得到以下形式：

$\min_{\textbf{x}}{\frac{1}{2}\left \| f(\textbf{x})+J(\textbf{x})\Delta \textbf{x} \right \|_2^2+\frac{\lambda}{2}\left \| D\Delta \textbf{x} \right \|^2}$

对 $\Delta \textbf{x}$ 求导并令导数等于0，可得以下方程

$(J(\textbf{x})^TJ(\textbf{x})+\lambda D^TD)\Delta \textbf{x}=-J(\textbf{x})^Tf(\textbf{x})$

求解以上增量方程，可得 $\Delta \textbf{x}$ 的值，进而令 $\textbf{x}=\textbf{x}+\Delta \textbf{x}$ 更新对优化变量 $\textbf{x}$ 的估计。

在实际应用中通常取 $D=I$ 或者 $D^TD=diag(J(\textbf{x})^TJ(\textbf{x}))$ ，同时还涉及到对 $\lambda$ 大小的调整。

在应用Levenberg-Marquardt优化算法时，最主要的步骤就是求函数值 $f(\textbf{x})$ 和雅可比 $J(\textbf{x})$ ，以及求解增量方程得出 $\Delta \textbf{x}$ 。

下面尝试一下Ceres优化库的一个曲线拟合的实例 $y=e^{mx+c}$ 。拟合用的数据点通过对真实曲线 $y=e^{0.3x+0.1}$ 进行采样并附加标准差 $\sigma=0.2$ 的高斯噪声生成。

#include<random>
#include<vector>//用来生成高斯噪声
std::default_random_engine e;
std::normal_distribution<double> n(0, 0.2);
//用来存储数据点
std::vector<std::pair<double, double>> data;
//生成数据点，假设x的范围是0-5
for(double x = 0; x < 5; x += 0.05)
{double y = exp(0.3*x + 0.1)+n(e);data.push_back(std::make_pair(x, y));
}

根据以上数据点拟合曲线的最小二乘优化问题为

$\min_{m,c}{\frac{1}{2}\sum_k (e^{mx_k+c}-y_k)^2}=\min_{m,c}{\frac{1}{2}\left \| \begin{bmatrix}e^{mx_1+c}-y_1\\e^{mx_2+c}-y_2\\\dots\\e^{mx_N+c}-y_N \end{bmatrix} \right \|^2_2}=\min_{m,c}{\frac{1}{2}\left \| F(m,c) \right \|^2_2}$

优化的目标是求得最佳的 $m$ 和 $c$ 的值，使得 $\frac{1}{2}\left \| F(m,c) \right \|^2_2$ 值最小。

令 $f_k(m,c)=e^{mx_k+c}-y_k$ ，则

$J(m,c)=\begin{bmatrix} \frac{\partial f_k}{\partial m} & \frac{\partial f_k}{\partial c}\end{bmatrix}_k=\begin{bmatrix} x_ke^{mx_k+c} & e^{mx_k+c} \end{bmatrix}_k$

#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/Dense>using namespace std;
using namespace Eigen;//为了方便，先定义每一项f(m,c)的误差与雅可比，
//整体的F(m,c)与雅可比J(m,c)可以通过将每个小块组合起来得到
class FitErrorBlock
{
public:FitErrorBlock(double x, double y) :x_(x), y_(y) {}double function(double m, double c){return (exp(m*x_ + c) - y_);}Eigen::Matrix<double,1,2> Jacobian(double m, double c){Eigen::Matrix<double, 1, 2> J;J[0] = x_ * exp(m*x_ + c);J[1] = exp(m*x_ + c);}double x_, y_;
};//接下来开始Levenberg-Marquardt优化
class LMOptimization
{
public://设置初值void setInitValue(double m, double c){m_ = m;c_ = c;}//优化算法主体void Optimize(int niters){double lambda = 1e-4;for (int iter = 0; iter < niters; iter++){//构造雅可比和函数值向量，将每一项作为一行MatrixXd Jacobian(errorTerms.size(), 2);VectorXd err(errorTerms.size());for (int k = 0; k < errorTerms.size(); k++){err[k] = errorTerms[k]->function(m_, c_);Jacobian.row(k) = errorTerms[k]->Jacobian(m_, c_);}//构造增量方程Matrix2d JTJ = Jacobian.transpose()*Jacobian;Matrix2d A = JTJ + lambda * Matrix2d(JTJ.diagonal().asDiagonal());Vector2d b = -Jacobian.transpose()*err;//求解增量方程Ax=bVector2d delta=A.colPivHouseholderQr().solve(b);//如果增量幅度小于阈值，则认为已经收敛，停止优化if (delta.norm() < 1e-10){break;}//计算更新前后的误差double err_before = 0.5*err.squaredNorm();double err_after = 0;for (int k = 0; k < errorTerms.size(); k++){double e= errorTerms[k]->function(m_ + delta[0], c_ + delta[1]);err_after += e * e;}err_after *= 0.5;//判断是否接受更新，如果更新后的误差减小，则接受更新，同时减小lambda；否则不接受更新，并增大lambdaif (err_after < err_before){m_ += delta[0];c_ += delta[1];lambda /= 10;err_before = err_after;}else{lambda *= 10;}cout << "iteration = " << iter << ", error = " << err_before << " , lambda = " << lambda << endl;}}void addErrorTerm(FitErrorBlock* e){errorTerms.push_back(e);}double m_, c_;//保存最小二乘优化的每一项std::vector<FitErrorBlock*> errorTerms;
};

在调用优化算法的时候，可以简单地将每一项加入到优化函数里面

int main()
{std::default_random_engine e;std::normal_distribution<double> n(0, 0.2);std::vector<std::pair<double, double>> data;for (double x = 0; x < 5; x += 0.05){double y = exp(0.3*x + 0.1)+n(e);data.push_back(std::make_pair(x, y));}LMOptimization opt;for (int k = 0; k < data.size(); k++){FitErrorBlock *e = new FitErrorBlock(data[k].first,data[k].second);opt.addErrorTerm(e);}opt.setInitValue(0, 0);opt.Optimize(50);cout << "m = " << opt.m_ << " , c = " << opt.c_ << endl;return 0;
}

结果如下

2 求解Bundle Adjustment

从以上实例可以看出，求解最小二乘优化主要分为四步：计算每一个误差项及其雅可比，将所有误差项组合起来构造增量方程，求解增量方程，更新优化变量。用相同的思想来求解BA，首先要定义每一个误差项以及雅可比，然后利用BA的稀疏结构构建增量方程并求解，最后增新优化变量。

在BA里面比较常见的就是路标点在图像里面的重投影误差，为了方便计算雅可比，这里采用“预测值-测量值”的形式。

$e_{ij}=\pi(R_{cw}P_w^j+t_{cw})-p_{ij}$

$\frac{\partial e_{ij}}{\partial \delta \xi_i}=\begin{bmatrix}\frac{f_x}{Z} & 0 & -\frac{f_x X}{Z^2}\\0 & \frac{f_y}{Z} & -\frac{f_y Y}{Z^2}\end{bmatrix}\begin{bmatrix}I & -P_c^{j \wedge}\end{bmatrix}$ , $P_c^j=\begin{bmatrix}X\\Y\\Z \end{bmatrix}=R_{cw}P_w^j+t_{cw}$ , $P_c^{j \wedge}=\begin{bmatrix}0 & -Z & Y\\Z & 0 & -X\\-Y & X & 0 \end{bmatrix}$

$\frac{\partial e_{ij}}{\partial P_w^j}=\begin{bmatrix}\frac{f_x}{Z} & 0 & -\frac{f_x X}{Z^2}\\0 & \frac{f_y}{Z} & -\frac{f_y Y}{Z^2}\end{bmatrix}R_{cw}$

class ReprojectionError
{
public://位姿采用旋转矩阵与平移向量表示typedef pair<Matrix3d, Vector3d> CameraPose;ReprojectionError() :fx(0), fy(0), cx(0), cy(0), u(0), v(0), camIdx(0), pointIdx(0){;}void setMeasurement(double u, double v){this->u = u;this->v = v;}void setCameraParams(double fx, double fy, double cx, double cy){this->fx = fx;this->fy = fy;this->cx = cx;this->cy = cy;}//计算重投影误差Vector2d function(const CameraPose cam,const Vector3d p){Matrix3d R = cam.first;Vector3d t = cam.second;Vector2d error;Vector3d pc = R * p + t;double xp = pc[0] / pc[2];double yp = pc[1] / pc[2];double up = f * xp + cx;double vp = f * yp + cy;error[0] = up - u;error[1] = vp - v;return error;}Matrix<double, 2, 9> Jacobian(const CameraPose cam,const Vector3d p){Matrix<double, 2, 9> J;Matrix3d R = cam.first;Vector3d t = cam.second;Vector3d pc = R * p + t;double x = pc[0], y = pc[1], z = pc[2];double z2 = z * z;Matrix<double, 2, 3> J_e_pc;J_e_pc << f / z,    0,       -f * x / z2,0,      f / z,    -f * y / z2;Matrix<double, 3, 6> J_pc_ksi;J_pc_ksi << Matrix3d::Identity(), -skew(pc);//对相机位姿的雅可比Matrix<double, 2, 6> J_e_ksi = J_e_pc * J_pc_ksi;Matrix3d J_pc_p = R;//对路标点位置的雅可比Matrix<double, 2, 3> J_e_p = J_e_pc * J_pc_p;J << J_e_ksi, J_e_p;return J;}Matrix3d skew(Vector3d phi){Matrix3d Phi;Phi << 0, -phi(2), phi(1),phi(2), 0, -phi(0),-phi(1), phi(0), 0;return Phi;}double fx, fy, cx, cy;     //保存相机内参double u, v;               //路标点在图像上的测量值int camIdx, pointIdx;      //哪个相机观测哪个路标点
};

在定义好了每一个误差项之后，接下来就是将各个误差项组合成一个大的雅可比和增量方程。

整体的雅可比可以看作是一个以每个误差项对应的雅可比作为一行的分块矩阵

$J=\begin{bmatrix} J_{11}\\J_{12}\\\cdots\\J_{N_cN_p} \end{bmatrix}$ ， $J^TJ=\begin{bmatrix} J_{11}^T & J_{12}^T & \cdots & J_{N_CN_p}^T \end{bmatrix}\begin{bmatrix} J_{11}\\J_{12}\\\cdots\\J_{N_cN_p} \end{bmatrix}=J_{11}^TJ_{11}+J_{12}^TJ_{12}+\cdots+J_{N_CN_p}^TJ_{N_CN_p}$

其中每一项雅可比块仅有两个非零子块

$J_{ij}=\begin{bmatrix} 0_{2\times 6} & \cdots & \frac{\partial e_{ij}}{\partial \delta \xi_i} & \cdots & 0_{2\times 6} \mid 0_{2\times 3} & \cdots & \frac{\partial e_{ij}}{\partial P_w^j} & \cdots & 0_{2 \times 3}\end{bmatrix}$

而 $J_{ij}^TJ_{ij}$ 也仅有四个子块非零，分别对应 $(i,i)$ 、 $(i,j)$ 、 $(j,i)$ 、 $(j,j)$ 四个位置。最终累加得到的 $J^TJ$ 具有特殊的稀疏结构

$J^TJ=\begin{bmatrix}H_{cc} & H_{cp}\\H_{pc} & H_{pp} \end{bmatrix}$

其中 $H_{cc}$ 和 $H_{pp}$ 均为分块对角阵， $H_{cp}=H_{pc}^T$ 。

在进行累加的时候，可以利用以下特点：

$H_{cc}$ 中的每个分块均为 $6 \times 6$ ，分块个数等于相机的个数；每一个 $H_{c_ic_i}$ 由 $\frac{\partial e_{ij}}{\partial \delta \xi_i}^T\frac{\partial e_{ij}}{\partial \delta \xi_i}$ 对所有的 $j$ 累加得到.
$H_{pp}$ 中的每个分块均为 $3 \times 3$ ，分块个数等于路标点的个数；每一个 $H_{p_jp_j}$ 由 $\frac{\partial e_{ij}}{\partial P_w^j}^T\frac{\partial e_{ij}}{\partial P_w^j}$ 对所有的 $i$ 累加得到.
$H_{cp}=H_{pc}^T$ 中的每个分块均为 $6 \times 3$ ，行数等于相机的个数，列数等于路标点的个数；每一个 $H_{c_ip_j}$ 由 $\frac{\partial e_{ij}}{\partial \delta \xi_i}^T\frac{\partial e_{ij}}{\partial P_w^j}$ 得到.

所以在计算时，可以对每一个误差项求出来 $\frac{\partial e_{ij}}{\partial \delta \xi_i}$ 和 $\frac{\partial e_{ij}}{\partial P_w^j}$ 后，根据下标 $i$ 和 $j$ 的值，分别将 $\frac{\partial e_{ij}}{\partial \delta \xi_i}^T\frac{\partial e_{ij}}{\partial \delta \xi_i}$ 、 $\frac{\partial e_{ij}}{\partial P_w^j}^T\frac{\partial e_{ij}}{\partial P_w^j}$ 和 $\frac{\partial e_{ij}}{\partial \delta \xi_i}^T\frac{\partial e_{ij}}{\partial P_w^j}$ 累加到对应的 $H_{c_ic_i}$ 、 $H_{p_jp_j}$ 和 $H_{c_ip_j}$ 中去。类似地，误差向量和增量方程右端项也可以通过累加得到。下面的代码计算了增量方程的左端和右端，其中右端根据左端的分块对应地分成了 $b_c$ 和 $b_p$ ，分别对应于相机和路标点。

for (int k = 0; k < ErrorTerms.size(); k++)
{ReprojectionError* term = ErrorTerms[k];Matrix<double,2,9> J = term->Jacobian(cameras[term->camIdx], points[term->pointIdx]);Matrix<double,2,6> Jc = J.block(0, 0, 2, 6);Matrix<double, 2, 3> Jp = J.block(0, 6, 2, 3);Matrix<double,6,6> JcTJc = Jc.transpose() * Jc;Hcc[term->camIdx] += JcTJc + lambda * JacobianCC(JcTJc.diagonal().asDiagonal());Matrix<double,3,3> JpTJp = Jp.transpose() * Jp;Hpp[term->pointIdx] += JpTJp + lambda * JacobianPP(JpTJp.diagonal().asDiagonal());Hcp[term->camIdx][term->pointIdx] += Jc.transpose()*Jp;Vector2d error = term->function(cameras[term->camIdx], points[term->pointIdx]);bc[term->camIdx] -= Jc.transpose()*error;bp[term->pointIdx] -= Jp.transpose()*error;
}

接下来最能利用BA特殊结构的时候到了，求解增量方程，主要利用了Schur消元。

$\begin{bmatrix} H_{cc} & H_{cp}\\H_{cp}^T & H_{pp} \end{bmatrix}\begin{bmatrix}\delta x_c\\\delta x_p \end{bmatrix}=\begin{bmatrix} b_c\\b_p \end{bmatrix}$

消去右上角的 $H_{cp}$ ，可以得到

$\begin{bmatrix} H_{cc}-H_{cp}H_{pp}^{-1}H_{cp}^T & 0\\H_{cp}^T & H_{pp} \end{bmatrix}\begin{bmatrix}\delta x_c\\\delta x_p \end{bmatrix}=\begin{bmatrix} b_c-H_{cp}H_{pp}^{-1}b_p\\b_p \end{bmatrix}$

第一行变成了和 $\delta x_p$ 无关的方程，可以直接求解 $\delta x_c$ ，

$(H_{cc}-H_{cp}H_{pp}^{-1}H_{cp}^T)\delta x_c= b_c-H_{cp}H_{pp}^{-1}b_p$

因为通常 $\delta x_c$ 维数比 $\delta x_p$ 低很多，而且 $H_{pp}$ 为分块对角阵，每一个分块均为3阶方阵，很容易求逆，所以求解相对较快。在求得 $\delta x_c$ 之后， $\delta x_p$ 可以将 $\delta x_c$ 代入求得

$\delta x_p=H_{pp}^{-1}(b_p-H_{cp}^T\delta x_c)$

在计算和求解上述两个方程时，也可以根据之前的结论将计算过程转化为维数较低的分块矩阵进行累加，避免进行大型矩阵的运算。具体过程不再详细写出来了，将 $H_{cc}$ 、 $H_{pp}$ 和 $H_{cp}$ 以及 $b_c$ 和 $b_p$ 分别写成分块的形式，直接进行分块运算就可以了。

void ComputeUpdate()
{int Nc = cameras.size();int Np = points.size();MatrixXd B(6 * Nc, 6 * Nc); //保存Hcc-HcpHpp^(-1)Hcp^(T)vector<Matrix3d> InvHpp(Np); //保存Hpp^(-1)的各个分块vector<Vector6d> ECbp(Nc,Vector6d::Zero());  //保存HcpHpp^(-1)bpVectorXd b(6 * Nc);  //保存bc-HcpHpp^(-1)bp//对Hpp各个分块求逆for (int k = 0; k < Np; k++){InvHpp[k] = Hpp[k].inverse();}//先将B赋值为Hcc，然后再减去HcpHpp^(-1)Hcp^(T)for (int k = 0; k < Nc; k++){B.block(6 * k, 6 * k, 6, 6) = Hcc[k];}//计算Hcc-HcpHpp^(-1)Hcp^(T)和HcpHpp^(-1)bpfor (int k = 0; k < Np; k++){for (int i = 0; i < Nc; i++){Matrix<double, 6, 3> EikCk = Hcp[i][k] * InvHpp[k];ECbp[i] += EikCk * bp[k];for (int j = i; j < Nc; j++){B.block(6 * i, 6 * j, 6, 6) -= EikCk * Hcp[j][k].transpose();}}}//因为B为对称矩阵，所以上面只计算了上三角部分，下三角部分直接赋值for (int i = 0; i < Nc; i++){for (int j = i+1; j < Nc; j++){B.block(6 * j, 6 * i, 6, 6) = B.block(6 * i, 6 * j, 6, 6).transpose();}}//计算bc-HcpHpp^(-1)bpfor (int k = 0; k < Nc; k++){b.segment(6 * k, 6) = bc[k] - ECbp[k];}cout << "Solving Pose updates." << endl;//求解delta xcVectorXd deltac=B.colPivHouseholderQr().solve(b);for (int k = 0; k < Nc; k++){deltacs[k] = deltac.segment(6 * k, 6);}//回代求delta xpfor (int j = 0; j < Np; j++){Vector3d Ekdeltack = Vector3d::Zero();for (int i = 0; i < Nc; i++){Ekdeltack += Hcp[i][j].transpose()*deltacs[i];}deltaps[j] = InvHpp[j] * (bp[j] - Ekdeltack);}cout << "Update Calculated." << endl;
}

最后对相机和路标点的估计进行更新，路标点的坐标直接按向量相加，相机位姿采用李代数运算左乘更新。

for (int k = 0; k < cameras.size(); k++)
{CameraPose X = cameras[k];Vector6d deltaX = deltacs[k];Matrix4d deltaT = SE3Type::exp(deltaX);Matrix3d deltaR = deltaT.block(0, 0, 3, 3);Vector3d deltat = deltaT.block(0, 3, 3, 1);Matrix3d Rnew = deltaR * X.first;Vector3d tnew = deltaR * X.second + deltat;camerasUpdated[k] = make_pair(Rnew, tnew);
}for (int k = 0; k < points.size(); k++)
{pointsUpdated[k] = points[k] + deltaps[k];
}

其余部分均和第1部分Levenberg-Marquardt优化的过程完全相同，包括判断更新前后误差变化、对lambda的增减、判断收敛等，此处不再赘述。完整的代码放在了github上面。

https://github.com/BruceWhiteSJTU/CSDN-Blog/tree/master/Bundle%20Adjustment

参考文献

[1] 高翔,张涛.视觉SLAM十四讲[M].北京:电子工业出版社,2017.

[2]Timothy D.Barfoot.State Estimation for Robotics[M].United Kingdom:Cambridge University Press,2017.

[3]Grisetti G , Ku，Mmerle R , Stachniss C , et al. A Tutorial on Graph-Based SLAM[J]. IEEE Intelligent Transportation Systems Magazine, 2010, 2(4):31-43.

[4]Triggs B . Bundle Adjustment -A Modern Synthesis[J]. 1999.

[5]Frank D , Michael K . Factor Graphs for Robot Perception[C]// 2017:1-139.