高精度3D圆弧拟合 （C++)

本文的目的是实现高精度的3D圆弧拟合，若对精度要求不高，可使用PCL的圆拟合接口，参见

PCL拟合空间3D圆周 fit3DCircle-CSDN博客

 --------------------------------------------------------------------------------------------------------------

实现思路

1）RANSAC拟合平面，得到平面模型，去掉离群点；

2）根据平面模型，构建新的坐标系：以源点云中心为坐标原点，以与平面法向量正交的两个单位正交向量分别作为X轴和Y轴正方向，将源点云投影至此坐标系XOY平面，得到2D圆弧点云；

3）拟合2D圆弧点云，得到圆心点和半径；参见

4）将圆心点结合2）中的坐标系，反变换至源点云坐标系中，得到3D圆弧圆心坐标，3D圆弧的半径值与3）的半径拟合结果一致。

其中，

1）的实现参见

《并行RANSAC平面拟合（C++)-CSDN博客》

2）、4）的实现参见

《三维平面点云与二维坐标点之间的转换（投影法）_eigen 3d plane fit-CSDN博客》

3）的实现可参见

 《高精度并行2D圆弧拟合（C++)-CSDN博客》

代码实现

依赖库包括 Eigen + GLM + Ceres-2.1.0 + glog-0.6.0 + gflag-2.2.2

给出部分关键的实现代码（其余接口代码在参考链接中已给出）

common.h

#pragma once
#include<glm/glm.hpp>
#include<glm/ext.hpp>
#include<iostream>
#include<vector>
#include<string>
#include<algorithm>
#include<Eigen/Dense>

typedef glm::dvec3 Point3d;
typedef glm::dvec2 Point2d;
typedef glm::dvec2 Vec2d;
typedef glm::dvec3 Vec3d;

Point3d calculateCenter(const std::vector<Point3d>& input);

common.cpp

#include"common.h"

Point3d calculateCenter(const std::vector<Point3d>& input) {
	Point3d mid{};
	if (input.empty()) return mid;

	for (const auto& p : input) {
		mid.x += p.x;
		mid.y += p.y;
		mid.z += p.z;
	}
	mid.x /= input.size();
	mid.y /= input.size();
	mid.z /= input.size();

	return mid;
}

 Coord2DTransformer.h

#pragma once
#include<vector>
#include<glm/glm.hpp>
#include<ransac_plane.h>
#include"common.h"

double angle_between_vectors(const glm::dvec3& v1, const glm::dvec3& v2);

class Coord2DTransformer
{
public:
	void SetData(std::vector<Point3d>& input)
	{
		// fit_plane参见博客《CGAL散乱点拟合最小二乘平面（3D平面拟合，基于Eigen）》
		// https://blog.csdn.net/xmyzqs1212/article/details/142742841
		int minInliers = std::floor(input.size() * 0.66666);
		Plane3D plane = parallelRansacFitPlane(input, 10000, 0.1, minInliers, 4, 0.8).param;
		centroid = calculateCenter(input);
		// 使用并行RANSAC拟合平面
		
		//PlaneModel plane = parallelRansacFitPlane(points, 10000, 0.1, 300, 4, 0.8);
		Vec3d norm = plane.normal();
		Vec3d arbitrary(1, 0, 0);
		auto angle = angle_between_vectors(norm, arbitrary);
		// 避免任意选取的向量与平面法向量方向一致（方向一致没办法通过向量积计算正交向量）
		if (angle < 1.0 || angle > 179) {
			arbitrary = Vec3d(0, 1, 0);
		}

		basis1 = glm::normalize(glm::cross(norm, arbitrary));
				basis2 = glm::normalize(glm::cross(norm, basis1));

		points2d.resize(input.size());
		for (int i = 0; i < input.size(); i++)
		{
			Vec3d v = input[i] - centroid;
			points2d[i][0] = glm::dot(v, basis1);
			points2d[i][1] = glm::dot(v, basis2);
		}
	}

	Point3d calcCoord3D(double x, double y)
	{
		auto v = x * basis1 + y * basis2;
		return Point3d(centroid.x + v.x, centroid.y + v.y, centroid.z + v.z);
	}

	void calcCoord3D(const std::vector<Vec2d>& pts2d, std::vector<Point3d>& result)
	{
		result.resize(pts2d.size());
		for (int i = 0; i < pts2d.size(); i++)
		{
			result[i] = calcCoord3D(pts2d[i][0], pts2d[i][1]);
		}
	}

	std::vector<Vec2d> points2d;
private:
	Vec3d basis1;
	Vec3d basis2;
	Point3d centroid;
};

Coord2DTransformer.cpp

#include "Coord2DTransformer.h"
#include<glm/ext.hpp>

double angle_between_vectors(const glm::dvec3& v1, const glm::dvec3& v2)
{
	double dot_product = glm::dot(v1, v2);
	double magnitude_v1 = glm::length(v1);
	double magnitude_v2 = glm::length(v2);

	double cos_angle = dot_product / (magnitude_v1 * magnitude_v2);
	// 确保cos_angle在[-1, 1]范围内，避免浮点数误差
	cos_angle = std::max(-1.0, std::min(1.0, cos_angle));

	return std::acos(cos_angle) / glm::pi<double>() * 180.0; // 返回角度
}

main.cpp 

int main()
{
	auto points3d = generateNoisyArc3D(Point3d(100.0, 77.0, 55.2), 
		20.0,
		Vec3d(2, 0, 1), 200, 10, 200, 0.01);

	Coord2DTransformer transformer;
	transformer.SetData(points3d);

	savePointCloudToTxt(points3d, "points3d.txt");
	savePointCloudToTxt(transformer.points2d, "points2d.txt");

	//auto points = generateNoisyArc2D(Point2d(100.0, 77.0), 2.0, 20, 130, 200, 0.01);
	auto model = parallelRansacFitCircle2D(transformer.points2d, 1000, 0.1, 0, 4, 0.99);
	Point3d newCenter = transformer.calcCoord3D(model.param.center.x, model.param.center.y);
	std::cout << model.param << std::endl;
	std::cout << model.inliers.size() << std::endl;
	return 0;
}