SLAM如何在Ceres中计算残差对四元数的雅可比矩阵
SLAM如何在Ceres中计算残差对四元数的雅可比矩阵
在SLAM（同时定位与地图构建）算法中，姿态优化是一个核心问题。本文将详细介绍如何在Ceres优化框架下计算残差对四元数的雅可比矩阵，这对于理解SLAM算法中的姿态优化过程具有重要参考价值。
姿态可以进行数学运算的表达式有两种。一种是四元数Q，一种是旋转矩阵R。
假设在SLAM算法中构建了一个姿态残差如下：
$$e = Q_{var}^{-1} * Q_2$$
或者
$$e = R_{var}^{-1} * R_2$$
其中 $Q_{var}$ 和 $R_{var}$ 是待优化变量，$Q_2$ 和 $R_2$ 是约束。
那么，如何对 $Q_{var}$ 和 $R_{var}$ 求导呢？
通过四元数方法进行求导
残差对Manifold变量求导
在VINS-MONO中预计分的姿态约束公式如下：
$$r_q = 2 * [ Q_i^j \otimes （ Q_i^{-1} \otimes Q_j ) ]_{vec}$$
其中 $Q_i^j$ 表示 $i, j$ 帧之间的姿态预积分的逆，$Q_i, Q_j$ 表示第 $i, j$ 帧的姿态是待优化的量。
然后 $r_q$ 对姿态 $Q_j$ 求导，得到：
$$J_r^Q = R_{left}(Q_i^j \otimes Q_i^{-1} \otimes Q_j).bottomRightCorner<3, 3>()$$
VINS-MONO是通过右扰动的方式推导残差对四元数的导数。以对 $Q_j$ 为例，对 $Q_j$ 右乘一个小扰动：
$$\lim_{\theta \to 0} \frac{ Q_i^j\otimes Q_i^{-1}\otimes （Q_j \otimes \begin{bmatrix} 1 \ \theta \end{bmatrix}）-Q_i^j\otimes Q_i^{-1}\otimes （Q_j \otimes \begin{bmatrix} 1 \ 0 \end{bmatrix}）} {\theta}$$
$$= R_{left}(Q_i^j \otimes Q_i^{-1} \otimes Q_j) \lim_{\theta \to 0} \frac{ \begin{bmatrix} 1 \ \theta \end{bmatrix}-\begin{bmatrix} 1 \ 0 \end{bmatrix}} {\theta}$$
$$= R_{left}(Q_i^j \otimes Q_i^{-1} \otimes Q_j)$$
其中 $R_{left}(Q_i^j \otimes Q_i^{-1} \otimes Q_j)$ 就是残差对四元数的导数，$R_{left}$ 表示四元数左矩阵。
因为VINS-MONO只取了四元数的虚部作为残差，所以只取了矩阵的下面三行。
具体推导过程可以看这个链接的第42页：VINS MONO公式推导
四元数的基础概念可以查看下述链接：四元数速查手册
Manifold变量对Tangent变量求导
然后VINS-MONO在LocalParameterization类中定义了四元数
Manifold空间相对于Tangent空间的jacobian矩阵：
$$\lim_{\theta \to 0} \frac{ \begin{bmatrix} 1 \ \theta \end{bmatrix}-\begin{bmatrix} 1 \ 0 \end{bmatrix}} {\theta} = \frac{\partial\begin{bmatrix} 1 \ \theta \end{bmatrix}}{\theta}$$
结果如下：前三行是位置的和姿态无关不用管，后四行是姿态的。
$$J_{manifold}^{tangent}=\begin{bmatrix} 1 & 0 & 0& 0 & 0& 0 \ 0 & 1 & 0& 0 & 0& 0\ 0 & 0 &1& 0 & 0& 0\ 0 & 0 &0& 1 & 0& 0\ 0 & 0 &0& 0 & 1& 0\ 0 & 0 &0& 0 & 0& 1\ 0 & 0 &0& 0 & 0& 0\ \end{bmatrix}$$
VINS-MONO中LocalParameterization中ComputeJacobian是：
$$\frac{\partial\begin{bmatrix} 1 \ \theta \end{bmatrix}}{\theta}$$
VINS-MONO的结果是：
$$\begin{bmatrix}1 &0 & 0 &0 \0 &1 & 0 &0 \0 & 0 & 1 &0 \0 &0 & 0 &0 \\end{bmatrix}$$
其中全是0的那行从第一行放到第四行。其对应的残差对Manifold变量求导的雅可比矩阵中第一列放到最后一列。
Ceres中自带的QuaternionParameterization 中ComputeJacobian是：
$$\frac{\partial Q \otimes \begin{bmatrix} 1 \ \theta \end{bmatrix}}{\theta}$$
其结果是：
Ceres根据链式法则最终计算残差对Manifold变量的雅可比矩阵和Manifold变量对Tangent变量的雅可比矩阵的乘积。殊途同归，两种方法只要最后的雅可比矩阵相同就行。关于LocalParameterization类型变量雅可比矩阵计算方式可以看我另一篇博客：
Ceres中LocalParameterization参数Jacobian矩阵计算过程
自定义的角度约束残差推导
然后借鉴VINS-MONO中求导的方法,自己定义的残差：
$$r_q = 2*[Q_{imu}^{-1} \otimes Q_i].vec$$
其中 $Q_i$ 是待优化变量, $r_q$ 对 $Q_i$ 求导, 其表达式为：
$$R_{left}(Q_{imu}^{-1} \otimes Q_i)$$
LocalParameterization中定义的jacobian为：
$$\begin{bmatrix}1 &0 & 0 &0 \0 &1 & 0 &0 \0 & 0 & 1 &0 \0 &0 & 0 &0 \\end{bmatrix}$$
其对应的残差对Manifold变量求导的雅可比矩阵中第一列放到最后一列。
开始用上述雅可比矩阵求导的时候，发现残差并不会收敛，发现是没有加Eigen::RowMajor。