PCL 点云处理实战：圆孔提取算法实现

原创已于 2025-06-26 08:35:17 修改 · 544 阅读
3 ·
CC 4.0 BY-SA版权
文章标签：
#算法
于 2025-06-25 12:01:21 首次发布
该文章已生成可运行项目，
一、背景介绍

对通过结构光相机拍摄圆孔获取的点云进行处理，计算圆孔的圆心半径数据以及近似法向量
二、核心算法原理

点云平面拟合分割出圆孔底面
边界提取：识别平面内的孔边界点
3D 圆拟合：通过 RANSAC 算法拟合孔的三维圆参数
三、效果展示

原始点云
结果可视化
四、完整代码

#include <iostream>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/common/common_headers.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/features/boundary.h>
#include <pcl/sample_consensus/sac_model_circle3d.h>
#include <pcl/sample_consensus/ransac.h>
pcl::PointCloud<pcl::PointXYZ>::Ptr extractBoundary(
    const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, int normalK = 20,
    int boundaryK = 91500, float angleThreshold = M_PI / 4) {
  // 1. 计算法线 (使用KNN)
  pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::Normal> ne;
  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
      new pcl::search::KdTree<pcl::PointXYZ>);

  ne.setInputCloud(cloud);
  ne.setSearchMethod(tree);
  ne.setKSearch(normalK);  // 使用KNN而非半径搜索
  ne.compute(*normals);

  // 2. 边界提取 (使用KNN)
  pcl::BoundaryEstimation<pcl::PointXYZ, pcl::Normal, pcl::Boundary> est;
  pcl::PointCloud<pcl::Boundary> boundaries;

  est.setInputCloud(cloud);
  est.setInputNormals(normals);
  est.setSearchMethod(tree);
  est.setKSearch(boundaryK);  // 一般K值越高，边界识别精度越好
  est.setAngleThreshold(angleThreshold);
  est.compute(boundaries);

  // 3. 提取边界点索引
  pcl::PointIndices::Ptr boundaryIndices(new pcl::PointIndices);
  for (size_t i = 0; i < boundaries.size(); i++) {
    if (boundaries[i].boundary_point) {
      boundaryIndices->indices.push_back(i);
    }
  }

  // 4. 分离边界点云
  pcl::ExtractIndices<pcl::PointXYZ> extract;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_boundary(
      new pcl::PointCloud<pcl::PointXYZ>);

  extract.setInputCloud(cloud);
  extract.setIndices(boundaryIndices);
  extract.filter(*cloud_boundary);

  return cloud_boundary;
}

// 平面拟合方法 返回拟合的内点和外点
void PlanarFitting(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in_points,
                   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out_points) {
  // 计算法线
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud(cloud);
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(
      new pcl::search::KdTree<pcl::PointXYZ>());
  ne.setSearchMethod(tree);
  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
  ne.setRadiusSearch(1);
  ne.compute(*normals);

  // 平面分割
  pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
  pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients);
  pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices);

  pcl::ExtractIndices<pcl::PointXYZ> extract;

  seg.setOptimizeCoefficients(true);
  seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
  seg.setNormalDistanceWeight(0.1);
  seg.setMethodType(pcl::SAC_RANSAC);
  seg.setMaxIterations(500);
  seg.setDistanceThreshold(0.3);
  seg.setInputCloud(cloud);
  seg.setInputNormals(normals);
  seg.segment(*inliers_plane, *coefficients_plane);

  std::cerr << "Plane coefficients: " << *coefficients_plane << std::endl;

  extract.setInputCloud(cloud);
  extract.setIndices(inliers_plane);
  extract.setNegative(false);
  extract.filter(*cloud_in_points);

  extract.setNegative(true);
  extract.filter(*cloud_out_points);
}

void circle_fitting(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,
                    Eigen::VectorXf& coeff) {
  pcl::SampleConsensusModelCircle3D<pcl::PointXYZ>::Ptr circle3D(
      new pcl::SampleConsensusModelCircle3D<pcl::PointXYZ>(cloud));
  pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(
      circle3D);                     // 定义RANSAC算法对象
  ransac.setDistanceThreshold(0.1);  // 设置距离阈值
  ransac.setMaxIterations(1500);     // 设置最大迭代次数

  ransac.computeModel();

  ransac.getModelCoefficients(coeff);  // 参数
  std::vector<int> ranSacInliers;      // 获取属于拟合出的内点
  ransac.getInliers(ranSacInliers);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloudInliers(
      new pcl::PointCloud<pcl::PointXYZ>);
}

int main(int argc, char** argv) {
  // 创建可视化界面
  pcl::visualization::PCLVisualizer viewer("PCL Viewer");
  viewer.setBackgroundColor(0, 0, 0);

  // 加载点云数据
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
  if (pcl::io::loadPLYFile("test.ply", *cloud) == -1) {
    PCL_ERROR("Couldn't read the PLY file\n");
    return (-1);
  }

  // 可视化原始点云
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_color(
      cloud, 255, 255, 255);
  viewer.addPointCloud<pcl::PointXYZ>(cloud, cloud_color, "cloud");
  viewer.setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "cloud");

  // 提取平面内点
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(
      new pcl::PointCloud<pcl::PointXYZ>);

  // 提取平面外点(剩余点云)
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_remaining(
      new pcl::PointCloud<pcl::PointXYZ>);

  PlanarFitting(cloud, cloud_plane, cloud_remaining);

  PlanarFitting(cloud_remaining, cloud_plane, cloud_remaining);

  // 可视化分割出的平面(红色)
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> plane_color(
      cloud_plane, 255, 0, 0);
  viewer.addPointCloud<pcl::PointXYZ>(cloud_plane, plane_color, "plane");
  viewer.setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "plane");

  // 可视化剩余点云(绿色)
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
      remaining_color(cloud_remaining, 0, 255, 0);
  viewer.addPointCloud<pcl::PointXYZ>(cloud_remaining, remaining_color,
                                      "remaining");
  viewer.setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "remaining");

  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_boundary =
      extractBoundary(cloud_plane);
  std::cerr << "Boundary points: " << cloud_boundary->size() << std::endl;
  pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
      boundary_color(cloud_boundary, 0, 0, 255);
  viewer.addPointCloud<pcl::PointXYZ>(cloud_boundary, boundary_color,
                                      "boundary");
  viewer.setPointCloudRenderingProperties(
      pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "boundary");
  Eigen::VectorXf coeff;
  circle_fitting(cloud_boundary, coeff);
  std::cout << "圆心的x坐标为: " << coeff[0] << std::endl;
  std::cout << "圆心的y坐标为: " << coeff[1] << std::endl;
  std::cout << "圆心的z坐标为: " << coeff[2] << std::endl;
  std::cout << "圆心的半径为: " << coeff[3] << std::endl;
  std::cout << "法线的x方向为: " << coeff[4] << std::endl;
  std::cout << "法线的y方向为: " << coeff[5] << std::endl;
  std::cout << "法线的z方向为: " << coeff[6] << std::endl;
  {
    // 显示圆心
    pcl::PointXYZ center;
    center.x = coeff[0];
    center.y = coeff[1];
    center.z = coeff[2];
    viewer.addSphere(center, 0.05, "circle_center");
    // 根据圆参数画圆
    // 计算圆的方向向量
    Eigen::Vector3f normals(coeff[4], coeff[5], coeff[6]);
    normals.normalize();

    // 创建一个正交基
    Eigen::Vector3f u, v;
    if (std::abs(normals[0]) < 0.9) {
      u = Eigen::Vector3f(1, 0, 0).cross(normals);
    } else {
      u = Eigen::Vector3f(0, 1, 0).cross(normals);
    }
    u.normalize();
    v = normals.cross(u);

    // 生成圆上的点
    pcl::PointCloud<pcl::PointXYZ>::Ptr circle_points(
        new pcl::PointCloud<pcl::PointXYZ>);
    const int num_points = 100;
    for (int i = 0; i < num_points; i++) {
      float angle = 2.0f * M_PI * i / num_points;
      pcl::PointXYZ point;
      point.x = center.x + coeff[3] * (u[0] * cos(angle) + v[0] * sin(angle));
      point.y = center.y + coeff[3] * (u[1] * cos(angle) + v[1] * sin(angle));
      point.z = center.z + coeff[3] * (u[2] * cos(angle) + v[2] * sin(angle));
      circle_points->points.push_back(point);
    }

    // 添加线段连接圆上的点
    for (int i = 0; i < num_points; i++) {
      int j = (i + 1) % num_points;
      std::string line_id = "line" + std::to_string(i);
      viewer.addLine(circle_points->points[i], circle_points->points[j], 0.0,
                     1.0, 0.0, line_id);
    }

    // 绘制法线向量
    pcl::PointXYZ normal_end;
    normal_end.x = center.x + normals[0] * coeff[3] * 1.5;
    normal_end.y = center.y + normals[1] * coeff[3] * 1.5;
    normal_end.z = center.z + normals[2] * coeff[3] * 1.5;
    viewer.addLine(center, normal_end, 1.0, 1.0, 0.0, "normal");
  }

  // 开始可视化循环
  while (!viewer.wasStopped()) {
    viewer.spinOnce(100);
  }

  return 0;
}
本文章已经生成可运行项目