PCL拟合并绘制平面(一)

1、直接使用PCL点云库拟合并绘制平面

PCL（Point Cloud Library）是一个开源的用于点云处理的C++库。要在PCL中绘制平面，可以使用PCL的可视化模块来实现。以下是一个简单的示例代码，演示如何加载点云数据并绘制检测到的平面：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/features/normal_3d.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>

int main(int argc, char** argv)
{
    //导入数据
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PCDReader reader;
    reader.read<pcl::PointXYZ>("input_cloud.pcd", *cloud);

    //法线估计
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    pcl::PointCloud<pcl::Normal>::Ptr cloud_normals(new pcl::PointCloud<pcl::Normal>);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
    ne.setInputCloud(cloud);
    ne.setSearchMethod(tree);
    ne.setKSearch(50);
    ne.compute(*cloud_normals);

    //平面分割
    pcl::SACSegmentationFromNormals<pcl::PointXYZ, pcl::Normal> seg;
    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_NORMAL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setMaxIterations(100);
    seg.setDistanceThreshold(0.02);  //如果点云单位是mm，建议设置为1.0
    seg.setInputCloud(cloud);
    seg.setInputNormals(cloud_normals);
    seg.segment(*inliers, *coefficients);

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

    //平面显示
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "input_cloud");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "input_cloud");
    viewer->addPlane(*coefficients, "plane");
    viewer->spin();

    return (0);
}

当然也可以不不使用法线，直接拟合平面：

pcl::SACSegmentation<pcl::PointXYZ> seg;				
seg.setOptimizeCoefficients(true);						
seg.setModelType(Model);								
seg.setMethodType(pcl::SAC_RANSAC);						
seg.setMaxIterations(1000);								
seg.setDistanceThreshold(0.02);       //如果点云单位是mm，建议设置为1.0
seg.setInputCloud(cloud);
seg.segment(*inliers, *coefficients);

2、存在问题

直接使用点云库虽然方便，但是存在以下两个问题：

1. 平面拟合时，虽然速度很快，但不一定能够找到最优平面，迭代就已经收敛退出了，这在实际对速度没有需求的时候，是不利的。
2. 显示平面时，不能控制平面的大小与位置，可能达不到想要的可视化效果。

后续文章将说明如何尽量优化这两个问题。