PCL中RANSAC使用，点云平面检测，显示，存储

#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <fstream>
#include <iosfwd>
#include <math.h>
#include <iomanip>  
#include "direct.h"
#include"stdlib.h"
#include "iostream"
#include "string"
using namespace std;

int
main(int argc, char** argv)
{
	// Read in the cloud data
	pcl::PCDReader reader;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>), cloud_f(new pcl::PointCloud<pcl::PointXYZ>);
	reader.read("1.pcd", *cloud);
	std::cout << "PointCloud before filtering has: " << cloud->points.size() << " data points." << std::endl; //*

	// Create the filtering object: downsample the dataset using a leaf size of 1cm
	pcl::VoxelGrid<pcl::PointXYZ> vg;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	vg.setInputCloud(cloud);
	vg.setLeafSize(0.01f, 0.01f, 0.01f);
	vg.filter(*cloud_filtered);
	std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size() << " data points." << std::endl; //*

	// Create the segmentation object for the planar model and set all the parameters
	pcl::SACSegmentation<pcl::PointXYZ> seg;
	pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
	pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane(new pcl::PointCloud<pcl::PointXYZ>());
	pcl::PCDWriter writer;
	seg.setOptimizeCoefficients(true);
	seg.setModelType(pcl::SACMODEL_PLANE);
	seg.setMethodType(pcl::SAC_RANSAC);
	seg.setMaxIterations(300);
	seg.setDistanceThreshold(0.5);

	int i = 0, nr_points = (int)cloud_filtered->points.size();
	std::stringstream ss;
	const char* filepath = "E:\\VS2013_projects\\myself\\play";
	mkdir(filepath);
	string s1 = filepath;

	while (cloud_filtered->points.size() > 0)
	{
		// Segment the largest planar component from the remaining cloud
		seg.setInputCloud(cloud_filtered);
		seg.segment(*inliers, *coefficients);
		if (inliers->indices.size() == 0)
		{
			std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
			break;
		}
		char szFileName[30] = { 0 };
		string filepath2;
		sprintf(szFileName, "OUTPUT_%06d.pcd", i);
		filepath2 = s1 + "\\" + szFileName;

		// Extract the planar inliers from the input cloud
		pcl::ExtractIndices<pcl::PointXYZ> extract;
		extract.setInputCloud(cloud_filtered);
		extract.setIndices(inliers);


		// Get the points associated with the planar surface
		extract.filter(*cloud_plane);

		if (cloud_plane->points.size()>0)
		{
			std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size() << " data points." << std::endl;

			pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud;
			colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared();
			std::vector<unsigned int> colors;
			colors.push_back(static_cast<unsigned int> (rand() % 256));
			colors.push_back(static_cast<unsigned int> (rand() % 256));
			colors.push_back(static_cast<unsigned int> (rand() % 256));

			colored_cloud->width = cloud_plane->width;
			colored_cloud->height = cloud_plane->height;

			colored_cloud->is_dense = cloud_plane->is_dense;
			for (size_t i_point = 0; i_point < cloud_plane->points.size(); i_point++)
			{
				pcl::PointXYZRGB point;
				point.x = *(cloud_plane->points[i_point].data);
				point.y = *(cloud_plane->points[i_point].data + 1);
				point.z = *(cloud_plane->points[i_point].data + 2);
				point.r = colors[0];
				point.g = colors[1];
				point.b = colors[2];
				colored_cloud->points.push_back(point);
			}
			pcl::io::savePCDFileASCII(filepath2, *colored_cloud);
			++i;
		}
		// Remove the planar inliers, extract the rest
		extract.setNegative(true);
		extract.filter(*cloud_f);
		*cloud_filtered = *cloud_f;
	}


}