PCL 最小二乘法拟合二次曲面

一、简介

PCL 最小二乘拟合二次曲面_点云侠的博客-CSDN博客_pcl可以拟合超曲面

最小二乘法：

 z=ax^2+by^2+cxy+dx+ey+f

 可以看出上面我们是有6个参数，那么我们的数据至少是6个点。

 二、代码显示

1、矩阵求解：

    Matrix2d A;  //2*2 的矩阵
    A << 1, 2,
        4, 5;
    Eigen::Vector2d b(3, 4);
    Eigen::Vector2d C = A.colPivHouseholderQr().solve(b);

    std::cout << C << std::endl;
    return  0;

 求出AX=b 中X的值：

-2.33333
 2.66667

2、 拟合二次曲面：

#if 1   //   最小二乘法拟合曲面
#include 
using  namespace  Eigen;
void FitSurfaceBySqure(const  pcl::PointCloud::Ptr  &  cloud)
{
    if (cloud->empty()||cloud->size()<6)  // 6个参数需要至少6个点来计算
    {
        cout << "加载文件失败！" << endl;
        return;
    }

    int  n = cloud->size();
    //  定义矩阵并初始化   Ax=B 的线性方程组
    MatrixXd   A = MatrixXd::Ones(n,6);//  n 行 6 列且 权是1 的矩阵
    VectorXd  B(n); // n个z值

    // 二次曲面方程： z=ax^2+by^2+cxy+dx+ey+f.，将点云中的数据放入矩阵当中
    for (size_t i = 0; i < n; i++)
    {
        A(i, 0) = pow(cloud->points[i].x, 2); // ax^2
        A(i, 1) = pow(cloud->points[i].y, 2); //by^2
        A(i, 2) = cloud->points[i].x * cloud->points[i].y; // cxy
        A(i, 3) = cloud->points[i].x; //dx
        A(i, 4) = cloud->points[i].y;// ey
          //f本来就是一个常量
        B(i) = cloud->points[i].z;  //z
    }

    VectorXd x = A.colPivHouseholderQr().solve(B); // 相当于是写成了 AX=B的矩阵的形式，求解的就是AX=b中的X

    cout << "拟合误差: " << (A * x - B).norm() / B.norm() << endl;
    cout << "曲面表达式的系数为:\n "
        "a = " << x[0] << " b = " << x[1] << " c = " << x[2] << " d = " << x[3]
        << " e = " << x[4] << " f =" << x[5] << endl;

    return ;
 }
int main()
{

   // 测试矩阵的求解
    Matrix2d A;  //2*2 的矩阵
    A << 1, 2,
        4, 5;
    Eigen::Vector2d b(3, 4);
    Eigen::Vector2d C = A.colPivHouseholderQr().solve(b);

    std::cout << C << std::endl;

    // 用点云来拟合二次曲面
    pcl::PointCloud::Ptr cloud(new pcl::PointCloud);
    // 加载点云
    if (-1 == pcl::io::loadPCDFile("Horse.pcd", *cloud))//放到与工程中的主.cpp同一位置的文件夹下
    {
        cout << "加载文件失败！" << endl;
        return -1;
    }
    cout << cloud->points.size() << endl;
    // 然后使用二次曲面拟合曲面
    pcl::console::TicToc time;  // 计算拟合需要的时间
    FitSurfaceBySqure(cloud);
    cout << "计算用时：" << time.toc() << " ms" << endl;


    return  0;
}

#endif

 这里拟合误差很大的原因是我用的是一个马的点云来拟合的，严格的意义上说我们应该使用接近数学上的那种点云来拟合。