Lidar Obstacle Detection
Github: https://github.com/williamhyin/SFND_Lidar_Obstacle_Detection
Email: williamhyin@outlook.com
知乎专栏: 自动驾驶全栈工程师
Lidar Sensors
激光雷达传感器通过发射成千上万的激光信号, 为我们提供高分辨率的数据. 这些激光被物体反射回传感器, 然后可以通过计算信号返回所需的时间来确定物体的距离. 我们还可以通过测量返回信号的强度来了解一点被J激光击中物体的情况. 每一束激光都处于红外光谱中, 并以不同的角度发射出去, 通常是360度的范围. 尽管激光雷达传感器为我们提供了非常高精度的3 d 世界模型, 但它们目前非常昂贵, 甚至有的高达6万美元.
- 激光雷达以不同的角度发射数千束激光
- 激光被发射出来, 从障碍物上反射出来, 然后用接收器探测到
- 根据激光发射和接收的时间差, 计算出距离
- 接收到的激光强度值可用于评价被激光反射物体的材料性质
这是Velodyne 激光雷达传感器, 由左至右采用 HDL 64, HDL 32, VLP 16. 传感器越大, 分辨率越高. 下面是 HDL 64激光雷达的规格说明. 激光雷达共有64层, 每一层都以与 z 轴不同的角度发射出去, 因此倾斜度也不同. 每一层都覆盖了360度的视角, 角分辨率为0.08度. 激光雷达平均每秒扫描十次. 激光雷达可以探测到远达120米的汽车和树木, 还可以探测到远达50米的人行道.
VLP 64示意图, 显示激光雷达发射器、接收器和外壳
这个传感器有64层, 角分辨率为0.09度, 平均更新频率为10Hz, 每秒收集(64x (360 / 0.08) x10)=288万个数据
PCD data
让我们深入研究激光雷达数据是如何存储的. 激光雷达数据以一种称为点云数据(简称 PCD)的格式存储. PCD 文件是(x, y, z)笛卡尔坐标和强度值的列表, 它是在一次扫描之后环境的一个快照. 使用 VLP 64激光雷达, PCD 文件将有大约256,000个(x, y, z, i)点云值.
点云数据的坐标系与汽车的本地坐标系相同. 在这个坐标系中, x 轴指向汽车的前部, y 轴指向汽车的左侧. 此外, z轴指向车的上方.
PCL库 广泛应用于机器人技术领域, 用于处理点云数据, 网上有许多教程可供使用. PCL 中有许多内置的功能可以帮助检测障碍物. 本项目后面会使用 PCL内置的分割、提取和聚类函数. 你在这里可以找到PCL库的文档.
Steps For Obstacle Detection
Stream PCD
首先我们需要流式载入激光点云数据.
template<typename PointT>
std::vector<boost::filesystem::path> ProcessPointClouds<PointT>::streamPcd(std::string dataPath) {
std::vector<boost::filesystem::path> paths(boost::filesystem::directory_iterator{dataPath},boost::filesystem::directory_iterator{});
// sort files in accending order so playback is chronological
sort(paths.begin(), paths.end());
return paths;
}
// ####################################################
ProcessPointClouds<pcl::PointXYZI>* pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
std::vector<boost::filesystem::path> stream = pointProcessorI >streamPcd("../src/sensors/data/pcd/data_1");
auto streamIterator = stream.begin();
pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloudI;
真实PCD数据
Point Processing
处理点云数据的第一步就是要创建一个processPointClouds
的对象, 这个对象中包含所有处理激光点云数据的模块, 如过滤, 分割, 聚类, 载入、存储PCD数据. 我们需要为不同的点云数据创建一个通用模板: template<typename PointT>
. 在真实点云数据中, 点云的类型是pcl::PointXYZI
. 创建pointProcessor
可以建立在Stack上也可以建立在Heap上, 但是建议在Heap上, 毕竟使用指针更加轻便.
// Build PointProcessor on the heap
ProcessPointClouds<pcl::PointXYZI> *pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
// Build PointProcessor on the stack
ProcessPointClouds<pcl::PointXYZI> pointProcessorI;
Filtering
值得注意的是点云数据一般有很高的分辨率和相当远的可视距离. 我们希望代码处理管道能够尽可能快地处理点云, 因此需要对点云进行过滤. 这里有两种方法可以用来做到这一点.
-
Voxel Grid
体素网格过滤将创建一个立方体网格, 过滤点云的方法是每个体素立方体内只留下一个点, 因此立方体每一边的长度越大, 点云的分辨率就越低. 但是如果体素网格太大, 就会损失掉物体原本的特征. 具体实现可以查看PCL-voxel grid filtering的文档 .
-
Region of Interest
定义感兴趣区域, 并删除感兴趣区域外的任何点. 感兴趣区域的选择两侧需要尽量覆盖车道的宽度, 而前后的区域要保证你可以及时检测到前后车辆的移动. 具体实现可以查看PCL-region of interest的文档. 在最终结果中, 我们使用
pcl CropBox
查找自身车辆车顶的点云数据索引, 然后将这些索引提供给pcl ExtractIndices
对象删除, 因为这些对于我们分析点云数据没有用处.感兴趣区域及体素网格过滤后的结果
以下是Filtering的代码实现:
filterRes
是体素网格的大小,minPoint/maxPoint
为感兴趣区域的最近点和最远点.我们首先执行VoxelGrid减少点云数量, 然后设置最近和最远点之间的感兴趣区域, 最后再从中删除车顶的点云.
// To note, "using PtCdtr = typename pcl::PointCloud<PointT>::Ptr;" template<typename PointT> PtCdtr<PointT> ProcessPointClouds<PointT>::FilterCloud(PtCdtr<PointT> cloud, float filterRes, Eigen::Vector4f minPoint,Eigen::Vector4f maxPoint) { // Time segmentation process auto startTime = std::chrono::steady_clock::now(); // TODO:: Fill in t