使用computeRange，切割完整路面种子点

frontierq

于 2020-04-14 20:22:44 发布

阅读量118

点赞数

分类专栏： 0毕设

本文链接：https://blog.csdn.net/qq_36477987/article/details/105485369

版权

0毕设专栏收录该内容

9 篇文章 1 订阅

订阅专栏

使用+，更新k，r，切比路面宽一点

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

using namespace std;

void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZ>& cloud);
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;
}

int main() {
	//输入点云，轨迹线，保存种子
	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 result(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr seedCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr colored(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointXYZ tempPnt;
	std::vector<int>pointIdx;
	//变量

	float r = 10;	//o2,使用+ 	2.5，5，10偏小，5刚好，12全覆盖
	int k = 7;//20

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

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

	/**
		开始循环
	*/
	for (size_t i = 0;i + k < trail->points.size(); 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, pointIdx);
		cout << i << "\t这段种子数：\t" << pointIdx.size() << endl;
		pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
		*seedCloud += *tempCloud;
	}




	cout << "切割后的点云: 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>

using namespace std;

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++;
	}
}
float computeRange(pcl::PointCloud<pcl::PointXYZRGB>& trail, float r, int index) {
	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 + 20].x;
	b2 = trail.points[index + 20].y;
	b3 = trail.points[index + 20].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;
}

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

	float r = 10;	//半径	1偏小，5刚好，7，10全覆盖

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

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

	/**
		开始循环
	*/
	for (size_t i = 0;i + 20 <= trail->points.size(); i += 20) {
		cout << "-------------------------------------------------------------" << endl;
		float range = computeRange(*trail, r, i);
		float center = trail->points[i].x;
		float Ymin = min(trail->points[i].y, trail->points[i + 20].y);
		float Ymax = max(trail->points[i].y, trail->points[i + 20].y);
		float Zmin = -10;
		float Zmax = max(trail->points[i].z, trail->points[i + 20].z);
		cout << "range: " << range << endl;
		//分段切割种子点
		Eigen::Vector3f Emin(trail->points[i].x - range, Ymin, Zmin);//0xc0c0c0c0	0x3f3f3f3f
		Eigen::Vector3f Emax(trail->points[i].x + range, Ymax - 0.001, Zmax);
		octree.boxSearch(Emin, Emax, pointIdx);
		cout << i << "\t这段种子数：\t" << pointIdx.size() << endl;
		pcl::copyPointCloud(*cloud, pointIdx, *tempCloud);
		*seedCloud += *tempCloud;

	}

	//cout << "上色的点云: colored.pcd" << endl;
	//pcl::io::savePCDFileASCII("0colored.pcd", *colored);
	//cout << "保存成功" << endl;

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

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

	return 0;
}

frontierq

关注

0
点赞
踩
0

收藏

觉得还不错? 一键收藏
0
评论
使用computeRange，切割完整路面种子点

初版#include <pcl/visualization/cloud_viewer.h>#include <iostream>//标准C++库中的输入输出类相关头文件。#include <pcl/io/io.h>#include <pcl/io/pcd_io.h>//pcd 读写类相关的头文件。#include <pcl/io/pl...
复制链接

扫一扫

专栏目录