1. 程式人生 > >apollo2.0 radar代碼分析

apollo2.0 radar代碼分析

刪除 acc ttr 跟蹤 寬高 repeated mea 成員 cit

從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
{

  1. 調用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共用的
    這裏面有做了幾個重要的事
    1. radar坐標轉化車身坐標,相對速度轉為絕對速度
    2. 判斷目標是否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;
      }
      1. 計算目標的外包矩形,radar是沒有目標形狀信息的,百度自己把長寬高都設為1,吐槽
        RadarUtil::MockRadarPolygon(point, object_ptr->length, object_ptr->width,
        theta, &(object_ptr->polygon));
  2. 調用void RadarTrackManager::Process(const SensorObjects &radar_obs)
    {
    radarobs = radar_obs;
    Update(&radarobs);
    }
    update函數
    1. AssignTrackObsIdMatch(radar_obs, &assignment, &unassigned_track, &unassigned_obs);
      id相同,並且距離小於2.5的是同一個目標
      百度計算距離沒有像lidar那樣用卡爾曼預測一步,直接當前速度
      時間差來預測,吐槽
      1. UpdateAssignedTrack(*radar_obs, assignment);
        用匹配到的object更新軌跡,百度沒有保存radar軌跡的歷史點,lidar是保留了軌跡歷史點的
    2. UpdateUnassignedTrack((*radar_obs).timestamp, unassigned_track);
      對於沒有匹配到object的軌跡,如果持續0.06沒有匹配到object,就置為null
    3. DeleteLostTrack();
      刪除null的軌跡
    4. CreateNewTrack(*radar_obs, unassigned_obs);
      沒有匹配到軌跡的object建立新的軌跡
      1. 調用bool ModestRadarDetector::CollectRadarResult(std::vector<ObjectPtr> *objects)
        非background的track輸出到objects
        }
        總結一下:
      2. 預留了rcs,沒用到
      3. 計算距離前預測方式太簡單
      4. radar的目標長寬高自定義1.0,
      5. ROI預留了map_polygons這個變量,但是目前empty,未向lidar那樣從hadmap得到ROI區域

apollo2.0 radar代碼分析