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、存在问题
直接使用点云库虽然方便,但是存在以下两个问题:
- 平面拟合时,虽然速度很快,但不一定能够找到最优平面,迭代就已经收敛退出了,这在实际对速度没有需求的时候,是不利的。
- 显示平面时,不能控制平面的大小与位置,可能达不到想要的可视化效果。
后续文章将说明如何尽量优化这两个问题。