使用Eigen拟合空间圆 C++

最小二乘拟合圆原理：https://blog.youkuaiyun.com/jiangjjp2812/article/details/106937333

直接看代码：

void FitCircle(std::vector<vector<float>> pts)
{
    vector<float> circle;
 
    int num = pts.size();
    int dim = 3;
 
    Eigen::MatrixXf M(num, dim);
 
    for (int i = 0; i < num; i++)
    {
        for (int j = 0; j < dim; j++)
        {
            M(i, j) = pts[i][j];
        }
    }
 
    Eigen::MatrixXf L1 = Eigen::MatrixXf::Ones(num, 1);
    Eigen::Vector3f A = (M.transpose() * M).inverse() * M.transpose() * L1;
 
    auto A1 = A;
    A1.normalize();
    printf("plane normal:%f,%f,%f\n", A1(0), A1(1), A1(2));
 
    Eigen::MatrixXf B = Eigen::MatrixXf::Zero(num - 1, 3);
 
    for (int i = 0; i < num - 1; i++)
    {
        B.row(i) = M.row(i + 1) - M.row(i);
    }
 
    Eigen::MatrixXf L2 = Eigen::MatrixXf::Zero(num - 1, 1);
    for (int i = 0; i < num - 1; i++)
    {
        L2(i) = (M(i + 1, 0) * M(i + 1, 0) + M(i + 1, 1) * M(i + 1, 1) + M(i + 1, 2) * M(i + 1, 2)
            - (M(i, 0) * M(i, 0) + M(i, 1) * M(i, 1) + M(i, 2) * M(i, 2))) / 2.0;
    }
 
    Eigen::Matrix4f D;
    D.setZero();
    D.block<3, 3>(0, 0) = B.transpose() * B;
    D.block<3, 1>(0, 3) = A;
    D.block<1, 3>(3, 0) = A.transpose();
 
    Eigen::Vector4f L3((B.transpose() * L2)(0), (B.transpose() * L2)(1), (B.transpose() * L2)(2), 1);
    Eigen::Vector4f C = D.inverse() * L3;
 
    float radius = 0;
    for (int i = 0; i < num; i++)
    {
        Eigen::Vector3f tmp(M.row(i)(0) - C(0), M.row(i)(1) - C(1), M.row(i)(2) - C(2));
        radius = radius + sqrt(tmp(0) * tmp(0) + tmp(1) * tmp(1) + tmp(2) * tmp(2));
    }
 
    radius = radius / num;
    printf("radius:%f\n", radius);
    printf("circle center:%f,%f,%f\n", C(0), C(1), C(2));
    printf("lamda:%f\n", C(3));
}

原文链接：https://blog.youkuaiyun.com/ts00001M/article/details/126903998