「Apollo」百度Apollo感知模块(perception)红绿灯检测代码完整+详细解析
1 背景
最近在讀apollo感知模塊下的紅綠燈檢測，apollo框架思路清晰，風(fēng)格規(guī)范，值得多讀。直接上代碼文件：trafficlights_perception_component.cc
trafficlights_perception_component.cc
整個trafficlights_perception_component.cc文件的函數(shù)主要分為三部分：以init開頭的初始化部分，紅綠燈模塊的主函數(shù)OnReceiveImage，以及被主函數(shù)直接/間接調(diào)用的其他函數(shù)。
突然想寫個記錄，主函數(shù)已經(jīng)扒得七七八八了，現(xiàn)在主要在摳細(xì)節(jié)，所以直接上細(xì)節(jié)函數(shù)的分析了，隨寫隨更，部分簡單的判斷函數(shù)就不寫了，懂的都懂。
ps: 下面的函數(shù)順序是按照函數(shù)的執(zhí)行順序?qū)懙?#xff0c;屬于該文件的函數(shù)寫在標(biāo)題，不屬于該文件的被調(diào)用的其他文件的函數(shù)掛在標(biāo)題函數(shù)下面
pps: 目前自己c++還不太到位，有些描述不準(zhǔn)確的自己看代碼
2 概述
2.1初始化（Init & Init—）部分
初始化部分就不每塊代碼都拿出來說了，只說一下大致內(nèi)容，重點(diǎn)會提出來。
2.1.1 GetGpuId()
讀取紅綠燈的config和proto文件，進(jìn)行初始化；獲取使用的gpu id，應(yīng)對裝有多個gpu的電腦，我的只裝了一個，不多說。
2.1.2 Init()
綜合了各個模塊初始化函數(shù)（下面init開頭的函數(shù)）的總初始化函數(shù)，最后還判斷了紅綠燈檢測的可視化模塊是否打開，代碼就不貼上來了，報(bào)錯的英文里有詳細(xì)說明。
2.1.3 InitConfig()
讀取trafficlights_perception_component.config文件中的各個變量。單獨(dú)把這些變量分離出來保存在一個文件中只是為了后續(xù)方便統(tǒng)一修改。
2.1.4 InitAlgorithmPlugin()
初始化一些算法插件
- 重置了預(yù)處理部分的類實(shí)例：preprocessor_；并且對preprocessor_做了一些初始化
- 將所有攝像頭名稱按照焦長降序排列
因?yàn)楹竺鏁z像頭進(jìn)行選擇，而紅綠燈一般采用長焦相機(jī)的圖像，所以焦長降序排列 - 對每個相機(jī)（傳感器）是否存在進(jìn)行判斷；如果不存在則報(bào)錯，存在則加載相應(yīng)相機(jī)的內(nèi)外參以及一些其他參數(shù)
存在判斷應(yīng)該是對滿足運(yùn)行apollo代碼所需的必要相機(jī)進(jìn)行檢查 - 初始化高精地圖hdmaps
判斷hd_map_指針是否為空，初始化高精地圖，重設(shè)了類實(shí)例traffic_light_pipeline_，重設(shè)后初始化紅綠燈功能的處理管道
這些都會在后續(xù)識別中用到，作為API被調(diào)用，不細(xì)講了。
2.1.5 InitCameraListeners()
該函數(shù)對每個相機(jī)通過std::function和std::bind函數(shù)來聽取各個相機(jī)傳遞來的數(shù)據(jù)，接收到數(shù)據(jù)后，調(diào)用主函數(shù)OnReceiveImage，開始紅綠燈檢測
2.1.6 InitV2XListener()
類似CameraListeners
2.1.7 InitCameraFrame()
該函數(shù)主要是初始化變量data_provider_init_options_，對其傳遞了如下參數(shù)：
- 圖像寬高（1920*1080）
- gpu id
- 是否校正畸變
- 對每個相機(jī)更新相機(jī)名、圖像幀
2.2 主函數(shù)部分
2.2.1 OnReceiveImage()
-
接收參數(shù)
const std::shared_ptr<apollo::drivers::Image> msg; // cyber message消息傳遞 const std::string& camera_name; // 相機(jī)名稱，每次調(diào)用主函數(shù)，只處理一個相機(jī)的內(nèi)容 -
函數(shù)體
std::lock_guard<std::mutex> lck(mutex_); // 進(jìn)入臨界區(qū)，先上鎖，操作系統(tǒng)知識double receive_img_timestamp = apollo::common::time::Clock::NowInSeconds(); // 讀取當(dāng)前時間戳double image_msg_ts = msg->measurement_time(); // 獲取圖像消息的時間戳image_msg_ts += image_timestamp_offset_; // 給圖像消息時間戳加上時間偏移，應(yīng)該是做一些修正last_sub_camera_image_ts_[camera_name] = image_msg_ts; // 更新子相機(jī)最后一張圖的時間戳
首先是時間獲取，記錄和處理：接著計(jì)算了時間延遲，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<< "]";}下面這一段實(shí)現(xiàn)的功能主要是檢查相機(jī)和圖像的狀態(tài)是否正常，然后記錄一些時間數(shù)據(jù)。
const std::string perf_indicator = "trafficlights";PERCEPTION_PERF_BLOCK_START(); // 應(yīng)該是一個計(jì)時器，主函數(shù)只調(diào)用了一次if (!CheckCameraImageStatus(image_msg_ts, check_image_status_interval_thresh_, camera_name)) {AERROR << "CheckCameraImageStatus failed";return;} // 調(diào)用CheckCameraImageStatus函數(shù)const auto check_camera_status_time = PERCEPTION_PERF_BLOCK_END_WITH_INDICATOR(perf_indicator,"CheckCameraImageStatus"); // 記錄該過程完成，并返回時間
通過if判斷被調(diào)函數(shù)CheckCameraImageStatus的返回結(jié)果，確認(rèn)函數(shù)是否正確執(zhí)行，正確執(zhí)行則繼續(xù)之后的代碼，未正確執(zhí)行則直接return(退出主函數(shù))：確認(rèn)相機(jī)正常工作后，申請了一個TLPreprocessorOption類實(shí)例：preprocess_option，用于傳遞預(yù)處理中的相應(yīng)變量：
camera::TLPreprocessorOption preprocess_option;preprocess_option.image_borders_size = &image_border_sizes_;在上面檢查相機(jī)和圖像的狀態(tài)正常后，開始調(diào)用UpdateCameraSelection函數(shù)。從函數(shù)名可以看出該函數(shù)主要的功能是更新相機(jī)的選擇，選擇相機(jī)后，后面就使用該相機(jī)的圖像持續(xù)做紅綠燈檢測了。
調(diào)用完UpdateCameraSelection函數(shù)函數(shù)后，還調(diào)用了前面說到的一個記錄過程完成的函數(shù)PERCEPTION_PERF_BLOCK_END_WITH_INDICATOR，并返回一個該過程結(jié)束時的時間：
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");
調(diào)用完UpdateCameraSelection函數(shù)后，此時能夠確定的是：用作紅綠燈檢測模塊的相機(jī)已經(jīng)確定下來了，且對應(yīng)相機(jī)的的紅綠燈的像素坐標(biāo)也已映射到了CameraFrame當(dāng)中，可以直接調(diào)用。
在做完上面的工作后，此時會做一個是否跳過該幀檢測的判斷；如果同時滿足條件“最后一次處理過程時間戳大于0”且“當(dāng)前接收圖像與最后一次處理的時間差小于既定處理時間間隔”，則跳過該圖像的處理，不執(zhí)行后面的內(nèi)容：
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;}然后將圖片加載到類實(shí)例frame_中，更新一些相關(guān)變量，檢查時間差是否符合相應(yīng)要求等：
// 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; //更新當(dāng)前圖片的時間戳到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;}做完上面的工作后，通過調(diào)用VerifyLightsProjection函數(shù)再最后一次確認(rèn)紅綠燈映射，記錄該模塊執(zhí)行時間，更新最后一次處理時間戳：
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();其實(shí)VerifyLightsProjection函數(shù)和UpdateCameraSelection所做工作均是：先申請pose和signal，然后調(diào)用QueryPoseAndSignals和GenerateTrafficLights，將pose和signal更新到frame_變量中。
但不同的是，UpdateCameraSelection調(diào)用了preprocessor_的UpdateCameraSelection函數(shù)，選擇合適的相機(jī)；而VerifyLightsProjection調(diào)用了preprocessor_的UpdateLightsProjection函數(shù)，UpdateLightsProjection函數(shù)判斷了當(dāng)前相機(jī)是否是最小焦長且返回的映射結(jié)果是否為空；如果不是最小焦長則判斷更大焦長的映射結(jié)果是否超出圖像邊界。
因此VerifyLightsProjection函數(shù)實(shí)現(xiàn)的功能如同它的名字，確認(rèn)映射結(jié)果。
確認(rèn)映射結(jié)果，記錄相應(yīng)時間戳后，紅綠燈檢測的預(yù)處理（preprocess）過程就完成了，開始進(jìn)入第二階段的處理：process
接下來將frame_和camera_perception_options_中保存的圖像信息放到感知模塊去識別，輸出相應(yīng)的結(jié)果到frame_中：
traffic_light_pipeline_->Perception(camera_perception_options_, &frame_); // 注意這里的frame_參數(shù)是引用然后記錄相應(yīng)的時間，打印日志信息：
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_中的信息，將紅綠燈模塊需要發(fā)布到公共topic的信心存儲到變量out_msg中，判斷輸出的信息是否成功，如果成功則調(diào)用writer_，將結(jié)果傳出：
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();最后的代碼是一些日志打印，就不寫了。
到這里，主函數(shù)的調(diào)用就講完啦。
2.3 被主函數(shù)調(diào)用的其他部分
CheckCameraImageStatus()
該函數(shù)主要是利用時間差判斷相機(jī)和圖像數(shù)據(jù)是否正常工作。
-
接收參數(shù)
double timestamp; // 時間戳 double interval; // 一個閾值，為1.0 const std::string& camera_name; // 相機(jī)名 -
重點(diǎn)關(guān)注變量
last_sub_camera_image_ts_： std::map關(guān)聯(lián)容器，類似Python中的字典，存儲了“相機(jī)名-該相機(jī)最后一幀時間戳”的映射 -
函數(shù)體
PERCEPTION_PERF_FUNCTION(); // 不太確定這一行記錄的是什么bool camera_ok = true; // 先將該變量設(shè)為真，后面會根據(jù)相機(jī)狀態(tài)修改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; // 獲取相機(jī)名double last_sub_camera_ts = pr.second; // 獲取相機(jī)名對應(yīng)的最后一幀的時間戳// 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或者當(dāng)前時間與最后一幀時間戳差超過閾值，則判斷相機(jī)沒有正常工作；此時應(yīng)該可以推測出，時間戳是一個累積的值，而不是一個階段長度。preprocessor_->SetCameraWorkingFlag(cam_name, false); // 相機(jī)非正常工作，修改相關(guān)記錄變量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);}}在上面的代碼中已經(jīng)確定了相機(jī)是否正常工作的Flag，下面的代碼對Flag做一些相應(yīng)修改，最后根據(jù)結(jié)果返回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()
該函數(shù)做了很多工作，首先判斷是否跳過該幀，通過判斷條件“最后一次詢問時間是否大于0”和“當(dāng)前時間戳和最后一次詢問的時間戳的差值是否小于既定的詢問時間間隔”是否同時滿足，若同時滿足則說明該幀還在時間間隔內(nèi)，為了有較好的性能，跳過該幀的處理。
如果不跳過該幀，則調(diào)用QueryPoseAndSignals函數(shù)，該函數(shù)更新了兩個變量，汽車的pose坐標(biāo)和signals信號；在汽車pose坐標(biāo)部分，從獲取汽車的gps坐標(biāo)到生成汽車的utm坐標(biāo)，還根據(jù)汽車的utm坐標(biāo)獲取到了紅綠燈的utm坐標(biāo)，并更新到了signals中。
接著調(diào)用了GenerateTrafficLights函數(shù)，將signals中的紅綠燈utm坐標(biāo)更新到CameraFrame中
最后通過調(diào)用與當(dāng)前函數(shù)的同名函數(shù)UpdateCameraSelection函數(shù)（屬于另一個類），此時還沒有確定選擇哪個相機(jī)，經(jīng)過該函數(shù)后，不僅確定了選擇哪個相機(jī)，還將對應(yīng)相機(jī)的紅綠燈做完了像素坐標(biāo)的映射。
-
接收參數(shù)
double timestamp; // 當(dāng)前圖像幀的時間戳 const camera::TLPreprocessorOption& option; // 預(yù)處理相關(guān) camera::CameraFrame* frame; // CameraFrame，一個記錄圖像幀所有數(shù)據(jù)的類，不只是局限于紅綠燈檢測 -
重點(diǎn)關(guān)注變量
pose：記錄該時刻圖像對應(yīng)的汽車信息的變量；該變量是在這個函數(shù)內(nèi)申請的
signals ：記錄該時刻圖像對應(yīng)的高精地圖傳來的信息的變量；該變量也是在這個函數(shù)內(nèi)申請的 -
函數(shù)體
PERCEPTION_PERF_FUNCTION(); // 不太確定該函數(shù)的作用，后面的其他函數(shù)也會在開始的時候都調(diào)用，姑且作為記錄時間的函數(shù)吧const double current_ts = apollo::common::time::Clock::NowInSeconds(); // 獲取實(shí)時時間if (last_query_tf_ts_ > 0.0 &¤t_ts - last_query_tf_ts_ < query_tf_interval_seconds_) { // 判斷最后一次詢問時間是否正常，并且計(jì)算當(dāng)前時間與最后一次詢問的時間差是否小于閾值，同時滿足兩個條件就舍棄該幀。AINFO << "skip current tf msg, img_ts: " << timestamp<< " , last_query_tf_ts_: " << last_query_tf_ts_;return true;}AINFO << "start select camera";
這一塊主要判斷是否需要舍去該幀。程序不可能傳來的每幀圖像都進(jìn)行計(jì)算，適當(dāng)pass一部分，提高性能：申請了一個CarPose類實(shí)例：pose，該類主要記錄了一些汽車的信息，作用域也不只是局限于紅綠燈模塊，和CameraFrame類似；接著申請了一個存儲高精地圖類信息的向量signals，signals是一個proto類，其中存儲的信息在后面會更新到CameraFrame中。之后就開始調(diào)用函數(shù)QueryPoseAndSignals，獲取汽車在高精地圖中的位置，然后更新相應(yīng)變量：
camera::CarPose pose;std::vector<apollo::hdmap::Signal> signals; if (!QueryPoseAndSignals(timestamp, &pose, &signals)) { // 函數(shù)調(diào)用，注意傳遞的參數(shù)是引用，意味著在函數(shù)調(diào)用過程中變量相關(guān)值會發(fā)生變化AINFO << "query_pose_and_signals failed, ts: " << timestamp;return false;}last_query_tf_ts_ = current_ts; // 詢問后更新最新的詢問時間經(jīng)過上面的代碼，我們獲取了汽車的utm坐標(biāo)和汽車前方的紅綠燈的utm坐標(biāo)（假定前方有紅綠燈），然后調(diào)用GenerateTrafficLights函數(shù)，將signals中的紅綠燈utm坐標(biāo)更新到CameraFrame中去：
GenerateTrafficLights(signals, &frame->traffic_lights);AINFO << "hd map signals " << frame->traffic_lights.size();更新數(shù)據(jù)后，開始調(diào)用tl_preprocessor.cc文件下的UpdateCameraSelection函數(shù)，該文件從文件名可以看出是紅綠燈的預(yù)處理部分；調(diào)完UpdateCameraSelection函數(shù)后，后續(xù)AINFO了紅綠燈的映射坐標(biāo)到日志中，映射完畢后函數(shù)就結(jié)束了，返回執(zhí)行結(jié)果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
-
接收參數(shù)
const CarPose &pose; // 汽車pose const TLPreprocessorOption &option; // 預(yù)處理參數(shù) std::vector<base::TrafficLightPtr> *lights; // 圖像中紅綠燈的指針 -
函數(shù)體
const double ×tamp = pose.getTimestamp();selected_camera_name_.first = timestamp;selected_camera_name_.second = GetMaxFocalLenWorkingCameraName(); // 獲取最長焦相機(jī)的相機(jī)名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;}
該段代碼主要是獲取一些變量，輸出一些日志，判斷傳入的紅綠燈指針是否非空：接著調(diào)用函數(shù)TLPreprocessor::ProjectLightsAndSelectCamera，根據(jù)函數(shù)的返回結(jié)果判斷紅綠燈映射和相機(jī)選擇是否成功執(zhí)行：
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
該函數(shù)將所有相機(jī)的紅綠燈坐標(biāo)映射到圖像坐標(biāo)，并選擇一個相機(jī)的圖片- 接收參數(shù)const CarPose &pose // 汽車pose const TLPreprocessorOption &option; // 存儲相機(jī)選擇的相關(guān)信息 std::string *selected_camera_name; // 選擇的相機(jī)的名字 std::vector<base::TrafficLightPtr> *lights; // 紅綠燈指針
- 函數(shù)體
下面這段主要是判斷函數(shù)名是否為空，紅綠燈指針是否為空，然后清空兩個記錄指針： 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循環(huán)遍歷所有相機(jī)，根據(jù)焦距下降的順序獲取相機(jī)名字，因此一般都會先選擇長焦相機(jī)（排在第一位）的圖像，便于識別紅綠燈及其信息；選擇某個相機(jī)后，就開始調(diào)用TLPreprocessor::ProjectLights函數(shù)對相機(jī)的圖像進(jìn)行紅綠燈從utm的坐標(biāo)映射到像素坐標(biāo)：依次更新每個相機(jī)的映射結(jié)果，for循環(huán)結(jié)束后所有的相機(jī)的映射均已完成： const auto &camera_names = projection_.getCameraNamesByDescendingFocalLen(); for (size_t cam_id = 0; cam_id < num_cameras_; ++cam_id) { // 根據(jù)id進(jìn)行for循環(huán)const std::string &camera_name = camera_names[cam_id]; // 根據(jù)id獲取相機(jī)名字if (!ProjectLights(pose, camera_name, lights, // 調(diào)用TLPreprocessor::ProjectLights函數(shù)，根據(jù)返回結(jié)果判斷映射是否成功，如果映射失敗則返回false，映射成功則更新相應(yīng)數(shù)據(jù)，注意參數(shù)傳遞中的指針和引用的使用&(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- 接收參數(shù)const CarPose &pose, const std::string &camera_name; // 相機(jī)名 std::vector<base::TrafficLightPtr> *lights; // 紅綠燈指針 base::TrafficLightPtrs *lights_on_image; // 記錄映射后在圖片范圍內(nèi)的像素坐標(biāo)的指針 base::TrafficLightPtrs *lights_outside_image; // 記錄映射后超出圖片范圍的像素坐標(biāo)的指針
- 重點(diǎn)關(guān)注參數(shù)
lights_on_image
lights_outside_image - 函數(shù)體
判斷l(xiāng)ights是否為空，相機(jī)是否是正確的相機(jī)，相機(jī)是否正常工作： 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循環(huán)遍歷lights中的每個紅綠燈light，通過if判斷映射結(jié)果，注意if的進(jìn)入條件，條件中的Project函數(shù)就是將紅綠燈utm坐標(biāo)映射到像素坐標(biāo)的函數(shù)，具體就不展開講了，有興趣的讀者自行查閱相機(jī)標(biāo)定和坐標(biāo)映射的文章： 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; // 進(jìn)入if則說明映射失敗，將失敗的lgiht push到lights_outside_image中*light_proj = *light;lights_outside_image->push_back(light_proj); // 進(jìn)入else則說明映射成功，將lgiht push到lights_on_image中} else {light->region.outside_image = false;*light_proj = *light;lights_on_image->push_back(light_proj);}}return true; // 無論映射是否超出圖片都返回真，
至此TLPreprocessor::UpdateCameraSelection函數(shù)結(jié)束，返回函數(shù)執(zhí)行結(jié)果。
至此，被主函數(shù)調(diào)用的UpdateCameraSelection才真正結(jié)束，現(xiàn)在返回到主函數(shù)中。
-
QueryPoseAndSignals()
該函數(shù)首先調(diào)用了GetCarPose函數(shù)，獲取到了汽車的utm坐標(biāo)；然后獲取高精地圖中紅綠燈的utm坐標(biāo)，最后返回函數(shù)是否成功執(zhí)行的bool值
其中紅綠燈的utm坐標(biāo)是根據(jù)汽車的utm坐標(biāo)來判斷前方是否有紅綠燈來獲取的。
-
接收參數(shù)
const double ts, camera::CarPose* pose; // 記錄汽車相關(guān)數(shù)據(jù)的類實(shí)例 std::vector<apollo::hdmap::Signal>* signals; // 記錄高精地圖信息的向量，向量每個元素記錄一個高精地圖信號 -
函數(shù)體
PERCEPTION_PERF_FUNCTION();// get poseif (!GetCarPose(ts, pose)) {AINFO << "query_pose_and_signals failed to get car pose, ts:" << ts;return false;}
通過調(diào)用函數(shù)GetCarPose，從gps中獲取汽車的utm坐標(biāo)，更新汽車實(shí)例pose中的其他變量：獲取到汽車的utm坐標(biāo)后，讀取汽車坐標(biāo)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;}高精地圖正確打開后，開始根據(jù)汽車的utm坐標(biāo)從高精地圖中獲取汽車即將遇到的紅綠燈的坐標(biāo)：
// get signalsEigen::Vector3d car_position = pose->getCarPosition(); // 獲取汽車的x, y, z坐標(biāo)if (!hd_map_->GetSignals(car_position, forward_distance_to_query_signals, signals)) { // 根據(jù)汽車的x, y, z坐標(biāo)，更新紅綠燈的utm坐標(biāo)到signals中，正常情況應(yīng)進(jìn)入else中，進(jìn)入if則說明獲取信號失敗，但不會終止程序運(yùn)行，而是使用最后也是最近存儲的信號代替，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，更新最后一個信號的時間戳為當(dāng)前時間戳，最后一個信號為當(dāng)前最新信號//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()
該函數(shù)看了個一知半解，主要功能我推測應(yīng)該是，獲取了汽車的gps坐標(biāo)，然后根據(jù)映射規(guī)則映射到utm坐標(biāo)。因?yàn)橹髸玫狡嚨膗tm坐標(biāo)獲取后續(xù)的其他數(shù)據(jù)。
-
接收參數(shù)
const double timestamp; // 當(dāng)前圖像幀的時間戳 camera::CarPose* pose; // 汽車類實(shí)例 -
重點(diǎn)關(guān)注變量
pose_matrix：先存儲了汽車utm坐標(biāo)，在將汽車坐標(biāo)更新到pose后，該變量又被更新為存儲映射參數(shù)矩陣，然后又更新到pose的另一個變量中，同一個變量存儲了兩種類型的數(shù)據(jù)。 -
函數(shù)體
Eigen::Matrix4d pose_matrix; // 申請一個4*4的矩陣，第一次里面存儲的是汽車的世界坐標(biāo)// get pose car(gps) to worldif (!GetPoseFromTF(timestamp, tf2_frame_id_, tf2_child_frame_id_, // child frame 其實(shí)不太懂 &pose_matrix)) {AERROR << "get pose from tf failed, child_frame_id: "<< tf2_child_frame_id_;return false;}
因此下面的代碼應(yīng)該是獲取gps坐標(biāo)，且將gps坐標(biāo)轉(zhuǎn)換成utm坐標(biāo)：然后將時間戳和汽車utm坐標(biāo)更新到汽車實(shí)例pose中：
pose->timestamp_ = timestamp;pose->pose_ = pose_matrix;接下來是相機(jī)標(biāo)定過程中的一些固參的獲取，感興趣的可以去找找相機(jī)參數(shù)標(biāo)定的文章看看，這里就不詳說了：
int state = 0; // 計(jì)數(shù)正確狀態(tài)的變量 bool ret = true; // 預(yù)設(shè)ret 為true，后面會修改Eigen::Affine3d affine3d_trans; // 申請一個仿射變換的矩陣for (const auto& camera_name : camera_names_) { // 遍歷所有相機(jī)名const auto trans_wrapper = camera2world_trans_wrapper_map_[camera_name]; // 獲取對應(yīng)相機(jī)的轉(zhuǎn)換映射指針ret = trans_wrapper->GetSensor2worldTrans(timestamp, &affine3d_trans); // 獲取傳感器到世界的轉(zhuǎn)換矩陣，返回獲取結(jié)果pose_matrix = affine3d_trans.matrix(); // 更新仿射變換矩陣到pose_matrix，這是第二次使用該變量存儲數(shù)據(jù)，第一次存儲的是汽車坐標(biāo)，已經(jīng)更新到pose中去了if (!ret) { // 判斷放射矩陣是否成功獲取，成功獲取則在else中更新相機(jī)對應(yīng)的相機(jī)到世界的參數(shù)矩陣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，使最后返回的結(jié)果為truestate += 1;}}return state > 0; // 最終判斷獲取正確狀態(tài)的數(shù)量是否大于0，返回一個bool值
GetPoseFromTF()
這個函數(shù)我就不細(xì)說了，大致應(yīng)該就是gps到utm的坐標(biāo)轉(zhuǎn)換，因?yàn)樵摵瘮?shù)接收了一個引用參數(shù)pose_matrix；在該函數(shù)內(nèi)最終這個矩陣更新為汽車的utm坐標(biāo)。具體原理沒有去了解，有興趣的讀者可以自行查閱。
GenerateTrafficLights()
該函數(shù)的主要功能就是遍歷signals中的每個signal，更新到每個trafficlight中。這里需要注意的是，每個紅綠燈帶有4個point，每個point記錄的是紅綠燈一個角的utm坐標(biāo)；每個signal代表一個紅綠燈。
-
接收參數(shù)
const std::vector<apollo::hdmap::Signal>& signals; // 存儲紅綠燈utm坐標(biāo)的signals std::vector<base::TrafficLightPtr>* traffic_lights; // 記錄紅綠燈信息的指針，在該函數(shù)中會更新 -
重點(diǎn)關(guān)注變量
light：存儲一個紅綠燈的指針，TrafficLightPtr類實(shí)例，該類中存儲了一個紅綠燈的各場景下的數(shù)據(jù)，比如說映射坐標(biāo)，ROI坐標(biāo)，檢測出的紅綠燈坐標(biāo)。具體可以看類的頭文件。 -
函數(shù)體
traffic_lights->clear(); // 清除指針for (auto signal : signals) { // for循環(huán)遍歷每個signalbase::TrafficLightPtr light; light.reset(new base::TrafficLight); // 重置指針light->id = signal.id().id(); // 更新每個signal傳遞來的對應(yīng)紅綠燈的id，每個紅綠燈是根據(jù)id區(qū)分的for (int i = 0; i < signal.boundary().point_size(); ++i) { // 開始遍歷signal中的四個point，每個點(diǎn)帶一組x, y, z, intensity，intensity具體含義沒細(xì)看base::PointXYZID point; // 每個point都是PointXYZID類型的數(shù)據(jù)結(jié)構(gòu)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); // 將存儲了數(shù)據(jù)的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(); // 這個也沒細(xì)看。到這里全部遍歷完后，該函數(shù)就完了，相應(yīng)變量已更新。}
VerifyLightsProjection()
該函數(shù)和UpdateCameraSelection函數(shù)執(zhí)行的功能在獲取pose和signals時基本沒區(qū)別，有區(qū)別的是最后一步。在該函數(shù)內(nèi)，調(diào)用的是preprocessor_的UpdateLightsProjection函數(shù)，
-
接收參數(shù)
const double& ts; // 圖像時間戳 const camera::TLPreprocessorOption& option; // 一些預(yù)處理時的選擇信息，里面包括了最終選擇的相機(jī)名稱 const std::string& camera_name; // 當(dāng)前圖像的相機(jī)名 camera::CameraFrame* frame; // 當(dāng)前圖像自帶的類實(shí)例frame_，記錄了該圖像中的各項(xiàng)信息 -
函數(shù)體
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;}
首先申請了兩個類實(shí)例pose和signals，然后調(diào)用函數(shù)QueryPoseAndSignals，獲取pose和signal的信息，并返回獲取結(jié)果是否成功的bool值：在獲取到pose和signals后，和UpdateCameraSelection函數(shù)同樣的做法，調(diào)用GenerateTrafficLights函數(shù)和預(yù)處理類下的UpdateLightsProjection函數(shù)：
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;調(diào)用完TLPreprocessor::UpdateLightsProjection后，返回相應(yīng)結(jié)果。
TLPreprocessor::UpdateLightsProjection()
-
接收參數(shù)
const CarPose &pose; // 汽車pose const TLPreprocessorOption &option; // 預(yù)處理是相關(guān)的選擇，包括選擇的相機(jī)名 const std::string &camera_name; // 當(dāng)前相機(jī)名 std::vector<base::TrafficLightPtr> *lights; // 該圖像中的紅綠燈信號指針 -
函數(shù)體
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;}
首先清除兩個指針，AINFO一些信息，然后判斷傳入的lights指針是否非空：接著開始調(diào)用映射函數(shù)TLPreprocessor::ProjectLights，將utm坐標(biāo)映射成圖像的像素坐標(biāo)：
if (!ProjectLights(pose, camera_name, lights, &lights_on_image_, // 注意這里的參數(shù)引用&lights_outside_image_)) {AERROR << "update_lights_projection project lights on " << camera_name<< " image failed";return false;}將utm坐標(biāo)映射成像素坐標(biāo)后，判斷映射結(jié)果是否有超出圖像大小：
if (lights_outside_image_.size() > 0) {AERROR << "update_lights_projection failed,"<< "lights_outside_image->size() " << lights_outside_image_.size()<< " ts: " << pose.getTimestamp();return false;}超出則報(bào)錯，返回false；未超出則繼續(xù)執(zhí)行下面的代碼。
下面的代碼首先獲取了最小焦長且正常工作的相機(jī)，然后判斷當(dāng)前相機(jī)名是否是最小焦長相機(jī)。
如果是，則返回條件“utm正確映射到像素坐標(biāo)的個數(shù)是否大于零”的bool值，意思是，盡管取了焦長最小的相機(jī)的映射結(jié)果，雖然保證了最小焦長的映射結(jié)果不會超出邊界限制，但我還是要判斷映射結(jié)果是否為空。
如果當(dāng)前相機(jī)不是最小焦長相機(jī)，那么就需要判斷當(dāng)前映射的結(jié)果是否超出了當(dāng)前相機(jī)的邊界限制。如果超出了則報(bào)錯，每超出則會返回true，映射的結(jié)果由于傳遞的是指針，因此不需要傳實(shí)際值回去：
auto min_focal_len_working_camera = GetMinFocalLenWorkingCameraName(); // 成功映射則獲取短焦相機(jī)的名字 if (camera_name == min_focal_len_working_camera) { // 判斷當(dāng)前相機(jī)名是否是短焦相機(jī)，是則判斷成功映射的坐標(biāo)是否為空，要同時滿足return lights_on_image_.size() > 0; }for (const base::TrafficLightPtr &light : lights_on_image_) { // 如果當(dāng)前相機(jī)不是短焦相機(jī)，則需要判斷每個映射結(jié)果是否超出圖像范圍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()
-
接收參數(shù)
const CarPose &pose; // 保存汽車相關(guān)信息的變量 const std::string &camera_name; // 傳來的相機(jī)名稱，因?yàn)樽詣玉{駛包括了多個攝像頭，所以必須區(qū)分不同相機(jī) std::vector<base::TrafficLightPtr> *lights; // 傳來紅綠燈信息指針，是個向量，向量每個元素就是一個紅綠燈信息 base::TrafficLightPtrs *lights_on_image; // 指向一個向量的指針，向量的每個元素都是一個紅綠燈指針，每個紅綠燈指針指向一個紅綠燈信息 base::TrafficLightPtrs *lights_outside_image; // 同上 ，只是上面保存的是映射在圖片中的紅綠燈信息，這個保存了映射在圖片外的紅綠燈信息 -
函數(shù)體
if (lights->empty()) {AINFO << "No lights to be projected";return true;}
判斷l(xiāng)ights指針是否為空。空則不需要做映射，因?yàn)榧t綠燈的檢測是階段性地啟動，在汽車進(jìn)入紅綠檢測范圍才會執(zhí)行紅綠燈檢測；非空則說明已經(jīng)通過汽車的坐標(biāo)獲取到了紅綠燈在三維世界的坐標(biāo)。判斷代碼很簡單，如下：判斷相機(jī)是否正確，相應(yīng)模塊是否啟動。HasCamera判斷相機(jī)相機(jī)名是否在相機(jī)名列表中，且相應(yīng)的模塊是否啟動，返回布爾值：
if (!projection_.HasCamera(camera_name)) {AERROR << "project_lights get invalid camera_name: " << camera_name;return false;}相機(jī)沒問題繼續(xù)執(zhí)行下面代碼。初始化布爾變量is_working，調(diào)用**GetCameraWorkingFlag()**函數(shù)執(zhí)行。注意，調(diào)用該函數(shù)時，傳進(jìn)的參數(shù)is_working是使用了引用&，也就意味著該函數(shù)執(zhí)行會改變is_working的值，然后根據(jù)返回的布爾結(jié)果判斷是否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在上面的代碼執(zhí)行后會從false變?yōu)閠rue，表示相機(jī)正常工作，繼續(xù)執(zhí)行下面的代碼：
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判斷被調(diào)函數(shù)Project結(jié)果，正常流程應(yīng)該走else，進(jìn)if則說明映射到幀上的坐標(biāo)超出了圖像大小light->region.outside_image = true;*light_proj = *light;lights_outside_image->push_back(light_proj);} else {light->region.outside_image = false; // 如果Project成功執(zhí)行，則將light指針下的region下的outside_image設(shè)為false，表示沒有超出圖片*light_proj = *light; // 傳遞正確映射的指針lights_on_image->push_back(light_proj); // 將指針推入lights_on_image向量中保存}}return true; // 返回ProjectLights函數(shù)執(zhí)行的結(jié)果，即正確執(zhí)行
-
-
GenerateTrafficLights()
GenerateTrafficLights()
GenerateTrafficLights()
GenerateTrafficLights()
GenerateTrafficLights()
GenerateTrafficLights()
GenerateTrafficLights()
總結(jié)
以上是生活随笔為你收集整理的「Apollo」百度Apollo感知模块(perception)红绿灯检测代码完整+详细解析的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。
- 上一篇： 鼎桥m40双卡怎么放？
- 下一篇： 军校和警校政审材料都交了,然后第一志愿填