PCL拟合空间3D圆周 fit3DCircle

PCL版本 1.15.0

main.cpp

#include<vector>
#include<iostream>
#include <array>
#include <string>
#include <windows.h>
#include <omp.h>
#include <charconv>  // C++17
#include <cstdlib>
#include<chrono>
#include"FitCircle3D.h"#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/sample_consensus/sac_model_circle3d.h>
#include <pcl/segmentation/sac_segmentation.h>int main() {// 生成一个3D圆的点云（圆心(0,0,0)，半径0.5，法向量(0,0,1)）pcl::PointXYZ center(0, 0, 0);Eigen::Vector3f normal0(0, 0, 1);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = generate3DCirclePoints(0.5, center, normal0, 200, 0, 0.25 * M_PI, 0.01);// 拟合并可视化pcl::PointIndices::Ptr inliers(new pcl::PointIndices);pcl::ModelCoefficients::Ptr coefficients = fit3DCircle(cloud, inliers);center = pcl::PointXYZ(coefficients->values[0], coefficients->values[1], coefficients->values[2]);auto normal = pcl::Normal(coefficients->values[4], coefficients->values[5], coefficients->values[6]);// ---------------------- 可视化 ----------------------pcl::visualization::PCLVisualizer viewer("3D Circle Fitting");viewer.setBackgroundColor(0, 0, 0);// 原始点云（白色）viewer.addPointCloud<pcl::PointXYZ>(cloud, "cloud");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 1, 1, "cloud");// 内点（绿色）pcl::PointCloud<pcl::PointXYZ>::Ptr inlier_cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::copyPointCloud(*cloud, *inliers, *inlier_cloud);viewer.addPointCloud<pcl::PointXYZ>(inlier_cloud, "inliers");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "inliers");viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "inliers");// 将栈对象转为智能指针（注意：viewer 的生命周期必须长于 ptr）pcl::visualization::PCLVisualizer::Ptr viewer_ptr(&viewer, [](void*) { /* 空删除器，避免重复释放 */ });拟合的圆（红色）draw3DCircle(viewer_ptr, center, normal, coefficients->values[3]);// 显示坐标系和法向量viewer.addCoordinateSystem(0.2);viewer.addArrow<pcl::PointXYZ>(pcl::PointXYZ(coefficients->values[0], coefficients->values[1], coefficients->values[2]),pcl::PointXYZ(coefficients->values[0] + 0.1 * coefficients->values[4],coefficients->values[1] + 0.1 * coefficients->values[5],coefficients->values[2] + 0.1 * coefficients->values[6]), 0, 1, 0, false, "normal");while (!viewer.wasStopped()) {viewer.spinOnce(100);}return 0;
}

FitCircle3D.h

#pragma once
#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/sac_model_circle3d.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>// 生成一个3D圆的点云（带噪声）
pcl::PointCloud<pcl::PointXYZ>::Ptr generate3DCirclePoints(float radius, const pcl::PointXYZ& center,const Eigen::Vector3f& normal, int num_points, float start_angle_rad = 0, float end_angle_rad = 2.0 * M_PI,float noise_level = 0.01);// 使用 SACMODEL_CIRCLE3D 拟合3D圆
pcl::ModelCoefficients::Ptr fit3DCircle(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointIndices::Ptr inliers);pcl::PointCloud<pcl::PointXYZ>::Ptr convertToPointCloud(std::vector<pcl::PointXYZ>& points);
pcl::PointCloud<pcl::Normal>::Ptr convertToNormals(std::vector<pcl::Normal>& normals);void draw3DCircle(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointXYZ& center,const pcl::Normal& normal, float radius, int numPoints = 200);

Fit3DCircle.cpp

#include "FitCircle3D.h"// 生成一个3D圆的点云（带噪声）
pcl::PointCloud<pcl::PointXYZ>::Ptr generate3DCirclePoints(float radius, const pcl::PointXYZ& center,const Eigen::Vector3f& normal, int num_points, float start_angle_rad, float end_angle_rad, float noise_level) {pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);cloud->resize(num_points);// 生成圆所在平面的两个正交向量Eigen::Vector3f u = normal.unitOrthogonal();Eigen::Vector3f v = normal.cross(u).normalized();// 生成圆上的点for (int i = 0; i < num_points; ++i) {float theta = (end_angle_rad - start_angle_rad) * i / num_points + start_angle_rad;float x = center.x + radius * (cos(theta) * u[0] + sin(theta) * v[0]);float y = center.y + radius * (cos(theta) * u[1] + sin(theta) * v[1]);float z = center.z + radius * (cos(theta) * u[2] + sin(theta) * v[2]);// 添加噪声x += noise_level * (2.0 * rand() / RAND_MAX - 1.0);y += noise_level * (2.0 * rand() / RAND_MAX - 1.0);z += noise_level * (2.0 * rand() / RAND_MAX - 1.0);(*cloud)[i] = pcl::PointXYZ(x, y, z);}return cloud;
}// 使用 SACMODEL_CIRCLE3D 拟合3D圆
pcl::ModelCoefficients::Ptr fit3DCircle(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointIndices::Ptr inliers) 
{// 拟合圆pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);pcl::SACSegmentation<pcl::PointXYZ> seg;seg.setOptimizeCoefficients(true);seg.setModelType(pcl::SACMODEL_CIRCLE3D);seg.setMethodType(pcl::SAC_RANSAC);seg.setDistanceThreshold(0.02);  // 内点距离阈值seg.setMaxIterations(1000);seg.setInputCloud(cloud);seg.segment(*inliers, *coefficients);if (inliers->indices.empty()) {std::cerr << "拟合失败！" << std::endl;return nullptr;}// 输出结果std::cout << "\n拟合结果：" << std::endl;std::cout << "圆心: (" << coefficients->values[0] << ", "<< coefficients->values[1] << ", "<< coefficients->values[2] << ")" << std::endl;std::cout << "半径: " << coefficients->values[3] << std::endl;std::cout << "法向量: (" << coefficients->values[4] << ", "<< coefficients->values[5] << ", "<< coefficients->values[6] << ")" << std::endl;return coefficients;
}pcl::PointCloud<pcl::PointXYZ>::Ptr convertToPointCloud(std::vector<pcl::PointXYZ>& points)
{pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);// 2. 设置点云的宽度、高度（如果是无序点云，高度设为1）cloud->width = points.size(); // 点云的点数cloud->height = 1;            // 无序点云cloud->is_dense = true;       // 数据是否包含 NaN 值（false 表示可能有无效点）// 3. 将 std::vector 的数据复制到 PointCloudcloud->points.resize(points.size());for (size_t i = 0; i < points.size(); ++i) {cloud->points[i] = points[i];}return cloud;
}pcl::PointCloud<pcl::Normal>::Ptr convertToNormals(std::vector<pcl::Normal>& normals)
{pcl::PointCloud<pcl::Normal>::Ptr cloud(new pcl::PointCloud<pcl::Normal>);// 设置点云的宽度和高度cloud->width = normals.size();  // 点云的宽度（列数）cloud->height = 1;              // 点云的高度（行数）cloud->is_dense = false;        // 是否是密集点云for (const auto& normal : normals){cloud->points.push_back(normal);}return cloud;
}void draw3DCircle(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointXYZ& center, const pcl::Normal& normal, float radius, int numPoints /*= 200*/)
{std::vector<pcl::PointXYZ> points;std::vector<pcl::Normal> normals;// 计算法向量的旋转矩阵，用于将局部坐标系的点映射到全局坐标系Eigen::Vector3f z_axis(0, 0, 1); // 假设法向量在局部坐标系中沿 z 轴Eigen::Vector3f normal_vec(normal.normal_x, normal.normal_y, normal.normal_z);Eigen::Vector3f axis = z_axis.cross(normal_vec);float angle = std::acos(z_axis.dot(normal_vec) / (z_axis.norm() * normal_vec.norm()));Eigen::Matrix3f rotation_matrix = Eigen::AngleAxisf(angle, axis.normalized()).toRotationMatrix();// 将点绘制为点云pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = generate3DCirclePoints(radius, center, Eigen::Vector3f(normal.normal_x, normal.normal_y, normal.normal_z), numPoints, 0, 2.0 * M_PI, 0.0f);pcl::PointCloud<pcl::Normal>::Ptr torusNormals = convertToNormals(normals);for (size_t i = 0; i < cloud->points.size() - 1; ++i) {const pcl::PointXYZ& point1 = cloud->points[i];const pcl::PointXYZ& point2 = cloud->points[i + 1];// 添加线段std::string line_id = "line_" + std::to_string(i);viewer->addLine<pcl::PointXYZ>(point1, point2, 1.0, 0.0, 0.0, line_id); // 白色线段}// 如果需要闭合图形，可以手动添加最后一条线段if (cloud->points.size() > 2) {const pcl::PointXYZ& first_point = cloud->points[0];const pcl::PointXYZ& last_point = cloud->points[cloud->points.size() - 1];viewer->addLine<pcl::PointXYZ>(last_point, first_point, 1.0, 0.0, 0.0, "line_closure"); // 白色线段}
}