切除道路上方高架

#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;

	float minx, maxx, miny, maxy;
	minx = maxx = miny = maxy = 0;
	minx = miny = 10000;
	maxx = maxy = -10000;
	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());
		if (p.GetX() < minx)
			minx = p.GetX();
		if (p.GetX() > maxx)
			maxx = p.GetX();
		if (p.GetY() < miny)
			miny = p.GetY();
		if (p.GetY() > maxy)
			maxy = p.GetY();

		i++;
	}

	float dx = maxx - minx;
	float dy = maxy - miny;
	cout << "x range: " << minx << "\t" << maxx << "\t" << dx << endl;
	cout << "y range: " << miny << "\t" << maxy << "\t" << dy << endl;
	float area = dx * dy;

	cout << header.GetPointRecordsCount() << endl;
	cout << "点密度" << header.GetPointRecordsCount() / area << endl;
	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;
	x2 = 1; y2 = 0;
	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;
}

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 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>midle_idx;
	//变量

	float r = 13;	//o1,使用+ 	2.5，5，10偏小，5刚好，10覆盖
	int k = 6;//20

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

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

	/**
		切割宽于路面部分
	*/

	k = computeK(trail, 0);
	cout << "切割宽于路面部分" << endl;
	for (size_t i = 0;i + k < trail->points.size(); ) {
		//for (size_t i = 0;i + k < 10000;) {
		cout << "-------------------------------------------------------------" << endl;
		float range = computeRange(*trail, r, i, k);
		//float range = 10000;
		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+3);
		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());

		//分段left
		Eigen::Vector3f Emin1(-10000, Ymin, Zmin);//0xc0c0c0c0	0x3f3f3f3f
		Eigen::Vector3f Emax1(center - range, Ymax, 10000);
		octree.boxSearch(Emin1, Emax1, idx);
		cout << i << "\t这段种子数：\t" << idx.size() << endl;
		midle_idx.insert(midle_idx.end(), idx.begin(), idx.end());


		//分段right
		Eigen::Vector3f Emin2(center+range, Ymin, Zmin);//0xc0c0c0c0	0x3f3f3f3f
		Eigen::Vector3f Emax2(10000, Ymax, 10000);
		octree.boxSearch(Emin2, Emax2, idx);
		cout << i << "\t这段种子数：\t" << idx.size() << endl;
		midle_idx.insert(midle_idx.end(), idx.begin(), idx.end());


		i += k;

		k = computeK(trail, i + k);
	}
	cout << "出来了" << endl;

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

	saveLasFile("o3_new.las", midle);
	cout << "保存midle.las成功" << endl;


	return 0;
}