pcl 最小二乘求平面点云法向量

求平面点云法向量实际上就是拟合平面ax+by+cz+d=0;

平面参数(a,b,c)便是平面点云法向量

一些相关最小二乘的过程的我写在了:pcl最小二乘求点云平面度-CSDN博客

void LS(pcl::PointCloud<PointT>::Ptr cloud) {
 
    Eigen::MatrixXf xyz_id = cloud->getMatrixXfMap(3, 4, 0);
 
    Eigen::Vector3f xyz_id_mean = xyz_id.rowwise().mean();                                 
    Eigen::MatrixXf xyz_id_centriod = xyz_id.colwise() - xyz_id_mean;
 
    Eigen::Vector3f xyz_all_Sum = xyz_id_centriod.rowwise().sum();                          //计算最小二乘法里矩阵A以及b的各元素值
    Eigen::Vector3f xyz_all_Pow2_Sum = xyz_id_centriod.array().square().rowwise().sum();
    auto xy_all_sum = xyz_id_centriod.row(0).dot(xyz_id_centriod.row(1).transpose());
    auto xz_all_sum = xyz_id_centriod.row(0).dot(xyz_id_centriod.row(2).transpose());
    auto yz_all_sum = xyz_id_centriod.row(1).dot(xyz_id_centriod.row(2).transpose());
 
    Eigen::Matrix3f A;
    A << xyz_all_Pow2_Sum(0), xy_all_sum, xyz_all_Sum(0),
        xy_all_sum, xyz_all_Pow2_Sum(1), xyz_all_Sum(1),
        xyz_all_Sum(0), xyz_all_Sum(1), cloud->size();
    Eigen::Vector3f B;
    B << xz_all_sum, yz_all_sum, xyz_all_Sum(2);
    Eigen::Vector3f X = A.lu().solve(B);                                                    
 
    cout<<"点云法向量是:"<<X(0)<<" "<< X(1)<<" "<< -1<<endl;
    return ;
}