基于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>