VIO的图优化模型

因子图结构

VIO在纯视觉的基础上添加了IMU约束，因子图如下：

状态变量

VIO中，待估计的状态变量为$\theta^i=(R_{WB}^i,_Wp_B^i,_Wv_B^i,b_i^g,b_i^a)$θi=(RWBi​,W​pBi​,W​vBi​,big​,bia​)*注意不是$T_{WC}$TWC​
观测量为$z_{il}\in C_i$zil​∈Ci​,表示第i帧看到的第j个路标点,输入$(\tilde{\omega_i},\tilde{a_i})\in I_{ij}$(ωi​~​,ai​~​)∈Iij​,第i、j准帧间惯导信息，其中$(i,j)\in K_k$(i,j)∈Kk​,为关键帧的集合。

误差优化函数

$\theta^*=\arg\min_\theta \sum_k[E_{proj}(k,j)+E_{IMU}(i,j)],k\in K_k$θ∗=argθmin​k∑​[Eproj​(k,j)+EIMU​(i,j)],k∈Kk​包括重投影误差项及IMU误差项。
重投影误差项:$E_{proj}(k,j)=\rho((x^k-\pi(X_c^k))^T\Sigma_k(x^k-\pi(X_c^k))) \\ \pi(X_c)=\left[ \begin{matrix} f_u \frac{X_c}{Z_c}+c_u \\ f_v \frac{Y_c}{Z_c}+c_v \end{matrix} \right],X_c=\left[ \begin{matrix}X_c&Y_c&Z_c \end{matrix} \right]^T \\ Xc=T_{CW}P_W=(T_{WB}T_{BC})^TP_W=T_{CB}T_{WB}^TP_W,T_{WB}^T=\left[ \begin{matrix} R_{WB}^T &-R_{WB}^T {_Wp_B} \\ 0^T&1 \end{matrix} \right] \\ X_c^k=R_{CB}R_{WB}^T(X_W^k-_Wp_B^j)+_cp_B$Eproj​(k,j)=ρ((xk−π(Xck​))TΣk​(xk−π(Xck​)))π(Xc​)=[fu​Zc​Xc​​+cu​fv​Zc​Yc​​+cv​​],Xc​=[Xc​​Yc​​Zc​​]TXc=TCW​PW​=(TWB​TBC​)TPW​=TCB​TWBT​PW​,TWBT​=[RWBT​0T​−RWBT​W​pB​1​]Xck​=RCB​RWBT​(XWk​−W​pBj​)+c​pB​IMU误差$E_{IMU}(i,j)=\rho(\left[\begin{matrix}e_R&e_v&e_p\end{matrix}\right]^T\Sigma_I\left[\begin{matrix}e_R&e_v&e_p\end{matrix} \right])+\rho(e_b^T\Sigma_Re_b) \\ e_R=Log(\Delta R_{ij}Exp(J_{\Delta R}^g\delta b_i^g)R_{BW}^iR_{WB}^j) \\ e_v=R_{BW}^i(_Wv_B^j-_Wv_B^i-g_W\Delta t_{ij})-(\Delta v_{ij}+J_{\Delta v}^g\delta b_i^g+J_{\Delta v}^a\delta b_i^a) \\ e_p=R_{BW}^i(_Wp_B^j-_Wp_B^i-_Wv_B^i\Delta t_{ij}-\frac12g_W\Delta t_{ij}^2)-(\Delta p_{ij}+J_{\Delta p}^g\delta b_i^g+J_{\Delta p}^a\delta b_i^a) \\ e_b=\left[ \begin{matrix}b_j^g-b_i^g \\ b_j^a-b_i^a \end{matrix} \right]$EIMU​(i,j)=ρ([eR​​ev​​ep​​]TΣI​[eR​​ev​​ep​​])+ρ(ebT​ΣR​eb​)eR​=Log(ΔRij​Exp(JΔRg​δbig​)RBWi​RWBj​)ev​=RBWi​(W​vBj​−W​vBi​−gW​Δtij​)−(Δvij​+JΔvg​δbig​+JΔva​δbia​)ep​=RBWi​(W​pBj​−W​pBi​−W​vBi​Δtij​−21​gW​Δtij2​)−(Δpij​+JΔpg​δbig​+JΔpa​δbia​)eb​=[bjg​−big​bja​−bia​​]

图优化模型节点及边

在ORB-SLAM2中，图优化存在于三个线程：Tracking、Local Mapping、Loop Closing。分别处理局部共视区域中的非线性优化，地图更新时的优化、位姿图优化检测闭环。

Tracking

1. 地图更新后第j个帧到达时（非关键帧），将其与最近的一个关键帧i建立联系，第i帧作为先验且固定，优化重投影及IMU误差估计第j帧状态
2. 优化结束，将得到的第j帧状态变量及Hessian矩阵传递至下次优化作为先验
3. 第j+1帧到达，使用第j帧的优化结果作为第j帧的初值和此次优化先验，联合优化第j、j+1帧，此时待优化的状态变量为$\theta=(R_{WB}^j,_Wp_B^j,_Wv_B^j,b_j^g,b_j^a,R_{WB}^{j+1},_Wp_B^{j+1},_Wv_B^{j+1},b_{j+1}^g,b_{j+1}^a)$θ=(RWBj​,W​pBj​,W​vBj​,bjg​,bja​,RWBj+1​,W​pBj+1​,W​vBj+1​,bj+1g​,bj+1a​),误差函数为$\theta^*=\arg\min_\theta \sum_k[E_{proj}(k,j+1)+E_{IMU}(j,j+1)+E_{prior}(j)]$θ∗=argminθ​∑k​[Eproj​(k,j+1)+EIMU​(j,j+1)+Eprior​(j)],增加了先验误差项.$E_{prior}(j)=\rho([e_Re_ve_pe_b]^T\Sigma_p[e_Re_ve_pe_b])\\e_R=Log(\bar{R}_{WB}^j{R}_{WB}^j), \ e_v=_W\bar{v}^{j}_{B}-_Wv^{j}_{B}, \ e_p=_W\bar{p}^{j}_{B}-_Wp^j_{B}, \ e_b=\bar{b}_j-b_j$Eprior​(j)=ρ([eR​ev​ep​eb​]TΣp​[eR​ev​ep​eb​])eR​=Log(RˉWBj​RWBj​), ev​=W​vˉBj​−W​vBj​, ep​=W​pˉ​Bj​−W​pBj​, eb​=bˉj​−bj​带横线的为先验。
4. 联合优化结束后，边缘化掉第j帧，重复步骤2、3，直到地图有更新（出现新的关键帧），重新执行1开始

该过程对应的图模型

一、LocalBAPRVIDP对应Tracking中的步骤一：

节点：

1. 外点：vTcb 类型：VertexNavStatePR；ID：ExtrinsicVertexId = KeyFrame::nNextId*3 + MapPoint::nNextId + 1（每一个关键帧三个节点，每一个路标点一个节点，一个外部节点）；固定不优化
2. LocalKeyFrame帧节点pKF0：
   1）PR节点（位姿）vNSPR 类型：VertexNavStatePR；
   2）V节点（速度）vNSV 类型：VertexNavStateV
   3）Bias节点 vNSBias 类型：VertexNavStateBias
3. FixedCameras 节点（固定）pKF1:
   1）PR节点（位姿）vNSPR 类型：VertexNavStatePR；对于常规的关键帧，只添加该节点。
   2）对于划窗前最近的关键帧，添加V和Bias节点
   V节点（速度）vNSV 类型：VertexNavStateV
   Bias节点 vNSBias 类型：VertexNavStateBias
4. MapPoint节点 vPoint：类型：VertexIDP

边：

1. 名称：epvr：连接PR0, PR1, V0, V1, B0
   类型：EdgeNavStatePRV
2. 名称：ebias：连接B0,B1
   类型：EdgeNavStateBias
3. 名称: e：连接vPoint(idp),refPR,curPR,TcbPR
   类型：EdgePRIDP

图模型：

二、GlobalBundleAdjustmentNavStatePRV

节点：

1. PR节点（位姿）vNSPR 类型：VertexNavStatePR；
2. V节点（速度）vNSV 类型：VertexNavStateV
3. Bias节点 vNSBias 类型：VertexNavStateBias
4. MapPoint节点 vPoint 类型：VertexSBAPointXYZ

边：

1. 名称： epvr：连接PR0, PR1, V0, V1, B0
   类型：EdgeNavStatePRV
2. 名称： ebias：连接B0,B1
   类型：EdgeNavStateBias
3. 名称： e：连接vPoint，vPR
   类型：EdgeNavStatePRPointXYZ

图模型：

三、LocalBundleAdjustmentNavStatePRV同上

四、GlobalBundleAdjustmentNavState

节点

1. PVR节点 vNSPVR：类型：VertexNavStatePVR
2. Bias节点 vBias： 类型：VertexNavStateBias
3. MapPoint节点 vPoint ：类型：VertexSBAPointXYZ

边

1. 名称：epvr：连接PVR0,PVR1,B0
   类型：EdgeNavStatePVR
2. 名称： ebias：连接B0,B1
   类型：EdgeNavStateBias
3. 名称：e：连接vPoint，vNSPVR
   类型：EdgeNavStatePVRPointXYZ

图模型

五、PoseOptimization对应Tracking中的步骤二、三：

节点

1. PVR节点 vNSPVR/ vNSFPVRlast：类型：VertexNavStatePVR
2. Bias节点 vNSFBias/ vNSFBiaslast： 类型：VertexNavStateBias

边

1. 名称：eNSPrior：连接vNSFPVRlast，vNSFBiaslast先验误差
   类型：EdgeNavStatePriorPVRBias
2. 名称：eNSPVR：连接vNSFPVRlast，vNSPVR，vNSFBiaslast IMU误差
   类型：EdgeNavStatePVR
3. 名称：eNSBias：连接vNSFBiaslast，vNSFBias
   类型：EdgeNavStateBias
4. 名称： e：连接vNSFPVRlast；连接vNSFPVR
   类型：EdgeNavStatePVRPointXYZOnlyPose

图模型

六、PoseOptimization

去掉了eNSPrior和只连接vPVRLast的边e

七、LocalBundleAdjustmentNavState同上相同？

八、OptimizeInitialGyroBias

该优化函数用于陀螺仪偏置初始化

节点

1. GyrBias节点：vBiasg：类型VertexGyrBias

边

1. 名称：eBiasg：连接vBiasg陀螺仪偏置一元边
   类型：EdgeGyrBias

九、LocalBundleAdjustment

节点

1. 位姿节点：vSE3：类型VertexSE3Expmap
2. MapPoint节点： vPoint：类型VertexSBAPointXYZ

边

1. 名称：e：连接vSE3，vPoint
   类型：EdgeStereoSE3ProjectXYZ

图模型