0. 简介

之前作者专门为点云匹配写了几篇博客,但是我们发现最近几年有更多的新方法已经在不断地被使用。同时之前有些内容也没有很好的概括,所以这里我们将作为一篇进阶文章来介绍这些方法的使用。

1. 地面点去除

处了使用一些复杂的方法(FEC)或是一些简单的方法(根据高度来滤除)以外,还可以使用Ransac的方法完成平面拟合

#include <pcl/point_types.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/sac_segmentation.h>

void RemovePointsUnderGround(const pcl::PointCloud<pcl::PointXYZI>& cloud_in,
                             pcl::PointCloud<pcl::PointXYZI>& cloud_out)
{
    // 对输入点云进行降采样
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_downsampled(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::VoxelGrid<pcl::PointXYZI> voxel_grid;
    voxel_grid.setInputCloud(cloud_in.makeShared());
    voxel_grid.setLeafSize(0.1f, 0.1f, 0.1f); // 设置体素格大小
    voxel_grid.filter(*cloud_downsampled);

    // 创建一个滤波器对象,用于提取地面平面
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::PassThrough<pcl::PointXYZI> pass_through;
    pass_through.setInputCloud(cloud_downsampled);
    pass_through.setFilterFieldName("z"); // 对z轴进行滤波
    pass_through.setFilterLimits(-1.5, 0.5); // 设置滤波范围,过滤掉z轴在-1.5到0.5之间的点
    pass_through.filter(*cloud_filtered);

    // 创建一个分割对象,用于提取地面平面
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZI> segmentation;
    segmentation.setInputCloud(cloud_filtered);
    segmentation.setModelType(pcl::SACMODEL_PLANE);
    segmentation.setMethodType(pcl::SAC_RANSAC);
    segmentation.setDistanceThreshold(0.1); // 设置距离阈值,点到平面的距离小于该阈值的点将被认为是地面点
    segmentation.segment(*inliers, *coefficients);

    // 创建一个提取对象,用于提取地面点
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_ground(new pcl::PointCloud<pcl::PointXYZI>);
    pcl::ExtractIndices<pcl::PointXYZI> extract;
    extract.setInputCloud(cloud_filtered);
    extract.setIndices(inliers);
    extract.setNegative(false); // 提取地面点,即保留inliers对应的点
    extract.filter(*cloud_ground);

    // 创建一个提取对象,用于提取非地面点
    pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_non_ground(new pcl::PointCloud<pcl::PointXYZI>);
    extract.setNegative(true); // 提取非地面点,即去除inliers对应的点
    extract.filter(*cloud_non_ground);

    // 将结果保存到输出点云中
    cloud_out = *cloud_non_ground;
}

2. PCA主成分判别

除了去除点云以外,还可以通过主成分判别来判断我们分割的是否是地面。其中eigenvectors()函数得到的矩阵中的三个列向量分别对应于数据的主成分轴。这些主成分轴是按照数据方差的降序排列的,即第一个列向量对应的是数据的第一主成分轴,第二个列向量对应的是数据的第二主成分轴,第三个列向量对应的是数据的第三主成分轴。对于PCA的特征值和特征向量可以从https://blog.csdn.net/lazysnake666/article/details/122404671这里理解。

#include <iostream>
#include <vector>
#include <pcl/point_types.h>
#include <pcl/common/pca.h>

bool EstimatePlane(const pcl::PointCloud<pcl::PointXYZI>& cloud)
{
    
    // 将输入点云数据转换为PCL点云格式
    for (const auto& point : cloud)
    {
        pcl::PointXYZ pclPoint;
        pclPoint.x = point.x();
        pclPoint.y = point.y();
        pclPoint.z = point.z();
        cloud->push_back(pclPoint);
    }
    
    // 创建PCA对象
    pcl::PCA<pcl::PointXYZ> pca;
    pca.setInputCloud(cloud);
    
    // 计算主成分
    Eigen::Vector3f eigenValues = pca.getEigenValues();
    Eigen::Matrix3f eigenVectors = pca.getEigenVectors();
    
    // 获取地面法向量,因为最小的就是第三列,所以最后一列是地面(0,0,1),如果是墙面那就(x,1-x,0)
    Eigen::Vector3f groundNormal = eigenVectors.col(2);#eigen_vector.block<3, 1>(0, 2)//最小成分的主成分向量,对应的是地面的法线,因为地面XY都存在比较大的主成分
    // 如果是其他的比如灯杆这种,一般的就会是fabs(eigen_vector.block<3, 1>(0, 0).dot(Eigen::Vector3f::UnitZ()))的形式,也就是最大主成分,沿着最大主成分方向
    
    bool is_ground = (fabs(groundNormal.dot(
                              Eigen::Vector3f::UnitZ())) > 0.98) &&
                         (eigenValues(2) < 0.05 * 0.05);//最小得列和地面法线重合|a||b|cos,并且eigenValues重要程度满足要求,因为地面基本等于0,所以特征值也很小   https://blog.csdn.net/xinxiangwangzhi_/article/details/118228160
    // 如果是其他的比如灯杆这种,一般的就会是eigen_values(0) > 10 * eigen_values(1)

    return is_ground;
}

3. GICP配准

…详情请参照古月居

11-01 15:00