VINS-Mono理论学习——后端非线性优化

前言

本文主要介绍VINS状态估计器模块(estimator)在完成了系统初始化后,对视觉与IMU信息进行基于滑动窗口的紧耦合过程中所用到的非线性优化理论。这部分对应论文第六章(VI. TIGHTLY-COUPLED MONOCULAR VIO),并参考了崔博的《VINS论文推导与代码解析》、深蓝学院的VIO课程内容。主要想对目标函数中视觉残差和IMU残差,以及对应的雅可比、协方差进行推导。

一、状态向量与代价函数

状态向量包括滑动窗口内的所有相机状态(位置P、旋转Q、速度V、加速度偏置ba、陀螺仪偏置bw)、相机到IMU的外参、所有3D点的逆深度:
X = [ x 0 , x 1 , ⋯ x n , x c b , λ 0 , λ 1 , ⋯ λ m ] \mathcal{X}=\left[\mathbf{x}_{0}, \mathbf{x}_{1}, \cdots \mathbf{x}_{n}, \mathbf{x}_{c}^{b}, \lambda_{0}, \lambda_{1}, \cdots \lambda_{m}\right] X=[x0,x1,xn,xcb,λ0,λ1,λm]

x k = [ p b k w , v b k w , q b k w , b a , b g ] , k ∈ [ 0 , n ] \mathbf{x}_{k}=\left[\mathbf{p}_{b_{k}}^{w}, \mathbf{v}_{b_{k}}^{w}, \mathbf{q}_{b_{k}}^{w}, \mathbf{b}_{a}, \mathbf{b}_{g}\right], k \in[0, n] xk=[pbkw,vbkw,qbkw,ba,bg],k[0,n]

x c b = [ p c b , q c b ] \mathbf{x}_{c}^{b}=\left[\mathbf{p}_{c}^{b}, \mathbf{q}_{c}^{b}\right] xcb=[pcb,qcb]

目标函数为
min ⁡ X { ∥ r p − H p X ∥ 2 + ∑ k ∈ B ∥ r B ( z ^ b k + 1 b k , X ) ∥ P b k + 1 b k 2 + ∑ ( l , j ) ∈ C ρ ( ∥ r C ( z ^ l c j , X ) ∥ P l c j 2 ) } \min _{\mathcal{X}}\left\{\left\|\mathbf{r}_{p}-\mathbf{H}_{p} \mathcal{X}\right\|^{2}+\sum_{k \in \mathcal{B}}\left\|\mathbf{r}_{\mathcal{B}}\left(\hat{\mathbf{z}}_{b_{k+1}}^{b_{k}}, \mathcal{X}\right)\right\|_{\mathbf{P}_{b_{k+1}}^{b_{k}}}^{2}+\right. \sum_{(l, j) \in \mathcal{C}} \rho\left(\left\|\mathbf{r}_{\mathcal{C}}\left(\hat{\mathbf{z}}_{l}^{c_{j}}, \mathcal{X}\right)\right\|_{\mathbf{P}_{l}^{c_{j}}}^{2}\right) \} Xmin{rpHpX2+kBrB(z^bk+1bk,X)Pbk+1bk2+(l,j)Cρ(rC(z^lcj,X)Plcj2)}
这三项分别为边缘化的先验信息、IMU的测量残差、视觉的重投影误差。其中边缘化过程及其产生的先验将单独进行讨论。

VINS-Mono理论学习——后端非线性优化_第1张图片
另外有一点需要指出的是,因为实际优化的是: m i n ( d ) = m i n ( r T P − 1 r ) min(d)=min(r^TP^{-1}r) min(d)=min(rTP1r),这里r代表残差,P代表协方差。而Ceres只接受最小二乘优化,即 m i n ( e T e ) min(e^Te) min(eTe),因此在代码中,需要把信息矩阵 P − 1 P^{-1} P1做LLT分解,即 L L T = P − 1 LL^T=P^{-1} LLT=P1,代码中对应为sqrt_info,这样最后优化的 m i n ( d ) = m i n ( r T P − 1 r ) = m i n ( ( L T r ) T ( L T r ) ) = m i n ( r ′ T r ′ ) min(d)=min(r^TP^{-1}r)=min((L^Tr)^T(L^Tr))=min(r'^Tr') min(d)=min(rTP1r)=min((LTr)T(LTr))=min(rTr)

因此在下面所有的残差和Jacobian中,代码在最后都乘上了sqrt_info。

二、视觉约束

1、视觉测量残差

视觉测量残差就是重投影误差,定义为一个特征点在归一化相机坐标系下的估计值与观测值的差。估计值即特征点的三维空间坐标 ( x , y , z ) T (x,y,z)^T (x,y,z)T,观测值为其在相机归一化平面的坐标。
r c = [ x z − u y z − v ] \mathbf{r}_{c}=\left[\begin{array}{l}{\frac{x}{z}-u} \\ {\frac{y}{z}-v}\end{array}\right] rc=[zxuzyv]

逆深度参数化:采用逆深度 λ \lambda λ来表示特征点在归一化相机坐标系下的坐标:
[ x y z ] = 1 λ [ u v 1 ] \left[\begin{array}{l}{x} \\ {y} \\ {z}\end{array}\right]=\frac{1}{\lambda}\left[\begin{array}{l}{u} \\ {v} \\ {1}\end{array}\right] xyz=λ1uv1

以逆深度作为参数的原因,一是因为一些观测到的特征点深度值可能会非常大,难以进行优化;二是可以减少实际优化的参数变量;三是逆深度更加服从高斯分布。

对于第l个路标点P,其在第i帧中被第一次观测到,其归一化相机坐标系的坐标 [ u c i v c i 1 ] \left[\begin{array}{c}{ u_{c_{i}}} \\ { v_{c_{i}}} \\1 \end{array}\right] ucivci1
转换到第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 ] \left[\begin{array}{c}{x_{c_{j}}} \\ {y_{c_{j}}} \\ {z_{c_{j}}} \\ {1}\end{array}\right]=\mathbf{T}_{b c}^{-1} \mathbf{T}_{w b_{j}}^{-1} \mathbf{T}_{w b_{i}} \mathbf{T}_{b c}\left[\begin{array}{c}{\frac{1}{\lambda} u_{c_{i}}} \\ {\frac{1}{\lambda} v_{c_{i}}} \\ {\frac{1}{\lambda}} \\ {1}\end{array}\right] xcjycjzcj1=Tbc1Twbj1TwbiTbcλ1uciλ1vciλ11

这里因为我们优化的状态量是IMU坐标系(Body坐标系)的位姿,因此相较于视觉SLAM的重投影误差,还需加上两项从相机到IMU的外参。

而观测值即P在第j帧被观测到的归一化相机坐标系下的坐标为 [ u c j v c j 1 ] \left[\begin{array}{c}{ u_{c_{j}}} \\ { v_{c_{j}}} \\1 \end{array}\right] ucjvcj1

因而得到视觉残差为:

r c = [ x c j z c j − u c j y c j z c j − v c j ] \mathbf{r}_{c}=\left[\begin{array}{l}{\frac{x_{c_{j}}}{z_{c_{j}}}-u_{c_{j}}} \\ {\frac{y_{c_{j}}}{z_{c_{j}}}-v_{c_{j}}}\end{array}\right] rc=zcjxcjucjzcjycjvcj

将重投影方程拆成三维坐标形式:
P l c j = R b c { R w b j [ R b i w ( R c b 1 λ [ u c i v c i 1 ] + p c b ) + p b i w ] + p w b j } + p b c \mathcal{P}_{l}^{c_{j}}=R^c_b\{R^{b_j}_w[R^w_{b_i} (R^b_c \frac{1}{\lambda}\left[\begin{array}{c}{u_{c_{i}}} \\ {v_{c_{i}}} \\ {1}\end{array}\right] +p^b_c)+p^w_{b_i}]+p^{b_j}_w\}+p^c_b Plcj=Rbc{Rwbj[Rbiw(Rcbλ1ucivci1+pcb)+pbiw]+pwbj}+pbc

因为对于变换矩阵有: R b c = ( R c b ) T , p b c = − ( R c b ) T p c b R^c_b=(R^b_c)^T,p^c_b=-(R^b_c)^Tp^b_c Rbc=(Rcb)Tpbc=(Rcb)Tpcb,所以

P l c j = R b c { R w b j [ R b i w ( R c b 1 λ [ u c i v c i 1 ] + p c b ) + p b i w ] − R w b j p b j w } − R b c p c b \mathcal{P}_{l}^{c_{j}}=R^c_b\{R^{b_j}_w[R^w_{b_i} (R^b_c \frac{1}{\lambda}\left[\begin{array}{c}{u_{c_{i}}} \\ {v_{c_{i}}} \\ {1}\end{array}\right] +p^b_c)+p^w_{b_i}]-R^{b_j}_wp_{b_j}^w\}-R_b^cp_c^b Plcj=Rbc{Rwbj[Rbiw(Rcbλ1ucivci1+pcb)+pbiw]Rwbjpbjw}Rbcpcb

= R b c R w b j R b i w R c b 1 λ [ u c i v c i 1 ] + R b c ( R w b j ( R b i w p c b + p b i w − p b j w ) − p c b ) =R^c_bR^{b_j}_wR^w_{b_i}R^b_c \frac{1}{\lambda}\left[\begin{array}{c}{u_{c_{i}}} \\ {v_{c_{i}}} \\ {1}\end{array}\right]+R^c_b(R^{b_j}_w(R^w_{b_i} p^b_c+p^w_{b_i}-p_{b_j}^w)-p_c^b) =RbcRwbjRbiwRcbλ1ucivci1+Rbc(Rwbj(Rbiwpcb+pbiwpbjw)pcb)


以上是对针孔相机模型的视觉残差,而在VINS论文中其实显示的是对一般相机模型的视觉残差,即使用了广角相机的球面模型。

最后定义的视觉残差为:
r C ( z ^ l c j , X ) = [ b 1 b 2 ] ( P ‾ ^ l c j − P l c j ∥ P l c j ∥ ) \mathbf{r}_{\mathcal{C}}\left(\hat{\mathbf{z}}_{l}^{c_{j}}, \mathcal{X}\right)=\left[\begin{array}{ll}{\mathbf{b}_{1}} \\ {\mathbf{b}_{2}}\end{array}\right]\left(\hat{\overline{\mathcal{P}}}_{l}^{c_{j}}-\frac{\mathcal{P}_{l}^{c_{j}}}{\left\|\mathcal{P}_{l}^{c j}\right\|}\right) rC(z^lcj,X)=[b1b2]P^lcjPlcjPlcj

P ‾ ^ l c j = π c − 1 ( [ u ^ l c j v ^ l c j ] ) \hat{\overline{\mathcal{P}}}_{l}^{c_{j}}=\pi_{c}^{-1}\left(\left[\begin{array}{c}{\hat{u}_{l}^{c_{j}}} \\ {\hat{v}_{l}^{c_{j}}}\end{array}\right]\right) P^lcj=πc1([u^lcjv^lcj])

这里将重投影和测量值都归一化为单位向量,将视觉残差为两个单位向量的差。因为视觉残差的自由度为2,再将视觉残差投影到正切平面上,b1b2为正切平面的两个正交基。

VINS-Mono理论学习——后端非线性优化_第2张图片
这两种相机模型在VINS-Mono的论文中都有用到:

#ifdef UNIT_SPHERE_ERROR 
    residual =  tangent_base * (pts_camera_j.normalized() - pts_j.normalized());
#else
    double dep_j = pts_camera_j.z();
    residual = (pts_camera_j / dep_j).head<2>() - pts_j.head<2>();
#endif

2、优化变量

包括两个时刻的状态量、外参,以及逆深度。
[ p b i w , q b i w ] 、 [ p b j w , q b j w ] 、 [ p c b , q c b ] 、 λ l [p^w_{b_i},q^w_{b_i}]、[p^w_{b_j},q^w_{b_j}]、[p^b_c,q^b_c]、 \lambda _l [pbiw,qbiw][pbjw,qbjw][pcb,qcb]λl

3、Jacobian

这里我们采用针孔相机的(归一化相机平面)视觉残差来进行推导,而相机球面模型的视觉残差推导过程也相似。

r c = [ x c j z c j − u c j y c j z c j − v c j ] \mathbf{r}_{c}=\left[\begin{array}{l}{\frac{x_{c_{j}}}{z_{c_{j}}}-u_{c_{j}}} \\ {\frac{y_{c_{j}}}{z_{c_{j}}}-v_{c_{j}}}\end{array}\right] rc=zcjxcjucjzcjycjvcj

这里先定义:

f c i = 1 λ l P ‾ l c i = 1 λ [ u c i v c i 1 ] f_{c_i}=\frac{1}{\lambda_{l}} \overline{P}_{l}^{c_{i}}=\frac{1}{\lambda}\left[\begin{array}{c}{u_{c_{i}}} \\ {v_{c_{i}}} \\ {1}\end{array}\right] fci=λl1Plci=λ1ucivci1

f c j f_{c_j} fcj即为原来的 P l c j \mathcal{P}_{l}^{c_{j}} Plcj

f c j = P l c j = R b c R w b j R b i w R c b f c i + R b c ( R w b j ( R b i w p c b + p b i w − p b j w ) − p c b ) f_{c_j}=\mathcal{P}_{l}^{c_{j}}=R^c_bR^{b_j}_wR^w_{b_i}R^b_c f_{c_i}+R^c_b(R^{b_j}_w(R^w_{b_i} p^b_c+p^w_{b_i}-p_{b_j}^w)-p_c^b) fcj=Plcj=RbcRwbjRbiwRcbfci+Rbc(Rwbj(Rbiwpcb+pbiwpbjw)pcb)

因为根据链式法则,对Jacobian的计算可以分成:
1)视觉残差对重投影3D点 f c j f_{c_j} fcj求导。

∂ 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 z c j 2 ] \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}}}{z_{c_{j}}^{2}}}\end{array}\right] fcjrc=zcj100zcj1zcj2xcjzcj2ycj

2) f c j f_{c_j} fcj对各个优化变量求导。(这里采用了崔博的推导结果)

J [ 0 ] 3 × 7 = [ ∂ f c j ∂ p b i w , ∂ f c j ∂ q b i w ] = [ R b c R w b j , − R b c R w b j R b i w ( R c b 1 λ l P ‾ l c i + p c b ) ∧ ] J[0]^{3 \times 7}=\left[\frac{\partial f_{c_j}}{\partial p_{b_{i}}^{w}}, \frac{\partial f_{c_j}}{\partial q_{b_{i}}^{w}}\right]=\left[R_{b}^{c} R_{w}^{b_{j}} ,-R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w}\left(R_{c}^{b} \frac{1}{\lambda_{l}} \overline{P}_{l}^{c_{i}}+p_{c}^{b}\right)^{\wedge}\right] J[0]3×7=[pbiwfcj,qbiwfcj]=[RbcRwbj,RbcRwbjRbiw(Rcbλl1Plci+pcb)]

J [ 1 ] 3 × 7 = [ ∂ f c j ∂ p b j w , ∂ f c j ∂ q b j w ] = [ − R b c R w b j , R b c { R w b j [ R b i w ( R c b P ‾ l c i λ l + p c b ) + p b i w − p b j w ] } ∧ ] J[1]^{3 \times 7}=\left[\frac{\partial f_{c_j}}{\partial p_{b_{j}}^{w}}, \frac{\partial f_{c_j}}{\partial q_{b_{j}}^{w}}\right]=\left[-R_{b}^{c} R_{w}^{b_{j}}, R_{b}^{c}\left\{R_{w}^{b_{j}}\left[R_{b_{i}}^{w}\left(R_{c}^{b} \frac{\overline{P}_{l}^{c_{i}}}{\lambda_{l}}+p_{c}^{b}\right)+p_{b_{i}}^{w}-p_{b_{j}}^{w}\right]\right\}^{\wedge}\right] J[1]3×7=[pbjwfcj,qbjwfcj]=[RbcRwbj,Rbc{Rwbj[Rbiw(RcbλlPlci+pcb)+pbiwpbjw]}]

J [ 2 ] 3 × 7 = [ ∂ f c j ∂ p c b , ∂ f c j ∂ q c b ] = [ R b c ( R w b j R b i w − I 3 × 3 ) − R b c R w b j R b i w R c b ( P ‾ l c i λ l ) ∧ + ( R b c R w b j R b i w R c b P ‾ l c i λ l ) ∧ + { R b c [ R w b j ( R b i w p c b + ∣ p b i w − p b j w ) − p c b ] } ∧ ] T J[2]^{3 \times 7}=\left[\frac{\partial f_{c_j}}{\partial p_{c}^{b}}, \frac{\partial f_{c_j}}{\partial q_{c}^{b}}\right] =\left[\begin{array}{c}{R_{b}^{c}\left(R_{w}^{b_{j}} R_{b_{i}}^{w}-I_{3 \times 3}\right)} \\ {-R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w} R_{c}^{b}\left(\frac{\overline{P}_{l}^{c_{i}}}{\lambda_{l}}\right)^{\wedge}+\left(R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w} R_{c}^{b} \frac{\overline{P}_{l}^{c_{i}}}{\lambda_{l}}\right)^{\wedge}+\left\{R_{b}^{c}\left[R_{w}^{b_{j}}\left(R_{b_{i}}^{w} p_{c}^{b}+| p_{b_{i}}^{w}-p_{b_{j}}^{w}\right)-p_{c}^{b}\right]\right\}^{\wedge}}\end{array}\right]^{T} J[2]3×7=[pcbfcj,qcbfcj]=Rbc(RwbjRbiwI3×3)RbcRwbjRbiwRcb(λlPlci)+(RbcRwbjRbiwRcbλlPlci)+{Rbc[Rwbj(Rbiwpcb+pbiwpbjw)pcb]}T

J [ 3 ] 3 × 1 = ∂ f c j ∂ λ l = − R b c R w b j R b i w R c b P ‾ l c i λ l 2 J[3]^{3 \times 1}=\frac{\partial f_{c_j}}{\partial \lambda_{l}}=-R_{b}^{c} R_{w}^{b_{j}} R_{b_{i}}^{w} R_{c}^{b} \frac{\overline{P}_{l}^{c_{i}}}{\lambda_{l}^{2}} J[3]3×1=λlfcj=RbcRwbjRbiwRcbλl2Plci

这一部分的具体代码实现在projection_factor.cpp 的 bool ProjectionFactor::Evaluate() 函数中,由于数学公式很容易找到对应,这里就不对代码做解释了。


下面是对这些Jacobian的证明:
推导结果把崔博和VIO课程的结果都放上去了,两者其实是一样的
再把 f c j f_{c_j} fcj写一遍以看的更清楚

f c j = R b c { R w b j [ R b i w ( R c b f c i + p c b ) + p b i w − p b j w ] − p c b } f_{c_j}=R^c_b\{R^{b_j}_w[R^w_{b_i} (R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]-p_c^b\} fcj=Rbc{Rwbj[Rbiw(Rcbfci+pcb)+pbiwpbjw]pcb}

  • 对i时刻位移 p b i w p^w_{b_i} pbiw

∂ f c j ∂ p b i w = R b c R w b j = R b c ⊤ R w b j ⊤ \frac{\partial f_{c_j}}{\partial p_{b_{i}}^{w}}=R^c_b R^{b_j}_w=\mathbf{R}_{b c}^{\top} \mathbf{R}_{w b_{j}}^{\top} pbiwfcj=RbcRwbj=RbcRwbj

  • 对i时刻旋转 q b i w q^w_{b_i} qbiw

∂ f c j ∂ q b i w = ∂ f c j ∂ δ θ b i = ∂ R b c { R w b j [ R b i w e x p ( [ δ θ b i ] × ) ( R c b f c i + p c b ) + p b i w − p b j w ] − p c b } ∂ δ θ b i = ∂ R b c R w b j R b i w e x p ( [ δ θ b i ] × ) ( R c b f c i + p c b ) ∂ δ θ b i = ∂ R b c R w b j R b i w ( [ δ θ b i ] × ) ( R c b f c i + p c b ) ∂ δ θ b i = − R b c R w b j R b i w [ R c b f c i + p c b ] × = − R b c ⊤ R w b j ⊤ R w b i [ f b i ] × \frac{\partial f_{c_j}}{\partial q_{b_{i}}^{w}}=\frac{\partial f_{c_j}}{\partial \delta \theta _{b_{i}}} = \frac{\partial R^c_b\{R^{b_j}_w[R^w_{b_i} exp([ \delta \theta _{b_{i}}]_{\times})(R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]-p_c^b\}}{\partial \delta \theta _{b_{i}}} \\ =\frac{\partial R^c_bR^{b_j}_wR^w_{b_i} exp([ \delta \theta _{b_{i}}]_{\times})(R^b_c f_{c_i} +p^b_c)}{\partial \delta \theta _{b_{i}}} \\=\frac{\partial R^c_bR^{b_j}_wR^w_{b_i} ([ \delta \theta _{b_{i}}]_{\times})(R^b_c f_{c_i} +p^b_c)}{\partial \delta \theta _{b_{i}}}\\ =-R^c_bR^{b_j}_wR^w_{b_i} [R^b_c f_{c_i} +p^b_c]_{\times}=-\mathbf{R}_{b c}^{\top} \mathbf{R}_{w b_{j}}^{\top} \mathbf{R}_{w b_{i}}\left[\mathbf{f}_{b_{i}}\right]_{ \times} qbiwfcj=δθbifcj=δθbiRbc{Rwbj[Rbiwexp([δθbi]×)(Rcbfci+pcb)+pbiwpbjw]pcb}=δθbiRbcRwbjRbiwexp([δθbi]×)(Rcbfci+pcb)=δθbiRbcRwbjRbiw([δθbi]×)(Rcbfci+pcb)=RbcRwbjRbiw[Rcbfci+pcb]×=RbcRwbjRwbi[fbi]×

  • 对j时刻位移 p b j w p^w_{b_j} pbjw

∂ f c j ∂ p b j w = − R b c R w b j = − R b c ⊤ R w b j ⊤ \frac{\partial f_{c_j}}{\partial p_{b_{j}}^{w}}=-R^c_b R^{b_j}_w=-\mathbf{R}_{b c}^{\top} \mathbf{R}_{w b_{j}}^{\top} pbjwfcj=RbcRwbj=RbcRwbj

  • 对j时刻旋转 q b j w q^w_{b_j} qbjw

∂ f c j ∂ q b j w = ∂ f c j ∂ δ θ b j = ∂ R b c { ( R b j w e x p ( [ δ θ b j ] × ) ) − 1 [ R b i w ( R c b f c i + p c b ) + p b i w − p b j w ] − p c b } ∂ δ θ b i = ∂ R b c e x p ( − [ δ θ b j ] × ) R w b j [ R b i w ( R c b f c i + p c b ) + p b i w − p b j w ] ∂ δ θ b i = ∂ R b c ( − [ δ θ b i ] × ) R w b j [ R b i w ( R c b f c i + p c b ) + p b i w − p b j w ] ∂ δ θ b i = R b c { R w b j [ R b i w ( R c b f c i + p c b ) + p b i w − p b j w ] } × = R b c ⊤ [ f b j ] x \frac{\partial f_{c_j}}{\partial q_{b_{j}}^{w}}=\frac{\partial f_{c_j}}{\partial \delta \theta _{b_{j}}} = \frac{\partial R^c_b\{ (R_{b_j}^w exp([ \delta \theta _{b_{j}}]_{\times}))^{-1}[R^w_{b_i} (R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]-p_c^b\}}{\partial \delta \theta _{b_{i}}} \\ =\frac{\partial R^c_b exp(-[ \delta \theta _{b_{j}}]_{\times}) R^{b_j}_w[R^w_{b_i}(R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]}{\partial \delta \theta _{b_{i}}} \\=\frac{\partial R^c_b (-[ \delta \theta _{b_{i}}]_{\times}) R^{b_j}_w[R^w_{b_i}(R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]}{\partial \delta \theta _{b_{i}}}\\ =R^c_b \{ R^{b_j}_w[R^w_{b_i}(R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]\}_{\times}=\mathbf{R}_{b c}^{\top}\left[\mathbf{f}_{b_{j}}\right]_{x} qbjwfcj=δθbjfcj=δθbiRbc{(Rbjwexp([δθbj]×))1[Rbiw(Rcbfci+pcb)+pbiwpbjw]pcb}=δθbiRbcexp([δθbj]×)Rwbj[Rbiw(Rcbfci+pcb)+pbiwpbjw]=δθbiRbc([δθbi]×)Rwbj[Rbiw(Rcbfci+pcb)+pbiwpbjw]=Rbc{Rwbj[Rbiw(Rcbfci+pcb)+pbiwpbjw]}×=Rbc[fbj]x

  • 对外参的平移 p c b p^b_{c} pcb

∂ f c j ∂ p c b = ∂ R b c { R w b j [ R b i w ( R c b f c i + p c b ) + p b i w − p b j w ] − p c b } ∂ p c b = R b c ( R w b j R b i w − I ) = R b c ⊤ ( R w b j ⊤ R w b i − I 3 × 3 ) \frac{\partial f_{c_j}}{\partial p^b_{c}}=\frac{\partial R^c_b\{R^{b_j}_w[R^w_{b_i} (R^b_c f_{c_i} +p^b_c)+p^w_{b_i}-p_{b_j}^w]-p_c^b\} }{\partial p^b_{c}}\\ = R^c_b(R^{b_j}_w R^w_{b_i}-I)=\mathbf{R}_{b c}^{\top}\left(\mathbf{R}_{w b_{j}}^{\top} \mathbf{R}_{w b_{i}}-\mathbf{I}_{3 \times 3}\right) pcbfcj=pcbRbc{Rwbj[Rbiw(Rcbfci+pcb)+pbiwpbjw]pcb}=Rbc(RwbjRbiwI)=Rbc(RwbjRwbiI3×3)

  • 对外参的旋转 q c b q^b_{c} qcb
    f c j = P l c j = R b c R w b j R b i w R c b f c i + R b c ( R w b j ( R b i w p c b + p b i w − p b j w ) − p c b ) = f c j 1 + f c j 2 f_{c_j}=\mathcal{P}_{l}^{c_{j}}=R^c_bR^{b_j}_wR^w_{b_i}R^b_c f_{c_i}+R^c_b(R^{b_j}_w(R^w_{b_i} p^b_c+p^w_{b_i}-p_{b_j}^w)-p_c^b)=f_{c_j}^1+f_{c_j}^2 fcj=Plcj=RbcRwbjRbiwRcbfci+Rbc(Rwbj(Rbiwpcb+pbiwpbjw)pcb)=fcj1+fcj2

分成两项分别求导。

∂ f c j 1 ∂ q c b = ∂ ( R c b e x p ( [ δ θ c b ] × ) ) − 1 R w b j R b i w R c b e x p ( [ δ θ c b ] × ) f c i ∂ δ θ c b = ∂ ( 1 − [ δ θ c b ] × ) R b c R w b j R b i w R c b ( 1 + [ δ θ c b ] × ) f c i ∂ δ θ c b = ∂ ( − [ δ θ c b ] × ) R b c R w b j R b i w R c b f c i + R b c R w b j R b i w R c b ( [ δ θ c b ] × ) f c i + o 2 ( δ θ c b ) ∂ δ θ c b = [ R b c R w b j R b i w R c b f c i ] × − R b c R w b j R b i w R c b [ f c i ] × = − R b c ⊤ R w b j ⊤ R w b k R b c [ f c i ] x + [ R b c ⊤ R w b j ⊤ R w b i R b c f c i ] x \frac{\partial f_{c_j}^1}{\partial q^b_{c}}=\frac{\partial (R^b_c exp([ \delta \theta^b_{c}]_{\times}))^{-1} R^{b_j}_w R^w_{b_i} R^b_c exp([ \delta \theta^b_{c}]_{\times}) f_{c_i} }{\partial \delta \theta^b_{c}} \\ =\frac{\partial (1-[ \delta \theta^b_{c}]_{\times}) R^c_b R^{b_j}_w R^w_{b_i} R^b_c (1+[ \delta \theta^b_{c}]_{\times}) f_{c_i} }{\partial \delta \theta^b_{c}} \\ = \frac{\partial (-[ \delta \theta^b_{c}]_{\times}) R^c_b R^{b_j}_w R^w_{b_i} R^b_c f_{c_i}+R^c_b R^{b_j}_w R^w_{b_i} R^b_c([ \delta \theta^b_{c}]_{\times}) f_{c_i} + o^2(\delta \theta^b_{c}) }{\partial \delta \theta^b_{c}} \\ =[R^c_b R^{b_j}_w R^w_{b_i} R^b_c f_{c_i}]_\times - R^c_b R^{b_j}_w R^w_{b_i} R^b_c [f_{c_i}]\times\\ =-\mathbf{R}_{b c}^{\top} \mathbf{R}_{w b_{j}}^{\top} \mathbf{R}_{w b_{k}} \mathbf{R}_{b c}\left[\mathbf{f}_{c_{i}}\right]_{x}+\left[\mathbf{R}_{b c}^{\top} \mathbf{R}_{w b_{j}}^{\top} \mathbf{R}_{w b_{i}} \mathbf{R}_{b c} \mathbf{f}_{c_{i}}\right]_{x} qcbfcj1=δθcb(Rcbexp([δθcb]×))1RwbjRbiwRcbexp([δθcb]×)fci=δθcb(1[δθcb]×)RbcRwbjRbiwRcb(1+[δθcb]×)fci=δθcb([δθcb]×)RbcRwbjRbiwRcbfci+RbcRwbjRbiwRcb([δθcb]×)fci+o2(δθcb)=[RbcRwbjRbiwRcbfci]×RbcRwbjRbiwRcb[fci]×=RbcRwbjRwbkRbc[fci]x+[RbcRwbjRwbiRbcfci]x
∂ f c j 2 ∂ q c b = ∂ ( R c b e x p ( [ δ θ c b ] × ) ) − 1 ( R w b j ( R b i w p c b + p b i w − p b j w ) − p c b ) ∂ δ θ c b = ∂ ( − [ δ θ c b ] × ) R b c ( R w b j ( R b i w p c b + p b i w − p b j w ) − p c b ) ∂ δ θ c b = [ R b c ( R w b j ( R b i w p c b + p b i w − p b j w ) − p c b ) ] × = [ 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 ) ] × \frac{\partial f_{c_j}^2}{\partial q^b_{c}}=\frac{\partial (R^b_c exp([ \delta \theta^b_{c}]_{\times}))^{-1} (R^{b_j}_w(R^w_{b_i} p^b_c+p^w_{b_i}-p_{b_j}^w)-p_c^b)}{\partial \delta \theta^b_{c}}\\ =\frac{\partial ( -[ \delta \theta^b_{c}]_{\times}) R^c_b (R^{b_j}_w(R^w_{b_i} p^b_c+p^w_{b_i}-p_{b_j}^w)-p_c^b)}{\partial \delta \theta^b_{c}} \\ =[R^c_b (R^{b_j}_w(R^w_{b_i} p^b_c+p^w_{b_i}-p_{b_j}^w)-p_c^b)]_\times \\ =\left[\mathbf{R}_{b c}^{\top}\left(\mathbf{R}_{w b_{j}}^{\top}\left(\left(\mathbf{R}_{w b_{i}} \mathbf{p}_{b c}+\mathbf{p}_{w b_{i}}\right)-\mathbf{p}_{w b_{j}}\right)-\mathbf{p}_{b c}\right)\right]_{ \times} qcbfcj2=δθcb(Rcbexp([δθcb]×))1(Rwbj(Rbiwpcb+pbiwpbjw)pcb)=δθcb([δθcb]×)Rbc(Rwbj(Rbiwpcb+pbiwpbjw)pcb)=[Rbc(Rwbj(Rbiwpcb+pbiwpbjw)pcb)]×=[Rbc(Rwbj((Rwbipbc+pwbi)pwbj)pbc)]×

  • 对逆深度 λ \lambda λ
    ∂ f c j ∂ λ = ∂ f c j ∂ f c i ∂ f c i ∂ λ = − R b c R w b j R b i w R c b 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 \frac{\partial f_{c_j}}{\partial \lambda}=\frac{\partial f_{c_j}}{\partial f_{c_i}}\frac{\partial f_{c_i}}{\partial \lambda}=-R^c_b R^{b_j}_w R^w_{b_i} R^b_c \frac{1}{\lambda^2}\left[\begin{array}{c}{u_{c_{i}}} \\ {v_{c_{i}}} \\ {1}\end{array}\right] =-\frac{1}{\lambda} \mathbf{R}_{b c}^{\top} \mathbf{R}_{w b_{j}}^{\top} \mathbf{R}_{w b_{i}} \mathbf{R}_{b c} \mathbf{f}_{c_{i}} λfcj=fcifcjλfci=RbcRwbjRbiwRcbλ21ucivci1=λ1RbcRwbjRwbiRbcfci

4、协方差

视觉约束的协方差与标定相机内参时的重投影误差有关。VINS在代码中sqrt_info取1.5个像素,对应到归一化相机平面还需除以焦距f,最后得到的信息矩阵:(这里真正的信息矩阵其实是sqrt_info的平方)
s q r t ( Ω v i s ) = s q r t ( Σ v i s − 1 ) = ( 1.5 f I 2 × 2 ) − 1 = f 1.5 I 2 × 2 sqrt(\Omega_{v i s})=sqrt(\Sigma_{v i s}^{-1})=\left(\frac{1.5}{f} I_{2 \times 2}\right)^{-1}=\frac{f}{1.5} I_{2 \times 2} sqrt(Ωvis)=sqrt(Σvis1)=(f1.5I2×2)1=1.5fI2×2

void Estimator::setParameter()
{
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        tic[i] = TIC[i];
        ric[i] = RIC[i];
    }
    f_manager.setRic(ric);
    ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    td = TD;
}

三、IMU约束

1、IMU 测量残差

在IMU预积分中我们已经推导了:

R w b k p b k + 1 w = R w b k ( p b k w + v b k w Δ t k − 1 2 g w Δ t k 2 ) + α b k + 1 b k R^{b_k}_w p^w_{b_{k+1}}=R^{b_k}_w (p^w_{b_k}+v^w_{b_k} \Delta t_k-\frac{1}{2} g^w\Delta t_k^2)+ \alpha^{b_k}_{b_{k+1}} Rwbkpbk+1w=Rwbk(pbkw+vbkwΔtk21gwΔtk2)+αbk+1bk

R w b k v b k + 1 w = R w b k ( v b k w − g w Δ t k ) + β b k + 1 b k R^{b_k}_w v^w_{b_{k+1}}=R^{b_k}_w (v^w_{b_k}- g^w\Delta t_k)+\beta^{b_k}_{b_{k+1}} Rwbkvbk+1w=Rwbk(vbkwgwΔtk)+βbk+1bk

q w b k ⊗ q b k + 1 w = γ b k + 1 b k q^{b_k}_w\otimes q^w_{b_{k+1}}=\gamma^{b_k}_{b_{k+1}} qwbkqbk+1w=γbk+1bk

因此其IMU的残差可以写成:

r B ( z b k + 1 b k , X ) = [ r p r q r v r b a r b g ] = [ q b i w ( p w b j − p w b i − v i w Δ t + 1 2 g w Δ t 2 ) − α b i b j 2 [ q b j b i ⊗ ( q b i w ⊗ q w b j ) ] x y z q b i w ( v j w − v i w + g w Δ t ) − β b i b j b j a − b i a b j g − b i g ] \mathbf{r}_{B}(\mathbf{z}_{b_{k+1}}^{b_{k}}, \mathcal{X})= \left[\begin{array}{c}{\mathbf{r}_{p}} \\ {\mathbf{r}_{q}} \\ {\mathbf{r}_{v}} \\ {\mathbf{r}_{b a}} \\ {\mathbf{r}_{b g}}\end{array}\right] = \left[\begin{array}{c}{\mathbf{q}_{b_{i} w}\left(\mathbf{p}_{w b_{j}}-\mathbf{p}_{w b_{i}}-\mathbf{v}_{i}^{w} \Delta t+\frac{1}{2} \mathbf{g}^{w} \Delta t^{2}\right)-\alpha_{b_{i} b_{j}}} \\ {2\left[\mathbf{q}_{b_{j} b_{i}} \otimes\left(\mathbf{q}_{b_{i}w} \otimes \mathbf{q}_{w b_{j}}\right)\right]_{x y z}} \\ {\mathbf{q}_{b_{i} w}\left(\mathbf{v}_{j}^{w}-\mathbf{v}_{i}^{w}+\mathbf{g}^{w} \Delta t\right)-\beta_{b_{i} b_{j}}} \\ {\mathbf{b}_{j}^{a}-\mathbf{b}_{i}^{a}} \\ {\mathbf{b}_{j}^{g}-\mathbf{b}_{i}^{g}}\end{array}\right] rB(zbk+1bk,X)=rprqrvrbarbg=qbiw(pwbjpwbiviwΔt+21gwΔt2)αbibj2[qbjbi(qbiwqwbj)]xyzqbiw(vjwviw+gwΔt)βbibjbjabiabjgbig

2、优化变量

优化变量主要包括第i、j时刻的p、q、v、ba、bg:

[ p b k w , q b k w ] , [ v b k w , b a k , b ω k ] , [ p b k + 1 w , q b k + 1 w ] , [ v b k + 1 w , b a k + 1 , b ω k + 1 ] \left[p_{b_{k}}^{w}, q_{b_{k}}^{w}\right], \left[v_{b_{k}}^{w}, b_{a_{k}}, b_{\omega_{k}}\right], \left[p_{b_{k+1}}^{w}, q_{b_{k+1}}^{w}\right],\left[v_{b_{k+1}}^{w}, b_{a_{k+1}}, b_{\omega_{k+1}}\right] [pbkw,qbkw],[vbkw,bak,bωk],[pbk+1w,qbk+1w],[vbk+1w,bak+1,bωk+1]

3、Jacobian

这部分的结果建议直接看VINS的源码,在imu_factor.h中的class IMUFactor : public ceres::SizedCostFunction<15, 7, 9, 7, 9> 的函数virtual bool Evaluate()中实现,其中parameters[0~3]分别对应了以上4组优化变量的参数块。


以下是部分证明:

对于pqv的求导可以直接采用对误差增量进行计算,而对ba、bg求导,因为i时刻的bias相关的预积分计算是通过迭代一步步递推的,直接求导太复杂,这里直接对预积分量在i时刻的bias附近用一阶泰勒展开来近似,而不是真的取迭代计算。

α b i b j = α b i b j + J b i a α δ b i a + J b i g α δ b i g β b i b j = β b i b j + J b i a β δ b i a + J b i g β δ b i g q b i b j = q b i b j ⊗ [ 1 1 2 J b i g q δ b i g ] \begin{aligned} \boldsymbol{\alpha}_{b_{i} b_{j}} &=\boldsymbol{\alpha}_{b_{i} b_{j}}+\mathbf{J}_{b_{i}^{a}}^{\alpha} \delta \mathbf{b}_{i}^{a}+\mathbf{J}_{b_{i}^{g}}^{\alpha} \delta \mathbf{b}_{i}^{g} \\ \boldsymbol{\beta}_{b_{i} b_{j}} &=\boldsymbol{\beta}_{b_{i} b_{j}}+\mathbf{J}_{b_{i}^{a}}^{\beta} \delta \mathbf{b}_{i}^{a}+\mathbf{J}_{b_{i}^{g}}^{\beta} \delta \mathbf{b}_{i}^{g} \\ \mathbf{q}_{b_{i} b_{j}} &=\mathbf{q}_{b_{i} b_{j}} \otimes\left[\begin{array}{c}{1} \\ {\frac{1}{2} \mathbf{J}_{b_{i}^{g}}^{q} \delta \mathbf{b}_{i}^{g}}\end{array}\right] \end{aligned} αbibjβbibjqbibj=αbibj+Jbiaαδbia+Jbigαδbig=βbibj+Jbiaβδbia+Jbigβδbig=qbibj[121Jbigqδbig]

关于bias的Jacobian可以根据IMU预积分讨论的协方差递推公式,一步步递推获得。
递推公式为:
J k + 1 = F J k , J 0 = I J_{k+1}=FJ_k,J_0=I J

你可能感兴趣的:(VINS,VINS论文学习与代码解读)