最小二乘求解点云平面方程及其对应法向量

csdn对latex语法支持不完整啊，太不友好了，那我只好上图片了。我把文档已经上传到csdn了，免积分，链接。
本文采用线性最小二乘直接解法（即对矩阵求逆），当矩阵维度大时，可能耗时比较多，可以采用矩阵分解方法缓解（文中已实现）。也可以把最后一步的求逆求解过程，换成迭代过程，即可变为线性最小二乘的迭代解法（未实现）。



4show the codes

// 10最小二乘拟合平面.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//
/********************
作者：xx
时间：2021.06.15
程序功能：c++实现点云最小二乘平面拟合
依赖项：pcl1.9
函数功能:fitPlane()为了验证最小二乘原理的正确性，设平面方程为x+2y+3z+4=0,假设有五个点分别为(-1,0,-1),(-2,-1,0),(0,1,-2),
(-4,3,-2),(3,-2,1)，根据五个点求解Ax+By+Cz+D=0
cloudPlane()输入点云实现平面参数与法线输出
注意事项：最小二乘对噪声敏感，请保证数据干净
*********************/

#include <iostream>
#include<string>
#include<stdexcept>

#include <Eigen/Dense>

#include<pcl/io/pcd_io.h>
#include<pcl/point_cloud.h>
#include<pcl/point_types.h>
#include <pcl/console/parse.h>
#include <pcl/filters/filter.h>
#include <pcl/filters/voxel_grid.h>
typedef pcl::PointXYZ pointT;
typedef pcl::PointCloud<pointT> cloud;
void fitPlane(const std::string & decomposition ="lu")
{
    //系数矩阵
    Eigen::Matrix<double, 5, 3> b_mat;
    b_mat(0, 0) = -1;
    b_mat(0, 1) = 0;
    b_mat(0, 2) = 1;

    b_mat(1, 0) = -2;
    b_mat(1, 1) = -1;
    b_mat(1, 2) = 1;

    b_mat(2, 0) = 0;
    b_mat(2, 1) = 1;
    b_mat(2, 2) = 1;

    b_mat(3, 0) = -4;
    b_mat(3, 1) = 3;
    b_mat(3, 2) = 1;

    b_mat(4, 0) = 3;
    b_mat(4, 1) = -2;
    b_mat(4, 2) = 1;
    std::cout <<"系数矩阵：\n"<< b_mat << std::endl;

    //列向量
    Eigen::Matrix<double, 5, 1> l_mat;
    l_mat(0, 0) = -1;
    l_mat(1, 0) = 0;
    l_mat(2, 0) = -2;
    l_mat(3, 0) = -2;
    l_mat(4, 0) = -1;
    std::cout << "列向量：\n" << l_mat << std::endl;
    Eigen::Matrix<double, 3, 1> a_mat;
    if (decomposition =="lu")
    {
        //lu分解求逆，对于大型矩阵加速。
        a_mat = ((b_mat.transpose() * b_mat)).lu().solve(b_mat.transpose() * l_mat);
    }
    else if (decomposition == "svd")
    {
        //SVD结果稳定特别是对于矩阵接近病态，但速度慢。
        a_mat = ((b_mat.transpose() * b_mat)).bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b_mat.transpose() * l_mat);
    }
    else
    {
        //直接解法
        a_mat = (b_mat.transpose() * b_mat).inverse() * b_mat.transpose() * l_mat;
    }

    std::cout << decomposition << "平面方程系数：a0,a1,a2:\n"<<a_mat << std::endl;
}
bool cloudPlane(const cloud::Ptr &cloud_origin, Eigen::Matrix<double, 3, 1> &aa_mat, const std::string& decomposition = "lu", const bool& downsample = false)
{

    //点云拟合平面
    cloud::Ptr cloud_in(new cloud);


    //************实例验证***********
    //cloud_in->points.push_back(pointT(-1, 0, -1));
    //cloud_in->points.push_back(pointT(-2, -1, 0));
    //cloud_in->points.push_back(pointT(0, 1, -2));
    //cloud_in->points.push_back(pointT(-4, 3, -2));
    //cloud_in->points.push_back(pointT(3, -2, -1));
    //cloud_in->height = 1;
    //cloud_in->width = 5;
    //cloud_in->resize(5);
    //************实例验证***********

    //去除nan点
    std::vector<int> indices;
    pcl::removeNaNFromPointCloud(*cloud_origin, *cloud_in, indices);

    //下采样，加快拟合速度，一般不需要这么多点，可选
    
    if (downsample)
    {
        pcl::VoxelGrid<pointT> sor;
        sor.setInputCloud(cloud_in);
        sor.setLeafSize(0.01f, 0.01f, 0.01f);
        sor.filter(*cloud_in);
    }
   
    //最小二乘平面拟合
    std::size_t cloud_size = cloud_in->points.size();
    std::cout << "方程个数："<<cloud_size << std::endl;
    try 
    {
        if (cloud_size < 3)//小于三个点无法计算
        {
            throw std::runtime_error("方程个数少于未知量个数，请检查数据");
            
        }
    }
    catch (std::runtime_error err)
    {
        std::cerr << err.what() << std::endl;
        return false;
    }
    Eigen::MatrixXd B;
    B.resize(cloud_size, 3);
    Eigen::MatrixXd L;
    L.resize(cloud_size, 1);

    for (size_t row = 0; row < cloud_size; ++row)
    {
        B(row, 0) = cloud_in->points[row].x;
        B(row, 1) = cloud_in->points[row].y;
        B(row, 2) = 1;
        L(row, 0) = cloud_in->points[row].z;
    }

    if (decomposition == "lu")
    {

        aa_mat = ((B.transpose() *B)).lu().solve ( B.transpose() * L);
    }
    else if (decomposition == "svd")
    {
        aa_mat = ((B.transpose() * B)).bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B.transpose() * L);
    }
    else
    {
        aa_mat = (B.transpose() * B).inverse() * B.transpose() * L;
    }

    std::cout <<decomposition<< "点云平面拟合结果\n" << aa_mat << std::endl;
    return true;
}

int main()
{
    std::cout << "***********一般最小二乘平面拟合实验：************\n";
    fitPlane("svd");


    std::cout << "**************点云平面拟合实验：**************\n";
    cloud::Ptr cloud_in(new cloud);
    Eigen::Matrix<double, 3, 1> aa_mat;

    if (pcl::io::loadPCDFile<pointT>("plane.pcd", *cloud_in) < 0)
    {
        PCL_ERROR("点云文件读取失败");
    }
    else
    {
        std::cout << "点云数量：" << cloud_in->size() << std::endl;
        if (cloudPlane(cloud_in, aa_mat, "lu"))
        {
            std::cout << "平面对应的法向量：\n"
                << -aa_mat(0, 0) << "  " << -aa_mat(1, 0) << "  " << 1 << std::endl;
        }

    }

    std::cout << "Hello World!\n";
}

5结果

可以发现：
fitPlane函数与假设值一致，验证了理论和程序的正确性。

对于cloudPlane函数计算的法向量结果与CC拟合得到的结果对比：可以发现法向量结果几乎一致，验证了程序的正确性。