Apollo感知解析之MinBox障碍物边框构建

程序员文章站 2022-04-16 10:05:07

...

参考：https://mp.weixin.qq.com/s/fsZREKxrHFQlII0VF60DBQ

Apollo感知模块具有识别障碍物和交通灯的能力，LiDAR利用点云感知，可以了解障碍物的位置、大小、类别、朝向、轨迹和速度等信息。
Apollo解决的障碍物感知问题有：
· 高精地图ROI过滤器（HDMap ROI Filter）
· 基于卷积神经网络分割（CNNSegmentation）
· MinBox障碍物边框构建（MinBoxBuilder）
· HM对象跟踪（HM Object Tracker）等
Apollo视觉感知输出的第一步是做detection（目标检测）和分割，第二步是要把结果2D-to-3D，同时还有一个tracking（追踪）的过程后，即完成了感知的主要输出步骤。
其中，tracking是做时序，2D和3D结果也会做tracking，这是无人车做感知常有的pipeline（线性模型），有这样的结果以后，就会输出被感知物体的位置和速度等信息。
在障碍物边框构建这一环节，开发者使用CNN Seg DL学习的方法，得到障碍物信息后，可使用MinBox的方法得到物体的2D框（最终边界框），结合tracking结合相机图像进行学习得到的物体边界框，则可得到物体3D的边界框。

以下是Apollo社区开发者朱炎亮在Github-Apollo-Note上分享的《如何构建MinBox障碍物边框》，感谢他为我们在MinBox Builder这一步所做的详细注解和释疑。
面对复杂多变、快速迭代的开发环境，只有开放才会带来进步，Apollo社区正在被开源的力量唤醒。

概述

对象构建器组件为检测到的障碍物建立一个边界框。因为LiDAR传感器的遮挡或距离，形成障碍物的点云可以是稀疏的，并且仅覆盖一部分表面。因此，盒构建器将恢复给定多边形点的完整边界框。即使点云稀疏，边界框的主要目的还是预估障碍物（例如，车辆）的方向。同样地，边框也用于可视化障碍物。
算法背后的想法是找到给定多边形点边缘的所有区域。在以下示例中，如果AB是边缘，则Apollo将其他多边形点投影到AB上，并建立具有最大距离的交点对，这是属于边框的边缘之一。然后直接获得边界框的另一边。通过迭代多边形中的所有边，如下图所示，Apollo确定了一个6边界边框，将选择具有最小面积的方案作为最终的边界框。

我们从代码入手，一步步解析障碍物边框构建的流程。
上一步CNN分割与后期处理，可以得到LiDAR一定区域内的障碍物集群。接下来，我们将对这些障碍物集群建立其标定框。标定框的作用除了标识物体，还有一个作用就是标记障碍物的长length、宽width、高height。其中规定长length大于宽width，障碍物方向就是长的方向direction。
MinBox构建过程如下：
· 计算障碍物2D投影(高空鸟瞰XY平面)下的多边形polygon(如下图B)。
· 根据上述多边形，计算最适边框(如下图C)。

大致的代码框架如下：

 1  /// file in apollo/modules/perception/obstacle/onboard/lidar_process_subnode.ccvoid LidarProcessSubnode::OnPointCloud(const sensor_msgs::PointCloud2& message) {  /// call hdmap to get ROI
 2    ...  /// call roi_filter
 3    ...  /// call segmentor
 4    ...  /// call object builder
 5    if (object_builder_ != nullptr) {
 6      ObjectBuilderOptions object_builder_options;    if (!object_builder_->Build(object_builder_options, &objects)) {
 7        ...
 8      }
 9    }
10  }/// file in apollo/modules/perception/obstacle/lidar/object_builder/min_box/min_box.ccbool MinBoxObjectBuilder::Build(const ObjectBuilderOptions& options, std::vector<ObjectPtr>* objects) {  for (size_t i = 0; i < objects->size(); ++i) {    if ((*objects)[i]) {      BuildObject(options, (*objects)[i]);
11      }
12    }
13  }void MinBoxObjectBuilder::BuildObject(ObjectBuilderOptions options, ObjectPtr object) {  ComputeGeometricFeature(options.ref_center, object);
14  }void MinBoxObjectBuilder::ComputeGeometricFeature(const Eigen::Vector3d& ref_ct, ObjectPtr obj) {  // step 1: compute 2D xy plane's polygen15    ComputePolygon2dxy(obj);  // step 2: construct box16    ReconstructPolygon(ref_ct, obj);
17  }

以上部分是MinBox障碍物边框构建的主题框架代码，构建的两个过程分别在 ComputePolygon2dxy 和 ReconstructPolygon 函数完成，下面我们就具体深入这两个函数，详细了解一下Apollo对障碍物构建的一个流程，和其中一些令人费解的代码段。

Step 1. MinBox构建–计算2DXY平面投影

这个阶段主要作用是障碍物集群做XY平面下的凸包多边形计算，最终得到这个多边形的一些角点。第一部分相对比较简单，没什么难点，计算凸包是调用plc库的ConvexHull组件(具体请参考pcl::ConvexHull)。
Apollo的凸包计算代码如下：

 1  /// file in apollo/modules/perception/obstacle/lidar/object_builder/min_box/min_box.ccvoid MinBoxObjectBuilder::ComputePolygon2dxy(ObjectPtr obj) {
 2    ...
 3    ConvexHull2DXY<pcl_util::Point> hull;
 4    hull.setInputCloud(pcd_xy);
 5    hull.setDimension(2);
 6    std::vector<pcl::Vertices> poly_vt;
 7    PointCloudPtr plane_hull(new PointCloud);
 8    hull.Reconstruct2dxy(plane_hull, &poly_vt);  if (poly_vt.size() == 1u) {
 9      std::vector<int> ind(poly_vt[0].vertices.begin(), poly_vt[0].vertices.end());    TransformPointCloud(plane_hull, ind, &obj->polygon);
10    } else {
11      ...
12    }
13  }/// file in apollo/modules/perception/common/convex_hullxy.htemplate <typename PointInT>class ConvexHull2DXY : public pcl::ConvexHull<PointInT> {public:14    void Reconstruct2dxy(PointCloudPtr hull, std::vector<pcl::Vertices> *polygons) {    PerformReconstruction2dxy(hull, polygons, true);
15    }  void PerformReconstruction2dxy(PointCloudPtr hull, std::vector<pcl::Vertices> *polygons, bool fill_polygon_data = false) {  
16      coordT *points = reinterpret_cast<coordT *>(calloc(indices_->size() * dimension, sizeof(coordT)));    // step1. Build input data, using appropriate projection17      int j = 0;    for (size_t i = 0; i < indices_->size(); ++i, j += dimension) {
18        points[j + 0] = static_cast<coordT>(input_->points[(*indices_)[i]].x);
19        points[j + 1] = static_cast<coordT>(input_->points[(*indices_)[i]].y);
20      }    // step2. Compute convex hull21      int exitcode = qh_new_qhull(dimension, static_cast<int>(indices_->size()), points, ismalloc, const_cast<char *>(flags), outfile, errfile);
22      std::vector<std::pair<int, Eigen::Vector4f>, Eigen::aligned_allocator<std::pair<int, Eigen::Vector4f>>> idx_points(num_vertices);
23      FORALLvertices {
24        hull->points[i] = input_->points[(*indices_)[qh_pointid(vertex->point)]];
25        idx_points[i].first = qh_pointid(vertex->point);
26        ++i;
27      }    // step3. Sort28      Eigen::Vector4f centroid;    pcl::compute3DCentroid(*hull, centroid);    for (size_t j = 0; j < hull->points.size(); j++) {
29        idx_points[j].second[0] = hull->points[j].x - centroid[0];
30        idx_points[j].second[1] = hull->points[j].y - centroid[1];
31      }    std::sort(idx_points.begin(), idx_points.end(), pcl::comparePoints2D);
32      polygons->resize(1);
33      (*polygons)[0].vertices.resize(hull->points.size());    for (int j = 0; j < static_cast<int>(hull->points.size()); j++) {
34        hull->points[j] = input_->points[(*indices_)[idx_points[j].first]];
35        (*polygons)[0].vertices[j] = static_cast<unsigned int>(j);
36      }
37    }
38  }

从以上代码的注释中，我们可以清楚地了解到这个多边形顶点的求解流程，具体函数由 PerformReconstruction2dxy 函数完成，这个函数其实跟pcl库自带的很像pcl::ConvexHull::performReconstruction2D/Line76，其实Apollo开发人员几乎将pcl库的performReconstruction2D原封不动地搬了过来，仅去掉了一些冗余的信息。

这个过程主要有：

构建输入数据，将输入的点云复制到coordT *points做处理。
计算障碍物点云的凸包，得到的结果是多边形顶点。调用qh_new_qhull函数。
顶点排序，从pcl::comparePoints2D/Line59可以看到排序是角度越大越靠前，atan2函数的结果是[-pi,pi]。所以就相当于是顺时针对顶点进行排序。

上图为计算多边形交点的流程示意图

该过程并不繁琐，这里不再深入解释每个模块。

Step 2. MinBox构建–边框构建

Apollo感知解析之MinBox障碍物边框构建

边框构建的逻辑，大致是针对过程中得到的多边形的每一条边，将剩下的所有点都投影到这条边上，就可以计算边框Box的一条边长度(最远的两个投影点距离)，同时选择距离该条边最远的点计算Box的高，这样就可以得到一个Box(上图case1-7分别是以这个多边形7条边作投影得到的7个Box)，最终选择Box面积最小的边框作为障碍物的边框。
上图中case7得到的Box面积最小，所以case7中的Box就是最终障碍物的边框。当边框确定以后，就可以得到障碍物的长度length(大边长)，宽度(小边长)，方向(大边上对应的方向)，高度（点云的平均高度，CNN分割与后期处理阶段得到）。
但是在此过程中，不得不承认有部分代码块相对难理解，而且出现了很多实际问题来优化这个过程。这里我将对这些问题一一进行解释。
根据代码，先简单地将这个过程归结为2步：

投影边长的选择(为什么要选择？因为背对lidar那一侧的点云是稀疏的，那一侧的多边形顶点是不可靠的，不用来计算Box)。
每个投影边长计算Box。

在进入正式的代码详解以前，这里有几个知识点需要了解。
假设向量a=(x0,y0)，向量b=(x1,y1)，那么有：
· （1）两个向量的点乘, a·b = x0x1 + y0y1。
· （2）计算向量a在向量b上的投影: v = a·b/(b^2)·b，投影点的坐标就是v+(b.x, b.y)。
· （3）两个向量的叉乘, axb = |a|·|b|sin(theta) = x0y1 - x1y0，叉乘方向与ab平面垂直，遵循右手法则。叉乘模大小另一层意义是: ab向量构成的平行四边形面积。
如果两个向量a，b共起点，那么axb小于0，那么a to b的逆时针夹角大于180度；等于则共线；大于0，a to b的逆时针方向夹角小于180度。
接下来正式解剖 ReconstructPolygon 构建的代码：

(1) Step1：投影边长的选择

 1  /// file in apollo/modules/perception/obstacle/lidar/object_builder/min_box/min_box.ccvoid MinBoxObjectBuilder::ReconstructPolygon(const Eigen::Vector3d& ref_ct, ObjectPtr obj) {  // compute max_point and min_point
 2    size_t max_point_index = 0;  size_t min_point_index = 0;
 3    Eigen::Vector3d p;
 4    p[0] = obj->polygon.points[0].x;
 5    p[1] = obj->polygon.points[0].y;
 6    p[2] = obj->polygon.points[0].z;
 7    Eigen::Vector3d max_point = p - ref_ct;
 8    Eigen::Vector3d min_point = p - ref_ct;  for (size_t i = 1; i < obj->polygon.points.size(); ++i) {
 9      Eigen::Vector3d p;
10      p[0] = obj->polygon.points[i].x;
11      p[1] = obj->polygon.points[i].y;
12      p[2] = obj->polygon.points[i].z;
13      Eigen::Vector3d ray = p - ref_ct;    // clock direction14      if (max_point[0] * ray[1] - ray[0] * max_point[1] < EPSILON) {
15        max_point = ray;
16        max_point_index = i;
17      }    // unclock direction18      if (min_point[0] * ray[1] - ray[0] * min_point[1] > EPSILON) {
19        min_point = ray;
20        min_point_index = i;
21      }
22    }
23  }

以上代码内容为计算min_point和max_point两个角点，该角点到底是什么？其中关于EPSILON的比较条件到底代表什么？

有一个前提我们已经在polygons多边形角点计算中可知：obj的polygon中所有角点都是顺时针按照arctan角度由大到小排序。
此过程可以通过下图了解其作用：

图中叉乘与0(EPSILON)的大小是根据前面提到的，两个向量的逆时针夹角。从上图中可以清晰地看到：max_point和min_point就代表了LiDAR能检测到障碍物的两个极端点。

 1  /// file in apollo/modules/perception/obstacle/lidar/object_builder/min_box/min_box.ccvoid MinBoxObjectBuilder::ReconstructPolygon(const Eigen::Vector3d& ref_ct, ObjectPtr obj) {  // compute max_point and min_point
 2    ...  // compute valid edge
 3    Eigen::Vector3d line = max_point - min_point;  double total_len = 0;  double max_dis = 0;  bool has_out = false;  for (size_t i = min_point_index, count = 0; count < obj->polygon.points.size(); i = (i + 1) % obj->polygon.points.size(), ++count) {    //Eigen::Vector3d p_x = obj->polygon.point[i]
 4      size_t j = (i + 1) % obj->polygon.points.size();    if (j != min_point_index && j != max_point_index) {      // Eigen::Vector3d p = obj->polygon.points[j];
 5        Eigen::Vector3d ray = p - min_point;      if (line[0] * ray[1] - ray[0] * line[1] < EPSILON) {
 6          ...
 7        }else {
 8          ...
 9        }
10      } else if ((i == min_point_index && j == max_point_index) || (i == max_point_index && j == min_point_index)) {
11        ...
12      } else if (j == min_point_index || j == max_point_index) {      // Eigen::Vector3d p = obj->polygon.points[j];13        Eigen::Vector3d ray = p_x - min_point;      if (line[0] * ray[1] - ray[0] * line[1] < EPSILON) {
14          ...
15        } else {
16          ...
17        }
18      }
19    }
20  }

当计算得到max_point和min_point后就需要执行这段代码，这段代码可能令人费解的原因为——为什么需要对每条边做一个条件筛选？

请看下图：
Apollo感知解析之MinBox障碍物边框构建
上图中，A演示了这段代码对一个汽车的点云多边形进行处理，最后的处理结果可以看到只有Edge45、Edge56、Edge67是有效的，最终会被计入total_len和max_dist。并且，相对应的边都是在line(max_point-min_point)这条分界线的一侧，同时也是靠近LiDAR这一侧。

这说明靠近LiDAR这一侧点云检测效果好，边稳定；而背离LiDAR那一侧，会因为遮挡原因，往往很难(有时候不可能)得到真正的顶点，如上图B所示。

(2) Step2：投影边长Box计算

投影边长Box计算由 ComputeAreaAlongOneEdge 函数完成，下面我们分析这个函数的代码：

 1  /// file in apollo/modules/perception/obstacle/lidar/object_builder/min_box/min_box.ccdouble MinBoxObjectBuilder::ComputeAreaAlongOneEdge(
 2    ObjectPtr obj, size_t first_in_point, Eigen::Vector3d* center,    double* lenth, double* width, Eigen::Vector3d* dir) {  // first for
 3    std::vector<Eigen::Vector3d> ns;
 4    Eigen::Vector3d v(0.0, 0.0, 0.0);      // 记录以(first_in_point,first_in_point+1)两个定点为边，所有点投影，距离这条边最远的那个点
 5    Eigen::Vector3d vn(0.0, 0.0, 0.0);     // 最远的点在(first_in_point,first_in_point+1)这条边上的投影坐标
 6    Eigen::Vector3d n(0.0, 0.0, 0.0);      // 用于临时存储
 7    double len = 0;  double wid = 0;  size_t index = (first_in_point + 1) % obj->polygon.points.size();  for (size_t i = 0; i < obj->polygon.points.size(); ++i) {    if (i != first_in_point && i != index) {      // o = obj->polygon.points[i]
 8        // a = obj->polygon.points[first_in_point]
 9        // b = obj->polygon.points[first_in_point+1]10        // 计算向量ao在ab向量上的投影，根据公式:k = ao·ab/(ab^2), 计算投影点坐标，根据公式k·ab+(ab.x, ab.y)11        double k =  ((a[0] - o[0]) * (b[0] - a[0]) + (a[1] - o[1]) * (b[1] - a[1]));
12        k = k / ((b[0] - a[0]) * (b[0] - a[0]) + (b[1] - a[1]) * (b[1] - a[1]));
13        k = k * -1;
14        n[0] = (b[0] - a[0]) * k + a[0];
15        n[1] = (b[1] - a[1]) * k + a[1];
16        n[2] = 0;      // 计算由ab作为边，o作为顶点的平行四边形的面积,利用公式|ao x ab|，叉乘的模就是四边形的面积，17        Eigen::Vector3d edge1 = o - b;
18        Eigen::Vector3d edge2 = a - b;      double height = fabs(edge1[0] * edge2[1] - edge2[0] * edge1[1]);      // 利用公式： 面积/length(ab)就是ab边上的高，即o到ab边的垂直距离， 记录最大的高19        height = height / sqrt(edge2[0] * edge2[0] + edge2[1] * edge2[1]);      if (height > wid) {
20          wid = height;
21          v = o;
22          vn = n;
23        }
24      } else {
25        ...
26      }
27      ns.push_back(n);
28    }
29  }

从以上部分代码可以看出，ComputeAreaAlongOneEdge 函数接受的输入包括多边形顶点集合，起始边first_in_point。代码将以first_in_point和first_in_point+1两个顶点构建一条边，将集合中其他点都投影到这条边上，并计算顶点距离这条边的高——也就是垂直距离。
最终的结果会保存到ns中。代码中，k的计算利用了两个向量点乘来计算投影点的性质；height的计算利用了两个向量叉乘的模等于两个向量组成的四边形面积的性质。

 1  /// file in apollo/modules/perception/obstacle/lidar/object_builder/min_box/min_box.ccdouble MinBoxObjectBuilder::ComputeAreaAlongOneEdge(  // first for
 2    ...  // second for
 3    size_t point_num1 = 0;  size_t point_num2 = 0;  // 遍历first_in_point和first_in_point+1两个点以外的，其他点的投影高，选择height最大的点，来一起组成Box
 4    // 这两个for循环是寻找ab边上相聚最远的投影点，因为要把所有点都包括到Box中，所以Box沿着ab边的边长就是最远两个点的距离，可以参考边框构建。
 5    for (size_t i = 0; i < ns.size() - 1; ++i) {
 6      Eigen::Vector3d p1 = ns[i];    for (size_t j = i + 1; j < ns.size(); ++j) {
 7        Eigen::Vector3d p2 = ns[j];      double dist = sqrt((p1[0] - p2[0]) * (p1[0] - p2[0]) + (p1[1] - p2[1]) * (p1[1] - p2[1]));      if (dist > len) {
 8          len = dist;
 9          point_num1 = i;
10          point_num2 = j;
11        }
12      }
13    }  // vp1和vp2代表了Box的ab边对边的那条边的两个顶点，分别在v的两侧，方向和ab方向一致。14    Eigen::Vector3d vp1 = v + ns[point_num1] - vn;
15    Eigen::Vector3d vp2 = v + ns[point_num2] - vn;  // 计算中心点和面积16    (*center) = (vp1 + vp2 + ns[point_num1] + ns[point_num2]) / 4;
17    (*center)[2] = obj->polygon.points[0].z;  if (len > wid) {
18      *dir = ns[point_num2] - ns[point_num1];
19    } else {
20      *dir = vp1 - ns[point_num1];
21    }
22    *lenth = len > wid ? len : wid;
23    *width = len > wid ? wid : len;  return (*lenth) * (*width);
24  }

剩下的代码就是计算Box的四个顶点坐标，以及它的面积Area。

综上所述，Box经过上述(1)(2)两个阶段，可以很清晰地得到每条有效边(靠近lidar一侧，在min_point和max_point之间)对应的Box四个顶点坐标、宽、高。最终选择Box面积最小的作为障碍物预测Box。
整个过程的代码部分相对较难理解，经过本节的学习，相信各位应该对MinBox边框的构建有了一定了解。