有效提取激光雷达点云平面点
#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;
}