xyzi
#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 "LasEdit.h"
using namespace std;
void loadLasFile(string s, pcl::PointCloud<pcl::PointXYZ>& cloud);
float computeRange(pcl::PointCloud<pcl::PointXYZI>& trail, float r, int index, int k);
void loadLASFileRGB(string s, pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud);
void saveLASFileRGB(string s, pcl::PointCloud<pcl::PointXYZI>::Ptr save_cloud);
int main() {
//������ƣ��켣�ߣ���������
pcl::PointCloud<pcl::PointXYZI>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr trail(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr part(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr midle_filtered(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr result(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZI>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointXYZI tempPnt;
std::vector<int>idx;
std::vector<int>part_idx;
//����
float r = 10000; //o1,ʹ��+ 2.5��5��10ƫС��5�պã�10����
int k = 6;//20
//const std::string path=
//��ȡ�������
cout << "read cloud" << endl;
CLasEdit le;
le.readLas2PointCloudXYZI("/home/qrh/桌面/experimentData/3data/data1.las",input_cloud);
//loadLASFileRGB("/home/qrh/桌面/experimentData/3data/data1.las",input_cloud);
//��ȡ�켣��
cout << "read trajectory" << endl;
le.readLas2PointCloudXYZI("/home/qrh/桌面/experimentData/3data/trajectory1.las", trail);
//loadLASFileRGB("/home/qrh/桌面/experimentData/3data/trajectory1.las", trail);
cout << "cloud_size:::::" << input_cloud->width << " \t" << "trajectory_size:::::: " << trail->width << endl;
//cloud�����˲���
cout << "�����˲���" << endl;
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZI> octree(resolution);
octree.setInputCloud(input_cloud);
octree.addPointsFromInputCloud();
cout << "�������" << endl;
/**
�и����·�沿��
*/
cout << "�и����·�沿��" << endl;
for (size_t i = 8000;i<9600; 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 = -10000;
float Zmax = 10000;
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;
part_idx.insert(part_idx.end(), idx.begin(), idx.end());
}
//��ȡmidle
boost::shared_ptr<std::vector<int>> midle_ptr = boost::make_shared<std::vector<int>>(part_idx);
pcl::ExtractIndices<pcl::PointXYZI> extract;
extract.setInputCloud(input_cloud);
extract.setIndices(midle_ptr);
extract.setNegative(false);//�����Ϊtrue,������ȡָ��index֮��ĵ���
extract.filter(*part);
cout << "С�α���Ϊ: data1_1.las" << endl;
le.savePointCloudXYZI2Las( part,"/home/qrh/桌面/experimentData/data1_1/record.las");
//saveLASFileRGB("/home/qrh/桌面/experimentData/data1_1/record.las", part);
//pcl::io::savePCDFileASCII("o_midle_filtered.pcd", *midle_filtered);
cout << "����ɹ�" << endl;
//��ȡ�켣��
cout << "�켣����Ϊ: data1_1.las" << endl;
tempCloud->width = 1600;
tempCloud->height = 1;
tempCloud->points.resize(tempCloud->width*tempCloud->height);
int count = 0;
// for (int i = 8000;i < 9600;++i) {
for (int i = 9599;i >=8000;--i) {
cout << 1 << endl;
/*tempPnt.x = trail->points[i].x;
tempPnt.y = trail->points[i].y;
tempPnt.z = trail->points[i].z;
tempPnt.rgb = trail->points[i].rgb;*/
//tempCloud->points[i] = tempPnt;
tempCloud->points[count++] = trail->points[i];
cout << "ok" << endl;
}
le.savePointCloudXYZI2Las(tempCloud,"/home/qrh/桌面/experimentData/data1_1/trajectory.las");
//saveLASFileRGB("/home/qrh/桌面/experimentData/data1_1/trajectory.las",tempCloud);
return 0;
}//main
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());
i++;
}
}
/**
* ����켣�㴦x�����䷶Χ
*/
float computeRange(pcl::PointCloud<pcl::PointXYZI>& trail, float r, int index, int k) {
//����n1��n2,a,bΪ�켣����2��
float x1, y1, x2, y2, a1, a2, a3, b1, b2, b3;
x2 = 1; y2 = 0;//n2Ϊx��������λ����
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;
}
/**
* ��ȡRGB las�ļ�
*/
void loadLASFileRGB(string s, pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud) {
// ��las�ļ�
std::ifstream ifs(s, std::ios::in | std::ios::binary);
// ��ȡlas�ļ�
liblas::ReaderFactory f;
liblas::Reader reader = f.CreateWithStream(ifs);
//��ȡ����ͷ
liblas::Header const& header = reader.GetHeader();
//���õ�������
input_cloud->width = header.GetPointRecordsCount(); //��֤��las���ݵ�ĸ���һ��
input_cloud->height = 1;//��ʾ�������
input_cloud->is_dense = false;//��ʾ���ܼ�����
input_cloud->points.resize(input_cloud->width * input_cloud->height);//���µ�ĸ���
int index = 0;
uint16_t red_1, green_1, black_1;
int red_2, green_2, black_2;
uint32_t rgb;
while (reader.ReadNextPoint()) {
liblas::Point const& temp_point = reader.GetPoint();
// ��ȡlas�ļ�3D����
input_cloud->points[index].x = (temp_point.GetX());//x����
input_cloud->points[index].y = (temp_point.GetY());//y����
input_cloud->points[index].z = (temp_point.GetZ());//z����
//��ȡlas�ļ���ɫ��Ϣ
red_1 = (temp_point.GetColor().GetRed());//red
green_1 = (temp_point.GetColor().GetGreen());//green
black_1 = (temp_point.GetColor().GetBlue());//black
//������ɫת��
red_2 = ceil(((float)red_1 / 65536) * (float)256);
green_2 = ceil(((float)green_1 / 65536) * (float)256);
black_2 = ceil(((float)black_1 / 65536) * (float)256);
rgb = ((int)red_2) << 16 | ((int)green_2) << 8 | ((int)black_2);
input_cloud->points[index].rgb = *reinterpret_cast<float*>(&rgb);
index++;
}
ifs.close();
}
/**
* ����RGB las�ļ�
*/
void saveLASFileRGB(string s, pcl::PointCloud<pcl::PointXYZRGB>::Ptr save_cloud) {
cout << save_cloud->points.size() << endl;
//���ļ�
std::ofstream ofs(s, ios::out | ios::binary);
liblas::Header header;
//���õ�����
header.SetPointRecordsCount(save_cloud->points.size());
//����x��y��z��������
header.SetScale(0.0001, 0.0001, 0.0001);
//ƫ����
header.SetOffset(0.0, 0.0, 0.0);
// 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 < save_cloud->points.size();++i) {
//���õ�x��y��z����
point.SetCoordinates(save_cloud->points[i].x, save_cloud->points[i].y, save_cloud->points[i].z);
point.SetColor(liblas::Color(save_cloud->points[i].r, save_cloud->points[i].g, save_cloud->points[i].b));
writer.WritePoint(point);
}
//writer.SetHeader(header);
//ofs.flush();
ofs.close();
}
xyzrgb
#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);
float computeRange(pcl::PointCloud<pcl::PointXYZRGB>& trail, float r, int index, int k);
void loadLASFileRGB(string s, pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud);
void saveLASFileRGB(string s, pcl::PointCloud<pcl::PointXYZRGB>::Ptr save_cloud);
int main() {
//输入点云,轨迹线,保存种子
pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
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 part(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr midle_filtered(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr result(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tempCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointXYZRGB tempPnt;
std::vector<int>idx;
std::vector<int>part_idx;
//变量
float r = 10000; //o1,使用+ 2.5,5,10偏小,5刚好,10覆盖
int k = 6;//20
//读取输入点云
cout << "读取点云" << endl;
loadLASFileRGB("data1.las",input_cloud);
//读取轨迹线
cout << "读取轨迹线" << endl;
loadLASFileRGB("trajectory1.las", trail);
cout << "原始点数: " << input_cloud->width << " \t" << "轨迹线点数: " << trail->width << endl;
//cloud构建八叉树
cout << "构建八叉树" << endl;
float resolution = 128.0f;
pcl::octree::OctreePointCloudSearch<pcl::PointXYZRGB> octree(resolution);
octree.setInputCloud(input_cloud);
octree.addPointsFromInputCloud();
cout << "构建完成" << endl;
/**
切割宽于路面部分
*/
cout << "切割宽于路面部分" << endl;
for (size_t i = 8000;i<9600; 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 = -10000;
float Zmax = 10000;
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;
part_idx.insert(part_idx.end(), idx.begin(), idx.end());
}
//提取midle
boost::shared_ptr<std::vector<int>> midle_ptr = boost::make_shared<std::vector<int>>(part_idx);
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
extract.setInputCloud(input_cloud);
extract.setIndices(midle_ptr);
extract.setNegative(false);//如果设为true,可以提取指定index之外的点云
extract.filter(*part);
cout << "小段保存为: data1_1.las" << endl;
saveLASFileRGB("data1_1.las", part);
//pcl::io::savePCDFileASCII("o_midle_filtered.pcd", *midle_filtered);
cout << "保存成功" << endl;
//提取轨迹线
cout << "轨迹保存为: data1_1.las" << endl;
tempCloud->width = 1600;
tempCloud->height = 1;
tempCloud->points.resize(tempCloud->width*tempCloud->height);
int count = 0;
for (int i = 8000;i < 9600;++i) {
cout << 1 << endl;
/*tempPnt.x = trail->points[i].x;
tempPnt.y = trail->points[i].y;
tempPnt.z = trail->points[i].z;
tempPnt.rgb = trail->points[i].rgb;*/
//tempCloud->points[i] = tempPnt;
tempCloud->points[count++] = trail->points[i];
cout << "ok" << endl;
}
saveLASFileRGB("data1_1_trajectory.las",tempCloud);
return 0;
}//main
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());
i++;
}
}
/**
* 计算轨迹点处x轴两变范围
*/
float computeRange(pcl::PointCloud<pcl::PointXYZRGB>& trail, float r, int index, int k) {
//向量n1,n2,a,b为轨迹线上2点
float x1, y1, x2, y2, a1, a2, a3, b1, b2, b3;
x2 = 1; y2 = 0;//n2为x轴正方向单位向量
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;
}
/**
* 读取RGB las文件
*/
void loadLASFileRGB(string s, pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud) {
// 打开las文件
std::ifstream ifs(s, std::ios::in | std::ios::binary);
// 读取las文件
liblas::ReaderFactory f;
liblas::Reader reader = f.CreateWithStream(ifs);
//获取公共头
liblas::Header const& header = reader.GetHeader();
//设置点云属性
input_cloud->width = header.GetPointRecordsCount(); //保证与las数据点的个数一致
input_cloud->height = 1;//表示无序点云
input_cloud->is_dense = false;//表示非密集点云
input_cloud->points.resize(input_cloud->width * input_cloud->height);//更新点的个数
int index = 0;
uint16_t red_1, green_1, black_1;
int red_2, green_2, black_2;
uint32_t rgb;
while (reader.ReadNextPoint()) {
liblas::Point const& temp_point = reader.GetPoint();
// 获取las文件3D坐标
input_cloud->points[index].x = (temp_point.GetX());//x坐标
input_cloud->points[index].y = (temp_point.GetY());//y坐标
input_cloud->points[index].z = (temp_point.GetZ());//z坐标
//获取las文件颜色信息
red_1 = (temp_point.GetColor().GetRed());//red
green_1 = (temp_point.GetColor().GetGreen());//green
black_1 = (temp_point.GetColor().GetBlue());//black
//进行颜色转换
red_2 = ceil(((float)red_1 / 65536) * (float)256);
green_2 = ceil(((float)green_1 / 65536) * (float)256);
black_2 = ceil(((float)black_1 / 65536) * (float)256);
rgb = ((int)red_2) << 16 | ((int)green_2) << 8 | ((int)black_2);
input_cloud->points[index].rgb = *reinterpret_cast<float*>(&rgb);
index++;
}
ifs.close();
}
/**
* 保存RGB las文件
*/
void saveLASFileRGB(string s, pcl::PointCloud<pcl::PointXYZRGB>::Ptr save_cloud) {
cout << save_cloud->points.size() << endl;
//打开文件
std::ofstream ofs(s, ios::out | ios::binary);
liblas::Header header;
//设置点数量
header.SetPointRecordsCount(save_cloud->points.size());
//设置x,y,z比例因子
header.SetScale(0.0001, 0.0001, 0.0001);
//偏移量
header.SetOffset(0.0, 0.0, 0.0);
// 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 < save_cloud->points.size();++i) {
//设置点x,y,z坐标
point.SetCoordinates(save_cloud->points[i].x, save_cloud->points[i].y, save_cloud->points[i].z);
point.SetColor(liblas::Color(save_cloud->points[i].r, save_cloud->points[i].g, save_cloud->points[i].b));
writer.WritePoint(point);
}
//writer.SetHeader(header);
//ofs.flush();
ofs.close();
}