在 VSLAM 的后端优化中的重投影误差的雅可比计算详细推导对于相机位姿的变换可以通过旋转矩阵或者四元数进行表示，除了

本文已参与「新人创作礼」活动，一起开启掘金创作之路。

对于相机位姿的变换可以通过旋转矩阵或者四元数进行表示，对于旋转矩阵的定义满足：

R \begin{cases} |R| = 1 \\ RR^T = I\\ \end{cases}

即 $R$ 为正交矩阵，且行列式为1 。旋转矩阵的行列式为什么等于1?

除了旋转矩阵外，还需要仿射矩阵，仿射矩阵是在经过线性变换后进行平移变换。因为平移变换无法用两个矩阵相乘的方式表示，因此通过齐次坐标可以很好的解决这个问题，所谓齐次坐标就是将一个原本是 $n$ 维的向量用一个 $n+1$ 维向量来表示。

相机的位姿变换为一个连续且平滑的过程，连续指的是左连续等于右连续，而平滑指的是存在高阶导数。那么这些连续的位姿变换再加上乘法即构成了李群 $SO(3)$ （特殊正交群）。

在后端优化的过程中，导入李代数的原因有：关于李群与李代数的的理解与总结（1）旋转矩阵需要满足正交矩阵的约束，额外的约束会增加优化的困难。（2）解决李群中不存在加法运算，无法描述求导的问题。

已知旋转矩阵的导数可以用一个反对称矩阵和旋转矩阵本身表示：

\dot{R(t)}= \phi(t) ^∧R(t)

那么根据微分方程，可以得到

{I \over R(t)}dR(t)= \phi(t) ^∧dt

lnR(t)= \phi(t) ^∧t+c

R(t)= e^{\phi(t) ^∧t+c }

且当 $t=0$ 时， $R$ 为单位阵 $I$ ，因此可简化得到： $R(t)= e^{\phi(t) ^∧t }$ 由此便引出了李代数与李群之间的关系。

在解决旋转矩阵求导之前，需要了解BCH(baker Campbell hausdorff)公式，描述了当两个李群的矩阵相乘时，李代数如何运算。

e^{\phi_1 ^∧}e^{\phi_2^∧} = e^{(\phi_1+\phi_2) ^∧}

ln(exp(\phi_1 ^∧)exp(\phi_2^∧)) = \phi_1 ^∧+\phi_2 ^∧+{1\over 2}[\phi_1 ^∧,\phi_2 ^∧]+{1\over 12}[\phi_1 ^∧,[\phi_1 ^∧,\phi_2 ^∧]]-{1\over 12}[\phi_2 ^∧,[\phi_1 ^∧,\phi_2 ^∧]]\cdots

其中 $[,]$ 为李括号，李括号的运算为：

[\phi_1,\phi_2]=[\Phi_1\Phi_2-\Phi_2\Phi_1]^\vee

BCH公式中，当向量为小量时，二次以上的项都将被忽略，所以BCH公式的近似表达式为：

ln(exp(\phi_1 ^∧)exp(\phi_2^∧)) \approx \begin{cases} J_l (\phi_2)^{-1}\phi_1 +\phi_2,当\phi_1为小量时\\ J_r (\phi_1)^{-1}\phi_2 +\phi_1, 当\phi_2为小量时\\ \end{cases}

近似的BCH公式按 $\phi$ 的大小不同，可以将两式分为左乘和右乘。 左乘近似雅可比可以表示成下式：

J_r = J = {sin\theta\over \theta}I+(1-{sin \theta \over \theta})aa^T+{1-cos \theta \over \theta }a^∧

J_r^{-1} = {\theta\over 2}cot{\theta\over 2}I+(1-{\theta\over 2}cot{\theta\over 2})aa^T-{\theta\over 2}a^∧

右乘近似雅可比仅需要对自变量取负号：

J_r(\phi) = J_l(-\phi)

参考文章：李代数求导与扰动模型

在SLAM中，我们经常会构建与位姿有关的残差函数，然后讨论该函数关于位姿的雅克比，优化当前的估计值。使用李代数解决求导问题的思路分为两种： （1）用李代数表示姿态，然后对根据李代数加法来对李代数求导。 根据导数的定义，在李代数上做加法：

exp((\phi+\Delta\phi)^∧ = exp((J_l \Delta \phi)^∧)exp(\phi^∧) = exp(\phi^∧)exp((J_r \Delta\phi)^∧)

用李代数表示姿态： $R=exp(\phi^∧)$

{d(RP) \over dR} \implies {d( exp(\phi^∧)p) \over d\phi} = \lim_{\delta \phi\to\ 0}{exp(\phi+\delta \phi)^∧p-exp(\phi^∧)p \over \delta \phi}

= \lim_{\delta \phi\to\ 0}{ exp((J_l \delta \phi)^∧)exp(\phi^∧) p-exp(\phi^∧)p \over \delta \phi}

利用极限公式： $e^{x} = 1+x$ $= \lim_{\delta \phi\to\ 0}{ (I+(J_l \delta \phi)^∧)exp(\phi^∧) p-exp(\phi^∧)p \over \delta \phi}$ $= \lim_{\delta \phi\to\ 0}{ (J_l \delta \phi)^∧exp(\phi^∧) p \over \delta \phi}$ 根据反对称矩阵： $A^TB = -B^TA$ $= \lim_{\delta \phi\to\ 0}{ (J_l \delta \phi)^∧exp(\phi^∧) p \over \delta \phi}= \lim_{\delta \phi\to\ 0}{ -(exp(\phi^∧) p)^∧J_l \delta \phi \over \delta \phi} = -(Rp)^∧J_l$

（2）对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。 把增量扰动直接添加在李群上，然后使用李代数表示此扰动（左扰动）： ${d(RP) \over d\varphi} = \lim_{\varphi\to\ 0}{exp(\varphi^∧)exp(\phi^∧)p-exp(\phi^∧)p \over \varphi}$ $= \lim_{\varphi\to\ 0}{(I+\varphi^∧)exp(\phi^∧)p-exp(\phi^∧)p \over \varphi}$ $= \lim_{\varphi\to\ 0}{\varphi^∧exp(\phi^∧)p \over \varphi} = \lim_{\varphi\to\ 0}{\varphi^∧Rp \over \varphi} = \lim_{\varphi\to\ 0}{- (Rp)^∧\varphi \over \varphi} =- (Rp)^∧$ 因此，左扰动模型比微分模型要少一个雅可比矩阵。

对于如何选择右扰动还是左扰动，参考知乎文章旋转的左扰动和右扰动。

一般旋转扰动的定义是在机体 $body$ 坐标系上添加一个小的旋转 $\theta^b_{b'}$ ，这样扰动的旋转角比较小，可以保证比较好的线性而且避免奇异性，即 $R_{b'}^b = [\theta^b_{b'}]^∧$ 。

（1）当 $pose$ 表达在 $world$ 坐标系时，根据旋转的叠加方式，添加的是右扰动： $R_{b'}^w = R_{b}^w R_{b'}^b = R_{b}^w(I+ [\theta^b_{b'}]^∧)$ （2）当 $pose$ 表达在 $body$ 坐标系时，根据旋转的叠加方式，添加的是左扰动： $R^{b'}_w = R^{b'}_b R^{b}_w = (I+ [\theta_b^{b'}]^∧)R^{b}_w = (I- [\theta^b_{b'}]^∧)R^{b}_w$

参考博客：VINS-MONO ProjectionFactor代码分析及公式推导在重投影误差中，以VINS为例，重投影误差计算的是单位球面切平面上的误差，单位球面模型相对于单位平面模型可以适用的相机模型更广泛。其中确定的常量为切平面基构成的矩阵 $B$ 和输入的特征点 $l$ 在单位相机平面的投影 $(P_l^{c_i},P_l^{cj})$ ，需要优化的变量包括第 $i、j$ 两帧在世界坐标系下的姿态 $(R_{b_i}^w,R_{b_j}^w)$ ，IMU与相机之间的外参关系 $(R_c^b,t_c^b)$ ，逆深度 $(\lambda_l)$ 。

那么第 $l$ 特征点在第 $j$ 帧下的重投影坐标 $P_l^{c_j'}$ 为： $P_l^{c_j'}= \underbrace{{R_c^b}^T \{ \underbrace{{R_{b_j}^{w}}^T[\overbrace{R_{b_i}^w(\underbrace{R_c^b{P_l^{c_i} \over \lambda_l}+t_c^b}_{\text{转换到IMU坐标系}} )+t_{b_i}^w}^{\text{转换到世界坐标系}}] - t_c^b}_{\text{转换到第}j\text{帧IMU坐标系下}} \}}_{\text{转换到第}j\text{帧相机坐标系下}}$

//pts_i为l个特征点在第i帧单位相机平面的投影,pts_j同理(常量)
// 下列变量中其中只有 tangent_base、pts_i、pts_j为常量，其他都是待优化变量
// 第i帧单位相机坐标系 -> 第i帧相机坐标系 -> 第i帧IMU 坐标系 -> 世界坐标系 -> 第j帧IMU 坐标系 ->  第j帧相机坐标系
Eigen::Vector3d pts_camera_i = pts_i / inv_dep_i; // 特征点的逆深度 inv_dep_i
Eigen::Vector3d pts_imu_i = qic * pts_camera_i + tic; // imu和相机间的外参
Eigen::Vector3d pts_w = Qi * pts_imu_i + Pi; // 第i帧的位姿
Eigen::Vector3d pts_imu_j = Qj.inverse() * (pts_w - Pj); // 第j帧的位姿
Eigen::Vector3d pts_camera_j = qic.inverse() * (pts_imu_j - tic); // pts_i在第j帧的重投影
Eigen::Map<Eigen::Vector2d> residual(residuals);

因此残差公式可以表示为： $e = B *({P_l^{c_j'} \over \lVert P_l^{c'_j} \rVert_2}-{P_l^{c_j} \over \lVert P_l^{c_j} \rVert_2})$ 令 $T$ 为待优化的变量， $r$ 表示投影前的残差，即 $r={P_l^{c_j'} \over \lVert P_l^{c'_j} \rVert_2}-{P_l^{c_j} \over \lVert P_l^{c_j} \rVert_2}$ ，通过链式法则，可得： ${∂e \over ∂T} ={∂e \over ∂r} {∂r \over ∂{P_l^{c_j'} \over \lVert P_l^{c'_j} \rVert_2}} {{P_l^{c_j'} \over \lVert P_l^{c'_j} \rVert_2} \over ∂T}$ 其中因为 ${P_l^{c_j} \over \lVert P_l^{c_j} \rVert_2}$ 为常量，因此： ${∂e \over ∂r} {∂r \over ∂{P_l^{c_j'} \over \lVert P_l^{c'_j} \rVert_2}} = B$ 化简得到： ${∂e \over ∂T} = B {{P_l^{c_j'} \over \lVert P_l^{c'_j} \rVert_2} \over ∂T}$ 综上，残差的雅可比矩阵，等于 ${{P_l^{c_j'} \over \lVert P_l^{c'_j} \rVert_2}}$ 的雅可比矩阵在切平面上的投影。

那么对于该雅可比矩阵如何求，设 $f(x) = P_l^{c_j'}$ ，由链式法则可以得到： ${d{f(x) \over \lVert f(x) \rVert_2}\over dx} =({1 \over \lVert f(x) \rVert_2}- \lVert f(x) \rVert_2^3)f(x)f(x)^T)f'(x)$ 具体求导过程省略，上式中前半部分是关于重投影坐标的已知函数，因此只需要求解得到 $f'(x)$ ，就可以得到残差的雅可比矩阵，接下来对 $P_l^{c_j'}$ 求导。

上一部分已经推导出第 $j$ 帧下的重投影坐标 $P_l^{c_j'}$ ，将该公式展开： $P_l^{c_j'} = {R_c^b}^T {R_{b_j}^w}^T {R_{b_i}^w} {R_{c}^b} {P_{l}^{c_i}}{1 \over \lambda_l} + {R_{c}^b}^T {R_{b_j}^w}^T {R_{b_i}^w} {t_{c}^b} + {R_{c}^b}^T {R_{b_j}^w}^T {t_{b_i}^w} - {R_{c}^b}^T {R_{b_j}^w}^T {t_{b_j}^w}- {R_{c}^b}^T {t_{c}^b}^T$ 需要优化的变量都需要进行求导。包括 $(R_{b_i}^w,R_{b_j}^w),(R_c^b,t_c^b),(\lambda_l)$ 。

（1）第 $i、j$ 两帧在世界坐标系下的姿态 $(R_{b_i}^w,R_{b_j}^w)$ （2）IMU与相机之间的外参关系 $(R_c^b,t_c^b)$ （3）逆深度 $(\lambda_l)$

以第 $i$ 帧为例，需要对旋转 ${R_{b_i}^w}$ 和平移 ${t_{b_i}^w}$ 进行求导。将除此之外的变量设为 $A = {R_{c}^b}^T {R_{b_j}^w}^T$ 和 $x = {R_{c}^b} {P_{l}^{c_i}}{1 \over \lambda_l} + {t_{c}^b}$ 即化简得到(与优化量无关的后两项舍去)：

P_l^{c_j'} = A(R_{b_j}^wx+t_{b_i}^w)

对 ${t_{b_i}^w}$ 进行求导：

{∂P_l^{c_j'} \over ∂{t_{b_i}^w}} =A= {R_{c}^b}^T {R_{b_j}^w}^T

对 ${R_{b_i}^w}$ 进行求导，因为 $pose$ 表达在 $world$ 坐标系，根据旋转的叠加方式，添加的是右扰动：

{∂P_l^{c_i'} \over ∂\varphi} = \lim_{\varphi\to\ 0}{ A({exp(\phi_i^∧)} {exp(\varphi^∧)}x+t_{b_i}^w)-A({exp(\phi^∧_i)}x+t_{b_i}^w)\over \varphi}

= \lim_{\varphi\to\ 0}{ A{exp(\phi_i^∧)} [{exp(\varphi^∧)}-I]x\over \varphi}

= \lim_{\varphi\to\ 0}{ A{exp(\phi_i^∧)} [{I+\varphi^∧}-I]x\over \varphi}

= \lim_{\varphi\to\ 0}{ A{exp(\phi_i^∧)} {\varphi^∧}x\over \varphi} = \lim_{\varphi\to\ 0}{- A{exp(\phi_i^∧)}x^∧ {\varphi}\over \varphi} = \lim_{\varphi\to\ 0}{- A{exp(\phi_i^∧)}x^∧}

将 $A$ 与 $x$ 带入可得到：

{∂P_l^{c_i'} \over ∂\varphi} = - {R_{c}^b}^T {R_{b_j}^w}^T{R_{b_i}^w}( {R_{c}^b} {P_{l}^{c_i}}{1 \over \lambda_l} + {t_{c}^b})^∧

综上重投影坐标 $P_l^{c_j'}$ 对位姿的求导已经推导完成，其余求导相似的方法，略略略！

扰动模型的求导可以替代关于状态量的雅可比，扰动模型 $F$ 下误差状态的求导，将它在 $dx=0$ 处做泰勒展开：

f(x_0+dx) = F(dx) \\ =F(0)+F'(0)(dx-0) \\ =f(x_0)+{∂f(x_0\bigoplus dx) \over ∂dx}dx