PCL采样一致性算法

版权声明：本文为博主原创文章，未经博主允许不得转载。 https://blog.csdn.net/u014801811/article/details/79995572

代码出处：
http://pointclouds.org/documentation/tutorials/random_sample_consensus.php#random-sample-consensus

**算法介绍：**https://www.cnblogs.com/li-yao7758258/p/6477007.html
直接记录代码，供后续参考学习。

/** Filename:  random_sample_consensus.cpp

** Date: 2018-4-18

**Description:

**/
#include "stdafx.h"
#include <iostream>
#include <pcl\console\parse.h>
#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>

boost::shared_ptr<pcl::visualization::PCLVisualizer> simpleVis(pcl::PointCloud<pcl::PointXYZ>::ConstPtr cloud,int v)
{
	// -----------------------------------------------
	// ---------Open 3D viewer and add point cloud ---
	// -----------------------------------------------
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0,v);//black 
	viewer->addPointCloud(cloud,"sample cloud",v);
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud",v);
	viewer->addCoordinateSystem(1000.0, "global",v);
	viewer->initCameraParameters();
	return viewer;
}


int main(int argc, char* argv[])
{
	// 初始化点云
	pcl::PointCloud<pcl::PointXYZ> ::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);

	//填充点云数据
	cloud->width =5000;
	cloud->height = 1;
	cloud->is_dense = false;
	cloud->resize(cloud->width * cloud->height);
	for (size_t i = 0; i < cloud->points.size(); i++)
	{
		if (pcl::console::find_argument(argc, argv, "-s") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
		{
			printf("随机生成 s or sf \n");
			cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0f);
			cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0f);
			if (i % 5 == 0)
				cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0f);
			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));
		}
		else
		{
			printf("others \n");
			cloud->points[i].x = 1024 * rand() / (RAND_MAX + 1.0);
			cloud->points[i].y = 1024 * rand() / (RAND_MAX + 1.0);
			if (i % 3 == 0)
				cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
			else
				cloud->points[i].z = 1024 * rand() / (RAND_MAX + 1.0);
		}
	}
	
	//for (size_t i = 0; i < cloud->points.size(); i++)
	//{
	//	printf("点云数据：\n");
	//	printf("%6.3f %6.3f %6.3f\n", cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
	//}
	printf("\n\n 点云大小为: %d \n", cloud->points.size());

	std::vector<int> inliers; //有效数据
	//创建 RANSAC 对象 并计算合适模型
	pcl::SampleConsensusModelSphere<pcl::PointXYZ> ::Ptr
		model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
		model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));

	//平面采样一致性
	if (pcl::console::find_argument(argc, argv, "-f") >= 0)
	{
		pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
		ransac.setDistanceThreshold(.01);
		//ransac.setMaxIterations(30);
		ransac.computeModel();
		ransac.getInliers(inliers);
	}
	//球面采样一致性
	else if (pcl::console::find_argument(argc,argv,"-sf")>=0)
	{
		pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
		ransac.setDistanceThreshold(.01);
		//ransac.setMaxIterations(30);
		ransac.computeModel();
		ransac.getInliers(inliers);
	}
	std::cout << "采样后点云数据大小11：" << inliers.size()<< std::endl;
	//将所有计算有效点云数据复制到另一个点云
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
	std::cout << "采样后点云数据大小：" << final->points.size() << std::endl;
	//创建可视化对象，
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D viewer"));
	int v1(0);
	int v2(1);
	viewer->createViewPort(0, 0, 0.5, 1, v1);
	viewer->createViewPort(0.5, 0, 1, 1, v2);
	if (pcl::console::find_argument(argc, argv, "-f") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0)
	{
		//viewer = simpleVis(cloud, v1);
		//viewer = simpleVis(final, v2);
		std::cout << "调用-f || -sf："  << std::endl;
		viewer->setBackgroundColor(0, 0, 0, v1);//black 
		viewer->addPointCloud(cloud, "sample cloud", v1);
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud", v1);
		viewer->addCoordinateSystem(1000.0, "global", v1);

		viewer->setBackgroundColor(0, 0, 0, v2);//black 
		viewer->addPointCloud(final, "Final cloud", v2);
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Final cloud", v2);
		viewer->addCoordinateSystem(1000.0, "global_v2", v2);
		//viewer->initCameraParameters();
	}
	else
	{
		//viewer = simpleVis(cloud, v1);
		//viewer = simpleVis(cloud, v2);
		std::cout << "调用qita" << std::endl;
		viewer->setBackgroundColor(0, 0, 0, v1);//black 
		viewer->addPointCloud(cloud, "sample cloud", v1);
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud", v1);
		viewer->addCoordinateSystem(1000.0, "global", v1);

		viewer->setBackgroundColor(0, 0, 0, v2);//black 
		viewer->addPointCloud(cloud, "Final cloud", v2);
		viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "Final cloud", v2);
		viewer->addCoordinateSystem(1000.0, "global_v2", v2);
		//viewer->initCameraParameters();
	}
	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	system("pause");
	return 0;
}