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

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