基于C++和PCL库的三维点云圆柱面拟合实现
一、核心代码实现
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/contentsfa/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章：几何模型拟合
