点云随笔（三）中心线点云提取

       中心线点云提取主要是基于KD-TREE构建后，对每个检索点进行判断，判断其检索邻域内x方向最小值与检索点x值之差，及其检索邻域内x方向最大值与检索点x值之差，通过两个差异间差值是否小于阈值，y方向同理，两者皆满足的情况下该检索点即为中心点，然后依次检索所有点云数据即可。

#include <algorithm>  
#include <string>
#include <cstdlib>
#include <cstdio>
#include <Eigen\Dense>
#include <Eigen/Eigenvalues>
#include <map>
#include <math.h>
#include<list>
#include <pcl/io/ply_io.h> 
#include <pcl/surface/gp3.h>
#include <pcl/common/common.h>
#include <pcl/common/time.h>
#include <pcl/console/parse.h>
#include <pcl/common/transforms.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/normal_space.h>
#include <pcl/filters/project_inliers.h>  // 投影滤波 
#include <pcl/geometry/planar_polygon.h>//定义多边形
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/point_cloud.h>
#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/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/features/boundary.h>
#include <vector>


using Eigen::MatrixXd;
using Eigen::Matrix2d;
using namespace Eigen;
using namespace std;

typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloudT;

int main_line_extract() {
	pcl::PCDReader reader;
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>),
		cloud2(new pcl::PointCloud<pcl::PointXYZRGB>);
	reader.read("D:\\learning\\Roadminus.pcd", *cloud);
	std::cout << "The cloud size is: " << cloud->points.size() << " data points." << std::endl;


	pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;
	kdtree.setInputCloud(cloud);
	pcl::PointXYZRGB searchPoint;
	for (int i = 0; i < cloud->size(); i++)
	{
		searchPoint = cloud->at(i);
		float radius = 20;
		pcl::PointCloud<pcl::PointXYZRGB>::Ptr source_key_Neigh(new pcl::PointCloud<pcl::PointXYZRGB>()), savefile(new pcl::PointCloud<pcl::PointXYZRGB>());
		std::vector<int> pointIdxRadiusSearch;
		std::vector<float> pointRadiusSquaredDistance;
		if (kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0)
		{
			for (size_t j = 0; j < pointIdxRadiusSearch.size(); j++)
			{
				/*if (abs(cloud->points[pointIdxRadiusSearch[j]].z - searchPoint.z) < 10)*/
				source_key_Neigh->push_back(cloud->points[pointIdxRadiusSearch[j]]);
			}
		}

		//中心线求取
		pcl::PointXYZRGB minPt, maxPt;
		pcl::getMinMax3D(*source_key_Neigh, minPt, maxPt);
		double x1 = abs(minPt.x - searchPoint.x), x2 = abs(maxPt.x - searchPoint.x);
		double y1 = abs(minPt.y - searchPoint.y), y2 = abs(maxPt.y - searchPoint.y);
		if ((abs(x1 - x2) < 0.1)|| (abs(y1 - y2) < 0.1))
		{
			cloud2->push_back(cloud->at(i));
		}
	}

	cloud2->width = cloud2->size();
	cloud2->height = 1;
	cloud2->points.resize(cloud2->width * cloud2->height);

	pcl::io::savePCDFileASCII("D:\\learning\\Roadminus_CL.pcd", *cloud2);
	cout << "output cloud size is: " << cloud2->points.size() << " data points." << std::endl;
	return(0);

}