分成7步對 Lidar 的流程進(jìn)行敘述：

1.坐標(biāo)及格式轉(zhuǎn)換

Apollo 使用了開源庫Eigen?進(jìn)行高效的矩陣計(jì)算，使用了PCL點(diǎn)云庫對點(diǎn)云進(jìn)行處理。

該部分中，Apollo 首先計(jì)算轉(zhuǎn)換矩陣 velodyne_trans，用于將 Velodyne 坐標(biāo)轉(zhuǎn)化為世界坐標(biāo)。之后將 Velodyne 點(diǎn)云轉(zhuǎn)為 PCL 點(diǎn)云庫格式，便于之后的計(jì)算。

2.獲取ROI區(qū)域

核心函數(shù)位置:obstacle/onboard/hdmap_input.cc

bool HDMapInput::GetROI(const PointD& pointd, const double& map_radius,

? ? ? ? ? ? ? ? ? ? ? ? HdmapStructPtr* mapptr);

查詢 HDmap, 根據(jù) Velodyne 的世界坐標(biāo)以及預(yù)設(shè)的半徑 (FLAG_map_radius) 來獲取 ROI 區(qū)域。

首先獲取指定范圍內(nèi)的道路以及道路交叉點(diǎn)的邊界，將兩者進(jìn)行融合后的結(jié)果存入 ROI 多邊形中。該區(qū)域中所有的點(diǎn)都位于世界坐標(biāo)系。

3.調(diào)用ROI過濾器

核心函數(shù)位置：obstacle/lidar/roi_filter/hdmap_roi_filter/hdmap_roi_filter.cc

boolHdmapROIFilter::Filter(constpcl_util::PointCloudPtr&cloud,constROIFilterOptions&roi_filter_options,pcl_util::PointIndices*roi_indices);

官方文檔的描述如下：

高精地圖 ROI 過濾器（往下簡稱“過濾器”）處理在ROI之外的激光雷達(dá)點(diǎn)，去除背景對象，如路邊建筑物和樹木等，剩余的點(diǎn)云留待后續(xù)處理。

一般來說，Apollo 高精地圖 ROI過濾器有以下三步：

1. 坐標(biāo)轉(zhuǎn)換

2. ROI LUT構(gòu)造

3. ROI LUT點(diǎn)查詢

ROI顯示查找表。藍(lán)色線條標(biāo)出了高精地圖ROI的邊界，包含路表與路口。紅色加粗點(diǎn)表示對應(yīng)于激光雷達(dá)傳感器位置的地方坐標(biāo)系原始位置。2D網(wǎng)格由8*8個綠色正方形組成，在ROI中的單元格，為藍(lán)色填充的正方形，而之外的是黃色填充的正方形。

ROI 過濾器部分涉及到了掃描線法和位圖編碼兩個技術(shù)。具體來看，該部分分以下幾步：

a.坐標(biāo)轉(zhuǎn)換

將地圖ROI多邊形和點(diǎn)云轉(zhuǎn)換至激光雷達(dá)傳感器位置的地方坐標(biāo)系。

b.確定地圖多邊形主方向

比較所有點(diǎn)的 x、y 方向的區(qū)間范圍，取區(qū)間范圍較小的方向?yàn)橹鞣较?。并將地圖多邊形 (map_polygons) 轉(zhuǎn)換為待加工的多邊形 (raw polygons)。

c.建立位圖

將 raw polygons 轉(zhuǎn)化為位圖 (bitmap)?中的格點(diǎn)，位圖有以下特點(diǎn)：

? ? ? ? ? ? 位圖范圍, 以 Lidar 為原點(diǎn)的一片區(qū)域 (-range, range)*(-range, range) 內(nèi)，range 默認(rèn) 70米

位圖用于以格點(diǎn) (grid)的方式存儲 ROI 信息。若某格點(diǎn)值為真，代表此格點(diǎn)屬于 ROI。

? ? ? ? ? ? 默認(rèn)的格點(diǎn)大小為 cell_size 0.25米。

? ? ? ? ? ? 在列方向上，1bit 代表 1grid。為了加速操作，Apollo 使用 uint64_t 來一次操縱64個grids。

為了在位圖中畫出一個多邊形，以下3個步驟需要被完成

i. 獲得主方向有效范圍

ii.將多邊形轉(zhuǎn)換為掃描線法所需的掃描間隔：將多變形在主方向上分解為線(多邊形->片段->線），計(jì)算每條線的掃描間隔。

iii. 基于掃描間隔在位圖中畫格點(diǎn)

位圖中畫多邊形流程

d.ROI點(diǎn)檢測

通過檢查 grid 的值，確定在位圖中得每一個 grid 是否屬于 ROI。

4.調(diào)用分割器（segmentor）

入口函數(shù)所在文件cnn_segmentation.cc

boolCNNSegmentation::Segment(constpcl_util::PointCloudPtr&pc_ptr,constpcl_util::PointIndices&valid_indices,constSegmentationOptions&options,vector<ObjectPtr>*objects)

分割器采用了caffe框架的深度完全卷積神經(jīng)網(wǎng)絡(luò)（FCNN）對障礙物進(jìn)行分割，有以下四步：

a.通道特征提取

計(jì)算以 Lidar 傳感器某一范圍內(nèi)的各個單元格 (grid) 中與點(diǎn)有關(guān)的8個統(tǒng)計(jì)量，將其作為通道特征（channel feature）輸入到 FCNN。

1. 單元格中點(diǎn)的最大高度

2. 單元格中最高點(diǎn)的強(qiáng)度

3. 單元格中點(diǎn)的平均高度

4. 單元格中點(diǎn)的平均強(qiáng)度

5. 單元格中的點(diǎn)數(shù)

6. 單元格中心相對于原點(diǎn)的角度

7. 單元格中心與原點(diǎn)之間的距離

8. 二進(jìn)制值標(biāo)示單元格是空還是被占用如

計(jì)算時默認(rèn)只使用 ROI 區(qū)域內(nèi)的點(diǎn)，也可使用整個 Lidar 范圍內(nèi)的點(diǎn)，使用標(biāo)志位 use_full_cloud_ 作為開關(guān)。

b.基于卷積神經(jīng)網(wǎng)絡(luò)的障礙物預(yù)測

工作原理如下：完全卷積神經(jīng)網(wǎng)絡(luò)由三層構(gòu)成：下游編碼層（特征編碼器）、上游解碼層（特征解碼器）、障礙物屬性預(yù)測層（預(yù)測器）

特征編碼器將通道特征圖像作為輸入，并且隨著特征抽取的增加而連續(xù)下采樣其空間分辨率。 然后特征解碼器逐漸對特征圖像上采樣到輸入2D網(wǎng)格的空間分辨率，可以恢復(fù)特征圖像的空間細(xì)節(jié)，以促進(jìn)單元格方向的障礙物位置、速度屬性預(yù)測。 根據(jù)具有非線性激活（即ReLu）層的堆疊卷積/分散層來實(shí)現(xiàn)下采樣和上采樣操作。

FCNN在單元格方向上的障礙物預(yù)測

基于 FCNN 的預(yù)測，Apollo 獲取了每個單元格的四個預(yù)測信息，分別用于之后的障礙物聚類和后期處理。

FCNN 預(yù)測結(jié)果及用途

c.障礙物集群（cluster2D）

核心函數(shù)位置obstacle/lidar/segmentation/cnnseg/cluster2d.h

voidCluster(constcaffe::Blob<float>&category_pt_blob,

constcaffe::Blob<float>&instance_pt_blob,

constapollo::perception::pcl_util::PointCloudPtr&pc_ptr,

constapollo::perception::pcl_util::PointIndices&valid_indices,

floatobjectness_thresh,booluse_all_grids_for_clustering);

Apollo基于單元格中心偏移預(yù)測構(gòu)建有向圖，采用壓縮的聯(lián)合查找算法（Union Find algorithm ）基于對象性預(yù)測有效查找連接組件，構(gòu)建障礙物集群。

need-to-insert-img

障礙聚類

（a）紅色箭頭表示每個單元格對象中心偏移預(yù)測；藍(lán)色填充對應(yīng)于物體概率不小于0.5的對象單元。

（b）固體紅色多邊形內(nèi)的單元格組成候選對象集群。

d.后期處理

涉及的函數(shù)obstacle/lidar/segmentation/cnnseg/cluster2d.h

voidFilter(constcaffe::Blob<float>&confidence_pt_blob,

constcaffe::Blob<float>&height_pt_blob);

voidClassify(constcaffe::Blob<float>&classify_pt_blob);

voidGetObjects(constfloatconfidence_thresh,constfloatheight_thresh,

constintmin_pts_num,std::vector<ObjectPtr>*objects);

聚類后，Apollo獲得一組包括若干單元格的候選對象集，每個候選對象集包括若干單元格。根據(jù)每個候選群體的檢測置信度分?jǐn)?shù)和物體高度，來確定最終輸出的障礙物集/分段。

從代碼中可以看到 CNN分割器最終識別的物體類型有三種：小機(jī)動車、大機(jī)動車、非機(jī)動車和行人。

在obstacle/lidar/segmentation/cnnseg/cluster2d.h中

enumMetaType{

META_UNKNOWN,

META_SMALLMOT,

META_BIGMOT,

META_NONMOT,

META_PEDESTRIAN,

MAX_META_TYPE

};

5.障礙物邊框構(gòu)架

void BuildObject(ObjectBuilderOptions options, ObjectPtr object)

邊界框的主要目的還是預(yù)估障礙物（例如，車輛）的方向。同樣地，邊框也用于可視化障礙物。

如圖，Apollo確定了一個6邊界邊框，將選擇具有最小面積的方案作為最終的邊界框

need-to-insert-img

minbox對象構(gòu)建

6.障礙物追蹤

入口函數(shù)位置：obstacle/lidar/tracker/hm_tracker/hm_tracker.cc

// @brief track detected objects over consecutive frames

// @params[IN] objects: recently detected objects

// @params[IN] timestamp: timestamp of recently detected objects

// @params[IN] options: tracker options with necessary information

// @params[OUT] tracked_objects: tracked objects with tracking information

// @return true if track successfully, otherwise return false

boolTrack(conststd::vector<ObjectPtr>&objects,doubletimestamp,

constTrackerOptions&options,

std::vector<ObjectPtr>*tracked_objects);

障礙物追蹤可分兩大部分，即數(shù)據(jù)關(guān)聯(lián)和跟蹤動態(tài)預(yù)估。Apollo 使用了名為HM tracker的對象跟蹤器。實(shí)現(xiàn)原理：

在HM對象跟蹤器中，匈牙利算法(Hungarian algorithm)用于檢測到跟蹤關(guān)聯(lián)，并采用魯棒卡爾曼濾波器(Robust Kalman Filter) 進(jìn)行運(yùn)動估計(jì)。

數(shù)據(jù)關(guān)聯(lián)

數(shù)據(jù)關(guān)聯(lián)的過程是確定傳感器接收到的量測信息和目標(biāo)源對應(yīng)關(guān)系的過程，是多傳感多目標(biāo)跟蹤系統(tǒng)最核心且最重要的過程 [11]。

Apollo 首先建立關(guān)聯(lián)距離矩陣，用于計(jì)算每個對象(object ) 和 每個軌跡?(track )之間的關(guān)聯(lián)距離。之后使用?匈牙利算法?為 object和 track 進(jìn)行最優(yōu)分配。

計(jì)算關(guān)聯(lián)距離時，Apollo 考慮了以下5個關(guān)聯(lián)特征，來評估 object 和 track 的運(yùn)動及外觀一致性，并為其分配了不同的權(quán)重。

need-to-insert-img

由上表可以看出，Apollo 在計(jì)算關(guān)聯(lián)距離時，重點(diǎn)考慮的還是幾何距離和兩者的形狀相似度。計(jì)算得到類似下圖的關(guān)聯(lián)距離矩陣后，使用匈牙利算法將 Object 與 Track 做匹配。

need-to-insert-img

關(guān)聯(lián)距離矩陣實(shí)例

跟蹤動態(tài)預(yù)估（Track motion Estimation）

使用卡爾曼濾波來對 track 的狀態(tài)進(jìn)行估計(jì)，使用魯棒統(tǒng)計(jì)技術(shù)來剔除異常數(shù)據(jù)帶來的影響。

不了解卡爾曼濾波原理的同學(xué)請參考：卡爾曼濾波器的原理以及在matlab中的實(shí)現(xiàn)[13]。這一部分的濾波整體看來是一個標(biāo)準(zhǔn)的卡爾曼濾波。在此基礎(chǔ)上，Apollo 團(tuán)隊(duì)加入了一些修改，根據(jù)官方文檔，Apollo 的跟蹤動態(tài)預(yù)估有以下三個亮點(diǎn) :

a.觀察冗余

在一系列重復(fù)觀測中選擇速度測量，即濾波算法的輸入，包括錨點(diǎn)移位、邊界框中心偏移、邊界框角點(diǎn)移位等。冗余觀測將為濾波測量帶來額外的魯棒性， 因?yàn)樗杏^察失敗的概率遠(yuǎn)遠(yuǎn)小于單次觀察失敗的概率。

卡爾曼更新的觀測值為速度。每次觀測三個速度值?:

錨點(diǎn)移位速度、邊界框中心偏移速度 和 邊界框角點(diǎn)位移速度。

從三個速度中，根據(jù)運(yùn)動的一致性，選出與之前觀測速度偏差最小的速度為最終的觀測值。

根據(jù)最近3次的速度觀測值，計(jì)算出加速度的觀測值。

b.分解

高斯濾波算法 （Gaussian Filter algorithms）總是假設(shè)它們的高斯分布產(chǎn)生噪聲。 然而，這種假設(shè)可能在運(yùn)動預(yù)估問題中失敗，因?yàn)槠錅y量的噪聲可能來自直方分布。 為了克服更新增益的過度估計(jì)，在過濾過程中使用故障閾值。

這里的故障閾值應(yīng)該對應(yīng)著程序中的 breakdown_threshold_。

該參數(shù)被用于以下兩個函數(shù)中，當(dāng)更新的增益過大時，它被用來克服增益的過度估計(jì)：

KalmanFilter::UpdateVelocity

KalmanFilter::UpdateAcceleration

兩者的區(qū)別在于：

速度的故障閾值是動態(tài)計(jì)算的，與速度誤差協(xié)方差矩陣有關(guān)

velocity_gain*=breakdown_threshold_;

加速度的故障閾值是定值，默認(rèn)為2

acceleration_gain *= breakdown_threshold;

c.更新關(guān)聯(lián)質(zhì)量（UpdateQuality）？？

原始卡爾曼濾波器更新其狀態(tài)不區(qū)分其測量的質(zhì)量。 然而，質(zhì)量是濾波噪聲的有益提示，可以估計(jì)。 例如，在關(guān)聯(lián)步驟中計(jì)算的距離可以是一個合理的測量質(zhì)量估計(jì)。 根據(jù)關(guān)聯(lián)質(zhì)量更新過濾算法的狀態(tài)，增強(qiáng)了運(yùn)動估計(jì)問題的魯棒性和平滑度

更新關(guān)聯(lián)質(zhì)量 update_quality 默認(rèn)為 1.0，當(dāng)開啟適應(yīng)功能時 (s_use_adaptive ==true）Apollo 使用以下兩種策略來計(jì)算更新關(guān)聯(lián)質(zhì)量：

1. 根據(jù) object 自身的屬性 — 關(guān)聯(lián)分?jǐn)?shù) (association_score) 來計(jì)算

2. 根據(jù)新舊兩個 object 點(diǎn)云數(shù)量的變化

首先根據(jù)這兩種策略分別計(jì)算更新關(guān)聯(lián)質(zhì)量，之后取得分小的結(jié)果來控制濾波器噪聲。

7.障礙物類型融合

入口函數(shù)位置：obstacle/lidar/type_fuser/sequence_type_fuser/sequence_type_fuser.cc

boolFuseType(constTypeFuserOptions&options,std::vector<ObjectPtr>*objects)override;

該部分負(fù)責(zé)對 object 序列 (object sequence) 進(jìn)行類型 (type) 的融合。

object 的type 如下代碼所示:

enum ObjectType {

? UNKNOWN = 0,

? UNKNOWN_MOVABLE = 1,

? UNKNOWN_UNMOVABLE = 2,

? PEDESTRIAN = 3,

? BICYCLE = 4,

? VEHICLE = 5,

? MAX_OBJECT_TYPE = 6,

};

Apollo 將被追蹤的objects 視為序列。

當(dāng) object 為 background 時，其類型為 "UNKNOW_UNMOVABLE"。

當(dāng) object 為 foreground 時，使用線性鏈條件隨機(jī)場(Linear chain Conditional Random Fields) 和 維特比（Viterbi）算法對 object sequence 進(jìn)行 object 的類型的融

鏈接：http://www.itdecent.cn/p/95a51214959b

https://blog.csdn.net/qq_17437129

來源：簡書

著作權(quán)歸作者所有。商業(yè)轉(zhuǎn)載請聯(lián)系作者獲得授權(quán)，非商業(yè)轉(zhuǎn)載請注明出處。