SLAM如何在Ceres中计算残差对四元数的雅可比矩阵

姿态可以进行数学运算的表达式有两种。一种是四元数Q，一种是旋转矩阵R。
假设在SLAM算法中构建了一个姿态残差如下：
$$e=Q_{var}^{-1}\ast Q_2$$
或者$$e=R_{var}^{-1}\ast R_2$$
$$其中Q_{var}，R_{var}是待优化变量，Q_2，R_2是约束$$
$$e如何对Q_{var}和R_{var}求导？$$

通过四元数方法进行求导

残差对Manifold变量求导

在VINS-MONO中预计分的姿态约束公式如下：
$$r_q=2\ast [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>()$$
$$VINSMONO中是通过右扰动的方式推导残差对四元数的导数$$$$以对Q_j为例,对Qj右乘一个小扰动\left[\begin{array}{c}1\\ \theta \end{array}\right]：$$
$$li{m}_{\theta \to 0}\frac{Q_i^j\otimes Q_i^{-1}\otimes （Q_j\otimes \left[\begin{array}{c}1\\ \theta \end{array}\right]）-Q_i^j\otimes Q_i^{-1}\otimes （Q_j\otimes \left[\begin{array}{c}1\\ 0\end{array}\right]）}{\theta }$$
$$=R_{left}(Q_i^j\otimes Q_i^{-1}\otimes Q_j)li{m}_{\theta \to 0}\frac{\left[\begin{array}{c}1\\ \theta \end{array}\right]-\left[\begin{array}{c}1\\ 0\end{array}\right]}{\theta }$$
$$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矩阵：
$$li{m}_{\theta \to 0}\frac{\left[\begin{array}{c}1\\ \theta \end{array}\right]-\left[\begin{array}{c}1\\ 0\end{array}\right]}{\theta }=\frac{\partial \left[\begin{array}{c}1\\ \theta \end{array}\right]}{\theta }$$
结果如下：前三行是位置的和姿态无关不用管，后四行是姿态的。
$$J_{manifold}^{tangent}=\left[\begin{array}{cccccc}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{array}\right]$$
VINS MONO中LocalParameterization中ComputeJacobian是$$\frac{\partial \left[\begin{array}{c}1\\ \theta \end{array}\right]}{\theta }$$
VINS MONO的结果是$$\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 0\end{array}\right]$$
其中全是0的那行从第一行放到第四行。其对应的残差对Manifold变量求导的雅可比矩阵中第一列放到最后一列。
Ceres中自带的QuaternionParameterization 中ComputeJacobian是：
$$\frac{\partial Q\otimes \left[\begin{array}{c}1\\ \theta \end{array}\right]}{\theta }$$
其结果是：

Ceres根据链式法则最终计算 残差对Manifold变量的雅可比矩阵和Manifold变量对Tangent变量的雅可比矩阵的乘积。殊途同归，两种方法只要最后的雅可比矩阵相同就行。关于LocalParameterization类型变量雅可比矩阵计算方式可以看我另一篇博客：
Ceres中LocalParameterization参数Jacobian矩阵计算过程

自定义的角度约束残差推导

然后借鉴VINS-MONO中求导的方法,自己定义的残差：
$$r_q=2\ast [Q_{imu}^{-1}\otimes Q_i].vec$$
$$其中Q_i是待优化变量,r_q对Q_i求导,其表达式为：$$
$$R_{left}(Q_{imu}^{-1}\otimes Q_i)$$
LocalParameterization中定义的jacobian为：
$$\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 0\end{array}\right]$$
其对应的残差对Manifold变量求导的雅可比矩阵中第一列放到最后一列。
开始用上述雅可比矩阵求导的时候，发现残差并不会收敛，发现是没有加Eigen::RowMajor。