在 VSLAM 的后端优化中的重投影误差的雅可比计算详细推导

对于相机位姿的变换可以通过旋转矩阵或者四元数进行表示，对于旋转矩阵的定义满足：
$$R\{\begin{array}{l}|R|=1\\ R{R}^T=I\end{array}$$
即$R$ 为正交矩阵，且行列式为1 。 旋转矩阵的行列式为什么等于1?

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

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

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

已知旋转矩阵的导数可以用一个反对称矩阵和旋转矩阵本身表示：
$$\dot{R(t)}=\varphi (t{)}^{\wedge }R(t)$$那么根据微分方程，可以得到$$\frac{I}{R(t)}dR(t)=\varphi (t{)}^{\wedge }dt$$$$lnR(t)=\varphi (t{)}^{\wedge }t+c$$$$R(t)=e^{\varphi (t{)}^{\wedge }t+c}$$且当$t=0$时，$R$为单位阵$I$，因此可简化得到：$$R(t)=e^{\varphi (t{)}^{\wedge }t}$$由此便引出了李代数与李群之间的关系。

---

在解决旋转矩阵求导之前，需要了解BCH(baker Campbell hausdorff)公式，描述了当两个李群的矩阵相乘时，李代数如何运算。
$$e^{{\varphi }_1^{\wedge }}{e}^{{\varphi }_2^{\wedge }}=e^{({\varphi }_1+{\varphi }_2{)}^{\wedge }}$$$$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }))={\varphi }_1^{\wedge }+{\varphi }_2^{\wedge }+\frac{1}{2}[{\varphi }_1^{\wedge },{\varphi }_2^{\wedge }]+\frac{1}{12}[{\varphi }_1^{\wedge },[{\varphi }_1^{\wedge },{\varphi }_2^{\wedge }]]-\frac{1}{12}[{\varphi }_2^{\wedge },[{\varphi }_1^{\wedge },{\varphi }_2^{\wedge }]]\cdots$$
其中$[,]$为李括号，李括号的运算为：$$[{\varphi }_1,{\varphi }_2]=[{\Phi }_1{\Phi }_2-{\Phi }_2{\Phi }_1{]}^{\vee }$$
BCH公式中，当向量为小量时，二次以上的项都将被忽略，所以BCH公式的近似表达式为：
$$ln(exp({\varphi }_1^{\wedge })exp({\varphi }_2^{\wedge }))\approx \{\begin{array}{l}{J}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2,当{\varphi }_1为小量时\\ J_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1,当{\varphi }_2为小量时\end{array}$$
近似的BCH公式按$\varphi$的大小不同，可以将两式分为左乘和右乘。
左乘近似雅可比可以表示成下式：

$$J_r=J=\frac{sin\theta }{\theta }I+(1-\frac{sin\theta }{\theta })a{a}^T+\frac{1-cos\theta }{\theta }{a}^{\wedge }$$$$J_r^{-1}=\frac{\theta }{2}cot\frac{\theta }{2}I+(1-\frac{\theta }{2}cot\frac{\theta }{2})a{a}^T-\frac{\theta }{2}{a}^{\wedge }$$

右乘近似雅可比仅需要对自变量取负号：
$$J_r(\varphi )=J_l(-\varphi )$$

---

参考文章：李代数求导与扰动模型
在SLAM中，我们经常会构建与位姿有关的残差函数，然后讨论该函数关于位姿的雅克比，优化当前的估计值。使用李代数解决求导问题的思路分为两种：
（1）用李代数表示姿态，然后对根据李代数加法来对李代数求导。
根据导数的定义，在李代数上做加法：
$$exp((\varphi +\Delta \varphi {)}^{\wedge }=exp((J_l\Delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })=exp({\varphi }^{\wedge })exp((J_r\Delta \varphi {)}^{\wedge })$$
用李代数表示姿态：$R=exp({\varphi }^{\wedge })$ $$\frac{d(RP)}{dR}⟹\frac{d(exp({\varphi }^{\wedge })p)}{d\varphi }=\underset{\delta \varphi \to 0}{\lim }\frac{exp(\varphi +\delta \varphi {)}^{\wedge }p-exp({\varphi }^{\wedge })p}{\delta \varphi }$$ $$=\underset{\delta \varphi \to 0}{\lim }\frac{exp((J_l\delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi }$$
利用极限公式：$e^x=1+x$ $$=\underset{\delta \varphi \to 0}{\lim }\frac{(I+(J_l\delta \varphi {)}^{\wedge })exp({\varphi }^{\wedge })p-exp({\varphi }^{\wedge })p}{\delta \varphi }$$ $$=\underset{\delta \varphi \to 0}{\lim }\frac{(J_l\delta \varphi {)}^{\wedge }exp({\varphi }^{\wedge })p}{\delta \varphi }$$ 根据反对称矩阵：$A^{T}B=-B^{T}A$ $$=\underset{\delta \varphi \to 0}{\lim }\frac{(J_l\delta \varphi {)}^{\wedge }exp({\varphi }^{\wedge })p}{\delta \varphi }=\underset{\delta \varphi \to 0}{\lim }\frac{-(exp({\varphi }^{\wedge })p{)}^{\wedge }{J}_l\delta \varphi }{\delta \varphi }=-(Rp{)}^{\wedge }{J}_l$$

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

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

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

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

---

参考博客：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^{\prime }}$为：
P l c j ′ = R c b T { R b j w T [ R b i w ( R c b P l c i λ l + t c b ⏟ 转换到IMU坐标系 ) + t b i w ⏞ 转换到世界坐标系 ] − t c b ⏟ 转换到第 j 帧IMU坐标系下 } ⏟ 转换到第 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{帧相机坐标系下}} Plcj′​​=转换到第j帧相机坐标系下 Rcb​T{转换到第j帧IMU坐标系下 Rbj​w​T[Rbi​w​(转换到IMU坐标系 Rcb​λl​Plci​​​+tcb​​​)+tbi​w​ ​转换到世界坐标系​]−tcb​​​}​​

//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\ast (\frac{P_l^{c_j^{\prime }}}{\Vert P_l^{c_j^{\prime }}{\Vert }_2}-\frac{P_l^{c_j}}{\Vert P_l^{c_j}{\Vert }_2})$$令$T$为待优化的变量，$r$表示投影前的残差，即$r=\frac{P_l^{c_j^{\prime }}}{\Vert P_l^{c_j^{\prime }}{\Vert }_2}-\frac{P_l^{c_j}}{\Vert P_l^{c_j}{\Vert }_2}$，通过链式法则，可得：$$\frac{\partial e}{\partial T}=\frac{\partial e}{\partial r}\frac{\partial r}{\partial \frac{P_l^{c_j^{\prime }}}{\Vert P_l^{c_j^{\prime }}{\Vert }_2}}\frac{\frac{P_l^{c_j^{\prime }}}{\Vert P_l^{c_j^{\prime }}{\Vert }_2}}{\partial T}$$其中因为$\frac{P_l^{c_j}}{\Vert P_l^{c_j}{\Vert }_2}$为常量，因此：$$\frac{\partial e}{\partial r}\frac{\partial r}{\partial \frac{P_l^{c_j^{\prime }}}{\Vert P_l^{c_j^{\prime }}{\Vert }_2}}=B$$ 化简得到：$$\frac{\partial e}{\partial T}=B\frac{\frac{P_l^{c_j^{\prime }}}{\Vert P_l^{c_j^{\prime }}{\Vert }_2}}{\partial T}$$
综上，残差的雅可比矩阵，等于$\frac{P_l^{c_j^{\prime }}}{\Vert P_l^{c_j^{\prime }}{\Vert }_2}$的雅可比矩阵在切平面上的投影。

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

---

上一部分已经推导出第$j$帧下的重投影坐标$P_l^{c_j^{\prime }}$，将该公式展开：
$$P_l^{c_j^{\prime }}={R_c^b}^T{R_{b_j}^w}^T{R}_{b_i}^w{R}_c^b{P}_l^{c_i}\frac{1}{{\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}\frac{1}{{\lambda }_l}+t_c^b$
即化简得到(与优化量无关的后两项舍去)：$$P_l^{c_j^{\prime }}=A(R_{b_j}^{w}x+t_{b_i}^w)$$对$t_{b_i}^w$进行求导：
$$\frac{\partial P_l^{c_j^{\prime }}}{\partial t_{b_i}^w}=A={R_c^b}^T{R_{b_j}^w}^T$$
对$R_{b_i}^w$进行求导，因为 $pose$ 表达在 $world$ 坐标系，根据旋转的叠加方式，添加的是右扰动：
$$\frac{\partial P_l^{c_i^{\prime }}}{\partial \phi }=\underset{\phi \to 0}{\lim }\frac{A(exp({\varphi }_i^{\wedge })exp({\phi }^{\wedge })x+t_{b_i}^w)-A(exp({\varphi }_i^{\wedge })x+t_{b_i}^w)}{\phi }$$$$=\underset{\phi \to 0}{\lim }\frac{Aexp({\varphi }_i^{\wedge })[exp({\phi }^{\wedge })-I]x}{\phi }$$$$=\underset{\phi \to 0}{\lim }\frac{Aexp({\varphi }_i^{\wedge })[I+{\phi }^{\wedge }-I]x}{\phi }$$$$=\underset{\phi \to 0}{\lim }\frac{Aexp({\varphi }_i^{\wedge }){\phi }^{\wedge }x}{\phi }=\underset{\phi \to 0}{\lim }\frac{-Aexp({\varphi }_i^{\wedge })x^{\wedge }\phi }{\phi }=\underset{\phi \to 0}{\lim }-Aexp({\varphi }_i^{\wedge })x^{\wedge }$$将$A$与$x$带入可得到：
$$\frac{\partial P_l^{c_i^{\prime }}}{\partial \phi }=-{R_c^b}^T{R_{b_j}^w}^T{R}_{b_i}^w(R_c^b{P}_l^{c_i}\frac{1}{{\lambda }_l}+t_c^b{)}^{\wedge }$$

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

扰动模型的求导可以替代关于状态量的雅可比，扰动模型$F$下误差状态的求导 ，将它在$dx=0$处做泰勒展开：
$$\begin{gathered} f(x_0+dx)=F(dx) \\ =F(0)+F^{\prime }(0)(dx-0) \\ =f(x_0)+\frac{\partial f(x_0⨁dx)}{\partial dx}dx \end{gathered}$$

---

在单位平面模型下：视觉残差函数及雅可比公式推导