2021@SDUSC

ROS-3DSLAM（3）：信息流

2021年10月17日星期一——2021年10月20日星期三

一、背景简介：

对于信息流的分析理解能够有助于我们对于整个项目的下一步研究学习。虽然对于局部代码的阅读可能看不出来这样做的好处，但是毫无疑问这样的分析有助于我们下一步的对于整个系统的代码的解读工作。

考虑到雷达包的阅读工作量的问题，本周我负责的任务即为处理这个信息流的工作，这样也方便和组内的同学们统一阅读进度。

二、学习历程：

因为是对于全局代码的分析，考虑到我的时间和精力的问题，直接阅读代码显得不太可行，所以我决定这次阅读的主要参考放在网络上其他人的解读上，这样做的好处是节省了精力，但是坏处就是可能会带来其他人的错误，为此，我需要多加参考比对资料来降低错误发生的概率。但是，又受限于资料有限的问题，能够用来参考的文章实在是屈指可数，所以只能是先尝试着一边看大佬的分析，一边结合代码来完成相应的工作了。先把信息流提取出来，如果后边的学习中发现问题再尝试着改正。

在学习的过程中，我发现了一种名为“tf树”的数据结构，可以用来展示节点间的坐标变换的消息，相应的，这样也把话题信息的传递过程展示了出来，使得我们能够查看信息流。但遗憾的是，这必须在ros运行时展示，受限于手头有限的工具，我目前还没有办法采取这一工作，以后有条件时我会补充上来。

而后，我又发现了一个更为强大的功能包：rqt，它可以直接给ros系统生成node graph（节点图），方便我们对于信息流的直接分析，虽然我依旧没有可以用的ros系统运行的情况，但是网上却有图可以帮助我解决这个问题。这样的话就很方便了，我可以直接从这张图入手，来结合代码进行分析。

三、信息流分析

A、ros信息流预备知识

在分析信息流之前，有必要先回顾一下ros的基本概念：

节点
节点管理器
话题
消息
服务
rqt功能包：Node Graph

后边遇到的稍微重要一些的名词术语我也放在这里了：

imu
odometry
闭环信息
Marker和MarkerArray
pose

B、Node Graph阅读

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-meWV32Qg-1634812390191)(https://xuwuzhou.top/images/LVI_SAM%E5%9B%BE%E7%89%872.png)]

（出处见文末）

这是通过rqt得到的节点图，下面我们开始结合代码对其进行分析。

C、重要节点：

通过上图不难看出这几个重要节点：

/lvi_sam_imuPreintegration(IMU预积分节点)
/lvi_sam_mapOptmization(图优化节点)
/lvi_sam_featureExtraction(特征提取节点)
/lvi_sam_imageProjection(生成深度图)
/lvi_sam_visual_feature(生成视觉特征节点)
/lvi_sam_visual_loop(闭环检测节点)
/lvi_sam_visual_odometry(视觉里程计节点)
/lvi_sam_rviz(传递给rviz节点？)

其中，前四个节点是雷达相关的节点，后面三个是视觉相关的节点，最后一个是rviz相关节点。

我们可以一一分析前七个包，而rviz相关节点的输入可以在其中体现。

我们下面依次来介绍。

我们的分析步骤很简单，先去源代码里找到相关的代码，根据代码分析其输入输出即可。

D、第一部分：雷达相关

1./lvi_sam_imuPreintegration(IMU预积分节点)

//声明
ros::Subscriber subImu;

  ros::Subscriber subOdometry;

  ros::Publisher pubImuOdometry;

  ros::Publisher pubImuPath;
//下文的使用
         subImu      = nh.subscribe<sensor_msgs::Imu>  (imuTopic, 2000, &IMUPreintegration::imuHandler, this, ros::TransportHints().tcpNoDelay());
        subOdometry = nh.subscribe<nav_msgs::Odometry>(PROJECT_NAME + "/lidar/mapping/odometry", 5, &IMUPreintegration::odometryHandler, this, ros::TransportHints().tcpNoDelay());

        pubImuOdometry = nh.advertise<nav_msgs::Odometry> ("odometry/imu", 2000);
        pubImuPath     = nh.advertise<nav_msgs::Path>     (PROJECT_NAME + "/lidar/imu/path", 1);

输入：imu信息 odometry信息输入是对于原始数据的处理

输出：odometry信息轨迹信息； path信息为了给rviz显示

2./lvi_sam_mapOptmization(图优化节点)

//输出
    
    ros::Publisher pubLaserCloudSurround;
    ros::Publisher pubOdomAftMappedROS;
    ros::Publisher pubKeyPoses;
    ros::Publisher pubPath;

    ros::Publisher pubHistoryKeyFrames;
    ros::Publisher pubIcpKeyFrames;
    ros::Publisher pubRecentKeyFrames;
    ros::Publisher pubRecentKeyFrame;
    ros::Publisher pubCloudRegisteredRaw;
    ros::Publisher pubLoopConstraintEdge;
//输出一
 pubKeyPoses = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/trajectory", 1);
 pubLaserCloudSurround= nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/map_global", 1);
 pubOdomAftMappedROS= nh.advertise<nav_msgs::Odometry>(PROJECT_NAME +"/lidar/mapping/odometry", 1);
 pubPath= nh.advertise<nav_msgs::Path>(PROJECT_NAME + "/lidar/mapping/path", 1);
//输入
        subLaserCloudInfo     = nh.subscribe<lvi_sam::cloud_info>     (PROJECT_NAME + "/lidar/feature/cloud_info", 5, &mapOptimization::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
        subGPS                = nh.subscribe<nav_msgs::Odometry>      (gpsTopic,                                   50, &mapOptimization::gpsHandler, this, ros::TransportHints().tcpNoDelay());
        subLoopInfo           = nh.subscribe<std_msgs::Float64MultiArray>(PROJECT_NAME + "/vins/loop/match_frame", 5, &mapOptimization::loopHandler, this, ros::TransportHints().tcpNoDelay());
//输出二
        pubHistoryKeyFrames   = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/loop_closure_history_cloud", 1);
        pubIcpKeyFrames       = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/loop_closure_corrected_cloud", 1);
        pubLoopConstraintEdge = nh.advertise<visualization_msgs::MarkerArray>(PROJECT_NAME + "/lidar/mapping/loop_closure_constraints", 1);

        pubRecentKeyFrames    = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/map_local", 1);
        pubRecentKeyFrame     = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/cloud_registered", 1);
        pubCloudRegisteredRaw = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/mapping/cloud_registered_raw", 1);

输入：激光点云信息、GPS信息、闭环信息

输出：

输出分为两部分，分别是：

输入一：

pubKeyPoses、pubLaserCloudSurround：点云信息；

pubOdomAftMappedROS：odometry信息；

Path：路径信息。

输入二：

pubHistoryKeyFrames、pubIcpKeyFrames ：点云信息；

pubLoopConstraintEdge：MarkerArray信息；

pubRecentKeyFrames、pubRecentKeyFrame、pubCloudRegisteredRaw：点云信息。

可以看出来，这个包的输入比较简单，但是输出有些复杂。

这里我们先搁置一下，最后再讨论这个节点的输入输出。

3./lvi_sam_featureExtraction(特征提取节点)

//声明 
	ros::Subscriber subLaserCloudInfo;

    ros::Publisher pubLaserCloudInfo;
    ros::Publisher pubCornerPoints;
    ros::Publisher pubSurfacePoints;
//输入
     subLaserCloudInfo = nh.subscribe<lvi_sam::cloud_info>(PROJECT_NAME + "/lidar/deskew/cloud_info", 5, &FeatureExtraction::laserCloudInfoHandler, this, ros::TransportHints().tcpNoDelay());
//输出
        pubLaserCloudInfo = nh.advertise<lvi_sam::cloud_info> (PROJECT_NAME + "/lidar/feature/cloud_info", 5);
        pubCornerPoints = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/feature/cloud_corner", 5);
        pubSurfacePoints = nh.advertise<sensor_msgs::PointCloud2>(PROJECT_NAME + "/lidar/feature/cloud_surface", 5);

输入：subLaserCloudInfo点云信息

输出：pubLaserCloudInfo、pubCornerPoints、pubSurfacePoints 点云信息

起到提取到点云信息之后做分类的功能

4./lvi_sam_imageProjection(生成深度图)

//声明
	ros::Subscriber subLaserCloud;
    ros::Publisher  pubLaserCloud;
    
    ros::Publisher pubExtractedCloud;
    ros::Publisher pubLaserCloudInfo;

    ros::Subscriber subImu;
    std::deque<sensor_msgs::Imu> imuQueue;

    ros::Subscriber subOdom;
//输入
 subImu        = nh.subscribe<sensor_msgs::Imu>        (imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
        subOdom       = nh.subscribe<nav_msgs::Odometry>      (PROJECT_NAME + "/vins/odometry/imu_propagate_ros", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());
//输出
        pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> (PROJECT_NAME + "/lidar/deskew/cloud_deskewed", 5);
        pubLaserCloudInfo = nh.advertise<lvi_sam::cloud_info>      (PROJECT_NAME + "/lidar/deskew/cloud_info", 5);

这个稍微有些特别，还是第一个输入比输出多的文件。

输入： subImu Imu信息； subOdom Odometry信息；subLaserCloud 点云信息

输出：pubExtractedCloud、pubLaserCloudInfo点云信息

cloud_deskewed是用于给rviz显示用的.

也就是第二个才是主要信息。

这个imageProjection是生成深度图的节点，获取的都是原始信息：imu、里程计、点云。

*注意：

除了第二个图优化节点之外，这里的所有的输入信息基本上都是原始信息（点云信息、imu信息、里程计信息等等）；
这正好对应了雷达系统辅助视觉系统初始化的体现；
而视觉部分的输入信息包含了雷达系统经过处理之后的加工信息；
所以我们首先分析雷达系统的部分的代码应该是正确的。

E、第二部分：视觉相关

5./lvi_sam_visual_feature(生成视觉特征节点)

//输入
	ros::Subscriber sub_img   = n.subscribe(IMAGE_TOPIC,       5,    img_callback);
	ros::Subscriber sub_lidar = n.subscribe(POINT_CLOUD_TOPIC, 5,    lidar_callback)
//输出
    pub_feature = n.advertise<sensor_msgs::PointCloud>(PROJECT_NAME + "/vins/feature/feature",     5);
	pub_match   = n.advertise<sensor_msgs::Image>     (PROJECT_NAME + "/vins/feature/feature_img", 5);
	pub_restart = n.advertise<std_msgs::Bool>         (PROJECT_NAME + "/vins/feature/restart",     5);

输入：sub_img 原始图像信息，sub_lidar 雷达经过变换之后的点云信息（POINT_CLOUD_TOPIC…）

输出： pub_feature点云信息、pub_match点云所匹配的图像、pub_restart重启信号（初始化验证是否成功的功能？）

值得注意的是，这里的sub_lidar所获取的数据应当是从image Projection当中获取的；但是联系前文和图中的描述，image Projection输出的两个点云中有一个是不重要的、交给rviz显示图像的。但是图中却很明显地显示imageProjection地那个不重要的点云信息传送给了/lvi_sam_visual_feature，也就是本节点，因此这里就产生了矛盾，这个点云信息到底重要与否存在疑问。

后边的输出和image Projection类似，也是深度图。

6./lvi_sam_visual_loop(闭环检测节点)

//输入
	ros::Subscriber sub_image     = n.subscribe(IMAGE_TOPIC, 30, image_callback);
    ros::Subscriber sub_pose      = n.subscribe(PROJECT_NAME + "/vins/odometry/keyframe_pose",  3, pose_callback);
    ros::Subscriber sub_point     = n.subscribe(PROJECT_NAME + "/vins/odometry/keyframe_point", 3, point_callback);
    ros::Subscriber sub_extrinsic = n.subscribe(PROJECT_NAME + "/vins/odometry/extrinsic",      3, extrinsic_callback);

//输出

	ros::Publisher pub_match_img;
	ros::Publisher pub_match_msg;
	ros::Publisher pub_key_pose;

 	pub_match_img = n.advertise<sensor_msgs::Image>             (PROJECT_NAME + "/vins/loop/match_image", 3);
    pub_match_msg = n.advertise<std_msgs::Float64MultiArray>    (PROJECT_NAME + "/vins/loop/match_frame", 3);
    pub_key_pose  = n.advertise<visualization_msgs::MarkerArray>(PROJECT_NAME + "/vins/loop/keyframe_pose", 3);

输入：从图中不难看出，这个节点的关键连接节点是下文中的/lvi_sam_visual_odometry(视觉里程计节点)，结合论文当中的闭环检测，从代码中我们可以看出来这个节点的输入信息都是从/lvi_sam_visual_odometry(视觉里程计节点)中的得到的，包括：sub_pose计算后的位姿信息、sub_point点云信息、sub_extrinsic（？存疑）等信息。

输出：

Image、Float64MultiArray和MarkerArray是最终提供给图优化节点的信息。包含了点云信息，闭环信息和图像信息。

7./lvi_sam_visual_odometry(视觉里程计节点)

//声明
//输入
	ros::Subscriber sub_imu     = n.subscribe(IMU_TOPIC,      5000, imu_callback,  			ros::TransportHints().tcpNoDelay());
	ros::Subscriber sub_odom    = n.subscribe("odometry/imu", 5000, odom_callback);
	ros::Subscriber sub_image   = n.subscribe(PROJECT_NAME + "/vins/feature/feature", 1, feature_callback);
	ros::Subscriber sub_restart = n.subscribe(PROJECT_NAME + "/vins/feature/restart", 1, restart_callback);
//输出
	pub_latest_odometry     = n.advertise<nav_msgs::Odometry>               (PROJECT_NAME + "/vins/odometry/imu_propagate", 1000);
	pub_latest_odometry_ros = n.advertise<nav_msgs::Odometry>               (PROJECT_NAME + "/vins/odometry/imu_propagate_ros", 1000);
	pub_path                = n.advertise<nav_msgs::Path>                   (PROJECT_NAME + "/vins/odometry/path", 1000);
	pub_odometry            = n.advertise<nav_msgs::Odometry>               (PROJECT_NAME + "/vins/odometry/odometry", 1000);
	pub_point_cloud         = n.advertise<sensor_msgs::PointCloud>          (PROJECT_NAME + "/vins/odometry/point_cloud", 1000);
	pub_margin_cloud        = n.advertise<sensor_msgs::PointCloud>          (PROJECT_NAME + "/vins/odometry/history_cloud", 1000);
	pub_key_poses           = n.advertise<visualization_msgs::Marker>       (PROJECT_NAME + "/vins/odometry/key_poses", 1000);
	pub_camera_pose         = n.advertise<nav_msgs::Odometry>               (PROJECT_NAME + "/vins/odometry/camera_pose", 1000);
	pub_camera_pose_visual  = n.advertise<visualization_msgs::MarkerArray>  (PROJECT_NAME + "/vins/odometry/camera_pose_visual", 1000);
	pub_keyframe_pose       = n.advertise<nav_msgs::Odometry>               (PROJECT_NAME + "/vins/odometry/keyframe_pose", 1000);
	pub_keyframe_point      = n.advertise<sensor_msgs::PointCloud>          (PROJECT_NAME + "/vins/odometry/keyframe_point", 1000);
	pub_extrinsic           = n.advertise<nav_msgs::Odometry>               (PROJECT_NAME + "/vins/odometry/extrinsic", 1000);

输入：

通过之前的几个节点的学习，不难猜出这个几个输入分别是：（也验证了的）

sub_imu Imu预积分节点得到的里程计信息、sub_odom原始的imu，odomtrey信息、sub_image、sub_restart都是前边的/lvi_sam_visual_feature输出的图像信息和重启信号。

输出：

这里的输出相当多，但是从节点图中不难看出，输出中真正有效的是”/vins/odometry/imu_propagate_ros”, /vins/odometry/extrinsic,/vins/odometry/keyframe_point, /vins/odometry/keyframe_pose, /vins/odometry/point_cloud

也就是点云信息、关键帧位姿信息和里程计信息。

8、再次回到/lvi_sam_mapOptmization（图优化节点）

看完了整个节点的信息流，我们会发现终点前的最终节点是雷达系统中的图优化节点。

他的输入很简单，就是从视觉系统、雷达系统获得的点云数据以及视觉系统中得到闭环信息，以及GPS信息。

但输出却十分多，我们下面一个一个来分析。

	ros::Publisher pubLaserCloudSurround;
    ros::Publisher pubOdomAftMappedROS;
    ros::Publisher pubKeyPoses;
    ros::Publisher pubPath;
	ros::Publisher pubHistoryKeyFrames;
	ros::Publisher pubIcpKeyFrames;
	ros::Publisher pubRecentKeyFrames;
	ros::Publisher pubRecentKeyFrame;
	ros::Publisher pubCloudRegisteredRaw;
	ros::Publisher pubLoopConstraintEdge;

在分析之前先要确定好几个概念：

- 关键帧：key frame
- 位姿：pose
- 激光和雷达：laser

pubKeyPoses：发布关键位姿信息

pubLaserCloudSurround：发布关键帧的map的特征点云

pubOdomAftMappedROS：发布激光里程计

pubPath：发布路径，主要是给rivz用于展示

pubHistoryKeyFrames：发布历史关键帧

pubIcpKeyFrames：// 发布当前关键帧经过闭环优化后的位姿变换之后的特征点云

pubLoopConstraintEdge：发布闭环边信息

pubRecentKeyFrames：发布局部map的降采样平面边重合

pubRecentKeyFrame：// 发布历史帧（累加的）的角点、平面点降采样集合

pubCloudRegisteredRaw：校准之后的点云信息

不过后边的发布的功能熟悉了概念之后也还是看不懂，这需要我们日后对于该包视觉系统部分的进一步学习。

*注意：

经过上面的分析，我们在后续的阅读代码工作中最好遵循一定的顺序：

图优化节点可以放在最后；
闭环检测节点可以放在倒数第二个；
可以先看雷达部分，再看视觉部分的代码；

四、参考资料：

LVI-SAM代码注释 – xuwuzhou的SLAM之路 – 静心，慎思，明辨，笃学

ROS 学习笔记｜1 ROS 介绍 - 知乎 (zhihu.com)

LVI-SAM源码
ROS学习之tf - 知乎 (zhihu.com)
[racecar仿真竞赛经验总结（五）- TF树 - Zacdeng Blog](https://zacdeng.github.io/2020/08/04/racecar仿真竞赛经验总结（五）- TF树/)
ROS 计算图级,理解ROS 节点、话题 - 简书 (jianshu.com)
ROS Twist和Odometry消息类型使用（Python） - 徐尚 - 博客园 (cnblogs.com)
回环检测详解 - 知乎 (zhihu.com)
ROS-Marker和MarkerArray使用方法 - 代码先锋网 (codeleading.com)
[ORBSLAM2–RGB-D模块(5):谈谈系统中的各种Pose - 知乎 (zhihu.com)](https://zhuanlan.zhihu.com/p/51848304#:~:text=Pose是相机的位姿信息，是SLAM系统中非常重要的输出数据，因此本文将帮助大家梳理系统中的各种Pose数据。在Frame class中，Pose信息放在mTcw 中，调用setPose,()%2C就可以更新旋转和平移矩阵。 Tcw是世界坐标系转向相机坐标系的矩阵，即from world to camera。)
LVI_SAM源码详解（4_1） - 知乎 (zhihu.com)

原文链接：https://blog.csdn.net/m0_49088083/article/details/120892224