VIO第3讲：基于优化的IMU与视觉信息融合之视觉残差函数构建

3 视觉重投影残差的 Jacobian

3.1 视觉重投影残差

• 特征点在归一化相机坐标系与在相机坐标系下的坐标关系为

$$\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\frac{1}{\lambda }\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]$$

  其中$\lambda =1/z$称之为逆深度！，记住这个形式，后续在求解雅可比的链式法则中，往往第一步就是利用这里的形式！

  归一化使得数值变小，数值稳定性更好。

• 视觉重投影残差—注意是归一化坐标之差（估计-观测），而不是像素系坐标之差

$${\mathbf{r}}_c=\left[\begin{array}{c}\frac{x_{c_j}}{z_{c_j}}-u_{c_j}\\ \frac{y_{c_j}}{z_{c_j}}-v_{c_j}\end{array}\right]$$

① 估计值（预测值）

  对于估计：同一个路标点P，第 i 帧（第一次看到）中的特征点，它投影到第 j 帧相机坐标系下的归一化值
$$\left[\begin{array}{c}{x}_{c_j}\\ y_{c_j}\\ z_{c_j}\\ 1\end{array}\right]={\mathbf{T}}_{bc}^{-1}{\mathbf{T}}_{w{b}_j}^{-1}{\mathbf{T}}_{w{b}_i}{\mathbf{T}}_{bc}\left[\begin{array}{c}\frac{1}{\lambda }{u}_{c_i}\\ \frac{1}{\lambda }{v}_{c_i}\\ \frac{1}{\lambda }\\ 1\end{array}\right]$$

<1> 推导

  对于这一过程详细的推导，下面${\pi }_c^{-1}$表示把一个像素坐标反投影到相机坐标系，即归一化坐标。反正知道这样表示归一化坐标即可，和上面是一致的。l表示第l个路标点，$R_c^b$表示外参，IMU坐标系到cam坐标系的转换。下面对应论文vins-mono。主要就是分别写旋转矩阵和平移量，

   P 在第 i 个相机归一化坐标系下坐标为：
$$\begin{array}{rl}& P_{u{v}_i}={\lambda }_l{\pi }_c\left(T_{b\leftarrow c}{}^{-1}{T}_{w\leftarrow b_i}{}^{-1}{P}_{w_l}\right)\\ & ⟺P_{w_l}=T_{w\leftarrow b_i}{T}_{b\leftarrow c}\frac{1}{{\lambda }_l}{\pi }_c{}^{-1}\left(P_{u{v}_i}\right)\\ & P_{w_l}=R_{b_l}^w(R_c^b\frac{1}{{\lambda }_l}{\pi }_c^{-1}(\left[\begin{array}{c}{u}_l^{c_i}\\ v_l^{c_l}\end{array}\right])+p_c^b)+p_{b_l}^w\end{array}$$
  P 在第 j 个相机的相机坐标系下坐标为
$$\begin{array}{c}{P}_l^{c_j}=T_{b\leftarrow c}{}^{-1}{T}_{w\leftarrow b_j}{}^{-1}{P}_{w_l}\\ ⟺P_{w_l}=T_{w\leftarrow b_j}{T}_{b\leftarrow c}{P}_l^{c_j}\end{array}$$
  代入到上面式子，就可以得到第l个路标点在第j个相机归一化坐标系下的可能坐标，本质上也对应第一个${\mathbf{T}}_{bc}^{-1}{\mathbf{T}}_{w{b}_j}^{-1}{\mathbf{T}}_{w{b}_i}{\mathbf{T}}_{bc}$变换，写开是为了后面更好求解优化变量的雅可比！
$$\begin{array}{c}{R}_{b_i}^w(R_c^b\frac{1}{{\lambda }_l}{\pi }_c^{-1}(\left[\begin{array}{c}{u}_l^{c_i}\\ v_l^{c_i}\end{array}\right])+p_c^b)+p_{b_i}^w=R_{b_j}^w\left(R_c^b{P}_l^{c_j}+p_c^b\right)+p_{b_j}^w\\ \Rightarrow P_l^{c_j}=R_b^c\{R_w^{b_j}[R_{b_i}^w(R_c^b\frac{1}{{\lambda }_l}{\pi }_c^{-1}(\left[\begin{array}{c}{u}_l^{c_l}\\ v_l^{c_i}\end{array}\right])+p_c^b)+p_{b_i}^w-p_{b_j}^w]-p_c^b\}\\ =R_b^c\left\{R_w^{b_j}\left[R_{b_i}^w\left(R_c^b\frac{1}{{\lambda }_l}{\bar{P}}_l^{c_i}+p_c^b\right)+p_{b_i}^w-p_{b_j}^w\right]-p_c^b\right\}\end{array}$$

<2> 引出因子图-优化变量

  涉及第i帧、第j帧、外参、以及逆深度
$$[p_{b_i}^w,q_{b_i}^w],\left[\begin{array}{c}{p}_{b_j}^w,q_{b_j}^w\end{array}\right],[p_c^b,q_c^b],{\lambda }_{l^{\circ }}$$
  为什么雅可比是下面这样的？因为虽然我们说是对状态量求导，实际上是对扰动求导！这个李代数扰动求导应该非常常见了。所以下面式子的角标就代表了扰动量！
$$\mathbf{J}=\left[\begin{array}{ccccc}\frac{\partial {\mathbf{r}}_c}{\partial \left[\begin{array}{c}\delta {\mathbf{p}}_{b_i{b}_i^{\prime }}\\ \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}\end{array}\right]}& \frac{\partial {\mathbf{r}}_c}{\partial \left[\begin{array}{c}\delta {\mathbf{p}}_{b_j{b}_j^{\prime }}\\ \delta {\mathbf{\theta }}_{b_j{b}_j^{\prime }}\end{array}\right]}& \frac{\partial {\mathbf{r}}_c}{\partial \left[\begin{array}{c}\delta {\mathbf{p}}_{c{c}^{\prime }}\\ \delta {\mathbf{\theta }}_{c{c}^{\prime }}\end{array}\right]}& \frac{\partial {\mathbf{r}}_c}{\partial \delta \lambda }& \end{array}\right]$$

<3> 简化形式

  把上面推导得到第l个路标点在第j个相机归一化坐标系下的可能坐标乘出来，就可以得到下面式子：
$$\begin{array}{rl}{\mathbf{f}}_{c_j}=\left[\begin{array}{c}{x}_{c_j}\\ y_{c_j}\\ z_{c_j}\end{array}\right]& ={\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}\frac{1}{\lambda }\left[\begin{array}{c}{u}_{c_i}\\ v_{c_i}\\ 1\end{array}\right]\\ & +{\mathbf{R}}_{bc}^{\top }({\mathbf{R}}_{w{b}_j}^{\top }(({\mathbf{R}}_{w{b}_i}{\mathbf{p}}_{bc}+{\mathbf{p}}_{w{b}_i})-{\mathbf{p}}_{w{b}_j})-{\mathbf{p}}_{bc})\end{array}$$
其中${\mathbf{f}}_{c_i}=\left[\begin{array}{c}{x}_{c_i}\\ y_{c_i}\\ z_{c_i}\end{array}\right]$，然后，分别定义如下变量：
$$\begin{array}{rl}{\mathbf{f}}_{b_i}& ={\mathbf{R}}_{bc}{\mathbf{f}}_{c_i}+{\mathbf{p}}_{bc}\\ {\mathbf{f}}_w& ={\mathbf{R}}_{w{b}_i}{\mathbf{f}}_{b_i}+{\mathbf{p}}_{w{b}_i}\\ {\mathbf{f}}_{b_j}& ={\mathbf{R}}_{w{b}_j}^{\top }({\mathbf{f}}_w-{\mathbf{p}}_{bc})\end{array}$$
​

② 观测值

  这个后面在补充下吧，纯视觉里面比如讲特征匹配结果作为了观测值；在MSCKF中将视觉的观测作为观测值。观测应该就是路标点P在第j帧的投影

3.2 重投影残差雅可比J

① 残差对归一化坐标点${\mathbf{f}}_{c_j}$导数

  这在视觉里面应该是很常见的基本操作了
$$\frac{\partial {\mathbf{r}}_c}{\partial {\mathbf{f}}_{c_j}}=\left[\begin{array}{ccc}\frac{1}{z_{c_j}}& 0& -\frac{x_{c_j}}{z_{c_j}^2}\\ 0& \frac{1}{z_{c_j}}& -\frac{y_{c_j}^2}{z_{c_j}^2}\end{array}\right]$$

② 归一化坐标点${\mathbf{f}}_{c_j}$对扰动量求导

<1> 对i时刻状态量求导

对 i 时刻位移-$p_{w{b}_i}$

$$\frac{\partial {\mathbf{f}}_{c_j}}{\partial \delta {\mathbf{p}}_{b_i{b}_i^{\prime }}}={\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }$$

对 i 时刻角度增量—旋转

$${\mathrm{f}}_{c_j}={\mathrm{R}}_{bc}^{\top }{\mathrm{R}}_{w{b}_j}^{\top }{\mathrm{R}}_{\mathrm{w}{\mathrm{b}}_{\mathrm{i}}}{\mathrm{R}}_{bc}{\mathrm{f}}_{c_i}+{\mathrm{R}}_{bc}^{\top }({\mathrm{R}}_{w{b}_j}^{\top }((R_{w{b}_i}{\mathrm{p}}_{bc}+{\mathrm{p}}_{w{b}_i})-{\mathrm{p}}_{w{b}_j})-{\mathrm{p}}_{bc})$$

  简化，丢弃不相关量
$$\begin{array}{rl}{\mathbf{f}}_{c_j}& ={\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}{\mathbf{f}}_{c_i}+{\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{p}}_{bc}+(...)\\ & ={\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}({\mathbf{R}}_{bc}{\mathbf{f}}_{c_i}+{\mathbf{p}}_{bc})+(...)\\ & ={\mathbf{R}}_{bc}^{|}{\mathbf{R}}_{w{b}_j}^{|}{\mathbf{R}}_{w{b}_i}{\mathbf{f}}_{b_i}+(...)\end{array}$$
  注意这里是全局坐标系，右扰动。这里分子简化了，这种求扰动是非常熟悉的泰勒展开约去，然后叉积交换等等
$$\begin{array}{rl}\frac{\partial {\mathbf{f}}_{c_j}}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}& \begin{array}{r}=\frac{\partial {\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}(\mathbf{I}+[\delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}{]}_{\times }){\mathbf{f}}_{b_i}}{\partial \delta {\mathbf{\theta }}_{b_i{b}_i^{\prime }}}\end{array}\\ & =-{\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}[{\mathbf{f}}_{b_i}{]}_{\times }\end{array}$$

<2> 对j时刻状态量求导

对 j 时刻位移

$$\frac{\partial {\mathbf{f}}_{c_j}}{\partial \delta {\mathbf{p}}_{b_j}{b}_j^{\prime }}=-{\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }$$

对 j 时刻角度增量—旋转

  简化，丢弃不相关量
$$\begin{array}{rl}{\mathbf{f}}_{c_j}& ={\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}{\mathbf{f}}_{c_i}+{\mathbf{R}}_{bc}^{\top }({\mathbf{R}}_{w{b}_j}^{\top }(({\mathbf{R}}_{w{b}_i}{\mathbf{p}}_{bc}+{\mathbf{p}}_{w{b}_i})-{\mathbf{p}}_{w{b}_j})-{\mathbf{p}}_{bc})\\ & ={\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }({\mathbf{R}}_{w{b}_i}({\mathbf{R}}_{bc}{\mathbf{f}}_{c_i}+{\mathbf{p}}_{bc})+{\mathbf{p}}_{w{b}_i}-{\mathbf{p}}_{w{b}_j})+(...)\\ & ={\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }({\mathbf{f}}_w-{\mathbf{p}}_{w{b}_j})+(...)\end{array}$$
  注意这里是局部坐标系，左扰动。还有，注意这里是负扰动。为什么是负的？因为扰动量是bb‘，但分子是局部坐标系，是bw，所以要加上个负号，表示反方向的旋转！
$$\begin{array}{rlr}\frac{\partial {\mathbf{f}}_{c_j}}{\partial \delta {\mathbf{\theta }}_{b_j{b}_j^{\prime }}}& \begin{array}{r}=\frac{\partial {\mathbf{R}}_{bc}^{\top }(\mathbf{I}-[\delta {\mathbf{\theta }}_{b_j{b}_j^{\prime }}{]}_{\times }){\mathbf{R}}_{w{b}_j}^{\top }({\mathbf{f}}_w-{\mathbf{p}}_{w{b}_j})}{\partial \delta {\mathbf{\theta }}_{b_j{b}_j^{\prime }}aligned}\end{array}& \\ & =\frac{\partial {\mathbf{R}}_{bc}^{\top }(\mathbf{I}-[\delta {\mathbf{\theta }}_{b_j{b}_j^{\prime }}{]}_{\times }){\mathbf{f}}_{b_j}}{\partial \delta {\mathbf{\theta }}_{b_j{b}_j^{\prime }}}& \\ & ={\mathbf{R}}_{bc}^{\top }[{\mathbf{f}}_{b_j}{]}_{\times }& \text{州一个州一位 Din Di}\end{array}$$

<3> 对 imu 和相机之间的外参求导

对位移$p_{bc}$求导，扰动量$c{c}^{{}^{\prime }}$

$$\frac{\partial {\mathbf{f}}_{c_j}}{\partial \delta {\mathbf{p}}_{c{c}^{\prime }}}={\mathbf{R}}_{bc}^{\top }({\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}-{\mathbf{I}}_{3\times 3})$$

对角度增量求导，由于 $f_{c_j}$ 都和$R_{bc}$有关，并且比较复杂，所以这
次分两部分求导

$$\begin{array}{rl}{\mathbf{f}}_{c_j}& ={\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}{\mathbf{f}}_{c_i}+{\mathbf{R}}_{bc}^{\top }({\mathbf{R}}_{w{b}_j}^{\top }(({\mathbf{R}}_{w{b}_i}{\mathbf{p}}_{bc}+{\mathbf{p}}_{w{b}_i})-{\mathbf{p}}_{w{b}_j})-{\mathbf{p}}_{bc})\\ & ={\mathbf{f}}_{c_j}^1+{\mathbf{f}}_{c_j}^2\end{array}$$

第一部分
$$\frac{\partial {\mathbf{f}}_{c_j}^1}{\partial \delta {\mathbf{\theta }}_{c{c}^{\prime }}}=\frac{\partial (\mathbf{I}-[\delta {\mathbf{\theta }}_{c{c}^{\prime }}{]}_{\times }){\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}(\mathbf{I}+[\delta {\mathbf{\theta }}_{c{c}^{\prime }}{]}_{\times }){\mathbf{f}}_{c_i}}{\partial \delta {\mathbf{\theta }}_{c{c}^{\prime }}}$$
分子可以写成
$$\begin{array}{rl}& \partial {\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}[\delta {\mathbf{\theta }}_{c{c}^{\prime }}{]}_{\times }{\mathbf{f}}_{c_i}-[\delta {\mathbf{\theta }}_{c{c}^{\prime }}{]}_{\times }{\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}{\mathbf{f}}_{c_i}\\ & +o^2(\delta {\mathbf{\theta }}_{c{c}^{\prime }})+(...)\end{array}$$

$$\frac{\partial {\mathbf{f}}_{c_j}^1}{\partial \delta {\mathbf{\theta }}_{c{c}^{\prime }}}=-{\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}[{\mathbf{f}}_{c_i}{]}_{\times }+[{\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}{\mathbf{f}}_{c_i}{]}_{\times }$$

第二部分
$$\begin{array}{c}\frac{\partial {\mathbf{f}}_{c_j}^2}{\partial \delta {\mathbf{\theta }}_{c{c}^{\prime }}}=\frac{(\mathbf{I}-[\delta {\mathbf{\theta }}_{c{c}^{\prime }}{]}_{\times }){\mathbf{R}}_{bc}^{†}({\mathbf{R}}_{w{b}_j}^{†}(({\mathbf{R}}_{w{b}_i}{\mathbf{p}}_{bc}+{\mathbf{p}}_{w{b}_i})-{\mathbf{p}}_{w{b}_j})-{\mathbf{p}}_{bc})}{\partial \delta {\mathbf{\theta }}_{c{c}^{\prime }}}\\ =[{\mathbf{R}}_{bc}^{\top }({\mathbf{R}}_{w{b}_j}^{\top }(({\mathbf{R}}_{w{b}_i}{\mathbf{p}}_{bc}+{\mathbf{p}}_{w{b}_i})-{\mathbf{p}}_{w{b}_j})-{\mathbf{p}}_{bc}){]}_{\times }\end{array}$$

两部分加起来就是视觉误差对外参的角度增量的最终结果

<4> 对特征逆深度的求导

$$\begin{array}{rl}\frac{\partial {\mathbf{f}}_{c_j}}{\partial \delta \lambda }& =\frac{\partial {\mathbf{f}}_{c_j}}{\partial {\mathbf{f}}_{c_i}}\frac{\partial {\mathbf{f}}_{c_i}}{\partial \delta \lambda }\\ & ={\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}(-\frac{1}{{\lambda }^2}\left[\begin{array}{c}{u}_{c_i}\\ v_{c_i}\\ 1\end{array}\right])\\ & =-\frac{1}{\lambda }{\mathbf{R}}_{bc}^{\top }{\mathbf{R}}_{w{b}_j}^{\top }{\mathbf{R}}_{w{b}_i}{\mathbf{R}}_{bc}{\mathbf{f}}_{c_i}\end{array}$$