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

?

大致的代碼框架如下：

/// file in apollo/modules/perception/obstacle/onboard/lidar_process_subnode.ccvoid LidarProcessSubnode::OnPointCloud(const sensor_msgs::PointCloud2& message) { /// call hdmap to get ROI... /// call roi_filter... /// call segmentor... /// call object builderif (object_builder_ != nullptr) {ObjectBuilderOptions object_builder_options; if (!object_builder_->Build(object_builder_options, &objects)) {...}}}/// 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]);}}}void MinBoxObjectBuilder::BuildObject(ObjectBuilderOptions options, ObjectPtr object) { ComputeGeometricFeature(options.ref_center, object);}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);}

以上部分是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原封不動地搬了過來，僅去掉了一些冗余的信息。

這個過程主要有：

1. 構建輸入數據，將輸入的點云復制到coordT *points做處理。

2. 計算障礙物點云的凸包，得到的結果是多邊形頂點。調用qh_new_qhull函數。

3. 頂點排序，從pcl::comparePoints2D/Line59

可以看到排序是角度越大越靠前，atan2函數的結果是[-pi,pi]。所以就相當于是順 時針對頂點進行排序。

上圖為計算多邊形交點的流程示意圖

該過程并不繁瑣，這里不再深入解釋每個模塊。

?

Step 2. MinBox構建--邊框構建

?

?

邊框構建的邏輯，大致是針對過程中得到的多邊形的每一條邊。

將剩下的所有點都投影到這條邊上，就可以計算邊框Box的一條邊長度(最遠的兩個投影點距離)；

同時選擇距離該條邊最遠的點計算Box的高，這樣就可以得到一個Box(上圖case1-7分別是以這個多邊形7條邊作投影得到的7個Box)，最終選擇Box面積最小的邊框作為障礙物的邊框。

?

上圖中case7得到的Box面積最小，所以case7中的Box就是最終障礙物的邊框。當邊框確定以后，就可以得到障礙物的長度length(大邊長)，寬度(小邊長)，方向(大邊上對應的方向)，高度（點云的平均高度，CNN分割與后期處理階段得到）。

但是在此過程中，不得不承認有部分代碼塊相對難理解，而且出現了很多實際問題來優化這個過程。這里我將對這些問題一一進行解釋。

?

根據代碼，先簡單地將這個過程歸結為2步：

1. 投影邊長的選擇(為什么要選擇？因為背對lidar那一側的點云是稀疏的，那一側的多邊形頂點是不可靠的，不用來計算Box)。

2. 每個投影邊長計算Box。

?

?

在進入正式的代碼詳解以前，這里有幾個知識點需要了解。

假設向量a=(x0,y0)，向量b=(x1,y1)，那么有：

·?兩個向量的點乘, a·b = x0x1 + y0y1\。

·?計算向量a在向量b上的投影: v = a·b/(b^2)·b，投影點的坐標就是v+(b.x, b.y)。

·?兩個向量的叉乘, 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_point2 ? ?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_point2 ? ?... ?// compute valid edge3 ? ?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后就需要執行這段代碼，這段代碼可能令人費解的原因為——為什么需要對每條邊做一個條件篩選？

?

請看下圖：

?

上圖中，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 for3 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 for2 ... // second for3 size_t point_num1 = 0; size_t point_num2 = 0; // 遍歷first_in_point和first_in_point+1兩個點以外的，其他點的投影高，選擇height最大的點，來一起組成Box4 // 這兩個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邊框的構建有了一定了解。

?

希望對你有幫助。

總結

以上是生活随笔為你收集整理的Apollo感知解析之MinBox障碍物边框构建的全部內容，希望文章能夠幫你解決所遇到的問題。

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