激光雷达与PCL点云库学习
运行环境是树莓派4b,很多包不能用指令下载1、ros中常用的包从下面这个链接下载(视觉、pcl、激光等等)2、一些找不到的包可以从github上搜(有时候名字不是表明那个,可以直接百度一下)3、PCL点云库:红色是X轴,绿色是Y轴,蓝色是Z1、必须懂得的概念编译的debug(慢)、release动态库(dll)、静态库(lib)C++的模板编程与基本用法cmake的原理和简单使用Elem* dat
前言:
运行环境是树莓派4b,很多包不能用指令下载
1、ros中常用的包从下面这个链接下载(视觉、pcl、激光等等)
https://github.com/ros-perception
2、一些找不到的包可以从github上搜(有时候名字不是表明那个,可以直接百度一下)
3、PCL点云库:红色是X轴,绿色是Y轴,蓝色是Z
一、基础知识
1、常见名词
-
角分辨率
激光雷达输出的图像也被称为“点云”图像,相邻两个点之间的夹角就是角分辨率。
-
帧率
一幅点云图像代表一帧,对应到激光雷达内部就是电机旋转一圈完成扫描。帧率即代表一秒钟内激光雷达电机旋转的圈数,也就是每秒钟完成一圈扫描的次数。
-
角分辨率和帧率的关系,和采样率的关系
由于激光雷达的采样率是一定的,因此帧率越高,角分辨率越低;帧率越低,角分辨率越高。采样率表示激光雷达每秒钟进行有效采集的次数,可直观理解为一秒内产生的点云数目
比如:
角分辨率0.08°时,每一帧的点云数目:360°/0.08°= 4500;
每秒10帧,则每秒的点云数目:4500×10=45000;
所以其采样率为45kHz -
点云数据合并
点云数据合并是指把相邻的点合并成一个点,一方面可以减少图像的数据量,另一方面可以增加点云的稳定度。
-
激光雷达数据
每一帧的数据都会有时间戳,根据时间戳进行后续和时间有关的计算(如距离信息的微分等)。因此N线激光雷达的点云数据结构如下图
每一线点云的数据结构又是由点云的数量和每一个点云的数据结构组成
最底层的是单个点云的数据结构。点的表达既可以使用theta/r的极坐标表示,也可以使用x/y/z的3维坐标表示
-
点云数据的一般处理方式
①数据预处理(坐标转换,去噪声等)
②聚类(根据点云距离或反射强度)
③提取聚类后的特征
④根据特征进行分类等后处理工作
2、点云了解
3、PCL点云库
-
PCL是一个开源的C++库用于处理点云数据,包括点云相关的获取,滤波,分割,配准,检索,特征提取,识别,追踪,曲面重建,可视化的模块。
-
激光点云是复杂的n维结构,有不同类型的存储格式,包括PointXYZ,PointXYZI,PointXYZRGB
-
PCL使用模板(Templates)处理不同格式的点云数据,以实现代码重用。
-
PCL可用的PointT类型如要如下:
PointXYZ——成员变量:float x, y, z;
PointXYZI-成员变量:float x, y, z, intensity;
PointXYZRGB-成员变量:float x, y, z, rgb;
PointXYZRGBA-成员变量:float x, y, z; unit32_t rgba;
简单的二维xy point结构
InterestPoint-成员变量:float x, y, z, strength; -
PointXYZ——成员变量:float x,y,z;
PointXYZ是使用最常见的点数据类型,只包含三维xyz坐标信息,附加一个浮点数来满足存储对齐,可通过.data[0]或.x来访问x轴坐标值。
union { float data[4]; struct { float x; float y; float z; }; };
-
PointXYZI-成员变量:float x,y,z,intensity(xyz坐标加intensity的point类型。)
union{ float data[4]; struct { float x; float y; float z; }; }; union{ struct{ float intensity; }; float data_c[4]; };
二、PCL点云学习
1、绪论
1.1 点云数据及获取
-
定义:三维数据点的集合
-
属性:三维坐标、强度、颜色、时间戳
-
获取方式:激光雷达、深度相机、双目相机
-
激光扫描仪:
- 原理:time-of-flight
- 分类:星载、机载、地面(三脚架)、机器人
1.2 点云数据处理
1、点云滤波:检测和移除点云中的噪声和不感兴趣的点
- 分类
- 基于统计信息
- 基于领域
- 基于投影
- 基于信号处理
- 基于偏微分方程
- 常用方法:
- 基于体素(voxel grid)
- 移动平均最小二乘(Moving Least Squares)
2、点云关键点
- 常用方法:
- 3D point cloud:ISS、NARF、移动平均最小二乘
- 2D interests point:Harris、SIFT
- 深度学习
3、点云分割
- 常用方法:
- 基于边缘的方法:变成图像,使用边缘信息
- 基于区域生长
- 几何模型拟合:拟合平面、球形、圆形等等
4、点云匹配
- 估计两帧或多帧之间的rigid body transformation信息
- 分类:
- 初/粗匹配
- 精匹配
- 全局匹配:例如激光slam
- 常用方法:
- 基于ICP
- 基于特征的匹配方法
- 基于深度学习
5、目标检测
- 点云目标检测
6、点云分类/语义分割
- 为每一个点云分配一个点云标签
1.3 常用软件和开源库
1、CloudCompare
- 点云处理软件,点云可视化和简单编辑,多平台
2、数据集
- ETH dataset:点云拼接的、室内室外场景
- Semantic 3d:场景大、语义分割
- KITTI:城市道路
2、PCL基础
2.1 PCL介绍和配置
1、必须懂得的概念
- 编译的debug(慢)、release
- 动态库(dll)、静态库(lib)
- C++的模板编程与基本用法
- cmake的原理和简单使用
template <class Elem>
class Array{
Elem* data;
int size;
public:
Array(int sz);
int GetSize();
Elem& operator[](int idx);
}
std::vector<int> obj;
2、PCL处理流程
- 创建处理对象(例如过滤、特征估计、分割等)
- 使用setlnputCloud通过输入点云数据,处理模块
- ·设置算法相关参数
- 调用计算(或过滤、分割等)得到输出
pcl : :search: :Search<pcl : :PointXYZ> : :Ptr tree =
boost::shared_ptr<pcl: :search::Search<pcl::PointXYZ> > (newpcl: :search::KdTree<pcl::PointXYZ>);//创建一个指向kd树搜索对象的共享指针
pcl : :PointCloud <pcl ::Normal> : :Ptr normals (newpcl : :PointCloud <pcl : :Normal>);
pcl : :NormalEstimation<pcl : :PointXYz,pcl : : Normal>normal_estimator ;//创建法线估计对象
normal_estimator.setSearchMethod (tree);//设置搜索方法
normal_estimator.setInputCloud (cloud);//设置法线估计对象输入点集
normal_estimator. setKSearch (KN_normal);//设置用于法问量估计的k近邻数目
normal_estimator.compute (*normals);//计算并输出法向量
3、PCL数据的读取和可视化
3.1 PCL点云存储格式和读取
1、支持的格式
- .pcd
- .ply
- .obj
- .xyz
- .vtk
- .png
- .tif
2、代码
VERSION 0.7 //指定PCD文件版本
FIELDS x y z //指定一个点的维度和字段的名字,此处表示点云维度是XYZ
SIZE 4 4 4 //用字节数指定每一个维度的大小
TYPE F F F //用一个字符指定每一个维度的类型,此处F表示浮点类型(另外还有i表示有符号类型,U表示无符号类型)
COUNT1 1 1 //每一个维度包含的元素数目,默认情况下为1
WIDTH6411 //用点的数量表示点云数据集的宽度
HEIGHT 1 //用点的数量表示点云数据集的高度
VIEWPOINT O 0 0 1 0 0 0 l指定数据集中点云的获取视点,平移+四元数
POINTS 6411 //点云中点的总数
DATA ascii //指定存储点云的数据类型
有序点云可以更高效地处理目标点与领域的关系
3、PCL文件的读写
//依赖的头文件
#include<iostream>
#include<pcl/io/pcd_io.h> //PCD读写类相关的头文件
#include<pcl/point_types.h> //PCL中支持的点类型头文件
typedef pcl::PointXYz PointT;//别名
typedef pcl::PointCloud<PointT> PointCloud;
PointCloud::Ptr cloud(new PointCloud);
// 1.读取
//方法一
pcl::PCDReader reader;
reader.read( "xxx.pcd",*cloud);
//方法二
pcl::io::loadPCDFile( "xxx.pcd",*cloud );
//2.保存
//方法一
pcl::PCDWriter writer;writer.write("xxx.pcd", *cloud);
writer.writeBinary( "xxx.pcd" , *cloud);
writer.writeASCII("xxx.pcd", *cloud);
writer.writeBinaryCompressed( " xxx.pcd" , *cloud);
//方法二
pcl::io::savePCDFile( " xxx.pcd", *cloud) ;
pcl::io::savePCDFileASCII( "xxx.pcd", *cloud;
pcl::io::savePCDFileBinary ( "xxx.pcd", *cloud);
pcl::io::savePCDFileBinaryCompressed("xxx.pcd" , *cloud);
4、拼接点云
(1)拼接相同类型的点云,点数可以不相同,包含相同的字段
(2)连接具有同字段的点云,要求点数相等,拼接不同的属性
pcl::PointCloud<pcl::PointXYZ> cloud_a,cloud_b,cloud_c;
pcl::PointCloud<pcl::Normal>n_cloud_b;
pcl::PointCloud<pcl::PointNormal>p_n_cloud_c;
//(1)拼接点云
cloud_c=cloud_a;
cloud_c+=cloud_b;//连接点云
//(2)连接点云
pcl::concatenateFields(cloud_a,n_cloud_b,p_n_cloud_c);//连接字段,a和b的点数相同
3.2 PCL中的数据结构
1、Kd-Tree(空间换时间)
-
kd-tree是一种空间划分的数据结构。常被用于高维空间中的搜索,比如范围搜索和最近邻搜索。kd-tree是二进制空间划分树的一种特殊情况
-
由于三维点云的数目一般都比较大,所以,使用kd-tree来进行检索,可以减少很多的时间消耗,可以确保点云的关联点寻找和配准处于实时的状态
struct kdtree{
Node-data-数据矢量 数据集中某个数据点,是n维矢量(这里也就是k维)Range-空间矢量该节点所代表的空间范围
split-整数 垂直于分割超平面的方向轴序号
Left- kd树 由位于该节点分割超平面左子空间内所有数据点所构成的k-d树
Right - kd树 由位于该节点分割超平面右子空间内所有数据点所构成的k-d树
parent - kd树 父节点
}
- PCL中实现
#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <iostream>
#include <vector>
pcl::KdTreeFLANN<pcl::PointXYZ>kdtree;
kdtree.setInputCloud (cloud);
pcl::PointXYZ searchPoint;
//k近邻搜索
int K =10;
std::vector<int>pointIdxNKNSearch(K);
std::vector<float>pointNKNSquaredDistance(K);
int num = kdtree.nearestKSearch (searchPoint,IxNKNSearch, pointNKNSquaredDistance)
//在半径r内搜索近邻
std::vector<int> pointIdxRadiusSearch;
std::vector<float>pointRadiusSquaredDistance;
int num = kdtree.radiusSearch(searchPoint,radius,pointIdxRadiusSearch,pointRadiusSquaredDistance)
2、OcTree
- 八叉树结构通过对三维空间的几何实体进行体元剖分,每个体元具有相同的时间和空间复杂度,通过循环递归的划分方法对大小为2n x 2n x 2n的三维空间的几何对象进行剖分,从而构成一个具有根节点的方向图。在八叉树结构中如果被划分的体元具有相同的属性,则该体元构成一个叶节点;否则继续对该体元剖分成8个子立方体,对于2n x 2n x 2n大小的空间对象,最多剖分n次。
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
pcl:: PointCloud<pcl:: PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
float resolution =128.0f;//分辨率
pcl::octree::OctreePointCloudSearch<pcl::PointXYZ>octree(resolution);
octree.setInputCloud (cloud);
octree.addPointsFromInputCloud ();
pcl::PointXYZ searchPoint;
//体素内近邻搜索
std::vector<int>pointIdxVec;
octree.voxelsearch (searchPoint,pointIdxVec)
//K近邻搜索
int K =10;
std::vector<int>pointIdxNKNSearch;
std::vector<float>pointNKNSquaredDistance;
int num = octree.nearestKSearch (searchPoint,K,pointIdxNKNSearch,pointNKNSquaredDistance)
//半径内近邻搜索
std::vector<int>pointIdxRadiusSearch;
std::vector<float>pointRadiusSquaredDistance;
int num = octree.radiusSearch (searchPoint,radius,pointIdxRadiusSearch,
pointRadiusSquaredDistance)
3.3 点云可视化
1、PCL中pcl_visualization库中提供了可视化相关的数据结构和组件,主要是可视化展示点云处理结果。其依赖于pcl_common、pcl_range_image、pcl_kdtree、pcl_IO以及VTK库。
-
简单可视化cloud_viewer :
pcl::visualization::CloudViewer viewer("Cloud Viewer");//创建viewer对象 viewer.showCloud(cloud) ;
-
高级可视化:
CloudViewer类本质上是PCVisualizer类的简化封装,如果需要更多操作,则需要使用PCLVisualizeropcl::visualization::PCLVisualizer viewer("3D View" );//创建视窗对象或者 //增加显示一个点云,并提供唯一的标志(标识ID)( string格式) viewer.addPointCloud<pcl::PointXYZ>(cloud," sample cloud"); // cloud是指针, //1.更新点云,使用updatePointCloud()函数 viewer.setBackgroundColor(100,100,100);//设置背景颜色,默认黑色 //---显示点云数据---- viewer.addPointCloud(cloud1, "cloud1" );// "cloud1"为显示id,默认cloud,显示多个点云时用默认会报警告。 //将点云设置颜色,默认白色 pcl::visualization::PointCloudColorHandlerCustom<PointT> red(cloud2,255,0,0); //rgb viewer.addPointCloud(cloud2,red,"cloud2"); //---显示网格数据--- viewer.addPogonMesh(mesh); //---显示文字--- viewer.addText(string,position.x of screen,ID=default("Text") ,viwportID=default0; //调用的一些函数 viewer.runOnVisualizationThreadOnce(sub/function);//参数是一个sub或者function,表示该函数在可视化时仅调用一次 viewer.runOnVisualizationThread( sub/function);//可视化时每次都调用,可用来显示text //开始显示2种方法,任选其l //1.阻塞式 viewer.spin(); //2.非阻塞式 while (!viewer.wasStopped()) viewer.spinOnce(100); boost::this_thread::sleep(boost::posix_time ::microseconds(100000));//可添加其他操作 }
-
法向显示
viewer.addPointCloudNormals<pcl::PointXYZRGB,pcl::Nonrmal> (cloud,normals,10,0.05,“nonmals"); //每10个点显示一条法线,每条法线长0.05,“normals"是该法线对象的标志符
-
一些形状
// line viewer.addLine<pcl::PointXYZRGB>(cloud->points[0],cloud->points[cloud->size()1], "line" ); // sphere viewer.addSphere (cloud->points[0],0.2,0.5,0.5, 0.0,"sphere" ); //0.2 means radius , rest number are RGB //plane pcl::ModelCoefficients coeffs; coeffs.values.push_back (0.0); coeffs.values.push_back (0.0); coeffs.values.push_back (1.0); coeffs.values.push_back (0.0); viewer.addPlane (coeffs,"plane" );//原点中心,z轴方向
-
多窗口
//第一个窗口int v1=0; viewer.creatViewerPort(0,0,0.5,1,v1); //Xmin , Ymin , Xmax, Ymax, view_ID viewer.setBackgroundColor(0,0,0,v1);//R,G,B,view_ID viewer.addText("TEXT内容",10,10,"v1text",v1);//文字内容,position.x ,position.y ,text_ID, view_ID pcl::visualization:: PointcloudColorHandlerRGBField<pcl::PointXYZRGB>rgb(cloud); viewer->addPointCloud<pcl:: PointXYZRGB>(cloud,rgb,"sample cloud1",v1); //第二个窗口 int v2(0); viewer.createViewPort(0.5,0.0,1.0,1.0,v2);viewer.setBackgroundColor(0.3,0.3,0.3,v2); viewer.addText( "Radius: 0.1",10,Q, "v2 text", v2) pcl::visualization ::PointCloudColorHandlerCustom<pcl::PointXYZRGB〉 single_color(cloud,0,255,0); :_color, "sample cloud2", v2); viewer.addPointCloud<pcl::PointXYZRGB>(cloud, single_color, "sample cloud2",v2);
-
键盘交互
viewer->registerKeyboardCallback(keyboardEventOccurred,(void*)&viewer); viewer->registerMouseCallback(mouseEventOccurred,(void*)&viewer); //mouseEventOccurre void mouseEventOccurred(const pcl::visualization::MouseEvent &event,void*viewer_void){ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared_ptrpcl: :visualization::PCLVisualizer> *>(viewer_void); if (event.getButton() == pcl::visualization::MouseEvent::LeftButton && event.getType()=-pcl: :visualization: : MouseEvent: : MouseButtonRelease) { //响应的内容 } //keyboardEventOccurred void keyboardEventOccurred(const pcl::visualization::KeyboardEvent &event,void*viewer_void){ boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer = *static_cast<boost::shared,ptr<pcl: :visualization::PCLVisualizer> *>(viewer_void); if(event.getKeySym() == "r" &&event.keyDown( )) {//响应的内容} }
3.4 点云和其他类型数据的转换
- 用到再学吧
4、点云滤波
4.1 经典滤波方法介绍
1、什么是点云滤波
点云处理的第一步,滤波可以去除噪声点、离群点、点云平滑以及空洞、数据压缩
2、为什么点云滤波?
去除噪声点
- 去噪、下采样、去除非地面点
3、直通滤波器
-
直通滤波器就是根据点云的属性在点的属性上设置范围,对点进行滤波,保留范围内的或保留范围外的。
(1)指定一个维度以及该维度下的值域
(2)遍历点云中的每个点,判断该点在指定维度上的取值是否在值域内,删除取值不在值域内的点
(3)遍历结束,留下的点即构成滤波后的点云。#include <pcl/filters/passthrough.h> //原点云获取后进行滤波 pcl::PassThrough<pcl::PointXYz> pass;//创建滤波器对象 pass.setInputCloud (cloud); //设置输入点云 pass.setFilterFieldName ("z"); //滤波字段名被设置为z轴方向pass.setFilterLimits (0.0,1.0);//可接受的范围为(0.0,1.0) //pass.setFilterLimitsNegative (true);//设置保留范围内还是过滤掉范围内 pass.filter (*cloud_filtered);//执行滤波,保存过滤结果在cloud_filtered
4、体素滤波器(下采样)
-
体素滤波器可以达到向下某样同时不破坏点云本身几何结构的功能,但是会移动点的位置。此外体素滤波器可以去除一定程度的噪音点及离群点。主要功能是用来进行降采样。
(1)它的原理是根据输入的点云,首先计算一个能够刚好包裹住该点云的立方体,然后根据设定的分辨率,将该大立方体分割成不同的小立方体。对于每一个小立方体内的点,计算他们的质心,并用该质心的坐标米近似该立万体内的若干点。(更精确,计算稍慢)
(2)ApproximateVoxelGrid的不同在于这种方法是利用每一个小立方体的中心来近似该立方体内的若干点。相比于VoxelGrid,计算速度稍快,但也损失了原始点云局部形态的精细度。(计算快、精度差)#include <pcl/filters /voxel_grid.h> //VoxelGrid pcl: :VoxelGrid<pcl: :PCLPointCloud2>sor; sor.setInputCloud (cloud); sor.setLeafSize (0.01f,0.01f,0.01f); sor.filter (*cloud_filtered) ; //Approximate体素格滤波器 pcl::ApproximateVoxelGrid<pcl::PointXYz> approximate e voxel filter; approximate_voxel_filter.setLeafSize (0.2,0.2,0.2); approximate_voxel_filter.setInputCloud (input_cloud); approximate_voxel_filter.filter (*filtered_cloud);
5、均匀采样滤波器
-
均匀采样滤波基本上等同于体素滤波器,但是其不改变点的位置。下采样后,其点云分布基本均匀,但是其点云的准确度要好于体素滤波,因为没有移动点的位置。
-
均匀采样算法:均匀采样通过构建指定半径的球体对点云进行下采样滤波,将每一个球内距离球体中心最近的点作为下采样之后的点输出。
-
体素滤波是建立立方体,均匀采样是建立一个球
#include <pcl/keypoints/uniform_sampling.h> // Uniform sampling object. pcl: :UniformSampling<pcl::PointXYZ> filter; filter.setInputCloud( cloud); filter.setRadiusSearch(0.01f); //we need an additional object to store the indices of surviving po1nc pcl::PointCloud<int> keypointIndices; filter.compute(keypointIndices);
6、统计滤波器(去噪)
-
统计滤波器主要用于去除明显离群点。
-
离群点特征在空间中分布稀疏。定义某处点云小于某个密度,既点云无效。计算每个点到其最近的k个点平均距离。则点云中所有点的距离应构成高斯分布。根据给定均值与方差,可剔除方差之外的点。
#include <pcl/filters/ statistical_outlier_removal.h> pcl::Statistical0utlierRemoval<pcl::PointXYZ> sor; sor.setInputCloud(cloud); sor.setMeank(50); //设置考虑查询点临近点数 sor.setStddevMulThresh (1.0);//设置判断是否为离群点的阀值 sor.filter(*cloud_filtered) ; //然后,使用同样的参数再次调用该滤波器,但是利用函数setNegative设置使输出取外点,以获取离群点数据(也就是原本滤除掉的点). sor.setNegative (true) ; sor.filter(*cloud_filtered) ;
7、条件滤波器
-
条件滤波器通过设定滤波条件进行滤波,删除不符合用户指定的一个或者多个条件直通滤波器是一种较简单的条件滤波器。
#include <pcl/filters/ conditionaremoval.h> pcl::ConditionAnd<pcl::PointXYZ> :Ptr range_cond(new pcl::ConditionAnd<pcl::PointXYZ>));//创建条件定义对象 //添加在z字段上大于0的比较算子 range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZ>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZ>("z", pcl::ComparisonOps : :GT,0.0))); //添加在z字段上小于0.8的比较算子 range_cond->addComparison (pcl::FieldComparison<pcl::PointXYz> onstPtr ( new pcl: :FieldComparison <pcl::PointXYz>("z", pcl::Comparison0ps: :LT,0.8))); //创建滤波器并用条件定义对象初始化 pcl::ConditionalRemoval<pcl::PointXYz> condrem; condrem.setCondition(range_cond ); condrem.setInputCloud (cloud); condrem.setKeepOrganized(true);//设置保持点云的结构 condrem.filter (*cloud_filtered);//执行滤波
8、半径滤波器(去噪)
-
半径滤波器以某点为中心画一个圆计算落在该圆中点的数量,当数量大于给定值时,则保留该点,数量小于给定值则剔除该点。
-
主要还是用于去除离群点,在一定程度上可以用来筛选边缘点。
#include <pcl/filters/radius_outlier_removal.h> pcl::RadiusOutlierRemoval<pcl::PointXYZ>outrem; outrem.setInputCloud(cloud); outrem.setRadiusSearch(0.8);//设置半径为0.8的范围内找临*点 outrem.setMinNeighborsInRadius(2);//设置查询点的邻域点集数小于2的删除 outrem.filter (*cloud_filtered); //在半径为0.8在此径内必须要有两个邻居点,此点才会保存
9、投影滤波
-
将点投影到一个参数化模型上,这个参数化模型可以是平面、圆球、圆柱、锥形等进行投影滤波。把三维点云投影到二维图像上,然后用图像处理的方法进行处理
//填充ModelCoefficients的值,使用ax+by+cz+d=0平面模型,其中 a=b=d=0,c=1也就是X—Y平面 //定义模型系数对象,并填充对应的数据 pcl::ModelCoefficients ::Ptr coefficients(new pcl: :Mocoefficients->values.resize(4); coefficients->values[0] = coefficients->values[1] = 0; coefficients->values[2] = 1.0; coefficients->values[3] = 0; //创建ProjectInliers对象,使用MqdelCoefficients作为投 投影对象的模型参数 pcl::ProjectInliers<pcl::PointXYz> proj; //创建投影滤波对象 proj.setModelType(pcl::SACMODEL_PLANE); //设置对象对应的投影模型 proj.setInputCloud(cloud); //设置输入点云 proj.setModelCoefficients(coefficients) ; //设置模型对应的系数 proj.filter(*cloud_projected); //投影结果存储
10、模型滤波
-
根据点到模型的距离,设置距离阈值过滤非模型点,基于模型的点分割操作,将模型外的点从点云中剔除
//x^2 + y^2 +z^2 =1 pcl::ModelCoefficients sphere_coeff; sphere_coeff.values.resize (4); sphere_coeff.values[0]= 0; sphere_coeff.values[1]= 0; sphere_coeff.values[2]= 0; sphere_coeff.values[3]= 1; pcl::ModeloutlierRemoval<pcl::PQintXYz> sphere_filter;sphere_filter.setModelCoefficients (sphere_coeff); sphere_filter.setThreshold (0.05); sphere_filter.setModelType (pcl::SACMODEL_SPHERE);sphere_filter.setInputCloud (cloud); sphere_filter.filter (*cloud_sphere_filtered);
11、高斯滤波
- 基于高斯核的卷积滤波实现
12、双边滤波(平滑)
- 双边滤波是一种非线性滤波器,它可以达到保持边缘、降噪平滑的效果。一定程度上弥补了高斯滤波的缺点,双边滤波对高斯噪声效果比较好。
13、总结
pcl::PassThrought<pcl::PointXYZ> pass; 直通滤波器pcl::VoxelGrid<pcl::PointXYZ> vox; 体素滤波器
pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor; 统计滤波器pcl::RadiusOutlierRemoval<pcl::PointXYZ> rador; 半径滤波器pcl::UniformSamplinqpcl:PointXYZ> unisam; 均匀采样
pcl::ConditionalRemoval<pcl::PointXYZ> condr; 条件滤波器pcl::ProjectInliers<pcl::PointXYZ> proj; 投影滤波器
pcl::ModelOutlierRemoval<pcl::PointXYZ> modr; 模型滤波器pcl::ExtractIndices<pcl::Pointx Yz>extr; 索引提取
空间裁剪滤波
pcl::Clipper3D<pcl::PointXYZ>
pcl::BoxCliper3D<pcl::PointXYZpcl:CropBox<pcl::PointXYZ>
pcl::CropHull<pcl::PointXYZ>
pcl::BilateralFilter<pcl::PointXYZ> bf; 双边滤波
pcl::filters::GaussianKernal<PointInT,PointOutT> 高斯滤波
4.2 针对机载激光雷达的滤波方法
1、形态学滤波
2、渐进三角网加密滤波(PTD)
- 其中“渐进三角网加密滤波”,简称PTD(算法发明者是Peter Axelsson),是一种广泛应用的地面点滤波算法,包括激光雷达数据处理软件TerraSolid,数字绿土的Lidar 360。
3、布料滤波
5、点云关键点、特征描述与提取
5.1 介绍
-
点云描述子
- image-like descriptors(PHF)
- 3D Geometric descriptors(ESF)
- Segment descriptors
- Deep learning descriptors
6、点云分割
6.1 基本概念
1、点云分割point cloud segmentation:
根据空间,几何和纹理等特征点进行划分,是同一划分内的点云拥有相似的特征。点云分割的目的是分块,从而便于单独处理。
2、点云分类point cloud classification:
为每个点分配一个语义标记。点云的分类是将点云分类到不同的点云集,同一个点云集具有相似或相同的属性,例如地面,树木,人等。也叫做点云语义分割。
3、基础概念
- 特征提取:单个点或一组点可以根据低级属性检测某种类型的点。
“低级属性”是指没有语义(例如,位置,高程,几何形状,颜色,强度,点密度等)的信息。“低级属性”信息通常可以从点云数据中获取而无需事先的高级知识。例如,平面提取和边缘检测、以及特仙描述子的计算都可以视为特征提取过程。 - 分割:基于上述低级属性将点分组为一个部分或一个对象的过程。
与单独对每个点处理或分析相比,分割过程对每个对象的进一步处理和分析,使其具有更丰富的信息。 - 物体识别:识别点云中一种或多种类型对象的过程。该过程通常通过根据特征提取和分割的结果执行分析,并基于先验知识在给定的约束和规则下进行。
- 分类:类似于对象识别的过程,该过程为每个点,线段或对象分配一个类别或标识,以表示呆些类型的对家(例如,标志,道路,标记或建筑物)。
对象识别和点云分类之间的区别在于,对象识别是利用一种方法以将一些特定对象与其他对象区分开,而分类的目的通常是在语义上标记整个场景。
6.2 经典分割算法
1、分类
- 随机采样一致方法(RANSAC)
- 欧式聚类分割方法
- 条件欧式聚类分割
- 基于区域生长的分割
- 基于颜色的区域生长分割
- 最小图割的分割
- 基于法线微分的分割
- 基于超体素的分割
2、随机采样一致方法(RANSAC)
-
采用迭代的方式从一组包含离群的被观测数据中估算出数学模型的参数。RANSAC算法假设数据中包含正确数据和异常数据(或称为噪声)。正确数据记为内点(inliers),异常数据记为外点(outliers)。同时RANSAC也假设,给定一组正确的数据,存在可以计算出符合这些数据的模型参数的万法。
-
该算法核心思想就定随机性和假设性,随机性是根据正确数据出现概率去随机选取抽样数据,根据大数定律,随机性模拟可以近似得到正确结果。假设性是假设选取出的抽样数据都是正确数据,然后用这些正确数据通过问题满足的模型,去计算其他点,然后对这次结果进行一个评分。
-
RANSAC算法被广泛应用在计算机视觉领域和数学领域,例如直线拟合、平面拟合、计算图像耿尽云同的文大疋阵、计算基础矩阵等方面,使用的非常多。
-
算法流程
- 1.要得到一个直线模型,需要两个点唯一确定一个直线方程。所以第一
步随机选择两个点。 - 2.通过这两个点,可以计算出这两个点所表示的模型方程y=ax+b。
- 3.将所有的数据点套到这个模型中计算误差。
- 4.找到所有满足误差阈值的点。
- 5.然后我们再重复1~4这个过程,直到达到一定迭代次数后,选出那个被
支持的最多的模型,作为问题的解。
- 1.要得到一个直线模型,需要两个点唯一确定一个直线方程。所以第一
-
PCL中的Sample_consensus模块
-
PCL中的 Sample_consensus库实现了随机采样一致性及其泛化估计算法,以及例如平面、柱面等各种常见几何模型,用不同的估计算法和不同的几何模型自由结合估算点云中隐含的具体几何模型的系数,实现点云中所处的几何模型的分割。
-
支持以下模型:(剩下的网上查)
SACMODEL_PLANE-用于确定平面模型。平面的四个系数SACMODEL_LINE-用于确定线模型。直线的六个系数由直线上的一个点和直线的方向给出。
SACMODEL_CIRCLE2D -用于确定平面中的2D圆。圆的三个系数由其中心和半径给出。
SACMODEL_CIRCLE3D-用于确定平面中的3D圆。圆的七个系数由其中心、半径和法线给出。SACMODEL_SPHERE-用于确定球体模型。球体的四个系数由其3D中心和半径给出。
SACMODEL_CYLINDER-用于确定气缸模型。圆柱体的七个系数由其轴上的点、轴方向和半径给出。SACMODEL_CONE-用于确定锥模型。锥体的七个系数由其顶点、轴方向和张角给出。
-
3、欧式聚类分割(聚类)
-
聚类方法,通过特征空间确定点和点的亲疏程度
-
算法流程:
-
- 找到空间中某点p,有kdTree找到离他最近的n个点,判断这n个点到p的距离。将距离小于阈值r的点p1,p2,p3…放在类Q里
- 2.在Q里找到一点p1,重复1,找到p22,p23,p24…全部放进Q里
- 3.当Q再也不能有新点加入了,则完成搜索了
pcl::EuclideanClusterExtraction<pcl::PointXYz> ec; ec.setClusterTolerance(0.02); ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); ec.extract (cluster_indices);
-
4、条件欧式聚类分割
-
使用类pcl::ConditionEuclideanClustering实现点云分割,与其他分割分割方法不同的是该方法的聚类约束条件(欧式距离、平滑度、RGB颜色等)可以由用户自己定义,即当搜索到一个近邻点时,用户可以自定义该邻域点是否合并到当前聚类的条件
-
代码
pcl::Conditiona1EuclideanClustering<PointTypeFull〉 cec(true);//创建条件聚类分割对象,并进行初始化。 cec.setInputCloud (cloud_with_normals);//设置输入点集 //用于选择不同条件函数 switch(Method) { case 1: cec.setConditionFunction (&enforceIntensitySimilarity); break; case 2: cec.setConditionFunction(&enforceCurvatureOrIntensitySimilarity ); break; case 3: cec.setConditionFunction (&customRegionGrowing); break; default: cec.setConditionFunction (&customRegionGrowing); break; } cec. setClusterTolerance ( 500.0);//设置聚类参考点的搜索距离 cec.setMinClusterSize (cloud_with_normals->points.size () / 1000); //设置过小聚类的标准 cec. setMaxClusterSize (cloud_with_normals->points.size () / 5); //设置过大聚类的标准 cec.segment (*clusters);//获取聚类的结果,分割结果保存在点云索引的向量中 cec.getRemovedClusters (small_clusters,large_clusters); //获取无效尺寸的聚类
-
这个条件的设置是可以由我们自定义的,因为除了距离检查,聚类的点还需要满足一个特殊的自定义的要求,就是以第一个点为标准作为种子点,候选其周边的点作为它的对比或者比较的对象,如果满足条件就加入到聚类的对象中.
//如果此函数返回true,则将添加候选点到种子点的簇类中。 bool customCondition(const pcl: :PointXYZ& seedPoint,const pcl: :PointXYz& candidatePoint, float squaredDistance) { //在这里你可以添加你自定义的条件 return false; return true; }
5、区域生长算法:
-
区域生长算法:
将具有相似性的点云集合起来构成区域。
首先对每个需要分割的区域找出一个种子点作为生长的起点,然后将种于点周围邻域中与狎丁有相回相似性质的点合并到种子像素所在的区域中。而新的点继续作为种子向四周生长,直到再没有满足条件的像素可以包括进来,一个区域就生长而成了。 -
算法流程:
- 1.计算法线normal和曲率curvatures,依据曲率升序排序;
- 2.选择曲率最低的为初始种子点,种子周围的临近点和种子点云相比较;
- 3.法线的方向是否足够相近(法线夹角足够r p y) ,法线夹角阈值;
- 4.曲率是否足够小(表面处在同一个弯曲程度),区域差值阈值;
- 5.如果满足2,3则该点可用做种子点;6.如果只满足2,则归类而不做种子;
-
算法是针对小曲率变化面设计的,尤其适合对连续阶梯平面进行分割
//区域增长聚类分割对象<点,法线> pcl::RegionGrowing<pcl::PointXYZ,pcl::Normal>reg; reg.setMinclusterSize (50);//最小的聚类的点数 reg.setMaxClusterSize (1000000);//最大的聚类的点数 reg.setSearchMethod (tree);//搜索方式 reg.setNumberOfNeighbours (30);//设置搜索的邻域点的个数 reg.setInputCloud (cloud); //输入点 //reg.setIndices (indices); reg.setInputNormals (normals );//输入的法线 reg.setSmoothnessThreshold(3.0 / 180.0 *M_PI);//设置平滑度法线差值阈值 reg.setCurvatureThreshold (1.0);//设置曲率的阀值
6、基于颜色的区域
-
基于颜色的区域生长分割原理上和基于曲率,法线的分割方法是一致的,只不过比较目标换成了颜色。可以认为,同一个颜色且挨得近,是一类的可能性很大,比较适合用于室内场景分割。
-
尤其是复杂室内场景,颜色分割可以轻松的将连续的场景点云变成不同的物体。哪怕是高低不平的地面,设法用采样一致分割器抽掉平面,颜色分割算法对不同的颜色的物体实现分割。
-
算法主要分为两步:
- (1)分割,当前种子点和领域点之间色差小于色差阀值的视为一个聚类
- (2)合并,聚类之间的色差小于色差阀值和并为一个聚类,且当前聚类中点的数量小于聚类点数量的与最近的聚类合并在一起
-
代码
//基于颜色的区域生成的对象 pcl::RegionGrowingRGB<pcl::PointXYZRGB>reg; reg.setInputCloud (cloud); reg.setIndices (indices); //点云的索引reg.setSearchMethod (tree); reg.setDistanceThreshold (10); //距离的阀值 reg.setPointColorThreshold (6); //点与点之间颜色容差 reg.setRegionColorThreshold (S)//区域之间容差 reg.setMinClustersize (600); //设置聚类的大小 std::vector <pcl::PointIndices> clusters; reg.extract (clusters); pcl::PointCloud <pcl::PointXYZRGB> ::Ptr colored_cloud = reg.getColoredCloud ();
7、最小图割
-
图论中的最小割(min-cut)广泛应用在网络规划,求解桥问题,图像分割等领域。
最小割算法是图论中的一个概念,其作用是以某种方式,将两个点分开,当然这两个点中间可能是通过无数的点再相连的。 -
如果要分开最左边的点和最右边的点,红绿两种割法都是可行的,但是红线跨过了三条线,绿线只跨过了两条。单从跨线数量上可以得出绿线这种切割方法更优的结论。但假设线上有不同的权值,那么最优切割则和权值有关了。、
-
算法主要思想
-
1.建图:对于给定的点云,算法将包含点云中每一个点的图构造为一组普通顶点和另外两个称为源点和汇点的顶点。每个普通顶点都有边缘,将对应的点与其最近的邻居连接起来。
-
2.算法为每条边缘分配权重。有三种不同的权:首先,它将权重分配到云点之间的边缘。这个权重称为平滑成本,
由公式计算:这里dist是点之间的距离。距离点越远,边被切割的可能性就越大
-
3.算法设置数据成本。它包括前景和背景惩罚。第一个是将云点与源顶点连接起来并具有用户定义的常量值的边缘的权重。
-
4.对点云的前景点和背景点进行划分。
-
-
代码(略)
8、基于法线微分的分割
-
根据不同尺度下法向量特征的差异性,利用pcl:DifferenceONormalsEstimation实现点云分割,在处理有较大尺度变化的场景点云分割效果较好,利用不同支撑半径去估算同一点的两个单位法向量,单位法向量的差定义DoN特征。
-
DoN算法:
DoN特征源于观察到基于所给半径估计的表面法向量可以反映曲面的内在几何特征,
因此这种分割算法是基于法线估计的,需要计算点云中某一点的法线估计。而通常在计算法线估计的时候都会用到邻域信息,很明显邻域大小的选取会影响法线估计的结果。
而在DoN算法中,邻域选择的大小就被称为support radius。对点云中某一点选取不同的支持半径,即可以得到不同的法线估计,而法线之间的差异,就是是所说的法线差异。 -
算法流程
- (1)对于输入点云数据中的每一点,利用较大的支撑半径计算法向量;
- (2)对于输入点云数据中的每一点,单位化每一点的法向量差异
- (3)过滤所得的向量域(DoN特征向量),分割出目标尺寸对应的点云;
-
代码
// Create output cloud for DoN results PointCloud<PointNormal>::Ptr doncloud (new pcl::PointCloud<PointNormalcopyPointCloud<PointXYZRGB,PointNormal>(*cloud,*doncloud); pcl::DifferenceOfNormalsEstimation<PointXYZRGB,Po ointNormal,PointNormal> don; don.setInputCloud (cloud); don.setNormalScaleLarge(normals_large_scale); don. setNormalScaleSmall (normals_small_scale);don.initCompute ( ); //Compute DoN don.computeFeature (*doncloud) ;
9、基于超体素的分割
-
超体(super voxel)是一种集合,集合的元素是“体素”。
-
与体素滤波器中的体类似,其本质是一个个的小方块。与之前提到的所有分割手段不同,超体聚类的目的并不是分割出某种特定物体,其对点云实施过分割(over segmentation),将场景点云化成很多小块,并研究每个小块之间的关系。
-
以八叉树对点云进行划分,获得不同点团之间的邻接关系。
-
与图像相似,点云的邻接关系也有很多,如面邻接、线邻接、点邻接。
6.
6.4 实战
三、速腾Lidar配置
1、配置网络
该激光雷达的网址已改为:
Device IP Address:192.168.2.200 (浏览器中访问的地址)
Destination IP Address: 192.168.2.102(电脑或者树莓派需要配置成这个网络)
树莓派系统中网络的配置文件在:/etc/dhcpcd.conf中
cd /etc/
xhost + # 远程桌面连接需要这个来给权限
sudo gedit dhcpcd,conf
编辑dhcpcd文件如下:
interface eth0
static ip_address=192.168.2.102 #这个是激光雷达要求设置的。设置了这个连不上远程桌面,所以也要设置WIFI固定IP
#interface wlan0
#static ip_address=192.168.1.23/24
#static routers=192.168.1.1 # 网关,router address 我们家路由器的地址(用Advanced IP Scanner查看)
#static domain_name_servers=192.168.1.1 #DNS 和路由器地址一样
值得注意的是,网口地址和WIFI地址设置在同一个网段下面,网络会不稳定,可以修改激光雷达的地址。
12.28运行有两个问题:
①在雷达系统设置界面,无法修改IP地址
解决:树莓派浏览器中点不开,用电脑进行配置
②、运行ROS节点时报错
./rslidar_sdk_node
解决:config文件中修改雷达对应的型号
2、编译雷达ROS驱动程序
2.1 下载驱动程序包
https://github.com/RoboSense-LiDAR/rslidar_sdk/releases
2.2 安装对应ROS
在ROS环境下使用雷达驱动,需要安装ROS相关依赖库。
Ubuntu 16.04 - ROS Kinetic desktop
Ubuntu 18.04 - ROS Melodic desktop
Ubuntu 20.04 - ROS Noetic desktop
安装方法请参考 http://wiki.ros.org。
强烈建议安装ROS desktop-full版。这个过程会自动安装一些兼容版本的依赖库,如PCL库等。这样可以避免花大量时间,去逐个安装和配置它们。
2.3 安装Yaml和libpcap(必需)
sudo apt-get update
sudo apt-get install -y libyaml-cpp-dev # 版本号: >= v0.5.2
sudo apt-get install -y libpcap-dev # 版本号: >= v1.7.4
2.4 编译
4.1 直接编译
(1) 打开工程内的CMakeLists.txt文件,将文件顶部的变量COMPILE_METHOD改为ORIGINAL.
#=======================================
Compile setup (ORIGINAL,CATKIN,COLCON)
#=======================================
set(COMPILE_METHOD ORIGINAL)
(2) 在ROS1(不适用于ROS2)中,直接编译、运行程序。
请先启动roscore,再运行rslidar_sdk_node,最后运行rviz查看点云。
cd rslidar_sdk
mkdir build && cd build
cmake .. && make -j4
./rslidar_sdk_node
4.2 依赖于ROS-catkin编译
(1) 打开工程内的CMakeLists.txt文件,将文件顶部的变量COMPILE_METHOD改为CATKIN.
set(COMPILE_METHOD CATKIN)
(2) 将rslidar_sdk工程目录下的package_ros1.xml文件复制到package.xml。
(3) 新建一个文件夹作为工作空间,然后再新建一个名为src的文件夹, 将rslidar_sdk工程放入src文件夹内。
(4) 返回工作空间目录,执行以下命令即可编译、运行。如果使用.zsh,将第二行替换成 source devel/setup.zsh。
catkin_make
source devel/setup.bash
roslaunch rslidar_sdk start.launch
3、PCL库安装和激光点云录制
3.1 树莓派中的PCL库安装
1 、扩展交换区大小
//在开始编译之前,建议你增加交换空间。这将使你使用树莓派的所有四个内核来编译OpenCV,而不会由于内存耗尽导致编译挂起。
sudo nano /etc/dphys-swapfile
然后编辑 CONF_SWAPSIZE 变量:从100增加到2048
重新启动交换服务:
sudo /etc/init.d/dphys-swapfile stop
sudo /etc/init.d/dphys-swapfile start
2 、安装PCL依赖库
$ sudo apt-get install libeigen3-dev
$ sudo apt-get install libboost-all-dev
$ sudo apt-get install libflann-dev
$ sudo apt-get install libvtk5-dev //树莓派4改成libvtk6-dev
3 、修改配置文件
下载pcl1.8.1
解压,进入目录,修改 pcl-pcl-1.8.1/cmake/pcl_find_sse.cmake 文件:
找到第19行:SET(SSE_FLAGS "${SSE_FLAGS} -march=native")
将:-march=native
改为:
-march=armv7 -mfloat-abi=hard -mfpu=vfp
如果是树莓派2,则是 armv6
4、 安装PCL
$ cd pcl-pcl-1.8.1
$ mkdir build && cd build
$ cmake -DCMAKE_BUILD_TYPE=Release ..
$ make -j1
$ sudo make install
5、把交换区换回来
sudo nano /etc/dphys-swapfile
重新启动交换服务:
sudo /etc/init.d/dphys-swapfile stop
sudo /etc/init.d/dphys-swapfile start
6、节省空间,clean掉编译生成的文件
make clean
【问题1】安装libvtk6-dev提示:下列软件包有未满足的依赖关系:
libvtk6-java : 依赖: libvtk6-jni (= 6.3.0+dfsg2-2+b6) 但是它将不会被安装
E: 无法修正错误,因为您要求某些软件包保持现状,就是它们破坏了软件包间的依赖关系。
【解决1】缺啥下载啥(按照提示下载就可以)
sudo apt-get install libvtk6-jni=6.3.0+dfsg2-2+b6
3.2 树莓派中的PCL_ROS安装
1、GitHub中下载软件包(注意选择适合你版本的分支)(安装包都保存在“激光雷达文件夹中”)
源码地址:https://github.com/ros-perception/perception_pcl.git
2、安装pcl-msgs(ros-perception)
https://github.com/ros-perception
3、安装tf2_eigen(ros/geometry2)
https://github.com/ros/geometry2/tree/melodic-devel/tf2_eigen
3.3 雷达点云录制
1、rostopic list 查看topic信息,然后rosbag record /rslidar_points,会在对应目录下生成一个以时间命名的文件,里面记录 的激光雷达点云数据
rostopic list
rosbag record /rslidar_points
2、将rosbag转换为pcd
rosrun pcl_ros bag_to_pcd Desktop/2018-10-10-14-19-26.bag /rslidar_points Desktop/pcd // rosrun pcl_ros rosbag路径 /rslidar_points 需要转换的topic pcd文件存储路径
3、解析pcd文件,并将点云可视化
四、Windows中点云
1、点云安装
https://blog.csdn.net/weixin_42585456/article/details/121914686
具体步骤:
1.1 下载并安装点云文件(win10+VS2019+PCL1.12.0)
https://github.com/PointCloudLibrary/pcl/releases
-
直接安装即可,要注意路径不要有中文,尽量不要安装再c盘
-
安装过程可能会弹出OPENNI2的安装提示,安装在PCL安装中的3rdparty文件夹中即可
-
若没弹出来OpenNI2的安装,则需要在3rdparty文件夹中选择OpenNI2文件夹,点击里面的文件,弹窗出来选择remove,之后再次点击该文件,即可完成安装、
-
解压pcl-1.12.0-pdb-msvc2019-win64.zip,将得到的pcd文件复制到F:\PCL 1.12.0\bin(PCL安装路径的bin文件)中
1.2 配置环境变量
- 右键点击此电脑,选择属性,接着点击右侧的高级系统设置,然后选择高级选项卡,选择环境变量
2、bin2pcd(KITTI)
#include <ctime>
#include <string>
#include <vector>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <fstream>
#include <io.h>
using namespace std;
static std::vector<std::string> file_lists;
bool computePairNum(std::string pair1, std::string pair2)
{
return pair1 < pair2;
}
void sort_filelists(std::vector<std::string>& filists, std::string type)
{
if (filists.empty())return;
```
std::sort(filists.begin(), filists.end(), computePairNum);
```
}
void readKittiPclBinData(std::string& in_file, std::string& out_file)
{
// load point cloud
std::fstream input(in_file.c_str(), std::ios::in | std::ios::binary);
if (!input.good()) {
std::cerr << "Could not read file: " << in_file << std::endl;
exit(EXIT_FAILURE);
}
input.seekg(0, std::ios::beg);
```
pcl::PointCloud<pcl::PointXYZI>::Ptr points(new pcl::PointCloud<pcl::PointXYZI>);
int i;
for (i = 0; input.good() && !input.eof(); i++) {
pcl::PointXYZI point;
input.read((char*)&point.x, 3 * sizeof(float));
input.read((char*)&point.intensity, sizeof(float));
points->push_back(point);
}
input.close();
std::cout << "Read KTTI point cloud with " << i << " points, writing to " << out_file << std::endl;
pcl::PCDWriter writer;
// Save DoN features
writer.write< pcl::PointXYZI >(out_file, *points);
//pcl::io::savePCDFileASCII(out_file, *points);
```
}
//获取所有的文件名
void GetAllFiles(string path, vector<string>& files)
{
```
//long hFile = 0;
intptr_t hFile = 0;
//文件信息
struct _finddata_t fileinfo;
string p;
if ((hFile = _findfirst(p.assign(path).append("\\*").c_str(), &fileinfo)) != -1)
{
do
{
if ((fileinfo.attrib & _A_SUBDIR))
{
if (strcmp(fileinfo.name, ".") != 0 && strcmp(fileinfo.name, "..") != 0)
{
files.push_back(p.assign(path).append("\\").append(fileinfo.name));
GetAllFiles(p.assign(path).append("\\").append(fileinfo.name), files);
}
}
else
{
files.push_back(p.assign(path).append("\\").append(fileinfo.name));
}
} while (_findnext(hFile, &fileinfo) == 0);
_findclose(hFile);
}
```
}
//获取特定格式的文件名
void GetAllFormatFiles(string path, vector<string>& files, string format)
{
//文件句柄
//long hFile = 0;
intptr_t hFile = 0;
//文件信息
struct _finddata_t fileinfo;
string p;
if ((hFile = _findfirst(p.assign(path).append("\\*" + format).c_str(), &fileinfo)) != -1)
{
do
{
if ((fileinfo.attrib & _A_SUBDIR))
{
if (strcmp(fileinfo.name, ".") != 0 && strcmp(fileinfo.name, "..") != 0)
{
//files.push_back(p.assign(path).append("\\").append(fileinfo.name) );
GetAllFormatFiles(p.assign(path).append("\\").append(fileinfo.name), files, format);
}
}
else
{
files.push_back(p.assign(path).append("\\").append(fileinfo.name));
}
} while (_findnext(hFile, &fileinfo) == 0);
```
_findclose(hFile);
}
```
}
int main(int argc, char** argv)
{
std::string bin_path = "F:\\东南大学项目\\激光雷达\\2011_09_26_drive_0020_sync\\2011_09_26\\2011_09_26_drive_0020_sync\\velodyne_points\\data";
std::string pcd_path = "F:\\东南大学项目\\激光雷达\\2011_09_26_drive_0020_sync\\2011_09_26\\2011_09_26_drive_0020_sync\\velodyne_points\\pcddata";
GetAllFormatFiles(bin_path, file_lists, "bin");
sort_filelists(file_lists, "bin");
for (int i = 0; i < file_lists.size(); ++i)
{
std::string bin_file = file_lists[i];
int pos = bin_file.rfind("\\");
std::string file_name_bin = bin_file.substr(pos, 11);
std::string file_name = file_name_bin.substr(0, 11);
std::string pcd_file = pcd_path + file_name + ".pcd";
readKittiPclBinData(bin_file, pcd_file);
}
```
return 0;
```
}
3、点云可视化(C++)
#include<iostream>
#include<pcl/io/pcd_io.h>
#include<pcl/point_types.h>
#include<pcl/point_cloud.h>
#include <pcl/visualization/cloud_viewer.h>
using namespace std;
using namespace pcl;
typedef pcl::PointXYZI PointIoType;//对激光点云类型进行宏定义 方便统一修改
int main(int argc, char* argv[])
{
pcl::PointCloud<PointIoType>::Ptr cloud(new pcl::PointCloud<PointIoType>);
pcl::io::loadPCDFile<PointIoType>("F:/Lidar_related/pcl_test_project/0000000001.pcd", *cloud);
if (pcl::io::loadPCDFile<PointIoType>("F:/Lidar_related/pcl_test_project/0000000001.pcd", *cloud) == -1)
{
std::cout << "Cloud reading failed." << std::endl;
return (-1);
}
//std::cout << cloud->width << std::endl;
//std::cout << cloud->height;
pcl::visualization::CloudViewer viewer("Simple Cloud Viewer");
viewer.showCloud(cloud);
while (!viewer.wasStopped()) //持久化显示激光点云图像
{
}
system("pause");
return (0);
}
【很多资料整理自网络,仅供个人学习使用】
更多推荐
所有评论(0)