本文的目的是实现高精度的3D圆弧拟合，若对精度要求不高，可使用PCL的圆拟合接口，参见
实现思路
1）RANSAC拟合平面，得到平面模型，去掉离群点；
2）根据平面模型，构建新的坐标系：以源点云中心为坐标原点，以与平面法向量正交的两个单位正交向量分别作为X轴和Y轴正方向，将源点云投影至此坐标系XOY平面，得到2D圆弧点云；
3）拟合2D圆弧点云，得到圆心点和半径；参见
4）将圆心点结合2）中的坐标系，反变换至源点云坐标系中，得到3D圆弧圆心坐标，3D圆弧的半径值与3）的半径拟合结果一致。
其中，
1）的实现参见
2）、4）的实现参见
3）的实现可参见
代码实现
依赖库包括 Eigen + GLM + Ceres-2.1.0 + glog-0.6.0 + gflag-2.2.2
给出部分关键的实现代码（其余接口代码在参考链接中已给出）
common.h
cpp
#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
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
cpp
#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
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
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;
}