由粗到精学习 LVI-SAM： imageProjection模块

3D视觉工坊

发布于 2022-03-11 13:19:39

7520

发布于 2022-03-11 13:19:39

文章被收录于专栏：3D视觉从入门到精通

一、LVI-SAM的节点关系

运行LVI-SAM，并打开rqt_graph，查看节点关系如图所示：

激光雷达原始数据的topic为/points_raw，观察发现它仅被imageProjection节点订阅，因此我们分析这个节点。

imageProjection节点订阅三个节点：points_raw节点、imu_raw节点、和imu_propagate_ros节点，并被cloud_info节点订阅。（rviz可视化部分暂不考虑）

可以猜想出，imageProjection节点的主要功能是将点云投影变成类似于图片的格式，进行一些预处理方便后续的slam过程。

二、主函数

int main(int argc, char** argv)
{
    ros::init(argc, argv, "lidar");
    //// 本node的主要工作都体现在ImageProjection的构造函数当中
    ImageProjection IP;
    
    ROS_INFO("\033[1;32m----> Lidar Cloud Deskew Started.\033[0m");
    //// 创建三个相同的线程，在A线程忙碌的时候启用B线程，A,B线程都忙碌的时候启用C线程
    ros::MultiThreadedSpinner spinner(3);
    spinner.spin();
    
    return 0;
}

三、ImageProjection类

3.1 父类ParamServer类

ParamServer是一个重要的类，它的子类包括：

FeatureExtraction

IMUPreintegration

ImageProjection

mapOptimization

它的成员包括：

上述ros参数均可以根据名称判断其含义，且与param_lidar.yaml参数文件紧密关联。

3.2 ImageProjection类的成员

3.3 ImageProjection类的构造函数

在主函数当中，主要依靠的就是ImageProjection类的构造函数，因此重点分析它，结合以上ImageProjection类成员变量和成员函数的解读可以很容易看懂其过程。

ImageProjection():
    deskewFlag(0)       // 去畸变标志位置零
    {
        // 订阅imu原始数据，回调函数负责将测量信息坐标变换到激光雷达并存储到队列
        subImu        = nh.subscribe<sensor_msgs::Imu>        (imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
        // 订阅Odom原始数据，此Odom来自于VIS，可能早于点云采集时间，也可能稍晚于点云采集时间，回调函数负责将位姿信息存放到队列
        subOdom       = nh.subscribe<nav_msgs::Odometry>      (PROJECT_NAME + "/vins/odometry/imu_propagate_ros", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
        // 订阅原始点云，回调函数负责检查、获取对齐的imu帧旋转矩阵、获取相邻帧的位姿变换、点云投影成图片从而使点云有序化，最后把去畸变有序化点云和自定义格式点云发布出去
        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);

        // 为下一次处理分配内存空间并重置所有参数
        allocateMemory();
        resetParameters();

        // 设置控制台输出信息
        pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
    }

本文仅做学术分享，如有侵权，请联系删文。

本文参与腾讯云自媒体同步曝光计划，分享自微信公众号。

原始发表：2022-02-09，如有侵权请联系 cloudcommunity@tencent.com 删除

cloud