主页传送门 : 📀 传送
??lidar,全称为Light Detection and Ranging
激光探测和测距,也被称为光学雷达。它是一种
融合了激光、全球定位系统(GPS)和惯性导航系统(INS)三种技术的系统,主要用于获取点云数据并生
成精确的数字化三维模型。这种技术结合了这三种技术后,可以在全球范围内一致且绝对地测量点
位,从而获取周围的三维实景。
??LiDAR利用发射和接收激光脉冲信号的时间差来实现对被测目标的距离测量,其测距公
式为:R=c*t/2,其中R是测量距离,c是光速,t是激光信号往返的时间差。时间获取的方法主要有两
种,即脉冲法和相位法。脉冲法(Time of Flight, TOF)就是通过利用被测目标对激光脉冲的漫反射
作用,通过接收和发射端的计时来获取时间差。尽管这种方法在测距很短的情况下可能受到脉冲宽度
和计数器时间分辨率的影响,导致测距精度不高,但其工作方式简单且效率高,适合精度要求不高的
场景。
??在自动驾驶系统的Perception模块中,Lidar数据流主要指的是激光雷达设备所获取并处理的数据信息。具体来说,
Lidar数据流包括激光雷达检测和识别跟踪两个主要步骤。
??首先,激光雷达通过发射激光束并接收反射回来的激光信号,以此测量目标物体与雷达之间的距离。这些测距信息
会转化为点云数据,反映出目标物体在空间中的位置分布。这一步骤通常称之为激光雷达的检测过程。
??然后,这些点云数据会被送入后续的处理模块进行识别和追踪。例如,Apollo感知代码中的激光雷达模块的追踪使用
的是SORT算法架构,主要包括关联模块、通用模块、测量值模块、多激光雷达融合模块等。
??最后,经过上述处理后,Lidar数据流会作为重要的感知信息输入给后续的决策和控制环节。值得一提的是,除了单
独的Lidar数据处理外,多传感器融合也是自动驾驶系统中常见的处理方式。这意味着,Lidar数据会与来自雷达和摄像头等其他传
感器的数据一起被处理,以提供更全面准确的环境感知信息。
Perception
中的lidar数据流如下:
本篇所介绍的lidar检测算法位于图中的Detection Component
中。
当前Detection Component
的架构如下:
??从以上结构中可以清楚地看到lidar检测算法是位于Detection Component
的
base_lidar_obstacle_detection
中的抽象成员类 base_lidar_detector
的派生类。
下面将详细介绍如何基于当前结构添加新的lidar检测算法。
??Apollo默认提供了2种lidar检测算法–PointPillars
和CNN
(NCut不再维护),可以轻松更
改或替换为不同的算法。每种算法的输入都是原始点云信息,输出都是目标级障碍物信息。
??本篇将介绍如何引入新的lidar检测算法,添加新算法的步骤如下:
为了更好的理解,下面对每个步骤进行详细的阐述:
??所有的lidar检测算法都必须继承基类base_lidar_detector,它定义了一组接口。
以下是检测算法继承基类的示例:
namespace apollo {
namespace perception {
namespace lidar {
class NewLidarDetector : public BaseLidarDetector {
public:
NewLidarDetector();
virtual ~NewLidarDetector() = default;
bool Init(const LidarDetectorInitOptions& options = LidarDetectorInitOptions()) override;
bool Detect(const LidarDetectorOptions& options, LidarFrame* frame) override;
std::string Name() const override;
}; // class NewLidarDetector
} // namespace lidar
} // namespace perception
} // namespace apollo
基类 base_lidar_detector 已定义好各虚函数签名,接口信息如下:
struct LidarDetectorInitOptions {
std::string sensor_name = "velodyne64";
};
struct LidarDetectorOptions {};
struct LidarFrame {
// point cloud
std::shared_ptr<base::AttributePointCloud<base::PointF>> cloud;
// world point cloud
std::shared_ptr<base::AttributePointCloud<base::PointD>> world_cloud;
// timestamp
double timestamp = 0.0;
// lidar to world pose
Eigen::Affine3d lidar2world_pose = Eigen::Affine3d::Identity();
// lidar to world pose
Eigen::Affine3d novatel2world_pose = Eigen::Affine3d::Identity();
// hdmap struct
std::shared_ptr<base::HdmapStruct> hdmap_struct = nullptr;
// segmented objects
std::vector<std::shared_ptr<base::Object>> segmented_objects;
// tracked objects
std::vector<std::shared_ptr<base::Object>> tracked_objects;
// point cloud roi indices
base::PointIndices roi_indices;
// point cloud non ground indices
base::PointIndices non_ground_indices;
// secondary segmentor indices
base::PointIndices secondary_indices;
// sensor info
base::SensorInfo sensor_info;
// reserve string
std::string reserve;
void Reset();
void FilterPointCloud(base::PointCloud<base::PointF> *filtered_cloud,
const std::vector<uint32_t> &indices);
};
为了确保新的检测算法能顺利工作,NewLidarDetector
至少需要重写base_lidar_detector
中定义的接口Init(),Detect()和Name()。其中Init()函数负责完成加载配置文件,初始化类成员等工作;而Detect()则负责实现算法的主体流程。一个具体的NewLidarDetector.cc
实现示例如下:
namespace apollo {
namespace perception {
namespace lidar {
bool NewLidarDetector::Init(const LidarDetectorInitOptions& options) {
/*
你的算法初始化部分
*/
}
bool NewLidarDetector::Detect(const LidarDetectorOptions& options, LidarFrame* frame) {
/*
你的算法实现部分
*/
}
std::string NewLidarDetector::Name() const {
/*
返回你的检测算法名称
*/
}
PERCEPTION_REGISTER_LIDARDETECTOR(NewLidarDetector); //注册新的lidar_detector
} // namespace lidar
} // namespace perception
} // namespace apollo
按照下面的步骤添加新lidar检测算法的配置和参数信息:
cnn_segmentation的proto
定义:modules/perception/lidar/lib/detector/cnn_segmentation/proto/cnnseg_config.proto
syntax = "proto2";
package apollo.perception.lidar;
message NewLidarDetectorConfig {
double parameter1 = 1;
int32 parameter2 = 2;
}
proto
文件。作为示例,可以参考以下位置的cnn_segmentation的proto
定义:modules/perception/lidar/lib/detector/cnn_segmentation/proto/cnnseg_param.proto
。同样地,在定义完成后输入以下内容:syntax = "proto2";
package apollo.perception.lidar;
//你的param参数
modules/perception/production/conf/perception/lidar/config_manager.config
文件:model_config_path: "./conf/perception/lidar/modules/newlidardetector_config.config"
modules/cnnseg.config
内容创建 newlidardetector.config
:model_configs {
name: "NewLidarDetector"
version: "1.0.0"
string_params {
name: "root_path"
value: "./data/perception/lidar/models/newlidardetector"
}
}
cnnseg
在目录 modules/perception/production/data/perception/lidar/models/
中创建 newlidardetector
文件夹,并根据需求创建不同传感器的.conf
文件:注意:此处 “*.conf” 和 “*param.conf” 文件应对应步骤1,2,3中的proto文件格式.
??要使用Apollo系统中的新lidar检测算法,需要将 modules/perception/production/data/perception/lidar/models/lidar_obstacle_pipline
中的对应传感器的 lidar_obstacle_detection.conf
文件的detector
字段值改为 “NewLidarDetector”
??在完成以上步骤后,您的新lidar检测算法便可在Apollo系统中生效。