PCL 垂距法实现点云精简

2026/6/30 22:54:12

一、垂距法

垂距法是道格拉斯普客算法的一种非递归、单遍扫描变体。它维护一个“当前有效基线”（从上一个保留点指向当前候选点），逐个检查中间点到该基线的垂距，若超过阈值则保留该中间点并更新基线起点。

二、代码实现

#include<iostream>#include<vector>#include<cmath>#include<pcl/io/pcd_io.h>#include<pcl/point_types.h>#include<pcl/visualization/pcl_visualizer.h>doubleperpendicularDistance(constpcl::PointXYZ&A,constpcl::PointXYZ&B,constpcl::PointXYZ&P){doubledx=B.x-A.x,dy=B.y-A.y,dz=B.z-A.z;doublelen=std::sqrt(dx*dx+dy*dy+dz*dz);if(len<1e-12)return0.0;doublecross_x=dy*(P.z-A.z)-dz*(P.y-A.y);doublecross_y=dz*(P.x-A.x)-dx*(P.z-A.z);doublecross_z=dx*(P.y-A.y)-dy*(P.x-A.x);returnstd::sqrt(cross_x*cross_x+cross_y*cross_y+cross_z*cross_z)/len;}pcl::PointCloud<pcl::PointXYZ>perpendicularSimplify(constpcl::PointCloud<pcl::PointXYZ>&cloud,doubledistance_threshold){if(cloud.empty())returnpcl::PointCloud<pcl::PointXYZ>();std::vector<pcl::PointXYZ>pts(cloud.begin(),cloud.end());intn=pts.size();std::vector<bool>kept(n,false);kept[0]=kept[n-1]=true;intanchor=0;// 当前基线的起点索引intfloater=2;// 当前基线的终点索引（候选点）while(floater<n){// 检查 anchor 到 floater 之间所有点boolfoundExceed=false;for(inti=anchor+1;i<floater;++i){doubled=perpendicularDistance(pts[anchor],pts[floater],pts[i]);if(d>distance_threshold){// 保留前一个点（floater-1）作为新锚点kept[floater-1]=true;anchor=floater-1;floater=anchor+2;foundExceed=true;break;}}if(!foundExceed){// 所有中间点都在阈值内，继续向前移动floaterfloater++;}}pcl::PointCloud<pcl::PointXYZ>out;out.reserve(std::count(kept.begin(),kept.end(),true));for(inti=0;i<n;++i){if(kept[i])out.push_back(pts[i]);}returnout;}intmain(){pcl::PointCloud<pcl::PointXYZ>::Ptrcloud(newpcl::PointCloud<pcl::PointXYZ>);if(pcl::io::loadPCDFile<pcl::PointXYZ>("sorted_cloud.pcd",*cloud)==-1){std::cerr<<"错误：无法读取点云文件！"<<std::endl;return-1;}std::cout<<"原始点数: "<<cloud->size()<<std::endl;doubledist_thresh=0.03;// 垂距阈值（米）pcl::PointCloud<pcl::PointXYZ>simplified=perpendicularSimplify(*cloud,dist_thresh);std::cout<<"简化后点数: "<<simplified.size()<<std::endl;std::cout<<"简化比例: "<<(double)simplified.size()/cloud->size()*100<<"%"<<std::endl;pcl::io::savePCDFileBinary("simplified_perp.pcd",simplified);std::cout<<"结果已保存至 simplified_perp.pcd"<<std::endl;// 可视化pcl::visualization::PCLVisualizerviewer(u8"垂距法简化");viewer.addPointCloud<pcl::PointXYZ>(cloud,"original");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,1.0,0.0,0.0,"original");viewer.addPointCloud<pcl::PointXYZ>(simplified.makeShared(),"simplified");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0.0,1.0,0.0,"simplified");viewer.spin();return0;}