「Apollo」百度Apollo感知模块(perception)红绿灯检测代码完整+详细解析

Liiipseoroinis

已于 2022-11-14 17:57:46 修改

阅读量7k

点赞数 8

分类专栏： Apollo 文章标签： c++ 百度自动驾驶

于 2021-06-11 17:55:21 首次发布

本文链接：https://blog.csdn.net/Liiipseoroinis/article/details/117823293

版权

1 背景

最近在读apollo感知模块下的红绿灯检测，apollo框架思路清晰，风格规范，值得多读。直接上代码文件：trafficlights_perception_component.cc

trafficlights_perception_component.cc

整个trafficlights_perception_component.cc文件的函数主要分为三部分：以init开头的初始化部分，红绿灯模块的主函数OnReceiveImage，以及被主函数直接/间接调用的其他函数。
突然想写个记录，主函数已经扒得七七八八了，现在主要在抠细节，所以直接上细节函数的分析了，随写随更，部分简单的判断函数就不写了，懂的都懂。
ps: 下面的函数顺序是按照函数的执行顺序写的，属于该文件的函数写在标题，不属于该文件的被调用的其他文件的函数挂在标题函数下面
pps: 目前自己c++还不太到位，有些描述不准确的自己看代码

2 概述

2.1初始化（Init & Init—）部分

初始化部分就不每块代码都拿出来说了，只说一下大致内容，重点会提出来。

2.1.1 GetGpuId()

读取红绿灯的config和proto文件，进行初始化；获取使用的gpu id，应对装有多个gpu的电脑，我的只装了一个，不多说。

2.1.2 Init()

综合了各个模块初始化函数（下面init开头的函数）的总初始化函数，最后还判断了红绿灯检测的可视化模块是否打开，代码就不贴上来了，报错的英文里有详细说明。

2.1.3 InitConfig()

读取trafficlights_perception_component.config文件中的各个变量。单独把这些变量分离出来保存在一个文件中只是为了后续方便统一修改。

2.1.4 InitAlgorithmPlugin()

初始化一些算法插件

重置了预处理部分的类实例：preprocessor_；并且对preprocessor_做了一些初始化
将所有摄像头名称按照焦长降序排列
因为后面会对摄像头进行选择，而红绿灯一般采用长焦相机的图像，所以焦长降序排列
对每个相机（传感器）是否存在进行判断；如果不存在则报错，存在则加载相应相机的内外参以及一些其他参数
存在判断应该是对满足运行apollo代码所需的必要相机进行检查
初始化高精地图hdmaps
判断hd_map_指针是否为空，初始化高精地图，重设了类实例traffic_light_pipeline_，重设后初始化红绿灯功能的处理管道

这些都会在后续识别中用到，作为API被调用，不细讲了。

2.1.5 InitCameraListeners()

该函数对每个相机通过std::function和std::bind函数来听取各个相机传递来的数据，接收到数据后，调用主函数OnReceiveImage，开始红绿灯检测

2.1.6 InitV2XListener()

类似CameraListeners

2.1.7 InitCameraFrame()

该函数主要是初始化变量data_provider_init_options_，对其传递了如下参数：

图像宽高（1920*1080）
gpu id
是否校正畸变
对每个相机更新相机名、图像帧

2.2 主函数部分

2.2.1 OnReceiveImage()

接收参数

const std::shared_ptr<apollo::drivers::Image> msg;                      // cyber message消息传递
const std::string& camera_name;                                                           // 相机名称，每次调用主函数，只处理一个相机的内容

函数体
首先是时间获取，记录和处理：

 std::lock_guard<std::mutex> lck(mutex_);                                                                                           // 进入临界区，先上锁，操作系统知识
 double receive_img_timestamp = apollo::common::time::Clock::NowInSeconds();        // 读取当前时间戳
 double image_msg_ts = msg->measurement_time();                                                                    // 获取图像消息的时间戳
 image_msg_ts += image_timestamp_offset_;                                                                                   // 给图像消息时间戳加上时间偏移，应该是做一些修正
 last_sub_camera_image_ts_[camera_name] = image_msg_ts;                                                // 更新子相机最后一张图的时间戳

接着计算了时间延迟，AINFO到日志中：

  {
     
    const double cur_time = apollo::common::time::Clock::NowInSeconds();
    const double start_latency = (cur_time - msg->measurement_time()) * 1e3;
    AINFO << "FRAME_STATISTICS:TrafficLights:Start:msg_time["
          << GLOG_TIMESTAMP(msg->measurement_time()) << "]:cur_time["
          << GLOG_TIMESTAMP(cur_time) << "]:cur_latency[" << start_latency
          << "]";
  }

下面这一段实现的功能主要是检查相机和图像的状态是否正常，然后记录一些时间数据。
通过if判断被调函数CheckCameraImageStatus的返回结果，确认函数是否正确执行，正确执行则继续之后的代码，未正确执行则直接return(退出主函数)：

  const std::string perf_indicator = "trafficlights";
  PERCEPTION_PERF_BLOCK_START();                                                               // 应该是一个计时器，主函数只调用了一次
  if (!CheckCameraImageStatus(image_msg_ts, check_image_status_interval_thresh_, camera_name)) {
     
    AERROR << "CheckCameraImageStatus failed";
    return;
  }                                                                                                                                         // 调用CheckCameraImageStatus函数
  const auto check_camera_status_time = 
  	PERCEPTION_PERF_BLOCK_END_WITH_INDICATOR(perf_indicator,"CheckCameraImageStatus");   // 记录该过程完成，并返回时间

确认相机正常工作后，申请了一个TLPreprocessorOption类实例：preprocess_option，用于传递预处理中的相应变量：

  camera::TLPreprocessorOption preprocess_option;
  preprocess_option.image_borders_size = &image_border_sizes_;

在上面检查相机和图像的状态正常后，开始调用UpdateCameraSelection函数。从函数名可以看出该函数主要的功能是更新相机的选择，选择相机后，后面就使用该相机的图像持续做红绿灯检测了。

调用完UpdateCameraSelection函数函数后，还调用了前面说到的一个记录过程完成的函数PERCEPTION_PERF_BLOCK_END_WITH_INDICATOR，并返回一个该过程结束时的时间：

  if (!UpdateCameraSelection(image_msg_ts, preprocess_option, &frame_)) {
     
    AWARN << "add_cached_camera_selection failed, ts: " << image_msg_ts;
  }
  const auto update_camera_selection_time =
      PERCEPTION_PERF_BLOCK_END_WITH_INDICATOR(perf_indicator,
                                               "UpdateCameraSelection");

调用完UpdateCameraSelection函数后，此时能够确定的是：用作红绿灯检测模块的相机已经确定下来了，且对应相机的的红绿灯的像素坐标也已映射到了CameraFrame当中，可以直接调用。

在做完上面的工作后，此时会做一个是否跳过该帧检测的判断；如果同时满足条件“最后一次处理过程时间戳大于0”且“当前接收图像与最后一次处理的时间差小于既定处理时间间隔”，则跳过该图像的处理，不执行后面的内容：

  if (last_proc_image_ts_ > 0.0 &&
      receive_img_timestamp - last_proc_image_ts_ < proc_interval_seconds_) {
   
    AINFO << "skip current image, img_ts: " << image_msg_ts
          << " , receive_img_timestamp: " << receive_img_timestamp
          << " ,_last_proc_image_ts: " << last_proc_image_ts_
          << " , _proc_interval_seconds: " << proc_interval_seconds_;
    return;
  }

如果不跳过，则判断图像同步是否OK：

  bool sync_image_ok =
      preprocessor_->SyncInformation(image_msg_ts, camera_name);
  const auto sync_information_time = PERCEPTION_PERF_BLOCK_END_WITH_INDICATOR(
      perf_indicator, "SyncInformation");

  if (!sync_image_ok) {
   
    AINFO << "PreprocessComponent not publish image, ts:" << image_msg_ts
          << ", camera_name: " << camera_name;
    //    SendSimulationMsg();
    return;
  }

然后将图片加载到类实例frame_中，更新一些相关变量，检查时间差是否符合相应要求等：

  // Fill camera frame_
  camera::DataProvider::ImageOptions image_options;
  image_options.target_color = base::Color::RGB;
  frame_.data_provider = data_providers_map_.at(camera_name).get();
  frame_.data_provider->FillImageData(                                          //加载图片
      image_height_, image_width_,
      reinterpret_cast<const uint8_t*>(msg->data().data()), msg->encoding());

  // frame_.data_provider->FillImageData(image.rows, image.cols,
  //                                      (const uint8_t *)(image.data), "bgr8");
  frame_.timestamp = image_msg_ts;                                        //更新当前图片的时间戳到frame_
  const auto fill_image_data_time =                                           //记录加载图片的时间
      PERCEPTION_PERF_BLOCK_END_WITH_INDICATOR(perf_indicator, "FillImageData");

  // caros monitor -- image system time diff
  const auto& diff_image_sys_ts = image_msg_ts - receive_img_timestamp;
  if (fabs(diff_image_sys_ts) > image_sys_ts_diff_threshold_) {
   
    const std::string metric_name = "perception traffic_light exception";
    const std::string debug_string =
        absl::StrCat("diff_image_sys_ts:", diff_image_sys_ts,
                     ",camera_id:", camera_name, ",camera_ts:", image_msg_ts);
    AWARN << "image_ts - system_ts(in seconds): " << diff_image_sys_ts
          << ". Check if image timestamp drifts."
          << ", camera_id: " + camera_name
          << ", debug_string: " << debug_string;
  }

做完上面的工作后，通过调用VerifyLightsProjection函数再最后一次确认红绿灯映射，记录该模块执行时间，更新最后一次处理时间戳：

if (!VerifyLightsProjection(image_msg_ts, preprocess_option, camera_name,
                              &frame_)) {
   
    AINFO << "VerifyLightsProjection on image failed, ts: " << image_msg_ts
          << ", camera_name: " << camera_name
          << " last_query_tf_ts_: " << last_query_tf_ts_
          << " need update_camera_selection immediately,"
          << " reset last_query_tf_ts_ to -1";
    last_query_tf_ts_ = -1.0;
  }
  const auto verify_lights_projection_time =
      PERCEPTION_PERF_BLOCK_END_WITH_INDICATOR(perf_indicator,
                                               "VerifyLightsProjection");
  last_proc_image_ts_ = apollo::common::time::Clock::NowInSeconds();