关于BA中梯度求法的总结

BA是SLAM中进行全局图优化的一个关键技术。SLAM主流架构是前端（视觉里程计）和后端（全局图优化）构成。里程计负责在轨迹中增量地构建地图，一般情况下里程计都会面临误差累积的问题。需要靠全局图优化调整机器人位姿与路标点位置将误差消除。

基础铺垫

里程计是基于前后帧图像/滑窗内N帧图像的位姿计算，每次都有误差，然后下一帧又要基于上一帧有误差的结果，导致误差逐渐积累最终不可容忍。所以需要在已经构建的地图上进行一个全局优化。而全局优化借助了以高斯牛顿法为基础的一系列优化算法。这类算法一般都是迭代算法每次算一个更新量，将更新量加到原来的状态量上。而这个更新量求解说到底就是个解方程组。方程组的左边与函数对于某状态量的梯度有关，右边与梯度和残差有关。期间还有舒尔布、边缘化等，这里不再展开，主要说说求梯度这一块的内容。
首先来看看一步迭代的公式：

\[J^TJ\delta x = -Jr \]

从公式上看似乎也挺简单。左边的\(J^TJ\)就是常说的Hession矩阵，而这个J在BA中大致有下面的形状（《SLAM十四》讲中有这部分的详细说明）。

可以看到J可以看成是一堆矩阵块构成的大矩阵。这些矩阵块有两类，一类是残差对于相机位姿的求导长方形，一类是对地图点求导，正方形。

雅克比块的形式

每一个小块的每个元素代表了残差的某个分量对状态某个分量的求导。这里具体的矩阵形式是这样的：

\[\frac{\partial{y}}{\partial{x^T}} = \left[ \begin{array}{cccc} \frac{\partial{y_1}}{\partial{x_1}} & \frac{\partial{y_1}}{\partial{x_2}} &...& \frac{\partial{y_1}}{\partial{x_m}} \\ \frac{\partial{y_2}}{\partial{x_1}} & \frac{\partial{y_2}}{\partial{x_2}} &...& \frac{\partial{y_2}}{\partial{x_m}}\\ ...&...&...&...\\ \frac{\partial{y_n}}{\partial{x_n}} & \frac{\partial{y_n}}{\partial{x_2}} &...& \frac{\partial{y_n}}{\partial{x_m}} \end{array} \right] \]

其中\(y\)为一个n维列向量，\(x\)为一个m维列向量，那么\(y\)对\(x\)求导就构成了一个\(n\times m\)的矩阵。注意所有的向量对向量求导其实都是对向量转置的求导，被导的向量元素按列排列，主导的向量元素按行排列。
至于大的\(J\)为什么是图片中的那样可以参考我的上篇文章《前端滑窗优化和BA的区别》。而由于\(J^TJ\delta x = -Jr\)，观察\(J^TJ\)的形式我们可以看出，Hession矩阵的行数，即\(\delta x\)的行数由\(J\)的列数决定，而\(J\)的列矩阵块按照先相机后点的方式摆放，而每有一个残差项，都会给\(J\)新增一行，但是在\(J^TJ\)中，行的信息都被融合掉了，所以即使来再多的残差，\(\delta x\)的维度都是保持不变的，都是对相机位姿、地图点的更新量。

BA中两种雅克比的推导

雅克比其实就是一阶导，推导过程依赖于具体的模型，本文按照BAL数据集中的模型进行推导，同时使用ceres完成整个优化过程。

BA数据集中的模型

BAL官方定义的模型如下，即一个地图点\(X\)经过相机位姿\(R,t\)变换到相机坐标系下。第二行即投影的第一步消去z分量，第三步为进行畸变校准，投影到像素坐标系，得到\(p'\)。这个\(p'\)即为模型预测量，还有一个实际的观测值\(p_{ob}\)，这个观测值和预测值构成了残差\(residual\)，而BA就是通过调整\(X,R,t,f,k1,k2\)来最小化残差（的模）进而优化整个地图。

P  =  R * X + t      (conversion from world to camera coordinates)
p  = -P / P.z         (perspective division)
p' =  f * r(p) * p    (conversion to pixel coordinates)
r(p) = 1.0 + k1 * ||p||^2 + k2 * ||p||^4.

符号描述

在ceres的最基础的BA代码中，公式符号和这里不太一致，为了更好的说明问题，本文采用以下符号约定。
记一个地图点在世界坐标系中的坐标为\(P_w\)，在相机坐标系中的坐标为\(P_c\)，即透视投影除法后的相机坐标系点为\(p_c\)，而像素坐标系下的地图点为为\(p_{final}\)。记残差项为\(r\)，将畸变参数用\(l_1,l_2\)来表示。总的来说，公式成为下面的样子：

\[\begin{array}{cccc} P_c = R*P_w +t\\ p_c = - \frac{P_c}{{P_c}_z}\\ p_{final} = f(1.0+l_1 p_c^Tp_c + l_2 * (p_c^Tp_c)^2)p_c\\ r = p_{final} - p_{ob}\\ \end{array} \]

残差\(r\)为2维向量，根据雅克比块的形式，可以知道雅克比块为2行，列数为具体被导变量的维数。优化变量也就是要求求到的变量包含了\(f,l_1,l_2,R,t,P_w\)，即需要被调整的变量

推导过程

• \(\frac{\partial{r}}{\partial{f}}\)

\[\frac{\partial{r}}{\partial{f}} = (1.0+l_1 p_c^Tp_c + l_2 * (p_c^Tp_c)^2)p_c \]

• \(\frac{\partial{r}}{\partial{l_1}}\)

\[\frac{\partial{r}}{\partial{l_1}} = fp_c^Tp_cp_c \]

• \(\frac{\partial{r}}{\partial{l_2}}\)

\[\frac{\partial{r}}{\partial{l_2}} = f(p_c^Tp_c)^2p_c \]

上面三项很简单地即可推导出来，而下面的对于相机位姿的求导就稍显复杂，首先相机位姿在上面的公式中是\(R,t\)一个矩阵和一个向量，而旋转矩阵\(R\)对加法是不满足封闭性的，不能直接求导相加进行更新。在此领域的解决办法就是使用对李代数的求导来间接地达到目的。\(R,t\)可以构成一个李群，这个群的元素只满足对乘法封闭，而每个李群元素都有一个对应的李代数se3即为\(\xi\)，\(\xi\)是满足加法封闭的，所以实际策略就是在求导时，求对于李代数se3的导数，然后将se3转化为SE3，“乘到”状态变量上去，（实际上也不是直接对位姿所表示的李代数求导，而是使用了扰动模型，对位姿的扰动\(\delta\xi\)求导，具体原因就不再展开了）。这里为什么打引号？因为在实际项目中，对于SE3也不是存它的旋转矩阵和偏移向量。比如在ceres中存的就是角-轴(angel-axis)表示的旋转量，因为任何一个位姿都可以看成绕某个轴旋转了多少度。所以在ceres的BA示例代码中，相机位姿是一个六维向量，前三维表示角轴旋转，后三维表示偏移。后面再仔细说说具体的操作过程。接下来先来推这个量。

• \(\frac{\partial{r}}{\partial{\delta\xi}}\)

\[\frac{\partial{r}}{\partial{\delta\xi}} = \frac{\partial{r}}{\partial{p_c}} \frac{\partial{p_c}}{\partial{P_c}} \frac{\partial{P_c}}{\partial{\delta\xi}} \]

其中：记\(A = p_c^Tp_c\)，\(B = (l_1 + p_c^T*p_c*l_2)\)，\(dist = (1.0+l_1 p_c^Tp_c + l_2 * (p_c^Tp_c)^2)\)则可对表达式进行一些简化，这样的情况下\(r=fdistp_c = f(1+AB)p_c\)，经过这样整理，可以很容易得到

\[\frac{\partial{r}}{\partial{p_c}}=f(2(B + Al_2)p_cp_c^T + dist\times I_2) \]

其中\(I_2\)为二维单位矩阵。

\[\frac{\partial{p_c}}{\partial{P_c}}=\left[ \begin{array}{cccc} -\frac{1}{{P_c}_z} & 0 & \frac{{P_c}_x}{{{P_c}_z}^2}\\ 0 & -\frac{1}{{P_c}_z} & \frac{{P_c}_y}{{{P_c}_z}^2} \end{array}\right] \]

\[\frac{\partial{P_c}}{\partial{\delta\xi}} = [I_3, -\hat{P_c}] \]

第三个公式涉及到了对李代数的求导，请自行查阅。我在这里遇到坑了，注意是\([I_3, -\hat{P_c}]\)而不是\([I_3, -\hat{P_w}]\)。

• \(\frac{\partial{r}}{\partial{P_w}}\)
  根据模型，很显然

\[\frac{\partial{P_c}}{\partial{P_w}} = R \]

这样根据链式求导法则，可得\(\frac{\partial{r}}{\partial{P_w}} = \frac{\partial{r}}{\partial{p_c}}\frac{\partial{p_c}}{\partial{P_c}}\frac{\partial{P_c}}{\partial{P_w}}\)

实际操作过程

由于在cere默认的数据存储方式中，相机为一个9维向量，\(\chi=[\phi_x,\phi_y,\phi_z,t_x,t_y,t_z,f,l_1,l_2]^T\)，地图点为一个三维向量\([P_x,P_y,P_z]^T\)。

• 算残差时就要将轴-角表示的旋转矩阵恢复出来，或者直接用罗德里格斯公式对世界坐标系点进行旋转。
• 求导时套用上面的各个参数的雅克比进行计算即可。
• 将优化出来的变量叠加到相机参数上时，要进行特殊处理，因为这是对se3的迭代步长，而不是对相机的迭代步长。叠加符号也不是\(+\)而是\(\chi_{new} = \chi_{old} \boxplus \delta\chi\)。具体操作方法安装如下过程就很容易理解了。
• 首先将\(\chi_{old}\)中位姿部分\([\phi_x,\phi_y,\phi_z,t_x,t_y,t_z]\)转换成\(\xi_{old}\)，然后将\(\delta\chi\)中位姿部分转换成\(\delta\xi\)。将两个李代数指数映射成李群，进行乘法。然后得到的李群元素转换成角-轴旋转和平移覆盖原本的\(\chi_{old}\)中位姿部分即可。至于非位姿部分是普通的函数求导，直接将\(\delta\chi\)相应部分叠加到\(\chi_{old}\)即可。

ceres中的应用

ceres实现了非线性最优化的整个流程。使用ceres解BA要做的最少情况下只需要告诉ceres如何算残差即可。但是为了准确性和效率，用户也可以自己实现雅克比的代码，同时对于像李代数这种不能直接进行叠加的公式，ceres给出执行广义加，即参数局部化的机制（se3其实就是SE3元素上局部的一个切空间，可以认为之前的推导是一种参数局部化）。具体地在CostFunction的派生类中的Evaluate方法中实现残差计算和雅克比计算（如果需要的话）。在LocalParameterization的派生类中实现参数化的代码，具体包含了广义加操作（Plus方法），获取原始空间的维度，获取局部空间的维度，原始空间对局部空间的导数（ComputeJacobian）方法。其中原始空间对局部空间的导数，在我们的公式中只看位姿部分的话即：

\[\frac{\partial{\chi_{pose}}}{\partial\delta\xi} \]

这个东西在推导过程中是没有出现过的，因为我们直接就求出了残差对\(\delta\xi\)的倒数。其实想想ceres为什么做就能够明白，ceres认为如果使用了局部参数化，在Evaluate中的雅克比乘以ComputeJacobian才是对局部参数\(\delta\xi\)的求导。不过我们在Evaluate直接就得到了对\(\delta\xi\)的倒数，所以这里整体给一个单位矩阵就行。
到这里基本上能够说明整个问题了，代码很少，也和本文符号高度一致，理解起来应该不难，有关求导的核心代码都在srcBA/ba_structs.h中。在showpath.tar.gz中是我实现的绘图的代码，基于ROS，如果需要可以下下来跑跑看。这里是rviz的配置文件。

上图为跑的一个测试用例：problem-16-22106-pre.txt。红色为优化后的点，白色为优化前的点。残差从最初的4.18e+06降到了1.80e+04。