经典激光SLAM框架LeGO-LOAM中依据曲率分类的三维点云直观可视化

1.1 概述

最近在看论文，找研究方向，受SuMa++[1]以及其他几篇文章的启发，想研究一下机械式激光雷达按照线束划分的，基于曲率分类的点云管理手段，看能不能根据点云特征值和特征向量做一些工作。之前在研究开源算法LeGO-LOAM[2]的时候曾经想过，原始代码中虽然将每一帧的角点平面点显示出来了，但是可视化效果不太好，想在原始代码的基础上加一点接口，可视化的显示整个算法工作过程中机械式激光雷达点云划分为各种点的过程。

1.2 整体探索及实现过程

主要添加的代码基于LeGO-LOAM的mapOptmization.cpp，前面点云预处理和特征提取部分的代码已经有很多大神剖析的十分清楚，不再作赘述，仅对整个全局地图的生成、维护和可视化过程作出一些基于个人见解的简短说明。

    void laserCloudOutlierLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
        timeLaserCloudOutlierLast = msg->header.stamp.toSec();
        laserCloudOutlierLast->clear();
        pcl::fromROSMsg(*msg, *laserCloudOutlierLast);
        newLaserCloudOutlierLast = true;
    }

    void laserCloudCornerLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
        timeLaserCloudCornerLast = msg->header.stamp.toSec();
        laserCloudCornerLast->clear();
        pcl::fromROSMsg(*msg, *laserCloudCornerLast);
        newLaserCloudCornerLast = true;
    }

    void laserCloudSurfLastHandler(const sensor_msgs::PointCloud2ConstPtr& msg){
        timeLaserCloudSurfLast = msg->header.stamp.toSec();
        laserCloudSurfLast->clear();
        pcl::fromROSMsg(*msg, *laserCloudSurfLast);
        newLaserCloudSurfLast = true;
    }

离群点、角点和平面点在featureAssociation.cpp中对原始激光点云分类后发出，在mapping阶段由上述三个回调函数接受并存入相应话题中，比较常规的rostopic转化为pcl点云信息。三个回调函数最后的bool量涉及到初始化，体现在run函数中。

if (newLaserCloudCornerLast  && std::abs(timeLaserCloudCornerLast  - timeLaserOdometry) < 0.005 &&
    newLaserCloudSurfLast    && std::abs(timeLaserCloudSurfLast    - timeLaserOdometry) < 0.005 &&
    newLaserCloudOutlierLast && std::abs(timeLaserCloudOutlierLast - timeLaserOdometry) < 0.005 &&
    newLaserOdometry)

只有过了这个if判断才会进行剩余的程序，保证已经接收到一帧的点云以及里程计信息。其他几个地方也有类似的判断，都是保证这几个指针里面都存有点云信息，然后才进行进一步的处理。

其实看到这里的时候笔者的心里感到一丝不妙，写这篇文章的初衷是笔者对点云可视化的需求，以往有几次调试LeGO-LOAM的时候，发现作者TiXiao Shan已经将几种点云和Rviz的显示做的十分完善，在maping过程中可以看到，原始激光点云已经被分类为角点平面点等，并在Rviz中实现了可视化，虽然显示的只是每一帧点云的分类结果，笔者的需求是将整个地图建立过程中的角点平面点等可视化并保存下来，观察TiXiao Shan发布的点云话题并寻找对应回调函数后，似乎将

laserCloudOutlierLast->clear();

点云回调函数中的这一段删除，可以将之前mapping过程中得到的角点和平面点保存下来，但是LaserCloudCornerLast等中的点云参与了cornerOptimization等高斯牛顿法的优化，直接将这一行代码去掉势必会影响scan_to_map的优化，那目前的思路就十分明确了：

（1）建立几个新的pcl指针，用来存放笔者想看的点云，分为两种类型，当前坐标系下和世界坐标系下

（2）利用已经有的点云回调函数，在源程序运行的基础上顺便把点云也存到上述当前坐标系下pcl指针中

（3）模仿或直接利用scan_to_map的算法，将每一帧的目标点云转化到世界坐标系下面后保存并发布，实现可视化

在topic发布及订阅部分加入如下两个话题，用来可视化

pubCornerPointsTotal = nh.advertise("/corner_points_total", 2);
pubSurfPointsTotal = nh.advertise("/surf_points_total", 2);

在private中加入如下几个点云指针

pcl::PointCloud::Ptr global_corner_points;
pcl::PointCloud::Ptr global_surf_points;
pcl::PointCloud::Ptr global_corner_point_tmp;
pcl::PointCloud::Ptr global_surf_point_tmp;

在两个点云回调函数中加入如下内容

global_corner_point_tmp->clear();
pcl::fromROSMsg(*msg, *global_corner_point_tmp);

global_surf_point_tmp->clear();
pcl::fromROSMsg(*msg, *global_surf_point_tmp);

在初始化函数中加入以下内容

global_corner_points.reset(new pcl::PointCloud());
global_surf_points.reset(new pcl::PointCloud());
global_corner_point_tmp.reset(new pcl::PointCloud());
global_surf_point_tmp.reset(new pcl::PointCloud());

原始的角点和平面点处理程序都经过了下采样，笔者简单的思索了一下，笔者的目的主要是观察机器人/移动平台在行进过程中分类好的两种点云的变化规律，不参与高斯牛顿优化，只要将每一帧点云坐标变换到合适的世界坐标系下就可以，这个过程并不会消耗太多的计算资源，所以不下采样应该也没关系。其实就是懒的写。

现在已经获取到每一帧基于车辆坐标系下的两种点云了，关于如何将每一帧点云实行坐标变换，笔者采取了比较简单的方式，利用源程序中的scan2MapOptimization函数，函数中实现了下采样的角点、平面点向世界坐标系的优化与注册。

观察程序运行时输出全局点云地图的topic，LeGO-LOAM-NOTED中是这个publisher

pubRecentKeyFrames = nh.advertise("/recent_cloud", 2);

和笔者印象中有一点出入，笔者去查看了一下之前使用过的没有加过注释的LeGO-LOAM发现原作者TiXiao Shan发布的版本多了一个publisher，与笔者所使用的这个NOTED的版本中道理差不多，不做深入讨论。在publishKeyPosesAndFrames加入如下函数，实现笔者想要的点云的输出

sensor_msgs::PointCloud2 cloudMsgTemp1;
sensor_msgs::PointCloud2 cloudMsgTemp2;
pcl::toROSMsg(*global_corner_points, cloudMsgTemp1);
pcl::toROSMsg(*global_surf_points, cloudMsgTemp2);
cloudMsgTemp1.header.stamp = ros::Time().fromSec(timeLaserOdometry);
cloudMsgTemp1.header.frame_id = "/camera_init";
cloudMsgTemp2.header.stamp = ros::Time().fromSec(timeLaserOdometry);
cloudMsgTemp2.header.frame_id = "/camera_init";
pubCornerPointsTotal.publish(cloudMsgTemp1);
pubSurfPointsTotal.publish(cloudMsgTemp2);

到这里先告一段落，进行一下测试看看有没有什么问题，换到ubuntu下执行程序，catkin_make后启动run,launch。使用的数据是笔者自录的，北理工中关村校区某垃圾回收站，所使用的雷达是速腾32线Helios，惯导为星网M2。

到这里为止基本符合笔者的预期，由于点云输出的内容是车体坐标系下的，所以全部跟着车走了，而且下采样似乎是必须的。在downsampleCurrentScan中将两个点云下采样。

global_corner_point_tmpDS->clear();
downSizeFilterCorner.setInputCloud(global_corner_point_tmp);
downSizeFilterCorner.filter(*global_corner_point_tmpDS);

global_surf_point_tmpDS->clear();
downSizeFilterSurf.setInputCloud(global_surf_point_tmp);
downSizeFilterSurf.filter(*global_surf_point_tmpDS);

然后就是最麻烦的将车体坐标系下的点云转化到世界坐标系下了，原run函数中经历了以下几个重要的子函数

//将来自于里程计的transformSum与transformBefMapped坐标转化为世界坐标系下，存放在transformTobeMapped中
transformAssociateToMap();
//回环也好，找之前帧也好，在laserCloudCornerFromMap和laserCloudSurfFromMap中装入点云，作为scan2map的匹配目标，即map
extractSurroundingKeyFrames();
//下采样，减少计算量
downsampleCurrentScan();
//scan2map优化，包含角点的选取、平面点的选取和高斯牛顿法优化
scan2MapOptimization();

其实写到这里的时候笔者才猛然惊觉，源程序中分别将角点和平面点的keyframe发布为单帧点云，所以其实只要把这一部分单帧点云保存下来就可以一定程度上打成笔者的目的，但笔者转念一想，这些点云都是经过cornerOptimization与surfOptimization优化选取过的点云，与笔者一开始想看的经过特征提取后的原点云有一些出入，遂继续了后面的工作。

在cornerOptimization中加入如下函数，将global_corner_point_tmpDS中的点转移到世界坐标系下

int cloudSize1 = global_corner_point_tmpDS->points.size();
for (int i = 0; i < cloudSize1; ++i){
    pointOri = global_corner_point_tmpDS->points[i];
    pointAssociateToMap(&pointOri, &pointSel);
    global_corner_points->push_back(pointSel);
}

在surfOptimization中加入如下函数，将global_surf_point_tmpDS中的点转移到世界坐标系下

int cloudSize1 = global_surf_point_tmpDS->points.size();
for (int i = 0; i < cloudSize1; ++i){
    pointOri = global_surf_point_tmpDS->points[i];
    pointAssociateToMap(&pointOri, &pointSel);
    global_surf_points->push_back(pointSel);
}

1.3 结果与分析

转换到ubuntu系统下，编译运行程序，结果如下

使用LeGO-LOAM原作者写好的rviz文件，把其中两个话题替换为笔者自己的话题，其中粉色点为global_surf_points点集，绿色点为global_corner_points点集，说实话程序在运行时显示这样的结果笔者也很震惊，有大量的地面点被判断为平面点暂且不论，大量的绿色角点出现在明显是平面障碍物的表面上，令人十分费解。

上图为原始rviz设置与输出，粉色点所采用的话题是/laser_cloud_less_flat，绿色点所采用的是/laser_cloud_less_sharp，应该与笔者所处理的点云话题是一致的，观察可以发现，以上图车体坐标系左侧一小段弧上的雷达点云为例，这一小段点云的边缘点也被处理为了角点，车体上方的一大坨点云中，在某一线激光点的中断处的点基本都被处理为了角点，这就导致了大量角点产生在平面障碍物的表面上，但前述函数cornerOptimization中对这些点做出了有效的处理，上述被判断为角点的绿色点在函数中经过了筛选，只有点-线距离满足要求的点才被认为是最终筛选出的角点。最终储存在了laserCloudOri中，若读者有兴趣，可自行发布为话题并观察输出结果，笔者不再进行上述无用功。

特征点的提取和优化选取仍然是SLAM现存的一个问题，从以上分析过程中可以明显的观察到基于机械式激光雷达点云曲率筛选方式仍残存在着大量无效点可以筛除，尤其是基于角点的筛选和匹配，浪费了不少计算资源，可以进行进一步的完善和改进。

笔者初学激光SLAM，代码阅读与撰写能力十分有限，欢迎批评指正。

参考文献

[1]X. Chen, A. Milioto, E. Palazzolo, P. Giguère, J. Behley and C. Stachniss, “SuMa++: Efficient LiDAR-based Semantic SLAM,” 2019 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2019, pp. 4530-4537, doi: 10.1109/IROS40897.2019.8967704.

[2]T. Shan and B. Englot, “LeGO-LOAM: Lightweight and Ground-Optimized Lidar Odometry and Mapping on Variable Terrain,” 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), 2018, pp. 4758-4765, doi: 10.1109/IROS.2018.8594299.

Original: https://blog.csdn.net/qq_46480449/article/details/125963138
Author: bit_morimori
Title: 经典激光SLAM框架LeGO-LOAM中依据曲率分类的三维点云直观可视化