简介

在读 LIO-SAM 代码的过程时，感觉里面对残差和雅可比的计算不是很直观，所以稍微花一点时间梳理一下里面的逻辑。中间涉及的代码大部分是从 LIO-SAM 里面摘取的（和 LOAM_Velodyne 也差不多一致）。

边缘线特征

对当前帧点云的每一个角点，边缘线特征匹配的流程如下：

• 利用初始位姿估计，将点转换至局部地图所处的坐标系下：
  • 如果匹配的是上一帧点云，则转换至上一阵 Lidar 坐标系下
  • 如果匹配的是全局地图，则转换至世界坐标系下
• 在局部地图中寻找到当前点所处的边缘线特征，不同论文中方法也有细微差别：
  • 直接寻找最近的两个点构造直线（相当于得到两点式方程）
  • 寻找最近的若干个点，计算中心值，对该点集进行简单的 PCA 求出最大特征值对应的特征向量作为直线方向（相当于得到点斜式方程），通常这样得到的直线会更准一些，因为借助 PCA 的思路我们还能对这些点集的共线性作一个判断
• 计算该点和线特征的残差和雅可比

边缘线特征残差构建

上一部分中提到了怎么获取目标线特征，这一部分主要是整理如何建立当前点对该线特征的残差。思路很简单，求点到线的距离即可。论文中介绍的方式如下：

• 在该直线上取两个点 $A = (x_1, y_1, z_1)$, $B = (x_2, y_2, z_2)$，设当前点为 $P_w = (x_0, y_0, z_0)$
• 计算 $\triangle ABP$ 的面积，除以 $AB$ 即可。（实际上相差一个 0.5 的系数，不过关系不大）

$\triangle ABP$ 的面积计算方式为：先计算出三个点形成的平行面积的 $|\overrightarrow{PA} \times \overrightarrow{PB}|$，这个面积会正比于三角形面积，所以可以作为残差使用，如下所示：

$$
\begin{aligned}
    S_{\triangle ABP} \propto& |\overrightarrow{PA} \times \overrightarrow{PB}|\\
    \overrightarrow{PA} \times \overrightarrow{PB} =& 
    \left|
    \begin{matrix}
        i & j & k\\
        x_1 - x_0 & y_1 - y_0 & z_1 - z_0\\
        x_2 - x_0 & y_2 - y_0 & z_2 - z_0\\
    \end{matrix}
    \right|\\
    =& 
    \begin{bmatrix}
        (y_1 - y_0)(z_2 - z_0) - (z_1 - z_0)(y_2 - y_0)\\
        (z_1 - z_0)(x_2 - x_0) - (x_1 - x_0)(z_2 - z_0)\\
        (x_1 - x_0)(y_2 - y_0) - (y_1 - y_0)(x_2 - x_0)
    \end{bmatrix}\\
    |\overrightarrow{PA} \times \overrightarrow{PB}| &= (\overrightarrow{PA} \times \overrightarrow{PB})^T(\overrightarrow{PA} \times \overrightarrow{PB})\\
    &= ((y_1 - y_0)(z_2 - z_0) - (z_1 - z_0)(y_2 - y_0))^2 \\
    &+ ((z_1 - z_0)(x_2 - x_0) - (x_1 - x_0)(z_2 - z_0))^2 \\
    &+ ((x_1 - x_0)(y_2 - y_0) - (y_1 - y_0)(x_2 - x_0))^2
\end{aligned}
$$

这里注意，我们关注的是实际上只是叉乘向量的大小，因此向量叉乘的顺序 ($\overrightarrow{PA} \times \overrightarrow{PB}$ 或者 $\overrightarrow{PB} \times \overrightarrow{PA}$) 以及向量本身的方向 ($\overrightarrow{PA}$ 或者 $\overrightarrow{AO}$) 实际对结果没有影响，注意叉乘公式使用时统一一种即可。

LOAM 系列中关于这部分面积的计算如下所示：

float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) * ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))     // k^2
                + ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) * ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))     // j^2
                + ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)) * ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));   // i^2

比较一下可以发现和上面我推导的结果是一致的（向量的系数有符号上的区别，不过不影响大小）。

因此残差为：

$$
e_{c} = \frac{|\overrightarrow{PA} \times \overrightarrow{PB}|}{|AB|}
$$

边缘线特征雅可比推导

在这一步中，我们需要推导残差关于关键帧位姿下降的方向。第一种通常的思路是求解雅可比，雅可比的推导利用链式求导法则分两步，先对该点求导，再对位姿求导，对于平面点和边缘线点，点对位姿求导的过程是一样的，因此这里只进行第一步，点对位姿求导放在最后推导：

$$
J = \frac{\partial e_c}{\partial T_{wb}} = \frac{\partial e_c}{\partial \boldsymbol{p}^w} \frac{\partial \boldsymbol{p}^w}{\partial \boldsymbol{T}_{wb}}
$$

先看第一步，残差对点求导，为了表达简洁，设 $\boldsymbol{p}_m =\overrightarrow{PM} = \overrightarrow{PA} \times \overrightarrow{PB}$，且由于 $|AB|$ 和 $P_w$ 不相关，所以这里把它作为常系数，对梯度方向没有影响，之后就不写出来了：

残差为：

$$
\begin{aligned}
e_c \propto& \sqrt{}((y_1 - y_0)(z_2 - z_0) - (z_1 - z_0)(y_2 - y_0))^2 \\
    &+ ((z_1 - z_0)(x_2 - x_0) - (x_1 - x_0)(z_2 - z_0))^2 \\
    &+ ((x_1 - x_0)(y_2 - y_0) - (y_1 - y_0)(x_2 - x_0))^2\\
    =& \sqrt{\boldsymbol{p}_m^T\boldsymbol{p}_m}\\
    =& \sqrt{(p_{mx}^2 + p_{my}^2 + p_{mz}^2)}
\end{aligned}
$$

对点的求导为分别对 $x_0, y_0, z_0$ 求导：

$$
\begin{aligned}
\frac{\partial e_c}{\partial \boldsymbol{p}^w} &=
\begin{bmatrix}
    \frac{\partial e_c}{\partial x_0}\\
    \frac{\partial e_c}{\partial y_0}\\
    \frac{\partial e_c}{\partial z_0}\\
\end{bmatrix}^T\\
&\propto\frac{1}{2}
\begin{bmatrix}
    2p_{my}((z_2 - z_0)-(z_1 - z_0)) + 2p_{mz}((y_1 - y_0)-(y_2 - y_0))\\
    2p_{mz}((x_2 - x_0)-(x_1 - x_0)) + 2p_{mx}((z_1 - z_0)-(z_2 - z_0))\\
    2p_{mx}((y_2 - y_0)-(y_1 - y_0)) + 2p_{my}((x_1 - x_0)-(x_2 - x_0))
\end{bmatrix}^T\\
&=
\begin{bmatrix}
p_{my}(z_2-z_1) + p_{mz}(y_1 - y_2)\\
p_{mz}(x_2 - x_1) + p_{mx}(z_1-z_2)\\
p_{mx}(y_2-y_1) + p_{my}(x_1 - x_2)
\end{bmatrix}^T
\end{aligned}
$$

根据这种求法得到的雅可比和代码中是一致的，如下所示，代码中对向量除 $|AB||PM|$ 是为了进行单位化，见以下几何推导部分：

// 计算 AB 模长
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

// 计算残差下降方向 且单位化
float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
            + (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;

float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1)) 
            - (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1)) 
            + (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;

我们还可以从几何角度来直接推导残差关于该点全局坐标下降的方向，如下图所示：

在第一步中，我们通过 $\overrightarrow{PA} \times \overrightarrow{PB}$ 并利用 $|PM|/|AB|$ 求出 $h$ 的大小（含常数系数），在 $\frac{\partial e_c}{\partial P_w}$ 我们实际想求的使 $|h|$ 下降的方向，即图中所示的 $h$ 的方向即：$h = \overrightarrow{AB}\times\overrightarrow{PM}$，证明过程也很简单，叉乘结果首先垂直于 $PM$ 因此和平面 $PAB$ 共面，其次和 $AB$ 垂直，将 $\overrightarrow{AB}$ 起点移至 $P$。因此该向量是一条起点在 $P$ 指向且垂直于 $AB$ 的向量，显然为图中 $h$ 的方向。

最后我们想要获得一个单位向量便于控制迭代步长，因此可以除以 $|PM||AB|$ 来进行单位化。因为：$\overrightarrow{AB}\times\overrightarrow{PM} = |PM||AB|\sin{\theta}$ 由于 $|PM|, |AB|$ 互相垂直，因此 $\overrightarrow{AB}\times\overrightarrow{PM} = |PM||AB|$

叉乘求解过程为：

$$
\begin{aligned}
\overrightarrow{AB}\times\overrightarrow{PM} &=
\left|
\begin{matrix}
    i & j & k\\
    x_2 - x_1 & y_2 - y_1 & z_2 - z_1\\
    p_{mx} & p_{my} & p_{mz}
\end{matrix}
\right|\\
&=
\begin{bmatrix}
    (y_2 - y_1)p_{mz} - (z_2 - z_1)p_{my}\\
    (z_2 - z_1)p_{mx} - (x_2 - x_1)p_{mz}\\
    (x_2 - x_1)p_{my} - (y_2 - y_1)p_{mx}
\end{bmatrix}
\end{aligned}
$$

这里我们发现，通过几何推导出的下降方向向量恰好和直接对点求雅可比得到的向量方向相反。这个很好理解，在之前的博客里面 最速下降法的推导，通过推导可以知道沿着雅可比负方向实际上才是残差减小的路线。这里代码的目的实际上是求雅可比（为了后面构建正则方程：$\boldsymbol{J}^T\boldsymbol{J}\Delta\boldsymbol{x} = -\boldsymbol{J}^T\boldsymbol{e}$），所以如果按照几何方式推导的结果需要乘上一个负号。

平面特征

对当前帧点云的每一个角点，平面特征匹配的流程如下：

• 利用初始位姿估计，将点转换至局部地图所处的坐标系下：
  • 如果匹配的是上一帧点云，则转换至上一阵 Lidar 坐标系下
  • 如果匹配的是全局地图，则转换至世界坐标系下
• 在局部地图中寻找到当前点所处的平面特征，不同论文和代码也有细微差别：
  • 直接寻找最近的若干个平面点构造平面，通过建立超定方程求解得到平面的一般式方程：$Ax + By + Cz + d = 0$，法向量为 $(A, B, C)$
  • 直接寻找最近的三个点 A, B, C 构建两个向量 AB 和 AC，通过叉乘得到法向量建立点法式方程 $A(x - x_0) + B(y - y_0) + C(z - z_0) = 0$
  • 寻找最近的若干个平面点，计算中心值，对该点集进行简单的 PCA 求出最小特征值对应的特征向量作为平面法向量方向，再利用中心点也可以得到点法式方程
• 计算该点和线特征的残差和雅可比

平面特征残差计算

LOAM 系列的代码为了求解距离方便，采取了第一种方法构建平面，即通过搜索 5 个最近的平面点建立超定方程，过程为假设 $d = 1$，建立超定方程：$A'x_i + B'y_i + C'z_i = -1$ 求解得到系数 $(A', B', C')$ 即可。为了后续方便计算，将系数归一化，即：

$$
\begin{aligned}
    s &= A'^2 + B'^2 + C'^2\\
    A &= \frac{A'}{s}, B = \frac{B'}{s}, C = \frac{C'}{s}, D = \frac{1}{s}\\
\end{aligned}
$$

用点到的平面的距离作为残差，则计算方式为：

$$
e_s = \frac{|Ax_0 + By_0 + Cz_0 + D|}{\sqrt{A^2 + B^2 + C^2}} = |Ax_0 + By_0 + Cz_0 + D|
$$

代码中距离计算方式如下：

// P 到 AB 的距离
float ld2 = a012 / l12;

注意这里并没有取绝对值，用意在雅可比推导部分说明。

平面特征雅可比计算

这里由于代码不是像线特征一样用一系列点来表示距离，因此直接对点求雅可比有点不直观（注：如果是像论文一样利用叉乘得到法向量计算距离的话是可以求雅可比的）。但是和线特征一样，我们可以思考雅可比的几何意义，是残差增大最快的方向。因此显然知道是沿法向量方向，但是是正方向还是负方向没办法确定。这里我们可以将平面公式利用起来，参考下图：

可以发现，如果点和法向量（正方向）在同一侧时，雅可比为法向量的正方向；如果点和法向量在不同侧时，雅可比为法向量的负方向。

即：

$$
\frac{\partial e_s}{\partial \boldsymbol{p}^w} =
\left\{
\begin{aligned}
    \boldsymbol{n}^T &\qquad \text{点和法向量正方向在同一侧}\\
    -\boldsymbol{n}^T &\qquad\text{点和法向量正方向在不同侧}
\end{aligned}
\right.
$$

而根据投影公式，我们知道点和法向量在不在同一侧是由 $Ax_0 + By_0 + Cz_0 + D$ 的符号决定的。因此在残差的部分中，我们保留了计算的符号。可以验证一下对之后的高斯牛顿法迭代的正则方程有没有影响：对 $\boldsymbol{H} = \boldsymbol{J}^T\boldsymbol{J}$，雅可比的符号没有影响。对信息向量 $\boldsymbol{b} = \boldsymbol{J}^T\boldsymbol{e}$，只需要确保 $\boldsymbol{J}^T\boldsymbol{e}$ 的符号正确即可，因此将符号放在哪一部分都可以。这就是为什么代码可以将符号留在残差 $\boldsymbol{e}$ 里面。LOAM 系列中这部分的代码如下所示：

// 用点到的平面的距离作为残差（带符号）
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;

// 核函数进行权重分配，这里由于加入点本身的坐标（通常比较大），实际上相当于人为将所有点的权重都增加了
float s = 1 - 0.9 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
        + pointSel.y * pointSel.y + pointSel.z * pointSel.z));

// 用法向量作为雅可比，方向由 pd2 间接决定
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.intensity = s * pd2;

点对位姿的雅可比

在上面的推导过程中，我们已经推导了残差雅可比的一部分，即残差对世界坐标下的点的求导，这里我们进行世界坐标系下的点对位姿矩阵的求导。

设 Lidar 相对于世界坐标系旋转矩阵为 $\boldsymbol{R}$，平移（位置）为 $\boldsymbol{t}$ 。将 Lidar 坐标系下的点转为世界坐标系的过程为：

$$
\boldsymbol{p}^w = \boldsymbol{R}\boldsymbol{p} + \boldsymbol{t}
$$

对平移雅可比的推导

对平移的推导很简单：

$$
\frac{\partial \boldsymbol{p}^w}{\partial\boldsymbol{t}} = \frac{\partial \boldsymbol{R}\boldsymbol{p} + \boldsymbol{t}}{\partial\boldsymbol{t}} = \boldsymbol{I}
$$

因此残差的平移的雅可比即为残差对全局坐标下的点的雅可比。LOAM 中这部分的代码如下，验证了我们的推导：

matA(i, 3) = coeff.x;   // x
matA(i, 4) = coeff.y;   // y
matA(i, 5) = coeff.z;   // z

对旋转角雅可比的推导

旋转的表示方法有很多中，常见的有旋转矩阵，四元数等。基于 LOAM 的代码中使用欧拉角来表示姿态，因此我们需要使用欧拉角来构造旋转矩阵。基于欧拉角的旋转方式有很多种，具体可以参考 Wikipedia - Euler angles，这里我们用 $\alpha$, $\beta$, $\gamma$ 来分别作为绕 x, y, 轴的旋转。LOAM 原作者选择的是 ZXY 外旋（沿固定 Z 轴旋转-> 沿固定 X 轴旋转 -> 沿固定 Y 轴旋转）表示，由于外旋的旋转矩阵顺序为作乘，因此 ZXY 内旋的旋转矩阵的计算方式为：$\boldsymbol{R} = \boldsymbol{R}_y\boldsymbol{R}_x\boldsymbol{R}_z$，即：

$$
\begin{aligned}
    \boldsymbol{R} &= \boldsymbol{R}_y\boldsymbol{R}_x\boldsymbol{R}_z\\
    &= 
    \begin{bmatrix}
        \cos{\beta} & 0 & \sin{\beta}\\
        0 & 1 & 0\\
        -\sin{\beta} & 0 & \cos{\beta}
    \end{bmatrix}
    \begin{bmatrix}
        1 & 0 & 0\\
        0 & \cos{\alpha} & -\sin{\alpha}\\
        0 & \sin{\alpha} & \cos{\alpha}
    \end{bmatrix}
    \begin{bmatrix}
    \cos{\gamma} & -\sin{\gamma} & 0\\
    \sin{\gamma} &  \cos{\gamma} & 0\\
    0 & 0 & 1
    \end{bmatrix}\\
    &=
    \begin{bmatrix}
        \cos{\beta}\cos{\gamma} + \sin{\beta}\sin{\alpha}\sin{\gamma} & \cos{\gamma}\sin{\beta}\sin{\alpha}-\cos{\beta}\sin{\gamma} & \cos{\alpha}\sin{\beta}\\
        \cos{\alpha}\sin{\gamma} & \cos{\alpha}\cos{\gamma} & -\sin{\alpha}\\
        \cos{\beta}\sin{\alpha}\sin{\gamma}-\cos{\gamma}\sin{\beta} & \cos{\beta}\cos{\gamma}\sin{\alpha}+\sin{\beta}\sin{\gamma} & \cos{\beta}\cos{\alpha}
    \end{bmatrix}
\end{aligned}
$$

下面对雅可比进行推导，由于平移和旋转角没关系，下面我们省略平移：

因此全局坐标点为：

$$
\begin{aligned}
\boldsymbol{R}\boldsymbol{p} &= \boldsymbol{R}_y\boldsymbol{R}_x\boldsymbol{R}_z\boldsymbol{p}\\
&=  
\begin{bmatrix}
    (\cos{\beta}\cos{\gamma} + \sin{\beta}\sin{\alpha}\sin{\gamma})p_x + (\cos{\gamma}\sin{\beta}\sin{\alpha}-\cos{\beta}\sin{\gamma})p_y + (\cos{\alpha}\sin{\beta})p_z\\
    (\cos{\alpha}\sin{\gamma})p_x + (\cos{\alpha}\cos{\gamma})p_y + (-\sin{\alpha})p_z\\
    (\cos{\beta}\sin{\alpha}\sin{\gamma}-\cos{\gamma}\sin{\beta})p_x + (\cos{\beta}\cos{\gamma}\sin{\alpha}+\sin{\beta}\sin{\gamma})p_y + (\cos{\beta}\cos{\alpha})p_z
\end{bmatrix}
\end{aligned}
$$

点对 $\alpha$ 求雅可比有：

$$
\frac{\partial(\boldsymbol{Rp})}{\partial\alpha} =
\begin{bmatrix}
    (\sin{\beta}\cos{\alpha}\sin{\gamma})p_x + (\cos{\gamma}\sin{\beta}\cos{\alpha})p_y - (\sin{\alpha}\sin{\beta})p_z\\
    -(\sin{\alpha}\sin{\gamma})p_x - (\sin{\alpha}\cos{\gamma})p_y - (\cos{\alpha})p_z\\
    (\cos{\beta}\cos{\alpha}\sin{\gamma})p_x + (\cos{\beta}\cos{\gamma}\cos{\alpha})p_y - (\cos{\beta}\sin{\alpha})p_z
\end{bmatrix}
$$

因此残差对 $\alpha$ 的雅可比为：

$$
\begin{aligned}
\boldsymbol{J}_{\alpha} &= \frac{\partial e_c}{\partial \alpha} = \frac{\partial e_c}{\partial \boldsymbol{p}^w} \frac{\partial \boldsymbol{p}^w}{\partial \alpha}\\
&= (\frac{\partial e_c}{\partial \boldsymbol{p}^w})_x(\frac{\partial \boldsymbol{p}^w}{\partial \alpha})_x + (\frac{\partial e_c}{\partial \boldsymbol{p}^w})_y(\frac{\partial \boldsymbol{p}^w}{\partial \alpha})_y + (\frac{\partial e_c}{\partial \boldsymbol{p}^w})_z(\frac{\partial \boldsymbol{p}^w}{\partial \alpha})_z
\end{aligned}
$$

对应到 LOAM 中的代码为：

float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
            + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
            + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;

其中，(sr/cr)(x/y/z) 表示 (sin/cos)(alpha/beta/gamma)。pointOri.x/y/z 对应 Lidar 坐标系下的 x/y/z 坐标，比较可以发现和我们推导结果一致。

点对 $\beta$ 的雅可比为：

$$
\frac{\partial(\boldsymbol{Rp})}{\partial\beta} =
\begin{bmatrix}
    (-\sin{\beta}\cos{\gamma} + \cos{\beta}\sin{\alpha}\sin{\gamma})p_x + (\cos{\gamma}\cos{\beta}\sin{\alpha}+\sin{\beta}\sin{\gamma})p_y + (\cos{\alpha}\cos{\beta})p_z\\
    0\\
    (-\sin{\beta}\sin{\alpha}\sin{\gamma}-\cos{\gamma}\cos{\beta})p_x + (-\sin{\beta}\cos{\gamma}\sin{\alpha}+\cos{\beta}\sin{\gamma})p_y - (\sin{\beta}\cos{\alpha})p_z
\end{bmatrix}
$$

残差对 $\beta$ 的雅可比为：

$$
\begin{aligned}
\boldsymbol{J}_{\beta} &= \frac{\partial e_c}{\partial \beta} = \frac{\partial e_c}{\partial \boldsymbol{p}^w} \frac{\partial \boldsymbol{p}^w}{\partial \beta}\\
&= (\frac{\partial e_c}{\partial \boldsymbol{p}^w})_x(\frac{\partial \boldsymbol{p}^w}{\partial \beta})_x + (\frac{\partial e_c}{\partial \boldsymbol{p}^w})_z(\frac{\partial \boldsymbol{p}^w}{\partial \beta})_z
\end{aligned}
$$

对应到 LOAM 中的代码为：

float ary = ((cry*srx*srz - crz*sry)*pointOri.x 
            + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
            + ((-cry*crz - srx*sry*srz)*pointOri.x 
            + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;

点对 $\gamma$ 的雅可比为：

$$
\frac{\partial(\boldsymbol{Rp})}{\partial\gamma} =
\begin{bmatrix}
    (-\cos{\beta}\sin{\gamma} + \sin{\beta}\sin{\alpha}\cos{\gamma})p_x + (-\sin{\gamma}\sin{\beta}\sin{\alpha}-\cos{\beta}\cos{\gamma})p_y\\
    (\cos{\alpha}\cos{\gamma})p_x - (\cos{\alpha}\sin{\gamma})p_y\\
    (\cos{\beta}\sin{\alpha}\cos{\gamma}+\sin{\gamma}\sin{\beta})p_x + (-\cos{\beta}\sin{\gamma}\sin{\alpha}+\sin{\beta}\cos{\gamma})p_y
\end{bmatrix}
$$

残差对 $\gamma$ 的雅可比为：

$$
\begin{aligned}
\boldsymbol{J}_{\gamma} &= \frac{\partial e_c}{\partial \gamma} = \frac{\partial e_c}{\partial \boldsymbol{p}^w} \frac{\partial \boldsymbol{p}^w}{\partial \gamma}\\
&= (\frac{\partial e_c}{\partial \boldsymbol{p}^w})_x(\frac{\partial \boldsymbol{p}^w}{\partial \gamma})_x + (\frac{\partial e_c}{\partial \boldsymbol{p}^w})_y(\frac{\partial \boldsymbol{p}^w}{\partial \gamma})_y + (\frac{\partial e_c}{\partial \boldsymbol{p}^w})_z(\frac{\partial \boldsymbol{p}^w}{\partial \gamma})_z
\end{aligned}
$$

对应到 LOAM 中的代码为：

float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
            + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
            + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;

使用 LOAM 中的雅可比推导结果中需要注意的问题

事实上上由于欧拉角有万向锁的问题，大部分新的代码框架不会选择用欧拉角来表示旋转状态。即便是在使用欧拉角的场合，大多数情况下都是采取外旋 XYZ （内旋 ZYX）方式，即常说的 roll, pitch, yaw 角的定义关系。这里由于 LOAM 的原作者在论文中只用了欧拉角一种旋转表示方式，因此旋转方式的影响不大（但是在引入 imu 等具有固定的旋转定义时也需要额外注意）。但是在后续的基于 LOAM 代码中，例如 LeGO-LOAM，LIO-SAM 这些使用了旋转矩阵或者四元数来表示旋转的时候，而且使用了第三方库如 TF、PCL、Eigen 时要注意。有一些库（Eigen, TF）可以指定旋转方式，有一些库（PCL）则默认使用外旋 XYZ 方式，例如 LIO-SAM 中使用了 PCL 的接口来对 RPY 角和其他旋转格式之间转换，如下所示：

pcl::getTranslationAndEulerAngles(transBetween, x, y, z, roll, pitch, yaw);

这个函数将位姿变换矩阵转化为三维平移（对应 x, y, z）和三维旋转（对应 roll, pitch, yaw）。而 PCL 中关于这个 RPY 的定义是在内旋 ZYX （外旋 XYZ），此时如果我们直接将 $\alpha, \beta, \gamma$ 对应到 roll, pitch, yaw 中会出现问题。那怎么办呢？有三个选择：

1. 自己定义转换方程，这样做可以自己选择任意欧拉角旋转方式，但这么做没有必要
2. 使用外旋 XYZ，重新推导一次上述雅可比结果，这么做比较稳妥，但用欧拉角涉及到很多三角函数的运算，推导实在很繁琐
3. （LIO-SAM 中使用的方法）在其他地方使用 RPY 角作为状态表示，在计算点对姿态角的残差雅可比时建立两种表示方法的映射，将 RPY 角（以及对应的坐标轴）转为 LOAM 定义的方式，然后可以直接用 LOAM 的推导结果
4. 可以使用 RPY 角，但在计算雅可比时可以先将 RPY 转为旋转矩阵，通过左右扰动求点对旋转矩阵的雅可比。更新完成后再将旋转矩阵转为 RPY 角即可
5. 从头到尾不适用 RPY 角，而使用其他旋转表示方式。

关于 LIO-SAM 中使用的这种映射方式，可以用下图说明：

因此，通过上图中的坐标旋转，我们找到 ZXY 外旋和 XYZ 外旋的对应关系，如下所示：

• （LOAM）X 轴 – (RPY) Y 轴
• （LOAM）Y 轴 – (RPY) Z 轴
• （LOAM）Z 轴 – (RPY) X 轴
• （LOAM）$\alpha$ – (RPY) pitch
• （LOAM）$\beta$ – (RPY) yaw
• （LOAM）$\gamma$ – (RPY) roll

LIO-SAM 中的代码也有类似的介绍（虽然不是很清楚为什么要提 Camera），整个转换过程的代码如下:

bool LMOptimization(int iterCount)
{
    // LOAM 原始代码的旋转是基于 ZXY 外旋，需要建立和 RPY 使用的 XYZ 外旋建立映射关系 
    // This optimization is from the original loam_velodyne by Ji Zhang, need to cope with coordinate transformation
    // lidar <- camera      ---     camera <- lidar
    // x = z                ---     x = y
    // y = x                ---     y = z
    // z = y                ---     z = x
    // roll = yaw           ---     roll = pitch
    // pitch = roll         ---     pitch = yaw
    // yaw = pitch          ---     yaw = roll



    // 使用 ZXY 定义计算 (sin/cos)(alpha/beta/gamma) 
    float srx = sin(transformTobeMapped[1]); // Lidar: sin(pitch) --> Camera: sin(roll) 
    float crx = cos(transformTobeMapped[1]); // Lidar: cos(pitch) --> Camera: cos(roll) 
    float sry = sin(transformTobeMapped[2]); // Lidar: sin(yaw) --> Camera: sin(pitch) 
    float cry = cos(transformTobeMapped[2]); // Lidar: cos(yaw) --> Camera: cos(pitch) 
    float srz = sin(transformTobeMapped[0]); // Lidar: sin(roll) --> Camera: sin(yaw) 
    float crz = cos(transformTobeMapped[0]); // Lidar: cos(roll) --> Camera: cos(yaw) 

    ...

    PointType pointOri, coeff; // ZXY 外旋定义坐标系下的表示
    

    for (int i = 0; i < laserCloudSelNum; i++) {
        // lidar -> camera -- 根据上面映射将点的坐标映射至 ZXY 外旋定义坐标系
        pointOri.x = laserCloudOri->points[i].y;
        pointOri.y = laserCloudOri->points[i].z;
        pointOri.z = laserCloudOri->points[i].x;
        // lidar -> camera -- 根据上面映射将残差对点的雅可比映射至 ZXY 外旋定义坐标系
        coeff.x = coeffSel->points[i].y;
        coeff.y = coeffSel->points[i].z;
        coeff.z = coeffSel->points[i].x;
        coeff.intensity = coeffSel->points[i].intensity;
        // in camera -- 进行 ZXY 外旋定义坐标系下的雅可比计算
        float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
                    + (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
                    + (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;

        float ary = ((cry*srx*srz - crz*sry)*pointOri.x 
                    + (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
                    + ((-cry*crz - srx*sry*srz)*pointOri.x 
                    + (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;

        float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz-srx*sry*srz)*pointOri.y)*coeff.x
                    + (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
                    + ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry-cry*srx*srz)*pointOri.y)*coeff.z;
        // lidar -> camera -- 将 ZXY 外旋定义坐标系下的计算结果重新映射为 XYZ 外旋代入雅可比矩阵中
        matA.at<float>(i, 0) = arz;
        matA.at<float>(i, 1) = arx;
        matA.at<float>(i, 2) = ary;
        matA.at<float>(i, 3) = coeff.z;
        matA.at<float>(i, 4) = coeff.x;
        matA.at<float>(i, 5) = coeff.y;
        matB.at<float>(i, 0) = -coeff.intensity;
    }

    ...
}

参考资料

• Wikipedia - Euler angles
• loam_velodyne
• 旋转的左乘与右乘
• 欧拉角与旋转矩阵的转换关系