VINS滑窗及边缘化

Nie_Xun

已于 2024-06-22 17:37:15 修改

阅读量963

点赞数 9

分类专栏： SLAM VINS 文章标签：算法

于 2024-05-16 18:43:30 首次发布

本文链接：https://blog.csdn.net/weixin_41469272/article/details/138969668

版权

SLAM 同时被 2 个专栏收录

40 篇文章 35 订阅

订阅专栏

VINS

3 篇文章 0 订阅

订阅专栏

1 最小二乘问题及优化方法

在初始化过程中，使用SFM方法(三角化/PNP交叉迭代)得到粗略估计的所有帧相机位姿。
VINS综合使用预积分、视觉重投影以及边缘化后的先验项共同构成带权重的最小二乘问题(马氏距离)，而后进行求解，从而得到更加准确且平滑的轨迹曲线，及更准确的特征点坐标。
最小二乘问题求解的介绍可参见https://blog.csdn.net/weixin_41469272/article/details/121994485

2 滑窗及边缘化

SLAM中，优化算法可以使用全局优化，或者局部优化。全局优化即对所有帧的位姿及所有特征点坐标进行优化。全局优化的运算量较大，滑动窗口作为灵活的局部优化方式，被运用到SLAM领域中。滑窗优化思想是指定一个待优化的窗口宽度，随着新帧的到来，将老的帧信息移出窗口，每次都优化窗口内的帧及特征点。
详细的原理参见https://blog.csdn.net/weixin_41469272/article/details/121994485

2.1 原理

通常滑动窗口与边缘化要联合使用。滑动窗口在移出老帧的信息时，可以通过边缘化的方式将移出的老帧信息转化为先验约束添加在约束方程中，从而使得老帧的信息也能够进一步对新帧起到约束作用。

2.2 VINS滑窗原理：

2.2.1 问题公式构建

VINS使用的边缘化的问题约束如下：
在这里插入图片描述

其中，待优化的变量为：

$\mathcal{X}$ 表示滑窗内所有待优化的变量，包括：

$n$ 帧位姿相关的参数 $x_k$ ， $x_k$ 包含该帧相机对应的平移、速度、角度以及加速度计、陀螺仪的零偏（零偏是随机游走的）。
注意：计算的位姿变量是imu体坐标系到世界坐标系下的变换关系。
相机与IMU之间的位姿差异 $x_c^b$ ，用于IMU与相机坐标系之间的转换
$m$ 个特征点的深度值 $\lambda_l, (l=1,…, m)$

约束方程构成：
 第一项为经过边缘化后得到的先验，公式的表述仅仅是示意(并不是真正的先验约束)。
 第二项为IMU与积分误差。
 第三项为视觉重投影误差。

2.2.2 预积分约束

计算公式如下：
在这里插入图片描述
即使用待估计变量计算得到结果(位姿、速度、零偏)与IMU预积分得到的结果差异，
可从体坐标系下，PVQ预积分公式得到，其协方差 $P_{b_{k+1}}^{b_k}$ 的计算
详细参见https://blog.csdn.net/weixin_41469272/article/details/138505746

(设第 $k$ 帧到 $k + 1$ 帧之间的预积分残差)其涉及到的变量包括：第 $k$ 帧位姿，第 $k$ 帧速度及零偏；第 $k + 1$ 帧位姿，第 $k + 1$ 帧速度及零偏。
在这里插入图片描述
其雅可比如下：

2.2.3 视觉重投影约束

在这里插入图片描述
将第l个特征点从第i帧图像坐标系转换到第j帧坐标系，并得到二维图像坐标与实际观测到的图像坐标进行对比。

$\hat{\bar{\mathcal{P}}}_{l}^{c_{j}}$ 和 $\frac{\mathcal{P}_{l}^{c_{j}}}{\|\mathcal{P}_{l}^{c_{j}}\|}$ 均是位于j帧相机为中心的球面上， $b_1$ 和 $b_2$ 为位于 $\hat{\bar{\mathcal{P}}}_{l}^{c_{j}}$ 切平面上的两个正交向量。
$\left.\left[\begin{array}{cc}\mathbf{b}_{1}&\mathbf{b}_{2}\end{array}\right.\right]^{T}\cdot\left(\hat{\bar{\mathcal{P}}}_{l}^{c_{j}}-\frac{\mathcal{P}_{l}^{c_{j}}}{\left\|\mathcal{P}_{l}^{c_{j}}\right\|}\right)$ 将误差投影到 $\hat{\bar{\mathcal{P}}}_{l}^{c_{j}}$ 切平面上。

视觉重投影误差对应的协方差 $P^{c_j}_l$ (代码中)设置为1.5像素映射到 $\hat{\bar{\mathcal{P}}}_{l}^{c_{j}}$ 切平面上(即归一化平面，即1.5/焦距)*Cauchy权重。
在这里插入图片描述

(设第 $i$ 帧与第 $j$ 帧对第 $l$ 个路标点的观测残差)涉及的参数包括：
第 $i$ 帧位姿，第 $j$ 帧位姿，第l个路标点的深度(世界坐标系下)。此外还有imu与相机之间的外参:
在这里插入图片描述
其雅可比计算方程如下：

详细参见《崔华坤_《主流VIO技术综述及VINS解析》》

2.3 边缘化的原理

利用schur 补将要移出的变量相关信息转为新的滑窗的先验信息。边缘化是基于信息矩阵的操作。
设以下为求解最小二乘问题的更新公式：
$H\cdot\Delta x=\begin{bmatrix}\Lambda_a&\Lambda_b\\{\Lambda_b}^T&\Lambda_c\end{bmatrix}\begin{bmatrix}\delta x_a\\\delta x_b\end{bmatrix}=\begin{bmatrix}b_a\\b_b\end{bmatrix}$

设我们要移出 $x_a$ 变量，使用边缘化的方法(利用schur补)，得到边缘化后的公式如下：
$(\Lambda_{c}-\Lambda_{b}^{T}\Lambda_{a}^{-1}\Lambda_{b})\delta x_{b}=b_{b}-\Lambda_{b}^{T}\Lambda_{a}^{-1}b_{a}$
详细原理介绍参见：https://blog.csdn.net/weixin_41469272/article/details/136250678
https://blog.csdn.net/weixin_41469272/article/details/132467293

以下来自《崔华坤_《主流VIO技术综述及VINS解析》》
一个边缘化的例子：设有四个相机位姿 $x_{p_i}$ ，以及6个路标点 $x_{m_k}$ （路标点用xyz的参数化），相机与路标点的边表示一次观测，相邻相机之间的边表示IMU约束，相互关系如下：

在这里插入图片描述
首先marg 相机位姿1 $x_{p_1}$ ，而后再marg掉landmark1 $x_{m_1}$ 。

我们可以看出，在marg掉变量后，与被marg的变量相关的变量之间会产生直接联系，从而它们对应的信息矩阵变的稠密。

2.4 VINS的两种边缘化策略

当倒数第二帧为关键帧时(marginalization_flag 设为 MARGIN_OLD)，将 marg掉最老帧，及其看到的路标点和相关联的IMU数据。
当倒数第二帧为非关键帧时(marginalization_flag 设为 MARGIN_SECOND_NEW)，直接丢弃该帧，但基于其积分得到下一帧的预积分数据。

滑窗里最后一帧是非关键帧，在初始阶段，滑窗保存的是非关键帧，当滑窗满之后，先marg滑窗中后来的非关键帧，而后当关键帧来后，将marg最老的帧，从而，慢慢的，滑窗中只包含关键帧和最新的非关键帧。

关键帧判定代码：
f_manager.addFeatureCheckParallax(frame_count, image, td)调用 compensatedParallax2(it_per_id, frame_count)来计算视差，此函数计算的是次新帧与次次新帧的视差。
此外，当滑窗中帧数较少以及特征点数较少时，也设次新帧为关键帧。

~~在VINS代码中，当新帧到来时，先创建问题，再求解，再边缘化的，边缘化在求解之后。~~
由于在滑窗problem进行求解前，需要将先验约束加入到problem中，然而，先验结束是上一轮边缘化得到的，从而对于窗口内的变量及约束而言，边缘化使用的是次新帧是不是关键帧，而不是通过对新帧的判断。

2.5 代码

VINS使用ceres 来完成滑窗及边缘化。
在VINS初始化过程中，使用SFM对特征点及相机位姿进行不带尺度(up-to-scale)的估计后，就使用了一次滑窗优化，此时尺度是任意设置的，且使用的是ceres自动微分功能。
在VINS初始化之后就进行了滑窗优化。VINS是先执行优化计算后marg的策略。
在每次优化时，都需要重新计算雅可比，残差。Marg后的信息矩阵及残差项会被一次次的再marg。

实现伪代码：当发生边缘化时，说明VINS初始化已经完成，从而滑窗中是足帧的。

循环：设置一个marg管理容器，存放待marg的变量及其相关的残差项，当一个新帧到来：
{
 首先判断该帧是否是关键帧，从而确定滑窗中要边缘化的帧？最老帧/次新帧。
 先求解已有滑窗的变量：

说明参数，添加约束(先验约束(上一轮边缘化后约束)，预积分约束，重投影约束)，计算雅可比。

 边缘化：

边缘化最老帧：最老帧的位姿，速度及零偏，以及该帧第一次观测到的路标点。
边缘化次新帧：直接丢弃该帧位姿及观测，保留预积分量到新帧。

 整理边缘化相关的参数及残差；
 边缘化
 整理滑窗内参数数据(索引)
}

代码简要介绍：
VINS后端优化只处理关键帧。文件：estimator.cpp
主要分为两个核心函数：
循环：{
solveOdometry();
|- f_manager.triangulate：首先进行特征点的重新三角化，由于新的特征观测的加入(由slideWindow()加入)，因而需要重新三角化；
optimization()：实现解算和边缘化。
slideWindow()：将管理关键帧的各种vector进行整理，去掉被marg的项，该迁移的迁移，该释放的释放。
}

以下为optimization()的步骤及代码片段
0：定义problem：

ceres::Problem problem;
ceres::LossFunction *loss_function;
//loss_function = new ceres::HuberLoss(1.0);
loss_function = new ceres::CauchyLoss(1.0);

1：添加参数块到problem:

添加相机位姿到problem，相机位姿的参数更新(李群更新)不是常规更新，因而定义了local_parameterization去指定位姿的更新计算形式。

   problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);

添加速度及零偏(加速度计及陀螺仪零偏)到problem

   problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);

可选项，添加相机与IMU之间的位姿关系到problem，同样参数是李群更新

   problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);

可选项，添加IMU及图像之间额外的硬件时间偏差

   problem.AddParameterBlock(para_Td[0], 1);

2：添加残差计算方式到problem

添加marg得到的先验项

MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
problem.AddResidualBlock(marginalization_factor, NULL,
                         last_marginalization_parameter_blocks);

添加预积分约束到problem：

 IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
 problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);

添加视觉重投影约束到problem:

//可选, 有TD(传感器发布时间差)
ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity, it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,  t_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
problem.AddResidualBlock(f_td, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]);

//无TD
ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j); 
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);

所有残差及雅可比的计算均在其对应的Factor，即xxxFactor::Evaluate()下。

3: 回环相关(后续补充)

4: 设置求解参数并求解

    ceres::Solver::Options options;

    options.linear_solver_type = ceres::DENSE_SCHUR;
    //options.num_threads = 2;
    options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = NUM_ITERATIONS;
    ......
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);

5. 设置边缘化信息
当要marg掉滑窗中的最老帧

设置先验信息中的marg信息

MarginalizationFactor *marginalization_factor = new MarginalizationFactor(last_marginalization_info);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(marginalization_factor, NULL, last_marginalization_parameter_blocks, drop_set);
//drop_set：包含最老帧(第0帧)的位姿、速度及零偏。

设置IMU预积分对应的marg信息

IMUFactor* imu_factor = new IMUFactor(pre_integrations[1]);
ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(imu_factor, NULL, vector<double *>{para_Pose[0], para_SpeedBias[0], para_Pose[1],      para_SpeedBias[1]}, vector<int>{0, 1});//参数最后一项对应drop_set，表示需要marg的变量为最老帧(第0帧)位姿、速度及零偏。
marginalization_info->addResidualBlockInfo(residual_block_info);

设置重投影对应的marg信息

   if (ESTIMATE_TD)
   {
      ProjectionTdFactor *f_td = new ProjectionTdFactor(pts_i, pts_j, it_per_id.feature_per_frame[0].velocity, it_per_frame.velocity,
                                                                  it_per_id.feature_per_frame[0].cur_td, it_per_frame.cur_td,
                                                                  it_per_id.feature_per_frame[0].uv.y(), it_per_frame.uv.y());
      ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f_td, loss_function,
                                                                                vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index], para_Td[0]},
                                                                                vector<int>{0, 3});
      marginalization_info->addResidualBlockInfo(residual_block_info);
   } else {
      ProjectionFactor *f = new ProjectionFactor(pts_i, pts_j);
      ResidualBlockInfo *residual_block_info = new ResidualBlockInfo(f, loss_function,
                                                                               vector<double *>{para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]},
                                                                               vector<int>{0, 3});  // 这里第0帧和地图点被margin
      marginalization_info->addResidualBlockInfo(residual_block_info);
   }

6. 计算待marg变量相关的雅可比，残差等信息

MarginalizationInfo::preMarginalize()
    for (auto it : factors)
    {
        it->Evaluate();
        ......
    }

7. 边缘化(多线程)
MarginalizationInfo::marginalize()：构造信息矩阵及残差向量，执行schur补操作。

Eigen::VectorXd bmm = b.segment(0, m);  // 带边缘化的大小
Eigen::MatrixXd Amr = A.block(0, m, m, n);  // 对应的四块矩阵
Eigen::MatrixXd Arm = A.block(m, 0, n, m);
Eigen::MatrixXd Arr = A.block(m, m, n, n);
Eigen::VectorXd brr = b.segment(m, n); // 剩下的参数
A = Arr - Arm * Amm_inv * Amr;
b = brr - Arm * Amm_inv * bmm;

vector2double()：ceres使用的是double数组，而VINS多用Eigen进行运算，这个函数用来处理eigen数据到double数组的转换，从而用于ceres。

问题：
VINS-mono 算法在经过滑窗方法计算出关键帧的位姿后，是如何优化非关键帧的位姿的？
此外由于初始化完成后，会调整参考世界坐标系的yaw角，但是并没有将特征点进行更新。

参考文献

《VINS 论文推导及代码解析崔华坤》
《VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator》

Nie_Xun

关注

9
点赞
踩
14

收藏

觉得还不错? 一键收藏
0
评论
VINS滑窗及边缘化

滑窗优化思想是指定一个待优化的窗口宽度，随着新帧的到来，将老的帧信息移出窗口，每次都优化窗口内的帧及特征点。由于在滑窗problem进行求解前，需要将先验约束加入到problem中，然而，先验结束是上一轮边缘化得到的，从而对于窗口内的变量及约束而言，边缘化使用的是次新帧是不是关键帧，而不是通过对新帧的判断。在VINS初始化过程中，使用SFM对特征点及相机位姿进行不带尺度(up-to-scale)的估计后，就使用了一次滑窗优化，此时尺度是任意设置的，且使用的是ceres自动微分功能。
复制链接

扫一扫