算法简介
1. 算法框架概述
VINS系列基于视觉-惯性融合实现高精度导航，分为四模块：
- 数据预处理：图像直方图均衡、KLT特征点提取、去畸变处理[图像畸变：镜头导致的像素位置失真]和光流跟踪；IMU数据预积分[避免重复计算，提升效率]。
- 系统初始化 ：纯视觉SFM[从运动中恢复结构]恢复无尺度位姿；通过匹配点**≥30个** 和视差**≥20像素**筛选关键帧，三角化3D点；在线标定IMU-相机旋转外参。
- 滑窗VIO ：紧耦合优化，残差包括边缘化先验、视觉重投影和IMU残差；使用滑动窗口控制变量规模，边缘化旧帧以保留约束。
- 回环检测 ：基于DBoW2词袋库，匹配回环帧后，通过本质矩阵RANSAC[随机采样一致性]和外点剔除，引入4自由度位姿图优化。
2. 关键技术深度解析
- 光流跟踪 ：核心为金字塔LK光流[利用图像金字塔处理大位移]，满足灰度一致性、连续小运动和空间一致性假设；vins-fusion引入双向光流检查 （像素差阈值**≤0.5**）提升鲁棒性。
- IMU预积分 ：推导连续到离散模型（中值积分），用误差卡尔曼滤波[避免四元数过参数化]优化状态估计；对零偏做一阶近似更新，减少计算量。关键公式：预积分协方差传递依赖雅可比矩阵。
- 初始化优化 ：视觉-惯性对齐恢复尺度、重力向量和帧间速度；重力细化后模长固定为9.81 m/s²，分解到切平面正交基优化。
- 紧耦合VIO：状态变量包括滑窗内IMU位姿、外参和逆深度；视觉残差在归一化平面单位球面计算，抑制噪声。
一、边缘化策略与FEJ原理
1. 边缘化的本质与目的
- 核心作用 ：维持SLAM系统的可观测性，解决边缘化旧状态变量 与保留约束信息的矛盾
- 数学本质 ：求条件概率分布 p ( x k ∣ x j ) p(x_k | x_j) p(xk∣xj) 的近似（Schur补实现）
Σ 11 Σ 12 Σ 21 Σ 22 \] − 1 = \[ Λ 11 Λ 12 Λ 21 Λ 22 \] \\begin{bmatrix} \\Sigma_{11} \& \\Sigma_{12} \\\\ \\Sigma_{21} \& \\Sigma_{22} \\end{bmatrix}\^{-1} = \\begin{bmatrix} \\Lambda_{11} \& \\Lambda_{12} \\\\ \\Lambda_{21} \& \\Lambda_{22} \\end{bmatrix} \[Σ11Σ21Σ12Σ22\]−1=\[Λ11Λ21Λ12Λ22
被边缘化部分的协方差： Σ 22 − 1 = Λ 22 − Λ 21 Λ 11 − 1 Λ 12 \Sigma_{22}^{-1} = \Lambda_{22} - \Lambda_{21}\Lambda_{11}^{-1}\Lambda_{12} Σ22−1=Λ22−Λ21Λ11−1Λ12
2. 关键帧/非关键帧差异化处理
关键帧 非关键帧 新帧到来 是否关键帧? MARGIN_OLD边缘化策略 MARGIN_SECOND_NEW策略 边缘化最老关键帧 边缘化次新非关键帧 联合边缘化: 位姿+特征点 只边缘化位姿状态
-
关键帧边缘化：当最老帧是关键帧时执行
- 状态变量： X m a r g = [ P i , V i , Λ j ] X_{marg} = [P_i, V_i, \Lambda_j] Xmarg=[Pi,Vi,Λj]（位姿+速度+关联特征点）
- 信息矩阵保留约束： H p r i o r = H 22 − H 21 H 11 − 1 H 12 H_{prior} = H_{22} - H_{21}H_{11}^{-1}H_{12} Hprior=H22−H21H11−1H12
-
非关键帧边缘化：
- 仅边缘化位姿状态 T i ∈ S E ( 3 ) T_i \in SE(3) Ti∈SE(3)
- 保留特征点约束至后续优化
3. First Estimate Jacobians (FEJ) 原理
- 核心问题：边缘化导致线性化点不一致
- FEJ解决方案：固定线性化点（图示）
使用 固定雅可比矩阵 首次边缘化时刻 构建先验因子 后续优化 保持零空间一致性
- 数学表达 ：
r FEJ = r ( x ^ ) + J x ^ ( x − x ^ ) r_{\text{FEJ}} = r(\hat{x}) + J_{\hat{x}}(x - \hat{x}) rFEJ=r(x^)+Jx^(x−x^)
其中 x ^ \hat{x} x^ 是首次线性化点
4. 特征点保留策略
-
关键帧边缘化：仅保留仍被滑窗观测的特征点
-
特征点管理 ：
list<Feature*> feature_pool（见feature_manager.cpp） -
淘汰条件 ：
cppif (feature.observations.size() < 2) { feature_pool.erase(feature); }
5. 源代码实现
-
位置：
vins_estimator/src/factor/marginalization_factor.cpp -
关键函数：
cppvoid MarginalizationInfo::marginalize() { // Schur补计算 Eigen::MatrixXd Amm_inv = Amm.ldlt().solve(Eigen::MatrixXd::Identity(...)); Eigen::MatrixXd tempB = Arm * Amm_inv; H_prior = Arr - tempB * Amr; b_prior = brr - tempB * bmm; }
二、时间戳同步策略（IMU & LiDAR）
1. 时间戳对齐原理
- 数学表达 ：三次样条插值
p ( t ) = ∑ i = 0 3 c i ( t − t k ) i p(t) = \sum_{i=0}^{3}c_i(t-t_k)^i p(t)=i=0∑3ci(t−tk)i
2. VINS-Mono实现（IMU-Camera同步）
-
核心代码（
System.cpp）：cppvoid Estimator::processMeasurements() { while (!imu_buf.empty() && imu_buf.front()->header.stamp <= img_msg->header.stamp) { imu_msg = imu_buf.front(); imu_buf.pop(); // 线性插值 if (imu_msg->header.stamp < img_msg->header.stamp) { double dt = (img_msg->header.stamp - imu_msg->header.stamp).toSec(); imu_msg_next = imu_buf.front(); double alpha = dt / (imu_msg_next->header.stamp - imu_msg->header.stamp); interpolateIMU(imu_msg, imu_msg_next, alpha); } } }
三、与ORB-SLAM3的架构级对比
| 维度 | VINS-Mono | ORB-SLAM3 |
|---|---|---|
| 初始化 | 视觉-惯性紧耦合对齐（10-15s） | 独立视觉初始化+惯性优化 |
| 优化方式 | 滑动窗紧耦合优化 | 关键帧BA+独立惯性优化 |
| 特征处理 | KLT光流跟踪+逆深度参数化 | ORB特征匹配+DBoW词袋 |
| 重投影 | 单位球面投影 π ( x z , y z ) \pi(\frac{x}{z},\frac{y}{z}) π(zx,zy) | 像素平面投影 |
| 回环检测 | DBoW2词袋库 | 多层次词袋+位置识别 |
| 参数化 | 逆深度 ρ = 1 / z \boxed{\text{逆深度}\ \rho=1/z} 逆深度 ρ=1/z | 3D坐标 [ X , Y , Z ] T [X,Y,Z]^T [X,Y,Z]T |
四、VINS-Mono初始化详解
1. 初始化流程
Vision IMU Alignment Results 纯视觉SFM重建 关键帧选择条件： 1. 视差>20像素 2. 特征点>30个 IMU预积分数据 视觉-惯性对齐 输出初始值： s(尺度) g(重力) v(速度) b(零偏) Vision IMU Alignment Results
2. 视觉-惯性对齐公式
目标函数：
min s , g c ∑ k ∈ B ∥ α b k b k + 1 − R c 0 b k ( s p ˉ c k + 1 c 0 − s p ˉ c k c 0 + 1 2 g c 0 Δ t k 2 − R c 0 b k v k b k Δ t k ) ∥ \min_{s,g^c}\sum_{k\in\mathcal{B}}\|\alpha_{b_k}^{b_{k+1}}-R_{c_0}^{b_k}(s\bar{p}{c{k+1}}^{c_0}-s\bar{p}{c_k}^{c_0}+\frac{1}{2}g^{c_0}\Delta t_k^2-R{c_0}^{b_k}v_k^{b_k}\Delta t_k)\| s,gcmink∈B∑∥αbkbk+1−Rc0bk(spˉck+1c0−spˉckc0+21gc0Δtk2−Rc0bkvkbkΔtk)∥
3. 退化情况分析（平面运动）
当运动为纯平面运动时：
-
不可观测变量：
- 尺度 s s s 与重力 g z g_z gz
- 偏航角（yaw） ψ \psi ψ
- v z v_z vz 速度分量
-
解决方法：
cpp// 重力向量修正（参考源码 gravityRefine()） Eigen::Vector3d g_vec = g.normalized() * 9.81; if (motion_type == PLANAR_MOTION) { g_vec.z() = fixed_value; // 固定Z轴分量 }
五、本质矩阵(F) vs 基础矩阵(E)
1. VINS选择基础矩阵的原因
-
鲁棒性考量 ：
F = K − T E K − 1 F = K^{-T}EK^{-1} F=K−TEK−1- F矩阵在特征点未去畸变时更稳定
- 低视差场景下数值稳定性更高
-
视差范围：
矩阵类型 适用视差范围 E >15像素 F 5-50像素
2. 源代码实现
cpp
// findFundamentalMat() 替代 findEssentialMat()
Mat F = findFundamentalMat(points1, points2, FM_RANSAC, 3.0, 0.99);六、Ceres边缘化实现
1. 自定义边缘化因子
cpp
class MarginalizationFactor : public ceres::CostFunction {
public:
bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const {
// 固定线性化点计算残差
Eigen::Map<const VectorXd> x(parameters[0], size);
residuals[0] = residual_0 + J * (x - x_linearized);
// FEJ: 固定雅可比矩阵
if (jacobians) {
Eigen::Map<MatrixXd> jac(jacobians[0], ...);
jac = J_fixed;
}
return true;
}
};2. VINS边缘化应用
cpp
// 创建边缘化残差块
ceres::CostFunction* marginalization_factor = new MarginalizationFactor(last_marg_info);
problem.AddResidualBlock(marginalization_factor, NULL, remaining_params);七、边缘化技术架构
1. 全流程实现
构建完整Hessian矩阵 状态变量分区 被边缘化变量: x_m 保留变量: x_r 计算Schur补 生成先验约束 作为下一轮先验因子 FEJ固定线性化点
2. 关键数学推导
边缘化后的信息矩阵：
Λ prior = Λ x r x r − Λ x r x m Λ x m x m − 1 Λ x m x r \Lambda_{\text{prior}} = \Lambda_{x_rx_r} - \Lambda_{x_rx_m}\Lambda_{x_mx_m}^{-1}\Lambda_{x_mx_r} Λprior=Λxrxr−ΛxrxmΛxmxm−1Λxmxr
八、工程实践技巧
1. 协方差传播：
cpp
// 预积分协方差传递
MatrixXd F = MatrixXd::Zero(15, 15);
F.block<3, 3>(0, 0) = exp(-delta_t * w_hat);
F.block<3, 3>(3, 6) = Matrix3d::Identity();
covariance = F * covariance * F.transpose() + G * noise * G.transpose();2. 退化处理策略：
cpp
if (isDegenerateMotion()) {
// 1. 锁定不可观测维度
problem.SetParameterBlockConstant(vel_z_ptr);
// 2. 添加伪观测
problem.AddResidualBlock(z_prior_factor, NULL, pose_z_ptr);
}