第三章：激光雷达感知功能开发
0. 激光雷达感知介绍
激光雷达的测距原理主流是TOF（time of flight），时间飞行法。通过光的飞行时间来测量距离。
按照激光雷达的扫描方式，有机械旋转式（hesai40p）、半固态（at128，一径ML30s）、纯固态（FT120）。
激光雷达有诸多特点：
长处：
- 三维重建：能够得到物体的三维点云，可以获得目标的几何特征；
- 准确测距、定位：测距准确，对感知的安全性有保证。
不足：
- 无法穿透雨、雪、尘、雾等：在特殊天气条件下，点云数据噪声较多，感知难度很大；
- 点云数据较稀疏，且没有纹理、颜色信息：场景信息的丰富度不高，对目标分类效果不够好。
自动驾驶中的激光雷达感知内容：
激光雷达感知，以空间的视角来看单帧点云，是在做全景分割：即每个点有一个语义类别，同时有instance label。能够区分地面、植被、建筑物等背景，也能检测出车辆、行人、自行车等目标。
以时间的视角来看多帧连续的点云，是在动静态的分析。浅层的信息：得到每个点在(x,y,z)三个方向在t时间的运动距离，得到每个点是静止还是运动。深层信息：得到目标的运动状态，例如位置、速度、朝向、角速度、加速度等。
1. 激光雷达感知流程
在驱动层面，将多个激光雷达（通常包含顶部一个主liar，侧向多个lidar）根据外参融合为一帧，然后做运动补偿，最后输出给感知模块。
激光雷达感知有六个component，处理流程是一个线性关系。
感知对激光雷达模块做了拆分，相比之前具备以下特点：
- 每个 component 完成一个独立的任务，相比之前更加简洁；
- 模块依赖关系呈线性，激光雷达感知的思路更加清晰；
- 降低开发者学习成本，易于二次开发。
1.1 pointcloud_preprocess
1.1.1 组件功能
点云预处理模块对点云做过滤，删除异常的、感知不需要的点云。
- 删除nan、inf、-inf值点云；
- 删除过远（超过1000m）的点云；
- 删除过高的点云；
- 点云转化到主车自身坐标系，删除扫描到主车身上的点云。
1.1.2 代码结构
本组件一级目录结构如下所示。
├── pointcloud_preprocess├── conf // 组件配置文件├── dag // dag文件├── data // 配置参数├── launch // 启动文件├── interface // 预处理接口类├── proto // proto定义，组件配置├── preprocessor // 预处理功能├── pointcloud_preprocess_component.cc // 组件定义，以及入口├── pointcloud_preprocess_component.h├── cyberfile.xml // 包管理文件├── README.md // 说明文档└── BUILD // 编译文件
1.2 pointcloud_map_based_roi
1.2.1 组件功能
点云基于地图计算兴趣区域（roi，region of interest），根据高精度地图的road和junction边界判断点云是否在高精度地图内，获得在高精地图内的点的索引。
基于地图roi过滤示意图，将地图元素投影到二维平面。roi内的像素设置标志位，计算每个点云所在像素的坐标，根据标志位判断点云是否在roi内。
下图是基于地图roi过滤的效果图。红色的是roi内的点云，白色的是roi外的点云。
红色的点云是高精度地图内的点云，白色的点云是高精度地图外的点云。此功能可用于后续做目标roi过滤。
1.2.2 代码结构
本组件一级目录结构如下所示。
├── pointcloud_map_based_roi├── conf // 组件配置文件├── dag // dag文件├── data // 配置参数├── launch // 启动文件├── map_manager // 地图管理类├── roi_filter // 过滤功能代码├── interface // roi filter接口定义├── proto // proto定义├── pointcloud_map_based_roi_component.cc // 组件定义，以及入口├── pointcloud_map_based_roi_component.h├── cyberfile.xml├── README.md└── BUILD
1.3 pointcloud_ground_detection
1.3.1 组件功能
地面点检测功能是检测出地面点，获得所有非地面点的索引，即non_ground_indices。
上图是地面点云检测的示例图，红色的点云是非地面点云，白色的是地面点云。分割出地面点云后，去除前景目标点云。然后用剩余的非地面点云做聚类，检测当前场景下的所有目标，保证自动驾驶的安全性。
1.3.2 代码结构
本组件一级目录结构如下所示。
├── pointcloud_ground_detection├── conf // 组件配置文件├── dag // dag文件├── data // 配置参数├── ground_detector // 地面点分割功能├── launch // 启动文件├── interface // 地面点检测接口类├── proto // proto定义├── pointcloud_ground_detection_component.cc // 组件定义，以及入口├── pointcloud_ground_detection_component.h├── cyberfile.xml├── README.md└── BUILD
1.4 lidar_detection
1.4.1 组件功能
激光雷达检测组件有四个模型，Centerpoint、Cnnseg、MaskPillars、PointPillars。检测模型完成目标检测功能，获得目标的如下结果：
cx, cy, cz, length, width, height, heading, type
其中，(cx, cy, cz)是中心点，(length, width, height)是长宽高，heading是朝向，type是目标类别。示例如下：
除了获取目标，还根据目标的3d bounding box，得到每个目标的所有点云。
最后结合上述的基于地图roi过滤，地面点分割，拿到每帧点云的secondary_indices。secondary_indices表示roi点和非地面点的交集，此交集去除前景目标点云，剩余点云的索引。
1.4.2 代码结构
本组件目录结构如下所示。
├── lidar_detection├── conf // 组件配置文件├── dag // dag文件├── data // 配置参数├── launch // 启动文件├── detector // 目标检测模型│ ├── center_point_detection // CenterPoint模型│ ├── cnn_segmentation // CNNSeg模型│ ├── mask_pillars_detection // MaskPillars模型│ └── point_pillars_detection // PointPillars模型├── interface // BaseLidarDetector定义，检测模型接口类├── object_builder // 计算目标2d polygon，根据朝向计算包围盒，计算中心点、长宽高├── proto // proto定义├── lidar_detection_component.cc // 组件定义，以及入口├── lidar_detection_component.h├── cyberfile.xml├── README.md└── BUILD
1.5 lidar_detection_filter
1.5.1 组件功能
完成激光雷达目标检测后，对检测目标做过滤。object_filter_bank可以同时使用多个过滤器，针对roi_boundary_filter做介绍，roi_boundary_filter只用来处理前景目标。roi_boundary_filter过滤规则如下图。
1.5.2 代码结构
本组件一级目录结构如下所示。
├── lidar_detection_filter├── conf // 组件配置文件├── dag // dag文件├── data // 配置参数├── object_filter_bank // 目标过滤库│ ├── object_filter_bank.cc // 目标过滤库入口│ ├── object_filter_bank.h│ └── roi_boundary_filter // roi边界过滤├── interface // 过滤接口类├── proto // proto定义├── lidar_detection_filter_component.cc // 组件定义，以及入口├── lidar_detection_filter_component.h├── cyberfile.xml├── README.md└── BUILD
1.6 lidar_tracking
1.6.1 组件功能
多目标跟踪，获取目标运动的历史轨迹，得到更加稳定的朝向、速度、位置等信息，得到跟踪id。多目标跟踪的结果可进一步用于障碍物轨迹预测。
下图是将多帧感知结果拼接起来，对其中一辆跟踪的效果。可以看到这一辆车的行驶轨迹。
多目标跟踪的整体流程如下图。
detections 来自最新检测的结果，tracks 表示历史的匹配结果。Match 是目标匹配算法，最终得到三种匹配结果：
- unassigned tracks：历史的目标没有和最新的检测结果匹配上，这种情况会更新历史 tracks，并删除过老的 tracks
- assignment：表示已经匹配上，根据dets和tracks更新tracks。这一步会更新多种类型的信息，例如：
- 运动状态（速度、朝向、加速度等）
- 几何特征（中心点，polygon，2d包围盒等）
- 类型（type）
- unassignement detections：最新检测的结果没有匹配上，添加到历史tracks中。这时会赋予一个新的track-id
多目标跟踪最主要的两步是匹配和融合。
- 匹配：指的是历史的tracks和detections的匹配，通常要计算目标之间的距离，得到一个距离矩阵。例如tracks数量为m，detections数量为n，得到的距离矩阵为m x n。然后根据距离矩阵计算二分图带权最优匹配，得到匹配结果，具体形式就是上述的unassigned track、assignment、unassignement detections；
- 融合：融合指的是已经匹配上的track和detection做目标融合，更新运动状态、几何特征等。apollo中的对运动状态的融合采用的是卡尔曼滤波算法。
1.6.2 代码结构
本组件一级目录结构如下所示。
├── lidar_tracking├── classifier // fused classfier├── conf // 组件配置文件├── dag // dag文件├── data // 配置参数├── launch // launch启动文件├── interface // 跟踪接口类├── proto // proto定义├── tracker // 多目标跟踪功能代码├── lidar_tracking_component.cc // 组件定义，以及入口├── lidar_tracking_component.h├── cyberfile.xml├── README.md└── BUILD
2. 激光雷达感知开发
本章针对激光雷达感知功能的二次开发做介绍，从以下进行说明：
- 组件开发，开发新的component；
- 插件开发，开发新插件。
2.1 组件开发
组件开发，即开发一个独立的component，用于开发一个独立的功能。
2.1.1 创建文件夹
创建文件夹，以组件的名称命名。
例如：pointcloud_ground_detection，pointcloud_map_based_roi，pointcloud_preprocess。
2.1.2 组件代码
首先，创建组件的必要代码，如下所示。
├── component_name├── component_name.cc // 组件定义，以及入口├── component_name.h└── BUILD
然后，根据组件的输入、输出数据，定义组件类接口。
【注意：接收数据类型如果是自定义的类、结构体，则新建component与上下游组件在一个进程内启动，才能正常收发消息。接收的消息如果是proto定义的数据类型，则上下游组件在不同进程内启动，也能够正常接受消息。】
最后，实现Init方法，用于初始化加载参数。实现Proc函数，实现组件功能。
class ComponentName: public cyber::Component<LidarFrameMessage> {public:ComponentName() = default;virtual ~ComponentName() = default;/*** @brief Init component** @return true* @return false*/bool Init() override;/*** @brief Process of component** @param lidar frame message* @return true* @return false*/bool Proc(const std::shared_ptr<LidarFrameMessage>& message) override;};
2.1.3 配置文件
在proto文件夹中定义component发送消息通道的名称，组件需要完成的功能的配置。代码结构如下：
├── component_name├── conf // 组件配置文件├── proto // proto定义├── component_name.cc├── component_name.h└── BUILD
通常配置定义示例如下。component的配置文件放到conf中。
message LidarDetectionComponentConfig {optional string output_channel_name = 1; // 输出通道名称optional PluginParam plugin_param = 2; // 功能配置，例如地面点分割、预处理等}// PluginParam 定义// message PluginParam {// optional string name = 1; // 功能名称// optional string config_path = 2; // 功能配置的路径// optional string config_file = 3; // 功能配置的文件，包含具体参数// }
2.1.4 功能定义
以地面点分割算法说明模块的功能定义。并在组件中声明该功能的基类，用于调用该功能。
├── component_name├── conf├── proto├── data // 功能配置参数├── ground_detector // 地面点分割功能├── interface // 地面点检测接口类，用于定义多种类。├── component_name.cc├── component_name.h└── BUILD
2.2 插件开发
插件是开发新的功能。插件开发的特点，独立性较强，可以不依赖具体component开发，同时可以被component使用。component可以根据需要加载插件。
插件开发，以lidar_detection_filter为例。其中object_filter_bank中可以执行多种filter，所有的filter都放到std::vector<std::shared_ptr<BaseObjectFilter>>中。
2.2.1 新建文件夹
在如下目录中新建文件夹，过滤器名称为new_filter，用于放代码和插件配置。
├── lidar_detection_filter├── conf├── dag├── data├── object_filter_bank│ ├── object_filter_bank.cc│ ├── object_filter_bank.h│ ├── roi_boundary_filter│ ├── new_filter // 新定义filter├── interface├── proto├── lidar_detection_filter_component.cc├── lidar_detection_filter_component.h├── cyberfile.xml├── README.md└── BUILD
2.2.2 功能定义
过滤器功能定义，首先，继承BaseObjectFilter类，实现Init（初始化方法），Filter（功能代码），Name（获取类名称）。
然后，在new_filter.h最后定义：CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::perception::lidar::NewFilter, BaseObjectFilter)。以此说明此功能是插件形式开发。
class NewFilter : public BaseObjectFilter {public:/*** @brief Construct a new NewFilter object**/NewFilter() = default;virtual ~NewFilter() = default;/*** @brief Init of NewFilter** @param options object filer options* @return true* @return false*/bool Init(const ObjectFilterInitOptions& options =ObjectFilterInitOptions()) override;/*** @brief filter objects using new filter algorithm** @param options object filter options* @param frame lidar frame to filter* @return true* @return false*/bool Filter(const ObjectFilterOptions& options, LidarFrame* frame) override;/*** @brief Name of NewFilter object** @return std::string name*/std::string Name() const override { return "NewFilter"; }private:... ...};CYBER_PLUGIN_MANAGER_REGISTER_PLUGIN(apollo::perception::lidar::NewFilter, BaseObjectFilter)
2.2.3 配置文件
在object_filter_bank/proto中定义new_filter的配置，在data/filter_bank.pb.txt中增加new_filter的配置。
├── lidar_detection_filter├── conf├── dag├── data│ ├── filter_bank.pb.txt // 过滤库，在其中增加new_filter配置├── object_filter_bank│ ├── object_filter_bank.cc│ ├── object_filter_bank.h│ ├── roi_boundary_filter│ ├── proto // new_filter 配置定义│ ├── new_filter├── interface├── proto├── lidar_detection_filter_component.cc├── lidar_detection_filter_component.h├── cyberfile.xml├── README.md└── BUILD
2.2.4 插件管理
首先，BUILD中的编译如下，插件需要用apollo_plugin的方式来编译。
load("//tools:apollo_package.bzl", "apollo_package", "apollo_plugin")load("//tools:cpplint.bzl", "cpplint")package(default_visibility = ["//visibility:public"])apollo_plugin(name = "libnew_filter.so",srcs = ["new_filter.cc"],hdrs = ["new_filter.h"],description = ":plugins.xml",deps = [],)
然后，新建new_filter/plugins.xml，并在其中定义：
<library path="modules/perception/lidar_detection_filter/object_filter_bank/new_filter/libnew_filter.so"><class type="apollo::perception::lidar::NewFilter" base_class="apollo::perception::lidar::BaseObjectFilter"></class></library>
3.激光雷达感知开发实验
本次实验目的是讲解开发组件、插件的具体步骤，上手开发组件、插件的框架，具体功能由开发者自定义。
3.1 准备工作
环境配置：https://apollo.chjunkong.com/community/article/1133
感知功能调试、参数调整：https://apollo.chjunkong.com/community/article/1134
# 进入到application-perception代码库cd application-perception# 拉取并启动docker容器，如果已拉取不需要执行这条命令aem start_gpu# 进入容器aem enter# 下载安装依赖包： 会拉取安装core目录下的cyberfile.xml里面所有的依赖包buildtool build --gpu
组件和插件新建通过命令的形式来实现，执行命令后代码框架就按照第2章的形式被自动创建。
3.2 组件开发
3.2.1 创建组件
进入到容器内后，执行命令创建组件。
#创建组件，--namespace说明命名空间，--template说明创建的是组件，最后是组件的路径buildtool create --namespace perception --template component modules/perception/lidar_test_component
组件创建成功后的代码结构：
3.2.2 组件开发
这次的组件应用于lidar感知，用命令生成的组件有一些默认配置，需要做修改。
代码修改：
- 修改模块代码
- 在cyberfile.xml中增加对perception-common的依赖
组件使用：
首先，修改lidar_test_component组件的输入、输出通道名称。
- 在/apollo_workspace/modules/perception/lidar_test_component/conf/lidar_test_component.conf中修改output_channel_name参数；
- 在/apollo_workspace/modules/perception/lidar_test_component/dag/lidar_test_component.dag中修改reader channel的名称。
然后修改上下游接口的输出、输入通道名称：【上游：lidar_detection_filter，下游：lidar_tracking】
- 在/apollo/modules/perception/lidar_tracking/dag中修改，reader channel为 "/perception/lidar/test"
最后，在/apollo/modules/perception/launch/perception_lidar.launch中增加lidar_test_component.dag。
修改完成后/apollo_workspace下执行：
buildtool build --gpu
3.3 插件开发
3.3.1 创建插件
在容器内，执行命令创建插件。
#创建插件，--namespace说明命名空间，--template说明创建的是插件，最后是插件的路径buildtool create --namespace perception --template plugin modules/perception/lidar_plugin
插件创建成功后的代码结构：
3.3.2 插件开发
此插件应用于lidar_detection_filter，命令生成的插件有默认的配置，需要做修改。
代码修改：
- 定义filter功能函数
- 在cyberfile.xml中增加perception-lidar-detection-filter包依赖
- 在plugin_lidar_plugin_descripton.xml中修改配置
插件使用：
在/apollo/modules/perception/lidar_detection_filter/data/filter_bank.pb.txt中增加lidar_plugin插件。如下图12行所示：
3.4 结果展示
上述都修改好之后，在dv中选中车型，如下所示：
然后，在终端启动transform和lidar感知，然后用cyber_recorder播包。
感知结果如下：
新模块和插件的日志如下。这里只搭建了组件、插件的框架，通过日志表明感知消息传递到了相应的组件和插件。具体组件、插件功能由开发者自己定义。
4. 总结
本节课程的内容总结。
第0章，介绍激光雷达的成像原理，根据激光雷达的扫描方式做了分类。然后介绍激光雷达的优点和不足。最后对自动驾驶中的激光雷达感知内容做了介绍。
第1章，介绍apollo中激光雷达感知各个组件（component）的功能，用了很多形象化的图进行了说明。然后讲解了每个组件的代码结构，方便开发者快速入门。
第2章，介绍在apollo上如何做二次开发，从组件开发、插件开发角度进行说明。
第3章，针对组件、插件做开发，做实例讲解。
