- 这是一个demo
- 利用PCL提供的接口,从一个自己定义的点云中分割出一个平面
- 先导入头文件
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
- 先定义两个点云 分别是 in 和 out
- 其中in点云我们进行一个初始化,它里面的点基本上都在一个平面上 0x + 0y + 1z - 1 = 0 上
- 只有三个我们自己设置的离群值。可视化一下。
![在这里插入图片描述](https://i-blog.csdnimg.cn/blog_migrate/98e25b38d0f8756b716e236ac0285f1a.png)
![有三个离群值](https://i-blog.csdnimg.cn/blog_migrate/f81e09ae360428477d83c4c0002f906e.png)
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_in_ptr(new pcl::PointCloud<pcl::PointXYZ>);
point_cloud_in_ptr->width = 30;
point_cloud_in_ptr->height = 1;
point_cloud_in_ptr->resize(point_cloud_in_ptr->width * point_cloud_in_ptr->height);
for (size_t i = 0; i < point_cloud_in_ptr->size(); i++) {
point_cloud_in_ptr->points[i].x = 10 * rand() / (RAND_MAX + 1.0f);
point_cloud_in_ptr->points[i].y = 10 * rand() / (RAND_MAX + 1.0f);
point_cloud_in_ptr->points[i].z = 1.0;
}
point_cloud_in_ptr->points[0].z = 0.0;
point_cloud_in_ptr->points[1].z = -2.0;
point_cloud_in_ptr->points[2].z = 3.0;
- 创建两个对象 一个是分割所用的模型对象 一个是分割后所得的那些点的索引对象
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
- 创建分割对象 基于采样一致性的算法(SAmple Consensus)
- pcl::SACSegmentationpcl::PointXYZ seg;
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.1);
seg.setInputCloud(point_cloud_in_ptr);
seg.segment(*inliers, *coefficients);
std::cerr << "Model coefficients: " << coefficients->values[0] << " "
<< coefficients->values[1] << " "
<< coefficients->values[2] << " "
<< coefficients->values[3] << " "
<< coefficients->header <<std::endl;
- Model coefficients: 0 0 1 -1 seq: 0 stamp: 0 frame_id:
- Model inliers: 27
- 模型的平面方程是 0x+0y+1z-1 = 0 得出的结果是27个点的索引, 也就是排除了3个离群点
![在这里插入图片描述](https://i-blog.csdnimg.cn/blog_migrate/303ed896206caf2b334165e5247d96af.png)
![在这里插入图片描述](https://i-blog.csdnimg.cn/blog_migrate/ed0bd777a881b68c5bed4e51a2b80867.png)
- 全部代码
#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/visualization/pcl_visualizer.h>
int main() {
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_in_ptr(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_out_ptr(new pcl::PointCloud<pcl::PointXYZ>);
point_cloud_in_ptr->width = 30;
point_cloud_in_ptr->height = 1;
point_cloud_in_ptr->resize(point_cloud_in_ptr->width * point_cloud_in_ptr->height);
for (size_t i = 0; i < point_cloud_in_ptr->size(); i++) {
point_cloud_in_ptr->points[i].x = 10 * rand() / (RAND_MAX + 1.0f);
point_cloud_in_ptr->points[i].y = 10 * rand() / (RAND_MAX + 1.0f);
point_cloud_in_ptr->points[i].z = 1.0;
}
point_cloud_in_ptr->points[0].z = 0.0;
point_cloud_in_ptr->points[1].z = -2.0;
point_cloud_in_ptr->points[2].z = 3.0;
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.1);
seg.setInputCloud(point_cloud_in_ptr);
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] << " "
<< coefficients->header <<std::endl;
std::cerr << "Model inliers: " << inliers->indices.size() << std::endl;
for (size_t i = 0; i < inliers->indices.size(); ++i) {
std::cerr << inliers->indices[i] << " " << point_cloud_in_ptr->points[inliers->indices[i]].x << " "
<< point_cloud_in_ptr->points[inliers->indices[i]].y << " "
<< point_cloud_in_ptr->points[inliers->indices[i]].z << std::endl;
point_cloud_out_ptr->push_back(point_cloud_in_ptr->points[inliers->indices[i]]);
}
std::cout << point_cloud_out_ptr->size() << std::endl;
pcl::visualization::PCLVisualizer viewer("v1");
viewer.addPointCloud(point_cloud_in_ptr, "in");
viewer.addPointCloud(point_cloud_out_ptr, "out");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,255,255,255, "in");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "in");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 255, 0, 0, "out");
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "out");
viewer.addCoordinateSystem(2.0);
viewer.spin();
}