有效提取激光雷达点云平面点

有效地面点云的提取和平面点的识别是通过一系列步骤实现的。以下是主要步骤：

高度过滤：
- 首先，根据激光雷达传感器的安装高度，对当前帧扫描得到的点云进行高度过滤，以初步分割出地面点云。假设第 $k$ 帧的点云为 $\left\{P_{0}, P_{1}, P_{2},\cdots P_{n}\right\}_{k}$ ，传感器离地面的安装高度为 $s\_H$ ，高度过滤阈值为 $t h\_H$ ，则地面点云可以通过以下公式初步提取：
  $\left\{P_{G 0}, P_{G 1},\ldots P_{G m}\right\}_k \text{ for } P_{G i}.z \in \left[s_{-} H - t h_{-} H, s_{-} H + \frac{t h_{-} H}{2}\right]$
- 这一步通过设定高度范围来初步筛选出可能的地面点。
法向量过滤：
- 在初步提取的地面点云中，仍然可能存在一些非地面点。为了进一步精炼地面点云，使用局部法向量过滤。假设以查询点 $P_{G i}$ 为中心，通过KD-Tree搜索得到局部点集 $\left\{P_{G i}, P_{G i+1},\cdots, P_{G n}\right\}$ 。
- 对局部点集进行主成分分析（PCA），计算协方差矩阵并求解最小特征值对应的特征向量，该特征向量即为局部平面的法向量。
- 计算局部平面法向量与先验地面法向量之间的夹角余弦值，保留夹角小于设定阈值的点集，从而实现法向量过滤。
RANSAC算法拟合平面系数：
- 在经过法向量过滤后，使用RANSAC算法对地面点云进行平面系数的拟合。RANSAC算法通过随机选择几个点计算平面方程 $A x + B y + C z + D = 0$ ，然后计算所有点到平面的距离，选择距离小于阈值的点作为内点，否则为外点。
- 通过多次迭代选择最佳拟合参数，从而有效地减少异常值对平面方程系数拟合的影响。

通过这些步骤，能够有效地提取和识别地面点云，并计算出准确的地面平面系数。


#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <iomanip>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

#include <pcl/common/transforms.h>
#include <pcl/features/normal_3d.h>
#include <pcl/search/impl/search.hpp>
#include <pcl/filters/impl/plane_clipper3D.hpp>
#include <pcl/filters/extract_indices.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>

typedef pcl::PointXYZ PointT;
double sensor_height = 0.2;
double normal_filter_thresh = 20.0;
double height_clip_range = 0.5; 
double floor_normal_thresh = 10.0;
double tilt_deg = 0.0;
bool use_normal_filtering = true;
  /**
   * @brief plane_clip
   * @param src_cloud
   * @param plane
   * @param negative
   * @return
   */
  pcl::PointCloud<PointT>::Ptr plane_clip(const pcl::PointCloud<PointT>::Ptr& src_cloud, const Eigen::Vector4f& plane, bool negative)  {
    pcl::PlaneClipper3D<PointT> clipper(plane);
    pcl::PointIndices::Ptr indices(new pcl::PointIndices);

    clipper.clipPointCloud3D(*src_cloud, indices->indices);
    std::cout << " src_cloud's points num is  "<< src_cloud->size() <<std::endl;

    pcl::PointCloud<PointT>::Ptr dst_cloud(new pcl::PointCloud<PointT>);

    pcl::ExtractIndices<PointT> extract;
    extract.setInputCloud(src_cloud);
    extract.setIndices(indices);
    extract.setNegative(negative);
    extract.filter(*dst_cloud);
    std::cout << " dst_cloud's points num is  "<< dst_cloud->size() <<std::endl;

    return dst_cloud;
  }


  /**
   * @brief filter points with non-vertical normals
   * @param cloud  input cloud
   * @return filtered cloud
   * 函数广泛应用于需要根据点云中的几何特征进行筛选的场景，例如在室外环境中区分水平地面与其他垂直结构，通过法线过滤可以提取具有特定性质的点
   */
  pcl::PointCloud<PointT>::Ptr normal_filtering(const pcl::PointCloud<PointT>::Ptr& cloud)  {
      // 创建一个法线估计对象，指定输入和输出的数据类型（PointT 为输入点类型，pcl::Normal 为输出法线类型）
      pcl::NormalEstimation<PointT, pcl::Normal> ne;
      // 设置输入点云，指定要计算法线的点云
      ne.setInputCloud(cloud);
      std::cout << " normal_filtering's cloud point number  is " << cloud->size() <<std::endl;

      // 创建一个 kd-tree 用于邻域查找
      pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);
      // 设置搜索方法，即使用 kd-tree 来加速近邻搜索
      ne.setSearchMethod(tree);

      // 用于存储计算得到的法线
      pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
      // 设置 k-邻域搜索的邻居数目，使用10个邻居来估计法线
      ne.setKSearch(10);
      // 设置视点位置，用于确定法线的方向（这里假设传感器高度为 sensor_height）
      ne.setViewPoint(0.0f, 0.0f, sensor_height);
      // 计算法线，以以上参数进行估计并存储结果在 normals 中
      ne.compute(*normals);

      // 创建一个新的点云，用于存储过滤后的点
      pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);
      // 预先分配足够的内存以容纳原点云大小（提高效率）
      filtered->reserve(cloud->size());

      // 遍历输入点云中的所有点
      for(int i = 0; i < cloud->size(); i++) {
          // 获取第 i 个点的法向量并归一化，然后计算它与全局 Z 轴的点积（夹角的余弦值）
          float dot = normals->at(i).getNormalVector3fMap().normalized().dot(Eigen::Vector3f::UnitZ());
          // 若点积的绝对值大于给定的余弦阈值，则保留该点（意味着与Z轴夹角较小）
          if(std::abs(dot) > std::cos(normal_filter_thresh * M_PI / 180.0)) {
              filtered->push_back(cloud->at(i)); // 将满足条件的点加入新的点云中
          }
      }

      // 设置过滤后点云的属性
      filtered->width = filtered->size(); // 宽度为点的数量
      filtered->height = 1; // 高度为1，这通常意味着这是一个平面而非三维数组
      filtered->is_dense = false; // 设置点云为非稠密，意味着可能存在无效/NaN点

      std::cout << "run normal_filtering() end" <<std::endl;

      // 返回过滤后的点云
      return filtered;
  }


  /**
   * @brief detect the floor plane from a point cloud
   * @param cloud  input cloud
   * @return detected floor plane coefficients
   */
Eigen::Vector4f detect(const std::string& file_path) {

    pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
    if (pcl::io::loadPCDFile<PointT>(file_path, *cloud) == -1 || cloud->empty()) {
        PCL_ERROR("Couldn't read file %s\n", file_path.c_str());
        exit(-1);
    }
    //打印点云的数量
    std::cout << "Cloud size: " << cloud->size() << std::endl;

    // compensate the tilt rotation
    Eigen::Matrix4f tilt_matrix = Eigen::Matrix4f::Identity();
    tilt_matrix.topLeftCorner(3, 3) = Eigen::AngleAxisf(tilt_deg * M_PI / 180.0f, Eigen::Vector3f::UnitY()).toRotationMatrix();

    // filtering before RANSAC (height and normal filtering)
    pcl::PointCloud<PointT>::Ptr filtered(new pcl::PointCloud<PointT>);
    pcl::transformPointCloud(*cloud, *filtered, tilt_matrix);
    filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height + height_clip_range), false);
    filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height - height_clip_range), true);

    if(use_normal_filtering) {
      //函数广泛应用于需要根据点云中的几何特征进行筛选的场景，例如在室外环境中区分水平地面与其他垂直结构，
      //通过法线过滤可以提取具有特定性质的点
      filtered = normal_filtering(filtered);
    }

    //打印
    std::cout << "Filtered size: " << filtered->size() << std::endl;

    pcl::transformPointCloud(*filtered, *filtered, static_cast<Eigen::Matrix4f>(tilt_matrix.inverse()));

    // too few points for RANSAC
    if(filtered->size() < 1024) {
      return Eigen::Vector4f(0,0,0,0);
    }

    // RANSAC
    //建立平面模型，然后使用 RANSAC 算法从点云中估计平面模型的参数
    pcl::SampleConsensusModelPlane<PointT>::Ptr model_p(new pcl::SampleConsensusModelPlane<PointT>(filtered));
    pcl::RandomSampleConsensus<PointT> ransac(model_p);
    ransac.setDistanceThreshold(0.05);//设置距离阈值，即点到平面的距离小于该值的点被认为是内点
    ransac.computeModel();//计算平面模型参数

    //提取内点
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    ransac.getInliers(inliers->indices);

    // too few inliers 检查内点数量是否足够
    if(inliers->indices.size() < 1024) {
      return Eigen::Vector4f(0,0,0,0);
    }

    //打印内点数量
    std::cout << "Inliers size: " << inliers->indices.size() << std::endl;

    // verticality check of the detected floor's normal
    // 检测平面法线是否垂直于全局 Z 轴
    Eigen::Vector4f reference = tilt_matrix.inverse() * Eigen::Vector4f::UnitZ();
    //估计模型系数
    Eigen::VectorXf coeffs;
    ransac.getModelCoefficients(coeffs);
    //检查法线coeffs.head<3>()与参考向量的夹角，通过计算点积来验证法线是否垂直于全局 Z 轴
    double dot = coeffs.head<3>().dot(reference.head<3>());
    if(std::abs(dot) < std::cos(floor_normal_thresh * M_PI / 180.0)) {
      // the normal is not vertical
      return Eigen::Vector4f(0,0,0,0);
    }

    // make the normal upward
    //调整法线方向，使其指向上方
    if(coeffs.head<3>().dot(Eigen::Vector3f::UnitZ()) < 0.0f) {
      coeffs *= -1.0f;
    }
    //打印
    // std::cout << "Plane coefficients: " << coeffs.transpose() << std::endl;
    //保存平面点云到文件
    pcl::PointCloud<PointT>::Ptr plane_cloud(new pcl::PointCloud<PointT>);
    pcl::copyPointCloud(*filtered, inliers->indices, *plane_cloud);
    pcl::io::savePCDFileBinary("plane.pcd", *plane_cloud);

    return Eigen::Vector4f(coeffs);
  }

int main() {
    std::string pcdDirectory = "../cloud.pcd";
    Eigen::Vector4f planeCoefficients = detect(pcdDirectory);
    //打印平面参数
    std::cout << "Plane coefficients: " << planeCoefficients.transpose() << std::endl;
    return 0;
}

cmake_minimum_required(VERSION 3.10)
project(PlaneOptimization)

# 设置C++标准
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)

# 查找PCL库
find_package(PCL 1.8 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

# 查找GTSAM库
# find_package(GTSAM REQUIRED)
# include_directories(${GTSAM_INCLUDE_DIR})

# 添加可执行目标
add_executable(PlaneOptimization ground_extraction.cpp)

# 链接库
# target_link_libraries(PlaneOptimization ${PCL_LIBRARIES} gtsam)

有效提取激光雷达点云平面点

悦读