本文主要是介绍【Lidar】Lidar激光雷达一篇全（两万字激光雷达详细介绍），希望对大家解决编程问题提供一定的参考价值，需要的开发者们随着小编来一起学习吧！
【Lidar】Lidar激光雷达一篇全
- 1. 激光雷达测距原理
- 1.1 三角测距
- 1.2 飞行时间测距（TOF，Time of Flight）
- 2. 车载激光雷达类别
- 2.1 机械旋转式
- 2.2 固态
- 2.2.1 相控阵
- 2.3 半固态（混合固态）
- 2.3.2 微转镜/棱镜
- 3. 激光雷达性能指标
- 3.1 点云性能指标
- 3.2 硬件性能
- 4. 激光雷达数据集
- 4.1 Kitti
- 4.2 nuScenes
- 4.3 Argoverse
- 5. 激光雷达相关硬件
- 5.1 GPS
- 5.2 IMU
- 5.3 激光雷达GPS及IMU联合标定
- 6. 激光雷达摄像头联合标定
- 6.1 联合标定方法
- 6.1.1 概述
- 6.1.2 制作标定板
- 6.1.3 设置参数
- 6.1.4 正式开始标定
- 6.1.5 标定结束后会输出标定参数
- 6.2 标定使用
- 7. 激光雷达在自动驾驶领域应用
- 7.1 目标检测
- 7.1.1 检测流程
- 7.1.2 点云库PCL中点云滤波去噪方法
- 7.1.2.1 体素网格滤波
- 7.1.2.2 直通滤波
- 7.1.2.3 半径滤波
- 7.1.2.4 统计滤波器
- 7.1.2.5 双边滤波
- 7.1.2.6 均匀采样滤波
- 7.1.2.7 高斯滤波
- 7.1.2.8 条件滤波
- 7.1.3 传统目标检测方法
- 7.1.3.1 欧几里得聚类
- 7.1.3.1.1 简介
- 7.1.3.1.2 按距离分割点云
- 7.1.3.1.3 聚类并计算障碍物中心和Bounding Box
- 7.1.3.2 Kmeans
- 7.1.3.3 DBSCAN
- 7.1.3.4 RANSAC
- 7.1.4 目标检测网络
- 7.1.4.1 cnn_seg
- 7.1.4.1.1 根据objectness层信息，检测出障碍物栅格点目标
- 7.1.4.1.2 根据center offset层信息，对检测到的障碍物栅格点进行聚类，得到聚类目标
- 7.1.4.1.3 根据positiveness和object height层信息，对背景以及每个点云簇中高处的点进行过滤
- 7.1.4.1.4 根据class probability，对每个聚类目标进行分类，得到最终的检测目标
- 7.1.4.1.5 算法结果
- 7.2 激光slam
- 8. 现有OEM使用的激光雷达
1. 激光雷达测距原理
1.1 三角测距
该方案主要用于单线激光雷达，常见的为扫地机器人使用的单线激光雷达
三角测距法是利用相似三角形本地可以测得透镜中心和照射到CMOS/CCD上的距离，就可以得到物距，本地的分辨率l决定了探测物体的分辨率。因此当物距越大，探测精度越低。因此该方法测距较短且需要大面积的光电探测器。
1.2 飞行时间测距（TOF，Time of Flight）
车载激光雷达常用
TOF为目前大多数的自动驾驶采用的雷达方案，通过发射脉冲激光，计算脉冲激光经过目标并反射回探测器的时间，即飞行时间。飞行时间等于脉冲数n和脉冲间隔t的乘积，距离就等于飞行时间和速度的乘积,如下面公式所示，f为脉冲频率。
Δt=n/f d=(c⋅Δt)/2
这种方式简单直接，测距的精度不随距离增加而恶化，但是由于光速快，短距下对器件的响应时间要求高。
2. 车载激光雷达类别
2.1 机械旋转式
2.2 固态
2.2.1 相控阵
阵列激光雷达类似于毫米波相控阵雷达，通过改变每个通道光相位，从而改变合成光的发射角度，然后进行逐行逐列的扫描
2.2.2 FLASH
FLASH雷达原理是激光发射器发出面阵的激光并接收，形成点云图像
2.3 半固态（混合固态）
2.3.1 MEMS
MEMS激光雷达原理是激光打在MEMS振镜上，通过振镜上下左右两个方向摆动的扫描方式探测周围环境
https://www.bilibili.com/video/BV1yY4y1N7Hy/?spm_id_from=333.999.0.0
2.3.2 微转镜/棱镜
微转镜和棱镜是类似的，激光经过微转镜或棱镜进行激光散射扫描探测
https://www.bilibili.com/video/BV1fG411c7of/?spm_id_from=333.788.video.desc.click
https://www.bilibili.com/video/BV1fG411c7of/?spm_id_from=333.999.0.0&vd_source=f47bec4f4d13ace8bb3f94678528dcaa
3. 激光雷达性能指标
3.1 点云性能指标
激光雷达点云的性能指标主要包括线束、波长、测量精度、探测距离、点频、垂直FOV、水平FOV、旋转频率
3.2 硬件性能
激光雷达的硬件性能主要包括工作温度、工作功率、时间同步类型、防尘防水等级
4. 激光雷达数据集
4.1 Kitti
Kitti数据集使用的激光雷达型号为Velodyne HDL-64E，该激光雷达为64线机械旋转式激光雷达
http://www.cvlibs.net/datasets/kitti/
4.2 nuScenes
nuScenes数据集使用的激光雷达型号为Velodyne HDL-32E，该激光雷达为32线机械旋转式激光雷达https://www.nuscenes.org/
4.3 Argoverse
Argoverse数据集使用的激光雷达型号为Velodyne VLP-32C，使用两台堆叠放置，该激光雷达为32线机械旋转式激光雷达
https://www.argoverse.org/index.html
5. 激光雷达相关硬件
激光雷达需要和GPS及IMU进行联合标定，以达到足够的精度
5.1 GPS
其中GPS主要给激光雷达授时，其能发出以下信号，发布话题为：经纬度/fix，速度/vel，gps时间/time_reference和航向角/heading
5.2 IMU
IMU给激光雷达自车pose信息及运动学信息
5.3 激光雷达GPS及IMU联合标定
激光雷达与GPS和IMU标定主要是为了得到传感器之间的平移矩阵和旋转矩阵以统一输出坐标系，使传感器之间得到位置关系
https://github.com/ApolloAuto/apollo/blob/43922a04d0ecbdb2da4bf2d77b445fd11017e563/docs/06_Perception/multiple_lidar_gnss_calibration_guide_cn.md?plain=1#L18
https://github.com/ApolloAuto/apollo/blob/43922a04d0ecbdb2da4bf2d77b445fd11017e563/docs/11_Hardware%20Integration%20and%20Calibration/%E4%BC%A0%E6%84%9F%E5%99%A8%E6%A0%87%E5%AE%9A/apollo_lidar_imu_calibration_guide.md?plain=1#L4
6. 激光雷达摄像头联合标定
6.1 联合标定方法
6.1.1 概述
在众多的lidar和camera标定的开源程序中，效果相对不错的就是cam_lidar_calibration了，其余开源要么标定过程复杂、要么误差太大，该开源包经过一些改版。下列是cam_lidar_calibration系列的开源算法
2019年最原始版本：Automatic extrinsic calibration between a camera and a 3D Lidar using 3D point and plane correspondences，利用棋盘格作为定目标，通过利用标定板的几何形状，计算lidar点云中的中线点和法向量，并求得其在camera下的中线点和法向量，优化后完成标定。 https://gitlab.acfr.usyd.edu.au/sverma/cam_lidar_calibration
2020年版本：根据2019的代码做了一点修改
https://github.com/AbangLZU/cam_lidar_calibration
2021年版本：Optimising the selection of samples for robust lidar camera calibration，同为悉尼大学ACFR实验室，基于2019年的改进版，采用质量变异性（VOQ）来减少用户选择数据的错误，克服了过度拟合的问题，比2019年版本的标定过程更为稳健。
https://github.com/acfr/cam_lidar_calibration
6.1.2 制作标定板
标定板要尽量做得大些，标定板文件PDF下载，有设置好的尺寸，A1 - 95mm squares - 7x5 verticies, 8x6 squares。将棋盘纸张固定在一个板上，使他们的中心尽量对齐，边缘保持平行，在后面需要设置参数。下图为A1大小的7*5内部顶点和95mm正方形。
6.1.3 设置参数
主要修改cam_lidar_calibration/cfg/camera_info.yaml和params.yaml
camera_info.yaml:设置是否为鱼眼相机、像素宽和高、内参矩阵和失真系数。相机参数自行标定
D矩阵：畸变矫正参数矩阵。它包含了相机图像坐标系下的径向畸变和切向畸变系数，用于将畸变像素坐标转化为无畸变的像素坐标。
K矩阵：内参矩阵。它描述了相机光轴、焦距、光心等内部参数
distortion_model: "non-fisheye"width: 640
height: 480
D: [0,0,0,0]
K: [617.68,0.0,325.963,0.0,617.875,242.513,0.0,0.0,1]params.yaml
# Topics
camera_topic: "/camera/color/image_raw"
camera_info: "/camera/color/camera_info"
lidar_topic: "/velodyne_points"#Dynamic rqt_reconfigure default bounds,点云的选取范围
feature_extraction:
x_min: -10.0
x_max: 10.0
y_min: -8.0
y_max: 8.0
z_min: -5.0
z_max: 5.0# Properties of chessboard calibration target
chessboard:
pattern_size: #棋盘的内部顶点7*5
height: 7
width: 5
square_length: 95 #棋盘格的长度mm
board_dimension: # 安装棋盘打印的背板的宽度和高度。
width: 594
height: 897
translation_error: #棋盘中心与背板中心的偏移量（见下图）。
x: 0
y: 0
棋盘中心与背板中心的偏移量
6.1.4 正式开始标定
开启程序采集表定数据，运行命令：
roslaunch cam_lidar_calibration run_optimiser.launch import_samples:=false
会出现RVIZ和rqt_reconfigure窗口，在RVIZ中panels->display修改相机的话题和激光雷达点云对应的frame_id。
采集数据：分割出标定板的点云
调整rqt_reconfigure /feature_extraction的xyz最大值最小值以使得标定板的点云和周围环境分开，使其仅显示棋盘。如果棋盘没有完全隔离，可能会影响棋盘的平面拟合，还会导致棋盘尺寸误差较大。下图是过滤点云前后效果：
在过滤周围环境点云后，在rviz中点击Capture sample采集样本，会出线绿色框代表根据点云拟合出来的标定板平面，如图：
若未出现绿色拟合的矩形，则需要重新调整点云的XYZ范围，再进行capture sample采集样本。
标定：
采集样本至少3个才能运行标定，样本越多越好，最好采集10个样本以上，再点击rviz中的optimise进行标定，在优化过程中将会在cam_lidar_calibration/data生成当前时间日期的文件夹，存放采集的图像、点云pcd、位姿，标定后camer和lidar外参文件。
6.1.5 标定结束后会输出标定参数
Calibration params (roll,pitch,yaw,x,y,z): -1.5809,0.0041,-1.4960,0.0706,0.0141,-0.1139
6.2 标定使用
坐标变换的方法采用了 Geiger, A 等在论文中提到的方法，最终将激光点云转换到相机坐标系下。毫米波雷达目标变换也是一样的。
要将 Velodyne 坐标中的点 x 投影到左侧的彩色图像中 y：
将 Velodyne 坐标中的点投影到右侧的彩色图像中：
是将 Velodyne 坐标中的点 x 投影到编号为 0 的相机（参考相机）坐标系中
是将 Velodyne 坐标中的点 x 投影到编号为 0 的相机（参考相机）坐标系中，再修正，
是将 Velodyne 坐标中的点 x 投影到编号为 0 的相机（参考相机）坐标系中，再修正，然后投影到编号为 2 的相机（左彩色相机）。
修正后的空间三维点云Tx (x, y, z,1) 投影到第 i个摄像机图像中的点Ty (u,v,1) 的投影为：
而将参考相机坐标中的 3D 点 x 投影到第 i 个图像平面上的点 y,还需要考虑校正旋转矩阵如下式：
扩展为 4×4 矩阵。最后,如果要把激光雷达坐标中的点转换到参考相机坐标上：
![[图片]](https://img-blog.csdnimg.cn/direct/53f9e8e9e50e4c3bbfaf593412d5db71.png)
图 3-3、图 3-4、图 3-5 为激光点云坐标系转换至摄像头的坐标系中，可以看出数据对齐的比较好。
7. 激光雷达在自动驾驶领域应用
7.1 目标检测
7.1.1 检测流程
目标检测流程主要包含三个任务，点云滤波去噪、地面点云分割、目标检测
7.1.2 点云库PCL中点云滤波去噪方法
7.1.2.1 体素网格滤波
体素网格滤波用于对稠密点云进行降采样，其首先把 3D 空间划分成多个很小的体素， 然后将每个体素网格的中心点作为该网格内的唯一点，体素网格滤波可以在减少数据量的同时很好的保留原始点云的几何结构。 PCL 点云库中通过 VoxelGrid 函数进行体素网格滤波，使用 setLeafSize 函数来设置网格的大小， leafSize 越大表示每个网格的体积越大，保留的点云数量越少。
#include <pcl/point_types_conversion.h>
#include <pcl/filters/voxel_grid.h>void DownSample_Filter(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_in,pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud_downsize,double downsize){pcl::VoxelGrid<pcl::PointXYZI> downsample_filter;downsample_filter.setLeafSize(downsize, downsize, downsize);downsample_filter.setDownsampleAllData(true); //对全字段进行下采样;downsample_filter.setInputCloud(cloud_in);downsample_filter.filter(*cloud_downsize);
}
7.1.2.2 直通滤波
直通滤波通过限定点云三轴坐标的范围快速剔除掉不在需求范围内的离群点。 PCL 点云库中直通滤波函数为 PassThrough， 使用函数 setFilterFieldName设置需要剔除点云的字段（x、 y、 z、intensity）， setFilterLimits 设置保留点云数据相应坐标轴的范围。
C++
#include <pcl/filters/passthrough.h>//直通滤波器，在Z轴方向上剪除多余的点
void PassThrough_filter(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud,pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud_medium,double min, double max)
{pcl::PassThrough<pcl::PointXYZI> pass; //创建滤波器对象pass.setInputCloud (cloud); //设置待滤波的点云pass.setFilterFieldName ("x"); //设置在x轴方向上进行滤波pass.setNegative (false); //false:保留当前范围内数据;true:取反;pass.setFilterLimits (min, max); //设置滤波范围为min~max,在范围之外的点会被剪除pass.filter (*cloud_medium); //存储
}
7.1.2.3 半径滤波
半滤波通过计算每个点在设置半径范围内是否有足够的点云数量来进行滤波处理，可以快速移除稀疏离群点。 PCL 点云库中的半径滤波函数为RadiusOutlierRemoval，通过函数 setRadius 设置进行统计计算的半径，setMinNeighborsInRadius 来判断该点是否为稀疏离群点的阈值。
C++
#include <pcl/point_types.h>
#include <pcl/filters/radius_outlier_removal.h>void Radius_outlier_filter(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_input,pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud_filter,float radius, int num)
{pcl::RadiusOutlierRemoval<pcl::PointXYZI> outrem; // 创建滤波器 outrem.setInputCloud(cloud_input); //设置输入点云/*搜索半径设为radius，在此半径内点必须要有至少num个邻居时，此点才会被保留/outrem.setRadiusSearch(radius); //设置在radius半径的范围内找邻近点outrem.setMinNeighborsInRadius(num); //设置查询点的邻近点集数小于num的删除outrem.filter(*cloud_filter); //执行条件滤波，存储结果到cloud_filter
}
7.1.2.4 统计滤波器
用于去除明显离群点，即噪声数据。噪声信息属于无用信息，信息量较小。所以离群点表达的信息可以忽略不计。考虑到离群点的特征，则可以定义某处点云小于某个密度，既点云无效。计算每个点到其最近的k个点平均距离。则点云中所有点的距离应构成高斯分布。
C++
//统计滤波器，删除离群点
void staticOutlierRemoval(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_input,pcl::PointCloud<pcl::PointXYZI>::Ptr &cloud_filtered,int num, float threshold)
{pcl::StatisticalOutlierRemoval<pcl::PointXYZI> Static; //创建滤波器对象Static.setInputCloud(cloud_input); //设置待滤波的点云Static.setMeanK(num); //设置在进行统计时考虑查询点临近点数Static.setStddevMulThresh(threshold); //设置判断是否为离群点的阈值Static.filter(*cloud_filtered); //存储
}
7.1.2.5 双边滤波
双边滤波是一种非线性滤波器，它可以达到保持边缘、降噪平滑的效果。一定程度上拟补了高斯滤波的缺点。双边滤波对高斯噪声效果比较好。双边滤波从单纯的考虑空间域点的位置的高斯滤波上，又加上一个维度上的权重。在点云处理上，可以叫做为特征域，即当前点的法向量与临近点的法向量。
C++
#include <pcl/filters/fast_bilateral.h>
void Bilateral_Filter(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_input,pcl::PointCloud<pcl::PointXYZI>::Ptr &result,float sigma_s, float sigma_R )
{pcl::FastBilateralFilter<pcl::PointXYZI> filter;filter.setInputCloud(cloud_input);filter.setSigmaS(sigma_s)； //0.5 空间邻域/窗口设置双边滤波器使用的高斯标准差filter.setSigmaR(sigma_R); //0.0004 设置高斯的标准偏差，用于控制由于强度差,而使相邻像素的权重降低的程度filter.applyFilter(*result);
}
7.1.2.6 均匀采样滤波
均匀采样滤波基本上等同于体素滤波器，但是其不改变点的位置。下采样后，其点云分布基本均匀，但是其点云的准确度要好于体素滤波，因为没有移动点的位置。
C++
#include <pcl/keypoints/uniform_sampling.h>void Uniform_Filter(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_input,pcl::PointCloud<pcl::PointXYZI>::Ptr &result,float radius)
{pcl::UniformSampling<pcl::PointXYZI> sor2; // 创建均匀采样对象sor2.setInputCloud(cloud_input); //设置需要过滤的点云sor2.setRadiusSearch(radius); //设置滤波时创建的体素体积为1cm的立方体,radius=0.01fsor2.filter(*result); //执行滤波处理，储存输出点云
}
7.1.2.7 高斯滤波
高斯滤波是一种非线性滤波器，采用加权平均的方式,高斯滤波相当于一个具有平滑性能的低通滤波器。在指定域内的权重是根据欧式距离的高斯分布，通过权重加权平均的方式得到当前点的滤波后的点。
7.1.2.8 条件滤波
条件滤波器通过设定滤波条件进行滤波，有点分段函数的味道，当点云在一定范围则留下，不在则舍弃。直通滤波器是一种较简单的条件滤波器。条件滤波器更像是一个不带有滤波核的工具。
C++
#include <pcl/filters/conditional_removal.h>//创建条件限定的下的滤波器
void conditional_Filter(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_input,pcl::PointCloud<pcl::PointXYZI>::Ptr &result,)
{pcl::ConditionAnd<pcl::PointXYZI>::Ptr range_cond (new pcl::ConditionAnd<pcl::PointXYZI> ()); //创建条件定义对象/为条件定义对象添加比较算子*/range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZI>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZI> ("z", pcl::ComparisonOps::GT, 0.0))); //添加在Z字段上大于0的比较算子range_cond->addComparison (pcl::FieldComparison<pcl::PointXYZI>::ConstPtr (new pcl::FieldComparison<pcl::PointXYZI> ("z", pcl::ComparisonOps::LT, 0.8))); //添加在Z字段上小于0.8的比较算子/创建滤波器并用条件定义对象初始化/pcl::ConditionalRemoval<pcl::PointXYZI> condrem;condrem.setCondition (range_cond); condrem.setInputCloud (cloud_input); //输入点云condrem.setKeepOrganized(true); //设置保持点云的结构condrem.filter (*result); //大于0.0小于0.8这两个条件用于建立滤波器
}
7.1.3 传统目标检测方法
7.1.3.1 欧几里得聚类
7.1.3.1.1 简介
欧几里得聚类使用了一个2维树，将点云压缩成了2维——即将所有点的z值（高度）设为0，这么做的原因在于一方面我们并不关心点云簇在z方向的搜索顺序（两个物体在z方向叠在一起我们可以将其视为一个障碍物），另一方面我们希望能够加快我们的聚类速度以满足无人车感知实时性的需求。此外，一个2维树以更方便读者理解KD Tree。使用二维树对平面上的点进行划分如下图所示（有在点上划分的，有在中点划分的，原理是一致的）：
关于欧式距离聚类具体算法流程，PCL官方文档提供了如下伪代码：
7.1.3.1.2 按距离分割点云
所以，首先将扫描的点云按照其到雷达的聚类切分成五个点云，以下是分类以及聚类的距离大小：
0-15m d=0.5
15-30 d=1
30-45 d=1.6
45-60 d=2.1
>60 d=2.6
代码：
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segment_pc_array(5);for (size_t i = 0; i < segment_pc_array.size(); i++){pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);segment_pc_array[i] = tmp;}for (size_t i = 0; i < in_pc->points.size(); i++){pcl::PointXYZ current_point;current_point.x = in_pc->points[i].x;current_point.y = in_pc->points[i].y;current_point.z = in_pc->points[i].z;float origin_distance = sqrt(pow(current_point.x, 2) + pow(current_point.y, 2));// 如果点的距离大于120m, 忽略该点if (origin_distance >= 120){continue;}if (origin_distance < seg_distance_[0]){segment_pc_array[0]->points.push_back(current_point);}else if (origin_distance < seg_distance_[1]){segment_pc_array[1]->points.push_back(current_point);}else if (origin_distance < seg_distance_[2]){segment_pc_array[2]->points.push_back(current_point);}else if (origin_distance < seg_distance_[3]){segment_pc_array[3]->points.push_back(current_point);}else{segment_pc_array[4]->points.push_back(current_point);}}
7.1.3.1.3 聚类并计算障碍物中心和Bounding Box
在五个不同区域内聚类，结束后计算其形心作为该障碍物的中心，同时计算点云簇的长宽高，从而确定一个能够将点云簇包裹的三维Bounding Box，代码如下
void EuClusterCore::cluster_segment(pcl::PointCloud<pcl::PointXYZ>::Ptr in_pc,double in_max_cluster_distance, std::vector<Detected_Obj> &obj_list)
{pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);//create 2d pcpcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2d(new pcl::PointCloud<pcl::PointXYZ>);pcl::copyPointCloud(*in_pc, *cloud_2d);//make it flatfor (size_t i = 0; i < cloud_2d->points.size(); i++){cloud_2d->points[i].z = 0;}if (cloud_2d->points.size() > 0)tree->setInputCloud(cloud_2d);std::vector<pcl::PointIndices> local_indices;pcl::EuclideanClusterExtraction<pcl::PointXYZ> euclid;euclid.setInputCloud(cloud_2d);euclid.setClusterTolerance(in_max_cluster_distance);euclid.setMinClusterSize(MIN_CLUSTER_SIZE);euclid.setMaxClusterSize(MAX_CLUSTER_SIZE);euclid.setSearchMethod(tree);euclid.extract(local_indices);for (size_t i = 0; i < local_indices.size(); i++){// the structure to save one detected objectDetected_Obj obj_info;float min_x = std::numeric_limits<float>::max();float max_x = -std::numeric_limits<float>::max();float min_y = std::numeric_limits<float>::max();float max_y = -std::numeric_limits<float>::max();float min_z = std::numeric_limits<float>::max();float max_z = -std::numeric_limits<float>::max();for (auto pit = local_indices[i].indices.begin(); pit != local_indices[i].indices.end(); ++pit){//fill new colored cluster point by pointpcl::PointXYZ p;p.x = in_pc->points[*pit].x;p.y = in_pc->points[*pit].y;p.z = in_pc->points[*pit].z;obj_info.centroid_.x += p.x;obj_info.centroid_.y += p.y;obj_info.centroid_.z += p.z;if (p.x < min_x)min_x = p.x;if (p.y < min_y)min_y = p.y;if (p.z < min_z)min_z = p.z;if (p.x > max_x)max_x = p.x;if (p.y > max_y)max_y = p.y;if (p.z > max_z)max_z = p.z;}//min, max pointsobj_info.min_point_.x = min_x;obj_info.min_point_.y = min_y;obj_info.min_point_.z = min_z;obj_info.max_point_.x = max_x;obj_info.max_point_.y = max_y;obj_info.max_point_.z = max_z;//calculate centroidif (local_indices[i].indices.size() > 0){obj_info.centroid_.x /= local_indices[i].indices.size();obj_info.centroid_.y /= local_indices[i].indices.size();obj_info.centroid_.z /= local_indices[i].indices.size();}//calculate bounding boxdouble length_ = obj_info.max_point_.x - obj_info.min_point_.x;double width_ = obj_info.max_point_.y - obj_info.min_point_.y;double height_ = obj_info.max_point_.z - obj_info.min_point_.z;obj_info.bounding_box_.header = point_cloud_header_;obj_info.bounding_box_.pose.position.x = obj_info.min_point_.x + length_ / 2;obj_info.bounding_box_.pose.position.y = obj_info.min_point_.y + width_ / 2;obj_info.bounding_box_.pose.position.z = obj_info.min_point_.z + height_ / 2;obj_info.bounding_box_.dimensions.x = ((length_ < 0) ? -1 * length_ : length_);obj_info.bounding_box_.dimensions.y = ((width_ < 0) ? -1 * width_ : width_);obj_info.bounding_box_.dimensions.z = ((height_ < 0) ? -1 * height_ : height_);obj_list.push_back(obj_info);}
}
放到 pcl::EuclideanClusterExtraction 是一个已经平面化的二维点云，这种做法能够带来速度的提升。这里我们定义了一个结构体 Detected_Obj ，使用Detected_Obj存储检测到的障碍物信息：
struct Detected_Obj{jsk_recognition_msgs::BoundingBox bounding_box_;pcl::PointXYZ min_point_;pcl::PointXYZ max_point_;pcl::PointXYZ centroid_;};
将检测的障碍物的Bounding Box发布到 /detected_bounding_boxs 话题上：
jsk_recognition_msgs::BoundingBoxArray bbox_array;for (size_t i = 0; i < global_obj_list.size(); i++){bbox_array.boxes.push_back(global_obj_list[i].bounding_box_);}bbox_array.header = point_cloud_header_;pub_bounding_boxs_.publish(bbox_array);原始点云地图：
检测结果：
对移动的行人（红框内）也能检测到。该方法虽然能够实现点云的聚类，但是受非道路元素的影响比较大
7.1.3.2 Kmeans
7.1.3.3 DBSCAN
7.1.3.4 RANSAC
7.1.4 目标检测网络
7.1.4.1 cnn_seg
CNN_SEG算法采用了基于鸟瞰俯视方向（Bird View, BV）点云训练的神经网络进行障碍物检测。其能够识别的对象类型有六类（car, bicycle, track，bus, pedestrian, sign and others）。测试所用的点云数据是Kitti开源数据。
cnnseg其实分为两步，第一步是用一个深度学习网络预测五层信息，center offset, objectness, positiveness, object height, class probability，第二步则是利用这五层信息进行聚类。具体步骤如下：
7.1.4.1.1 根据objectness层信息，检测出障碍物栅格点目标
分别用图像和点云来说明objectness层的作用。objectness输出的是一个[0,1]的二维矩阵，与点云的栅格图对应（在cnn_seg中，对点云构建了2.5D栅格，每个栅格对应8维特征）。通过对objectness置信度取阈值（默认0.5），就可将object的像素点检测出来。下图用点云的反射强度信息显示被检测出的区域。
7.1.4.1.2 根据center offset层信息，对检测到的障碍物栅格点进行聚类，得到聚类目标
center offset是一个两层的二维矩阵，分别是x轴的offset和y轴的offset，将这两层结合在一起，就对应了每个栅格的offset，用箭头的方向和长短表示offset。
7.1.4.1.3 根据positiveness和object height层信息，对背景以及每个点云簇中高处的点进行过滤
positiveness是一个filter信息。从上一步其实可以看到，得到的点云簇，有很多的背景区域。通过统计每个点云簇的positiveness的均值，在经过阈值（默认0.1），就可以过滤掉一些背景的聚类了。
object height也是一个filter信息，过滤掉栅格中较高的点。
7.1.4.1.4 根据class probability，对每个聚类目标进行分类，得到最终的检测目标
通过以上几步，cnnseg已经将点云分割成一个个的点云簇了，类型分别是大车，小车，行人，自行车，未知。通过统计每个点云簇的每一类的概率值，得分高者为其类，从而实现了对每个点云簇的分类，得到最终的target。
下图中，用红色表示小车，蓝色表示大车，绿色表示行人。
7.1.4.1.5 算法结果
下图展现了该算法检测出的对象，如图中方形盒子所示，上面还有各对象的类型及距本车的距离。
下图检测后的点云话题输出，可以看出路边的车辆以及对向行驶的一辆车。另一张为检测到的目标的话题输出，可以看出各种类型的对象检测的都比较正确，至此，该部分算法测试完成。
7.2 激光slam
https://www.bilibili.com/video/BV1Bv411t7Z2/?spm_id_from=333.337.search-card.all.click&vd_source=f47bec4f4d13ace8bb3f94678528dcaa
回环检测
https://www.bilibili.com/video/BV1sT4y1D7DC/?spm_id_from=333.337.search-card.all.click&vd_source=f47bec4f4d13ace8bb3f94678528dcaa
8. 现有OEM使用的激光雷达
这篇关于【Lidar】Lidar激光雷达一篇全（两万字激光雷达详细介绍）的文章就介绍到这儿，希望我们推荐的文章对编程师们有所帮助！
