自动驾驶系列（3）ALOAM源码解析-scanRegistration.cpp

ALOM节点图

1.特征点提取原理

根据曲率来提取两种特征点，分别为角点与面点

2.实现

2.1主函数

功能：初始化ROS节点，订阅激光点云数据，发布处理后的点云数据。代码的主要功能如下：

1. 初始化ROS节点，设置节点名称和参数。
2. 设置扫描线数量（N_SCANS），最小距离（MINIMUM_RANGE）。
3. 打印扫描线数量和最小距离。
4. 检查扫描线数量是否支持16、32或64条扫描线，否则输出提示信息并退出程序。
5. 订阅激光点云数据（/points_raw），并设置回调函数laserCloudHandler。
6. 发布处理后的点云数据（/velodyne_cloud_2），（/laser_cloud_sharp），（/laser_cloud_less_sharp），（/laser_cloud_flat），（/laser_cloud_less_flat），（/laser_remove_points）。
7. 如果PUB_EACH_LINE设置为true，则为每个扫描线发布单独的点云数据。
8. 运行ROS事件循环。

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

    // 获取参数：扫描线数量
    nh.param<int>("scan_line", N_SCANS, 16);

    // 获取参数：最小距离
    nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);

    // 打印参数：扫描线数量
    printf("scan line number %d \n", N_SCANS);

    // 判断扫描线数量是否支持
    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>("/points_raw", 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);

    // 判断是否发布每个扫描线
    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::spin();

    return 0;
}

2.2 laserCloudHandler 函数

订阅LiDAR点云数据，并对其进行处理，最后将处理后的点云数据发布出去。处理过程包括去噪、过滤、投影、提取角点、分割平面等。

1.定义了一些变量和结构体，如点云类型、点云尺寸等。

2.定义了一个名为laserCloudHandler的回调函数，该函数会在接收到LiDAR点云数据时被调用。

3.在laserCloudHandler函数中，首先检查系统是否初始化，如果没有，则计数器自增，直到达到系统延迟时间。然后，计算预处理时间。

4.对LiDAR点云数据进行预处理，包括去噪、过滤等。预处理后的点云数据存储在laserCloudIn变量中。

5.计算处理时间。

6.遍历预处理后的点云数据，提取角点、分割平面等。

7.将处理后的点云数据发布出去。

2.3 removeClosedPointCloud函数

接收两个pcl::PointCloud类型的参数：cloud_in（输入点云）和cloud_out（输出点云）。函数的目的是从输入点云中移除那些距离原点（坐标系原点）小于特定阈值的点。

原理:是遍历输入点云中的每个点，检查其三维空间坐标（x、y和z）是否满足条件。如果满足条件，将该点复制到输出点云中，否则跳过该点。最后，调整输出点云的大小以匹配有效点的数量。

注意：

输入和输出点云必须是指向同一内存空间的指针，否则会导致未定义行为。
函数使用三维空间中的点距离公式（平方和）来检查点是否满足条件。
函数使用size_t类型来表示点cloud_in中的索引和cloud_out中的有效点数。

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;
    cloud_out.width = static_cast<uint32_t>(j);
    cloud_out.is_dense = true;
}