VINS之estimator节点小结

最新推荐文章于 2024-06-11 23:58:29 发布

酸菜余

最新推荐文章于 2024-06-11 23:58:29 发布

阅读量938

点赞数 4

分类专栏： VINS学习

本文链接：https://blog.csdn.net/weixin_43821376/article/details/103426276

版权

VINS学习专栏收录该内容

9 篇文章 6 订阅

订阅专栏

VINS的核心节点，包括VIO的初始化过程、紧耦合的非线性化过程、边缘化处理过程

主要流程步骤

1、主函数线程

订阅了四个topic，分别调用回调函数；创建了13个话题发布器；开辟了一个VIO线程

IMU_TOPIC：存入 imu_buf，并发布最新的PQV位姿信息（未优化过的），用于高频的IMU输出，便于实时控制
feature_tracker/feature ：存入feature_buf
pose_graph/match_points：存入relo_buf
feature_tracker/restart：清空数据，重启estimator，时间重置
registerPub()：在visualition函数类中

2、Process函数（VIO线程函数）

2.1 getMeasurements( )

通过条件变量互锁机制，得到时间同步的measurements数据
示意图：

2.2 estimator.processIMU( )

新建一个IMU类，中值积分（同时不断更新IMU的雅克比矩阵和协方差矩阵）
雅克比矩阵在ceres优化时，构建IMU残差项的代价函数时会用到
协方差矩阵在ceres优化时，计算IMU残差时，作为权重使用

2.3 estimator.setReloFrame( )

设置一些回环帧的变量

2.4 estimator.processImage( )

2.5 发送topic

pubOdometry(estimator, header);//"odometry"、"path"、"relocalization_path" 、输出位姿数据到文件中
pubKeyPoses(estimator, header);//"key_poses"（markerarray ,球形，红色，应该是滑动窗口中的一组球列表)
pubCameraPose(estimator, header);//"camera_pose" 、"camera_pose_visual"（markerarray，绿色的长方形框和四条线)
pubPointCloud(estimator, header);//"point_cloud"、"history_cloud"（边缘化的点云） 
pubTF(estimator, header);//tf发布器发布了tf信息和"extrinsic" 相机到IMU的外参
pubKeyframe(estimator);//"keyframe_point"、"keyframe_pose" 
pubRelocalization(estimator);//"relo_relative_pose"

2.6 update( )

更新优化过的高频IMU输出数据

代码文件介绍：

在这里插入图片描述

factor（主要包括ceres优化时残差项的代价函数）
- imu_factor.h：imu残差项的代价函数
- integration_base.h：imu数据处理类
- marginalization_factor.h/.cpp：边缘化后的约束信息及其代价函数
- pose_local_parameterization.h/.cpp：位姿的过参数处理类
- projection_factor.h/cpp：特征点重投影误差项的代价函数
initial（主要包括vio初始化时的函数）
- inital_alignment.h/.cpp：视觉惯性联合
- inital_ex_rotation.h/.cpp：求取外参ric
- inital_sfm.h/.cpp：纯视觉恢复窗口帧位姿
- solve_5pts.h/.cpp：2d-2d的对极几何约束恢复位姿
utility（功能函数）
CameraPoseVisualization.h/.cpp：相机的可视化类
tic_toc.h：时间统计类
utility.h/.cpp：eigen数据的一些运算处理，如左乘、右乘、变成反对称阵形式、变成欧拉角形式
visualization.h/.cpp：创建和发布topic的函数集合
estimator_node.cpp：主节点函数
estimator.h/.cpp：estimator评估器类
feature_manager.h/.cpp：特征点管理器类
parameters.h/.cpp：配置参数读取

核心步骤的实现

ProcessImage（）

图像数据的处理

1. f_manager.addFeatureCheckParallax( )

添加图像帧的特征点到f_manager特征点管理器中，并通过检测两帧之间的视差判断次新帧是否作为关键帧
如果计算视差：
根据特征点在最近出现的两帧的归一化坐标的欧氏距离确定，所有特征点的平均距离即为视差，视差大于30认为可以作为关键帧

2. 构建all_image_frame变量

map<double, ImageFrame> all_image_frame;  //key是时间戳，ImageFrame中包含了图像帧信息和预积分信息

ImageFrame类在intial_alignment定义
all_image_frame包括了窗口跨越的时间段内所有的图像帧

3. 如果需要，进行外参评估得到ric

initial_ex_rotation.CalibrationExRotation( )

vins外参推导

4. VIO松耦合初始化

initialStructure()

1）判断IMU是否充分运动
2）将特征点存入sfm_f中

vector<SFMFeature> sfm_f;

3）从窗口中选出一个参考帧l（要求和当前帧的视差足够大）

relativePose(relative_R, relative_T, l)

4）全局sfm恢复窗口中每帧的相对位姿

sfm.construct（）

5）恢复all_frame_frame中的所有帧的位姿
6）视觉惯性初始化

visualInitialAlign()

VisualIMUAlignment() //包括恢复bias、g、尺度s、所有图像帧的速度
投影到标准世界坐标系

5. 三角化解得所有特征点深度，并进行非线性优化

solveOdometry()

6. 滑动窗口

slideWindow()

如果次新帧是关键帧，则边缘化最老帧，将其看到的特征点和IMU数据转化为先验信息约束
如果次新帧不是关键帧，则舍弃视觉测量而保留IMU测量值，从而保证IMU预积分的连贯性

7. 故障检测与恢复

failureDetection()

系统切换回初始化状态，但保留keyframebase

Optimization( )

1. 添加优化参数块

problem.AddParameterBlock(para_Pose[i], SIZE_POSE, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], SIZE_SPEEDBIAS);
problem.AddParameterBlock(para_Ex_Pose[i], SIZE_POSE, local_parameterization);
//problem.SetParameterBlockConstant(para_Ex_Pose[i]);
problem.AddParameterBlock(para_Td[0], 1);
problem.AddParameterBlock(relo_Pose, SIZE_POSE, local_parameterization);

注意这里少了特征点逆深度的参数块para_Feature[i]，其在AddResidualBlock中默认加入优化参数中

2. 添加残差块

problem.AddResidualBlock(marginalization_factor, NULL, last_marginalization_parameter_blocks);    
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
problem.AddResidualBlock(f, loss_function, para_Pose[imu_i], para_Pose[imu_j], para_Ex_Pose[0], para_Feature[feature_index]);
problem.AddResidualBlock(f, loss_function, para_Pose[start], relo_Pose, para_Ex_Pose[0], para_Feature[feature_index]);

在计算残差时，包含了四个部分：先验项、IMU残差、视觉残差、回环帧残差
注意这里用到了IMUFactor、ProjectionFactor、MarginalizationFactor三个代价函数类，里面包含了残差对优化参数的雅克比矩阵以及残差的计算，残差的计算采用马氏距离，需要把协方差矩阵的逆(信息矩阵)作为权重加入其中。
关于雅克比矩阵的推导，可以参考崔华坤的笔记
IMU残差项的雅克比矩阵：
重投影残差项的雅克比矩阵：
边缘化先验信息的雅克比矩阵：
通过舒尔补得到

3. 求解

ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.trust_region_strategy_type = ceres::DOGLEG;
options.max_num_iterations = NUM_ITERATIONS;
options.max_solver_time_in_seconds = SOLVER_TIME;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);

4. 边缘化得到新的先验信息

在这里插入图片描述

边缘化策略：
VINS 根据次新帧是否为关键帧,分为两种边缘化策略
- 当次新帧为关键帧时, 将 marg 掉最老帧,及其看到的路标点和相关联的 IMU 数据,将其转化为先验信息加到整体的目标函数中:
1. 把上一次先验项中的残差项(尺寸为 n)传递给当前先验项,并从中去除需要丢弃的状态量;
2. 将滑窗内第 0 帧和第 1 帧间的 IMU 预积分因子(pre_integrations[1])放到marginalization_info 中,即图中上半部分中 x 0 和 x 1 之间的表示 IMU 约束的黄色块;
3. 挑选出第一次观测帧为第 0 帧的路标点 , 将对应的多组视觉观测放到marginalization_info 中,即图中上半部分中 x 0 所看到的红色五角星的路标点;
4. marginalization_info->preMarginalize():得到每次 IMU 和视觉观测(cost_function)对应的参数块(parameter_blocks),雅可比矩阵(jacobians),残差值(residuals);
5. marginalization_info->marginalize():多线程计整个先验项的参数块,雅可比矩阵和残差值
- 当次新帧不是关键帧时,将直接扔掉次新帧及它的视觉观测边,而不对次新帧进行 marg,因为我们认为当前帧和次新帧很相似,也就是说当前帧跟路标点之间的约束和次新帧与路标点的约束很接近,直接丢弃并不会造成整个约束关系丢失过多信息。但注意要保留次新帧的 IMU 数据,从而保证 IMU 预积分的连贯性。

这里的边缘化和slam十四讲中的边缘化不同。

高翔书中的边缘化目的是为了加速求解，并非真的将路标点给边缘化掉，在计算过程中，先消去路标点变量(很多)，实现先求解相机位姿，然后再利用求解出来的相机位姿反过来计算路标点的过程。
$\left[\begin{array}{cc}{\boldsymbol{I}} & {-\boldsymbol{E} \boldsymbol{C}^{-1}} \\ {\boldsymbol{0}} & {\boldsymbol{I}}\end{array}\right]\left[\begin{array}{cc}{\boldsymbol{B}} & {\boldsymbol{E}} \\ {\boldsymbol{E}^{\boldsymbol{T}}} & {\boldsymbol{C}}\end{array}\right]\left[\begin{array}{c}{\Delta \boldsymbol{x}_{c}} \\ {\Delta \boldsymbol{x}_{p}}\end{array}\right]=\left[\begin{array}{cc}{\boldsymbol{I}} & {-\boldsymbol{E} \boldsymbol{C}^{-1}} \\ {\boldsymbol{0}} & {\boldsymbol{I}}\end{array}\right]\left[\begin{array}{l}{\boldsymbol{v}} \\ {\boldsymbol{w}}\end{array}\right]$
$\left[\begin{array}{cc}{B-E C^{-1} \boldsymbol{E}^{T}} & {0} \\ {E^{T}} & {C}\end{array}\right]\left[\begin{array}{c}{\Delta x_{c}} \\ {\Delta x_{p}}\end{array}\right]=\left[\begin{array}{c}{v-E C^{-1} w} \\ {w}\end{array}\right]$
$\left[\boldsymbol{B}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{E}^{T}\right] \Delta \boldsymbol{x}_{c}=\boldsymbol{v}-\boldsymbol{E} \boldsymbol{C}^{-1} \boldsymbol{w}$
vins中则真正需要边缘化掉滑动窗口中的最老帧或者次新帧，目的是希望不再计算这一帧的位姿或者与其相关的路标点，但同事希望保留该帧对窗口内其他帧的约束关系。

$\left[\begin{array}{cc}{I} & {0} \\ {-\Lambda_{b}^{T} \Lambda_{a}^{-1}} & {I}\end{array}\right]\left[\begin{array}{cc}{\Lambda_{a}} & {\Lambda_{b}} \\ {\Lambda_{b}^{T}} & {\Lambda_{c}}\end{array}\right]\left[\begin{array}{c}{\delta x_{a}} \\ {\delta x_{b}}\end{array}\right]=\left[\begin{array}{cc}{I} & {0} \\ {-\Lambda_{b}^{T} \Lambda_{a}^{-1}} & {I}\end{array}\right]\left[\begin{array}{l}{g_{a}} \\ {g_{b}}\end{array}\right]$
$\left[\begin{array}{cc}{\Lambda_{a}} & {\Lambda_{b}} \\ {0} & {\Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}}\end{array}\right]\left[\begin{array}{c}{\delta x_{a}} \\ {\delta x_{b}}\end{array}\right]=\left[\begin{array}{c}{g_{a}} \\ {g_{b}-\Lambda_{b}^{T} \Lambda_{a}^{-1} g_{a}}\end{array}\right]$
$\left(\Lambda_{c}-\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}\right) \delta x_{b}=g_{b}-\Lambda_{b}^{T} \Lambda_{a}^{-1} g_{a}$
其中， $\Lambda_{b}^{T} \Lambda_{a}^{-1} \Lambda_{b}$ 就称为 $\Lambda_{a}$ 在 $\Lambda_{b}$ 中的Schur项

酸菜余

关注

4
点赞
踩
10

收藏

觉得还不错? 一键收藏
0
评论
VINS之estimator节点小结

VINS的核心节点，包括VIO的初始化过程、紧耦合的非线性化过程、边缘化处理过程主要流程步骤1、主函数线程订阅了四个topic，分别调用回调函数；创建了13个话题发布器；开辟了一个VIO线程IMU_TOPIC：存入 imu_buf，并发布最新的PQV位姿信息（未优化过的），用于高频的IMU输出，便于实时控制feature_tracker/feature ：存入feature_b...
复制链接

扫一扫

专栏目录