myRegionGrowing

2.3 自己编写计算法向量方法

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>//标准C++库中的输入输出类相关头文件。
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h> //PCL中支持的点类型头文件。
#include <pcl/octree/octree.h>
#include<fstream>  
#include <string>  
#include <vector> 
//#include <LasOperator.h> 
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/common/pca.h>

using namespace std;

void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZ>& cloud) {
	std::ifstream ifs(s, std::ios::in | std::ios::binary); // 打开las文件
	liblas::ReaderFactory f;
	liblas::Reader reader = f.CreateWithStream(ifs); // 读取las文件

	liblas::Header const& header = reader.GetHeader();

	cloud.width = header.GetPointRecordsCount();	//保证与las数据点的个数一致	
	cloud.height = 1;
	cloud.is_dense = false;
	cloud.points.resize(cloud.width * cloud.height);

	int i = 0;
	while (reader.ReadNextPoint()) {
		liblas::Point const& p = reader.GetPoint();
		// 获取las数据的x，y，z信息
		cloud.points[i].x = (p.GetX());
		cloud.points[i].y = (p.GetY());
		cloud.points[i].z = (p.GetZ());

		i++;
	}
	ifs.close();
}

void saveLasFile(string s, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
	cout << cloud->points.size() << endl;
	std::ofstream ofs(s, ios::out | ios::binary);

	liblas::Header header;
	header.SetScale(0.0001, 0.0001, 0.0001);
	header.SetPointRecordsCount(cloud->points.size());

	// fill other header members
	// here the header has been serialized to disk into the *file.las*
	liblas::Writer writer(ofs, header);
	liblas::Point point(&header);

	// fill other properties of point record
	for (int i = 0;i < cloud->points.size();++i) {
		point.SetCoordinates(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
		writer.WritePoint(point);
	}
	writer.SetHeader(header);
	ofs.flush();
	ofs.close();
}

float computeRange(pcl::PointCloud<pcl::PointXYZ>& trail, float r, int index, int k) {
	float x1, y1, x2, y2, a1, a2, a3, b1, b2, b3;//向量1，a,b为轨迹线上2点
	x2 = 0; y2 = 1;
	a1 = trail.points[index].x;
	a2 = trail.points[index].y;
	a3 = trail.points[index].z;
	b1 = trail.points[index + k].x;
	b2 = trail.points[index + k].y;
	b3 = trail.points[index + k].z;
	x1 = a1 - b1;
	y1 = a2 - b2;
	float cosa = (x1 * x2 + y1 * y2) / (sqrt(x1 * x1 + y1 * y1) + sqrt(x2 * x2 + y2 * y2));
	float sina = sqrt(1 - cosa * cosa);
	float range = r / sina;

	//float radian = acos(cosa);
	//float angle = radian * 180.0 / M_PI;
	//if (angle > 90)
	//	angle = 180 - angle;
	//cout << "angle: " << angle << "\t";

	return range;
}

void cutMidle(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr trail, float r, int k, pcl::PointCloud<pcl::PointXYZ>::Ptr& midle) {
	vector<int> idx;
	vector<int> midle_idx;
	//cloud构建八叉树
	cout << "构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();
	cout << "构建完成" << endl;

	/**
		切割宽于路面部分
	*/
	cout << "切割宽于路面部分" << endl;
	for (size_t i = 0;i + k < trail->points.size(); i += k) {
		//for (size_t i = 8000;i<8500; i += k) {
		//cout << "-------------------------------------------------------------" << endl;
		float range = computeRange(*trail, r, i, k);
		float center = trail->points[i].x;
		float Ymin = min(trail->points[i].y, trail->points[i + k].y);
		float Ymax = max(trail->points[i].y, trail->points[i + k].y);
		float Zmin = -10;
		float Zmax = max(trail->points[i].z, trail->points[i + k].z);
		//cout << "range: " << range << endl;

		//分段切割种子点
		Eigen::Vector3f Emin(center - range, Ymin, Zmin);//0xc0c0c0c0	0x3f3f3f3f
		Eigen::Vector3f Emax(center + range, Ymax, Zmax);
		octree.boxSearch(Emin, Emax, idx);
		//cout << i << "\t这段种子数：\t" << idx.size() << endl;
		cout << i << endl;
		midle_idx.insert(midle_idx.end(), idx.begin(), idx.end());
	}

	//提取midle
	boost::shared_ptr<std::vector<int>> midle_ptr = boost::make_shared<std::vector<int>>(midle_idx);
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(midle_ptr);
	extract.setNegative(false);//如果设为true,可以提取指定index之外的点云
	extract.filter(*midle);
}

void filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float radius, int neighbors, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered) {
	//去杂点
	pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;
	ror.setInputCloud(cloud);
	ror.setRadiusSearch(radius);
	ror.setMinNeighborsInRadius(neighbors);
	ror.filter(*cloud_filtered);
}

float comupteNormalAngle(Eigen::Vector4f p, Eigen::Vector4f q) {
	float cos = (p[0] * q[0] + p[1] * q[1] + p[2] * q[2]) / (sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]) * sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]));

	float radian = acos(cos);
	float angle = radian * 180.0 / M_PI;
	//cout << "原来角度：" << angle << endl;
	if (angle > 90)
		angle = 180 - angle;
	//cout << "cos: " << cos << "\tradian: " << radian << endl;
	return angle;
}

int computeK(pcl::PointCloud<pcl::PointXYZ>::Ptr trail, int begin) {
	float d;
	for (int i = begin;i < trail->points.size();++i) {
		d = abs(trail->points[i].y - trail->points[begin].y);
		if (d > 0.3) {
			cout << "k为： " << i - begin << "y距离" << trail->points[i].y - trail->points[begin].y << endl;
			return i - begin;
		}
	}
	return -1;
}

void computeNormal(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::Vector4f& normal) {
	pcl::PointXYZ tempPnt;
	Eigen::Vector4f centroid;
	pcl::compute3DCentroid(*cloud, centroid);
	/*for (int i = 0;i < centroid.size();++i) {
		cout << centroid[i] << endl;
	}*/

	tempPnt.x = centroid[0];
	tempPnt.y = centroid[1];
	tempPnt.z = centroid[2];
	cloud->push_back(tempPnt);
	//cout << cloud->points.size() << endl;

	//求法线
	pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);
	pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
	pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
	ne.setSearchMethod(tree);
	ne.setInputCloud(cloud);
	//ne.setRadiusSearch(0.4);
	ne.setKSearch(cloud->points.size() - 1);
	//ne.setKSearch(30);
	ne.compute(*normals);
	int count = normals->points.size();
	normal[0] = normals->points[count - 1].normal_x;
	normal[1] = normals->points[count - 1].normal_y;
	normal[2] = normals->points[count - 1].normal_z;
	//pcl::io::savePCDFileASCII("0normal.pcd", *normals);
	//cout << "save successful" << endl;
}

void computeNormalPca(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::Vector4f& normal) {

	pcl::PointXYZ tempPnt;
	Eigen::Vector4f centroid;
	pcl::compute3DCentroid(*cloud, centroid);
	//for (int i = 0;i < centroid.size();++i) {
	//	cout << centroid[i] << endl;
	//}

	tempPnt.x = centroid[0];
	tempPnt.y = centroid[1];
	tempPnt.z = centroid[2];

	cloud->push_back(tempPnt);

	pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);
	tree->setInputCloud(cloud);
	//cout << "111" << endl;
	//for (auto& pt : cloud->points) {
	std::vector<int> k_indices;
	std::vector<float> k_sqr_distances;

	/*if (tree->radiusSearch(pt, 5, k_indices, k_sqr_distances) < 3) {
		continue;
	}*/
	/*if (tree->nearestKSearch(pt, input_cloud->points.size(), k_indices, k_sqr_distances) < 3) {
		continue;
	}*/
	tree->nearestKSearch(tempPnt, cloud->points.size() - 1, k_indices, k_sqr_distances);
	cout << "k_indices大小：" << k_indices.size() << endl;

	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_search(new pcl::PointCloud<pcl::PointXYZ>);

	pcl::copyPointCloud(*(tree->getInputCloud()), k_indices, *cloud_search);
	pcl::PCA<pcl::PointXYZ> pca;
	pca.setInputCloud(cloud_search);
	Eigen::Matrix3f eigen_vector = pca.getEigenVectors();
	//cout << "333" << endl;
	Eigen::Vector3f vect_2 = eigen_vector.col(2);// please fit to your own coordinate
	/*tempPnt.normal_x = vect_2[0];
	tempPnt.normal_y = vect_2[1];
	tempPnt.normal_z = vect_2[2];*/

	normal[0] = vect_2[0];
	normal[1] = vect_2[1];
	normal[2] = vect_2[2];

	//}

}

int main() {
	//输入点云，轨迹线，保存种子
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr trail(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr midle(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr midle_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr read(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointXYZ tempPnt;
	std::vector<int>idx;
	std::vector<int>idx1;
	//变量

	Eigen::Vector4f n;
	Eigen::Vector4f m;
	Eigen::Vector4f m1;
	std::vector<int>result_idx;
	float k1, k2, k3, t1, t2, t3;
	k1 = k2 = k3 = t1 = t2 = t3 = 0;


	float r = 8;//10
	int k = 6;//7
	float d = 0.3;//0.2  0.4
	float Z_range = 0.3;
	float angle_Threshold = 50;//角度
	float p_curvature = 1;

	//读取输入点云
	cout << "读取点云" << endl;
	loadLasFile("o1.las", *input_cloud);
	//pcl::io::loadPCDFile("o.pcd", *input_cloud);
	//读取轨迹线
	cout << "读取轨迹线" << endl;
	//pcl::io::loadPCDFile("trail1.pcd", *trail);
	loadLasFile("trail1.las", *trail);
	cout << "原始点数: " << input_cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;



	//切割宽于中间路面得midle
	cutMidle(input_cloud, trail, r, k, midle);

	//滤波去杂点
	filter(midle, 0.02, 2, midle_filtered);//0.03  2

	//cout << "读取midle" << endl;
	//pcl::io::loadPCDFile("trail.pcd", *trail);

	//cloud构建八叉树
	cout << "cloud构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	octree.setInputCloud(midle_filtered);
	octree.addPointsFromInputCloud();
	cout << "过滤后点数" << midle_filtered->points.size() << endl;

	/**
		开始循环
	*/
	int begin = 8432;
	/*k = computeK(trail, begin);
	cout << "k为： " << k << endl;;
	cout << "区域生长" << endl;*/
	//for (size_t i = 0; i + k <= trail->points.size(); i += k) {//cloud1->points.size()		cloud1->points.size()+500   500 i是down，i+k是up
	for (size_t i = 0;i < 9000 && i != -1; i += k) {//8600, 9000  end 9647		 i + k <= trail->points.size()   9300
	//for (size_t i = 8000;i < 8493; i += k) {
		cout << "-------------------------------------------------------------" << endl;
		cout << i << endl;
		float center = trail->points[i].x;
		float range = computeRange(*trail, r, i, k);
		//cout << "range: " << range << endl;
		float left = center - d / 2;
		float right = center + d / 2;
		float Ymin = min(trail->points[i].y, trail->points[i + k].y);
		float Ymax = max(trail->points[i].y, trail->points[i + k].y);
		float Zmin = 0;
		float Zmax = max(trail->points[i].z, trail->points[i + k].z);

		//切割中间的种子区域	中间
		Eigen::Vector3f Emin(left, Ymin, Zmin);
		Eigen::Vector3f Emax(right, Ymax, Zmax);
		octree.boxSearch(Emin, Emax, idx);
		
		result_idx.insert(result_idx.end(), idx.begin(), idx.end());
		//cout << midle_filtered->points[idx[0]].z << endl;
		float height = midle_filtered->points[idx[0]].z;

		//求中间法线
		//pcl::computePointNormal(*midle_filtered, idx, n, p_curvature);
		pcl::copyPointCloud(*midle_filtered, idx, *temp_cloud);

		//computeNormal(temp_cloud,n);
		computeNormalPca(temp_cloud, n);

		cout << "Xrange" << center-range << "\t" << center+range << endl;
		cout << "Yrange" << Ymin << "\t" << Ymax << endl;
		cout << "Zrange" << Zmin << "\t" << Zmax << endl;
		cout << "中间法向量" << n[0] << "\t" << n[1] << "\t" << n[2] << endl;

		m = n;
		bool flag = true;//继续循环
		while (flag) {//left
			cout << "-------------------------------------------------------------" << endl;
			right -= d;
			left -= d;
			if (left <= center - range) {
				flag = false;
				left = center - range;
			}

			cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			cout << "上一格法向量" << m[0] << "\t" << m[1] << "\t" << m[2] << endl;

			Eigen::Vector3f Emin1(left, Ymin, Zmin);
			Eigen::Vector3f Emax1(right, Ymax, Zmax);
			octree.boxSearch(Emin1, Emax1, idx);

			if (idx.size() < 3)
				continue;

			//求法线
			/*pcl::computePointNormal(*midle_filtered, idx, m1, p_curvature);
			cout << "点数" << idx.size() << "p_curvature:	" << p_curvature << endl;
			cout << "1当前法向量" << m1[0] << "\t" << m1[1] << "\t" << m1[2] << endl;*/

			pcl::copyPointCloud(*midle_filtered, idx, *temp_cloud);

			//computeNormal(temp_cloud, m1);
			computeNormalPca(temp_cloud, m1);
			cout << "2当前法向量" << m1[0] << "\t" << m1[1] << "\t" << m1[2] << endl;

			float angle = comupteNormalAngle(m, m1);
			//angle = min(angle,temp_angle);
			m = m1;
			cout << "角度是： " << angle << endl;

			if (angle > angle_Threshold) {//2.1
				flag = false;
			}
			else {
				for (int j = 0;j < idx.size();++j) {
					float kkk = abs(midle_filtered->points[idx[j]].z - height);

					if (kkk < Z_range) {//0.2
						idx1.push_back(idx[j]);
					}
				}
				result_idx.insert(result_idx.end(), idx1.begin(), idx1.end());
				idx1.clear();
			}

		}//left


		flag = true;
		left = center - d / 2;
		right = center + d / 2;
		m = n;

		while (flag) {//right
			cout << "-------------------------------------------------------------" << endl;
			right += d;
			left += d;
			if (right >= center + range) {
				flag = false;
				right = center + range;
			}

			cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			cout << "上一格法向量" << m[0] << "\t" << m[1] << "\t" << m[2] << endl;

			Eigen::Vector3f Emin1(left, Ymin, Zmin);
			Eigen::Vector3f Emax1(right, Ymax, Zmax);
			octree.boxSearch(Emin1, Emax1, idx);

			if (idx.size() < 3)
				continue;

			//求法线
			/*pcl::computePointNormal(*midle_filtered, idx, m1, p_curvature);
			cout << "点数" << idx.size() << "p_curvature:	" << p_curvature << endl;
			cout << "1当前法向量" << m1[0] << "\t" << m1[1] << "\t" << m1[2] << endl;*/

			pcl::copyPointCloud(*midle_filtered, idx, *temp_cloud);

			//computeNormal(temp_cloud, m1);
			computeNormalPca(temp_cloud, m1);
			cout << "2当前法向量" << m1[0] << "\t" << m1[1] << "\t" << m1[2] << endl;

			float angle = comupteNormalAngle(m, m1);
			//angle = min(angle,temp_angle);
			m = m1;
			cout << "角度是： " << angle << endl;

			if (angle > angle_Threshold) {//2.1
				flag = false;
			}
			else {
				for (int j = 0;j < idx.size();++j) {
					float kkk = abs(midle_filtered->points[idx[j]].z - height);

					if (kkk < Z_range) {//0.2
						idx1.push_back(idx[j]);
					}
				}
				result_idx.insert(result_idx.end(), idx1.begin(), idx1.end());
				idx1.clear();
			}

		}//right


		//k = computeK(trail, i+k);
	}

	//保存result.pcd
	boost::shared_ptr<std::vector<int>> result_ptr = boost::make_shared<std::vector<int>>(result_idx);

	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(midle_filtered);
	extract.setIndices(result_ptr);
	extract.setNegative(false);//如果设为true,可以提取指定index之外的点云
	extract.filter(*result);

	cout << "生长后的点云保存到result.pcd，0result.pcd" << endl;
	//pcl::io::savePCDFileASCII("0result.pcd", *result);
	saveLasFile("0result.las", result);
	cout << "保存成功" << endl;

	return 0;
}

2.2 根据0.3确定k

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>//标准C++库中的输入输出类相关头文件。
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h> //PCL中支持的点类型头文件。
#include <pcl/octree/octree.h>
#include<fstream>  
#include <string>  
#include <vector> 
//#include <LasOperator.h> 
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>

using namespace std;

void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZ>& cloud) {
	std::ifstream ifs(s, std::ios::in | std::ios::binary); // 打开las文件
	liblas::ReaderFactory f;
	liblas::Reader reader = f.CreateWithStream(ifs); // 读取las文件

	liblas::Header const& header = reader.GetHeader();

	cloud.width = header.GetPointRecordsCount();	//保证与las数据点的个数一致	
	cloud.height = 1;
	cloud.is_dense = false;
	cloud.points.resize(cloud.width * cloud.height);

	int i = 0;
	while (reader.ReadNextPoint()) {
		liblas::Point const& p = reader.GetPoint();
		// 获取las数据的x，y，z信息
		cloud.points[i].x = (p.GetX());
		cloud.points[i].y = (p.GetY());
		cloud.points[i].z = (p.GetZ());

		i++;
	}
	ifs.close();
}
void saveLasFile(string s, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
	cout << cloud->points.size() << endl;
	std::ofstream ofs(s, ios::out | ios::binary);

	liblas::Header header;
	header.SetScale(0.0001, 0.0001, 0.0001);
	header.SetPointRecordsCount(cloud->points.size());

	// fill other header members
	// here the header has been serialized to disk into the *file.las*
	liblas::Writer writer(ofs, header);
	liblas::Point point(&header);

	// fill other properties of point record
	for (int i = 0;i < cloud->points.size();++i) {
		point.SetCoordinates(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
		writer.WritePoint(point);
	}
	writer.SetHeader(header);
	ofs.flush();
	ofs.close();
}

float computeRange(pcl::PointCloud<pcl::PointXYZ>& trail, float r, int index, int k) {
	float x1, y1, x2, y2, a1, a2, a3, b1, b2, b3;//向量1，a,b为轨迹线上2点
	x2 = 0; y2 = 1;
	a1 = trail.points[index].x;
	a2 = trail.points[index].y;
	a3 = trail.points[index].z;
	b1 = trail.points[index + k].x;
	b2 = trail.points[index + k].y;
	b3 = trail.points[index + k].z;
	x1 = a1 - b1;
	y1 = a2 - b2;
	float cosa = (x1 * x2 + y1 * y2) / (sqrt(x1 * x1 + y1 * y1) + sqrt(x2 * x2 + y2 * y2));
	float sina = sqrt(1 - cosa * cosa);
	float range = r / sina;

	//float radian = acos(cosa);
	//float angle = radian * 180.0 / M_PI;
	//if (angle > 90)
	//	angle = 180 - angle;
	//cout << "angle: " << angle << "\t";

	return range;
}
void cutMidle(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr trail, float r, int k, pcl::PointCloud<pcl::PointXYZ>::Ptr& midle) {
	vector<int> idx;
	vector<int> midle_idx;
	//cloud构建八叉树
	cout << "构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();
	cout << "构建完成" << endl;

	/**
		切割宽于路面部分
	*/
	cout << "切割宽于路面部分" << endl;
	for (size_t i = 0;i + k < trail->points.size(); i += k) {
		//for (size_t i = 8000;i<8500; i += k) {
		//cout << "-------------------------------------------------------------" << endl;
		float range = computeRange(*trail, r, i, k);
		float center = trail->points[i].x;
		float Ymin = min(trail->points[i].y, trail->points[i + k].y);
		float Ymax = max(trail->points[i].y, trail->points[i + k].y);
		float Zmin = -10;
		float Zmax = max(trail->points[i].z, trail->points[i + k].z);
		//cout << "range: " << range << endl;

		//分段切割种子点
		Eigen::Vector3f Emin(center - range, Ymin, Zmin);//0xc0c0c0c0	0x3f3f3f3f
		Eigen::Vector3f Emax(center + range, Ymax, Zmax);
		octree.boxSearch(Emin, Emax, idx);
		//cout << i << "\t这段种子数：\t" << idx.size() << endl;
		cout << i  << endl;
		midle_idx.insert(midle_idx.end(), idx.begin(), idx.end());
	}

	//提取midle
	boost::shared_ptr<std::vector<int>> midle_ptr = boost::make_shared<std::vector<int>>(midle_idx);
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(midle_ptr);
	extract.setNegative(false);//如果设为true,可以提取指定index之外的点云
	extract.filter(*midle);
}

void filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float radius, int neighbors, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered) {
	//去杂点
	pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;
	ror.setInputCloud(cloud);
	ror.setRadiusSearch(radius);
	ror.setMinNeighborsInRadius(neighbors);
	ror.filter(*cloud_filtered);
}

float comupteNormalAngle(Eigen::Vector4f p, Eigen::Vector4f q) {
	float cos = (p[0] * q[0] + p[1] * q[1] + p[2] * q[2]) / (sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]) * sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]));

	float radian = acos(cos);
	float angle = radian * 180.0 / M_PI;
	//cout << "原来角度：" << angle << endl;
	if (angle > 90)
		angle = 180 - angle;
	//cout << "cos: " << cos << "\tradian: " << radian << endl;
	return angle;
}
int computeK(pcl::PointCloud<pcl::PointXYZ>::Ptr trail,int begin) {
	float d;
	for (int i = begin;i < trail->points.size();++i){
		d = abs(trail->points[i].y - trail->points[begin].y);
		if (d > 0.3){
			cout << "k为： " << i - begin << "y距离" << trail->points[i].y - trail->points[begin].y << endl;
			return i - begin;
		}
	}
	return -1;
}

int main() {
	//输入点云，轨迹线，保存种子
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr trail(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr midle(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr midle_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointXYZ tempPnt;
	std::vector<int>idx;
	std::vector<int>idx1;
	//变量

	Eigen::Vector4f n;
	Eigen::Vector4f m;
	Eigen::Vector4f m1;
	std::vector<int>result_idx;
	float k1, k2, k3, t1, t2, t3;

	float r = 8;//10
	int k = 6;//7
	float d = 0.3;//0.2  0.4
	float Z_range = 0.3;
	float angle_Threshold = 75;//角度
	float p_curvature = 0;

	//读取输入点云
	cout << "读取点云" << endl;
	loadLasFile("o1.las", *input_cloud);
	//pcl::io::loadPCDFile("o.pcd", *input_cloud);
	//读取轨迹线
	cout << "读取轨迹线" << endl;
	pcl::io::loadPCDFile("trail1.pcd", *trail);
	cout << "原始点数: " << input_cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;
	


	//切割宽于中间路面得midle
	cutMidle(input_cloud, trail, r, k, midle);

	//滤波去杂点
	filter(midle, 0.02, 2, midle_filtered);//0.03  2

	//cout << "读取midle" << endl;
	//pcl::io::loadPCDFile("trail.pcd", *trail);

	//cloud构建八叉树
	cout << "cloud构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	octree.setInputCloud(midle_filtered);
	octree.addPointsFromInputCloud();
	cout << "过滤后点数" << midle_filtered->points.size() << endl;

	/**
		开始循环
	*/
	int begin = 8000;
	k = computeK(trail, begin);
	cout << "k为： " << k << endl;;
	cout << "区域生长" << endl;
	//for (size_t i = 0; i + k <= trail->points.size(); i += k) {//cloud1->points.size()		cloud1->points.size()+500   500 i是down，i+k是up
	for (size_t i = begin;i < 8439&&i!=-1; i += k) {//8600, 9000  end 9647		 i + k <= trail->points.size()   9300
	//for (size_t i = 8492;i < 8493; i += k) {
		cout << "-------------------------------------------------------------" << endl;
		cout << i << endl;
		float center = trail->points[i].x;
		float range = computeRange(*trail, r, i, k);
		//cout << "range: " << range << endl;
		float left = center - d / 2;
		float right = center + d / 2;
		float Ymin = min(trail->points[i].y, trail->points[i + k].y);
		float Ymax = max(trail->points[i].y, trail->points[i + k].y);
		float Zmin = 0;
		float Zmax = max(trail->points[i].z, trail->points[i + k].z);

		//切割中间的种子区域	中间
		Eigen::Vector3f Emin(left, Ymin, Zmin);
		Eigen::Vector3f Emax(right, Ymax, Zmax);
		octree.boxSearch(Emin, Emax, idx);
		//pcl::copyPointCloud(*cloud, idx, *tempCloud);
		result_idx.insert(result_idx.end(), idx.begin(), idx.end());
		//cout << midle_filtered->points[idx[0]].z << endl;
		float height = midle_filtered->points[idx[0]].z;

		//求中间法线
		pcl::computePointNormal(*midle_filtered, idx, n, p_curvature);

		cout << "Yrange" << Ymin << "\t" << Ymax << endl;
		cout << "Zrange" << Zmin << "\t" << Zmax << endl;
		cout << "中间法向量" << n[0] << "\t" << n[1] << "\t" << n[2] << endl;

		m = n;
		bool flag = true;//继续循环
		while (flag) {//left
			cout << "-------------------------------------------------------------" << endl;
			right -= d;
			left -= d;
			if (left <= center - range) {
				flag = false;
				left = center - range;
			}

			cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			cout << "上一格法向量" << m[0] << "\t" << m[1] << "\t" << m[2] << endl;

			Eigen::Vector3f Emin1(left, Ymin, Zmin);
			Eigen::Vector3f Emax1(right, Ymax, Zmax);
			octree.boxSearch(Emin1, Emax1, idx);

			if (idx.size() == 0)
				break;

			//求法线
			pcl::computePointNormal(*midle_filtered, idx, m1, p_curvature);
			cout << "点数" << idx.size() << endl;
			cout << "当前法向量" << m1[0] << "\t" << m1[1] << "\t" << m1[2] << endl;
			pcl::copyPointCloud(*midle_filtered, idx, *temp_cloud);
			pcl::io::savePCDFile("0tempcloud.pcd",*temp_cloud);


			float angle = comupteNormalAngle(m, m1);
			m = m1;
			cout << "角度是： " << angle << endl;

			if (angle > angle_Threshold) {//2.1
				flag = false;
			}
			else {
				for (int j = 0;j < idx.size();++j) {
					float kkk = abs(midle_filtered->points[idx[j]].z - height);

					if (kkk < Z_range) {//0.2
						idx1.push_back(idx[j]);
					}
				}
				result_idx.insert(result_idx.end(), idx1.begin(), idx1.end());
				idx1.clear();
			}

		}//left

		k = computeK(trail, i+k);

		flag = true;
		left = center - d / 2;
		right = center + d / 2;
		m = n;

		//while (flag) {//right
		//	//cout << "-------------------------------------------------------------" << endl;
		//	right += d;
		//	left += d;
		//	if (right >= center + range) {
		//		flag = false;
		//		right = center + range;
		//	}

		//	/*cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
		//	cout << "上一格法向量" << m[0] << "\t" << m[1] << "\t" << m[2] << endl;*/

		//	Eigen::Vector3f Emin1(left, Ymin, Zmin);
		//	Eigen::Vector3f Emax1(right, Ymax, Zmax);
		//	octree.boxSearch(Emin1, Emax1, idx);

		//	//pcl::copyPointCloud(*cloud, idx, *temp_cloud);
		//	//pcl::io::savePCDFileASCII("0tempcloud.pcd",*temp_cloud);

		//	if (idx.size() == 0)
		//		break;

		//	//求法线
		//	pcl::computePointNormal(*midle_filtered, idx, m1, p_curvature);
		//	/*cout << "点数" << idx.size() << endl;
		//	cout << "当前法向量" << m1[0] << "\t" << m1[1] << "\t" << m1[2] << endl;*/


		//	float angle = comupteNormalAngle(m, m1);
		//	m = m1;
		//	//cout << "角度是： " << angle << endl;

		//	if (angle > angle_Threshold) {//2.1
		//		flag = false;
		//	}
		//	else {
		//		for (int j = 0;j < idx.size();++j) {
		//			float kkk = abs(midle_filtered->points[idx[j]].z - height);

		//			if (kkk < Z_range) {//0.2
		//				idx1.push_back(idx[j]);
		//			}
		//		}
		//		result_idx.insert(result_idx.end(), idx1.begin(), idx1.end());
		//		idx1.clear();
		//	}
		//}//right
	}

	//保存result.pcd
	boost::shared_ptr<std::vector<int>> result_ptr = boost::make_shared<std::vector<int>>(result_idx);

	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(midle_filtered);
	extract.setIndices(result_ptr);
	extract.setNegative(false);//如果设为true,可以提取指定index之外的点云
	extract.filter(*result);

	cout << "生长后的点云保存到result.pcd，0result.pcd" << endl;
	//pcl::io::savePCDFileASCII("0result.pcd", *result);
	saveLasFile("0result.las", result);
	cout << "保存成功" << endl;

	/*cout << "滤波后的点云: o_midle_filtered.pcd" << endl;
	pcl::io::savePCDFileASCII("o_midle_filtered.pcd", *midle_filtered);
	cout << "保存成功" << endl;*/

	return 0;
}

2.1 完整，有地方残缺,使用las文件读写

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>//标准C++库中的输入输出类相关头文件。
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h> //PCL中支持的点类型头文件。
#include <pcl/octree/octree.h>
#include<fstream>  
#include <string>  
#include <vector> 
//#include <LasOperator.h> 
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>

using namespace std;

void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZ>& cloud) {
	std::ifstream ifs(s, std::ios::in | std::ios::binary); // 打开las文件
	liblas::ReaderFactory f;
	liblas::Reader reader = f.CreateWithStream(ifs); // 读取las文件

	liblas::Header const& header = reader.GetHeader();

	cloud.width = header.GetPointRecordsCount();	//保证与las数据点的个数一致	
	cloud.height = 1;
	cloud.is_dense = false;
	cloud.points.resize(cloud.width * cloud.height);

	int i = 0;
	while (reader.ReadNextPoint()) {
		liblas::Point const& p = reader.GetPoint();
		// 获取las数据的x，y，z信息
		cloud.points[i].x = (p.GetX());
		cloud.points[i].y = (p.GetY());
		cloud.points[i].z = (p.GetZ());

		i++;
	}
	ifs.close();
}
void saveLasFile(string s, pcl::PointCloud<pcl::PointXYZ>::Ptr cloud) {
	cout << cloud->points.size() << endl;
	std::ofstream ofs(s, ios::out | ios::binary);

	liblas::Header header;
	header.SetScale(0.0001, 0.0001, 0.0001);
	header.SetPointRecordsCount(cloud->points.size());

	// fill other header members
	// here the header has been serialized to disk into the *file.las*
	liblas::Writer writer(ofs, header);
	liblas::Point point(&header);

	// fill other properties of point record
	for (int i = 0;i < cloud->points.size();++i) {
		point.SetCoordinates(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z);
		writer.WritePoint(point);
	}
	writer.SetHeader(header);
	ofs.flush();
	ofs.close();
}

float computeRange(pcl::PointCloud<pcl::PointXYZ>& trail, float r, int index, int k) {
	float x1, y1, x2, y2, a1, a2, a3, b1, b2, b3;//向量1，a,b为轨迹线上2点
	x2 = 0; y2 = 1;
	a1 = trail.points[index].x;
	a2 = trail.points[index].y;
	a3 = trail.points[index].z;
	b1 = trail.points[index + k].x;
	b2 = trail.points[index + k].y;
	b3 = trail.points[index + k].z;
	x1 = a1 - b1;
	y1 = a2 - b2;
	float cosa = (x1 * x2 + y1 * y2) / (sqrt(x1 * x1 + y1 * y1) + sqrt(x2 * x2 + y2 * y2));
	float sina = sqrt(1 - cosa * cosa);
	float range = r / sina;

	//float radian = acos(cosa);
	//float angle = radian * 180.0 / M_PI;
	//if (angle > 90)
	//	angle = 180 - angle;
	//cout << "angle: " << angle << "\t";

	return range;
}
void cutMidle(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr trail, float r, int k, pcl::PointCloud<pcl::PointXYZ>::Ptr& midle) {
	vector<int> idx;
	vector<int> midle_idx;
	//cloud构建八叉树
	cout << "构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();
	cout << "构建完成" << endl;

	/**
		切割宽于路面部分
	*/
	cout << "切割宽于路面部分" << endl;
	for (size_t i = 0;i + k < trail->points.size(); i += k) {
		//for (size_t i = 8000;i<8500; i += k) {
		cout << "-------------------------------------------------------------" << endl;
		float range = computeRange(*trail, r, i, k);
		float center = trail->points[i].x;
		float Ymin = min(trail->points[i].y, trail->points[i + k].y);
		float Ymax = max(trail->points[i].y, trail->points[i + k].y);
		float Zmin = -10;
		float Zmax = max(trail->points[i].z, trail->points[i + k].z);
		cout << "range: " << range << endl;

		//分段切割种子点
		Eigen::Vector3f Emin(center - range, Ymin, Zmin);//0xc0c0c0c0	0x3f3f3f3f
		Eigen::Vector3f Emax(center + range, Ymax, Zmax);
		octree.boxSearch(Emin, Emax, idx);
		cout << i << "\t这段种子数：\t" << idx.size() << endl;
		midle_idx.insert(midle_idx.end(), idx.begin(), idx.end());
	}

	//提取midle
	boost::shared_ptr<std::vector<int>> midle_ptr = boost::make_shared<std::vector<int>>(midle_idx);
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(midle_ptr);
	extract.setNegative(false);//如果设为true,可以提取指定index之外的点云
	extract.filter(*midle);
}

void filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float radius, int neighbors, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered) {
	//去杂点
	pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;
	ror.setInputCloud(cloud);
	ror.setRadiusSearch(radius);
	ror.setMinNeighborsInRadius(neighbors);
	ror.filter(*cloud_filtered);
}

float comupteNormalAngle(Eigen::Vector4f p, Eigen::Vector4f q) {
	float cos = (p[0] * q[0] + p[1] * q[1] + p[2] * q[2]) / (sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]) * sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]));

	float radian = acos(cos);
	float angle = radian * 180.0 / M_PI;
	cout << "原来角度：" << angle << endl;
	if (angle > 90)
		angle = 180 - angle;
	//cout << "cos: " << cos << "\tradian: " << radian << endl;
	return angle;
}

int main() {
	//输入点云，轨迹线，保存种子
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr trail(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr midle(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr midle_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointXYZ tempPnt;
	std::vector<int>idx;
	std::vector<int>idx1;
	//变量

	Eigen::Vector4f n;
	Eigen::Vector4f m;
	Eigen::Vector4f m1;
	std::vector<int>result_idx;

	float r = 10;//10
	int k = 6;//7
	float d = 0.3;//0.2  0.4
	float Z_range = 0.3;
	float angle_Threshold = 75;//角度
	float p_curvature = 0;

	//读取输入点云
	cout << "读取点云" << endl;
	loadLasFile("o.las", *input_cloud);
	//pcl::io::loadPCDFile("o.pcd", *input_cloud);
	//读取轨迹线
	cout << "读取轨迹线" << endl;
	pcl::io::loadPCDFile("trail.pcd", *trail);
	cout << "原始点数: " << input_cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;

	//切割宽于中间路面得midle
	cutMidle(input_cloud, trail, r, k, midle);

	//滤波去杂点
	filter(midle, 0.02, 2, midle_filtered);//0.03  2

	//cout << "读取midle" << endl;
	//pcl::io::loadPCDFile("trail.pcd", *trail);

	//cloud构建八叉树
	cout << "cloud构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	octree.setInputCloud(midle_filtered);
	octree.addPointsFromInputCloud();
	cout << "过滤后点数" << midle_filtered->points.size() << endl;

	/**
		开始循环
	*/
	cout << "区域生长" << endl;
	//for (size_t i = 0; i + k <= trail->points.size(); i += k) {//cloud1->points.size()		cloud1->points.size()+500   500 i是down，i+k是up
	//for (size_t i = 8090;i < 8091; i += k) {//8600, 9000  end 9647		 i + k <= trail->points.size()   9300
	for (size_t i = 8000;i < 8500; i += k) {
		cout << "-------------------------------------------------------------" << endl;
		cout << i << endl;
		float center = trail->points[i].x;
		float range = computeRange(*trail, r, i, k);
		cout << "range: " << range << endl;
		float left = center - d / 2;
		float right = center + d / 2;
		float Ymin = min(trail->points[i].y, trail->points[i + k].y);
		float Ymax = max(trail->points[i].y, trail->points[i + k].y);
		float Zmin = 0;
		float Zmax = max(trail->points[i].z, trail->points[i + k].z);

		//切割中间的种子区域	中间
		Eigen::Vector3f Emin(left, Ymin, Zmin);
		Eigen::Vector3f Emax(right, Ymax, Zmax);
		octree.boxSearch(Emin, Emax, idx);
		//pcl::copyPointCloud(*cloud, idx, *tempCloud);
		result_idx.insert(result_idx.end(), idx.begin(), idx.end());
		cout << midle_filtered->points[idx[0]].z << endl;
		float height = midle_filtered->points[idx[0]].z;

		//求中间法线
		pcl::computePointNormal(*midle_filtered, idx, n, p_curvature);

		cout << "Yrange" << Ymin << "\t" << Ymax << endl;
		cout << "Zrange" << Zmin << "\t" << Zmax << endl;
		cout << "中间法向量" << n[0] << "\t" << n[1] << "\t" << n[2] << endl;

		m = n;
		bool flag = true;//继续循环
		while (flag) {//left
			cout << "-------------------------------------------------------------" << endl;
			right -= d;
			left -= d;
			if (left <= center - range) {
				flag = false;
				left = center - range;
			}

			/*cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			cout << "上一格法向量" << m[0] << "\t" << m[1] << "\t" << m[2] << endl;*/

			Eigen::Vector3f Emin1(left, Ymin, Zmin);
			Eigen::Vector3f Emax1(right, Ymax, Zmax);
			octree.boxSearch(Emin1, Emax1, idx);

			//pcl::copyPointCloud(*cloud, idx, *temp_cloud);
			//pcl::io::savePCDFileASCII("0tempcloud.pcd",*temp_cloud);

			if (idx.size() == 0)
				break;

			//求法线
			pcl::computePointNormal(*midle_filtered, idx, m1, p_curvature);
			/*cout << "点数" << idx.size() << endl;
			cout << "当前法向量" << m1[0] << "\t" << m1[1] << "\t" << m1[2] << endl;*/


			float angle = comupteNormalAngle(m, m1);
			m = m1;
			//cout << "角度是： " << angle << endl;

			if (angle > angle_Threshold) {//2.1
				flag = false;
			}
			else {
				for (int j = 0;j < idx.size();++j) {
					float kkk = abs(midle_filtered->points[idx[j]].z - height);

					if (kkk < Z_range) {//0.2
						idx1.push_back(idx[j]);
					}
				}
				result_idx.insert(result_idx.end(), idx1.begin(), idx1.end());
				idx1.clear();
			}

		}//left


		flag = true;
		left = center - d / 2;
		right = center + d / 2;
		m = n;

		while (flag) {//right
			cout << "-------------------------------------------------------------" << endl;
			right += d;
			left += d;
			if (right >= center + range) {
				flag = false;
				right = center + range;
			}

			/*cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			cout << "上一格法向量" << m[0] << "\t" << m[1] << "\t" << m[2] << endl;*/

			Eigen::Vector3f Emin1(left, Ymin, Zmin);
			Eigen::Vector3f Emax1(right, Ymax, Zmax);
			octree.boxSearch(Emin1, Emax1, idx);

			//pcl::copyPointCloud(*cloud, idx, *temp_cloud);
			//pcl::io::savePCDFileASCII("0tempcloud.pcd",*temp_cloud);

			if (idx.size() == 0)
				break;

			//求法线
			pcl::computePointNormal(*midle_filtered, idx, m1, p_curvature);
			/*cout << "点数" << idx.size() << endl;
			cout << "当前法向量" << m1[0] << "\t" << m1[1] << "\t" << m1[2] << endl;*/


			float angle = comupteNormalAngle(m, m1);
			m = m1;
			//cout << "角度是： " << angle << endl;

			if (angle > angle_Threshold) {//2.1
				flag = false;
			}
			else {
				for (int j = 0;j < idx.size();++j) {
					float kkk = abs(midle_filtered->points[idx[j]].z - height);

					if (kkk < Z_range) {//0.2
						idx1.push_back(idx[j]);
					}
				}
				result_idx.insert(result_idx.end(), idx1.begin(), idx1.end());
				idx1.clear();
			}
		}//right
	}

	//保存result.pcd
	boost::shared_ptr<std::vector<int>> result_ptr = boost::make_shared<std::vector<int>>(result_idx);

	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(midle_filtered);
	extract.setIndices(result_ptr);
	extract.setNegative(false);//如果设为true,可以提取指定index之外的点云
	extract.filter(*result);

	cout << "生长后的点云保存到result.pcd，0result.pcd" << endl;
	//pcl::io::savePCDFileASCII("0result.pcd", *result);
	saveLasFile("0result.las", result);
	cout << "保存成功" << endl;

	/*cout << "滤波后的点云: o_midle_filtered.pcd" << endl;
	pcl::io::savePCDFileASCII("o_midle_filtered.pcd", *midle_filtered);
	cout << "保存成功" << endl;*/

	return 0;
}

2.0 程序完整，但是会有地方残缺，整体效果ok

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>//标准C++库中的输入输出类相关头文件。
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h> //PCL中支持的点类型头文件。
#include <pcl/octree/octree.h>
#include<fstream>  
#include <string>  
#include <vector> 
//#include <LasOperator.h> 
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/radius_outlier_removal.h>

using namespace std;

void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZ>& cloud) {
	/*
		*读取las文件
		*/
	std::ifstream ifs(s, std::ios::in | std::ios::binary); // 打开las文件
	liblas::ReaderFactory f;
	liblas::Reader reader = f.CreateWithStream(ifs); // 读取las文件

	unsigned long int nbPoints = reader.GetHeader().GetPointRecordsCount();//获取las数据点的个数

	cloud.width = nbPoints;	//保证与las数据点的个数一致	
	cloud.height = 1;
	cloud.is_dense = false;
	cloud.points.resize(cloud.width * cloud.height);

	int i = 0;
	uint16_t r1, g1, b1;
	int r2, g2, b2;
	uint32_t rgb;
	while (reader.ReadNextPoint()) {
		// 获取las数据的x，y，z信息
		cloud.points[i].x = (reader.GetPoint().GetX());
		cloud.points[i].y = (reader.GetPoint().GetY());
		cloud.points[i].z = (reader.GetPoint().GetZ());

		//获取las数据的r，g，b信息
		/*r1 = (reader.GetPoint().GetColor().GetRed());
		g1 = (reader.GetPoint().GetColor().GetGreen());
		b1 = (reader.GetPoint().GetColor().GetBlue());
		r2 = ceil(((float)r1 / 65536) * (float)256);
		g2 = ceil(((float)g1 / 65536) * (float)256);
		b2 = ceil(((float)b1 / 65536) * (float)256);
		rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);
		cloud.points[i].rgb = *reinterpret_cast<float*>(&rgb);*/

		i++;
	}
}
float computeRange(pcl::PointCloud<pcl::PointXYZ>& trail, float r, int index, int k) {
	float x1, y1, x2, y2, a1, a2, a3, b1, b2, b3;//向量1，a,b为轨迹线上2点
	x2 = 0; y2 = 1;
	a1 = trail.points[index].x;
	a2 = trail.points[index].y;
	a3 = trail.points[index].z;
	b1 = trail.points[index + k].x;
	b2 = trail.points[index + k].y;
	b3 = trail.points[index + k].z;
	x1 = a1 - b1;
	y1 = a2 - b2;
	float cosa = (x1 * x2 + y1 * y2) / (sqrt(x1 * x1 + y1 * y1) + sqrt(x2 * x2 + y2 * y2));
	float sina = sqrt(1 - cosa * cosa);
	float range = r / sina;

	//float radian = acos(cosa);
	//float angle = radian * 180.0 / M_PI;
	//if (angle > 90)
	//	angle = 180 - angle;
	//cout << "angle: " << angle << "\t";

	return range;
}
void cutMidle(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, pcl::PointCloud<pcl::PointXYZ>::Ptr trail, float r, int k, pcl::PointCloud<pcl::PointXYZ>::Ptr& midle) {
	vector<int> idx;
	vector<int> midle_idx;
	//cloud构建八叉树
	cout << "构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();
	cout << "构建完成" << endl;

	/**
		切割宽于路面部分
	*/
	cout << "切割宽于路面部分" << endl;
	for (size_t i = 0;i + k < trail->points.size(); i += k) {
		//for (size_t i = 8000;i<8500; i += k) {
		cout << "-------------------------------------------------------------" << endl;
		float range = computeRange(*trail, r, i, k);
		float center = trail->points[i].x;
		float Ymin = min(trail->points[i].y, trail->points[i + k].y);
		float Ymax = max(trail->points[i].y, trail->points[i + k].y);
		float Zmin = -10;
		float Zmax = max(trail->points[i].z, trail->points[i + k].z);
		cout << "range: " << range << endl;

		//分段切割种子点
		Eigen::Vector3f Emin(center - range, Ymin, Zmin);//0xc0c0c0c0	0x3f3f3f3f
		Eigen::Vector3f Emax(center + range, Ymax, Zmax);
		octree.boxSearch(Emin, Emax, idx);
		cout << i << "\t这段种子数：\t" << idx.size() << endl;
		midle_idx.insert(midle_idx.end(), idx.begin(), idx.end());
	}

	//提取midle
	boost::shared_ptr<std::vector<int>> midle_ptr = boost::make_shared<std::vector<int>>(midle_idx);
	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(midle_ptr);
	extract.setNegative(false);//如果设为true,可以提取指定index之外的点云
	extract.filter(*midle);
}

void filter(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float radius, int neighbors, pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_filtered) {
	//去杂点
	pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;
	ror.setInputCloud(cloud);
	ror.setRadiusSearch(radius);
	ror.setMinNeighborsInRadius(neighbors);
	ror.filter(*cloud_filtered);
}

float comupteNormalAngle(Eigen::Vector4f p, Eigen::Vector4f q) {
	float cos = (p[0] * q[0] + p[1] * q[1] + p[2] * q[2]) / (sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]) * sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]));

	float radian = acos(cos);
	float angle = radian * 180.0 / M_PI;
	cout << "原来角度：" << angle << endl;
	if (angle > 90)
		angle = 180 - angle;
	//cout << "cos: " << cos << "\tradian: " << radian << endl;
	return angle;
}

int main() {
	//输入点云，轨迹线，保存种子
	pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr trail(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr midle(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr midle_filtered(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr result(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointXYZ tempPnt;
	std::vector<int>idx;
	std::vector<int>idx1;
	//变量

	Eigen::Vector4f n;
	Eigen::Vector4f m;
	Eigen::Vector4f m1;
	std::vector<int>result_idx;

	float r = 10;//10
	int k = 6;//7
	float d = 0.3;//0.2  0.4
	float Z_range = 0.3;
	float angle_Threshold = 75;//角度
	float p_curvature = 0;

	//读取输入点云
	cout << "读取点云" << endl;
	loadLasFile("o.las", *input_cloud);
	//pcl::io::loadPCDFile("o.pcd", *input_cloud);
	//读取轨迹线
	cout << "读取轨迹线" << endl;
	pcl::io::loadPCDFile("trail.pcd", *trail);
	cout << "原始点数: " << input_cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;

	//切割宽于中间路面得midle
	cutMidle(input_cloud, trail, r, k, midle);

	//滤波去杂点
	filter(midle, 0.02, 2, midle_filtered);//0.03  2

	cout << "读取midle" << endl;
	pcl::io::loadPCDFile("trail.pcd", *trail);

	//cloud构建八叉树
	cout << "cloud构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZ> octree(resolution);
	octree.setInputCloud(midle_filtered);
	octree.addPointsFromInputCloud();
	cout << "过滤后点数" << midle_filtered->points.size() << endl;

	/**
		开始循环
	*/
	cout << "区域生长" << endl;
	for (size_t i = 0; i + k <= trail->points.size(); i += k) {//cloud1->points.size()		cloud1->points.size()+500   500 i是down，i+k是up
	//for (size_t i = 8090;i < 8091; i += k) {//8600, 9000  end 9647		 i + k <= trail->points.size()   9300
	//for (size_t i = 8000;i < 8500; i += k) {
		cout << "-------------------------------------------------------------" << endl;
		cout << i << endl;
		float center = trail->points[i].x;
		float range = computeRange(*trail, r, i, k);
		cout << "range: " << range << endl;
		float left = center - d / 2;
		float right = center + d / 2;
		float Ymin = min(trail->points[i].y, trail->points[i + k].y);
		float Ymax = max(trail->points[i].y, trail->points[i + k].y);
		float Zmin = 0;
		float Zmax = max(trail->points[i].z, trail->points[i + k].z);
		


		//切割中间的种子区域	中间
		Eigen::Vector3f Emin(left, Ymin, Zmin);
		Eigen::Vector3f Emax(right, Ymax, Zmax);
		octree.boxSearch(Emin, Emax, idx);
		//pcl::copyPointCloud(*cloud, idx, *tempCloud);
		result_idx.insert(result_idx.end(), idx.begin(), idx.end());
		cout << midle_filtered->points[idx[0]].z << endl;
		float height = midle_filtered->points[idx[0]].z;

		//求中间法线
		pcl::computePointNormal(*midle_filtered, idx, n, p_curvature);

		cout << "Yrange" << Ymin << "\t" << Ymax << endl;
		cout << "Zrange" << Zmin << "\t" << Zmax << endl;
		cout << "中间法向量" << n[0] << "\t" << n[1] << "\t" << n[2] << endl;

		m = n;
		bool flag = true;//继续循环
		while (flag) {//left
			cout << "-------------------------------------------------------------" << endl;
			right -= d;
			left -= d;
			if (left <= center - range) {
				flag = false;
				left = center - range;
			}

			cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			cout << "上一格法向量" << m[0] << "\t" << m[1] << "\t" << m[2] << endl;

			Eigen::Vector3f Emin1(left, Ymin, Zmin);
			Eigen::Vector3f Emax1(right, Ymax, Zmax);
			octree.boxSearch(Emin1, Emax1, idx);

			//pcl::copyPointCloud(*cloud, idx, *temp_cloud);
			//pcl::io::savePCDFileASCII("0tempcloud.pcd",*temp_cloud);

			if (idx.size() == 0)
				break;

			//求法线
			pcl::computePointNormal(*midle_filtered, idx, m1, p_curvature);
			/*cout << "点数" << idx.size() << endl;
			cout << "当前法向量" << m1[0] << "\t" << m1[1] << "\t" << m1[2] << endl;*/


			float angle = comupteNormalAngle(m, m1);
			m = m1;
			//cout << "角度是： " << angle << endl;

			if (angle > angle_Threshold) {//2.1
				flag = false;
			}
			else {
				for (int j = 0;j < idx.size();++j) {
					float kkk = abs(midle_filtered->points[idx[j]].z - height);

					if (kkk < Z_range) {//0.2
						idx1.push_back(idx[j]);
					}
				}
				result_idx.insert(result_idx.end(), idx1.begin(), idx1.end());
				idx1.clear();
			}

		}//left


		flag = true;
		left = center - d / 2;
		right = center + d / 2;
		m = n;

		while (flag) {//right
			cout << "-------------------------------------------------------------" << endl;
			right += d;
			left += d;
			if (right >= center + range) {
				flag = false;
				right = center + range;
			}

			cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			cout << "上一格法向量" << m[0] << "\t" << m[1] << "\t" << m[2] << endl;

			Eigen::Vector3f Emin1(left, Ymin, Zmin);
			Eigen::Vector3f Emax1(right, Ymax, Zmax);
			octree.boxSearch(Emin1, Emax1, idx);

			//pcl::copyPointCloud(*cloud, idx, *temp_cloud);
			//pcl::io::savePCDFileASCII("0tempcloud.pcd",*temp_cloud);

			if (idx.size() == 0)
				break;

			//求法线
			pcl::computePointNormal(*midle_filtered, idx, m1, p_curvature);
			/*cout << "点数" << idx.size() << endl;
			cout << "当前法向量" << m1[0] << "\t" << m1[1] << "\t" << m1[2] << endl;*/


			float angle = comupteNormalAngle(m, m1);
			m = m1;
			//cout << "角度是： " << angle << endl;

			if (angle > angle_Threshold) {//2.1
				flag = false;
			}
			else {
				for (int j = 0;j < idx.size();++j) {
					float kkk = abs(midle_filtered->points[idx[j]].z - height);

					if (kkk < Z_range) {//0.2
						idx1.push_back(idx[j]);
					}
				}
				result_idx.insert(result_idx.end(), idx1.begin(), idx1.end());
				idx1.clear();
			}

		}//right

	}

	//保存result.pcd
	boost::shared_ptr<std::vector<int>> result_ptr = boost::make_shared<std::vector<int>>(result_idx);

	pcl::ExtractIndices<pcl::PointXYZ> extract;
	extract.setInputCloud(midle_filtered);
	extract.setIndices(result_ptr);
	extract.setNegative(false);//如果设为true,可以提取指定index之外的点云
	extract.filter(*result);

	cout << "生长后的点云保存到result.pcd，0result.pcd" << endl;
	pcl::io::savePCDFileASCII("0result.pcd", *result);
	cout << "保存成功" << endl;

	/*cout << "滤波后的点云: o_midle_filtered.pcd" << endl;
	pcl::io::savePCDFileASCII("o_midle_filtered.pcd", *midle_filtered);
	cout << "保存成功" << endl;*/

	return 0;
}

添加了下采样，但结果不对

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>//标准C++库中的输入输出类相关头文件。
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h> //PCL中支持的点类型头文件。
#include <pcl/octree/octree.h>
#include<fstream>  
#include <string>  
#include <vector> 
//#include <LasOperator.h> 
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include<math.h>
#include<stdio.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>

using namespace std;

void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZRGB>& cloud) {
	/*
		*读取las文件
		*/
	std::ifstream ifs(s, std::ios::in | std::ios::binary); // 打开las文件
	liblas::ReaderFactory f;
	liblas::Reader reader = f.CreateWithStream(ifs); // 读取las文件

	unsigned long int nbPoints = reader.GetHeader().GetPointRecordsCount();//获取las数据点的个数

	cloud.width = nbPoints;	//保证与las数据点的个数一致	
	cloud.height = 1;
	cloud.is_dense = false;
	cloud.points.resize(cloud.width * cloud.height);

	int i = 0;
	uint16_t r1, g1, b1;
	int r2, g2, b2;
	uint32_t rgb;
	while (reader.ReadNextPoint()) {
		// 获取las数据的x，y，z信息
		cloud.points[i].x = (reader.GetPoint().GetX());
		cloud.points[i].y = (reader.GetPoint().GetY());
		cloud.points[i].z = (reader.GetPoint().GetZ());

		//获取las数据的r，g，b信息
		r1 = (reader.GetPoint().GetColor().GetRed());
		g1 = (reader.GetPoint().GetColor().GetGreen());
		b1 = (reader.GetPoint().GetColor().GetBlue());
		r2 = ceil(((float)r1 / 65536) * (float)256);
		g2 = ceil(((float)g1 / 65536) * (float)256);
		b2 = ceil(((float)b1 / 65536) * (float)256);
		rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);
		cloud.points[i].rgb = *reinterpret_cast<float*>(&rgb);

		i++;
	}
}
float computeRange(pcl::PointCloud<pcl::PointXYZRGB>& trail, float r, int index, int k) {
	float x1, y1, x2, y2, a1, a2, a3, b1, b2, b3;//向量1，a,b为轨迹线上2点
	x2 = 0; y2 = 1;
	a1 = trail.points[index].x;
	a2 = trail.points[index].y;
	a3 = trail.points[index].z;
	b1 = trail.points[index + k].x;
	b2 = trail.points[index + k].y;
	b3 = trail.points[index + k].z;
	x1 = a1 - b1;
	y1 = a2 - b2;
	float cosa = (x1 * x2 + y1 * y2) / (sqrt(x1 * x1 + y1 * y1)*sqrt(x2 * x2 + y2 * y2));
	float sina = sqrt(1 - cosa * cosa);
	float range = r / sina;
	return range;
}
float comupteNormalAngle(Eigen::Vector4f p, Eigen::Vector4f q) {
	float cos = (p[0] * q[0] + p[1] * q[1] + p[2] * q[2]) / (sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]) * sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]));

	float radian = acos(cos);
	float angle = radian * 180.0 / M_PI;
	if (angle > 90)
		angle = 180 - angle;
	//cout << "cos: " << cos << "\tradian: " << radian << endl;
	return angle;
}


int main() {
	//输入点云，轨迹线，保存种子
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr trail(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr part(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr partSeed(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr seedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointXYZRGB tempPnt;
	std::vector<int>pointIdx;
	std::vector<int>pointIdx1;
	std::vector<int>abandon;
	Eigen::Vector4f n;
	Eigen::Vector4f m;

	std::vector<int>result_idx;
	//变量
	float p_curvature = 0;
	int count = 0;

	float r = 10;
	float d = 0.2;//0.2
	int k = 7;//7
	float Z_range = 0.3;
	float angle_Threshold = 75;//角度

	//读取输入电云
	cout << "读取点云" << endl;
	loadLasFile("o.las", *tempCloud);

	pcl::VoxelGrid<pcl::PointXYZRGB> downSampled;  //创建滤波对象
	//downSampled.set
	downSampled.setInputCloud(tempCloud);            //设置需要过滤的点云给滤波对象
	downSampled.setLeafSize(0.3f, 0.3f,0.3f);  //设置滤波时创建的体素体积为1cm的立方体，三个参数表示体素栅格叶大小，分别表示体素在XYZ方向的尺寸
	//downSampled.setDownsampleAllData(false);//设置是否对所有的字段进行下采样
	downSampled.filter(*cloud);           //执行滤波处理，存储输出

	pcl::io::savePCDFileASCII("sampled.pcd",*cloud);

	//读取轨迹线
	cout << "读取轨迹线" << endl;
	pcl::io::loadPCDFile("trail.pcd", *trail);
	cout << "原始点: " << tempCloud->width << " \t" << "采样后： "<<cloud->width<<"\t"<<"轨迹线点数: " << trail->width << endl;

	//cloud构建八叉树
	cout << "cloud构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();

	/**
		开始循环
	*/
	//for (size_t i = 0; i + k <= trail->points.size(); i += k) {//cloud1->points.size()		cloud1->points.size()+500   500 i是down，i+k是up
	for (size_t i = 9300;i + k <= trail->points.size(); i += k) {//8600, 9000  end 9647		 i + k <= trail->points.size()
		//cout << "-------------------------------------------------------------" << endl;
		cout << i << endl;
		float height = 0;
		float center = trail->points[i].x;
		float range = computeRange(*trail, r, i, k);
		float left = center - d / 2;
		float right = center + d / 2;
		float Ymin = min(trail->points[i].y, trail->points[i + k].y);
		float Ymax = max(trail->points[i].y, trail->points[i + k].y);
		float Zmin = -10;
		float Zmax = max(trail->points[i].z, trail->points[i + k].z);

		//切割中间的种子区域	中间
		Eigen::Vector3f Emin(left, Ymin, Zmin);
		Eigen::Vector3f Emax(right, Ymax, Zmax);
		octree.boxSearch(Emin, Emax, pointIdx);
		//pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
		result_idx.insert(result_idx.end(), pointIdx.begin(), pointIdx.end());

		height = cloud->points[pointIdx[0]].z;
		//cout << "height: " << height << endl;

		//求中间法线
		pcl::computePointNormal(*cloud, pointIdx, n, p_curvature);

		bool flag = true;//继续循环


		while (flag) {//left
			//cout << "-------------------------------------------------------------" << endl;
			right -= d;
			left -= d;
			if (left <= center - range) {
				flag = false;
				left = center - range;
			}

			//cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			Eigen::Vector3f Emin(left, Ymin, Zmin);
			Eigen::Vector3f Emax(right, Ymax, Zmax);
			octree.boxSearch(Emin, Emax, pointIdx);
			//pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);

			if (pointIdx.size() == 0)
				break;

			//求法线
			pcl::computePointNormal(*cloud, pointIdx, m, p_curvature);
			float angle = comupteNormalAngle(n, m);

			if (angle > angle_Threshold) {//2.1
				flag = false;
			}
			else {
				for (int j = 0;j < pointIdx.size();++j) {
					float kkk = abs(cloud->points[pointIdx[j]].z - height);

					if (kkk < Z_range) {//0.2
						pointIdx1.push_back(pointIdx[j]);
					}
				}
				result_idx.insert(result_idx.end(), pointIdx1.begin(), pointIdx1.end());
				pointIdx1.clear();
			}
		}//left

		flag = true;
		left = center - d / 2;
		right = center + d / 2;


		while (flag) {//right
			right += d;
			left += d;
			if (right >= center + range) {
				flag = false;
				right = center + range;
			}

			Eigen::Vector3f Emin(left, Ymin, Zmin);
			Eigen::Vector3f Emax(right, Ymax, Zmax);
			octree.boxSearch(Emin, Emax, pointIdx);
			//pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
			if (pointIdx.size() == 0)
				break;

			//求法线
			pcl::computePointNormal(*cloud, pointIdx, m, p_curvature);
			float angle = comupteNormalAngle(n, m);

			if (angle > angle_Threshold) {//2.1
				flag = false;
			}
			else {
				for (int j = 0;j < pointIdx.size();++j) {
					float kkk = abs(cloud->points[pointIdx[j]].z - height);

					if (kkk < Z_range) {//0.2
						pointIdx1.push_back(pointIdx[j]);
						//cout << "	这里取得了一个点kkk:" << kkk<<endl;
						count++;
					}
				}
				result_idx.insert(result_idx.end(), pointIdx1.begin(), pointIdx1.end());
				pointIdx1.clear();
			}
		}//right
	}

	//保存result.pcd
	boost::shared_ptr<std::vector<int>> result_ptr = boost::make_shared<std::vector<int>>(result_idx);

	pcl::ExtractIndices<pcl::PointXYZRGB> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(result_ptr);
	extract.setNegative(false);//如果设为true,可以提取指定index之外的点云
	extract.filter(*result);

	cout << "生长后的点云保存到result.pcd，0result.pcd" << endl;
	pcl::io::savePCDFileASCII("0result.pcd", *result);
	cout << "保存成功" << endl;
	cout << "count: " << count << endl;

	return 0;
}

添加了Z_range、float angle_Threshold，效果加强

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>//标准C++库中的输入输出类相关头文件。
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h> //PCL中支持的点类型头文件。
#include <pcl/octree/octree.h>
#include<fstream>  
#include <string>  
#include <vector> 
//#include <LasOperator.h> 
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include<math.h>
#include<stdio.h>
#include <pcl/filters/extract_indices.h>

using namespace std;

void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZRGB>& cloud) {
	/*
		*读取las文件
		*/
	std::ifstream ifs(s, std::ios::in | std::ios::binary); // 打开las文件
	liblas::ReaderFactory f;
	liblas::Reader reader = f.CreateWithStream(ifs); // 读取las文件

	unsigned long int nbPoints = reader.GetHeader().GetPointRecordsCount();//获取las数据点的个数

	cloud.width = nbPoints;	//保证与las数据点的个数一致	
	cloud.height = 1;
	cloud.is_dense = false;
	cloud.points.resize(cloud.width * cloud.height);

	int i = 0;
	uint16_t r1, g1, b1;
	int r2, g2, b2;
	uint32_t rgb;
	while (reader.ReadNextPoint()) {
		// 获取las数据的x，y，z信息
		cloud.points[i].x = (reader.GetPoint().GetX());
		cloud.points[i].y = (reader.GetPoint().GetY());
		cloud.points[i].z = (reader.GetPoint().GetZ());

		//获取las数据的r，g，b信息
		r1 = (reader.GetPoint().GetColor().GetRed());
		g1 = (reader.GetPoint().GetColor().GetGreen());
		b1 = (reader.GetPoint().GetColor().GetBlue());
		r2 = ceil(((float)r1 / 65536) * (float)256);
		g2 = ceil(((float)g1 / 65536) * (float)256);
		b2 = ceil(((float)b1 / 65536) * (float)256);
		rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);
		cloud.points[i].rgb = *reinterpret_cast<float*>(&rgb);

		i++;
	}
}
float computeRange(pcl::PointCloud<pcl::PointXYZRGB>& trail, float r, int index, int k) {
	float x1, y1, x2, y2, a1, a2, a3, b1, b2, b3;//向量1，a,b为轨迹线上2点
	x2 = 0; y2 = 1;
	a1 = trail.points[index].x;
	a2 = trail.points[index].y;
	a3 = trail.points[index].z;
	b1 = trail.points[index + k].x;
	b2 = trail.points[index + k].y;
	b3 = trail.points[index + k].z;
	x1 = a1 - b1;
	y1 = a2 - b2;
	float cosa = (x1 * x2 + y1 * y2) / (sqrt(x1 * x1 + y1 * y1) + sqrt(x2 * x2 + y2 * y2));
	float sina = sqrt(1 - cosa * cosa);
	float range = r / sina;
	return range;
}
float comupteNormalAngle(Eigen::Vector4f p, Eigen::Vector4f q) {
	float cos = (p[0] * q[0] + p[1] * q[1] + p[2] * q[2]) / (sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]) * sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]));

	float radian = acos(cos);
	float angle = radian * 180.0 / M_PI;
	if (angle > 90)
		angle = 180 - angle;
	//cout << "cos: " << cos << "\tradian: " << radian << endl;
	return angle;
}


int main() {
	//输入点云，轨迹线，保存种子
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr trail(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr part(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr partSeed(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr seedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointXYZRGB tempPnt;
	std::vector<int>pointIdx;
	std::vector<int>pointIdx1;
	std::vector<int>abandon;
	Eigen::Vector4f n;
	Eigen::Vector4f m;

	std::vector<int>result_idx;
	//变量
	float p_curvature = 0;
	int count = 0;

	float r = 10;
	float d = 0.2;//0.2
	int k = 7;//7
	float Z_range = 0.3;
	float angle_Threshold = 75;//角度

	//读取输入电云
	cout << "读取点云" << endl;
	loadLasFile("o.las", *cloud);
	//读取轨迹线
	cout << "读取轨迹线" << endl;
	pcl::io::loadPCDFile("trail.pcd", *trail);
	cout << "原始点数: " << cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;

	//cloud构建八叉树
	cout << "cloud构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();

	/**
		开始循环
	*/
	//for (size_t i = 0; i + k <= trail->points.size(); i += k) {//cloud1->points.size()		cloud1->points.size()+500   500 i是down，i+k是up
	for (size_t i = 9000;i + k <= trail->points.size(); i += k) {//8600, 9000  end 9647		 i + k <= trail->points.size()
		//cout << "-------------------------------------------------------------" << endl;
		cout << i << endl;
		float height = 0;
		float center = trail->points[i].x;
		float range = computeRange(*trail, r, i, k);
		float left = center - d / 2;
		float right = center + d / 2;
		float Ymin = min(trail->points[i].y, trail->points[i + k].y);
		float Ymax = max(trail->points[i].y, trail->points[i + k].y);
		float Zmin = -10;
		float Zmax = max(trail->points[i].z, trail->points[i + k].z);

		//切割中间的种子区域	中间
		Eigen::Vector3f Emin(left, Ymin, Zmin);
		Eigen::Vector3f Emax(right, Ymax, Zmax);
		octree.boxSearch(Emin, Emax, pointIdx);
		//pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
		result_idx.insert(result_idx.end(), pointIdx.begin(), pointIdx.end());

		height = cloud->points[pointIdx[0]].z;
		//cout << "height: " << height << endl;

		//求中间法线
		pcl::computePointNormal(*cloud, pointIdx, n, p_curvature);

		bool flag = true;//继续循环


		while (flag) {//left
			//cout << "-------------------------------------------------------------" << endl;
			right -= d;
			left -= d;
			if (left <= center - range) {
				flag = false;
				left = center - range;
			}

			//cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			Eigen::Vector3f Emin(left, Ymin, Zmin);
			Eigen::Vector3f Emax(right, Ymax, Zmax);
			octree.boxSearch(Emin, Emax, pointIdx);
			//pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);

			if (pointIdx.size() == 0)
				break;

			//求法线
			pcl::computePointNormal(*cloud, pointIdx, m, p_curvature);
			float angle = comupteNormalAngle(n, m);

			if (angle > angle_Threshold) {//2.1
				flag = false;
			}
			else {
				for (int j = 0;j < pointIdx.size();++j) {
					float kkk = abs(cloud->points[pointIdx[j]].z - height);

					if (kkk < Z_range) {//0.2
						pointIdx1.push_back(pointIdx[j]);
					}
				}
				result_idx.insert(result_idx.end(), pointIdx1.begin(), pointIdx1.end());
				pointIdx1.clear();
			}
		}//left

		flag = true;
		left = center - d / 2;
		right = center + d / 2;


		while (flag) {//right
			right += d;
			left += d;
			if (right >= center + range) {
				flag = false;
				right = center + range;
			}

			Eigen::Vector3f Emin(left, Ymin, Zmin);
			Eigen::Vector3f Emax(right, Ymax, Zmax);
			octree.boxSearch(Emin, Emax, pointIdx);
			//pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
			if (pointIdx.size() == 0)
				break;

			//求法线
			pcl::computePointNormal(*cloud, pointIdx, m, p_curvature);
			float angle = comupteNormalAngle(n, m);

			if (angle > angle_Threshold) {//2.1
				flag = false;
			}
			else {
				for (int j = 0;j < pointIdx.size();++j) {
					float kkk = abs(cloud->points[pointIdx[j]].z - height);

					if (kkk < Z_range) {//0.2
						pointIdx1.push_back(pointIdx[j]);
						//cout << "	这里取得了一个点kkk:" << kkk<<endl;
						count++;
					}
				}
				result_idx.insert(result_idx.end(), pointIdx1.begin(), pointIdx1.end());
				pointIdx1.clear();
			}
		}//right
	}

	//保存result.pcd
	boost::shared_ptr<std::vector<int>> result_ptr = boost::make_shared<std::vector<int>>(result_idx);

	pcl::ExtractIndices<pcl::PointXYZRGB> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(result_ptr);
	extract.setNegative(false);//如果设为true,可以提取指定index之外的点云
	extract.filter(*result);

	cout << "生长后的点云保存到result.pcd，0result.pcd" << endl;
	pcl::io::savePCDFileASCII("0result.pcd", *result);
	cout << "保存成功" << endl;
	cout << "count: " << count << endl;

	return 0;
}

算法整体完成，但是效果差。更新了ExtractIndices

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>//标准C++库中的输入输出类相关头文件。
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h> //PCL中支持的点类型头文件。
#include <pcl/octree/octree.h>
#include<fstream>  
#include <string>  
#include <vector> 
//#include <LasOperator.h> 
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include<math.h>
#include <pcl/filters/extract_indices.h>

using namespace std;

void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZRGB>& cloud) {
	/*
		*读取las文件
		*/
	std::ifstream ifs(s, std::ios::in | std::ios::binary); // 打开las文件
	liblas::ReaderFactory f;
	liblas::Reader reader = f.CreateWithStream(ifs); // 读取las文件

	unsigned long int nbPoints = reader.GetHeader().GetPointRecordsCount();//获取las数据点的个数

	cloud.width = nbPoints;	//保证与las数据点的个数一致	
	cloud.height = 1;
	cloud.is_dense = false;
	cloud.points.resize(cloud.width * cloud.height);

	int i = 0;
	uint16_t r1, g1, b1;
	int r2, g2, b2;
	uint32_t rgb;
	while (reader.ReadNextPoint()) {
		// 获取las数据的x，y，z信息
		cloud.points[i].x = (reader.GetPoint().GetX());
		cloud.points[i].y = (reader.GetPoint().GetY());
		cloud.points[i].z = (reader.GetPoint().GetZ());

		//获取las数据的r，g，b信息
		r1 = (reader.GetPoint().GetColor().GetRed());
		g1 = (reader.GetPoint().GetColor().GetGreen());
		b1 = (reader.GetPoint().GetColor().GetBlue());
		r2 = ceil(((float)r1 / 65536) * (float)256);
		g2 = ceil(((float)g1 / 65536) * (float)256);
		b2 = ceil(((float)b1 / 65536) * (float)256);
		rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);
		cloud.points[i].rgb = *reinterpret_cast<float*>(&rgb);

		i++;
	}
}
float computeRange(pcl::PointCloud<pcl::PointXYZRGB>& trail, float r, int index, int k) {
	float x1, y1, x2, y2, a1, a2, a3, b1, b2, b3;//向量1，a,b为轨迹线上2点
	x2 = 0; y2 = 1;
	a1 = trail.points[index].x;
	a2 = trail.points[index].y;
	a3 = trail.points[index].z;
	b1 = trail.points[index + k].x;
	b2 = trail.points[index + k].y;
	b3 = trail.points[index + k].z;
	x1 = a1 - b1;
	y1 = a2 - b2;
	float cosa = (x1 * x2 + y1 * y2) / (sqrt(x1 * x1 + y1 * y1) + sqrt(x2 * x2 + y2 * y2));
	float sina = sqrt(1 - cosa * cosa);
	float range = r / sina;
	return range;
}
float comupteNormalAngle(Eigen::Vector4f p, Eigen::Vector4f q) {
	float cos = (p[0] * q[0] + p[1] * q[1] + p[2] * q[2]) / (sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]) * sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]));

	float radian = acos(cos);
	float angle = radian * 180.0 / M_PI;
	if (angle > 90)
		angle = 180 - angle;
	//cout << "cos: " << cos << "\tradian: " << radian << endl;
	return angle;
}

int main() {
	//输入点云，轨迹线，保存种子
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr trail(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr part(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr partSeed(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr seedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointXYZRGB tempPnt;
	std::vector<int>pointIdx;
	Eigen::Vector4f n;
	Eigen::Vector4f m;

	std::vector<int>result_idx;
	//变量
	float p_curvature = 0;

	float r = 10;
	float d = 1;//0.2
	int k = 20;//7

	//读取输入电云
	cout << "读取点云" << endl;
	loadLasFile("o.las", *cloud);
	//读取轨迹线
	cout << "读取轨迹线" << endl;
	pcl::io::loadPCDFile("trail.pcd", *trail);
	cout << "原始点数: " << cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;

	//cloud构建八叉树
	cout << "cloud构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();

	/**
		开始循环
	*/
	//for (size_t i = 0; i + k <= trail->points.size(); i += k) {//cloud1->points.size()		cloud1->points.size()+500   500 i是down，i+k是up
	for (size_t i = 0; i + k <= trail->points.size(); i += k) {//8600,end
		//cout << "-------------------------------------------------------------" << endl;
		cout << i << endl;
		float center = trail->points[i].x;
		float range = computeRange(*trail, r, i, k);
		float left = center - d / 2;
		float right = center + d / 2;
		float Ymin = min(trail->points[i].y, trail->points[i + k].y);
		float Ymax = max(trail->points[i].y, trail->points[i + k].y);
		float Zmin = -10;
		float Zmax = max(trail->points[i].z, trail->points[i + k].z);

		//切割中间的种子区域	中间
		Eigen::Vector3f Emin(left, Ymin, Zmin);
		Eigen::Vector3f Emax(right, Ymax, Zmax);
		octree.boxSearch(Emin, Emax, pointIdx);
		//pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
		result_idx.insert(result_idx.end(), pointIdx.begin(), pointIdx.end());
		//*result += *tempCloud;

		//求中间法线
		pcl::computePointNormal(*cloud, pointIdx, n, p_curvature);

		bool flag = true;//继续循环


		while (flag) {//left
			//cout << "-------------------------------------------------------------" << endl;
			right -= d;
			left -= d;
			if (left <= center - range) {
				flag = false;
				left = center - range;
			}

			//cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			Eigen::Vector3f Emin(left, Ymin, Zmin);
			Eigen::Vector3f Emax(right, Ymax, Zmax);
			octree.boxSearch(Emin, Emax, pointIdx);
			//pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
			
			if (pointIdx.size() == 0)
				break;

			//求法线
			pcl::computePointNormal(*cloud, pointIdx, m, p_curvature);
			float angle = comupteNormalAngle(n, m);
			if (angle >75)//2.1
				flag = false;
			else
			//*result += *tempCloud;
			result_idx.insert(result_idx.end(), pointIdx.begin(), pointIdx.end());
			//result.
		}//left

		flag = true;
		left = center - d / 2;
		right = center + d / 2;


		while (flag) {//right
			right += d;
			left += d;
			if (right >= center + range) {
				flag = false;
				right = center + range;
			}

			Eigen::Vector3f Emin(left, Ymin, Zmin);
			Eigen::Vector3f Emax(right, Ymax, Zmax);
			octree.boxSearch(Emin, Emax, pointIdx);
			//pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
			if (pointIdx.size() == 0)
				break;

			//求法线
			pcl::computePointNormal(*cloud, pointIdx, m, p_curvature);
			float angle = comupteNormalAngle(n, m);

			if (angle > 75) {//2.1
				flag = false;
			}
			else {
				//*result += *tempCloud;
				result_idx.insert(result_idx.end(), pointIdx.begin(), pointIdx.end());
			}
		}//right
	}

	boost::shared_ptr<std::vector<int>> result_ptr = boost::make_shared<std::vector<int>>(result_idx);

	pcl::ExtractIndices<pcl::PointXYZRGB> extract;
	extract.setInputCloud(cloud);
	extract.setIndices(result_ptr);
	extract.setNegative(false);//如果设为true,可以提取指定index之外的点云
	extract.filter(*result);

	cout << "生长后的点云保存到result.pcd，0result.pcd" << endl;
	pcl::io::savePCDFileASCII("0result.pcd", *result);
	cout << "保存成功" << endl;

	/*cout << "切割后的点云保存到seedCloud，seedCloud.pcd" << endl;
	pcl::io::savePCDFileASCII("0seedCloud.pcd", *seedCloud);
	cout << "保存成功" << endl;*/

	return 0;
}

切part

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>//标准C++库中的输入输出类相关头文件。
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h> //PCL中支持的点类型头文件。
#include <pcl/octree/octree.h>
#include<fstream>  
#include <string>  
#include <vector> 
//#include <LasOperator.h> 
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include<math.h>

using namespace std;

void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZRGB>& cloud) {
	/*
		*读取las文件
		*/
	std::ifstream ifs(s, std::ios::in | std::ios::binary); // 打开las文件
	liblas::ReaderFactory f;
	liblas::Reader reader = f.CreateWithStream(ifs); // 读取las文件

	unsigned long int nbPoints = reader.GetHeader().GetPointRecordsCount();//获取las数据点的个数

	cloud.width = nbPoints;	//保证与las数据点的个数一致	
	cloud.height = 1;
	cloud.is_dense = false;
	cloud.points.resize(cloud.width * cloud.height);

	int i = 0;
	uint16_t r1, g1, b1;
	int r2, g2, b2;
	uint32_t rgb;
	while (reader.ReadNextPoint()) {
		// 获取las数据的x，y，z信息
		cloud.points[i].x = (reader.GetPoint().GetX());
		cloud.points[i].y = (reader.GetPoint().GetY());
		cloud.points[i].z = (reader.GetPoint().GetZ());

		//获取las数据的r，g，b信息
		r1 = (reader.GetPoint().GetColor().GetRed());
		g1 = (reader.GetPoint().GetColor().GetGreen());
		b1 = (reader.GetPoint().GetColor().GetBlue());
		r2 = ceil(((float)r1 / 65536) * (float)256);
		g2 = ceil(((float)g1 / 65536) * (float)256);
		b2 = ceil(((float)b1 / 65536) * (float)256);
		rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);
		cloud.points[i].rgb = *reinterpret_cast<float*>(&rgb);

		i++;
	}
}
float computeRange(pcl::PointCloud<pcl::PointXYZRGB>& trail, float r, int index, int k) {
	float x1, y1, x2, y2, a1, a2, a3, b1, b2, b3;//向量1，a,b为轨迹线上2点
	x2 = 0; y2 = 1;
	a1 = trail.points[index].x;
	a2 = trail.points[index].y;
	a3 = trail.points[index].z;
	b1 = trail.points[index + k].x;
	b2 = trail.points[index + k].y;
	b3 = trail.points[index + k].z;
	x1 = a1 - b1;
	y1 = a2 - b2;
	float cosa = (x1 * x2 + y1 * y2) / (sqrt(x1 * x1 + y1 * y1) + sqrt(x2 * x2 + y2 * y2));
	float sina = sqrt(1 - cosa * cosa);
	float range = r / sina;
	return range;
}
float comupteNormalAngle(Eigen::Vector4f p, Eigen::Vector4f q) {
	float cos = (p[0] * q[0] + p[1] * q[1] + p[2] * q[2]) / (sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]) * sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]));

	float radian = acos(cos);
	float angle = radian * 180.0 / M_PI;
	if (angle > 90)
		angle = 180 - angle;
	cout << "cos: " << cos << "\tradian: " << radian << endl;
	return angle;
}

int main() {
	//输入点云，轨迹线，保存种子
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr trail(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr part(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr partSeed(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr seedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointXYZRGB tempPnt;
	std::vector<int>pointIdx;
	Eigen::Vector4f n;
	Eigen::Vector4f m;
	//变量
	float p_curvature = 0;

	float r = 10;
	float d = 0.2;
	int k = 7;

	//读取输入电云
	cout << "读取点云" << endl;
	loadLasFile("o.las", *cloud);
	//读取轨迹线
	cout << "读取轨迹线" << endl;
	pcl::io::loadPCDFile("trail.pcd", *trail);
	cout << "原始点数: " << cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;

	//cloud构建八叉树
	cout << "cloud构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();

	/**
		开始循环
	*/
	//for (size_t i = 0; i < i + k <= trail->points.size(); i += k) {//cloud1->points.size()		cloud1->points.size()+500   500 i是down，i+k是up
	for (size_t i = 8600; i < 8601; i += k) {
		cout << "-------------------------------------------------------------" << endl;
		float center = trail->points[i].x;
		float range = computeRange(*trail, r, i, k);
		float left = center - d / 2;
		float right = center + d / 2;
		float Ymin = min(trail->points[i].y, trail->points[i + k].y);
		float Ymax = max(trail->points[i].y, trail->points[i + k].y);
		float Zmin = -10;
		float Zmax = max(trail->points[i].z, trail->points[i + k].z);

		//切割保存part
		Eigen::Vector3f Emin1(center - range, Ymin, Zmin);
		Eigen::Vector3f Emax1(center + range, Ymax, Zmax);
		octree.boxSearch(Emin1, Emax1, pointIdx);
		pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
		pcl::io::savePCDFileASCII("0part.pcd", *tempCloud);
		cout << "保存part.pcd成功：" << pointIdx.size() << endl;


		//切割中间的种子区域	中间
		Eigen::Vector3f Emin(left, Ymin, Zmin);
		Eigen::Vector3f Emax(right, Ymax, Zmax);
		octree.boxSearch(Emin, Emax, pointIdx);
		cout << "中间这块种子数：" << pointIdx.size() << endl;
		pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);

		*result += *tempCloud;

		//求中间法线
		pcl::computePointNormal(*cloud, pointIdx, n, p_curvature);
		cout << "range: " << range << "\tcurvature: " << p_curvature << endl;
		cout << "中间法向量: " << n[0] << "\t" << n[1] << "\t" << n[2] << "\t" << endl;

		bool flag = true;//继续循环


		while (flag) {//left
			cout << "-------------------------------------------------------------" << endl;
			right -= d;
			left -= d;
			if (left <= center - range) {
				flag = false;
				cout << "到边缘了" << endl;
				left = center - range;
			}

			cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			Eigen::Vector3f Emin(left, Ymin, Zmin);
			Eigen::Vector3f Emax(right, Ymax, Zmax);
			octree.boxSearch(Emin, Emax, pointIdx);
			pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
			cout << "这一段点数：" << pointIdx.size() << endl;
			if (pointIdx.size() == 0)
				break;
			pcl::io::savePCDFileASCII("0tempCloud.pcd", *tempCloud);
			cout << "保存left 0tempcloud.pcd: 成功" << endl;

			//求法线
			pcl::computePointNormal(*cloud, pointIdx, m, p_curvature);
			float angle = comupteNormalAngle(n, m);
			cout << "这一段法向量：" << m[0] << "\t" << m[1] << "\t" << m[2] << endl;
			cout << "角度是：" << angle << endl;

			//getchar();
			if (angle >75)//2.1
				flag = false;
			else
			*result += *tempCloud;
			cout << flag << endl;
		}//left

		flag = true;
		left = center - d / 2;
		right = center + d / 2;
		while (flag) {//right
			cout << "-------------------------------------------------------------" << endl;
			right += d;
			left += d;
			if (right >= center + range) {
				flag = false;
				cout << "到边缘了" << endl;
				right = center + range;
			}

			cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			Eigen::Vector3f Emin(left, Ymin, Zmin);
			Eigen::Vector3f Emax(right, Ymax, Zmax);
			octree.boxSearch(Emin, Emax, pointIdx);
			pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
			cout << "这一段点数：" << pointIdx.size() << endl;
			if (pointIdx.size() == 0)
				break;
			pcl::io::savePCDFileASCII("0tempCloud.pcd", *tempCloud);
			cout << "保存left 0tempcloud.pcd: 成功" << endl;

			//求法线
			pcl::computePointNormal(*cloud, pointIdx, m, p_curvature);
			float angle = comupteNormalAngle(n, m);
			cout << "这一段法向量：" << m[0] << "\t" << m[1] << "\t" << m[2] << endl;
			cout << "角度是：" << angle << endl;

			getchar();
			if (angle > 75) {//2.1
				flag = false;
				cout << "到75°了，停止" << endl;
			}
			else {
				*result += *tempCloud;
			}
			cout << flag << endl;
		}//right
	}

	cout << "生长后的点云保存到result.pcd，0result.pcd" << endl;
	pcl::io::savePCDFileASCII("0result.pcd", *result);
	cout << "保存成功" << endl;

	/*cout << "切割后的点云保存到seedCloud，seedCloud.pcd" << endl;
	pcl::io::savePCDFileASCII("0seedCloud.pcd", *seedCloud);
	cout << "保存成功" << endl;*/

	return 0;
}

更新x方向d，y方向k，可以切part的一半

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>//标准C++库中的输入输出类相关头文件。
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h> //PCL中支持的点类型头文件。
#include <pcl/octree/octree.h>
#include<fstream>  
#include <string>  
#include <vector> 
//#include <LasOperator.h> 
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include<math.h>

using namespace std;

void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZRGB>& cloud) {
	/*
		*读取las文件
		*/
	std::ifstream ifs(s, std::ios::in | std::ios::binary); // 打开las文件
	liblas::ReaderFactory f;
	liblas::Reader reader = f.CreateWithStream(ifs); // 读取las文件

	unsigned long int nbPoints = reader.GetHeader().GetPointRecordsCount();//获取las数据点的个数

	cloud.width = nbPoints;	//保证与las数据点的个数一致	
	cloud.height = 1;
	cloud.is_dense = false;
	cloud.points.resize(cloud.width * cloud.height);

	int i = 0;
	uint16_t r1, g1, b1;
	int r2, g2, b2;
	uint32_t rgb;
	while (reader.ReadNextPoint()) {
		// 获取las数据的x，y，z信息
		cloud.points[i].x = (reader.GetPoint().GetX());
		cloud.points[i].y = (reader.GetPoint().GetY());
		cloud.points[i].z = (reader.GetPoint().GetZ());

		//获取las数据的r，g，b信息
		r1 = (reader.GetPoint().GetColor().GetRed());
		g1 = (reader.GetPoint().GetColor().GetGreen());
		b1 = (reader.GetPoint().GetColor().GetBlue());
		r2 = ceil(((float)r1 / 65536) * (float)256);
		g2 = ceil(((float)g1 / 65536) * (float)256);
		b2 = ceil(((float)b1 / 65536) * (float)256);
		rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);
		cloud.points[i].rgb = *reinterpret_cast<float*>(&rgb);

		i++;
	}
}
float computeRange(pcl::PointCloud<pcl::PointXYZRGB>& trail, float r, int index, int k) {
	float x1, y1, x2, y2, a1, a2, a3, b1, b2, b3;//向量1，a,b为轨迹线上2点
	x2 = 0; y2 = 1;
	a1 = trail.points[index].x;
	a2 = trail.points[index].y;
	a3 = trail.points[index].z;
	b1 = trail.points[index + k].x;
	b2 = trail.points[index + k].y;
	b3 = trail.points[index + k].z;
	x1 = a1 - b1;
	y1 = a2 - b2;
	float cosa = (x1 * x2 + y1 * y2) / (sqrt(x1 * x1 + y1 * y1) + sqrt(x2 * x2 + y2 * y2));
	float sina = sqrt(1 - cosa * cosa);
	float range = r / sina;
	return range;
}
float comupteNormalAngle(Eigen::Vector4f p, Eigen::Vector4f q) {
	float cos = (p[0] * q[0] + p[1] * q[1] + p[2] * q[2]) / (sqrt(p[0] * p[0] + p[1] * p[1] + p[2] * p[2]) * sqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2]));

	float radian = acos(cos);
	float angle = radian * 180.0 / M_PI;
	if (angle > 90)
		angle = 180 - angle;
	cout << "cos: " << cos << "\tradian: " << radian << endl;
	return angle;
}

int main() {
	//输入点云，轨迹线，保存种子
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr trail(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr part(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr partSeed(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr seedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointXYZRGB tempPnt;
	std::vector<int>pointIdx;
	Eigen::Vector4f n;
	Eigen::Vector4f m;
	//变量
	float p_curvature = 0;

	float r = 10;
	float d = 0.2;
	int k = 7;

	//读取输入电云
	cout << "读取点云" << endl;
	loadLasFile("o.las", *cloud);
	//读取轨迹线
	cout << "读取轨迹线" << endl;
	pcl::io::loadPCDFile("trail.pcd", *trail);
	cout << "原始点数: " << cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;

	//cloud构建八叉树
	cout << "cloud构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();

	/**
		开始循环
	*/
	//for (size_t i = 0; i < i + k <= trail->points.size(); i += k) {//cloud1->points.size()		cloud1->points.size()+500   500 i是down，i+k是up
	for (size_t i = 8600; i < 8601; i += k) {
		cout << "-------------------------------------------------------------" << endl;
		float center = trail->points[i].x;
		float range = computeRange(*trail, r, i, k);
		float left = center - d / 2;
		float right = center + d / 2;
		float Ymin = min(trail->points[i].y, trail->points[i + k].y);
		float Ymax = max(trail->points[i].y, trail->points[i + k].y);
		float Zmin = -10;
		float Zmax = max(trail->points[i].z, trail->points[i + k].z);

		//切割保存part
		Eigen::Vector3f Emin1(center - range, Ymin, Zmin);
		Eigen::Vector3f Emax1(center + range, Ymax, Zmax);
		octree.boxSearch(Emin1, Emax1, pointIdx);
		pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
		pcl::io::savePCDFileASCII("0part.pcd", *tempCloud);
		cout << "保存part.pcd成功：" << pointIdx.size() << endl;


		//切割中间的种子区域	中间
		Eigen::Vector3f Emin(left, Ymin, Zmin);
		Eigen::Vector3f Emax(right, Ymax, Zmax);
		octree.boxSearch(Emin, Emax, pointIdx);
		cout << "中间这块种子数：" << pointIdx.size() << endl;
		pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);

		*result += *tempCloud;

		//求中间法线
		pcl::computePointNormal(*cloud, pointIdx, n, p_curvature);
		cout << "range: " << range << "\tcurvature: " << p_curvature << endl;
		cout << "中间法向量: " << n[0] << "\t" << n[1] << "\t" << n[2] << "\t" << endl;

		bool flag = true;//继续循环

		while (flag) {//left
			cout << "-------------------------------------------------------------" << endl;
			right -= d;
			left -= d;
			if (left <= center - range) {
				flag = false;
				cout << "到边缘了" << endl;
				left = center - range;
			}

			cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			Eigen::Vector3f Emin(left, Ymin, Zmin);
			Eigen::Vector3f Emax(right, Ymax, Zmax);
			octree.boxSearch(Emin, Emax, pointIdx);
			pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
			cout << "这一段点数：" << pointIdx.size() << endl;
			if (pointIdx.size() == 0)
				break;
			pcl::io::savePCDFileASCII("0tempCloud.pcd", *tempCloud);
			cout << "保存left 0tempcloud.pcd: 成功" << endl;

			//求法线
			pcl::computePointNormal(*cloud, pointIdx, m, p_curvature);
			float angle = comupteNormalAngle(n, m);
			cout << "这一段法向量：" << m[0] << "\t" << m[1] << "\t" << m[2] << endl;
			cout << "角度是：" << angle << endl;

			//getchar();
			if (angle >75)//2.1
				flag = false;
			else
			*result += *tempCloud;
			cout << flag << endl;
		}
	}

	cout << "生长后的点云保存到result.pcd，0result.pcd" << endl;
	pcl::io::savePCDFileASCII("0result.pcd", *result);
	cout << "保存成功" << endl;

	/*cout << "切割后的点云保存到seedCloud，seedCloud.pcd" << endl;
	pcl::io::savePCDFileASCII("0seedCloud.pcd", *seedCloud);
	cout << "保存成功" << endl;*/

	return 0;
}

加入了求点云法向量函数

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>//标准C++库中的输入输出类相关头文件。
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h> //PCL中支持的点类型头文件。
#include <pcl/octree/octree.h>
#include<fstream>  
#include <string>  
#include <vector> 
//#include <LasOperator.h> 
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include<math.h>

using namespace std;
using namespace pcl;

void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZRGB>& cloud);
void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZRGB>& cloud) {
	/*
		*读取las文件
		*/
	std::ifstream ifs(s, std::ios::in | std::ios::binary); // 打开las文件
	liblas::ReaderFactory f;
	liblas::Reader reader = f.CreateWithStream(ifs); // 读取las文件

	unsigned long int nbPoints = reader.GetHeader().GetPointRecordsCount();//获取las数据点的个数

	cloud.width = nbPoints;	//保证与las数据点的个数一致	
	cloud.height = 1;
	cloud.is_dense = false;
	cloud.points.resize(cloud.width * cloud.height);

	int i = 0;
	uint16_t r1, g1, b1;
	int r2, g2, b2;
	uint32_t rgb;
	while (reader.ReadNextPoint()) {
		// 获取las数据的x，y，z信息
		cloud.points[i].x = (reader.GetPoint().GetX());
		cloud.points[i].y = (reader.GetPoint().GetY());
		cloud.points[i].z = (reader.GetPoint().GetZ());

		//获取las数据的r，g，b信息
		r1 = (reader.GetPoint().GetColor().GetRed());
		g1 = (reader.GetPoint().GetColor().GetGreen());
		b1 = (reader.GetPoint().GetColor().GetBlue());
		r2 = ceil(((float)r1 / 65536) * (float)256);
		g2 = ceil(((float)g1 / 65536) * (float)256);
		b2 = ceil(((float)b1 / 65536) * (float)256);
		rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);
		cloud.points[i].rgb = *reinterpret_cast<float*>(&rgb);

		i++;
	}
}

int main() {

	/**
		定义变量
	*/
	//输入点云，轨迹线，保存种子
	PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr trail(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr part(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr partSeed(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr seedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointXYZRGB tempPnt;
	std::vector<int>pointIdx;
	Eigen::Vector4f plane;
	//变量
	float x1, y1, x2, y2, a1, a2, a3, b11, b22, b3;//向量1，a,b为轨迹线上2点
	x1 = y1 = a1 = a2 = a3 = b11 = b22 = b3 = 0;
	x2 = 0; y2 = 1;

	float n1, n2, n3, m1, m2, m3;
	n1 = n2 = n3 = m1 = m2 = m3 = 0;
	float p_curvature = 0;

	float r = 10;	//半径2
	float d = 0.4;

	//读取输入电云
	cout << "读取点云" << endl;
	loadLasFile("o.las", *cloud);
	//读取轨迹线
	cout << "读取轨迹线" << endl;
	pcl::io::loadPCDFile("trail.pcd", *trail);
	cout << "原始点数: " << cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;

	//cloud构建八叉树
	cout << "cloud构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();

	/**
		开始循环
	*/
	for (size_t i = 8820; i < 8821; i += 20) {//cloud1->points.size()		cloud1->points.size()+500   500
		cout << "-------------------------------------------------------------" << endl;
		a1 = trail->points[i].x;
		a2 = trail->points[i].y;//cloud1->points[i+8].y
		a3 = trail->points[i].z;
		b11 = trail->points[i + 20].x;
		b22 = trail->points[i + 20].y;//cloud1->points[i+12].y
		b3 = trail->points[i + 20].z;
		x1 = a1 - b11;
		y1 = a2 - b22;
		float cosa = (x1 * x2 + y1 * y2) / (sqrt(x1 * x1 + y1 * y1) + sqrt(x2 * x2 + y2 * y2));
		float sina = sqrt(1 - cosa * cosa);
		float range = r / sina;

		//分段  part
		Eigen::Vector3f n(a1 - range, min(a2, b22), -100000);//0xc0c0c0c0	0x3f3f3f3f
		Eigen::Vector3f m(a1 + range, max(a2, b22) - 0.001, min(a3, b3));
		octree.boxSearch(n, m, pointIdx);
		cout << i << " 这段有点数：" << pointIdx.size() << "\t";
		pcl::copyPointCloud(*cloud, pointIdx, *part);
		pcl::io::savePCDFileASCII("0part.pcd", *part);
		cout << "保存成功part" << endl;

		//切割中间的种子区域	partSeed
		pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree1(resolution);
		octree1.setInputCloud(part);
		octree1.addPointsFromInputCloud();

		Eigen::Vector3f p(a1 - d / 2, min(a2, b22), -100000);//min(a2, b22)
		Eigen::Vector3f q(a1 + d / 2, max(a2, b22) - 0.001, min(a3, b3));//max(a2, b22) - 0.001
		octree1.boxSearch(p, q, pointIdx);
		cout << "中间这块种子数：" << pointIdx.size() << endl;
		pcl::copyPointCloud(*part, pointIdx, *partSeed);
		//*seedCloud += *partSeed;
		*result += *partSeed;
		/*pcl::io::savePCDFileASCII("0partSeed.pcd", *partSeed);
		cout << "保存成功partSeed" << endl;*/

		//求partSeed法线
		pcl::computePointNormal(*part, pointIdx, plane, p_curvature);

		n1 = plane[0];
		n2 = plane[1];
		n3 = plane[2];
		cout << "中间这块种子的法向量: " << n1 << "\t" << n2 << "\t" << n3 << "\t" << endl;

		bool flag = true;
		float left, right;
		left = a1 - d / 2;
		right = a1 + d / 2;

		while (flag) {//left
			cout << "-------------------------------------------------------------" << endl;
			right -= d;
			left -= d;
			cout << "left-right: " << "\t" << left << "\t" << right << "\t" << endl;
			Eigen::Vector3f pp(left, min(a2, b22), -100000);//min(a2, b22)
			Eigen::Vector3f qq(right, max(a2, b22) - 0.001, min(a3, b3));//max(a2, b22) - 0.001
			octree1.boxSearch(pp, qq, pointIdx);
			pcl::copyPointCloud(*part, pointIdx, *tempCloud);
			cout << "这一段点数：" << pointIdx.size() << endl;
			pcl::io::savePCDFileASCII("0tempCloud.pcd", *tempCloud);
			cout << "保存left tempcloud.pcd: 成功" << endl;

			if (pointIdx.size() == 0) {
				cout << "到边缘了" << endl;
				break;
			}

			//求法线
			pcl::computePointNormal(*part, pointIdx, plane, p_curvature);

			m1 = plane[0];
			m2 = plane[1];
			m3 = plane[2];
			float cos = (n1 * m1 + n2 * m2 + n3 * m3) / (sqrt(n1 * n1 + n2 * n2 + n3 * n3) + sqrt(m1 * m1 + m2 * m2 + m3 * m3));
			float angle = acos(cos);
			cout << "这一段法向量：" << m1 << "\t" << m2 << "\t" << m3 << endl;
			cout << "角度是：" << angle << endl;
			
			getchar();
			if (angle > 2.1)
				flag = false;
			else
				*result += *tempCloud;
			cout << flag << endl;
		}
	}

	cout << "生长后的点云保存到result.pcd，0result.pcd" << endl;
	pcl::io::savePCDFileASCII("0result.pcd", *result);
	cout << "保存成功" << endl;

	/*cout << "切割后的点云保存到seedCloud，seedCloud.pcd" << endl;
	pcl::io::savePCDFileASCII("0seedCloud.pcd", *seedCloud);
	cout << "保存成功" << endl;*/

	return 0;
}

初版

#include <pcl/visualization/cloud_viewer.h>
#include <iostream>//标准C++库中的输入输出类相关头文件。
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>//pcd 读写类相关的头文件。
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h> //PCL中支持的点类型头文件。
#include <pcl/octree/octree.h>
#include<fstream>  
#include <string>  
#include <vector> 
//#include <LasOperator.h> 
#include <liblas/liblas.hpp>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include<math.h>

using namespace std;
using namespace pcl;

void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZRGB>& cloud);
void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZRGB>& cloud) {
	/*
		*读取las文件
		*/
	std::ifstream ifs(s, std::ios::in | std::ios::binary); // 打开las文件
	liblas::ReaderFactory f;
	liblas::Reader reader = f.CreateWithStream(ifs); // 读取las文件

	unsigned long int nbPoints = reader.GetHeader().GetPointRecordsCount();//获取las数据点的个数

	cloud.width = nbPoints;	//保证与las数据点的个数一致	
	cloud.height = 1;
	cloud.is_dense = false;
	cloud.points.resize(cloud.width * cloud.height);

	int i = 0;
	uint16_t r1, g1, b1;
	int r2, g2, b2;
	uint32_t rgb;
	while (reader.ReadNextPoint()) {
		// 获取las数据的x，y，z信息
		cloud.points[i].x = (reader.GetPoint().GetX());
		cloud.points[i].y = (reader.GetPoint().GetY());
		cloud.points[i].z = (reader.GetPoint().GetZ());

		//获取las数据的r，g，b信息
		r1 = (reader.GetPoint().GetColor().GetRed());
		g1 = (reader.GetPoint().GetColor().GetGreen());
		b1 = (reader.GetPoint().GetColor().GetBlue());
		r2 = ceil(((float)r1 / 65536) * (float)256);
		g2 = ceil(((float)g1 / 65536) * (float)256);
		b2 = ceil(((float)b1 / 65536) * (float)256);
		rgb = ((int)r2) << 16 | ((int)g2) << 8 | ((int)b2);
		cloud.points[i].rgb = *reinterpret_cast<float*>(&rgb);

		i++;
	}
}

int main() {

	/**
		定义变量
	*/
	//输入点云，轨迹线，保存种子
	PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr trail(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr part(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr partSeed(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr seedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGB>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
	pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr tempCloudNormal(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
	pcl::PointXYZRGB tempPnt;
	std::vector<int>pointIdx;
	//变量
	float x1, y1, x2, y2, a1, a2, a3, b11, b22, b3;//向量1，a,b为轨迹线上2点
	x1 = y1 = a1 = a2 = a3 = b11 = b22 = b3 = 0;
	x2 = 0; y2 = 1;

	float n1, n2, n3, m1, m2, m3;
	n1 = n2 = n3 = m1 = m2 = m3 = 0;

	float r = 10;	//半径2
	float d = 0.4;

	//读取输入电云
	cout << "读取点云" << endl;
	loadLasFile("o.las", *cloud);
	//读取轨迹线
	cout << "读取轨迹线" << endl;
	pcl::io::loadPCDFile("trail.pcd", *trail);
	cout << "原始点数: " << cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;

	//cloud构建八叉树
	cout << "cloud构建八叉树" << endl;
	float resolution = 128.0f;
	pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution);
	octree.setInputCloud(cloud);
	octree.addPointsFromInputCloud();

	/**
		开始循环
	*/
	for (size_t i = 8000; i < 8001; i += 20) {//cloud1->points.size()		cloud1->points.size()+500   500
		cout << "-------------------------------------------------------------" << endl;
		a1 = trail->points[i].x;
		a2 = trail->points[i].y;//cloud1->points[i+8].y
		a3 = trail->points[i].z;
		b11 = trail->points[i + 20].x;
		b22 = trail->points[i + 20].y;//cloud1->points[i+12].y
		b3 = trail->points[i + 20].z;
		x1 = a1 - b11;
		y1 = a2 - b22;
		float cosa = (x1 * x2 + y1 * y2) / (sqrt(x1 * x1 + y1 * y1) + sqrt(x2 * x2 + y2 * y2));
		float sina = sqrt(1 - cosa * cosa);
		float range = r / sina;

		//分段  part
		Eigen::Vector3f n(a1 - range, min(a2, b22), -100000);//0xc0c0c0c0	0x3f3f3f3f
		Eigen::Vector3f m(a1 + range, max(a2, b22) - 0.001, min(a3, b3));
		octree.boxSearch(n, m, pointIdx);
		cout << i << " 这段有点数：" << pointIdx.size() << "\t";
		pcl::copyPointCloud(*cloud, pointIdx, *part);
		pcl::io::savePCDFileASCII("0part.pcd", *part);
		cout << "保存成功part" << endl;

		//切割中间的种子区域	partSeed
		pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree1(resolution);
		octree1.setInputCloud(part);
		octree1.addPointsFromInputCloud();

		Eigen::Vector3f p(a1 - d, min(a2, b22), -100000);//min(a2, b22)
		Eigen::Vector3f q(a1 + d, max(a2, b22) - 0.001, min(a3, b3));//max(a2, b22) - 0.001
		octree1.boxSearch(p, q, pointIdx);
		cout << "这段种子数：" << pointIdx.size();
		pcl::copyPointCloud(*part, pointIdx, *partSeed);
		//*seedCloud += *partSeed;
		*result += *partSeed;
		/*pcl::io::savePCDFileASCII("0partSeed.pcd", *partSeed);
		cout << "保存成功partSeed" << endl;*/

		//求法线
		pcl::search::Search<pcl::PointXYZRGB>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGB> >(new pcl::search::KdTree<pcl::PointXYZRGB>);
		pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);
		pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimator;
		normal_estimator.setSearchMethod(tree);
		normal_estimator.setInputCloud(partSeed);
		//normal_estimator.setKSearch(8000);
		normal_estimator.setRadiusSearch(0.4);
		normal_estimator.compute(*normals);
		//computePointNormal(const pcl::PointCloud<PointInT> & cloud, const std::vector<int> & indices,
		//	float& nx, float& ny, float& nz, float& curvature)
		//normal_estimator.computePointNormal(partSeed,);


		//pcl::computeCovarianceMatrixNormalized();

		n1 = normals->points[200].normal_x;
		n2 = normals->points[200].normal_y;
		n3 = normals->points[200].normal_z;

		bool flag = true;
		float left, right;
		left = a1 - d / 2;
		right = a1 + d / 2;

		while (flag) {//left
			right = left;
			left = right - d;
			cout << "left-right:" << "\t" << left << "\t" << right << "\t" << endl;
			Eigen::Vector3f pp(left, min(a2, b22), -100000);//min(a2, b22)
			Eigen::Vector3f qq(right, max(a2, b22) - 0.001, min(a3, b3));//max(a2, b22) - 0.001
			octree1.boxSearch(pp, qq, pointIdx);
			pcl::copyPointCloud(*part, pointIdx, *tempCloud);
			if (pointIdx.size() == 0) {
				cout << "到边缘了" << endl;
				break;
			}

			//求法线
			pcl::search::Search<pcl::PointXYZRGB>::Ptr tree1 = boost::shared_ptr<pcl::search::Search<pcl::PointXYZRGB> >(new pcl::search::KdTree<pcl::PointXYZRGB>);
			pcl::PointCloud <pcl::Normal>::Ptr normals1(new pcl::PointCloud <pcl::Normal>);
			pcl::NormalEstimation<pcl::PointXYZRGB, pcl::Normal> normal_estimator1;
			normal_estimator1.setSearchMethod(tree1);
			normal_estimator1.setInputCloud(tempCloud);
			normal_estimator1.setRadiusSearch(0.4);
			//normal_estimator1.setKSearch(1000);
			normal_estimator1.compute(*normals1);
			normal_estimator1.computePointNormal;
			

			//pcl::concatenateFields(*tempCloud, *normals1, *tempCloudNormal);


			cout << "这一段点数：" << pointIdx.size() << endl;
			pcl::io::savePCDFileASCII("0tempCloud.pcd", *tempCloud);
			cout << "保存left tempcloud.pcd: 成功" << endl;

			//取中间点的法向量
			pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree2(resolution);
			octree2.setInputCloud(tempCloud);
			octree2.addPointsFromInputCloud();
			Eigen::Vector3f ppp(left + d / 2 - 0.01, trail->points[i + 8].y, -100000);//min(a2, b22)
			Eigen::Vector3f qqq(right - d / 2 + 0.01, trail->points[i + 12].y, min(a3, b3));//max(a2, b22) - 0.001
			octree2.boxSearch(ppp, qqq, pointIdx);
			cout << "可取的向量点处处 ：" << pointIdx.size();
			//pcl::copyPointCloud(*part, pointIdx, *partSeed);

			m1 = normals1->points[pointIdx[0]].normal_x;
			m2 = normals1->points[pointIdx[0]].normal_y;
			m3 = normals1->points[pointIdx[0]].normal_z;
			float cos = (n1 * m1 + n2 * m2 + n3 * m3) / (sqrt(n1 * n1 + n2 * n2 + n3 * n3) + sqrt(m1 * m1 + m2 * m2 + m3 * m3));
			float angle = acos(cos);
			cout << "这一段法向量：" << m1 << "\t" << m2 << "\t" << m3 << endl;
			cout << "角度是：" << angle << endl;
			int kkk = 0;
			cin >> kkk;
			if (angle > 2.1)
				flag = false;
			else
				*result += *tempCloud;
			cout << flag << endl;
		}
	}

	cout << "生长后的点云保存到result.pcd，0result.pcd" << endl;
	pcl::io::savePCDFileASCII("0result.pcd", *result);
	cout << "保存成功" << endl;

	/*cout << "切割后的点云保存到seedCloud，seedCloud.pcd" << endl;
	pcl::io::savePCDFileASCII("0seedCloud.pcd", *seedCloud);
	cout << "保存成功" << endl;*/

	return 0;
}