VINS-Fusion开源代码之loop_fusion_node数据流分析

于 2024-09-16 15:09:29 发布
阅读量1.1k 收藏 18
点赞数 8
分类专栏： Linux Physics 文章标签： linux 无人机 VINS-Fusion
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/lida2003/article/details/142302857
版权
Linux 同时被 2 个专栏收录
196 篇文章 13 订阅
订阅专栏
Physics
74 篇文章 17 订阅
订阅专栏

VINS-Fusion开源代码之loop_fusion_node数据流分析

1. 源由

之前对VINS-Fusion做了简单了解，详见：《Linux 35.5 + JetPack v5.1.3@VINS-Fusion编译安装》。

为了更好的从模块化视角了解该模块功能，我们需要通过类似灰盒的角度去分析模块的数据流，以便进一步深入模块设计。

在《VINS-Fusion开源代码之vins_node数据流分析》已经对其中一个模块进行了分析，接下来研究loop_fusion_node。

2. 方法

分析方法，请参考：《VINS-Fusion开源代码之vins_node数据流分析》。

3. 仿真分析

借助脚本和ROS系统的强大DEBUG功能，在实际执行以下模拟命令时：

  • terminal 1 仿真环境
$ source ./devel/setup.sh
$ roslaunch vins vins_rviz.launch
  • terminal 2 逻辑算法1 - vins_node
$ source ./devel/setup.sh
$ rosrun vins vins_node src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
  • terminal 3 逻辑算法2 - loop_fusion_node
$ source ./devel/setup.sh
$ rosrun loop_fusion loop_fusion_node src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
  • terminal 4 仿真回放
$ source ./devel/setup.sh
$ rosbag play MH/MH_04_difficult.bag

VINS-Fusion | loop_fusion_node

3.1 rostopic 清单

根据启动脚本vins_rviz.launch，具体内容详见附录。

以及rostopic list指令，可以清晰的看到整个环境目前所有的话题。

$ rostopic  list
/cam0/image_raw
/cam1/image_raw
/clicked_point
/clock
/feature_tracker/feature
/globalEstimator/car_model
/globalEstimator/global_path
/imu0
/initialpose
/leica/position
/loop_fusion/base_path
/loop_fusion/camera_pose_visual
/loop_fusion/margin_cloud_loop_rect
/loop_fusion/match_image
/loop_fusion/match_image/mouse_click
/loop_fusion/odometry_rect
/loop_fusion/path_1
/loop_fusion/path_2
/loop_fusion/path_3
/loop_fusion/path_4
/loop_fusion/path_5
/loop_fusion/path_6
/loop_fusion/path_7
/loop_fusion/path_8
/loop_fusion/path_9
/loop_fusion/point_cloud_loop_rect
/loop_fusion/pose_graph
/loop_fusion/pose_graph_path
/move_base_simple/goal
/rosout
/rosout_agg
/tf
/tf_static
/vins_cam_switch
/vins_estimator/camera_pose
/vins_estimator/camera_pose_visual
/vins_estimator/extrinsic
/vins_estimator/image_track
/vins_estimator/image_track/mouse_click
/vins_estimator/imu_propagate
/vins_estimator/key_poses
/vins_estimator/keyframe_point
/vins_estimator/keyframe_pose
/vins_estimator/margin_cloud
/vins_estimator/odometry
/vins_estimator/path
/vins_estimator/point_cloud
/vins_imu_switch
/vins_restart

3.2 rosbag 输入

仿真数据在回放过程，将会向系统注入以下topic：

  • /imu0
  • /cam0/image_raw
  • /cam1/image_raw
  • /leica/position
$ rosbag info MH/MH_04_difficult.bag
path:        MH/MH_04_difficult.bag
version:     2.0
duration:    1:42s (102s)
start:       Jun 25 2014 03:28:47.33 (1403638127.33)
end:         Jun 25 2014 03:30:29.91 (1403638229.91)
size:        1.4 GB
messages:    25855
compression: none [1356/1356 chunks]
types:       geometry_msgs/PointStamped [c63aecb41bfdfd6b7e1fac37c7cbe7bf]
             sensor_msgs/Image          [060021388200f6f0f447d0fcd9c64743]
             sensor_msgs/Imu            [6a62c6daae103f4ff57a132d6f95cec2]
topics:      /cam0/image_raw    2033 msgs    : sensor_msgs/Image
             /cam1/image_raw    2032 msgs    : sensor_msgs/Image
             /imu0             20320 msgs    : sensor_msgs/Imu
             /leica/position    1470 msgs    : geometry_msgs/PointStamped

数据格式详见：

$ rosmsg show geometry_msgs/PointStamped
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Point point
  float64 x
  float64 y
  float64 z

daniel@daniel-nvidia:~/ego-planner$ rosmsg show sensor_msgs/Imu
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Quaternion orientation
  float64 x
  float64 y
  float64 z
  float64 w
float64[9] orientation_covariance
geometry_msgs/Vector3 angular_velocity
  float64 x
  float64 y
  float64 z
float64[9] angular_velocity_covariance
geometry_msgs/Vector3 linear_acceleration
  float64 x
  float64 y
  float64 z
float64[9] linear_acceleration_covariance

$ rosmsg show geometry_msgs/PointStamped
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/Point point
  float64 x
  float64 y
  float64 z

$ rosmsg show sensor_msgs/Image
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
uint32 height
uint32 width
string encoding
uint8 is_bigendian
uint32 step
uint8[] data

4. 代码分析

4.1 启动入口

算法逻辑代码启动过程如下：

rosrun loop_fusion loop_fusion_node src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
 └──> loop_fusion/loop_fusion_node/loop_fusion
     └──> VINS-Fusion\loop_fusion\src\pose_graph_node.cpp (main)

4.2 配置文件

配置文件中含有各类数据，包括输入的topic路径：

VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml
 ├──> "/imu0"
 ├──> "/cam0/image_raw"
 ├──> "/cam1/image_raw"
 ├──> "cam0_mei.yaml"
 ├──> "cam1_mei.yaml"
 └──> "~/output/pose_graph/"
../support_files/brief_k10L6.bin
../support_files/brief_pattern.yml

4.3 业务数据流

loop_fusion/loop_fusion_node/loop_fusion
                                            +---------------------------+ 
INT.1 "/vins_estimator/odometry"       ---> |    vio_callback         --|---+
INT.2 "/cam0/image_raw"                ---> |    image_callback       --|---|---+
INT.3 "/vins_estimator/keyframe_pose"  ---> |    pose_callback          |   |   |
INT.4 "/vins_estimator/extrinsic"      ---> |    extrinsic_callback     |   |   |
INT.5 "/vins_estimator/keyframe_point" ---> |    point_callback       --|---|---|---+
INT.6 "/vins_estimator/margin_cloud"   ---> |    margin_point_callback--|---|---|---|---+
                                            |       process           --|---|---|---|---|---+
                                            +---------------------------+   |   |   |   |   |
                                                                            |   |   |   |   |
                                                                            |   |   |   |   |
                                                                            |   |   |   |   |
OUT                                                            <------------+   |   |   |   |
 ├──> OUT.6 visualization_msgs::MarkerArray "camera_pose_visual"                |   |   |   |
 └──> OUT.9 nav_msgs::Odometry "odometry_rect"                                  |   |   |   |
                                                                                |   |   |   |
OUT                                  <------------------------------------------+   |   |   |
 ├──> OUT.1 nav_msgs::Path "pose_graph_path"                                        |   |   |
 ├──> OUT.2 nav_msgs::Path "base_path"                                              |   |   |
 ├──> OUT.3 visualization_msgs::MarkerArray "pose_graph"                            |   |   |
 └──> OUT.4 nav_msgs::Path "path_1..9"                                              |   |   |
                                                                                    |   |   |
OUT.7 sensor_msgs::PointCloud "point_cloud_loop_rect"     <-------------------------+   |   |
                                                                                        |   |
OUT.8 sensor_msgs::PointCloud "margin_cloud_loop_rect"    <-----------------------------+   |
                                                                                            |
OUT.5 sensor_msgs::Image "match_image"                    <---------------------------------+

5. vins_node + loop_fusion_node 总流程图

                                                 vins_node                                                                        loop_fusion_node
                                      +-------------------------------------+
                                      |     +---------------------------+   |                                                  +--------------------------+ 
IN.1 "/imu0"                      ----|-->  |   imu_callback            |   +---------------------------------------------->   |   image_callback         |
IN.2 "/feature_tracker/feature"   ----|-->  |   feature_callback -------|---+-- nav_msgs::Odometry "odometry" ------------->   |   vio_callback           |
IN.3 "/cam0/image_raw"            ----+-->  |   img0_callback    ----+  |   +-- nav_msgs::Odometry "keyframe_pose" -------->   |   pose_callback          |  ===>  rviz
IN.4 "/cam1/image_raw"            ------->  |   img1_callback    ----+  |   +-- sensor_msgs::PointCloud "margin_cloud"  --->   |   margin_point_callback  |   ^
IN.5 "/vins_restart"              ------->  |   restart_callback     |  |   +-- nav_msgs::Odometry "extrinsic"  ----------->   |   extrinsic_callback     |   |
IN.6 "/vins_imu_switch"           ------->  |   imu_switch_callback  |  |   +-- sensor_msgs::PointCloud "keyframe_point" -->   |   point_callback         |   |
IN.7 "/vins_cam_switch"           ------->  |   cam_switch_callback  |  |   |                                                  +--------------------------+   |
                                            |   sync_process   <-----+  |   +-------------------------------------------------------------------------------->+
                                            |       |                   |                                                                                     |
                                            |       +-------------------|------ sensor_msgs::Image "image_track" ---------------------------------------------+
                                            +---------------------------+

6. 附录

6.1 loop_fusion_node 本地文件

"~/output/vio_loop.csv"

"~/output/pose_graph/i_briefdes.dat"
"~/output/pose_graph/i_keypoints.txt"
"~/output/pose_graph/pose_graph.txt"

6.2 vins_node 数据流

vins/vins_node/vins_estimator
                                            +---------------------------+  
IN.1 "/imu0"                       --->     |   imu_callback     -------|--+
IN.2 "/feature_tracker/feature"    --->     |   feature_callback -------|--|--+
IN.3 "/cam0/image_raw"             --->     |   img0_callback    ----+  |  |  |
IN.4 "/cam1/image_raw"             --->     |   img1_callback    ----+  |  |  |
IN.5 "/vins_restart"               --->     |   restart_callback     |  |  |  |
IN.6 "/vins_imu_switch"            --->     |   imu_switch_callback  |  |  |  |
IN.7 "/vins_cam_switch"            --->     |   cam_switch_callback  |  |  |  |
                                            |   sync_process   <-----+  |  |  |
                                            |       |                   |  |  |
                                            |       +-------------------|--|--|--+
                                            +---------------------------+  |  |  |
                                                                           |  |  |
OUT.1 nav_msgs::Odometry "imu_propagate"     <-----------------------------+  |  |
                                                                              |  |
OUT      <--------------------------------------------------------------------+  |
 ├──> pubOdometry                                                                |
 │    ├──> OUT.2 nav_msgs::Path> "path"                                          |
 │    ├──> OUT.3 nav_msgs::Odometry "odometry"                                   |
 │    └──> "~/output/pose_graph/vio.csv"                                         |
 ├──> pubKeyPoses                                                                |
 │    └──> OUT.6 visualization_msgs::Marker "key_poses"                          |
 ├──> pubCameraPose                                                              |
 │    ├──> OUT.7 nav_msgs::Odometry "camera_pose"                                |
 │    └──> OUT.8 visualization_msgs::MarkerArray "camera_pose_visual"            |
 ├──> pubPointCloud                                                              |
 │    ├──> OUT.4 sensor_msgs::PointCloud "point_cloud"                           |
 │    └──> OUT.5 sensor_msgs::PointCloud "margin_cloud"                          |
 ├──> pubKeyframe                                                                |
 │    ├──> OUT.9 nav_msgs::Odometry "keyframe_pose"                              |
 │    └──> OUT.10 sensor_msgs::PointCloud "keyframe_point"                       |
 └──> pubTF                                                                      |
      └──> OUT.11 nav_msgs::Odometry "extrinsic"                                 |
                                                                                 |
OUT.12 sensor_msgs::Image "image_track"      <-----------------------------------+
lida2003
关注
专栏目录
从零完成slam实战，以Vins-Fusion为例
u010196944的博客
10-10 6017
ubuntu20.04 + d435i + vins-fusion，从零完成slam实战
Vins-Fusion——三角化triangulate
u010196944的博客
11-14 1490
Vins-Fusion双目初始化时，三角化triangulate求解特征点的3D位置（路标点）
VINS-Fusion
04-02
香港科技大学的项目VINS-Fusion，单目视觉惯性系统的在线时间校准
Vins-Fusion 学习笔记
qq_43525734的博客
03-18 4223
VINS-Fusion 基本介绍 VINS-Fusion 是继 VINS-Mono 和 VINS-Mobile（单目视觉惯导 SLAM 方案）后，香港科技大学沈劭劼老师开源的双目视觉惯导 SLAM 方案，VINS-Fusion 是一种基于优化的多传感器状态估计器，可实现自主应用（无人机，汽车和 AR / VR）的精确自定位。 VINS-Fusion 是 VINS-Mono 的扩展，支持多种视觉惯性...
vinsfusion
danmeng8068的博客
04-08 676
https://blog.csdn.net/huanghaihui_123/article/details/86518880 https://blog.csdn.net/weixin_41843971/article/details/86537228 https://blog.csdn.net/wangbaodong070411209/article/details/8655...
VINS_FUSION
qqGHJ的博客
10-25 8205
VINS_FUSION代码解析
VINS-Fusion 回环检测pose_graph_node
u010196944的博客
07-22 742
回环检测pose_graph_node，检测到回环的两帧，当前帧和数据库中回环帧。当前帧在世界坐标系下的位姿包含累计偏移，当前帧下的世界坐标系与数据库中回环帧不重合，闭环重定位的作用就是将当前帧世界坐标系累计的误差重映射到原世界坐标系，达成全局一致位姿。
VINS-FUSION-leanrning:VINS-FUSION中文注释版。现有网络上对于VINS-mono的代码已经有很多讲解和注释了，但是对于VINS-FUSION（以下简称VF）的注释还是很少的，刚好本人最近也正在学习VIO的相关知识，所以对VF按照程序执行顺序进行了十分详细的注释，同时为了和大家进行交流学习，所以把相关注释代码进行开源。因水平有限，错误肯定很多，还请各位大佬们指正-源码时代
03-24
VINS-Fusion中文注释版 创新学习用。 十分感谢港科大各位老师的工作。 目前唯一注释了前端VIO部分。 相应博客 参考内容 以及一些其他的博文，在代码中也进行了注释。 基于优化的多传感器状态估计器 VINS-Fusion是...
Vins-Fusion整体框架，数据流分析
u010196944的博客
10-14 2893
Vins-Fusion整体框架，数据流分析
VINS-Fusion 多传感器全局位姿估计的一种通用优化框架
最新发布
u010196944的博客
08-05 159
对于自动导航的机器人来说，精确的状态估计是基本问题。为了实现局部精确和全局无漂移的位姿估计，通常将具有互补属性的多个传感器进行融合。在一个小的区域内，局部传感器，如相机、IMU、Lidar等，提供了精确的位姿，而在一个大场景环境下，全局传感器，如gps、magnetometer、barometer等，提供有噪声但全局无漂移的定位。在本文中，我们提出一种传感器融合框架，将局部状态和全局传感器融合，实现局部精确和全局无漂移的位姿估计。现有的VO/VIO方法产生的局部估计，在位姿图优化中与全局传感器融合。在图优化
VINS论文及代码解析.pdf
11-13
崔老师的VINS论文及代码解析，里面详细对vins算法的实现进行了讲解，以及对应论文的讲解，对应代码的讲解
bench_ws:用于与不同状态估计算法（即 VINS-Mono、VINS-Fusion、ORBSLAM3、Stereo-MSCKF 等）进行比较的 catkin 工作区
05-29
在 EuroC 数据集上运行 首先获取数据集。 默认情况下，此项目提供的启动文件假定您有一个名为/data/euroc_mav/rosbags的目录，并且其中包含 rosbags，如下所示： MH_01.bag MH_02.bag MH_03.bag MH_04.bag MH_05....
【VINS-Fusion】学习笔记
weixin_48852054的博客
06-06 1176
【代码】【VINS-Fusion】学习。
ubuntu20.04运行VINS-Fusion（跑EuRoC数据集）
2301_80311277的博客
12-16 598
修改VINS-Fusion中的euroc_mono_imu_config.yaml文件，将其中结果输出保存路径修改为本地的绝对路径或相对路径。其中vinsfusion_ws是我的工作空间，修改为你的工作空间名即可。把工作空间放入.bashrc文件中，这样就不用每次source。以EuRoC数据集为例，打开四个终端，分别输入。这个博客写的非常全，按照要求修改即可。和VINS-Mono一样的流程。
vins-fusion解读
bufengzj的博客
01-04 5056
关心vins-fusion，尤其是融合双目与imu那块。 1. 如何安装测试？数据集与自己的相机 1.1 安装测试 1.2 数据集 1.3 自己的相机 目前的相机有小觅相机（带有硬件时间对齐IMU)，realsense，ZED。 2. 论文与资源 代码：https://github.com/HKUST-Aerial-Robotics/VINS-Fusion 文章：T. Q...
【VSLAM】VINS-Fusion原理及源码解析（一）——初识VINS-Fusion
caiqidong321的博客
08-16 4237
本系列博客以`VINS-Fusion`源码流程为导向，从入口函数一步步讲解前端和后端，最后完成仿真和真机实验部署，希望可以细致入微地对`VINS-Fusion`作一个完整全面的讲解。
VINS-fusion 跑通Euroc、TUM、KITTI数据集，以及评估工具EVO的下载和使用
weixin_49247766的博客
10-09 4601
ubuntu20.04中python版本是3.x，因此对应的pip工具会变成pip3，可以先参考如下链接查看自己系统的python和pip版本。再调用如下命令对evo工具进行安装。
vins-fusion全注释 代码
05-01
vins-fusion（Visual-Inertial State Estimation）是一种基于视觉惯性传感器融合的状态估计方法，它可以通过采集视觉传感器和惯性传感器的数据来预测系统的运动状态和姿态。vins-fusion的代码主要包含以下部分： 1.数据预处理：数据预处理是将来自视觉传感器和惯性传感器的数据进行处理和同步，以便后续使用。它包括预处理图像、同步时间戳和计算IMU预积分等步骤。 2.初始化：vins-fusion需要先进行初始化，以估计系统的初始状态和姿态。初始化的过程需要使用一些先验知识，如初始姿态、重力矢量等。 3.特征提取和匹配：vins-fusion利用特征点来进行运动估计和位姿估计。在特征提取和匹配阶段，需要对图像进行特征提取和描述，然后匹配特征点，以估计相机的相对运动。 4.状态估计：vins-fusion利用多个相机和IMU数据来估计系统的状态和姿态，包括相机和IMU的位姿、速度和加速度等参数。状态估计的过程主要包括相机-IMU联合优化和后端优化两个步骤。 5.后处理：后处理主要是对估计结果进行滤波和校验，以提高系统的鲁棒性和精度。在后处理阶段，需要进行滑动窗口优化和误差校正等操作。 总之，vins-fusion全注释的代码实现了基于视觉惯性传感器融合的状态估计方法，可以在实际应用中用于机器人导航、自主驾驶等场景中。
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值