从点云中识别拟合平面

基于RANSAC算法从PCL点云中识别、拟合平面

拟合出平面方程： ax+by+cz+d=0

该函数依次打印a、b、c、d、拟合平面的点的数量

该平面的法向量为（a，b，c）

std::vector<Eigen::VectorXd> PlaneFitterRANSAC(pcl::PointCloud<pcl::PointXYZ>::Ptr &input_cloud) {
    pcl::PointCloud<pcl::PointXYZ>::Ptr plane_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);

    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
    pcl::SACSegmentation<pcl::PointXYZ> segmentation;
    segmentation.setOptimizeCoefficients(true);
    segmentation.setModelType(pcl::SACMODEL_PLANE);
    segmentation.setMethodType(pcl::SAC_RANSAC);
    segmentation.setDistanceThreshold(30);
    pcl::ExtractIndices<pcl::PointXYZ> extract_index;
    size_t input_point_size = input_cloud->points.size();

    std::vector<Eigen::VectorXd> plane_coefficients_buffer;
    while (input_cloud->points.size() > input_point_size / 10) {
        segmentation.setInputCloud(input_cloud);
        segmentation.segment(*inliers, *coefficients);
        if (inliers->indices.empty()) {
            std::cout << "[Plane Fitter] No plane! " << std::endl;
            break;
        }
        extract_index.setInputCloud(input_cloud);
        extract_index.setIndices(inliers);
        extract_index.setNegative(false);
        extract_index.filter(*plane_cloud);
        size_t plane_point_size = plane_cloud->points.size();
        cout << "[Plane Fitter] Coefficients: "
             << coefficients->values[0] << " " << coefficients->values[1] << " "
             << coefficients->values[2] << " " << coefficients->values[3] << " Size: "
             << plane_point_size << endl;

        Eigen::VectorXd plane_coefficients = Eigen::VectorXd::Zero(5);
        plane_coefficients(0) = coefficients->values[0];
        plane_coefficients(1) = coefficients->values[1];
        plane_coefficients(2) = coefficients->values[2];
        plane_coefficients(3) = coefficients->values[3];
        plane_coefficients(4) = (double) plane_point_size;
        plane_coefficients_buffer.push_back(plane_coefficients);

        extract_index.setNegative(true);
        extract_index.filter(*filtered_cloud);
        input_cloud.swap(filtered_cloud);
    }
    return plane_coefficients_buffer;
}

 头文件：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Eigen>