LeGO-LOAM 几个特有函数的分析

一、地面点分离算法

?1.1LeGO-LOAM

void groundRemoval(){
    size_t lowerInd, upperInd; // 定义两个索引变量，用于标记当前处理的点云中的点
    float diffX, diffY, diffZ, angle; // 定义差分坐标和角度变量

    // 遍历水平扫描的所有点
    for (size_t j = 0; j < Horizon_SCAN; ++j){
        // 遍历地面扫描的指定范围内的点，从每一列开始找
        for (size_t i = 0; i < groundScanInd; ++i){

            // 计算当前点和上方点的索引
            lowerInd = j + ( i )*Horizon_SCAN;
            upperInd = j + (i+1)*Horizon_SCAN;

            // 检查这两个点是否有效，如果无效则继续下一个循环
            if (fullCloud->points[lowerInd].intensity == -1 ||
                fullCloud->points[upperInd].intensity == -1){
                groundMat.at<int8_t>(i,j) = -1;
                continue;
            }
                
            // 计算相邻点之间的坐标差异
            diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
            diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
            diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;

            // 根据坐标差异计算两点之间的夹角，弧度值转换为角度制
            angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;

            // 判断这个角度是否在预设的阈值内，来判定是否为地面
            if (abs(angle - sensorMountAngle) <= 10){
                groundMat.at<int8_t>(i,j) = 1; // 标记为地面
                groundMat.at<int8_t>(i+1,j) = 1; // 标记上方的点也为地面
            }
        }
    }

    // 在地面点云提取和分割标记的第二个步骤中
    for (size_t i = 0; i < N_SCAN; ++i){
        for (size_t j = 0; j < Horizon_SCAN; ++j){
            // 如果点是地面或者是无效点，则在labelMat中标记为-1
            if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
                labelMat.at<int>(i,j) = -1;
            }
        }
    }

    // 如果有订阅者，则发布地面点云
    if (pubGroundCloud.getNumSubscribers() != 0){
        for (size_t i = 0; i <= groundScanInd; ++i){
            for (size_t j = 0; j < Horizon_SCAN; ++j){
                // 将被标记为地面的点添加到地面点云中
                if (groundMat.at<int8_t>(i,j) == 1)
                    groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
            }
        }
    }
}

1. 初始化变量：

   • lowerInd, upperInd: 用于在点云中索引点的变量。
   • diffX, diffY, diffZ, angle: 用于计算点与点之间的差异及其夹角。

2. 遍历点云：

   • 对于水平扫描（Horizon_SCAN）的每一个点，遍历到一定高度（groundScanInd）。

3. 计算相邻点差异：

   • 对于每一对相邻点（由 lowerInd 和 upperInd 索引），计算它们在 X, Y, Z 坐标上的差异。
   • 忽略无效点（强度值为 -1）。

4. 计算角度：

   • 使用X, Y, Z坐标差来计算两点之间的夹角（转换为度）。

5. 地面点判断：

   • 如果计算得出的角度与传感器安装角度的差在10度以内，则认为这些点属于地面。

6. 标记地面点：

   • 在 groundMat 中标记地面点。如果一个点是地面点，那么它上方的点也被标记为地面点。

7. 过滤和标记点云：

   • 对于整个扫描范围，如果一个点被标记为地面点或者是无效点，则在 labelMat 中将其标记为 -1。

8. 发布地面点云：

   • 如果有订阅者，遍历到 groundScanInd 的高度，将所有标记为地面的点添加到 groundCloud 点云中，然后发布。

1.2 改进的地面点分离算法

// 定义一个函数用于地面点的检测
void groundPointDectionnew()
{
    // 1.分离有效点和标记无效点，并复原labelMat和groundMat
    pcl::PointCloud<pcl::PointXYZI> deskew_point;
    // 遍历所有扫描线
    for(size_t i = 0; i < N_SCAN; i++)
    {
        // 遍历一条扫描线上的所有点
        for(size_t j = 0; j < Horizon_SCAN; j++)
        {
            // 检查点是否有效（即rangeMat中的值不是FLT_MAX）
            if(rangeMat.at<float>(i, j) != FLT_MAX)
            {
                // 创建一个点，并从fullCloud中复制数据
                pcl::PointXYZI cur;
                cur.x = fullCloud->points[j + i * Horizon_SCAN].x;
                cur.y = fullCloud->points[j + i * Horizon_SCAN].y;
                cur.z = fullCloud->points[j + i * Horizon_SCAN].z;
                cur.intensity = fullCloud->points[j + i * Horizon_SCAN].intensity;

                // 将点添加到deskew_point点云中
                deskew_point.push_back(cur);
            }
            else
            {
                // 如果点无效，将labelMat和groundMat对应位置设置为-1
                labelMat.at<int>(i, j) = -1;
                groundMat.at<int8_t>(i, j) = -1;
            }
        }
    }

    // 2.测试的算法，原封不动地写上
    // 创建一个分割器，用于从点云中分割地面
    pcl::SACSegmentation<pcl::PointXYZI> seg;
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);

    // 设置分割器的参数
    seg.setOptimizeCoefficients(true);  // 设置优化系数
    seg.setModelType(pcl::SACMODEL_PLANE);  // 设置模型类型为平面
    seg.setMethodType(pcl::SAC_RANSAC);  // 设置方法类型为RANSAC
    seg.setMaxIterations(1000);  // 设置最大迭代次数
    seg.setDistanceThreshold(0.2);  // 设置距离阈值

    // 对deskew_point中的点云进行地面分割
    seg.setInputCloud(deskew_point.makeShared());
    seg.segment(*inliers, *coefficients);

    // 从原始点云中提取地面点
    pcl::ExtractIndices<pcl::PointXYZI> extract;
    extract.setInputCloud(deskew_point.makeShared());
    extract.setIndices(inliers);
    extract.setNegative(false);
    pcl::PointCloud<pcl::PointXYZI> ground;
    extract.filter(ground);

    // 3.构建八叉树，用于高效的空间搜索
    pcl::KdTreeFLANN<pcl::PointXYZI> kdtree;
    kdtree.setInputCloud(fullCloud);

    // 4.从八叉树恢复点云
    for(int i = 0; i < ground.size(); i++)
    {
        pcl::PointXYZI searchpoint;
        // 设置搜索点
        searchpoint.x = ground.points[i].x;
        searchpoint.y = ground.points[i].y;
        searchpoint.z = ground.points[i].z;
        searchpoint.intensity = ground.points[i].intensity;

        std::vector<int> pointIdxNKNSearch(1);
        std::vector<float> pointNKNSquaredDistance(1);

        int nearest_point_index;
        float nearest_point_distance;

        // 使用K近邻搜索找到最近的点
        if (kdtree.nearestKSearch(searchpoint, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0) 
        {
            nearest_point_index = pointIdxNKNSearch[0];
            nearest_point_distance = pointNKNSquaredDistance[0];
            // 根据找到的最近邻点的索引和距离进行处理（此处代码注释掉了）
            // ROS_WARN("Nearest Neighbor: Index %d,dist %f",nearest_point_index,nearest_point_distance);
        }

        // 将最近邻点的强度设置为1.0，表示这是地面点
        fullCloud->points[nearest_point_index].intensity = 1.0;
    }

    // 构建地面点云和非地面点云
    pcl::PointCloud<pcl::PointXYZI> ground_cloud;
    for(size_t i = 0; i < N_SCAN; i++)
    {
        for (size_t j = 0; j < Horizon_SCAN; j++)
        {
            // 如果点的强度为1.0，表示它是地面点
            if(fullCloud->points[j + i * Horizon_SCAN].intensity == 1.0)
            {
                pcl::PointXYZI po;
                po.x = fullCloud->points[j + i * Horizon_SCAN].x;
                po.y = fullCloud->points[j + i * Horizon_SCAN].y;
                po.z = fullCloud->points[j + i * Horizon_SCAN].z;
                po.intensity = fullCloud->points[j + i * Horizon_SCAN].intensity;

                // 将地面点添加到ground_cloud中
                ground_cloud.push_back(po);
                // 更新groundMat和labelMat
                groundMat.at<int8_t>(i, j) = 1;
                labelMat.at<int>(i, j) = -1;
            }
            else if(fullCloud->points[j + i * Horizon_SCAN].intensity > 0)
            {
                pcl::PointXYZI po;
                po.x = fullCloud->points[j + i * Horizon_SCAN].x;
                po.y = fullCloud->points[j + i * Horizon_SCAN].y;
                po.z = fullCloud->points[j + i * Horizon_SCAN].z;
                po.intensity = fullCloud->points[j + i * Horizon_SCAN].intensity;

                // 将非地面点添加到notgroundClouds中
                notgroundClouds->push_back(po);
            }
        }
    }

    // 将最终的地面点云赋值给groundClouds
    *groundClouds = ground_cloud;
}

总体步骤：

1. 分离有效点和标记无效点

首先，该算法遍历整个点云（通常从激光雷达获取），这个点云被组织成一个二维矩阵（每一行代表一个扫描线，每一列代表一个扫描点）。算法检查每个点是否有效（即是否被检测到）。有效点被保存在一个新的点云结构中，无效点则在 labelMat 和 groundMat 矩阵中被标记为 -1。

2. 地面分割

接下来，算法使用随机样本一致性（RANSAC）方法来分割地面。RANSAC 是一个迭代方法，用于从一组包含异常值的观测数据中估计数学模型的参数。在这个场景中，它被用于识别和分离出地面点（即属于地面平面的点）。

• 设置分割器的参数：算法配置了一个分割器，指定了模型类型为平面，并使用 RANSAC 方法。设置了最大迭代次数和距离阈值，这些参数控制了算法的精确度和鲁棒性。

• 地面点提取：算法从之前筛选的有效点中提取出地面点。这是通过将点云传递给分割器并获取分类为地面的点的索引来完成的。

3. 构建八叉树

然后，算法使用八叉树（或Kd树）数据结构来优化空间搜索过程。八叉树是一种树状数据结构，用于将三维空间划分为更小的部分，从而加快搜索和查询操作。

4. 从八叉树恢复点云

在这一步中，算法遍历提取出的地面点。对于每个地面点，它使用八叉树来快速找到原始点云中最接近的点，并将这个点的强度设置为 1.0，表示这是一个地面点。

5. 构建地面和非地面点云

最后，算法再次遍历整个原始点云。根据点的强度值，它将点分为地面点和非地面点，并相应地更新 groundMat 和 labelMat 矩阵。地面点被添加到 ground_cloud 点云中，而其他点则添加到 notgroundClouds 点云中。

1.3 RANSAC 方法原理

RANSAC 是一种用于鲁棒估计数学模型参数的算法，特别适用于数据中含有大量异常值（outliers）的情况。其基本思想是重复地从数据集中随机选择最小数量的样本，并尝试拟合模型，然后验证这个模型是否适用于一大部分数据点。

在地面点提取的情境中，RANSAC 主要用于识别和分割出构成地面的点集。具体步骤如下：

1. 选择随机样本：从输入点云中随机选择最小数量的点，对于一个平面模型，这通常意味着选择三个点。

2. 拟合模型：使用这些点来定义一个平面模型。在三维空间中，平面可以由方程 Ax + By + Cz + D = 0 表示，其中 A、B、C 和 D 是平面的参数，可以通过这三个点计算得出。

3. 评估模型：对于输入点云中的每个点，计算其到拟合平面的垂直距离。如果这个距离小于预设的阈值（这个阈值是 seg.setDistanceThreshold(0.2); 中设定的），则认为这个点是一个内点（inlier），即可能属于地面。

4. 验证模型：计算所有内点的比例。如果内点的比例高于某个设定阀值（表示有足够多的点符合当前的平面模型），则认为这个模型是有效的。

5. 迭代：重复以上步骤直到达到预定的迭代次数（seg.setMaxIterations(1000);）。每次迭代都可能产生不同的模型。选择内点数量最多的模型作为最终模型。

6. 提取地面点：一旦找到最佳的平面模型，算法就会使用这个模型来识别地面点。所有被识别为地面的点（即与平面距离小于设定阈值的点）被提取出来形成地面点云。

二、广度优先遍历

广度优先遍历（Breadth-First Search, BFS）是一种用于遍历或搜索树或图的算法。这种算法从树的根（或图的某一指定节点）开始，然后探索邻近的节点，之后对每一个邻近的节点，它再去探索它们各自相邻的节点，这个过程持续进行直到访问所有可达的节点。

广度优先遍历的主要特点是它按照距离起始点的“层次”来遍历。首先访问距离起点最近的节点，然后是它们的邻居，如此类推。

2.1 广度优先遍历的步骤：

1. 初始化：首先将起始节点放入队列中。

2. 遍历：

   • 从队列中弹出一个节点。
   • 检查该节点是否为目标节点。如果是，则完成搜索。
   • 将该节点的所有未访问过的邻居节点加入队列。

3. 重复：重复步骤2，直到队列为空或找到目标节点。

4. 结束：当队列为空且目标未找到，或已找到目标节点时，算法结束。

2.2基于 BFS 的点云聚类和外点剔除