ch11——11.3random_sample_consensus采样一致性

1.本节学习用random_sample_consensus类获得点云的拟合平面模型。
2.代码

#include <iostream>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <ctime>
using namespace std;
using namespace pcl;

boost::shared_ptr<visualization::PCLVisualizer> simpleVis(PointCloud<PointXYZ>::ConstPtr cloud) {
    //打开视窗并添加点云
	boost::shared_ptr<visualization::PCLVisualizer> viewer(new visualization::PCLVisualizer("3d viewer"));
	viewer->setBackgroundColor(0,0,0);
	viewer->addPointCloud<PointXYZ>(cloud,"sample cloud");
	viewer->setPointCloudRenderingProperties(visualization::PCL_VISUALIZER_FONT_SIZE,3,"sample cloud");
	viewer->initCameraParameters();
	return viewer;
}

int main() {
	srand(time(NULL));
	PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
	PointCloud<PointXYZ>::Ptr final(new PointCloud<PointXYZ>);
	cloud->width = 5000;
	cloud->height = 1;
	cloud->is_dense = false;
	cloud->points.resize(cloud->width*cloud->height);
	for (size_t i = 0; i < cloud->points.size(); i++)
	{
		//（1）用x²+y²+z²=1设置一部分点云数据，此时点云组成1/4个球体作为内点
		//cloud->points[i].x = rand() / (RAND_MAX + 1.0);
		//cloud->points[i].y = rand() / (RAND_MAX + 1.0);
		//if (i%5==0)
		//{
		//	cloud->points[i].z=rand()/ (RAND_MAX + 1.0);//局外点
		//}
		//else if (i % 2 == 0)
		//{
		//	cloud->points[i].z = sqrt(1-(cloud->points[i].x*cloud->points[i].x)-
		//	(cloud->points[i].y*cloud->points[i].y));
		//}
		//else
		//{
		//	cloud->points[i].z = -sqrt(1 - (cloud->points[i].x*cloud->points[i].x) -
		//		(cloud->points[i].y*cloud->points[i].y));
		//}
		
		// （2）用x+y+z=1设置一部分点云数据，此时点云组成菱形平面作为局内点
		cloud->points[i].x = rand() / (RAND_MAX + 1.0);
		cloud->points[i].y = rand() / (RAND_MAX + 1.0);
		if (i % 2 == 0)
		{
			cloud->points[i].z = rand() / (RAND_MAX + 1.0);//局外点
		}
		else
		{
			cloud->points[i].z = -1 * (cloud->points[i].x+ cloud->points[i].y);
		}

	}
	vector<int>inliers;//存储局内点集合的点的索引的向量
	//创建随机采样一致性对象
	   //针对球模型的对象
		SampleConsensusModelSphere<PointXYZ>::Ptr model_s(new SampleConsensusModelSphere<PointXYZ>(cloud));
		//针对平面模型的对象
		SampleConsensusModelPlane<PointXYZ>::Ptr model_p(new SampleConsensusModelPlane<PointXYZ>(cloud));
	//随机估算对应的平面模型，并存储估计的局内点
	{	RandomSampleConsensus<PointXYZ> ransac(model_p);
		ransac.setDistanceThreshold(0.01);//与平面距离小于0.01的点作为局内点考虑
		ransac.computeModel();//执行随机参数估计
		ransac.getInliers(inliers);//存储估计所得的局内点
	}

	 //随机估算对应的球面模型，并存储估计的局内点
	{ 	RandomSampleConsensus<PointXYZ> ransac(model_s);
		ransac.setDistanceThreshold(0.01);//与球面距离小于0.01的点作为局内点考虑
		ransac.computeModel();//执行随机参数估计
		ransac.getInliers(inliers);//存储估计所得的局内点
	}
	
	//复制估算模型所有局内点到final中
	copyPointCloud<PointXYZ>(*cloud,inliers,*final);
	//创建可视化对象，并加入原始点云或所有的局内点
	boost::shared_ptr<visualization::PCLVisualizer> viewer;
	//（3）viewer = simpleVis(final);
	//（4）viewer= simpleVis(cloud);
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}
	return 0;
}

3.显示
（2）（4）组合：噪声点立方体和菱形平面

（2）（3）组合：局内点菱形平面

（1）（4）组合：立方体和1/4圆球

（1）（3）组合：局内点1/4圆球