高精度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/142742841int 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;
}