首页 > 编程语言> > Apollo perception源码阅读 | radar

Apollo perception源码阅读 | radar

2021-07-19 12:32:00 作者：互联网

#! https://zhuanlan.zhihu.com/p/391021814

Apollo perception源码阅读 | radar

本文为Apollo感知融合源码阅读笔记，建议参照Apollo6.0源码阅读本文，水平有限，有错误的地方希望大佬多加指正!
个人代码注释链接： leox24 / perception_learn

Radar模块

主要是radar检测的框架代码，其实并没有什么核心内容…
具体也就是融合的那一套东西，预处理，关联匹配，跟踪，虽然radar有写滤波器adaptive kalman，但是实际上并没有用。

1 目录结构

.
├── app
│   ├── BUILD
│   ├── radar_obstacle_perception.cc //框架实现
│   └── radar_obstacle_perception.h
├── common
│   ├── BUILD
│   ├── radar_util.cc
│   ├── radar_util.h
│   └── types.h
└── lib
    ├── detector
    ├── dummy
    ├── interface
    ├── preprocessor
    ├── roi_filter
    └── tracker

2 入口

在Apollo/modules/perception/production/launch并没有单独的启动radar的launch文件，也没有单一的dag文件，雷达组件的定义都在融合的dag文件中Apollo/modules/perception/production/dag/dag_streaming_perception.dag。

  components {
    class_name: "RadarDetectionComponent"
    config {
      name: "FrontRadarDetection"
      config_file_path: "/apollo/modules/perception/production/conf/perception/radar/front_radar_component_conf.pb.txt"
      readers {
          channel: "/apollo/sensor/radar/front"
        }
    }
  }

  components {
    class_name: "RadarDetectionComponent"
    config {
      name: "RearRadarDetection"
      config_file_path: "/apollo/modules/perception/production/conf/perception/radar/rear_radar_component_conf.pb.txt"
      readers {
          channel: "/apollo/sensor/radar/rear"
        }
    }
  }

可见radar的组件类是RadarDetectionComponent，分别对前雷达和后雷达进行处理。

2.1 RadarDetectionComponent

Apollo/modules/perception/onboard/component/radar_detection_component.cc

RadarDetectionComponent::Init()
初始化参数，参数列表如下：

  radar_name: "radar_front"
  tf_child_frame_id: "radar_front"
  radar_forward_distance: 200.0
  radar_preprocessor_method: "ContiArsPreprocessor"
  radar_perception_method: "RadarObstaclePerception"
  radar_pipeline_name: "FrontRadarPipeline"
  odometry_channel_name: "/apollo/localization/pose"
  output_channel_name: "/perception/inner/PrefusedObjects"

  radar_name: "radar_rear"
  tf_child_frame_id: "radar_rear"
  radar_forward_distance: 120.0
  radar_preprocessor_method: "ContiArsPreprocessor"
  radar_perception_method: "RadarObstaclePerception"
  radar_pipeline_name: "RearRadarPipeline"
  odometry_channel_name: "/apollo/localization/pose"
  output_channel_name: "/perception/inner/PrefusedObjects"

  FLAGS_obs_enable_hdmap_input=true

RadarDetectionComponent::InternalProc()
- 输入：ContiRadar原始radar信息
- 输出：SensorFrameMessageradar处理结果
  主要是两步，Preprocess，Perceive，下文展开介绍预处理和核心处理函数。

bool RadarDetectionComponent::InternalProc(
    const std::shared_ptr<ContiRadar>& in_message,
    std::shared_ptr<SensorFrameMessage> out_message) {

  // 初始化预处理参数
  // Init preprocessor_options
  radar::PreprocessorOptions preprocessor_options;
  // 预处理结果
  ContiRadar corrected_obstacles;
  // 预处理
  radar_preprocessor_->Preprocess(raw_obstacles, preprocessor_options,
                                  &corrected_obstacles);
  PERF_BLOCK_END_WITH_INDICATOR(radar_info_.name, "radar_preprocessor");
  timestamp = corrected_obstacles.header().timestamp_sec();

  out_message->timestamp_ = timestamp;
  out_message->seq_num_ = seq_num_;
  out_message->process_stage_ = ProcessStage::LONG_RANGE_RADAR_DETECTION;
  out_message->sensor_id_ = radar_info_.name;

  // 初始化参数：radar2world转换矩阵，radar2自车转换矩阵、自车线速度和角速度
  // Init radar perception options
  radar::RadarPerceptionOptions options;
  options.sensor_name = radar_info_.name;
  // Init detector_options
  Eigen::Affine3d radar_trans;
  // radar2world的转换矩阵
  if (!radar2world_trans_.GetSensor2worldTrans(timestamp, &radar_trans)) {
    out_message->error_code_ = apollo::common::ErrorCode::PERCEPTION_ERROR_TF;
    AERROR << "Failed to get pose at time: " << timestamp;
    return true;
  }
  Eigen::Affine3d radar2novatel_trans;
  // radar2自车的转换矩阵
  if (!radar2novatel_trans_.GetTrans(timestamp, &radar2novatel_trans, "novatel",
                                     tf_child_frame_id_)) {
    out_message->error_code_ = apollo::common::ErrorCode::PERCEPTION_ERROR_TF;
    AERROR << "Failed to get radar2novatel trans at time: " << timestamp;
    return true;
  }
  PERF_BLOCK_END_WITH_INDICATOR(radar_info_.name, "GetSensor2worldTrans");
  Eigen::Matrix4d radar2world_pose = radar_trans.matrix();
  options.detector_options.radar2world_pose = &radar2world_pose;
  Eigen::Matrix4d radar2novatel_trans_m = radar2novatel_trans.matrix();
  options.detector_options.radar2novatel_trans = &radar2novatel_trans_m;
  // 自车车速
  if (!GetCarLocalizationSpeed(timestamp,
                               &(options.detector_options.car_linear_speed),
                               &(options.detector_options.car_angular_speed))) {
    AERROR << "Failed to call get_car_speed. [timestamp: " << timestamp;
    // return false;
  }
  PERF_BLOCK_END_WITH_INDICATOR(radar_info_.name, "GetCarSpeed");
  // Init roi_filter_options
  // radar到world的T矩阵偏移量
  base::PointD position;
  position.x = radar_trans(0, 3);
  position.y = radar_trans(1, 3);
  position.z = radar_trans(2, 3);
  options.roi_filter_options.roi.reset(new base::HdmapStruct());
  if (FLAGS_obs_enable_hdmap_input) {
    hdmap_input_->GetRoiHDMapStruct(position, radar_forward_distance_,
                                    options.roi_filter_options.roi);
  }
  PERF_BLOCK_END_WITH_INDICATOR(radar_info_.name, "GetRoiHDMapStruct");
  // Init object_filter_options
  // Init track_options
  // Init object_builder_options
  // 处理预处理后的radar obj
  std::vector<base::ObjectPtr> radar_objects;
  if (!radar_perception_->Perceive(corrected_obstacles, options,
                                   &radar_objects)) {
    out_message->error_code_ =
        apollo::common::ErrorCode::PERCEPTION_ERROR_PROCESS;
    AERROR << "RadarDetector Proc failed.";
    return true;
  }
  out_message->frame_.reset(new base::Frame());
  out_message->frame_->sensor_info = radar_info_;
  out_message->frame_->timestamp = timestamp;
  out_message->frame_->sensor2world_pose = radar_trans;
  out_message->frame_->objects = radar_objects;

  return true;
}

3 ContiArsPreprocessor

Apollo/modules/perception/radar/lib/preprocessor/conti_ars_preprocessor/conti_ars_preprocessor.cc

ContiArsPreprocessor::Preprocess()
雷达预处理的实现，包括跳过不在时间范围内的目标，重新分配ID，修正时间戳(减去0.07)三个步骤。个人觉得这个预处理是跟传感器数据的特征有关的，不同传感器有不同的预处理方法(经验)，对我来说并没有什么参考价值。感兴趣的可以看我的注释代码，我就不展开阐述了。

// 雷达预处理
bool ContiArsPreprocessor::Preprocess(
    const drivers::ContiRadar& raw_obstacles,
    const PreprocessorOptions& options,
    drivers::ContiRadar* corrected_obstacles) {
  PERF_FUNCTION();
  SkipObjects(raw_obstacles, corrected_obstacles);
  ExpandIds(corrected_obstacles);
  CorrectTime(corrected_obstacles);
  return true;
}

4 RadarObstaclePerception

Apollo/modules/perception/radar/app/radar_obstacle_perception.cc

RadarObstaclePerception::Perceive()
核心处理函数，包括检测，ROI过滤，跟踪三个步骤。

bool RadarObstaclePerception::Perceive(
    const drivers::ContiRadar& corrected_obstacles,
    const RadarPerceptionOptions& options,
    std::vector<base::ObjectPtr>* objects) {
  base::FramePtr detect_frame_ptr(new base::Frame());
  // 检测 ContiArsDetector
  if (!detector_->Detect(corrected_obstacles, options.detector_options,
                         detect_frame_ptr)) {
    AERROR << "radar detect error";
    return false;
  }
  // ROI过滤 HdmapRadarRoiFilter
  if (!roi_filter_->RoiFilter(options.roi_filter_options, detect_frame_ptr)) {
    ADEBUG << "All radar objects were filtered out";
  }
  base::FramePtr tracker_frame_ptr(new base::Frame);

  // 跟踪 ContiArsTracker
  if (!tracker_->Track(*detect_frame_ptr, options.track_options,
                       tracker_frame_ptr)) {
    AERROR << "radar track error";
    return false;
  }
  *objects = tracker_frame_ptr->objects;

  return true;
}

4.1 ContiArsDetector::Detect

Apollo/modules/perception/radar/lib/detector/conti_ars_detector/conti_ars_detector.cc
说是检测，其实是对传感器数据的处理，radar2world和radar2自车的坐标系转换，相对速度和绝对速度的转换，运动状态、类别等属性的赋值，这部分对做radar的是可以有一定参考的。
其中需要注意的是转换速度时，需要补偿自车转弯旋转导致的速度变化，就是在radar速度的xy分量上补偿掉自车角速度带来的速度。

void ContiArsDetector::RawObs2Frame(
    const drivers::ContiRadar& corrected_obstacles,
    const DetectorOptions& options, base::FramePtr radar_frame) {
  // radar2world转换矩阵
  const Eigen::Matrix4d& radar2world = *(options.radar2world_pose);
  // radar到自车转换矩阵
  const Eigen::Matrix4d& radar2novatel = *(options.radar2novatel_trans);
  // 自车角速度，应该是xyz三个方向上的角速度，应该只有转弯时的yawrate
  const Eigen::Vector3f& angular_speed = options.car_angular_speed;

  Eigen::Matrix3d rotation_novatel;
  rotation_novatel << 0, -angular_speed(2), angular_speed(1), angular_speed(2),
      0, -angular_speed(0), -angular_speed(1), angular_speed(0), 0;
  // 补偿自车转弯旋转时的速度变化。
  Eigen::Matrix3d rotation_radar = radar2novatel.topLeftCorner(3, 3).inverse() *
                                   rotation_novatel *
                                   radar2novatel.topLeftCorner(3, 3);
  Eigen::Matrix3d radar2world_rotate = radar2world.block<3, 3>(0, 0);
  Eigen::Matrix3d radar2world_rotate_t = radar2world_rotate.transpose();
  // Eigen::Vector3d radar2world_translation = radar2world.block<3, 1>(0, 3);
  ADEBUG << "radar2novatel: " << radar2novatel;
  ADEBUG << "angular_speed: " << angular_speed;
  ADEBUG << "rotation_radar: " << rotation_radar;
  for (const auto radar_obs : corrected_obstacles.contiobs()) {
    base::ObjectPtr radar_object(new base::Object);
    radar_object->id = radar_obs.obstacle_id();
    radar_object->track_id = radar_obs.obstacle_id();
    // 局部位姿
    Eigen::Vector4d local_loc(radar_obs.longitude_dist(),
                              radar_obs.lateral_dist(), 0, 1);
    // 世界位姿
    Eigen::Vector4d world_loc =
        static_cast<Eigen::Matrix<double, 4, 1, 0, 4, 1>>(radar2world *
                                                          local_loc);
    // 世界坐标系下的xy，z=0
    radar_object->center = world_loc.block<3, 1>(0, 0);
    radar_object->anchor_point = radar_object->center;

    // 相对速度
    Eigen::Vector3d local_vel(radar_obs.longitude_vel(),
                              radar_obs.lateral_vel(), 0);
    // 补偿自车转弯旋转时的速度变化。雷达的相对速度的xy分量，加减自车转弯的xy速度分量
    Eigen::Vector3d angular_trans_speed =
        rotation_radar * local_loc.topLeftCorner(3, 1);
    Eigen::Vector3d world_vel =
        static_cast<Eigen::Matrix<double, 3, 1, 0, 3, 1>>(
            radar2world_rotate * (local_vel + angular_trans_speed));
    // 绝对速度
    Eigen::Vector3d vel_temp =
        world_vel + options.car_linear_speed.cast<double>();
    radar_object->velocity = vel_temp.cast<float>();

    Eigen::Matrix3d dist_rms;
    dist_rms.setZero();
    Eigen::Matrix3d vel_rms;
    vel_rms.setZero();
    // rms是均方根，但这里可能是指方差相关？
    dist_rms(0, 0) = radar_obs.longitude_dist_rms();
    dist_rms(1, 1) = radar_obs.lateral_dist_rms();
    vel_rms(0, 0) = radar_obs.longitude_vel_rms();
    vel_rms(1, 1) = radar_obs.lateral_vel_rms();
    // 计算位置不确定性
    radar_object->center_uncertainty =
        (radar2world_rotate * dist_rms * dist_rms.transpose() *
         radar2world_rotate_t)
            .cast<float>();
    // 计算速度不确定性
    radar_object->velocity_uncertainty =
        (radar2world_rotate * vel_rms * vel_rms.transpose() *
         radar2world_rotate_t)
            .cast<float>();
    double local_obj_theta = radar_obs.oritation_angle() / 180.0 * PI;
    Eigen::Vector3f direction(static_cast<float>(cos(local_obj_theta)),
                              static_cast<float>(sin(local_obj_theta)), 0.0f);
    // 方向和角度
    direction = radar2world_rotate.cast<float>() * direction;
    radar_object->direction = direction;
    radar_object->theta = std::atan2(direction(1), direction(0));
    radar_object->theta_variance =
        static_cast<float>(radar_obs.oritation_angle_rms() / 180.0 * PI);
    radar_object->confidence = static_cast<float>(radar_obs.probexist());
    
    // 运动状态：运动、静止、未知
    // 目标置信度
    int motion_state = radar_obs.dynprop();
    double prob_target = radar_obs.probexist();
    if ((prob_target > MIN_PROBEXIST) &&
        (motion_state == CONTI_MOVING || motion_state == CONTI_ONCOMING ||
         motion_state == CONTI_CROSSING_MOVING)) {
      radar_object->motion_state = base::MotionState::MOVING;
    } else if (motion_state == CONTI_DYNAMIC_UNKNOWN) {
      radar_object->motion_state = base::MotionState::UNKNOWN;
    } else {
      radar_object->motion_state = base::MotionState::STATIONARY;
      radar_object->velocity.setZero();
    }
    // 类别
    int cls = radar_obs.obstacle_class();
    if (cls == CONTI_CAR || cls == CONTI_TRUCK) {
      radar_object->type = base::ObjectType::VEHICLE;
    } else if (cls == CONTI_PEDESTRIAN) {
      radar_object->type = base::ObjectType::PEDESTRIAN;
    } else if (cls == CONTI_MOTOCYCLE || cls == CONTI_BICYCLE) {
      radar_object->type = base::ObjectType::BICYCLE;
    } else {
      radar_object->type = base::ObjectType::UNKNOWN;
    }
    // 长宽高
    radar_object->size(0) = static_cast<float>(radar_obs.length());
    radar_object->size(1) = static_cast<float>(radar_obs.width());
    radar_object->size(2) = 2.0f;  // vehicle template (pnc required)
    if (cls == CONTI_POINT) {
      radar_object->size(0) = 1.0f;
      radar_object->size(1) = 1.0f;
    }
    // extreme case protection
    if (radar_object->size(0) * radar_object->size(1) < 1.0e-4) {
      if (cls == CONTI_CAR || cls == CONTI_TRUCK) {
        radar_object->size(0) = 4.0f;
        radar_object->size(1) = 1.6f;  // vehicle template
      } else {
        radar_object->size(0) = 1.0f;
        radar_object->size(1) = 1.0f;
      }
    }
    MockRadarPolygon(radar_object);

    float local_range = static_cast<float>(local_loc.head(2).norm());
    float local_angle =
        static_cast<float>(std::atan2(local_loc(1), local_loc(0)));
    radar_object->radar_supplement.range = local_range;
    radar_object->radar_supplement.angle = local_angle;

    radar_frame->objects.push_back(radar_object);

  }
}

4.2 HdmapRadarRoiFilter::RoiFilter

Apollo/modules/perception/radar/lib/roi_filter/hdmap_radar_roi_filter/hdmap_radar_roi_filter.cc
和lidar的处理一样，过滤掉ROI区域的目标。

bool HdmapRadarRoiFilter::RoiFilter(const RoiFilterOptions& options,
                                    base::FramePtr radar_frame) {
  // 滤除Roi范围外的障碍物
  std::vector<base::ObjectPtr> origin_objects = radar_frame->objects;
  return common::ObjectInRoiCheck(options.roi, origin_objects,
                                  &radar_frame->objects);
}

4.3 ContiArsTracker::Track

Apollo/modules/perception/radar/lib/tracker/conti_ars_tracker/conti_ars_tracker.cc
跟踪也就是经典的关联，匹配，更新。
关联值只使用了距离计算，匹配使用了ID匹配和匈牙利匹配，更新则是直接更新，没有使用滤波

ContiArsTracker::TrackObjects()

void ContiArsTracker::TrackObjects(const base::Frame &radar_frame) {
  std::vector<TrackObjectPair> assignments;
  std::vector<size_t> unassigned_tracks;
  std::vector<size_t> unassigned_objects;
  TrackObjectMatcherOptions matcher_options;
  const auto &radar_tracks = track_manager_->GetTracks();
  // 关联匹配
  matcher_->Match(radar_tracks, radar_frame, matcher_options, &assignments,
                  &unassigned_tracks, &unassigned_objects);
  // 更新四步骤：更新匹配的，更新未匹配的，删除丢失的，创建新的
  UpdateAssignedTracks(radar_frame, assignments);
  // 超过0.06秒设置为死亡
  UpdateUnassignedTracks(radar_frame, unassigned_tracks);
  DeleteLostTracks();
  CreateNewTracks(radar_frame, unassigned_objects);
}

HMMatcher::Match()
这里要注意的是HMMatcher是BaseMatcher的派生类，IDMatch()是基类的实现，而在IDMatch()中有个RefinedTrack()是派生类重写了的，会调用派生类的RefinedTrack()。

bool HMMatcher::Match(const std::vector<RadarTrackPtr> &radar_tracks,
                      const base::Frame &radar_frame,
                      const TrackObjectMatcherOptions &options,
                      std::vector<TrackObjectPair> *assignments,
                      std::vector<size_t> *unassigned_tracks,
                      std::vector<size_t> *unassigned_objects) {
  // ID匹配，相同id且距离小于2.5直接匹配上
  IDMatch(radar_tracks, radar_frame, assignments, unassigned_tracks,
          unassigned_objects);
  // 计算关联值，匈牙利匹配
  TrackObjectPropertyMatch(radar_tracks, radar_frame, assignments,
                           unassigned_tracks, unassigned_objects);
  return true;
}

HMMatcher::TrackObjectPropertyMatch()
这里几乎和fusion的匹配一样，都是计算关联值之后使用匈牙利进行匹配，唯一的区别是关联值的计算方式。

void HMMatcher::TrackObjectPropertyMatch(
    const std::vector<RadarTrackPtr> &radar_tracks,
    const base::Frame &radar_frame, std::vector<TrackObjectPair> *assignments,
    std::vector<size_t> *unassigned_tracks,
    std::vector<size_t> *unassigned_objects) {
  if (unassigned_tracks->empty() || unassigned_objects->empty()) {
    return;
  }
  std::vector<std::vector<double>> association_mat(unassigned_tracks->size());
  for (size_t i = 0; i < association_mat.size(); ++i) {
    association_mat[i].resize(unassigned_objects->size(), 0);
  }
  // 计算关联矩阵
  ComputeAssociationMat(radar_tracks, radar_frame, *unassigned_tracks,
                        *unassigned_objects, &association_mat);

  // from perception-common
  common::SecureMat<double> *global_costs =
      hungarian_matcher_.mutable_global_costs();
  global_costs->Resize(unassigned_tracks->size(), unassigned_objects->size());
  for (size_t i = 0; i < unassigned_tracks->size(); ++i) {
    for (size_t j = 0; j < unassigned_objects->size(); ++j) {
      (*global_costs)(i, j) = association_mat[i][j];
    }
  }
  std::vector<TrackObjectPair> property_assignments;
  std::vector<size_t> property_unassigned_tracks;
  std::vector<size_t> property_unassigned_objects;
  // 匈牙利匹配，和fusion一样
  hungarian_matcher_.Match(
      BaseMatcher::GetMaxMatchDistance(), BaseMatcher::GetBoundMatchDistance(),
      common::GatedHungarianMatcher<double>::OptimizeFlag::OPTMIN,
      &property_assignments, &property_unassigned_tracks,
      &property_unassigned_objects);
...(省略)
}

radar这里的关联值的距离计算了两次，然后取了平均。分别用了上一帧和当前帧的速度做预测。

// 计算航迹往前预测和观测的中心点距离
double distance_forward = DistanceBetweenObs(
    track_object, track_timestamp, frame_object, frame_timestamp);
// 计算观测往后预测和航迹的中心点距离
double distance_backward = DistanceBetweenObs(
    frame_object, frame_timestamp, track_object, track_timestamp);
association_mat->at(i).at(j) =
    0.5 * distance_forward + 0.5 * distance_backward;

ContiArsTracker::UpdateAssignedTracks()
这里的更新track，虽然写了adaptive kalman，但是并没有使用滤波器，而是直接用观测量做了更新，猜测可能是传感器供应商已经做好了跟踪。简单看了下adaptive kalman，好像也就是CV模型的KF。
剩下的更新未匹配的函数，删除丢失track，创建新track也没有什么需要阐述的，就是简单的处理。

5 小结

radar模块的处理并没有很多，所以也没有很详细的写，有兴趣可以自己看看，大多是比较简单的实现。
欢迎指正～

标签：perception,frame,object,unassigned,radar,源码,Apollo,options
来源： https://blog.csdn.net/weixin_43152152/article/details/118892974