一、核心代码实现
1. 环境配置
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/passthrough.h>using namespace pcl;
using namespace std;
2. 数据预处理与法线估计
void preprocess(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud) {// 体素下采样VoxelGrid<pcl::PointXYZ> voxel;voxel.setInputCloud(cloud);voxel.setLeafSize(0.01f, 0.01f, 0.01f);voxel.filter(*cloud);// 去地面（Z轴范围过滤）PassThrough<pcl::PointXYZ> pass;pass.setInputCloud(cloud);pass.setFilterFieldName("z");pass.setFilterLimits(0.0, 1.5);pass.filter(*cloud);
}void estimateNormals(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,pcl::PointCloud<pcl::Normal>::Ptr &normals) {NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;ne.setInputCloud(cloud);search::KdTree<pcl::PointXYZ>::Ptr tree(new search::KdTree<pcl::PointXYZ>);ne.setSearchMethod(tree);ne.setKSearch(50);ne.compute(*normals);
}
3. 区域生长分割
vector<pcl::PointIndices> regionSegmentation(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud,pcl::PointCloud<pcl::Normal>::Ptr &normals) {RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;reg.setMinClusterSize(100);reg.setMaxClusterSize(INT_MAX);reg.setSearchMethod(search::KdTree<pcl::PointXYZ>::Ptr(new search::KdTree<pcl::PointXYZ>));reg.setNumberOfNeighbours(30);reg.setInputCloud(cloud);reg.setInputNormals(normals);reg.setSmoothnessThreshold(M_PI/180.0 * 30.0); // 30度平滑度阈值reg.setCurvatureThreshold(0.5);vector<pcl::PointIndices> clusters;reg.extract(clusters);return clusters;
}
4. 圆柱面RANSAC拟合
void fitCylinder(pcl::PointCloud<pcl::PointXYZ>::Ptr &cluster,pcl::ModelCoefficients::Ptr &coefficients,pcl::PointIndices::Ptr &inliers) {SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;seg.setOptimizeCoefficients(true);seg.setModelType(SACMODEL_CYLINDER);seg.setMethodType(SAC_RANSAC);seg.setNormalDistanceWeight(0.1);seg.setMaxIterations(5000);seg.setDistanceThreshold(0.05);seg.setRadiusLimits(0.0, 0.1);seg.setInputCloud(cluster);seg.setInputNormals(normals);seg.segment(*inliers, *coefficients);if (inliers->indices.size() < cluster->size()*0.3) {PCL_WARN("低质量圆柱拟合，内点比例不足30%%
");coefficients->values.clear();}
}
5. 主程序流程
int main(int argc, char** argv) {// 加载点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile<pcl::PointXYZ>("input.pcd", *cloud) == -1) {PCL_ERROR("文件加载失败！
");return -1;}// 预处理preprocess(cloud);// 法线估计pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);estimateNormals(cloud, normals);// 区域分割auto clusters = regionSegmentation(cloud, normals);// 可视化器初始化PCLVisualizer viewer("Cylinder Fitting");viewer.setBackgroundColor(0, 0, 0);viewer.addPointCloud<pcl::PointXYZ>(cloud, "original_cloud");// 逐簇处理for (size_t i=0; i<clusters.size(); ++i) {pcl::PointCloud<pcl::PointXYZ>::Ptr cluster(new pcl::PointCloud<pcl::PointXYZ>);pcl::ExtractIndices<pcl::PointXYZ> extract;extract.setInputCloud(cloud);extract.setIndices(boost::make_shared<pcl::PointIndices>(clusters[i]));extract.filter(*cluster);// 圆柱拟合pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);pcl::PointIndices::Ptr inliers(new pcl::PointIndices);fitCylinder(cluster, coefficients, inliers);// 可视化if (!coefficients->values.empty()) {visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> color(cluster, rand()%256, rand()%256, rand()%256);viewer.addPointCloud<pcl::PointXYZ>(cluster, color, "cylinder_"+to_string(i));viewer.addLineStrip(coefficients->values, "axis_"+to_string(i));}}while (!viewer.wasStopped()) {viewer.spinOnce(100);}return 0;
}
二、关键参数优化指南
1. 区域生长参数
| 参数 |
推荐值 |
作用说明 |
setMinClusterSize |
100-500 |
过滤噪声点簇 |
setSmoothnessThresh |
20-45度 |
控制区域平滑度 |
setCurvatureThresh |
0.1-1.0 |
限制曲面曲率变化 |
2. RANSAC参数
| 参数 |
推荐值 |
作用说明 |
setMaxIterations |
3000-10000 |
复杂场景需增加迭代次数 |
setDistanceThresh |
0.03-0.1 |
距离阈值影响内点数量 |
setRadiusLimits |
(0.05, 0.3) |
根据物体尺寸调整半径范围 |
三、典型应用场景
1. 工业检测（管道直径测量）
// 直径计算
float radius = coefficients->values[6] * 2.0f;
cout << "检测到圆柱直径：" << radius << " mm" << endl;
2. 机器人抓取（圆柱定位）
// 提取圆柱轴线参数
Eigen::Vector3f axis_dir(coefficients->values[0], coefficients->values[1], coefficients->values[2]);
Eigen::Vector3f axis_point(coefficients->values[3], coefficients->values[4], coefficients->values[5]);
3. 三维重建（缺失部分补全）
// 生成圆柱网格
pcl::PolygonMesh::Ptr cylinder_mesh(new pcl::PolygonMesh);
pcl::generateCylinderMesh(*cylinder_mesh, coefficients);
四、调试与验证
1. 可视化调试
// 显示拟合误差
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> error_cloud(inliers_cloud, 255, 0, 0);
viewer.addPointCloud(error_cloud);
2. 精度评估
// 计算点到圆柱距离误差
float total_error = 0.0f;
for (auto &pt : cloud->points) {float dist = pointToCylinderDistance(pt, coefficients);total_error += dist;
}
cout << "平均拟合误差：" << total_error/cloud->size() << " mm" << endl;
参考代码 C++ 三维点云的圆柱面拟合 www.youwenfan.com/contentcnr/56445.html
五、扩展功能实现
1. 多圆柱分离
// 基于聚类的圆柱分离
pcl::search::KdTree<pcl::PointXYZ>::Ptr search_tree(new pcl::search::KdTree<pcl::PointXYZ>);
search_tree->setInputCloud(cluster);std::vector<pcl::PointIndices> sub_clusters;
pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
ec.setClusterTolerance(0.1);
ec.setMinClusterSize(50);
ec.setSearchMethod(search_tree);
ec.setInputCloud(cluster);
ec.extract(sub_clusters);
2. 动态参数调整
// 根据点云密度自动调整参数
float density = cloud->size() / cloud->getBoundingBox().volume();
if (density > 1000) {voxel.setLeafSize(0.005, 0.005, 0.005);
} else {voxel.setLeafSize(0.01, 0.01, 0.01);
}
六、参考资料
- PCL官方文档：Region Growing Segmentation
- 《点云处理实战》第5章：几何模型拟合
