A-LOAM的安装和源码解析（1）

水理璇浮

已于 2024-01-02 17:26:57 修改

阅读量1.5k

点赞数 17

文章标签：学习 linux ubuntu

于 2023-12-27 15:16:01 首次发布

本文链接：https://blog.csdn.net/weixin_44760904/article/details/135228095

版权

A-LOAM（Advanced LiDAR Odometry and Mapping）是一个用于激光雷达（LiDAR）数据的实时里程计和地图构建系统。它基于LOAM（LiDAR Odometry and Mapping），这是一个广泛使用的、高效的激光雷达SLAM（Simultaneous Localization and Mapping）系统。A-LOAM 对原始的LOAM进行了改进，提高了其鲁棒性和精度。

一、首先就是先跑起来看看

其源码地址：

GitHub - HKUST-Aerial-Robotics/A-LOAM: Advanced implementation of LOAMAdvanced implementation of LOAM. Contribute to HKUST-Aerial-Robotics/A-LOAM development by creating an account on GitHub.https://github.com/HKUST-Aerial-Robotics/A-LOAM 本机版本：ubuntu18.04 melodic pcl1.8.1

编译：

    cd ~/catkin_ws/src
    git clone https://github.com/HKUST-Aerial-Robotics/A-LOAM.git
    cd ../
    catkin_make
    source ~/catkin_ws/devel/setup.bash

运行：

终端1:

roslaunch aloam_velodyne aloam_velodyne_VLP_16.launch

终端2:

    rosbag play YOUR_DATASET_FOLDER/nsh_indoor_outdoor.bag

包下载地址：https://drive.google.com/file/d/1s05tBQOLNEDDurlg48KiUWxCp-YqYyGH/view

效果：

rqt_graph

二、论文算法原理参考博客：

大佬总结的很好

https://ppipp.blog.csdn.net/article/details/125039343?spm=1001.2014.3001.5502&ydreferer=aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDUvYXJ0aWNsZS9kZXRhaWxzLzEyNTAzOTM5Nz9vcHNfcmVxdWVzdF9taXNjPSUyNTdCJTI1MjJyZXF1ZXN0JTI1NUZpZCUyNTIyJTI1M0ElMjUyMjE3MDM1NzYzNTUxNjgwMDE4ODUyMDU3MiUyNTIyJTI1MkMlMjUyMnNjbSUyNTIyJTI1M0ElMjUyMjIwMTQwNzEzLjEzMDEwMjMzNC4uJTI1MjIlMjU3RCZyZXF1ZXN0X2lkPTE3MDM1NzYzNTUxNjgwMDE4ODUyMDU3MiZiaXpfaWQ9MCZ1dG1fbWVkaXVtPWRpc3RyaWJ1dGUucGNfc2VhcmNoX3Jlc3VsdC5ub25lLXRhc2stYmxvZy0yfmFsbH5zb2JhaWR1ZW5kfmRlZmF1bHQtMi0xMjUwMzkzOTctbnVsbC1udWxsLjE0Ml52OThecGNfc2VhcmNoX3Jlc3VsdF9iYXNlNiZ1dG1fdGVybT1hbG9hbSVFNiVCQSU5MCVFNyVBMCU4MSVFOCVBNyVBMyVFNiU5RSU5MCZzcG09MTAxOC4yMjI2LjMwMDEuNDE4Nw%3D%3Dhttps://ppipp.blog.csdn.net/article/details/125039343?spm=1001.2014.3001.5502&ydreferer=aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3A5NDIwMDU0MDUvYXJ0aWNsZS9kZXRhaWxzLzEyNTAzOTM5Nz9vcHNfcmVxdWVzdF9taXNjPSUyNTdCJTI1MjJyZXF1ZXN0JTI1NUZpZCUyNTIyJTI1M0ElMjUyMjE3MDM1NzYzNTUxNjgwMDE4ODUyMDU3MiUyNTIyJTI1MkMlMjUyMnNjbSUyNTIyJTI1M0ElMjUyMjIwMTQwNzEzLjEzMDEwMjMzNC4uJTI1MjIlMjU3RCZyZXF1ZXN0X2lkPTE3MDM1NzYzNTUxNjgwMDE4ODUyMDU3MiZiaXpfaWQ9MCZ1dG1fbWVkaXVtPWRpc3RyaWJ1dGUucGNfc2VhcmNoX3Jlc3VsdC5ub25lLXRhc2stYmxvZy0yfmFsbH5zb2JhaWR1ZW5kfmRlZmF1bHQtMi0xMjUwMzkzOTctbnVsbC1udWxsLjE0Ml52OThecGNfc2VhcmNoX3Jlc3VsdF9iYXNlNiZ1dG1fdGVybT1hbG9hbSVFNiVCQSU5MCVFNyVBMCU4MSVFOCVBNyVBMyVFNiU5RSU5MCZzcG09MTAxOC4yMjI2LjMwMDEuNDE4Nw%3D%3D

三、对源码进行理解

3.1首先是读取数据和提取特征点主要内容在scanRegistration.cpp里

3.1.1 主函数

int main(int argc, char **argv)
{
    // 初始化 ROS 节点
    ros::init(argc, argv, "scanRegistration");
    // 创建 ROS 节点句柄
    ros::NodeHandle nh;

    // 从 ROS 参数服务器获取扫描线数量，如果未设置则默认为 16
    nh.param<int>("scan_line", N_SCANS, 16);
    // 获取最小测量范围，如果未设置则默认为 0.1
    nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);
    // 打印扫描线数量
    printf("scan line number %d \n", N_SCANS);

    // 确保扫描线数量是受支持的（16、32 或 64），如果不是，则退出程序
    if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)
    {
        printf("only support velodyne with 16, 32 or 64 scan line!");
        return 0;
    }

    // 订阅激光雷达原始点云数据
    ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);

    // 创建一系列发布者，用于发布处理后的点云数据
    pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);
    pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
    pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);
    pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
    pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);
    pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);

    // 如果设置了 PUB_EACH_LINE 标志，则为每个扫描线创建一个单独的发布者
    if(PUB_EACH_LINE)
    {
        for(int i = 0; i < N_SCANS; i++)
        {
            ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" + std::to_string(i), 100);
            pubEachScan.push_back(tmp);
        }
    }

    // 进入 ROS 消息处理循环，直到节点关闭
    ros::spin();

    return 0;
}

首先是判断一下线数，然后接收/velodyne_points话题，每接收到一个，就调用一次回调函数laserCloudHandler，核心是这行：

ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);

这行代码的作用是创建一个 ROS 订阅者，它订阅名为 "/velodyne_points" 的话题，接收 sensor_msgs::PointCloud2 类型的消息，并使用 laserCloudHandler 函数来处理这些消息。主要的处理都在 laserCloudHandler 函数里实现。后续创建了一系列发布者，发布处理后的点云数据，如锐化的角点、较不锐化的角点、平坦的表面点等,供给我们随时publish。

3.1.2 `laserCloudHandler` 函数


    // 判断系统是否初始化完成
    if (!systemInited)
    { 
        systemInitCount++;
        // 如果系统初始化计数达到预设的延迟值，则标记系统为初始化完成
        if (systemInitCount >= systemDelay)
        {
            systemInited = true;
        }
        else
            return;  // 如果系统还未初始化完成，则返回，不处理这个点云
    }

    // 创建两个时间记录对象，用于测量处理时间
    TicToc t_whole;
    TicToc t_prepare;

    // 初始化两个向量，用于记录每个扫描线的起始和结束索引
    std::vector<int> scanStartInd(N_SCANS, 0);
    std::vector<int> scanEndInd(N_SCANS, 0);

    // 创建一个 PCL 点云对象，用于存储转换后的点云数据
    pcl::PointCloud<pcl::PointXYZ> laserCloudIn;

    // 将 ROS 消息中的点云数据转换为 PCL 点云格式
    pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);

    // 创建一个整数向量，用于存储去除 NaN 点后的索引
    std::vector<int> indices;

    // 移除点云中的 NaN 点
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);

    // 移除距离过近的点
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);

开始初始化，systemInitCount初始值是0，如果我们自己实时跑可以通过调整systemDelay来丢弃前面几帧，然后就是把ros格式的点云数据通过PCL库转化为pcl点云格式，去除NAN点，然后通过removeClosedPointCloud函数距离过近的点。

`3.1.3 removeClosedPointCloud函数`

template <typename PointT>
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
                              pcl::PointCloud<PointT> &cloud_out, float thres)
{
    // 检查输入和输出点云是否为不同的对象
    if (&cloud_in != &cloud_out)
    {
        cloud_out.header = cloud_in.header;  // 复制点云的头信息
        cloud_out.points.resize(cloud_in.points.size());  // 重设输出点云的大小
    }

    size_t j = 0;

    // 遍历输入点云中的每个点
    for (size_t i = 0; i < cloud_in.points.size(); ++i)
    {
        // 计算点到原点的距离的平方
        if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
            continue;  // 如果距离小于阈值，忽略该点

        cloud_out.points[j] = cloud_in.points[i];  // 将有效的点复制到输出点云
        j++;
    }
    // 调整输出点云的大小，如果有些点被移除了
    if (j != cloud_in.points.size())
    {
        cloud_out.points.resize(j);
    }

    // 设置输出点云的其他属性
    cloud_out.height = 1;  // 高度为1，表示这是一个无序点云
    cloud_out.width = static_cast<uint32_t>(j);  // 宽度等于有效点的数量
    cloud_out.is_dense = true;  // 没有NaN点，是一个密集的点云
}

removeClosedPointCloud 函数是一个模板函数，用于移除点云中距离传感器过近的点。它接受三个参数：输入点云 cloud_in，输出点云 cloud_out，以及距离阈值 thres。该函数的主要功能是遍历输入点云中的每个点，并根据点的位置（距离原点的距离）决定是否将其保留在输出点云中。距离根据自己数据集情况设置，KITTI数据集64线设置的是5米。

3.1.4计算点云角度范围分配时间戳

// 获取点云中点的总数
int cloudSize = laserCloudIn.points.size();
// 计算起始和结束方向的角度
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;

// 调整结束角度，以确保它在合理范围内
if (endOri - startOri > 3 * M_PI)
{
    endOri -= 2 * M_PI;
}
else if (endOri - startOri < M_PI)
{
    endOri += 2 * M_PI;
}

bool halfPassed = false; // 标志位，用于判断是否处理了一半的点云
int count = cloudSize;   // 有效点计数
PointType point;         // 当前处理的点
std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS); // 存储每条扫描线的点云

// 遍历点云中的每个点
for (int i = 0; i < cloudSize; i++)
{
    point.x = laserCloudIn.points[i].x;
    point.y = laserCloudIn.points[i].y;
    point.z = laserCloudIn.points[i].z;

    // 计算点的垂直角度
    float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
    int scanID = 0;

    // ... [此部分代码与之前的相同，计算每个点的scanID] ...

    // 计算点的方位角
    float ori = -atan2(point.y, point.x);
    // 判断并调整点的方位角，以保持连续性
    if (!halfPassed)
    { 
        if (ori < startOri - M_PI / 2)
        {
            ori += 2 * M_PI;
        }
        else if (ori > startOri + M_PI * 3 / 2)
        {
            ori -= 2 * M_PI;
        }

        if (ori - startOri > M_PI)
        {
            halfPassed = true;
        }
    }
    else
    {
        ori += 2 * M_PI;
        if (ori < endOri - M_PI * 3 / 2)
        {
            ori += 2 * M_PI;
        }
        else if (ori > endOri + M_PI / 2)
        {
            ori -= 2 * M_PI;
        }
    }

    // 计算相对时间
    float relTime = (ori - startOri) / (endOri - startOri);
    // 设置点的强度为扫描线ID加上相对时间
    point.intensity = scanID + scanPeriod * relTime;
    // 将点添加到对应扫描线的点云中
    laserCloudScans[scanID].push_back(point); 
}

// 更新有效点的数量
cloudSize = count;
printf("points size %d \n", cloudSize);

atan2(a,b)是4象限反正切，它的取值不仅取决于a/b的atan值，还取决于点 (b, a) 落入哪个象限
当点(b, a) 落入第一象限时，atan2(a,b)的范围是 0 ~ pi/2;
当点(b, a) 落入第二象限时，atan2(a,b)的范围是 pi/2 ~ pi;
当点(b, a) 落入第三象限时，atan2(a,b)的范围是 -pi/2～０;
当点(b, a) 落入第四象限时，atan2(a,b)的范围是－pi～－pi/2

atan2( )默认返回逆时针角度，由于LiDAR通常是顺时针扫描，所以往往使用-atan2( )函数。

确定每帧点云和结束的角度，通过俯仰角确定线数，然后遍历每个点计算当前点的角度，通过float relTime = (ori - startOri) / (endOri - startOri)计算当前点时间戳，记录到point.intensity中，point.intensity = scanID + scanPeriod * relTime;整数代表线数，小数代表到开始时刻的时间差。scanPeriod 是 0.1，代表10hz。

3.1.5计算点云点曲率

// 创建一个新的点云来存储所有扫描的数据
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());

// 遍历每一个扫描
for (int i = 0; i < N_SCANS; i++)
{ 
    // 为当前扫描设置开始索引，考虑到边界
    scanStartInd[i] = laserCloud->size() + 5;
    // 将当前扫描的点云数据加入到总的点云中
    *laserCloud += laserCloudScans[i];
    // 设置结束索引，考虑到边界
    scanEndInd[i] = laserCloud->size() - 6;
}

// 输出准备阶段耗费的时间
printf("prepare time %f \n", t_prepare.toc());

// 遍历点云中的每个点，计算曲率
for (int i = 5; i < cloudSize - 5; i++)
{ 
    // 计算每个点相对于其邻居点的曲率（使用差分方法）
    // 这里用到了一种特殊的加权差分方法来增强中心点的影响
    float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + ... - 10 * laserCloud->points[i].x + ... + laserCloud->points[i + 5].x;
    float diffY = /* 同上，对Y坐标 */;
    float diffZ = /* 同上，对Z坐标 */;

    // 计算曲率并存储
    cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
    // 初始化相关数组
    cloudSortInd[i] = i;
    cloudNeighborPicked[i] = 0;
    cloudLabel[i] = 0;
}

// 开始提取特征点
TicToc t_pts;

// 创建存储各种特征点的点云
pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;

// 用于记录排序时间
float t_q_sort = 0;

// 遍历每个扫描
for (int i = 0; i < N_SCANS; i++)
{
    // 如果扫描的点太少，就跳过这个扫描
    if (scanEndInd[i] - scanStartInd[i] < 6)
        continue;

    // 为较平的表面点创建一个新的点云
    pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);

    // 将每个扫描分成6部分进行处理
    for (int j = 0; j < 6; j++)
    {
        // 确定每部分的起始和结束索引
        int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; 
        int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;

        // 对当前部分的点按曲率进行排序
        TicToc t_tmp;
        std::sort(cloudSortInd + sp, cloudSortInd + ep + 1, comp);
        t_q_sort += t_tmp.toc();

这里把每条线上的点都加到一起了，然后前5个后5个不要，然后开始计算曲率，根据点云点的曲率提取特征点，把特别尖锐的边线点与特别平坦的平面点作为特征点。

当计算某个特定点的曲率时，该算法选择该点在同一扫描线上的左右两侧各5个邻近点。这些邻近点的坐标与目标点的坐标进行比较，以此来计算目标点的曲率。在这种情况下，如果目标点位于一个棱角或尖角位置，它与周围点的坐标差异会比较大，导致计算出的曲率值较高。相反，如果目标点位于一个相对平坦的表面上，那么它与周围点的坐标差异较小，从而产生的曲率值也会相对较低。这里中间有一部分不属于同一根线上的点也给计算了，不过不用担心，后续后处理。


            int largestPickedNum = 0;
            for (int k = ep; k >= sp; k--)
            {
                int ind = cloudSortInd[k]; 
//角点
                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[in d] > 0.1)
                {

                    largestPickedNum++;
                    if (largestPickedNum <= 2)
                    {                        
                        cloudLabel[ind] = 2;
                        cornerPointsSharp.push_back(laserCloud->points[ind]);
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else if (largestPickedNum <= 20)
                    {                        
                        cloudLabel[ind] = 1; 
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else
                    {
                        break;
                    }

                    cloudNeighborPicked[ind] = 1; 

                    for (int l = 1; l <= 5; l++)
                    {
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--)
                    {
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }
//面点
            int smallestPickedNum = 0;
            for (int k = sp; k <= ep; k++)
            {
                int ind = cloudSortInd[k];

                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] < 0.1)
                {

                    cloudLabel[ind] = -1; 
                    surfPointsFlat.push_back(laserCloud->points[ind]);

                    smallestPickedNum++;
                    if (smallestPickedNum >= 4)
                    { 
                        break;
                    }

                    cloudNeighborPicked[ind] = 1;
                    for (int l = 1; l <= 5; l++)
                    { 
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                    for (int l = -1; l >= -5; l--)
                    {
                        float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                        float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                        float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                        if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                        {
                            break;
                        }

                        cloudNeighborPicked[ind + l] = 1;
                    }
                }
            }

            for (int k = sp; k <= ep; k++)
            {
                if (cloudLabel[k] <= 0)
                {
                    surfPointsLessFlatScan->push_back(laserCloud->points[k]);
                }
            }
        }
       //体素滤波
        pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
        pcl::VoxelGrid<PointType> downSizeFilter;
        downSizeFilter.setInputCloud(surfPointsLessFlatScan);
        downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
        downSizeFilter.filter(surfPointsLessFlatScanDS);

        surfPointsLessFlat += surfPointsLessFlatScanDS;
    }
    printf("sort q time %f \n", t_q_sort);
    printf("seperate points time %f \n", t_pts.toc());

这里提取了各种特征点，并且对次极小平面点进行了一次降采样，接下来就是对当前点云、特征点云发布，供给laserodometry 通信，将预处理的数据传给里程计节点，然后是否按线发布，这里源码里一直是FALSE，没有用到，最后判定了一下时间，观察能否满足10HZ的条件。

  sensor_msgs::PointCloud2 laserCloudOutMsg;
    pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
    laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
    laserCloudOutMsg.header.frame_id = "/camera_init";
    pubLaserCloud.publish(laserCloudOutMsg);

    sensor_msgs::PointCloud2 cornerPointsSharpMsg;
    pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
    cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsSharpMsg.header.frame_id = "/camera_init";
    pubCornerPointsSharp.publish(cornerPointsSharpMsg);

    sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
    pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
    cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
    pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);

    sensor_msgs::PointCloud2 surfPointsFlat2;
    pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
    surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsFlat2.header.frame_id = "/camera_init";
    pubSurfPointsFlat.publish(surfPointsFlat2);

    sensor_msgs::PointCloud2 surfPointsLessFlat2;
    pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
    surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsLessFlat2.header.frame_id = "/camera_init";
    pubSurfPointsLessFlat.publish(surfPointsLessFlat2);

    // pub each scam
    if(PUB_EACH_LINE)
    {
        for(int i = 0; i< N_SCANS; i++)
        {
            sensor_msgs::PointCloud2 scanMsg;
            pcl::toROSMsg(laserCloudScans[i], scanMsg);
            scanMsg.header.stamp = laserCloudMsg->header.stamp;
            scanMsg.header.frame_id = "/camera_init";
            pubEachScan[i].publish(scanMsg);
        }
    }

    printf("scan registration time %f ms *************\n", t_whole.toc());
    if(t_whole.toc() > 100)
        ROS_WARN("scan registration process over 100ms");
}

后续链接A-LOAM的源码解析（2）-CSDN博客