關(guān)于MinBox障礙物邊框構(gòu)建的干貨！

Apollo感知模塊具有識別障礙物和交通燈的能力，LiDAR利用點(diǎn)云感知，可以了解障礙物的位置、大小、類別、朝向、軌跡和速度等信息。

Apollo解決的障礙物感知問題有：

·高精地圖ROI過濾器（HDMap ROI Filter）

·基于卷積神經(jīng)網(wǎng)絡(luò)分割（CNNSegmentation）

·MinBox障礙物邊框構(gòu)建（MinBoxBuilder）

·HM對象跟蹤（HM Object Tracker）等

Apollo視覺感知輸出的第一步是做detection（目標(biāo)檢測）和分割，第二步是要把結(jié)果2D-to-3D，同時(shí)還有一個(gè)tracking（追蹤）的過程后，即完成了感知的主要輸出步驟。

其中，tracking是做時(shí)序，2D和3D結(jié)果也會(huì)做tracking，這是無人車做感知常有的pipeline（線性模型），有這樣的結(jié)果以后，就會(huì)輸出被感知物體的位置和速度等信息。

在障礙物邊框構(gòu)建這一環(huán)節(jié)，開發(fā)者使用CNN Seg DL學(xué)習(xí)的方法，得到障礙物信息后，可使用MinBox的方法得到物體的2D框（最終邊界框），結(jié)合tracking結(jié)合相機(jī)圖像進(jìn)行學(xué)習(xí)得到的物體邊界框，則可得到物體3D的邊界框。

以下是Apollo社區(qū)開發(fā)者朱炎亮在Github-Apollo-Note上分享的《如何構(gòu)建MinBox障礙物邊框》，感謝他為我們在MinBox Builder這一步所做的詳細(xì)注解和釋疑。

面對復(fù)雜多變、快速迭代的開發(fā)環(huán)境，只有開放才會(huì)帶來進(jìn)步，Apollo社區(qū)正在被開源的力量喚醒。

對象構(gòu)建器組件為檢測到的障礙物建立一個(gè)邊界框。因?yàn)長iDAR傳感器的遮擋或距離，形成障礙物的點(diǎn)云可以是稀疏的，并且僅覆蓋一部分表面。因此，盒構(gòu)建器將恢復(fù)給定多邊形點(diǎn)的完整邊界框。即使點(diǎn)云稀疏，邊界框的主要目的還是預(yù)估障礙物（例如，車輛）的方向。同樣地，邊框也用于可視化障礙物。

算法背后的想法是找到給定多邊形點(diǎn)邊緣的所有區(qū)域。在以下示例中，如果AB是邊緣，則Apollo將其他多邊形點(diǎn)投影到AB上，并建立具有最大距離的交點(diǎn)對，這是屬于邊框的邊緣之一。然后直接獲得邊界框的另一邊。通過迭代多邊形中的所有邊，如下圖所示，Apollo確定了一個(gè)6邊界邊框，將選擇具有最小面積的方案作為最終的邊界框。

我們從代碼入手，一步步解析障礙物邊框構(gòu)建的流程。

上一步CNN分割與后期處理，可以得到LiDAR一定區(qū)域內(nèi)的障礙物集群。接下來，我們將對這些障礙物集群建立其標(biāo)定框。標(biāo)定框的作用除了標(biāo)識物體，還有一個(gè)作用就是標(biāo)記障礙物的長length、寬width、高h(yuǎn)eight。其中規(guī)定長length大于寬width，障礙物方向就是長的方向direction。

MinBox構(gòu)建過程如下：

·計(jì)算障礙物2D投影(高空鳥瞰XY平面)下的多邊形polygon(如下圖B)。

·根據(jù)上述多邊形，計(jì)算最適邊框(如下圖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* 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障礙物邊框構(gòu)建的主題框架代碼，構(gòu)建的兩個(gè)過程分別在ComputePolygon2dxy

和ReconstructPolygon函數(shù)完成，下面我們就具體深入這兩個(gè)函數(shù)，詳細(xì)了解一下Apollo對障礙物構(gòu)建的一個(gè)流程，和其中一些令人費(fèi)解的代碼段。

Step 1. MinBox構(gòu)建--計(jì)算2DXY平面投影

這個(gè)階段主要作用是障礙物集群做XY平面下的凸包多邊形計(jì)算，最終得到這個(gè)多邊形的一些角點(diǎn)。第一部分相對比較簡單，沒什么難點(diǎn)，計(jì)算凸包是調(diào)用plc庫的ConvexHull組件(具體請參考pcl::ConvexHull)。

Apollo的凸包計(jì)算代碼如下：

1 /// file in apollo/modules/perception/obstacle/lidar/object_builder/min_box/min_box.ccvoid MinBoxObjectBuilder::ComputePolygon2dxy(ObjectPtr obj) { 2 ... 3 ConvexHull2DXY hull; 4 hull.setInputCloud(pcd_xy); 5 hull.setDimension(2); 6 std::vector poly_vt; 7 PointCloudPtr plane_hull(new PointCloud); 8 hull.Reconstruct2dxy(plane_hull, &poly_vt); if (poly_vt.size() == 1u) { 9 std::vector 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 class ConvexHull2DXY : public pcl::ConvexHull {public:14 void Reconstruct2dxy(PointCloudPtr hull, std::vector *polygons) { PerformReconstruction2dxy(hull, polygons, true); 15 } void PerformReconstruction2dxy(PointCloudPtr hull, std::vector *polygons, bool fill_polygon_data = false) { 16 coordT *points = reinterpret_cast(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(input_->points[(*indices_)[i]].x); 19 points[j + 1] = static_cast(input_->points[(*indices_)[i]].y); 20 } // step2. Compute convex hull21 int exitcode = qh_new_qhull(dimension, static_cast(indices_->size()), points, ismalloc, const_cast(flags), outfile, errfile); 22 std::vector, Eigen::aligned_allocator>> 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(hull->points.size()); j++) { 34 hull->points[j] = input_->points[(*indices_)[idx_points[j].first]]; 35 (*polygons)[0].vertices[j] = static_cast(j); 36 } 37 } 38 }

從以上代碼的注釋中，我們可以清楚地了解到這個(gè)多邊形頂點(diǎn)的求解流程，具體函數(shù)由PerformReconstruction2dxy函數(shù)完成，這個(gè)函數(shù)其實(shí)跟pcl庫自帶的很像pcl::ConvexHull::performReconstruction2D/Line76，其實(shí)Apollo開發(fā)人員幾乎將pcl庫的performReconstruction2D原封不動(dòng)地搬了過來，僅去掉了一些冗余的信息。

這個(gè)過程主要有：

1. 構(gòu)建輸入數(shù)據(jù)，將輸入的點(diǎn)云復(fù)制到coordT *points做處理。

2. 計(jì)算障礙物點(diǎn)云的凸包，得到的結(jié)果是多邊形頂點(diǎn)。調(diào)用qh_new_qhull函數(shù)。

3. 頂點(diǎn)排序，從pcl::comparePoints2D/Line59

可以看到排序是角度越大越靠前，atan2函數(shù)的結(jié)果是[-pi,pi]。所以就相當(dāng)于是順 時(shí)針對頂點(diǎn)進(jìn)行排序。

上圖為計(jì)算多邊形交點(diǎn)的流程示意圖

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

Step 2. MinBox構(gòu)建--邊框構(gòu)建

邊框構(gòu)建的邏輯，大致是針對過程中得到的多邊形的每一條邊。將剩下的所有點(diǎn)都投影到這條邊上，就可以計(jì)算邊框Box的一條邊長度(最遠(yuǎn)的兩個(gè)投影點(diǎn)距離)，同時(shí)選擇距離該條邊最遠(yuǎn)的點(diǎn)計(jì)算Box的高，這樣就可以得到一個(gè)Box(上圖case1-7分別是以這個(gè)多邊形7條邊作投影得到的7個(gè)Box)，最終選擇Box面積最小的邊框作為障礙物的邊框。

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

但是在此過程中，不得不承認(rèn)有部分代碼塊相對難理解，而且出現(xiàn)了很多實(shí)際問題來優(yōu)化這個(gè)過程。這里我將對這些問題一一進(jìn)行解釋。

根據(jù)代碼，先簡單地將這個(gè)過程歸結(jié)為2步：

1. 投影邊長的選擇(為什么要選擇？因?yàn)楸硨idar那一側(cè)的點(diǎn)云是稀疏的，那一側(cè)的多邊形頂點(diǎn)是不可靠的，不用來計(jì)算Box)。

2. 每個(gè)投影邊長計(jì)算Box。

在進(jìn)入正式的代碼詳解以前，這里有幾個(gè)知識點(diǎn)需要了解。

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

·兩個(gè)向量的點(diǎn)乘, a·b = x0x1 + y0y1\。

·計(jì)算向量a在向量b上的投影: v = a·b/(b^2)·b，投影點(diǎn)的坐標(biāo)就是v+(b.x, b.y)。

·兩個(gè)向量的叉乘, axb = |a|·|b|sin(theta) = x0y1 - x1y0，叉乘方向與ab平面垂直，遵循右手法則。叉乘模大小另一層意義是: ab向量構(gòu)成的平行四邊形面積。

如果兩個(gè)向量a，b共起點(diǎn)，那么axb小于0，那么a to b的逆時(shí)針夾角大于180度；等于則共線；大于0，a to b的逆時(shí)針方向夾角小于180度。

接下來正式解剖

ReconstructPolygon構(gòu)建的代碼：

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

以上代碼內(nèi)容為計(jì)算min_point和max_point兩個(gè)角點(diǎn)，該角點(diǎn)到底是什么？其中關(guān)于EPSILON的比較條件到底代表什么？

有一個(gè)前提我們已經(jīng)在polygons多邊形角點(diǎn)計(jì)算中可知：obj的polygon中所有角點(diǎn)都是順時(shí)針按照arctan角度由大到小排序。

此過程可以通過下圖了解其作用：

圖中叉乘與0(EPSILON)的大小是根據(jù)前面提到的，兩個(gè)向量的逆時(shí)針夾角。從上圖中可以清晰地看到：max_point和min_point就代表了LiDAR能檢測到障礙物的兩個(gè)極端點(diǎn)。

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 ?}

當(dāng)計(jì)算得到max_point和min_point后就需要執(zhí)行這段代碼，這段代碼可能令人費(fèi)解的原因?yàn)椤獮槭裁葱枰獙γ織l邊做一個(gè)條件篩選？

請看下圖：

上圖中，A演示了這段代碼對一個(gè)汽車的點(diǎn)云多邊形進(jìn)行處理，最后的處理結(jié)果可以看到只有Edge45、Edge56、Edge67是有效的，最終會(huì)被計(jì)入total_len和max_dist。并且，相對應(yīng)的邊都是在line(max_point-min_point)這條分界線的一側(cè)，同時(shí)也是靠近LiDAR這一側(cè)。

這說明靠近LiDAR這一側(cè)點(diǎn)云檢測效果好，邊穩(wěn)定；而背離LiDAR那一側(cè)，會(huì)因?yàn)檎趽踉颍茈y(有時(shí)候不可能)得到真正的頂點(diǎn)，如上圖B所示。

(2) Step2：投影邊長Box計(jì)算

投影邊長Box計(jì)算由ComputeAreaAlongOneEdge函數(shù)完成，下面我們分析這個(gè)函數(shù)的代碼：

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 ns; 4 Eigen::Vector3d v(0.0, 0.0, 0.0); // 記錄以(first_in_point,first_in_point+1)兩個(gè)定點(diǎn)為邊，所有點(diǎn)投影，距離這條邊最遠(yuǎn)的那個(gè)點(diǎn) 5 Eigen::Vector3d vn(0.0, 0.0, 0.0); // 最遠(yuǎn)的點(diǎn)在(first_in_point,first_in_point+1)這條邊上的投影坐標(biāo) 6 Eigen::Vector3d n(0.0, 0.0, 0.0); // 用于臨時(shí)存儲(chǔ) 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 // 計(jì)算向量ao在ab向量上的投影，根據(jù)公式:k = ao·ab/(ab^2), 計(jì)算投影點(diǎn)坐標(biāo)，根據(jù)公式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; // 計(jì)算由ab作為邊，o作為頂點(diǎn)的平行四邊形的面積,利用公式|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函數(shù)接受的輸入包括多邊形頂點(diǎn)集合，起始邊f(xié)irst_in_point。代碼將以first_in_point和first_in_point+1兩個(gè)頂點(diǎn)構(gòu)建一條邊，將集合中其他點(diǎn)都投影到這條邊上，并計(jì)算頂點(diǎn)距離這條邊的高——也就是垂直距離。

最終的結(jié)果會(huì)保存到ns中。代碼中，k的計(jì)算利用了兩個(gè)向量點(diǎn)乘來計(jì)算投影點(diǎn)的性質(zhì)；height的計(jì)算利用了兩個(gè)向量叉乘的模等于兩個(gè)向量組成的四邊形面積的性質(zhì)。

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兩個(gè)點(diǎn)以外的，其他點(diǎn)的投影高，選擇height最大的點(diǎn)，來一起組成Box 4 // 這兩個(gè)for循環(huán)是尋找ab邊上相聚最遠(yuǎn)的投影點(diǎn)，因?yàn)橐阉悬c(diǎn)都包括到Box中，所以Box沿著ab邊的邊長就是最遠(yuǎn)兩個(gè)點(diǎn)的距離，可以參考邊框構(gòu)建。 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邊對邊的那條邊的兩個(gè)頂點(diǎn)，分別在v的兩側(cè)，方向和ab方向一致。14 Eigen::Vector3d vp1 = v + ns[point_num1] - vn; 15 Eigen::Vector3d vp2 = v + ns[point_num2] - vn; // 計(jì)算中心點(diǎn)和面積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 }

剩下的代碼就是計(jì)算Box的四個(gè)頂點(diǎn)坐標(biāo)，以及它的面積Area。

綜上所述，Box經(jīng)過上述(1)(2)兩個(gè)階段，可以很清晰地得到每條有效邊(靠近lidar一側(cè)，在min_point和max_point之間)對應(yīng)的Box四個(gè)頂點(diǎn)坐標(biāo)、寬、高。最終選擇Box面積最小的作為障礙物預(yù)測Box。

整個(gè)過程的代碼部分相對較難理解，經(jīng)過本節(jié)的學(xué)習(xí)，相信各位應(yīng)該對MinBox邊框的構(gòu)建有了一定了解。

自Apollo平臺(tái)開放已來，我們收到了大量開發(fā)者的咨詢和反饋，越來越多開發(fā)者基于Apollo擦出了更多的火花，并愿意將自己的成果貢獻(xiàn)出來，這充分體現(xiàn)了Apollo『貢獻(xiàn)越多，獲得越多』的開源精神。為此我們開設(shè)了『開發(fā)者說』板塊，希望開發(fā)者們能夠踴躍投稿，更好地為廣大自動(dòng)駕駛開發(fā)者營造一個(gè)共享交流的平臺(tái)！