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 diffconst 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();

其實VerifyLightsProjection函數和UpdateCameraSelection所做工作均是：先申請pose和signal，然后調用QueryPoseAndSignals和GenerateTrafficLights，將pose和signal更新到frame_變量中。

但不同的是，UpdateCameraSelection調用了preprocessor_的UpdateCameraSelection函數，選擇合適的相機；而VerifyLightsProjection調用了preprocessor_的UpdateLightsProjection函數，UpdateLightsProjection函數判斷了當前相機是否是最小焦長且返回的映射結果是否為空；如果不是最小焦長則判斷更大焦長的映射結果是否超出圖像邊界。

因此VerifyLightsProjection函數實現的功能如同它的名字，確認映射結果。

確認映射結果，記錄相應時間戳后，紅綠燈檢測的預處理（preprocess）過程就完成了，開始進入第二階段的處理：process

接下來將frame_和camera_perception_options_中保存的圖像信息放到感知模塊去識別，輸出相應的結果到frame_中：

traffic_light_pipeline_->Perception(camera_perception_options_, &frame_); // 注意這里的frame_參數是引用

然后記錄相應的時間，打印日志信息：

const auto traffic_lights_perception_time =PERCEPTION_PERF_BLOCK_END_WITH_INDICATOR(perf_indicator, "TrafficLightsPerception");for (auto light : frame_.traffic_lights) {AINFO << "after tl pipeline " << light->id << " color "<< static_cast<int>(light->status.color);}

然后同步frame_中的信息，將紅綠燈模塊需要發布到公共topic的信心存儲到變量out_msg中，判斷輸出的信息是否成功，如果成功則調用writer_，將結果傳出：

SyncV2XTrafficLights(&frame_); std::shared_ptr<apollo::perception::TrafficLightDetection> out_msg =std::make_shared<apollo::perception::TrafficLightDetection>();if (!TransformOutputMessage(&frame_, camera_name, &out_msg)) {AERROR << "transform_output_message failed, msg_time: "<< GLOG_TIMESTAMP(msg->measurement_time());return;}// send msgwriter_->Write(out_msg);// SendSimulationMsg();

最后的代碼是一些日志打印，就不寫了。
到這里，主函數的調用就講完啦。

2.3 被主函數調用的其他部分

CheckCameraImageStatus()

該函數主要是利用時間差判斷相機和圖像數據是否正常工作。

• 接收參數

  double timestamp; // 時間戳 double interval; // 一個閾值，為1.0 const std::string& camera_name; // 相機名

• 重點關注變量
  last_sub_camera_image_ts_： std::map關聯容器，類似Python中的字典，存儲了“相機名-該相機最后一幀時間戳”的映射

• 函數體

  PERCEPTION_PERF_FUNCTION(); // 不太確定這一行記錄的是什么bool camera_ok = true; // 先將該變量設為真，后面會根據相機狀態修改std::string no_image_camera_names = "";for (const auto& pr : last_sub_camera_image_ts_) { // 使用指針遍歷last_sub_camera_image_ts_const auto cam_name = pr.first; // 獲取相機名double last_sub_camera_ts = pr.second; // 獲取相機名對應的最后一幀的時間戳// should be 0.0, change to 1 in case of float precisionif (last_sub_camera_ts < 1.0 || timestamp - last_sub_camera_ts > interval) { // 如果時間戳小于1s或者當前時間與最后一幀時間戳差超過閾值，則判斷相機沒有正常工作；此時應該可以推測出，時間戳是一個累積的值，而不是一個階段長度。preprocessor_->SetCameraWorkingFlag(cam_name, false); // 相機非正常工作，修改相關記錄變量AWARN << "camera is probably not working"<< " , current ts: " << timestamp<< " , last_sub_camera_ts: " << last_sub_camera_ts<< " , camera_name: " << cam_name;camera_ok = false;AINFO << "camera status:" << camera_ok;no_image_camera_names += (" " + cam_name);}}

  在上面的代碼中已經確定了相機是否正常工作的Flag，下面的代碼對Flag做一些相應修改，最后根據結果返回false或者true：

  bool is_camera_working = false;if (!preprocessor_->GetCameraWorkingFlag(camera_name, &is_camera_working)) {AERROR << "get_camera_is_working_flag ts: " << timestamp<< " camera_name: " << camera_name;return false;}if (!is_camera_working) {if (!preprocessor_->SetCameraWorkingFlag(camera_name, true)) {AERROR << "set_camera_is_working_flag ts: " << timestamp<< " camera_name: " << camera_name;return false;}}return true;

UpdateCameraSelection()

該函數做了很多工作，首先判斷是否跳過該幀，通過判斷條件“最后一次詢問時間是否大于0”和“當前時間戳和最后一次詢問的時間戳的差值是否小于既定的詢問時間間隔”是否同時滿足，若同時滿足則說明該幀還在時間間隔內，為了有較好的性能，跳過該幀的處理。

如果不跳過該幀，則調用QueryPoseAndSignals函數，該函數更新了兩個變量，汽車的pose坐標和signals信號；在汽車pose坐標部分，從獲取汽車的gps坐標到生成汽車的utm坐標，還根據汽車的utm坐標獲取到了紅綠燈的utm坐標，并更新到了signals中。

接著調用了GenerateTrafficLights函數，將signals中的紅綠燈utm坐標更新到CameraFrame中

最后通過調用與當前函數的同名函數UpdateCameraSelection函數（屬于另一個類），此時還沒有確定選擇哪個相機，經過該函數后，不僅確定了選擇哪個相機，還將對應相機的紅綠燈做完了像素坐標的映射。

• 接收參數

  double timestamp; // 當前圖像幀的時間戳 const camera::TLPreprocessorOption& option; // 預處理相關 camera::CameraFrame* frame; // CameraFrame，一個記錄圖像幀所有數據的類，不只是局限于紅綠燈檢測

• 重點關注變量
  pose：記錄該時刻圖像對應的汽車信息的變量；該變量是在這個函數內申請的
  signals ：記錄該時刻圖像對應的高精地圖傳來的信息的變量；該變量也是在這個函數內申請的

• 函數體
  這一塊主要判斷是否需要舍去該幀。程序不可能傳來的每幀圖像都進行計算，適當pass一部分，提高性能：

  PERCEPTION_PERF_FUNCTION(); // 不太確定該函數的作用，后面的其他函數也會在開始的時候都調用，姑且作為記錄時間的函數吧const double current_ts = apollo::common::time::Clock::NowInSeconds(); // 獲取實時時間if (last_query_tf_ts_ > 0.0 &&current_ts - last_query_tf_ts_ < query_tf_interval_seconds_) { // 判斷最后一次詢問時間是否正常，并且計算當前時間與最后一次詢問的時間差是否小于閾值，同時滿足兩個條件就舍棄該幀。AINFO << "skip current tf msg, img_ts: " << timestamp<< " , last_query_tf_ts_: " << last_query_tf_ts_;return true;}AINFO << "start select camera";

  申請了一個CarPose類實例：pose，該類主要記錄了一些汽車的信息，作用域也不只是局限于紅綠燈模塊，和CameraFrame類似；接著申請了一個存儲高精地圖類信息的向量signals，signals是一個proto類，其中存儲的信息在后面會更新到CameraFrame中。之后就開始調用函數QueryPoseAndSignals，獲取汽車在高精地圖中的位置，然后更新相應變量：

  camera::CarPose pose;std::vector<apollo::hdmap::Signal> signals; if (!QueryPoseAndSignals(timestamp, &pose, &signals)) { // 函數調用，注意傳遞的參數是引用，意味著在函數調用過程中變量相關值會發生變化AINFO << "query_pose_and_signals failed, ts: " << timestamp;return false;}last_query_tf_ts_ = current_ts; // 詢問后更新最新的詢問時間

  經過上面的代碼，我們獲取了汽車的utm坐標和汽車前方的紅綠燈的utm坐標（假定前方有紅綠燈），然后調用GenerateTrafficLights函數，將signals中的紅綠燈utm坐標更新到CameraFrame中去：

  GenerateTrafficLights(signals, &frame->traffic_lights);AINFO << "hd map signals " << frame->traffic_lights.size();

  更新數據后，開始調用tl_preprocessor.cc文件下的UpdateCameraSelection函數，該文件從文件名可以看出是紅綠燈的預處理部分；調完UpdateCameraSelection函數后，后續AINFO了紅綠燈的映射坐標到日志中，映射完畢后函數就結束了，返回執行結果true or false：

  if (!preprocessor_->UpdateCameraSelection(pose, option,&frame->traffic_lights)) { // 注意這里的引用AERROR << "add_cached_lights_projections failed, ts: " << timestamp;} else {AINFO << "add_cached_lights_projections succeed, ts: " << timestamp;}for (auto& light : frame->traffic_lights) {AINFO << "x " << light->region.projection_roi.x << " y "<< light->region.projection_roi.y << " w "<< light->region.projection_roi.width << " h "<< light->region.projection_roi.height;}return true;

  TLPreprocessor::UpdateCameraSelection

  • 接收參數

    const CarPose &pose; // 汽車pose const TLPreprocessorOption &option; // 預處理參數 std::vector<base::TrafficLightPtr> *lights; // 圖像中紅綠燈的指針

  • 函數體
    該段代碼主要是獲取一些變量，輸出一些日志，判斷傳入的紅綠燈指針是否非空：

    const double &timestamp = pose.getTimestamp();selected_camera_name_.first = timestamp;selected_camera_name_.second = GetMaxFocalLenWorkingCameraName(); // 獲取最長焦相機的相機名AINFO << "TLPreprocessor Got signal number: " << lights->size()<< ", ts: " << timestamp;if (lights->empty()) { // 判斷紅綠燈信息指針是否為空AINFO << "No signals, select camera with max focal length: "<< selected_camera_name_.second;return true;}

    接著調用函數TLPreprocessor::ProjectLightsAndSelectCamera，根據函數的返回結果判斷紅綠燈映射和相機選擇是否成功執行：

    if (!ProjectLightsAndSelectCamera(pose, option,&(selected_camera_name_.second), lights)) {AERROR << "project_lights_and_select_camera failed, ts: " << timestamp;}AINFO << "selected_camera_id: " << selected_camera_name_.second;return true;

    TLPreprocessor::ProjectLightsAndSelectCamera
    該函數將所有相機的紅綠燈坐標映射到圖像坐標，并選擇一個相機的圖片

    • 接收參數const CarPose &pose // 汽車pose const TLPreprocessorOption &option; // 存儲相機選擇的相關信息 std::string *selected_camera_name; // 選擇的相機的名字 std::vector<base::TrafficLightPtr> *lights; // 紅綠燈指針
    • 函數體
      下面這段主要是判斷函數名是否為空，紅綠燈指針是否為空，然后清空兩個記錄指針： if (selected_camera_name == nullptr) {AERROR << "selected_camera_name is not available";return false;}if (lights == nullptr) {AERROR << "lights is not available";return false;}for (auto &light_ptrs : lights_on_image_array_) {light_ptrs.clear();}for (auto &light_ptrs : lights_outside_image_array_) {light_ptrs.clear();} 接著利用for循環遍歷所有相機，根據焦距下降的順序獲取相機名字，因此一般都會先選擇長焦相機（排在第一位）的圖像，便于識別紅綠燈及其信息；選擇某個相機后，就開始調用TLPreprocessor::ProjectLights函數對相機的圖像進行紅綠燈從utm的坐標映射到像素坐標：依次更新每個相機的映射結果，for循環結束后所有的相機的映射均已完成： const auto &camera_names = projection_.getCameraNamesByDescendingFocalLen(); for (size_t cam_id = 0; cam_id < num_cameras_; ++cam_id) { // 根據id進行for循環const std::string &camera_name = camera_names[cam_id]; // 根據id獲取相機名字if (!ProjectLights(pose, camera_name, lights, // 調用TLPreprocessor::ProjectLights函數，根據返回結果判斷映射是否成功，如果映射失敗則返回false，映射成功則更新相應數據，注意參數傳遞中的指針和引用的使用&(lights_on_image_array_[cam_id]),&(lights_outside_image_array_[cam_id]))) {AERROR << "select_camera_by_lights_projection project lights on "<< camera_name << " image failed";return false;}} TLPreprocessor::ProjectLights
      • 接收參數const CarPose &pose, const std::string &camera_name; // 相機名 std::vector<base::TrafficLightPtr> *lights; // 紅綠燈指針 base::TrafficLightPtrs *lights_on_image; // 記錄映射后在圖片范圍內的像素坐標的指針 base::TrafficLightPtrs *lights_outside_image; // 記錄映射后超出圖片范圍的像素坐標的指針
      • 重點關注參數
        lights_on_image
        lights_outside_image
      • 函數體
        判斷lights是否為空，相機是否是正確的相機，相機是否正常工作： if (lights->empty()) {AINFO << "project_lights get empty signals.";return true;}if (!projection_.HasCamera(camera_name)) {AERROR << "project_lights get invalid camera_name: " << camera_name;return false;}bool is_working = false;if (!GetCameraWorkingFlag(camera_name, &is_working) || !is_working) {AWARN << "TLPreprocessor::project_lights not project lights, "<< "camera is not working, camera_name: " << camera_name;return true;} 接著開始for循環遍歷lights中的每個紅綠燈light，通過if判斷映射結果，注意if的進入條件，條件中的Project函數就是將紅綠燈utm坐標映射到像素坐標的函數，具體就不展開講了，有興趣的讀者自行查閱相機標定和坐標映射的文章： for (size_t i = 0; i < lights->size(); ++i) { // 遍歷lightsbase::TrafficLightPtr light_proj(new base::TrafficLight);auto light = lights->at(i);if (!projection_.Project(pose, ProjectOption(camera_name), light.get())) { light->region.outside_image = true; // 進入if則說明映射失敗，將失敗的lgiht push到lights_outside_image中*light_proj = *light;lights_outside_image->push_back(light_proj); // 進入else則說明映射成功，將lgiht push到lights_on_image中} else {light->region.outside_image = false;*light_proj = *light;lights_on_image->push_back(light_proj);}}return true; // 無論映射是否超出圖片都返回真，
      所有相機完成像素坐標映射后，初始化projections_outside_all_images_變量，這里的初始化也是有技巧的，是根據lights指針是否非空的bool結果初始化，如果lights空，則不會進行后續的操作： projections_outside_all_images_ = !lights->empty(); 然后通過循環每個相機id，判斷每個相機是否都存在超出映射的情況，projections_outside_all_images_的值是由所有相機的交集決定的，如果每個相機的映射結果都超出圖像范圍，則projections_outside_all_images_就會是true，否則只要有一個相機沒超出映射圖像范圍，則projections_outside_all_images_為false： for (size_t cam_id = 0; cam_id < num_cameras_; ++cam_id) {projections_outside_all_images_ =projections_outside_all_images_ &&(lights_on_image_array_[cam_id].size() < lights->size());}if (projections_outside_all_images_) { // 若所有相機映射都超出圖像范圍，則會AWARN，但不會報錯AWARN << "lights projections outside all images";} 進行上述判斷后，選擇其中一個相機（一般是長焦相機），確定相機選擇參數，更新相關變量，返回true： // select which camera to be usedSelectCamera(&lights_on_image_array_, &lights_outside_image_array_, option,selected_camera_name);return true;

    至此TLPreprocessor::UpdateCameraSelection函數結束，返回函數執行結果。

  至此，被主函數調用的UpdateCameraSelection才真正結束，現在返回到主函數中。

QueryPoseAndSignals()

該函數首先調用了GetCarPose函數，獲取到了汽車的utm坐標；然后獲取高精地圖中紅綠燈的utm坐標，最后返回函數是否成功執行的bool值
其中紅綠燈的utm坐標是根據汽車的utm坐標來判斷前方是否有紅綠燈來獲取的。

• 接收參數

  const double ts, camera::CarPose* pose; // 記錄汽車相關數據的類實例 std::vector<apollo::hdmap::Signal>* signals; // 記錄高精地圖信息的向量，向量每個元素記錄一個高精地圖信號

• 函數體
  通過調用函數GetCarPose，從gps中獲取汽車的utm坐標，更新汽車實例pose中的其他變量：

  PERCEPTION_PERF_FUNCTION();// get poseif (!GetCarPose(ts, pose)) {AINFO << "query_pose_and_signals failed to get car pose, ts:" << ts;return false;}

  獲取到汽車的utm坐標后，讀取汽車坐標x, y，AINFO到日志中：

  auto pos_y = std::to_string(pose->getCarPose()(1, 3));AINFO << "query_pose_and_signals get position (x, y): "<< " (" << pos_x << ", " << pos_y << ").";

  然后判斷高精地圖是否初始化（打開）：

  if (!hd_map_) {AERROR << "hd_map_ not init.";return false;}

  高精地圖正確打開后，開始根據汽車的utm坐標從高精地圖中獲取汽車即將遇到的紅綠燈的坐標：

  // get signalsEigen::Vector3d car_position = pose->getCarPosition(); // 獲取汽車的x, y, z坐標if (!hd_map_->GetSignals(car_position, forward_distance_to_query_signals, signals)) { // 根據汽車的x, y, z坐標，更新紅綠燈的utm坐標到signals中，正常情況應進入else中，進入if則說明獲取信號失敗，但不會終止程序運行，而是使用最后也是最近存儲的信號代替，AWARN到日志中if (ts - last_signals_ts_ < valid_hdmap_interval_) { *signals = last_signals_;AWARN << "query_pose_and_signals failed to get signals info. "<< "Now use last info. ts:" << ts << " pose:" << *pose<< " signals.size(): " << signals->size();} else {AERROR << "query_pose_and_signals failed to get signals info. "<< "ts:" << ts << " pose:" << *pose;}} else { // 正確獲取signals，更新最后一個信號的時間戳為當前時間戳，最后一個信號為當前最新信號//AERROR << "query_pose_and_signals succeeded, signals.size(): "// << signals->size();// here need mutex lock_guard, added at the beginning of OnReceiveImage()last_signals_ts_ = ts;last_signals_ = *signals;}return true; // 更新完畢后返回true

GetCarPose()

該函數看了個一知半解，主要功能我推測應該是，獲取了汽車的gps坐標，然后根據映射規則映射到utm坐標。因為之后會用到汽車的utm坐標獲取后續的其他數據。

• 接收參數

  const double timestamp; // 當前圖像幀的時間戳 camera::CarPose* pose; // 汽車類實例

• 重點關注變量
  pose_matrix：先存儲了汽車utm坐標，在將汽車坐標更新到pose后，該變量又被更新為存儲映射參數矩陣，然后又更新到pose的另一個變量中，同一個變量存儲了兩種類型的數據。

• 函數體
  因此下面的代碼應該是獲取gps坐標，且將gps坐標轉換成utm坐標：

  Eigen::Matrix4d pose_matrix; // 申請一個4*4的矩陣，第一次里面存儲的是汽車的世界坐標// get pose car(gps) to worldif (!GetPoseFromTF(timestamp, tf2_frame_id_, tf2_child_frame_id_, // child frame 其實不太懂 &pose_matrix)) {AERROR << "get pose from tf failed, child_frame_id: "<< tf2_child_frame_id_;return false;}

  然后將時間戳和汽車utm坐標更新到汽車實例pose中：

  pose->timestamp_ = timestamp;pose->pose_ = pose_matrix;

  接下來是相機標定過程中的一些固參的獲取，感興趣的可以去找找相機參數標定的文章看看，這里就不詳說了：

  int state = 0; // 計數正確狀態的變量 bool ret = true; // 預設ret 為true，后面會修改Eigen::Affine3d affine3d_trans; // 申請一個仿射變換的矩陣for (const auto& camera_name : camera_names_) { // 遍歷所有相機名const auto trans_wrapper = camera2world_trans_wrapper_map_[camera_name]; // 獲取對應相機的轉換映射指針ret = trans_wrapper->GetSensor2worldTrans(timestamp, &affine3d_trans); // 獲取傳感器到世界的轉換矩陣，返回獲取結果pose_matrix = affine3d_trans.matrix(); // 更新仿射變換矩陣到pose_matrix，這是第二次使用該變量存儲數據，第一次存儲的是汽車坐標，已經更新到pose中去了if (!ret) { // 判斷放射矩陣是否成功獲取，成功獲取則在else中更新相機對應的相機到世界的參數矩陣pose->ClearCameraPose(camera_name);AERROR << "get pose from tf failed, camera_name: " << camera_name;} else {pose->c2w_poses_[camera_name] = pose_matrix; // ret為真，則更新仿射矩陣到pose中，state+1，使最后返回的結果為truestate += 1;}}return state > 0; // 最終判斷獲取正確狀態的數量是否大于0，返回一個bool值

GetPoseFromTF()

這個函數我就不細說了，大致應該就是gps到utm的坐標轉換，因為該函數接收了一個引用參數pose_matrix；在該函數內最終這個矩陣更新為汽車的utm坐標。具體原理沒有去了解，有興趣的讀者可以自行查閱。

GenerateTrafficLights()

該函數的主要功能就是遍歷signals中的每個signal，更新到每個trafficlight中。這里需要注意的是，每個紅綠燈帶有4個point，每個point記錄的是紅綠燈一個角的utm坐標；每個signal代表一個紅綠燈。

• 接收參數

  const std::vector<apollo::hdmap::Signal>& signals; // 存儲紅綠燈utm坐標的signals std::vector<base::TrafficLightPtr>* traffic_lights; // 記錄紅綠燈信息的指針，在該函數中會更新

• 重點關注變量
  light：存儲一個紅綠燈的指針，TrafficLightPtr類實例，該類中存儲了一個紅綠燈的各場景下的數據，比如說映射坐標，ROI坐標，檢測出的紅綠燈坐標。具體可以看類的頭文件。

• 函數體

  traffic_lights->clear(); // 清除指針for (auto signal : signals) { // for循環遍歷每個signalbase::TrafficLightPtr light; light.reset(new base::TrafficLight); // 重置指針light->id = signal.id().id(); // 更新每個signal傳遞來的對應紅綠燈的id，每個紅綠燈是根據id區分的for (int i = 0; i < signal.boundary().point_size(); ++i) { // 開始遍歷signal中的四個point，每個點帶一組x, y, z, intensity，intensity具體含義沒細看base::PointXYZID point; // 每個point都是PointXYZID類型的數據結構point.x = signal.boundary().point(i).x();point.y = signal.boundary().point(i).y();point.z = signal.boundary().point(i).z();light->region.points.push_back(point); // 將存儲了數據的point push到light指向的region points中，具體自行看traffic_light.h文件}int cur_semantic = 0; // 變量含義不清楚 不重要light->semantic = cur_semantic; // 更新traffic_lights->push_back(light); // 再將每個light指針push到traffic_lights中stoplines_ = signal.stop_line(); // 這個也沒細看。到這里全部遍歷完后，該函數就完了，相應變量已更新。}

VerifyLightsProjection()

該函數和UpdateCameraSelection函數執行的功能在獲取pose和signals時基本沒區別，有區別的是最后一步。在該函數內，調用的是preprocessor_的UpdateLightsProjection函數，

• 接收參數

  const double& ts; // 圖像時間戳 const camera::TLPreprocessorOption& option; // 一些預處理時的選擇信息，里面包括了最終選擇的相機名稱 const std::string& camera_name; // 當前圖像的相機名 camera::CameraFrame* frame; // 當前圖像自帶的類實例frame_，記錄了該圖像中的各項信息

• 函數體
  首先申請了兩個類實例pose和signals，然后調用函數QueryPoseAndSignals，獲取pose和signal的信息，并返回獲取結果是否成功的bool值：

  PERCEPTION_PERF_FUNCTION();camera::CarPose pose;std::vector<apollo::hdmap::Signal> signals;if (!QueryPoseAndSignals(ts, &pose, &signals)) {AERROR << "query_pose_and_signals failed, ts: " << ts;// (*image_lights)->debug_info.is_pose_valid = false;return false;}

  在獲取到pose和signals后，和UpdateCameraSelection函數同樣的做法，調用GenerateTrafficLights函數和預處理類下的UpdateLightsProjection函數：

  GenerateTrafficLights(signals, &frame->traffic_lights);// // tianyu// ModifyHDData(&frame->traffic_lights, &pose);if (!preprocessor_->UpdateLightsProjection(pose, option, camera_name,&frame->traffic_lights)) {AWARN << "verify_lights_projection failed to update_lights_projection, "<< " ts: " << ts;return false;}AINFO << "VerifyLightsProjection success " << frame->traffic_lights.size();return true;

  調用完TLPreprocessor::UpdateLightsProjection后，返回相應結果。

  TLPreprocessor::UpdateLightsProjection()

  • 接收參數

    const CarPose &pose; // 汽車pose const TLPreprocessorOption &option; // 預處理是相關的選擇，包括選擇的相機名 const std::string &camera_name; // 當前相機名 std::vector<base::TrafficLightPtr> *lights; // 該圖像中的紅綠燈信號指針

  • 函數體
    首先清除兩個指針，AINFO一些信息，然后判斷傳入的lights指針是否非空：

    lights_on_image_.clear();lights_outside_image_.clear();AINFO << "clear lights_outside_image_ " << lights_outside_image_.size();if (lights->empty()) {AINFO << "No lights to be projected";return true;}

    接著開始調用映射函數TLPreprocessor::ProjectLights，將utm坐標映射成圖像的像素坐標：

    if (!ProjectLights(pose, camera_name, lights, &lights_on_image_, // 注意這里的參數引用&lights_outside_image_)) {AERROR << "update_lights_projection project lights on " << camera_name<< " image failed";return false;}

    將utm坐標映射成像素坐標后，判斷映射結果是否有超出圖像大小：

    if (lights_outside_image_.size() > 0) {AERROR << "update_lights_projection failed,"<< "lights_outside_image->size() " << lights_outside_image_.size()<< " ts: " << pose.getTimestamp();return false;}

    超出則報錯，返回false；未超出則繼續執行下面的代碼。

    下面的代碼首先獲取了最小焦長且正常工作的相機，然后判斷當前相機名是否是最小焦長相機。

    如果是，則返回條件“utm正確映射到像素坐標的個數是否大于零”的bool值，意思是，盡管取了焦長最小的相機的映射結果，雖然保證了最小焦長的映射結果不會超出邊界限制，但我還是要判斷映射結果是否為空。

    如果當前相機不是最小焦長相機，那么就需要判斷當前映射的結果是否超出了當前相機的邊界限制。如果超出了則報錯，每超出則會返回true，映射的結果由于傳遞的是指針，因此不需要傳實際值回去：

    auto min_focal_len_working_camera = GetMinFocalLenWorkingCameraName(); // 成功映射則獲取短焦相機的名字 if (camera_name == min_focal_len_working_camera) { // 判斷當前相機名是否是短焦相機，是則判斷成功映射的坐標是否為空，要同時滿足return lights_on_image_.size() > 0; }for (const base::TrafficLightPtr &light : lights_on_image_) { // 如果當前相機不是短焦相機，則需要判斷每個映射結果是否超出圖像范圍if (OutOfValidRegion(light->region.projection_roi, // 判斷是否超出圖像范圍projection_.getImageWidth(camera_name),projection_.getImageHeight(camera_name),option.image_borders_size->at(camera_name))) {AINFO << "update_lights_projection light project out of image region. "<< "camera_name: " << camera_name;return false; // 映射超出圖像范圍則返回false} } AINFO << "UpdateLightsProjection success"; return true;

    TLPreprocessor::ProjectLights()

    • 接收參數

      const CarPose &pose; // 保存汽車相關信息的變量 const std::string &camera_name; // 傳來的相機名稱，因為自動駕駛包括了多個攝像頭，所以必須區分不同相機 std::vector<base::TrafficLightPtr> *lights; // 傳來紅綠燈信息指針，是個向量，向量每個元素就是一個紅綠燈信息 base::TrafficLightPtrs *lights_on_image; // 指向一個向量的指針，向量的每個元素都是一個紅綠燈指針，每個紅綠燈指針指向一個紅綠燈信息 base::TrafficLightPtrs *lights_outside_image; // 同上 ，只是上面保存的是映射在圖片中的紅綠燈信息，這個保存了映射在圖片外的紅綠燈信息

    • 函數體
      判斷lights指針是否為空。空則不需要做映射，因為紅綠燈的檢測是階段性地啟動，在汽車進入紅綠檢測范圍才會執行紅綠燈檢測；非空則說明已經通過汽車的坐標獲取到了紅綠燈在三維世界的坐標。判斷代碼很簡單，如下：

      if (lights->empty()) {AINFO << "No lights to be projected";return true;}

      判斷相機是否正確，相應模塊是否啟動。HasCamera判斷相機相機名是否在相機名列表中，且相應的模塊是否啟動，返回布爾值：

      if (!projection_.HasCamera(camera_name)) {AERROR << "project_lights get invalid camera_name: " << camera_name;return false;}

      相機沒問題繼續執行下面代碼。初始化布爾變量is_working，調用**GetCameraWorkingFlag()**函數執行。注意，調用該函數時，傳進的參數is_working是使用了引用&，也就意味著該函數執行會改變is_working的值，然后根據返回的布爾結果判斷是否AWARN到日志中：

      // camera is not workingbool is_working = false;if (!GetCameraWorkingFlag(camera_name, &is_working) || !is_working) {AWARN << "TLPreprocessor::project_lights not project lights, " << "camera is not working, camera_name: " << camera_name;return true;}

      is_working在上面的代碼執行后會從false變為true，表示相機正常工作，繼續執行下面的代碼：

      for (size_t i = 0; i < lights->size(); ++i) {base::TrafficLightPtr light_proj(new base::TrafficLight); // 申請一個新的TrafficLightPtr指針auto light = lights->at(i); // 遍歷lights if (!projection_.Project(pose, ProjectOption(camera_name), light.get())) { // 利用if判斷被調函數Project結果，正常流程應該走else，進if則說明映射到幀上的坐標超出了圖像大小light->region.outside_image = true;*light_proj = *light;lights_outside_image->push_back(light_proj);} else {light->region.outside_image = false; // 如果Project成功執行，則將light指針下的region下的outside_image設為false，表示沒有超出圖片*light_proj = *light; // 傳遞正確映射的指針lights_on_image->push_back(light_proj); // 將指針推入lights_on_image向量中保存}}return true; // 返回ProjectLights函數執行的結果，即正確執行

GenerateTrafficLights()

GenerateTrafficLights()

GenerateTrafficLights()

GenerateTrafficLights()

GenerateTrafficLights()

GenerateTrafficLights()

GenerateTrafficLights()

總結

以上是生活随笔為你收集整理的「Apollo」百度Apollo感知模块(perception)红绿灯检测代码完整+详细解析的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。