VINS-MONO边缘化策略

VINS中的滑窗优化策略，将滑出窗外的帧与滑窗内的帧的约束使用边缘化的形式保存为先验误差进行后续非线性优化，以保留约束信息。本文对具体的方案进行记录。
紧耦合VIO处理的两种误差分别为IMU误差与图像误差，采用LM、GN、Dogleg等方式迭代求解待优化参数增量，以求得最大似然估计，最小化非线性化误差。
迭代优化的核心即求解增量方程：
Hδx=bH\delta x=bHδx=b 其中H=JTJ,b=JTe,e=(JT)+bH=J^TJ,b=J^Te,e=(J^T)^+bH=JTJ,b=JTe,e=(JT)+b.此处的()+()^+()+为伪逆，eee为残差，JJJ为残差对于状态的Jacobian。
由于引入滑窗后，对于滑到窗口之外的帧不再优化其参数，但其与滑窗内的数据仍然存在约束，直接丢掉这些窗外帧会造成约束信息丢失，所以要将其封装成先验信息，加入到大的非线性优化问题中作为一部分误差，这一步即为边缘化。假设要边缘化的状态为x2x_2x2，要保留的状态为x1x_1x1，对于上述增量方程，变为：[H11H12H21H22][δx1δx2]=[b1b2]\left[ \begin{matrix} H_{11} & H_{12} \\ H_{21} & H_{22} \end{matrix} \right] \left[ \begin{matrix} \delta x_1\\ \delta x_2\end{matrix} \right]=\left[ \begin{matrix} b_1\\ b_2\end{matrix} \right] [H11H21H12H22][δx1δx2]=[b1b2] 边缘化的方法为舒尔补：[I−H12H22−10I][H11H12H21H22][δx1δx2]=[I−H12H22−10I][b1b2][H11−H12H22−1H210H21H22][δx1δx2]=[b1−H12H22−1b2b2]\left[ \begin{matrix} I & -H_{12}H_{22}^{-1} \\ 0 & I \end{matrix} \right]\left[ \begin{matrix} H_{11} & H_{12} \\ H_{21} & H_{22} \end{matrix} \right] \left[ \begin{matrix} \delta x_1\\ \delta x_2\end{matrix} \right]=\left[ \begin{matrix} I & -H_{12}H_{22}^{-1} \\ 0 & I \end{matrix} \right]\left[ \begin{matrix} b_1\\ b_2\end{matrix} \right] \\ \left[ \begin{matrix} H_{11}-H_{12}H_{22}^{-1}H_{21} & 0 \\ H_{21} & H_{22} \end{matrix} \right]\left[ \begin{matrix} \delta x_1\\ \delta x_2\end{matrix} \right]=\left[ \begin{matrix} b_{1}-H_{12}H_{22}^{-1}b_{2}\\ b_2\end{matrix} \right][I0−H12H22−1I][H11H21H12H22][δx1δx2]=[I0−H12H22−1I][b1b2][H11−H12H22−1H21H210H22][δx1δx2]=[b1−H12H22−1b2b2] 这样只需要求解第一个方程，即(H11−H12H22−1H21)δx1=b1−H12H22−1b2,(H_{11}-H_{12}H_{22}^{-1}H_{21})\delta x_1= b_{1}-H_{12}H_{22}^{-1}b_{2},(H11−H12H22−1H21)δx1=b1−H12H22−1b2, 这样既没有损失约束信息，又不需要求解δx2\delta x_2δx2,把H11−H12H22−1H21H_{11}-H_{12}H_{22}^{-1}H_{21}H11−H12H22−1H21记为H0∗H_0^*H0∗，b1−H12H22−1b2b_{1}-H_{12}H_{22}^{-1}b_{2}b1−H12H22−1b2记为b0∗b_0^*b0∗。
上式为边缘化后待求解的增量方程，我们要根据这个增量方程恢复出边缘化封装的先验误差的具体形式epe_pep
随着迭代的过程，状态量被不断更新。但在边缘化时，被边缘化的状态值固定，舒尔补时使用的雅克比即为当时泰勒展开使用该固定的值(x线性化点)求得的雅克比，都需要固定。在VINS中，线性化点处的参数值x0被保存为keep_block_data。此即为EFJ，更多解释详看滑窗优化、边缘化、舒尔补、FEJ及fill-in问题。但在上述方程中，由于b=JTeb=J^Teb=JTe，其中的eee随着状态的更新而变化，因此bbb也需随之变化。
状态的更新（此处δx\delta xδx表示当前参数值相对x0x_0x0的变化）：x′=Exp(δx)⊗x0x'=Exp(\delta x)\otimes x_0x′=Exp(δx)⊗x0 此时b的更新可以表示为b=b0+∂b∂x∣x0δx=b0+JT∂e∂x∣x0δx=b0+JTJδx=b0+Hδxb=b_0+\frac{\partial b}{\partial x}|_{x_0}\delta x\\=b_0+J^T\frac{\partial e}{\partial x}|_{x_0}\delta x\\=b_0+J^TJ\delta x=b_0+H\delta xb=b0+∂x∂b∣x0δx=b0+JT∂x∂e∣x0δx=b0+JTJδx=b0+Hδx 有[b1b2]=[b1,0b2,0]+[H11H12H21H22][δx1δx2]b1=b1,0+H11δx1+H12δx2b2=b2,0+H21δx1+H22δx2\left[ \begin{matrix} b_1\\b_2\end{matrix} \right]=\left[ \begin{matrix} b_{1,0}\\b_{2,0}\end{matrix} \right]+\left[ \begin{matrix} H_{11} & H_{12} \\ H_{21} & H_{22} \end{matrix} \right] \left[ \begin{matrix} \delta x_1\\ \delta x_2\end{matrix} \right] \\ b_1=b_{1,0}+H_{11} \delta x_1+ H_{12} \delta x_2\\ b_2=b_{2,0}+H_{21} \delta x_1+ H_{22} \delta x_2[b1b2]=[b1,0b2,0]+[H11H21H12H22][δx1δx2]b1=b1,0+H11δx1+H12δx2b2=b2,0+H21δx1+H22δx2 联立b∗=b1−H12H22−1b2b^*=b_{1}-H_{12}H_{22}^{-1}b_{2}b∗=b1−H12H22−1b2 有b∗=b1−H12H22−1b2=(b1,0+H11δx1+H12δx2)−H12H22−1(b2,0+H21δx1+H22δx2)=b1,0−H12H22−1b2,0+(H11−H12H22−1H21)δx1=b0∗+H0∗δx1b^*=b_{1}-H_{12}H_{22}^{-1}b_{2}\\=(b_{1,0}+H_{11} \delta x_1+ H_{12} \delta x_2)-H_{12}H_{22}^{-1}(b_{2,0}+H_{21} \delta x_1+ H_{22} \delta x_2)\\ =b_{1,0}-H_{12}H_{22}^{-1}b_{2,0}+(H_{11}-H_{12}H_{22}^{-1}H_{21})\delta x_1\\ =b_0^*+H_0^*\delta x_1b∗=b1−H12H22−1b2=(b1,0+H11δx1+H12δx2)−H12H22−1(b2,0+H21δx1+H22δx2)=b1,0−H12H22−1b2,0+(H11−H12H22−1H21)δx1=b0∗+H0∗δx1 由上述推导，原增量方程变为H0∗δx=JlTJlδx=b∗=b0∗+H0∗dx=b0∗+JlTJldx=JlT(JlT)+b0∗+JlTJldx=JlT((JlT)+b0∗+Jldx)H_0^*\delta x=J_l^TJ_l\delta x=b^*=b_0^*+H_0^*dx\\ =b_0^*+J_l^TJ_ldx=J_l^T(J_l^T)^+b_0^*+J_l^TJ_ldx=J_l^T((J_l^T)^+b_0^*+J_ldx)H0∗δx=JlTJlδx=b∗=b0∗+H0∗dx=b0∗+JlTJldx=JlT(JlT)+b0∗+JlTJldx=JlT((JlT)+b0∗+Jldx) 其中JlJ_lJl为H0∗H_0^*H0∗分解出的等价雅克比，δx\delta xδx为每次参数更新增量，dxdxdx为当前参数值与线性化点x0x_0x0的差值。
因此，边缘化后等价的先验误差ep=(JlT)+b∗=(JlT)+b0∗+Jldxe_p=(J_l^T)^+b^*\\=(J_l^T)^+b_0^*+J_ldxep=(JlT)+b∗=(JlT)+b0∗+Jldx至此，边缘化的步骤可以总结为：

舒尔补求解H0∗=H11−H12H22−1H21,b0∗=b1,0−H12H22−1b2,0H_0^*=H_{11}-H_{12}H_{22}^{-1}H_{21},b_0^*=b_{1,0}-H_{12}H_{22}^{-1}b_{2,0}H0∗=H11−H12H22−1H21,b0∗=b1,0−H12H22−1b2,0
分解H0∗H_0^*H0∗求解Jl、JlT、(JlT)+J_l、J_l^T、(J_l^T)^+Jl、JlT、(JlT)+，求解e0=(JlT)+b0∗e_0=(J_l^T)^+b_0^*e0=(JlT)+b0∗，保存Jl,e0J_l,e_0Jl,e0
求解δx\delta xδx由H0∗δx=b0∗H_0^*\delta x=b_0^*H0∗δx=b0∗（ceres完成）
使用δx\delta xδx更新xxx,计算当前状态xxx与x0x_0x0的差值dxdxdx,更新eee:ep=e0+Jldxe_p=e_0+J_ldxep=e0+Jldx（costfunction定义residuals，
同时定义costfunction需要的jacobians，把JlJ_lJl由localsize转为globalsize）
ceres自动更新b∗b^*b∗:b∗=JlTepb^*=J_l^Te_pb∗=JlTep:
求解δx\delta xδx由H0∗δx=b∗H_0^*\delta x=b^*H0∗δx=b∗（ceres完成）
循环4,5

附录：

使用H分解J的方式如下：
H=USUT=JTJH=USU^T=J^TJH=USUT=JTJ 对H进行奇异值分解，由于H为半正定实矩阵，S也即为特征值矩阵，这样J=S12UT(JT)+=S−12UTJ=S^\frac12U^T\\(J^T)^+=S^{-\frac12}U^T J=S21UT(JT)+=S−21UT

VINS-MONO边缘化策略相关推荐

m2dgr数据集在vins mono运行
m2dgr数据集在vins mono运行 1.代码的修改 1.1 yaml文件的修改 1.2修改VINS-mono轨迹保存代码 1.3修改launch文件 2 EVO显示轨迹 2.1运行比较结果参考 ...
SLAM之小觅相机跑开源方案（ORB_SLAM2，VINS MONO，VINS FUSION，RTAB-Map）
传感器: 小觅相机标准版开源SLAM方案: ORB_SLAM2,VINS MONO,VINS FUSION,RTAB-Map 测试地点: 室内大厅(光线不均)/ 露天阳台实现形式: 小觅相机 / ...
图解vins后端边缘化
后端代码逻辑 vins后端大部分代码集中在optimization函数中,而此函数中的边缘化占据相当一部分内容. 对边缘化后Hδx=b的处理因为对于ceres,传入的是雅可比和残差进行迭代,所以首先 ...
的环境下 qt 运行在_Ubuntu16.04环境下运行vins mono（环境配置及编译）之ROS kinetic的安装...
所需环境:ubuntu16.04+ROS kinetic+opencv 3.3.1+eigen3.3.3+ceres solver 1.14 1.ROS Kinetic 的安装 (1)设置source ...
边缘化你必须知道的一件事！(FEJ知识点总结)
vins和g2o边缘化的异同:(已经做到ppt里面了,简单回顾一下) 1.<视觉slam14讲>中提及的边缘化(G2O边缘化)是在计算求解过程中,先消去路标点变量,实现先求解相机位姿,然后 ...
VINS Fusion GPS融合部分
概述 VINS Fusion在VINS Mono的基础上,添加了GPS等可以获取全局观测信息的传感器,使得VINS可以利用全局信息消除累计误差,进而减小闭环依赖. 局部传感器(如相机,IMU,激光雷达 ...
GVINS：基于GNSS视觉惯性紧耦合的平滑状态估计方法
文章:GVINS: Tightly Coupled GNSS-Visual-Inertial Fusion for Smooth and Consistent State Estimation 作者: ...
GVINS文章暴力翻译（仅供自学）
GVINS文章暴力翻译(仅供自学) 摘要 1. 介绍 2. 相关工作 3.符号和定义 A. 框架 b .状态 4.GNSS基本介绍 A. GNSS 概述 B. 伪距测量 C. 多普勒测量 D. SPP ...
VSLAM 相关知识点总结
VSLAM 相关知识点这篇文章是对VSLAM涉及的知识点进行系统性的总结和更新,一些内容来源至VSLAM经典教材,博客,和开源项目(引用材料如下表) SLAM十四讲高博古月老师的技术博客崔神的g ...
ORBSLAM3论文翻译
ORB-SLAM3: An Accurate Open-Source Library for Visual, Visual-Inertial and Multi-Map SLAM 摘要本文介绍了OR ...

VINS-MONO边缘化策略

附录：

VINS-MONO边缘化策略相关推荐

最新文章

热门文章