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

1. 高度过滤：

   • 首先，根据激光雷达传感器的安装高度，对当前帧扫描得到的点云进行高度过滤，以初步分割出地面点云。假设第 $k$ 帧的点云为 { P 0 , P 1 , P 2 , ⋯ P n } k \left\{P_{0}, P_{1}, P_{2},\cdots P_{n}\right\}_{k} {P0​,P1​,P2​,⋯Pn​}k​，传感器离地面的安装高度为 $s\_H$，高度过滤阈值为 $th\_H$，则地面点云可以通过以下公式初步提取：
     { P G 0 , P G 1 , … P G m } k for  P G i . z ∈ [ s − H − t h − H , s − H + t h − H 2 ] \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] {PG0​,PG1​,…PGm​}k​ for PGi​.z∈[s−​H−th−​H,s−​H+2th−​H​]
   • 这一步通过设定高度范围来初步筛选出可能的地面点。

2. 法向量过滤：

   • 在初步提取的地面点云中，仍然可能存在一些非地面点。为了进一步精炼地面点云，使用局部法向量过滤。假设以查询点 $P_{Gi}$ 为中心，通过KD-Tree搜索得到局部点集 { P G i , P G i + 1 , ⋯ , P G n } \left\{P_{G i}, P_{G i+1},\cdots, P_{G n}\right\} {PGi​,PGi+1​,⋯,PGn​}。
   • 对局部点集进行主成分分析（PCA），计算协方差矩阵并求解最小特征值对应的特征向量，该特征向量即为局部平面的法向量。
   • 计算局部平面法向量与先验地面法向量之间的夹角余弦值，保留夹角小于设定阈值的点集，从而实现法向量过滤。

3. RANSAC算法拟合平面系数：

   • 在经过法向量过滤后，使用RANSAC算法对地面点云进行平面系数的拟合。RANSAC算法通过随机选择几个点计算平面方程 $Ax+By+Cz+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 rotationEigen::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 RANSACif(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 verticalreturn 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)