C++ Qt/Eigen拟合三维平面与三维圆

std::optional<std::tuple<double, double, double, double>> FitPlane(const QList<QVector3D>& points)
{
    // 如果点数小于3，无法拟合平面
    if (points.size() < 3) {
        return std::nullopt;
    }

    double x0x0 = 0, x0y1 = 0, x0z2 = 0, x0_3 = 0;
    double x1y0 = 0, y1y1 = 0, y1z2 = 0, y1_3 = 0;
    double x2z0 = 0, y2z1 = 0, z2z2 = 0, z2_3 = 0;
    double x3_0 = 0, y3_1 = 0, z3_2 = 0, n_3_3 = 0;
    for (auto&& point : points) {
        x0x0 += point.x() * point.x();
        x0y1 += point.x() * point.y();
        x0z2 += point.x() * point.z();
        x0_3 += point.x();
        x1y0 += point.x() * point.y();
        y1y1 += point.y() * point.y();
        y1z2 += point.y() * point.z();
        y1_3 += point.y();
        x2z0 += point.x() * point.z();
        y2z1 += point.y() * point.z();
        z2z2 += point.z() * point.z();
        z2_3 += point.z();
        x3_0 += point.x();
        y3_1 += point.y();
        z3_2 += point.z();
    }
    n_3_3 = points.size();
    // 构造误差矩阵
    Eigen::Matrix4d A1;
    A1 << x0x0, x0y1, x0z2, x0_3, x1y0, y1y1, y1z2, y1_3, x2z0, y2z1, z2z2, z2_3, x3_0, y3_1, z3_2, n_3_3;
    try {
        // A1 * [A, B, C, D].T = [0, 0, 0, 0].T, 求解[A, B, C, D].T的最优值,约束条件||[A, B, C, D].T||=1, min||A1 * [A, B, C, D].T||^2
        Eigen::JacobiSVD<Eigen::Matrix4d> svd(A1, Eigen::ComputeFullU | Eigen::ComputeFullV);
        Eigen::Matrix4d V = svd.matrixV();
        Eigen::Vector4d result = V.col(3);
        // 归一化
        result.normalize();
        return std::make_tuple(result(0), result(1), result(2), result(3));
    } catch (...) {
        return std::nullopt;
    }
}

std::optional<QPair<double, QVector3D>> Fit2DCircle(const QList<QVector3D>& points)
{
    if (points.size() < 3) {
        return { std::nullopt };
    }

    Eigen::Vector3d center;
    double radius = 0.0f;

    double sum_x = 0.0f, sum_y = 0.0f;
    double sum_x2 = 0.0f, sum_y2 = 0.0f;
    double sum_x3 = 0.0f, sum_y3 = 0.0f;
    double sum_xy = 0.0f, sum_x1y2 = 0.0f, sum_x2y1 = 0.0f;

    int N = points.size();
    for (int i = 0; i < N; i++) {
        double x = points[i].x();
        double y = points[i].y();
        double x2 = x * x;
        double y2 = y * y;
        sum_x += x;
        sum_y += y;
        sum_x2 += x2;
        sum_y2 += y2;
        sum_x3 += x2 * x;
        sum_y3 += y2 * y;
        sum_xy += x * y;
        sum_x1y2 += x * y2;
        sum_x2y1 += x2 * y;
    }

    double C, D, E, G, H;
    double a, b, c;

    C = N * sum_x2 - sum_x * sum_x;
    D = N * sum_xy - sum_x * sum_y;
    E = N * sum_x3 + N * sum_x1y2 - (sum_x2 + sum_y2) * sum_x;
    G = N * sum_y2 - sum_y * sum_y;
    H = N * sum_x2y1 + N * sum_y3 - (sum_x2 + sum_y2) * sum_y;
    a = (H * D - E * G) / (C * G - D * D);
    b = (H * C - E * D) / (D * D - G * C);
    c = -(a * sum_x + b * sum_y + sum_x2 + sum_y2) / N;

    center[0] = a / (-2);
    center[1] = b / (-2);
    radius = sqrt(a * a + b * b - 4 * c) / 2;
    return std::make_pair(radius, QVector3D(center[0], center[1], 0));
}

std::optional<std::tuple<double, QVector3D, QVector3D>> Fit3DCircle(const QList<QVector3D>& points)
{
    if (points.size() < 3) {
        return { std::nullopt };
    }

    QList<QVector3D> cycle_pts = points;

    auto&& plane = Utils::Plane::FitPlane(cycle_pts);
    if (!plane.has_value()) {
        return { std::nullopt };
    }
    auto&& [A, B, C, D] = plane.value();
    QVector3D abcd(DF(A), DF(B), DF(C));

    Eigen::Vector3d normal(A, B, C);

    // 与Z轴叉乘，得到旋转轴向量
    Eigen::Vector3d axis = normal.cross(Eigen::Vector3d::UnitZ());
    // 旋转轴向量归一化
    axis.normalize();
    // 旋转角度
    double rad = acos(normal.dot(Eigen::Vector3d::UnitZ()) / normal.norm());
    // 将所有点绕旋转轴旋转angle角度
    Eigen::AngleAxisd rotation(rad, axis);
    Eigen::Matrix3d rotation_matrix = rotation.toRotationMatrix();
    for (auto&& point : cycle_pts) {
        Eigen::Vector3d eigen_point(point.x(), point.y(), point.z());
        Eigen::Vector3d rotated_point = rotation_matrix * eigen_point;
        point.setX(rotated_point.x());
        point.setY(rotated_point.y());
        point.setZ(rotated_point.z());
    }

    std::tie(A, B, C, D) = Utils::Plane::FitPlane(cycle_pts).value();
	double z = -D / C;

    // 拟合二维圆
    auto&& circle = Fit2DCircle(cycle_pts);
    if (!circle.has_value()) {
        return { std::nullopt };
    }

    // 计算圆心
    Eigen::Vector3d center(std::get<1>(circle.value()).x(), std::get<1>(circle.value()).y(), z);
    // 圆心反旋转回去
    Eigen::Vector3d rotated_center = rotation_matrix.inverse() * center;
    QVector3D center_point(rotated_center.x(), rotated_center.y(), rotated_center.z());
    Eigen::Vector3d p1 = Eigen::Vector3d(cycle_pts[0].x(), cycle_pts[0].y(), cycle_pts[0].z());
    Eigen::Vector3d r_p1 = rotation_matrix.inverse() * p1;

    double radius = std::get<0>(circle.value());

    return { { radius, center_point, abcd } };
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149