备份：获取dbh

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/sac_model_circle.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/ModelCoefficients.h>
#include <vector>

typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PCLPointT;

//struct save_per_ciecle
//{
//	float x_point, y_point, x_y_rad;
//};
//
//struct save_circles
//{
//	double x, y, z;
//};



int main(int argc, char **argv)
{
	///load datas.
	pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
	pcl::PCDReader reader_pcd;
	pcl::PCDWriter writer_pcd;
	reader_pcd.read(argv[1], *cloud);
	std::cerr << "loading " << cloud->points.size() << "datas." << std::endl;

	///passthrough filter.
	pcl::PointCloud<PointT>::Ptr cloud_filtered(new pcl::PointCloud<PointT>);
	pcl::PassThrough<PointT> pass_filter;
	pass_filter.setInputCloud(cloud);
	pass_filter.setFilterFieldName("z");
	pass_filter.setFilterLimits(1.28, 1.32);
	pass_filter.setFilterLimitsNegative(false);
	pass_filter.filter(*cloud_filtered);
	std::cerr << "there are " << cloud_filtered->points.size() << " datas." << std::endl;

	writer_pcd.write("cloud_filtered.pcd", *cloud_filtered);


	/*std::vector<int> m;*/
	//std::vector<save_per_ciecle> coefficients_circles;
	std::vector<std::vector<pcl::PointXYZ>> cloud_circles;
	pcl::PointCloud<PointT>::Ptr coefficients_circles(new pcl::PointCloud<PointT>);
	pcl::PointCloud<pcl::PointXYZI>::Ptr save_cloud_circle_i(new pcl::PointCloud<pcl::PointXYZI>);
	pcl::PointCloud<pcl::PointXYZI> save_circle;
	int couti = 0;
	//pcl::PointCloud<PointT>::Ptr cloud_circles(new pcl::PointCloud<PointT>);
	//pcl::PointCloud<PointT>::Ptr cloud_circle(new pcl::PointCloud<PointT>);

	///拟合圆面。
	pcl::ModelCoefficients::Ptr coefficients_circle(new pcl::ModelCoefficients);
	pcl::PointIndices::Ptr inliers_circle(new pcl::PointIndices);
	// Create the segmentation object
	pcl::SACSegmentation<PointT> seg_circle;
	seg_circle.setOptimizeCoefficients(true);
	seg_circle.setModelType(pcl::SACMODEL_CIRCLE2D);
	seg_circle.setMethodType(pcl::SAC_RANSAC);
	seg_circle.setDistanceThreshold(0.15);  0.15
	seg_circle.setRadiusLimits(3, 15);
	seg_circle.setDistanceThreshold(0.05);
	seg_circle.setMaxIterations(100);
	int i = 0;

	do
	{
		seg_circle.setInputCloud(cloud_filtered);
		seg_circle.segment(*inliers_circle, *coefficients_circle);
		
		///获取圆心（单木位置）与半径
		PointT save_per;
		save_per.x = coefficients_circle->values[0];
		save_per.y = coefficients_circle->values[1];
		save_per.z = coefficients_circle->values[2];
		coefficients_circles->push_back(save_per);


		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_circle(new pcl::PointCloud<pcl::PointXYZ>);
		pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_circle_i(new pcl::PointCloud<pcl::PointXYZI>);
		pcl::ExtractIndices<PointT> extract_circle;      //点提取对象
		extract_circle.setInputCloud(cloud_filtered);
		extract_circle.setIndices(inliers_circle);
		extract_circle.setNegative(false);
		extract_circle.filter(*cloud_circle);
		pcl::copyPointCloud(*cloud_circle, *cloud_circle_i);
		for (size_t j = 0; j < cloud_circle_i->points.size(); j++)
		{
			cloud_circle_i->points[j].intensity = static_cast<float>(i);
		}
		*save_cloud_circle_i = *save_cloud_circle_i + *cloud_circle_i;
		cloud_circle_i->clear();

		extract_circle.setNegative(true);
		extract_circle.filter(*cloud_filtered);
		i++;
	} while (cloud_filtered->points.size()>50);

	
	writer_pcd.write("coefficients_circles.pcd", *coefficients_circles);

	writer_pcd.write("save_cloud_circle_i.pcd", *save_cloud_circle_i);
	
	system("pause");

	return (0);
}