分成7步對 Lidar 的流程進行敘述：

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

Apollo 使用了開源庫?Eigen?進行高效的矩陣計算暮刃，使用了?PCL?點云庫對點云進行處理多柑。

該部分中讨勤，Apollo 首先計算轉(zhuǎn)換矩陣 velodyne_trans链嘀，用于將 Velodyne 坐標轉(zhuǎn)化為世界坐標。之后將 Velodyne 點云轉(zhuǎn)為 PCL 點云庫格式鹃栽，便于之后的計算躏率。

2.獲取ROI區(qū)域

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

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

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

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

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

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之外的激光雷達點丰嘉，去除背景對象夯到，如路邊建筑物和樹木等，剩余的點云留待后續(xù)處理饮亏。

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

1. 坐標轉(zhuǎn)換

2. ROI LUT構(gòu)造

3. ROI LUT點查詢

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

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

a.坐標轉(zhuǎn)換

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

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

比較所有點的 x赫粥、y 方向的區(qū)間范圍观话，取區(qū)間范圍較小的方向為主方向。并將地圖多邊形 (map_polygons) 轉(zhuǎn)換為待加工的多邊形 (raw polygons)越平。

c.建立位圖

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

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

? ? ? ? ? ? 位圖用于以格點 (grid)?的方式存儲 ROI 信息秦叛。若某格點值為真晦溪，代表此格點屬于 ROI。

? ? ? ? ? ? 默認的格點大小為 cell_size 0.25米挣跋。

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

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

i. 獲得主方向有效范圍

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

iii. 基于掃描間隔在位圖中畫格點

位圖中畫多邊形流程

d.ROI點檢測

通過檢查 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）?對障礙物進行分割整慎，有以下四步：

a.通道特征提取

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

1. 單元格中點的最大高度

2. 單元格中最高點的強度

3. 單元格中點的平均高度

4. 單元格中點的平均強度

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

6. 單元格中心相對于原點的角度

7. 單元格中心與原點之間的距離

8. 二進制值標示單元格是空還是被占用如

計算時默認只使用 ROI 區(qū)域內(nèi)的點裤园，也可使用整個 Lidar 范圍內(nèi)的點，使用標志位 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ù)特征圖像的空間細節(jié)，以促進單元格方向的障礙物位置饮怯、速度屬性預(yù)測闰歪。 根據(jù)具有非線性激活（即ReLu）層的堆疊卷積/分散層來實現(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)建障礙物集群。

障礙聚類

（a）紅色箭頭表示每個單元格對象中心偏移預(yù)測贪壳；藍色填充對應(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ù)每個候選群體的檢測置信度分數(shù)和物體高度蚂且，來確定最終輸出的障礙物集/分段配猫。

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

在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邊界邊框，將選擇具有最小面積的方案作為最終的邊界框

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?的對象跟蹤器。實現(xiàn)原理：

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

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

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

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

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

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

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

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

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

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

a.觀察冗余

在一系列重復(fù)觀測中選擇速度測量，即濾波算法的輸入瓢谢，包括錨點移位畸写、邊界框中心偏移、邊界框角點移位等氓扛。冗余觀測將為濾波測量帶來額外的魯棒性枯芬， 因為所有觀察失敗的概率遠遠小于單次觀察失敗的概率。

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

錨點移位速度千所、邊界框中心偏移速度 和 邊界框角點位移速度。

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

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

b.分解

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

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

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

KalmanFilter::UpdateVelocity

KalmanFilter::UpdateAcceleration

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

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

velocity_gain*=breakdown_threshold_;

加速度的故障閾值是定值，默認為2

acceleration_gain *= breakdown_threshold;

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

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

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

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

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

首先根據(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;

該部分負責對 object 序列 (object sequence) 進行類型 (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 視為序列唤殴。

當 object 為 background 時，其類型為 "UNKNOW_UNMOVABLE"到腥。

當 object 為 foreground 時朵逝，使用線性鏈條件隨機場(Linear chain Conditional Random Fields) 和 維特比（Viterbi）算法對 object sequence 進行 object 的類型的融合。