VIO第3讲：基于优化的IMU与视觉信息融合之视觉残差雅可比推导

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

3 视觉重投影残差的 Jacobian

3.1 视觉重投影残差

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

[ x y z ] = 1 λ [ u v 1 ] \begin{bmatrix}x\\y\\z\end{bmatrix}=\dfrac{1}{\lambda}\begin{bmatrix}u\\v\\1\end{bmatrix} ​xyz​ ​=λ1​ ​uv1​ ​

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

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

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

r c = [ x c j z c j − u c j y c j z c j − v c j ] \mathbf{r}_c=\begin{bmatrix}\frac{x_{c_j}}{z_{c_j}}-u_{c_j}\\\frac{y_{c_j}}{z_{c_j}}-v_{c_j}\end{bmatrix} rc​= ​zcj​​xcj​​​−ucj​​zcj​​ycj​​​−vcj​​​ ​

① 估计值（预测值）

  对于估计：同一个路标点P，第 i 帧（第一次看到）中的特征点，它投影到第 j 帧相机坐标系下的归一化值
[ x c j y c j z c j 1 ] = T b c − 1 T w b j − 1 T w b i T b c [ 1 λ u c i 1 λ v c i 1 λ 1 ] \begin{bmatrix}x_{c_j}\\y_{c_j}\\z_{c_j}\\1\end{bmatrix}=\mathbf{T}_{bc}^{-1}\mathbf{T}_{wb_j}^{-1}\mathbf{T}_{wb_i}\mathbf{T}_{bc}\begin{bmatrix}\frac1\lambda u_{c_i}\\\frac1\lambda v_{c_i}\\\frac1\lambda\\1\end{bmatrix} ​xcj​​ycj​​zcj​​1​ ​=Tbc−1​Twbj​−1​Twbi​​Tbc​ ​λ1​uci​​λ1​vci​​λ1​1​ ​

<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 }}$$
  为什么雅可比是下面这样的？因为虽然我们说是对状态量求导，实际上是对扰动求导！这个李代数扰动求导应该非常常见了。所以下面式子的角标就代表了扰动量！
J = [ ∂ r c ∂ [ δ p b i b i ′ δ θ b i b i ′ ] ∂ r c ∂ [ δ p b j b j ′ δ θ b j b j ′ ] ∂ r c ∂ [ δ p c c ′ δ θ c c ′ ] ∂ r c ∂ δ λ ] \mathbf{J}=\begin{bmatrix}\dfrac{\partial\mathbf{r}_c}{\partial\begin{bmatrix}\delta\mathbf{p}_{b_ib_i^{\prime}}\\\delta\boldsymbol{\theta}_{b_ib_i^{\prime}}\end{bmatrix}}&\dfrac{\partial\mathbf{r}_c}{\partial\begin{bmatrix}\delta\mathbf{p}_{b_jb_j^{\prime}}\\\delta\boldsymbol{\theta}_{b_jb_j^{\prime}}\end{bmatrix}}&\dfrac{\partial\mathbf{r}_c}{\partial\begin{bmatrix}\delta\mathbf{p}_{cc^{\prime}}\\\delta\boldsymbol{\theta}_{cc^{\prime}}\end{bmatrix}}& \frac{\partial\mathbf{r}_c}{\partial\delta\lambda}&\end{bmatrix} J= ​∂[δpbi​bi′​​δθbi​bi′​​​]∂rc​​​∂[δpbj​bj′​​δθbj​bj′​​​]∂rc​​​∂[δpcc′​δθcc′​​]∂rc​​​∂δλ∂rc​​​​ ​

<3> 简化形式

  把上面推导得到第l个路标点在第j个相机归一化坐标系下的可能坐标乘出来，就可以得到下面式子：
f c j = [ x c j y c j z c j ] = R b c ⊤ R w b j ⊤ R w b i R b c 1 λ [ u c i v c i 1 ] + R b c ⊤ ( R w b j ⊤ ( ( R w b i p b c + p w b i ) − p w b j ) − p b c ) \begin{aligned} \mathbf{f}_{c_j}=\begin{bmatrix}x_{c_j}\\y_{c_j}\\z_{c_j}\end{bmatrix}& =\mathbf{R}_{bc}^\top\mathbf{R}_{wb_j}^\top\mathbf{R}_{wb_i}\mathbf{R}_{bc}\frac1\lambda\begin{bmatrix}u_{c_i}\\v_{c_i}\\1\end{bmatrix} \\ &+\mathbf{R}_{bc}^\top(\mathbf{R}_{wb_j}^\top((\mathbf{R}_{wb_i}\mathbf{p}_{bc}+\mathbf{p}_{wb_i})-\mathbf{p}_{wb_j})-\mathbf{p}_{bc}) \end{aligned} fcj​​= ​xcj​​ycj​​zcj​​​ ​​=Rbc⊤​Rwbj​⊤​Rwbi​​Rbc​λ1​ ​uci​​vci​​1​ ​+Rbc⊤​(Rwbj​⊤​((Rwbi​​pbc​+pwbi​​)−pwbj​​)−pbc​)​
其中 f c i = [ x c i y c i z c i ] \mathbf{f}_{c_i}=\begin{bmatrix}x_{c_i}\\y_{c_i}\\z_{c_i}\end{bmatrix} fci​​= ​xci​​yci​​zci​​​ ​，然后，分别定义如下变量：
$$\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}$导数

  这在视觉里面应该是很常见的基本操作了
∂ r c ∂ f c j = [ 1 z c j 0 − x c j z c j 2 0 1 z c j − y c j 2 z c j 2 ] \frac{\partial\mathbf{r}_c}{\partial\mathbf{f}_{c_j}}=\begin{bmatrix}\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{bmatrix} ∂fcj​​∂rc​​= ​zcj​​1​0​0zcj​​1​​−zcj​2​xcj​​​−zcj​2​ycj​2​​​ ​

② 归一化坐标点${\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> 对特征逆深度的求导

∂ f c j ∂ δ λ = ∂ f c j ∂ f c i ∂ f c i ∂ δ λ = R b c ⊤ R w b j ⊤ R w b i R b c ( − 1 λ 2 [ u c i v c i 1 ] ) = − 1 λ R b c ⊤ R w b j ⊤ R w b i R b c f c i \begin{aligned} \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}_{wb_j}^\top\mathbf{R}_{wb_i}\mathbf{R}_{bc}(-\frac{1}{\lambda^2}\begin{bmatrix}u_{c_i}\\v_{c_i}\\1\end{bmatrix}) \\ &=-\frac1\lambda\mathbf{R}_{bc}^\top\mathbf{R}_{wb_j}^\top\mathbf{R}_{wb_i}\mathbf{R}_{bc}\mathbf{f}_{c_i} \end{aligned} ∂δλ∂fcj​​​​=∂fci​​∂fcj​​​∂δλ∂fci​​​=Rbc⊤​Rwbj​⊤​Rwbi​​Rbc​(−λ21​ ​uci​​vci​​1​ ​)=−λ1​Rbc⊤​Rwbj​⊤​Rwbi​​Rbc​fci​​​