再谈Bundle Adjustment

BA这个事情感觉自己总是反反复复在看,然后反反复复的忘掉,昨天感觉自己比较大彻大悟,所以今天写一点东西来记一下

前言,优化过程中信息矩阵的组成

再讲实际的BA之前,先讲一讲抽象的一个例子,也就是一个图优化的简单例子.用这个例子来更直观的去看优化过程中信息矩阵\(H\)的组成.

对于以下最小二乘优化系统,其对应的图模型也如下图所示

\[ \boldsymbol{\xi}=\underset{\boldsymbol{\xi}}{\operatorname{argmin}} \frac{1}{2} \sum_{i}\left\|\mathbf{r}_{i}\right\|_{\boldsymbol{\Sigma}_{i}}^{2} \] 其中对于该最小二乘系统,\(\xi\)表示不同的状态量,\(r\)表示相关状态量之间获得的误差项

对于图而言,圆圈表示顶点,也就是各个待估计的状态变量\(\xi\),边表示状态量之间构建的残差,或者说因为状态量的影响所产生的残差项\(r\) \[ \boldsymbol{\xi}=\left[\begin{array}{c} \boldsymbol{\xi}_{1} \\ \boldsymbol{\xi}_{2} \\ \ldots \\ \boldsymbol{\xi}_{6} \end{array}\right], \mathbf{r}=\left[\begin{array}{l} \mathbf{r}_{12} \\ \mathbf{r}_{13} \\ \mathbf{r}_{14} \\ \mathbf{r}_{15} \\ \mathbf{r}_{56} \end{array}\right] \] 对于上述的最小二乘问题,所获得的高斯牛顿求解方式为: \[ \underbrace{\mathbf{J}^{\top} \boldsymbol{\Sigma}^{-1} \mathbf{J}}_{\mathbf{H} \text { or } \mathbf{\Lambda}} \delta \boldsymbol{\xi}=\underbrace{-\mathbf{J}^{\top} \boldsymbol{\Sigma}^{-1} \mathbf{r}}_{\mathbf{b}} \] (对于二小二乘优化问题逃不掉这个方程解)

这里要着重关注雅克比矩阵\(J\),方程左边是优化变量状态量的增量,右边是所获得的误差项,所以这边的雅克比就是解释误差项和状态变量之间的关联程度

因此,就可以理解,此处的雅克比矩阵为:

\[ \mathbf{J}=\frac{\partial \mathbf{r}}{\partial \boldsymbol{\xi}} \] 将误差项全部拆写开可以得到 \[ \mathbf{J}=\frac{\partial \mathbf{r}}{\partial \boldsymbol{\xi}}=\left[\begin{array}{l} \frac{\partial \mathbf{r}_{12}}{\partial \boldsymbol{\xi}} \\ \frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}} \\ \frac{\partial \mathbf{r}_{14}}{\partial \boldsymbol{\xi}} \\ \frac{\partial \mathbf{r}_{15}}{\partial \boldsymbol{\xi}} \\ \frac{\partial \mathbf{r}_{56}}{\partial \boldsymbol{\xi}} \end{array}\right]=\left[\begin{array}{l} \mathbf{J}_{1} \\ \mathbf{J}_{2} \\ \mathbf{J}_{3} \\ \mathbf{J}_{4} \\ \mathbf{J}_{5} \end{array}\right], \mathbf{J}^{\top}=\left[\begin{array}{lllll} \mathbf{J}_{1}^{\top} & \mathbf{J}_{2}^{\top} & \mathbf{J}_{3}^{\top} & \mathbf{J}_{4}^{\top} & \mathbf{J}_{5}^{\top} \end{array}\right] \] 因此,高斯牛顿方程可以写成累加的形式 \[ \sum_{i=1}^{5} \mathbf{J}_{i}^{\top} \boldsymbol{\Sigma}_{i}^{-1} \mathbf{J}_{i} \delta \boldsymbol{\xi}=-\sum_{i=1}^{5} \mathbf{J}^{\top} \boldsymbol{\Sigma}_{i}^{-1} \mathbf{r} \] 这么做本质上其实就是在还原代码实现的过程.

如果仔细去看代码就可以发现,在求解雅克比的过程中,也是先按照误差,然后再按照状态进行循环,从而将信息矩阵进行填充.

这里不妨以\(J_2\)为例子,将\(J_2\)写出来就是 \[ \mathbf{J}_{2}=\frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}}=\left[\begin{array}{llllll} \frac{\partial \mathbf{r}_{13}}{\partial \boldsymbol{\xi}_{1}} & \mathbf{0} & \frac{\partial \mathbf{r}_{13}}{\partial \xi_{3}} & \mathbf{0} & \mathbf{0} & \mathbf{0} \end{array}\right] \] 也就是说,分别包含了状态变量13之间的误差分别对状态1和3做的偏导

如果误差项是2维,\(\xi_1\)是6维,那么第一项就是2x6的一个矩阵块,\(\xi_2\)是三维,那么第二项就是2x3的矩阵块

写成二次型的形式也就是信息矩阵的形式便如下 \[ \boldsymbol{\Lambda}_{2}=\mathbf{J}_{2}^{\top} \boldsymbol{\Sigma}_{2}^{-1} \mathbf{J} = \left[\begin{array}{ccccc} \left(\frac{\partial \mathbf{r}_{13}}{\partial \xi_{1}}\right)^{\top} \boldsymbol{\Sigma}_{2}^{-1} \frac{\partial \mathbf{r}_{13}}{\partial \xi_{1}} & \mathbf{0} & \left(\frac{\partial \mathbf{r}_{13}}{\partial \xi_{1}}\right)^{\top} \boldsymbol{\Sigma}_{2}^{-1} \frac{\partial \mathbf{r}_{13}}{\partial \xi_{3}} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \left(\frac{\partial \mathbf{r}_{13}}{\partial \xi_{3}}\right)^{\top} \mathbf{\Sigma}_{2}^{-1} \frac{\partial \mathbf{r}_{13}}{\partial \xi_{1}} & \mathbf{0} & \left(\frac{\partial \mathbf{r}_{13}}{\partial \xi_{3}}\right)^{\top} \boldsymbol{\Sigma}_{2}^{-1} \frac{\partial \mathbf{r}_{13}}{\partial \xi_{3}} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} & \mathbf{0} \end{array}\right] \] 继续使用之前的假设,那么第一个矩阵块就是6x6维度,第二个矩阵块就是6x3的维度,第三个矩阵块就是3x6的维度,第四个就是3x3的维度

每一个误差项的信息矩阵都可以写出来,然后进行累加就可以得到最后的信息矩阵\(\Lambda\)

虽然说图上画的是一个小方格,但是实际上是这样的

所以,状态量维度的变化就会改变每个小方格的维度

对视觉BA的理解

至此,回到视觉BA

计算机视觉或者说SLAM问题中的最小二乘优化问题,简单来说就是

1. 我要优化的是什么变量,什么状态量
2. 待优化变量发生变化之后会对什么误差量产生影响

在诸多的书籍以及博客中其实对上面两个问题都有过解释,也就是

1. 在SLAM问题中,待优化的变量是每一帧相机的位姿(6维)以及路标点landmark的位置(3维).
2. 各个状态量发生变化之后产生的直观表现就是在二维图像上的重投影误差(2维)的改变.
3. 老生常谈,该最小问题有其特殊性也就是,路标点之间没有关联,所以两个路标点不会对图像重投影误差产生影响.

所以,这里很重要的一点就是,各个状态量在优化过程中发生变化,其直观的表现就是在重投影误差上的变化

这个很重要,感觉理解之后就很舒坦了

因此,我们需要找到的就是重投影误差对于各个状态量之间的雅克比,这边的雅克比简单来说就是去找到一种关系,也就是坐标点发生微小变化之后观测值(重投影误差)会怎么变,相机位姿发生微小变化之后观测值会怎么变, 雅克比矩阵就是阐述了这种变化的关系.因为各个变量之间的关系常常是非线性的,因此在这个过程中需要进行线性化处理,所以务必要关注是在哪个时刻哪个位置进行线性化的,线性化的工作点在哪.

那么,所要求解的就是两个雅克比,这里借用一下视觉SLAM十四讲的结论 P167

使用相机的投影模型可以得到变换到相机坐标系下三维坐标点到二维平面uv的雅克比(2x3)

P表示世界坐标系下的值,P'表示变换到当前相机系下的值 \[ \boldsymbol{P}^{\prime}=\left(\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}\right)_{1: 3} \]

\[ u=f_{x} \frac{X^{\prime}}{Z^{\prime}}+c_{x}, \quad v=f_{y} \frac{Y^{\prime}}{Z^{\prime}}+c_{y} \]

\[ \frac{\partial \boldsymbol{r}}{\partial \boldsymbol{P}^{\prime}}=-\left[\begin{array}{ccc} \frac{\partial u}{\partial X^{\prime}} & \frac{\partial u}{\partial Y^{\prime}} & \frac{\partial u}{\partial Z^{\prime}} \\ \frac{\partial v}{\partial X^{\prime}} & \frac{\partial v}{\partial Y^{\prime}} & \frac{\partial v}{\partial Z^{\prime}} \end{array}\right]=-\left[\begin{array}{ccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} \end{array}\right] \]

而第二项就是重投影误差和位姿之间的雅克比(2x6),这个可以用两步来求解,首先得到变换到当前相机位姿坐标系下的三维点对当前相机位姿的雅克比(3x6) \[ \frac{\partial \boldsymbol{P}^{\prime}}{\partial \delta \boldsymbol{\xi}}=\left[\boldsymbol{I},-\boldsymbol{P}^{\prime \wedge } \right] \] 然后和之前求得的变换到相机坐标系下三维坐标点到二维平面uv的雅克比(2x3)相乘,就可以得到最后的结果(2x6) \[ \frac{\partial \boldsymbol{r}}{\partial \delta \boldsymbol{\xi}}=-\left[\begin{array}{ccccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} & -\frac{f_{x} X^{\prime} Y^{\prime}}{Z^{\prime 2}} & f_{x}+\frac{f_{x} X^{2}}{Z^{\prime 2}} & -\frac{f_{x} Y^{\prime}}{Z^{\prime}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} & -f_{y}-\frac{f_{y} Y^{\prime 2}}{Z^{\prime 2}} & \frac{f_{y} X^{\prime} Y^{\prime}}{Z^{\prime 2}} & \frac{f_{y} X^{\prime}}{Z^{\prime}} \end{array}\right] \] 而对于重投影误差对空间点P的倒数,也可以使用链式法则进行求解(2x3)

注意,这边所计算的是误差杜绝世界坐标系下的点P的导数

(所以为什么要用世界坐标系下的导数呢) \[ \frac{\partial \boldsymbol{r}}{\partial \boldsymbol{P}}=\frac{\partial \boldsymbol{r}}{\partial \boldsymbol{P}^{\prime}} \frac{\partial \boldsymbol{P}^{\prime}}{\partial \boldsymbol{P}} \]

\[ \boldsymbol{P}^{\prime}=\exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{P}=\boldsymbol{R} \boldsymbol{P}+\boldsymbol{t} \]

\[ \frac{\partial \boldsymbol{r}}{\partial \boldsymbol{P}}=-\left[\begin{array}{ccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{f_{x} X^{\prime}}{Z^{\prime 2}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{f_{y} Y^{\prime}}{Z^{\prime 2}} \end{array}\right] \boldsymbol{R} \]

代码

这里简单看一下一部分代码

	std::default_random_engine generator;
   std::vector<Eigen::Vector3d> points;
// featurenum就是所有路标点的数目
// 进行每一个三维点状态量的循环
   for(int j = 0; j < featureNums; ++j)
   {
       // 此处进行随机点的生成
       std::uniform_real_distribution<double> xy_rand(-4, 4.0);
       std::uniform_real_distribution<double> z_rand(8., 10.);
       double tx = xy_rand(generator);
       double ty = xy_rand(generator);
       double tz = z_rand(generator);
	// 获得点的表示形式
       Eigen::Vector3d Pw(tx, ty, tz);
       points.push_back(Pw);
	// 进行每一个位姿状态量的循环
       for (int i = 0; i < poseNums; ++i) {
           // 获得当前的位姿
           Eigen::Matrix3d Rcw = camera_pose[i].Rwc.transpose();
           // 获得该位姿坐标系下的点的坐标
           Eigen::Vector3d Pc = Rcw * (Pw - camera_pose[i].twc);

           double x = Pc.x();
           double y = Pc.y();
           double z = Pc.z();
           double z_2 = z * z;
           
           Eigen::Matrix<double,2,3> jacobian_uv_Pc;
           jacobian_uv_Pc<< fx/z, 0 , -x * fx/z_2,
                   0, fy/z, -y * fy/z_2;
           // 误差对于点(三自由度)的雅克比
           Eigen::Matrix<double,2,3> jacobian_Pj = jacobian_uv_Pc * Rcw;
           // 误差对于位姿(六自由度)的雅克比
           Eigen::Matrix<double,2,6> jacobian_Ti;
           jacobian_Ti << -x* y * fx/z_2, (1+ x*x/z_2)*fx, -y/z*fx, fx/z, 0 , -x * fx/z_2,
                           -(1+y*y/z_2)*fy, x*y/z_2 * fy, x/z * fy, 0,fy/z, -y * fy/z_2;

           // 每一个元素进行遍历填写
           // 这里的6x6本质上就是代表了之前格子里画的一小格
           // 位姿对位姿的雅克比填写
           H.block(i*6,i*6,6,6) += jacobian_Ti.transpose() * jacobian_Ti;
           // 路标点对路标点的雅克比填写
           H.block(j*3 + 6*poseNums,j*3 + 6*poseNums,3,3) += jacobian_Pj.transpose() * jacobian_Pj;
           // 位姿对路标点的雅克比填写
           H.block(i*6,j*3 + 6*poseNums, 6,3) += jacobian_Ti.transpose() * jacobian_Pj;
           // 路标点对位姿的雅克比填写
           H.block(j*3 + 6*poseNums,i*6 , 3,6) += jacobian_Pj.transpose() * jacobian_Ti;
       } // 位姿循环
   } // 特征点循环