PCL 最小二乘法拟合平面和RANSAC拟合平面

最小二乘法拟合平面

写这篇文章是网上很多一个拟合平面的方法都是什么VIP文章我真的是服气了，遂有了这篇文章，如果觉得有用请点赞收藏让更多的人看到。

这里使用全部点云使用最小二乘法拟合平面:

#include <iostream>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <Eigen/Dense>

// 直接使用全部点进行最小二乘平面拟合
void computeLeastSquaresPlaneDirectly(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, double& A, double& B, double& C, double& D) {
    if (cloud->empty()) {
        std::cerr << "点云数据为空！" << std::endl;
        return;
    }

    // 构建矩阵A和向量b
    Eigen::MatrixXd A_matrix(cloud->size(), 3);
    Eigen::VectorXd b_vector(cloud->size());

    for (size_t i = 0; i < cloud->size(); ++i) {
        const pcl::PointXYZ& point = cloud->points[i];
        A_matrix(i, 0) = point.x;
        A_matrix(i, 1) = point.y;
        A_matrix(i, 2) = 1.0;
        b_vector(i) = -point.z;
    }

    // 求解最小二乘问题
    Eigen::VectorXd solution = A_matrix.colPivHouseholderQr().solve(b_vector);

    // 提取平面方程的系数
    A = solution(0);
    B = solution(1);
    C = 1.0;
    D = solution(2);
}

RANSAC 拟合平面

使用ransac拟合平面要想达到一个很好的拟合效果，我们需要设置好距离阈值， 距离推荐设置为分辨率的3-4倍。

#include <iostream>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>

typedef pcl::PointXYZ PointT;

int calculateAverageDistance(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2)
{
    // 创建分割对象
    pcl::SACSegmentation<PointT> seg;
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());

    // 配置分割参数
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.2); // 根据点云密度调整阈值
    seg.setMaxIterations(1000);
    // 拟合平面
    seg.setInputCloud(cloud1);
    seg.segment(*inliers, *coefficients);

    if (inliers->indices.size() == 0)
    {
        PCL_ERROR("Could not estimate a planar model for the given dataset.");
        return -1;
    }

    //// 输出平面参数
    std::cerr << "Model coefficients: " << coefficients->values[0] << " "
        << coefficients->values[1] << " "
        << coefficients->values[2] << " "
        << coefficients->values[3] << std::endl;
}