C++ Qt/Eigen拟合三维平面与三维圆-CSDN博客

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());
    }

    double z = -D / C;
    // 计算旋转后的向量
    Eigen::Vector3d v = normal;
    v = rotation_matrix * v;
    // 如果normal的z分量与v的z分量不同向
    if (v.z() * normal.z() < 0) {
        z = -z;
    }

    plane = Utils::Plane::FitPlane(cycle_pts);
    if (!plane.has_value()) {
        return { std::nullopt };
    }

    // 拟合二维圆
    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 } };
}