Bundle Adjustment

如上图所述，假设场景中有\(m\)个相机，记为\(c_1, c_2 \cdots c_m\)，有\(n\)个三维点，记为\(p_1,p_2 \cdots p_n\)。Bundle Adjustment描述的是通过相机在不同位置观测环境中的三维点已达到求解相机位姿以及三维点的目的。并可以通过最小二乘方法求解。 设\(u_i\)为三维点对应的2d相机观测像素点，\(K\)为相机内参矩阵\(s_i\)为\(u_i\)对应的深度值。则Bundle Adjustment可写成如下方程：

\[ \{t_{pi}, \xi_{c_j}\} = min \frac{1}{2} \sum_{i = 0}^{n} || u_i - \frac{1}{s_i} Kexp(\xi^{\land})p_i ||^2 \]

对于求解上述方程，对于高斯牛顿，迭代方程为：

\[ \begin{aligned} &J^TJ\Delta x = -J^Tf(x) \\ &x = x + \Delta x \end{aligned} \]

因此需要先求解方程对于求解变量的雅克比矩阵：

误差函数对相机位姿求导：

\[ J_c = \frac{\partial u}{\partial \xi} = \frac{\partial u}{\partial p} \frac{\partial p}{\partial \xi}= \begin{bmatrix} \frac{xy}{z^2}f_x & -(1 + \frac{x^2}{z^2})f_x & \frac{y}{z}f_x & -\frac{1}{z}f_x & 0 & \frac{x}{z^2}f_x \\ (1 + \frac{y^2}{z^2})f_y & -\frac{xy}{z^2} f_y & -\frac{x}{z}f_y & 0 & -\frac{1}{z}f_y & \frac{y}{z^2}f_y\end{bmatrix}_{2 \times 6} \]

误差函数对3D点坐标求导：

\[ J_p = \frac{\partial u}{\partial p^w} = \frac{\partial u}{\partial p^c} \frac{\partial p^c}{\partial p^w} = \begin{bmatrix} -\frac{f_x}{z} & 0 & \frac{xf_x}{z^2} \\ 0 & -\frac{f_y}{z} & \frac{yf_y}{z^2}\end{bmatrix}_{2 \times 3} \cdot R \]

上述矩阵\(J^TJ\)是一个\((6m+3n) \times (6m + 3n)\)的对称矩阵,\(b\)和\(x\)是一个\((6m+3n) \times 1\)的列向量，可以细分为关于相机位姿的部分和关于3D点位置的部分，其中A与D为广义对角矩阵：

\[ \begin{aligned} \begin{bmatrix} A_{6m\times6m} & C^T_{6m\times3n} \\ C_{3n\times6m} & D_{3n\times3n} \end{bmatrix} \begin{bmatrix} {x_{\xi}}_{6m \times 1} \\ {x_{p}}_{3n \times 1} \end{bmatrix} &= \begin{bmatrix} b_{\xi} \\ b_{p} \end{bmatrix} \\ \begin{bmatrix} J_c^T * J_c & J_c^T * J_p \\ J_p^T * J_c & J_p^T * J_p \end{bmatrix} \begin{bmatrix} J_c^T * e \\ J_p^T * e \end{bmatrix} &= \begin{bmatrix} b_{\xi} \\ b_{p} \end{bmatrix} \end{aligned} \]

如上图\(J^TJ\)矩阵所示为5个相机位姿和10个3D点的hessian矩阵。矩阵各分块如上述公式所示。代码实现如下：

int size = observations.size() * 6 + object_points.size() * 3;// transform * 6 + object * 3

Eigen::MatrixXd H = Eigen::MatrixXd::Zero(size, size);
Eigen::VectorXd b = Eigen::VectorXd::Zero(size);
double cost = 0.;

for(int iobs = 0; iobs < observations.size(); iobs ++){
    for(int ipts = 0; ipts < object_points.size(); ipts ++){
        const Eigen::Vector3d &obs = observations[iobs][ipts];
        const Eigen::Vector4d &object = object_points[ipts];
        const Eigen::Matrix4d transform = transforms[iobs];

        Eigen::Vector4d pc = transform * object;
        double x = pc.x(); double y = pc.y(); double z = pc.z();
        Eigen::Vector2d obse = {fx * (x/z) + cx, fy * (y/z) + cy};
        Eigen::Vector2d e = obs.head(2) - obse;
        cost += e.squaredNorm();

        Eigen::MatrixXd Jt = Eigen::MatrixXd::Zero(2, 6);
        Eigen::MatrixXd Jp = Eigen::MatrixXd::Zero(2, 3);

        // fix first
        if(iobs){
            Jt(0,0) = -(fx/z); Jt(0,1) = 0;       Jt(0,2) = (fx*x/(z*z)); Jt(0,3) = (fx*x*y/(z*z));    Jt(0,4) = -(fx*x*x/(z*z)+fx); Jt(0,5) = (fx*y/z);
            Jt(1,0) = 0;       Jt(1,1) = -(fy/z); Jt(1,2) = (fy*y/(z*z)); Jt(1,3) = (fy*y*y/(z*z)+fy); Jt(1,4) = -(fy*x*y/(z*z));    Jt(1,5) = -(fy*x/z);
        }
        if(ipts){
            Eigen::Matrix<double, 2, 3> K;
            K << -(fx / z), 0, (fx *x / (z*z)), 0, -(fy / z), fy * y / (z*z);
            Eigen::Matrix3d R = transform.block<3, 3>(0, 0);
            Jp = K * R;
        }
        int tpos = iobs * 6;
        int ppos = observations.size() * 6 + ipts * 3;
        H.block<6, 6>(tpos, tpos) += Jt.transpose() * Jt;
        H.block<3, 3>(ppos, ppos) += Jp.transpose() * Jp;
        H.block<6, 3>(tpos, ppos) += Jt.transpose() * Jp;
        H.block<3, 6>(ppos, tpos) += (Jt.transpose() * Jp).transpose();
        b.segment<6>(tpos) += -Jt.transpose() * e;
        b.segment<3>(ppos) += -Jp.transpose() * e;
    }
}

这里需要解释为什么对于多个观测\(H\)和\(b\)需要做加法？

我们知道线性最小二乘定理：线性方程组\(Ax =b\)的线性最小二乘问题一定有解，且求解线性最小二乘问题与求解线性方程组的法方程组等价。

\[ A^TA x = A^Tb \]

推论：当秩（\(A_{m\times n}\)）= n 时，\(A^TA\)为对称正定矩阵，最小二乘有唯一解。

\[ x_{ls} = (A^TA)^{-1}A^Tb \]

其中\((A^TA)^{-1}A^T\)称为A的伪逆矩阵。

那么对于多个观测，可以写成如下形式：

\[ \begin{aligned} &A = \left[ \begin{array}{l}{A_{1}} \\ {A_{2}} \\ \cdots \\ {A_{n}}\end{array}\right] b = \left[ \begin{array}{l}{b_{1}} \\ {b_{2}} \\ \cdots \\ {b_{n}}\end{array}\right] \\ \end{aligned} \]

可以通过变换得：

\[ \begin{aligned} &\left[ \begin{array}{l}{A_{1}} \\ {A_{2}} \\ \cdots \\ {A_{n}}\end{array}\right]^T \left[ \begin{array}{l}{A_{1}} \\ {A_{2}} \\ \cdots \\ {A_{n}}\end{array}\right] x = \left[ \begin{array}{l}{A_{1}} \\ {A_{2}} \\ \cdots \\ {A_{n}}\end{array}\right]^T \left[ \begin{array}{l}{b_{1}} \\ {b_{2}} \\ \cdots \\ {b_{n}}\end{array}\right] \\ &\sum_{i =0}^{n} A_i^T A_i = \sum_{i = 0}^{n} A_i^Tb_i \end{aligned} \]

Hessian矩阵稀疏性与Schur Complement加速求解

在实际应用中，3D点会特别多，因此hessian矩阵就会变得特别稀疏，如下图所示：

矩阵求逆是\(O(n^3)\)的时间复杂度，可以看到，右下角矩阵块是一个巨大的对角矩阵，对对角矩阵单独求逆是非常快的，只要对小分块矩阵分别求逆即可，但实际上此对角矩阵和3D点的参数化相关，如果参数化为逆深度，会是一个就对的对角矩阵，只需取倒数即可。因此可以用到舒尔消元的知识：

对于方程：

\[ \left[ \begin{array}{c}{\Lambda_{a}, \Lambda_{b}} \\ {\Lambda_{b}^{T}, \Lambda_{c}}\end{array}\right] \left[ \begin{array}{c}{\delta x_{1}} \\ {\delta x_{2}}\end{array}\right]=\left[ \begin{array}{l}{b_{1}} \\ {b_{2}}\end{array}\right] \]

进行舒尔消元，得到：

\[ \left[ \begin{array}{cc}{I,} & {0} \\ {-\Lambda_{b}^{T} \Lambda_{a}^{-1}, I}\end{array}\right] \left[ \begin{array}{c}{\Lambda_{a}, \Lambda_{b}} \\ {\Lambda_{b}^{T}, \Lambda_{c}}\end{array}\right] \left[ \begin{array}{c}{\delta x_{1}} \\ {\delta x_{2}}\end{array}\right]=\left[ \begin{array}{c}{I,} & {0} \\ {-\Lambda_{b}^{T} \Lambda_{a}^{-1}, I}\end{array}\right] \left[ \begin{array}{l}{b_{1}} \\ {b_{2}}\end{array}\right] \]

即：

\[ \left[ \begin{array}{cc}{\Lambda_{a},} & {\Lambda_{b}} \\ {0,} & {\Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}}\end{array}\right] \left[ \begin{array}{c}{\delta x_{1}} \\ {\delta x_{2}}\end{array}\right]=\left[ \begin{array}{c}{b_{1}} \\ {b_{2}-\Lambda_{b}^{T} \Lambda_{a}^{-1} b_{1}}\end{array}\right] \]

由此即可得到在不求取\(\delta x_1\)的情况下求取\(\delta x_2\)的增量迭代公式：

\[ \underbrace{ \left(\Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}\right)}_{H}\underbrace{\delta x_{2}}_{\delta x} = \underbrace{b_{2}-\Lambda_{b}^{T} \Lambda_{a}^{-1} b_{1}}_{b} \]

自此，求得了\(\delta x_2\)。带入上式即可求得\(\delta x_1\)。

\[ \Lambda_{a} \delta x_1 + \Lambda_{b} \delta x_2 = b_1 \]

代码实现如下：

int obssize = observations.size() * 6;
int ptssize = object_points.size() * 3;
Eigen::MatrixXd Hmm = H.block(obssize, obssize, ptssize, ptssize);
Eigen::MatrixXd Hpm = H.block(0, obssize, obssize, ptssize);
Eigen::MatrixXd Hmp = H.block(obssize, 0, ptssize, obssize);
Eigen::VectorXd bpp = b.segment(0, obssize);
Eigen::VectorXd bmm = b.segment(obssize, ptssize);

Eigen::MatrixXd iHmm = Eigen::MatrixXd::Zero(ptssize, ptssize);
for(int pos = 1; pos < object_points.size(); pos ++){
    iHmm.block<3, 3>(pos * 3, pos * 3) = Hmm.block<3, 3>(pos * 3, pos * 3).inverse();
}
Eigen::MatrixXd HpmtiHmm = Hpm * iHmm;
Eigen::MatrixXd schurHpp = H.block(0, 0, obssize, obssize) - HpmtiHmm * Hmp;
Eigen::VectorXd schurbpp = bpp - HpmtiHmm * bmm;

Eigen::VectorXd dxpp = schurHpp.ldlt().solve(schurbpp);
Eigen::VectorXd dxmm = iHmm * (bmm - Hmp * dxpp);
dx.head(obssize) = dxpp;
dx.tail(ptssize) = dxmm;

Sliding Window Filter

随着时间的推移，不断有新的相机位姿以及3D点加入，BundleAdjustment 维度越来越大计算量也在不断上升，为了保证计算实时性，需要保持优化变量个数在一定范围内，因此需要使用滑动窗口的方法动态增加移除优化变量。

这里需要考虑两个话题，第一如何边缘化掉一部分变量，第二如果将边缘化的部分转化为先验。

---

Reference:

[1] M.I.A. Lourakis and A.A. Argyros, SBA: A Software Package for Generic Sparse Bundle Adjustment, ACM Transactions on Mathematical Software, 2009. [2] Sibley, G., Matthies, L., & Sukhatme, G. (2010). Sliding window filter with application to planetary landing. Journal of Field Robotics, 27(5), 587–608. doi:10.1002/rob.20360 [3] A Sliding Window Filter for SLAM