从 apollo2.0 的代码上看, 百度是想做 lidar 和 radar 融合的
先看传感器输出数据定义
- class SensorRawFrame
- {
- public:
- SensorType sensortype;
- double timestamp;
- Eigen::Matrix4d pose;
- };
- // lidar 输出数据定义, 用的是 Velodyne 的产品, 包含了一块点云, pcl 库格式
class Velodyne 的产品 RawFrame : public SensorRawFrame
- {
- public:
- VelodyneRawFrame() {}
- ~VelodyneRawFrame() {}
- public:
- pclutil::PointCloudPtr cloud;
- };
- // radar 输出数据定义, 用的是大陆的产品, 包含了一个 ContiRadar 成员
- class RadarRawFrame : public SensorRawFrame
- {
- public:
- RadarRawFrame() {}
- ~RadarRawFrame() {}
- public:
- ContiRadar rawobstacles;
- Eigen::Vector3f car_linearspeed;
- };
ContiRadar 定义在 Conti_Radar.proto 中
- message ContiRadar
- {
- optional apollo.common.Header header = 1;
- repeated ContiRadarObs contiobs = 2; //conti radar obstacle array
- optional RadarState_201 radar_state = 3;
- optional ClusterListStatus_600 cluster_list_status = 4;
- optional ObjectListStatus_60A object_list_status = 5;
- }
ContiRadarObs 是最重要的部分, 代表了 radar 输出的一个目标点, 同样定义在 Conti_Radar.proto 中
- message ContiRadarObs {
- optional apollo.common.Header header = 1;
- optional bool clusterortrack = 2; // 0 = track, 1 = cluster
- optional int32 obstacle_id = 3; // obstacle Id
- required double longitude_dist = 4;
- required double lateral_dist = 5;
- required double longitude_vel = 6;
- required double lateral_vel = 7;
- optional double rcs = 8;
- optional int32 dynprop = 9;
- optional double longitude_dist_rms = 10;
- optional double lateral_dist_rms = 11;
- optional double longitude_vel_rms = 12;
- optional double lateral_vel_rms = 13;
- optional double probexist = 14;
- //The following is only valid for the track object message
- optional int32 meas_state = 15;
- optional double longitude_accel = 16;
- optional double lateral_accel = 17;
- optional double oritation_angle = 18;
- optional double longitude_accel_rms = 19;
- optional double lateral_accel_rms = 20;
- optional double oritation_angle_rms = 21;
- optional double length = 22;
- optional double width = 23;
- optional int32 obstacle_class = 24;
- }
值得注意的是百度增加了 rcs, 虽然 2.0 的代码未用到 rcs
入口函数, 整个函数做了两件事,
1. 根据 sensor_type_分别处理 lidar 和 radar
2.lidar 和 radar 融合
本次只分析 radar,lidar 和融合下次再写
- bool ObstaclePerception::Process(SensorRawFrame frame, std::vector<ObjectPtr> out_objects)
- {
- std::shared_ptr<SensorObjects> sensor_objects(new SensorObjects());
- if (frame->sensortype == VELODYNE_64)
- {
- }
- else if (frame->sensortype == RADAR)
- {
- // radar 处理
- RadarRawFrame radar_frame = dynamic_cast<RadarRawFrame>(frame);
- RadarDetectorOptions options;
- options.radar2world_pose = &(radarframe->pose);
- options.car_linear_speed = radar_frame->car_linearspeed;
- std::vector<ObjectPtr> objects;
- std::vector<PolygonDType> map_polygons;
- if (!radardetector->Detect(radar_frame->rawobstacles, map_polygons,
- options, &objects)) {
- AERROR << "Radar perception error!," << std::fixed
- << std::setprecision(12) << radarframe->timestamp;
- return false;
- }
- sensor_objects->objects = objects;
- AINFO << "radar objects size:" << objects.size();
- PERF_BLOCK_END("radar_detection");
- // set frame content
- if (FLAGS_enable_visualization && FLAGS_show_radar_obstacles) {
- framecontent.SetTrackedObjects(sensor_objects->objects);
- }
- }
- }
radar 处理两个不步骤
1. 调用 bool ModestRadarDetector::Detect 函数, 输出检测跟踪之后的 objects
2.objects 保存到 framecontent, 用于融合
- bool ModestRadarDetector::Detect
- {
调用 void ObjectBuilder::Build(const ContiRadar &raw_obstacles,
- const Eigen: :Matrix4d & radar_pose,
- const Eigen: :Vector2d & main_velocity,
- SensorObjects * radar_objects)
这个 build 函数根据 raw_obstacles 原始数据, 构建 Object 对象, Object 的定义是 lidar 和 radar 共用的
这里面有做了几个重要的事
radar 坐标转化车身坐标, 相对速度转为绝对速度
判断目标是否 is_background, 这个判断的条件比较有意思
情况 1: 比较出现的次数, 目标出现的次数小于 delay_frames_的 is_background
- if (current_con_ids[obstacle_id] <= delayframes)
- {
- object_ptr->is_background = true;
- }
情况 2:
- ContiRadarUtil::IsFp(raw_obstacles.contiobs(i), contiparams,
- delayframes, tracking_times))
情况 3: 比较目标速度和车身速度的夹角,(1/4Pi,3/4Pi) (-1/4Pi,-3/4Pi) 范围的不是 background
- if (ContiRadarUtil::IsConflict(ref_velocity, object_ptr->velocity.cast<float>()))
- {
- object_ptr->is_background = true;
- }
计算目标的外包矩形, radar 是没有目标形状信息的, 百度自己把长宽高都设为 1, 吐槽
- RadarUtil::MockRadarPolygon(point, object_ptr->length, object_ptr->width,
- theta, &(object_ptr->polygon));
调用 void RadarTrackManager::Process(const SensorObjects &radar_obs)
- {
- radarobs = radar_obs;
- Update( & radarobs);
- }
update 函数
AssignTrackObsIdMatch(radar_obs, &assignment, &unassigned_track, &unassigned_obs);
id 相同, 并且距离小于 2.5 的是同一个目标
百度计算距离没有像 lidar 那样用卡尔曼预测一步, 直接当前速度 时间差来预测, 吐槽
UpdateAssignedTrack(*radar_obs, assignment);
用匹配到的 object 更新轨迹, 百度没有保存 radar 轨迹的历史点, lidar 是保留了轨迹历史点的
UpdateUnassignedTrack((*radar_obs).timestamp, unassigned_track);
对于没有匹配到 object 的轨迹, 如果持续 0.06 没有匹配到 object, 就置为 null
DeleteLostTrack();
删除 null 的轨迹
CreateNewTrack(*radar_obs, unassigned_obs);
没有匹配到轨迹的 object 建立新的轨迹
调用 bool ModestRadarDetector::CollectRadarResult(std::vector<ObjectPtr> *objects)
非 background 的 track 输出到 objects
}
总结一下:
预留了 rcs, 没用到
计算距离前预测方式太简单
radar 的目标长宽高自定义 1.0,
ROI 预留了 map_polygons 这个变量, 但是目前 empty, 未向 lidar 那样从 hadmap 得到 ROI 区域
来源: http://www.bubuko.com/infodetail-2494802.html