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 的数据复制到 PointCloud
	cloud->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"); // 白色线段
	}
}