自动驾驶平台Apollo 3.0阅读手记：perception模块之lane post processing

背景

之前写过一篇杂文《自动驾驶平台Apollo 2.5阅读手记：perception模块之camera detector》，介绍到用于camera输入的DNN模型不仅会输出物体检测结果，还会输出车道线的语义分割结果。但要得到最终的车道线信息，还要经过后处理。项目中自带一个例子cc_lane_post_processor_test，我们就以这个例子为线索，看下它的大体实现。

首先按《自动驾驶平台Apollo 2.5环境搭建》中介绍的进入到Apollo的docker运行环境中，编译完后，执行以下命令就可以跑了：

# 加GLOG_v=4是为了输出更多信息便于分析
GLOG_v=4 ./bazel-bin/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor_test

可能会碰到下面错误：The width of input lane map does not match。这是因为modules/perception/data/cc_lane_post_processor_test目录中的lane_map.jpg是全尺寸的，而配置文件lane_post_process_config.pb.txt中指定的是缩放后的ROI区域，大小为960/384。所以只要将lane_map.jpg换成yolo_camera_detector_test输出的这个：modules/perception/data/yolo_camera_detector_test/lane_map.jpg就行。如果要将结果画在原尺寸原图上，测试程序中也要做相应修改。另外改了一些参数，包括 1) 坐标空间类型，因为这里我们的输入是图片。2) 对lane map的阀值，默认的阀值太高，一被过滤就不剩什么了。3) 默认指定的模型目录是空的，所以换了个目录。具体修改如下：

diff --git a/modules/perception/model/camera/lane_post_process_config.pb.txt b/modules/perception/model/camera/lane_post_process_config.pb.txt
index c958661..8712672 100644
--- a/modules/perception/model/camera/lane_post_process_config.pb.txt
+++ b/modules/perception/model/camera/lane_post_process_config.pb.txt
@@ -1,6 +1,6 @@
 name: "CCLanePostProcessor"
 version: "0.1.0"
-space_type: "vehicle"
+space_type: "image"
 image_width: 1920
 image_height: 1080
 roi: 0
@@ -14,7 +14,7 @@ non_mask: 1260
 non_mask: 1079
 non_mask: 660
 non_mask: 1079
-lane_map_confidence_thresh: 0.97
+lane_map_confidence_thresh: 0.5
 cc_split_siz: 50.0
 cc_split_len: 15
 min_cc_pixel_num: 50
diff --git a/modules/perception/model/yolo_camera_detector/lane.pt b/modules/perception/model/yolo_camera_detector/lane.pt
index 6ed2033..a15a1a3 100644
--- a/modules/perception/model/yolo_camera_detector/lane.pt
+++ b/modules/perception/model/yolo_camera_detector/lane.pt
@@ -1,5 +1,5 @@
 model_param {
-  model_name: "lane2d_0612"
+  model_name: "lane2d_0627"
   proto_file: "deploy_lane_high.pt"
   weight_file: "deploy_lane_high.md"
   offset_ratio: 0.28843
diff --git a/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor_test.cc b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor_test.cc
index 880d478..88292c2 100644
--- a/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor_test.cc
+++ b/modules/perception/obstacle/camera/lane_post_process/cc_lane_post_processor/cc_lane_post_processor_test.cc
@@ -96,12 +96,14 @@ TEST_F(CCLanePostProcessorTest, test_lane_post_process_vis) {
   for (int h = 0; h < lane_map.rows; ++h) {
     for (int w = 0; w < lane_map.cols; ++w) {
       if (lane_map.at<float>(h, w) >= lane_map_conf_thresh) {
-        vis_lane_objects_image.at<cv::Vec3b>(h, w)[0] =
-            static_cast<unsigned char>(lane_mask_color[0]);
-        vis_lane_objects_image.at<cv::Vec3b>(h, w)[1] =
-            static_cast<unsigned char>(lane_mask_color[1]);
-        vis_lane_objects_image.at<cv::Vec3b>(h, w)[2] =
-            static_cast<unsigned char>(lane_mask_color[2]);
+        int orig_h = h * 2 + config_.start_y_pos();
+        int orig_w = w * 2;
+        vis_lane_objects_image.at<cv::Vec3b>(orig_h, orig_w)[0] =
+          static_cast<unsigned char>(lane_mask_color[0]);
+        vis_lane_objects_image.at<cv::Vec3b>(orig_h, orig_w)[1] =
+          static_cast<unsigned char>(lane_mask_color[1]);
+        vis_lane_objects_image.at<cv::Vec3b>(orig_h, orig_w)[2] =
+          static_cast<unsigned char>(lane_mask_color[2]);
       }
     }
   }

将结果可视化后得到：

黄色为lane map结果；红色代表L_0车道线（最靠近当前车左车道线），蓝色代表R_0车道线（最靠近当前车右车道线）。判断L_0和R_0的逻辑小改了下，因为按默认的逻辑貌似会找到外边的车道线上去。

流程

车道线后处理代码主要位于perception/obstacle/camera/lane_post_process目录。测试程序代码为cc_lane_post_processor_test.cc，先看下它的大体流程。

#　读入配置文件。
GetProtoFromFile(FLAGS_cc_lane_post_processor_config_file, &config_);
...
# 初始化CCLanePostProcessor类。
cc_lane_post_processor_->Init();
...
# 读入原图test_image.jpg。
cv::Mat test_image_ori = cv::imread(input_test_image_file, CV_LOAD_IMAGE_COLOR);
# 读入车道线语义分割结果lane_map.jpg。
cv::Mat lane_map_ori = cv::imread(input_lane_map_file, CV_LOAD_IMAGE_GRAYSCALE)
...
# 将上面读入的lane map元素转为FLOAT32。
cv::Mat lane_map;
lane_map_ori.convertTo(lane_map, CV_32FC1, 1.0f / 255.0f, 0.0f);
...
# 主处理过程，结果放于lane_objects中。
cc_lane_post_processor->Process(lane_map, lane_post_process_options, &lane_objects);

# 按预定阀值(lane_map_conf_thresh)对lane map进行过滤，并可视化。
cv::Mat vis_lane_objects_image;
for (int h = 0; h < lane_map.rows; ++h)
    for (int w = 0; w < lane_map.cols, ++w) {
        if (lane_map.at<float>(h, w) >= lane_map_conf_thresh)
            ...
    }
    
# 可视化后处理得到的车道线对象。
for (size_t k = 0; k < lane_objects->size(); ++k) {
    ...
    # 画车道线标识。
    for (auto p = lane_objects->at(k).image_pos.begin(), p != lane_objects->at(k).image_pos.end(); ++p) {
            cv::circle(vis_lane_objects_image, ...)
        }
    }
    
    # 得到拟合出的多项式曲线上的点，并可视化。
    ...
    for(float l = start; l <=end; l++) {
        cv::circle(vis_lane_objects_image, ...)
    }
}

#　将可视化结果存到图片中。
cv::imwrite(lane_objects_image_file, vis_lane_objects_image);

该测试程序主要包含以下几步：

1. 调用GetProtoFromFile()函数读入config文件存于变量config_中。根据cc_lane_post_processor_config_file这个option（见perception_gflags.cc），车道线后处理模块的config文件默认在modules/perception/model/camera/lane_post_process_config.pb.txt。
2. 在测试初始化时已经创建了类型为CCLanePostProcessor的变量cc_lane_post_processor_。接下来调用其Init()函数进行初始化。
3. 接下去读入lane_map.jpg文件，并进行数据格式转化，放于变量lane_map中。这是车道线检测模型的输出结果，是一张关于车道线的语义分割图。每个像素中的值越大，代表这个点为车道线上的点的概率越大。
4. 接下去调用cc_lane_post_processor_的Process()函数，并将刚才读入的lane_map作为参数传入，输出会放在变量lane_objects中。
5. 将lane_map，以及前面的处理结果（车道线标识及拟合的多项式曲线）进行可视化。

可见主要的算法处理在CCLanePostProcessor类中的Init()和Process()两个函数中。下面分别看下这两个函数。它们的实现在文件cc_lane_post_processor.cc中。Init()函数中主要的步骤如下：

1. 读config文件，存入变量config_。然后把其中的值赋到成员变量options_中，方便后面使用。其中包括后面算法中要用到的各种阀值参数。
2. 如果坐标空间是大地坐标的话，创建Projector类，用于从图像坐标到大地坐标的转换。这个测试中最后我们想将结果画于图像上，所以这里用的图像坐标。
3. 创建ConnectedComponentGenerator类，用于产生连通区域。如果CUDA_CC为true，用的是GPU实现版本，否则是CPU实现版本。默认为CPU版本。
4. 创建LaneFrame类。这个后面再细看。

再来看下Process()函数，这里是算法的主要部分，大体流程图如下：

主要可分为以下几步：

1. 如果指定使用历史信息，调用InitLaneHistory()初始化。这个测试中只有单张图，所以这条路不执行。
2. cur_lane_instances_为LaneInstance对象的vector。调用GenerateLaneInstances()函数生成车道线实例，然后对其按size进行排序。
3. 生成LaneObject，放入变量lane_objects中。对于前面给出的每一个LaneInstance，调用AddInstanceIntoLaneObjectImage()函数将LaneInstance转为图像中的LaneObject对象。从graphs_中拿出对应的graph（即lane cluster），然后遍历其中的元素，将对应的marker信息放入LaneObject对象。然后是调用PolyFit()对LaneObject中的点进行多项式拟合。如果小于4个点的就用直线，否则用二次曲线。最后是用PolyEval()函数计算lateral distance。根据lateral distance与x中心坐标，就可以判断该车道线是左还是右车道线。
4. 调用EnrichLaneInfo()函数，对于车道线点的图像坐标进行多项式曲线拟合。不过因为这个测试中输入就是图像坐标，所以其实这里和上一步中的多项式拟合差不多。
5. 如果指定要用到历史信息话，这里会利用历史信息来修正当前车道线检测结果。

其中第二步中的GenerateLaneInstance()函数步骤稍微复杂一些，再细化看下其中主要步骤：

1. 通过对lane_map限定阀值得到二值的mask，存于变量lane_mask。
2. 通过ConnectedComponentGenerator的FindConnectedComponents()函数从lane_mask找到连通图，结果放到类型为ConnectedComponentPtr的变量cc_list中。这里用到的数据结构是并查集。里边主要是两个循环：
   1. 第一个循环从左上到右下方向遍历ROI区域。对于每一个像素点，如果为背景点，将变量frame_label_中对应位置设为-1；如果为前景点（即为车道线所在像素），分四种情况：
      1. 左边和上面没有邻接点，则在并查集中加入新的元素，代表新的连通区域。变量frame_label_的相应元素赋值为连通区域编号；变量root_map_加入元素-1。
      2. 左边有邻接点，在变量frame_label_中将左邻接点的连通区域编号赋到当前点。
      3. 上方有邻接点，在变量frame_label_中将上邻接点的连通区域编号赋到当前点。
      4. 左边和上方都有邻接点，则将连通区域编号小的赋到当前点，同时在并查集中合并这两个集合。
   2. 第二个循环，还是从左上到右下遍历ROI区域，对于每个连通区域，生成ConnectedComponent结构，放于变量cc中。前一个循环中，对于每个左边和上面没有邻接点的前景像素点，都加入了root_map_中，但其中有些区域已经在后面被连接起来了，因此，这里对于每个像素点，通过并查集的Find()函数找到其root，然后看root_map_中如果为-1，就创建新ConnectedComponent对象，放入返回变量cc中，交将连通区域序号存于root_map_中；如果不为-1，代表当前位置对应的root已经创建ConnectedComponent对象，将当前点通过AddPixel()对象放入该对象即可。
3. 找内边（靠车这侧的边）和分离轮廓。遍历上一步中找到的存于变量cc_list中的所有连通区域元素，作以下操作：
   1. 调用函数FindBboxPixels()找到边界框上的像素点。遍历该连通区域内的所有像素，找出在bbox上的那些点，将它们的index存入成员bbox_.bbox_pixel_idx中。
   2. 调用函数FindVertices()找到顶点。这里的策略是先把bbox边界上的点分别加入到left_boundary/up_boundary/right_boundary/down_boundary中，然后遍历上一步中找到的bbox上的点，结合边界信息判断是否为顶点，是的话就加入成员vertices_。
   3. 调用函数FindEdges()根据前面计算出的顶点计算出内边。
   4. 调用DetermineSplit()函数判断是否要进入split流程。当bbox对角线长度超过指定阀值（默认为50）时返回真，如高度大于5则纵向split，否则横向。如DetermineSplit()返回真，先调用FindCountourForSplit()函数找到轮廓像素。然后调用SplitContour()进行轮廓的split。
4. 联接车道线标识，确定车道线实例标签。
   1. 创建LaneFrame对象，然后调用其Init()函数初始化。根据连通区域的每条内边段创建一个Marker对象，放到成员markers_中。
   2. 调用LaneFrame的Process()函数。
      1. 先通过GreedyGroupConnectAssociation()函数做marker association。
      2. 调用ComputeBbox()函数计算graph的边界框。
      3. 创建LaneInstance对象，存入instances变量中。

其中的GreedyGroupConnectAssociation()函数主要步骤：

1. 根据连通区域产生marker组。将一个连通区域内的marker分一组，由Group类表示。
2. 遍历所有的组，估计方向。主要在Group::ComputeOrientation()函数中计算。
3. 计算marker组间的距离矩阵。距离通过Group::ComputeDistance()函数计算。这个距离会考虑几方面的因素，并进行加权。如果有两个组的距离大于0，代表可以连接。信息记录在connect_mat，edges，to_group_idx和from_group_idx结构中。
4. 将上面的的group间距离排序，从小到大进行组间的连接。
5. 生成lane cluster。对于没有可连接的组，创建Graph对象（表示lane cluster），并放入变量groups_中。然后把相连接的组加到这个Graph中。

小结

可以看到，Apollo中车道线后处理的流程还是比较长的。大体来说，首先会通过DNN得到车道线的语义分割结果，根据预定阀值转为二值图：

然后通过连通图检测和边缘检测等处理，得到车道线的内侧边缘。

接着得到Marker信息，再将这些Marker聚类分组，形成Group。这些信息会通过Graph组织起来。

之后，基于这些信息生成每条车道线对应的LaneInstance结构。最后，LaneInstance经过筛选存到LaneObject中。

这个过程相对比较长的一个原因是前面语义分割网络是对车道线所占像素进行分割，因此像虚线的各个部分在lane map中是分离的，需要在后处理中进行一系列操作将它们组合连接起来。在Apollo 3.0中加入了另一种方式，称为"Whole lane line"。看结构像有可能是位于modules/perception/model/yolo_camera_detector/lane13d_0716/目录下这个模型。看关键字可能是类似AAAI 2008上的论文《Spatial As Deep: Spatial CNN for Traffic Scene Understanding》（TuSimple车道线检测竞赛的第一名）。这种方案是在训练标注中就以整条车道线为单位，这样训练得到的网络也能输出相对完整的线，因此后处理就可以简单化很多。在Apollo的官方文档中提到，前一种方式用作视觉定位，后一种用于车道保持。