【Baidu Apollo】4.1 感知

首先非常感謝知乎上幾位大牛和Apollo開發者社區分享的相關資料，整個Baidu Apollo的系列文章，筆者這裏做一些資料的收集整理，希望可以和大家共同學習。主要的參考來源包括：

• 知乎冀淵的專欄眼中的世界
• Apollo阿波羅自動駕駛開發者社區
• Apollo 開發者在線文檔
• CSDN 作者 知行合一2018
• CSDN 作者 DinnerHowe
• Baidu Apollo GitHub

今天的感知模塊的內容整理自冀淵的文章:
Apollo 2.0 框架及源碼分析(二) | 感知模塊 | Lidar
Apollo 2.0 框架及源碼分析(三) | 感知模塊 | Radar & Fusion

Apollo 感知模塊 (Perception) 實現框架

無人駕駛系統車端的部分，大致可分爲3塊內容，感知，決策，控制。其中，感知是其餘兩者的基礎，是無人駕駛中極爲重要的一個部分。

無人駕駛中所用到得傳感器各有長處和短板，單一的傳感器難以滿足複雜場景的需求，因此使用多種傳感器進行相互融合（sensor funsion）就顯得十分有必要了。

下圖是 Apollo 公開課對無人駕駛中常見傳感器的介紹，從圖中看出，各個傳感器擅長的情況不一，而依靠傳感器的融合則可以應對大多數情況。

Apollo 的這張圖整體表述不錯，十分清晰。
冀淵認爲圖中對 Lidar 在 Lane Tracking 上的描述有點欠妥。他認爲可以根據路面和車道線對 Lidar 射線反射強度的不同來追蹤車道。因此 Lidar 在車道線追蹤的能力應該可以算得上是 Fair 而不是和 Radar 一樣的 Poor。

另外，雷鋒網的這篇文章中也提到了基於激光雷達回波信號檢測車道線的可能性。專欄 | 如何利用激光雷達檢測車道線？這裏提供了4種方法
Apollo 2.0 感知的整體框架如下圖所示，分爲 3D障礙物感知 和 交通燈感知 兩大部分。

其實這兩者在 Apollo 官方文檔和講座中，已經有了比較清晰的講解。交通信號燈感知和3D 障礙物感知
Apollo 官方公開課中的對感知部分的功能是用下圖描述的。

從圖中可知，在 Apollo 中感知模塊有以下幾個職能：

1. 探測物體 （是否有障礙物）
2. 對物體分類 （障礙物是什麼）
3. 語義解析 (障礙物從背景中分割)
4. 物體追蹤 (障礙物追蹤)

本篇接下來會敘述 Apollo 3D障礙物感知部分是如何實現上述職能的。

障礙物感知部分的框架如下圖所示。障礙物感知主要依靠的是 Lidar 和 Radar 兩者的感知結果的相互融合。該部分輸入爲傳感器的原始數據，根據接收到的數據的來源的不同，進行不同的處理，最終輸出融合後的結果。

從代碼上看，障礙物感知由3個主體部分組成: Lidar、Radar 和 fusion。接下來，本篇將重點描述其中的 Lidar 部分的實現原理。

Lidar 部分

官方對 Lidar 部分的原理解釋得極其清楚。大家結合上文列出的參考文檔和PPT資料 ，應該就會對 Lidar 部分的整體工作原理會有個清晰的認識了。

如上圖 PPT 所示，Lidar 部分簡單來說是這樣工作的：

1. 輸入爲 Lidar 得到的點雲數據，輸出爲檢測到的障礙物
2. 由 HDmap 來確定 ROI (region of interest），過濾 ROI 區域外的點
3. 處理點雲數據，探測並識別障礙物 (由 AI 完成)
4. 障礙物追蹤

冀淵對代碼也做了講解

入口函數位置: apollo/modules/perception/obstacle/onboard/lidar_process_subnode.cc

void LidarProcessSubnode::OnPointCloud(
    const sensor_msgs::PointCloud2& message);  

根據代碼的註釋，這裏分成7步對 Lidar 的流程進行敘述。

1. 座標及格式轉換 (get velodyne2world transform)

Apollo 使用了開源庫 Eigen 進行高效的矩陣計算，使用了 PCL 點雲庫對點雲進行處理。

該部分中，Apollo 首先計算轉換矩陣 velodyne_trans，用於將 Velodyne 座標轉化爲世界座標。之後將 Velodyne 點雲轉爲 PCL 點雲庫格式，便於之後的計算。

2. 獲取ROI區域 (call hdmap to get ROI)

核心函數位置: obstacle/onboard/hdmap_input.cc

bool HDMapInput::GetROI(const PointD& pointd, const double& map_radius,
                        HdmapStructPtr* mapptr);

查詢 HDmap, 根據 Velodyne 的世界座標以及預設的半徑 (FLAG_map_radius) 來獲取 ROI 區域。

首先獲取指定範圍內的道路以及道路交叉點的邊界，將兩者進行融合後的結果存入 ROI 多邊形中。該區域中所有的點都位於世界座標系。

3. 調用ROI過濾器 (call roi_filter）

核心函數位置：obstacle/lidar/roi_filter/hdmap_roi_filter/hdmap_roi_filter.cc

bool HdmapROIFilter::Filter(const pcl_util::PointCloudPtr& cloud,
                            const ROIFilterOptions& roi_filter_options,
                            pcl_util::PointIndices* roi_indices);

官方文檔對該部分是這麼描述的:

高精地圖 ROI 過濾器（往下簡稱“過濾器”）處理在ROI之外的激光雷達點，去除背景對象，如路邊建築物和樹木等，剩餘的點雲留待後續處理。
一般來說，Apollo 高精地圖 ROI過濾器有以下三步：
1. 座標轉換
2. ROI LUT構造
3. ROI LUT點查詢

藍色線條標出了高精地圖ROI的邊界，包含路表與路口。紅色加粗點表示對應於激光雷達傳感器位置的地方座標系原始位置。2D網格由8*8個綠色正方形組成，在ROI中的單元格，爲藍色填充的正方形，而之外的是黃色填充的正方形。

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

a. 座標轉換

將地圖 ROI 多邊形和點雲轉換至激光雷達傳感器位置的地方座標系。

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

比較所有點的 x、y 方向的區間範圍，取區間範圍較小的方向爲主方向。並將地圖多邊形 (map_polygons) 轉換爲待加工的多邊形 (raw polygons)。

c. 建立位圖

將 raw polygons 轉化爲位圖 (bitmap) 中的格點，位圖有以下特點：

• 位圖範圍, 以 Lidar 爲原點的一片區域 (-range, range)*(-range, range) 內，range 默認 70米
• 位圖用於以格點 (grid) 的方式存儲 ROI 信息。若某格點值爲真，代表此格點屬於 ROI。
• 默認的格點大小爲 cell_size 0.25米。
• 在列方向上，1bit 代表 1grid。爲了加速操作，Apollo 使用 uint64_t 來一次操縱64個grids。

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

i. 獲得主方向有效範圍

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

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

關於掃描線，這裏推薦筆者參考的一篇解析 掃描線算法完全解析

d. ROI 點查詢

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

4. 調用分割器 (segmentor)

入口函數所在文件cnn_segmentation.cc

bool CNNSegmentation::Segment(const pcl_util::PointCloudPtr& pc_ptr,
                              const pcl_util::PointIndices& valid_indices,
                              const SegmentationOptions& options,
                              vector<ObjectPtr>* objects)

分割器採用了 caffe 框架的深度完全卷積神經網絡（FCNN） 對障礙物進行分割

簡單來說有以下四步：

a. 通道特徵提取

計算以 Lidar 傳感器某一範圍內的各個單元格 (grid) 中與點有關的8個統計量，將其作爲通道特徵（channel feature）輸入到 FCNN。

1. 單元格中點的最大高度
2. 單元格中最高點的強度
3. 單元格中點的平均高度
4. 單元格中點的平均強度
5. 單元格中的點數
6. 單元格中心相對於原點的角度
7. 單元格中心與原點之間的距離
8. 二進制值標示單元格是空還是被佔用如

計算時默認只使用 ROI 區域內的點，也可使用整個 Lidar 範圍內的點，使用標誌位 use_full_cloud_ 作爲開關。

b. 基於卷積神經網絡的障礙物預測

• 與 caffe 相關的 FCNN 源碼貌似是不開源的
• Apllo 官方敘述了其工作原理，摘錄如下

完全卷積神經網絡由三層構成：下游編碼層（特徵編碼器）、上游解碼層（特徵解碼器）、障礙物屬性預測層（預測器）

特徵編碼器將通道特徵圖像作爲輸入，並且隨着特徵抽取的增加而連續下采樣其空間分辨率。 然後特徵解碼器逐漸對特徵圖像
上採樣到輸入2D網格的空間分辨率，可以恢復特徵圖像的空間細節，以促進單元格方向的障礙物位置、速度屬性預測。
根據具有非線性激活（即ReLu）層的堆疊卷積/分散層來實現 下采樣和 上採樣操作

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

c. 障礙物集羣 （Cluster2D）

核心函數位置 obstacle/lidar/segmentation/cnnseg/cluster2d.h

void Cluster(const caffe::Blob<float>& category_pt_blob,
               const caffe::Blob<float>& instance_pt_blob,
               const apollo::perception::pcl_util::PointCloudPtr& pc_ptr,
               const apollo::perception::pcl_util::PointIndices& valid_indices,
               float objectness_thresh, bool use_all_grids_for_clustering);

Apollo基於單元格中心偏移預測構建有向圖，採用壓縮的聯合查找算法（Union Find algorithm ）基於對象性預測有效查找連接組件，構建障礙物集羣。

（a）紅色箭頭表示每個單元格對象中心偏移預測；藍色填充對應於物體概率不小於0.5的對象單元。
（b）固體紅色多邊形內的單元格組成候選對象集羣。

d. 後期處理

涉及的函數 obstacle/lidar/segmentation/cnnseg/cluster2d.h

void Filter(const caffe::Blob<float>& confidence_pt_blob,
              const caffe::Blob<float>& height_pt_blob);
void Classify(const caffe::Blob<float>& classify_pt_blob);
void GetObjects(const float confidence_thresh, const float height_thresh,
                  const int min_pts_num, std::vector<ObjectPtr>* objects);

聚類後，Apollo獲得一組包括若干單元格的候選對象集，每個候選對象集包括若干單元格。根據每個候選羣體的檢測置信度分數和物體高度，來確定最終輸出的障礙物集/分段。從代碼中可以看到 CNN分割器最終識別的物體類型有三種：小機動車、大機動車、非機動車和行人

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

enum MetaType {
  META_UNKNOWN,
  META_SMALLMOT,
  META_BIGMOT,
  META_NONMOT,
  META_PEDESTRIAN,
  MAX_META_TYPE
};

5. 障礙物邊框構建

入口函數位置 obstacle/lidar/object_builder/min_box/min_box.cc

void BuildObject(ObjectBuilderOptions options, ObjectPtr object)

邊界框的主要目的還是預估障礙物（例如，車輛）的方向。同樣地，邊框也用於可視化障礙物。
如圖，Apollo確定了一個6邊界邊框，將選擇具有最小面積的方案作爲最終的邊界框

6. 障礙物追蹤

入口函數位置：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
  bool Track(const std::vector<ObjectPtr>& objects, double timestamp,
             const TrackerOptions& options,
             std::vector<ObjectPtr>* tracked_objects); 

障礙物追蹤可分兩大部分，即 數據關聯 和 跟蹤動態預估。Apollo 使用了名爲 HM tracker 的對象跟蹤器。實現原理：

在HM對象跟蹤器中，匈牙利算法(Hungarian algorithm)用於檢測到跟蹤關聯，並採用 魯棒卡爾曼濾波器(Robust
Kalman Filter) 進行運動估計。

數據關聯

數據關聯的過程是確定傳感器接收到的量測信息和目標源對應關係的過程，是多傳感多目標跟蹤系統最核心且最重要的過程 [11]。

Apollo 首先建立關聯距離矩陣，用於計算每個對象 (object ) 和 每個軌跡 (track )之間的關聯距離。之後使用 匈牙利算法 爲 object和 track 進行最優分配。

計算關聯距離時，Apollo 考慮了以下5個關聯特徵，來評估 object 和 track 的運動及外觀一致性，併爲其分配了不同的權重。

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

關於匈牙利算法的實現原理，這裏有篇很有趣的解釋 趣寫算法系列之–匈牙利算法 - CSDN博客

跟蹤動態預估 （Track Motion Estimation）

使用卡爾曼濾波來對 track 的狀態進行估計，使用魯棒統計技術來剔除異常數據帶來的影響。

不瞭解卡爾曼濾波原理的同學請參考：卡爾曼濾波器的原理以及在matlab中的實現 。這一部分的濾波整體看來是一個標準的卡爾曼濾波。在此基礎上，Apollo 團隊加入了一些修改，根據官方文檔，Apollo 的跟蹤動態預估有以下三個亮點 :

觀察冗餘

在一系列重複觀測中選擇速度測量，即濾波算法的輸入，包括錨點移位、邊界框中心偏移、邊界框角點移位等。冗餘觀測將爲濾波測量帶來額外的魯棒性，因爲所有觀察失敗的概率遠遠小於單次觀察失敗的概率。

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

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

從三個速度中，根據運動的一致性，選出與之前觀測速度偏差最小的速度爲最終的觀測值。

根據最近3次的速度觀測值，計算出加速度的觀測值。

分解

高斯濾波算法 （Gaussian Filter algorithms）總是假設它們的高斯分佈產生噪聲。然而，這種假設可能在運動預估問題中失敗，因爲其測量的噪聲可能來自直方分佈。 爲了克服更新增益的過度估計，在過濾過程中使用故障閾值。

這裏的故障閾值應該對應着程序中的 breakdown_threshold_。

該參數被用於以下兩個函數中，當更新的增益過大時，它被用來克服增益的過度估計：

• KalmanFilter::UpdateVelocity
• KalmanFilter::UpdateAcceleration

兩者的區別在於：

速度的故障閾值是動態計算的，與速度誤差協方差矩陣有關

velocity_gain *= breakdown_threshold_; 

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

acceleration_gain *= breakdown_threshold;  

更新關聯質量 （UpdateQuality）

原始卡爾曼濾波器更新其狀態不區分其測量的質量。 然而，質量是濾波噪聲的有益提示，可以估計。例如，在關聯步驟中計算的距離可以是一個合理的測量質量估計。 根據關聯質量更新過濾算法的狀態，增強了運動估計問題的魯棒性和平滑度

更新關聯質量 update_quality 默認爲 1.0，當開啓適應功能時 (s_use_adaptive ==true）Apollo 使用以下兩種策略來計算更新關聯質量：

• 根據 object 自身的屬性 — 關聯分數 (association_score) 來計算
• 根據新舊兩個 object 點雲數量的變化

首先根據這兩種策略分別計算更新關聯質量，之後取得分小的結果來控制濾波器噪聲。

7. 障礙物類型融合 （call type fuser）

這個函數貌似再3.0的版本里刪除了。

該部分負責對 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 的類型的融合。

Radar 部分

入口函數位置：obstacle/onboard/radar_process_subnode.cc

void RadarProcessSubnode::OnRadar(const ContiRadar &radar_obs) ;

相比 Lidar 部分的令人驚豔，Apollo 2.0 中 Radar 就稍顯誠意不足了。Apollo 2.0 中 Radar 探測器模塊名爲“ModestRadarDetector”(“適度的雷達探測器”，從起名上就感覺Apollo 2.0 對 Radar 這部分有些不自信）。

Radar 部分大體的實現方式與 Lidar 類似，可以看做是其簡化版，其大體流程如下圖所示。

從代碼實現上來看，Radar 的工作可分爲以下4步：

一. 計算 Radar 的座標轉換矩陣

這裏有個細節十分有趣。雷達的世界座標的轉換矩陣是由以下代碼來實現的。

*radar2world_pose = *velodyne2world_pose *
short_camera_extrinsic_ *  radar_extrinsic_;

這段代碼描述了這樣一個式子：

Radar 世界座標 = Lidar 世界座標 * 短攝像頭參數* radar參數。

這個式子隱隱透露着， Apollo 的座標體系是以 Lidar 爲基準的。Apollo 可能認爲 Velodyne 的位置是最準確的，因此 Camera 的位置標定參考 Velodyne, Radar 的標定參考 Camera。

二. 獲取 ROI 區域

從高精度地圖讀取 ROI 區域，並將其轉化爲地圖多邊形 (map_polygons) 備用。Radar 和 Lidar 使用了相同的函數來獲取 ROI 區域和地圖多邊形，獲得的 ROI 地圖多邊形處於世界座標下。

函數位置: obstacle/onboard/hdmap_input.cc

void HdmapROIFilter::MergeHdmapStructToPolygons(
    const HdmapStructConstPtr& hdmap_struct_ptr,
    std::vector<PolygonDType>* polygons)

函數位置: obstacle/lidar/roi_filter/hdmap_roi_filter/hdmap_roi_filter.cc

void HdmapROIFilter::MergeHdmapStructToPolygons(
    const HdmapStructConstPtr& hdmap_struct_ptr,
    std::vector<PolygonDType>* polygons) 

三. 計算汽車線速度

四. 探測障礙物

核心函數位置：obstacle/radar/modest/modest_radar_detector.cc

bool ModestRadarDetector::Detect(const ContiRadar &raw_obstacles,
                                 const std::vector<PolygonDType> &map_polygons,
                                 const RadarDetectorOptions &options,
                                 std::vector<ObjectPtr> *objects) ;

這部分爲 Radar 的核心部分，這段又可分爲如下4步。

1. 從原始數據構造 object

實現函數位置: obstacle/radar/modest/object_builder.cc

void ObjectBuilder::Build(const ContiRadar &raw_obstacles,
                          const Eigen::Matrix4d &radar_pose,
                          const Eigen::Vector2d &main_velocity,
                          SensorObjects *radar_objects) ;

該部分涉及到了以下方面的內容

• 判斷該障礙物是否爲 backgroundobject
• 錨點 (anchor_point) 的座標系轉換: Radar 座標 -> 世界座標
• 速度的轉化: 相對速度-> 絕對速度；

我們重點闡述下第一項內容。Apollo 提供了三種方式用來判斷該障礙物是否爲 background：

a. 根據障礙物出現的次數

當障礙物出現次數小於 delay_frames_ （默認值爲4）時，認爲它是 background。

b. 根據障礙物出現的 存在概率 和 均根方值（rms）

實現函數位置: obstacle/radar/modest/conti_radar_util.cc

bool ContiRadarUtil::IsFp(const ContiRadarObs &contiobs,
                          const ContiParams &params, const int delay_frames,
                          const int tracking_times)

當滿足以下兩個條件之一時，認爲該障礙物是 background

• 障礙物的存在概率小於 Radar 參數預設值
• 當障礙物在車身橫縱兩個方向上的 距離 或 速度 的 rms 大於 Radar 參數預設值

代碼中對這個功能是這樣實現的（爲方便說明，這裏隱去部分代碼）。

bool ContiRadarUtil::IsFp(const ContiRadarObs &contiobs,
                          const ContiParams &params, const int delay_frames,
                          const int tracking_times) {
    int cls = contiobs.obstacle_class();
      ...
    if (cls == CONTI_CAR || cls == CONTI_TRUCK) {
      ...
    } else if (cls == CONTI_PEDESTRIAN) {
      ...
    } else if (cls == CONTI_MOTOCYCLE || cls == CONTI_BICYCLE) {
      ...
    } else if (cls == CONTI_POINT || cls == CONTI_WIDE ||
               cls == CONTI_UNKNOWN) {
      ...
    }
      ...
}

從中可以看出，Apollo 對不同類型的障礙物分別進行了處理，也就是說，Apollo 使用的 Radar 可以分辨出車、行人和自行車 等不同的障礙物。

Apollo 推薦使用的毫米波爲Continental 的 ARS 408-21。ARS 408-21的介紹文檔裏也有簡單提到，它可以對障礙物進行分類。

但實際上，我對於毫米波雷達的障礙物類型的分辨結果的可靠性和可用性仍存有疑惑。筆者也諮詢過同事和朋友，大家認爲毫米波雷達也許可以根據回波特性和被檢測物的速度來區分障礙物。但這種方式的分辨率、可靠性和可用性都有待提高。另外文檔中括號內 “> 120 single cluster”這個條件，我一直不能理解，希望可以和大家一起探討一下。

c. 根據 障礙物速度 和 車身速度 的夾角 (均在絕對速度下)

當車速和障礙物速度都大於閾值 (velocity_threshold, 默認值 1e-1) 時，計算兩個速度的夾角。當夾角在（1/4 Pi, 3/4 Pi）或者 (-3/4 Pi, -1/4 Pi ) 之外的範圍時，認爲該障礙物是 background。

這裏還有一個槽點，Apollo 2.0 在構造 object 外形的時候的代碼如下:

object_ptr->length = 1.0;
object_ptr->width = 1.0;
object_ptr->height = 1.0;
object_ptr->type = UNKNOWN; 

從代碼中可以看到，Apollo 將障礙物長寬高都設成1米。毫米波雷達確實很難獲取到物體的輪廓信息，但這樣處理，還是覺得有點簡單粗暴了。

另外這裏的障礙物類型直接設置爲 UNKNOW ，這就讓人有些疑惑了。既然之前 Apollo 在構造 object 時已經可以直接從 Radar 獲得障礙物類型，爲什麼這裏不使用呢？難道 Apollo 自己其實也認爲 Radar 獲得的物體類型的可靠性不高麼？這樣處理豈不是有點前後矛盾？

2. 調用 ROI 濾波器

實現函數位置: obstacle/radar/modest/modest_radar_detector.cc

void RoiFilter(const std::vector<PolygonDType> &map_polygons,
                 std::vector<ObjectPtr>* filter_objects)

Radar 的 ROI 濾波器與 Lidar 有所不同，被簡化了許多。

• Radar 沒有使用位圖和掃描線，而是直接判斷每個 object 是否處於地圖多邊形內
• Radar ROI 濾波器中使用的點和地圖多邊形的座標均爲世界座標，而 Lidar 的 ROI 使用了地方座標。

3. 障礙物追蹤

入口函數位置: obstacle/radar/modest/radar_track_manager.cc

// @brief: process radar obstacles
// @param [in]: built radar obstacles
// @return nothing
void Process(const SensorObjects &radar_obs);

Radar 的數據關聯相對簡單，只考慮 目標 object 和 tracked object 兩者的 track_id 和 幾何距離 兩個因素。

當兩者的 track_id 相同，且兩者之間距離小於 RADAR_TRACK_THRES （默認 2.5）時，認爲兩者爲同一目標，即目標 object 屬於該 track。

幾何距離 = 兩object 的中心點的距離+object 的速度*時間差。

另外對比 Lidar 在進行數據關聯時，計算了障礙物的關聯距離且使用了匈牙利算法進行最優分配，Radar 部分的數據關聯顯得太簡單和太單薄了。

除此之外，在進行障礙物追蹤時，Radar 的 tracker 也並未存儲 object 的歷史數據，只保存最新的一個 object，並且也沒有使用卡爾曼濾波器對 objects 進行狀態估計。

4. 收集 Objects

收集objects， 爲最後一步融合做準備。

總得來說，Radar 部分“無愧於” modest 的名稱，與 Lidar 部分相比，Radar 的實現顯得過於單薄。雖然與 Lidar 相比，Radar 的精確度確實相對較低，並且我也相信 Lidar 是未來的趨勢。但現階段 Radar 在惡劣天氣下的表現，在速度的探測上和探測距離上都比 Lidar 要好。在許多量產方案中 (比如奧迪A8，特斯拉) Radar 也都發揮了極其重要的作用。Radar 也可以作爲 Lidar 的冗餘，在 Lidar 出故障時，用來保證系統的安全。

我期望百度能夠讓 Radar 發揮更大的作用，讓 Apollo 系統得以進一步完善。

融合部分

入口函數位置: obstacle/onboard/fusion_subnode.cc

apollo::common::Status Process(const EventMeta &event_meta,
                                 const std::vector<Event> &events);

核心函數位置：obstacle/fusion/probabilistic_fusion/probabilistic_fusion.cc

// @brief: fuse objects from multi sensors(64-lidar, 16-lidar, radar...)
// @param [in]: multi sensor objects.
// @param [out]: fused objects.
// @return true if fuse successfully, otherwise return false
virtual bool Fuse(const std::vector<SensorObjects> &multi_sensor_objects,
                    std::vector<ObjectPtr> *fused_objects);

融合部分總體來看，沒有特別大的驚喜，仍是使用了傳統的卡爾曼濾波。

Apollo 介紹中說到，他們使用了 object-level 的數據融合，該部分的輸入爲各傳感器處理後的得到的object。

這裏順便一提，多源信息的數據融合中，根據數據抽象層次，融合可分爲三個級別[2]：

• 數據級融合 傳感器裸數據融合，精度高、實時性差，要求傳感器是同類的
• 特徵級融合 融合傳感器抽象的特徵向量(速度，方向等)，數據量小、損失部分信息
• 決策級融合 傳感器自身先做出決策，融合決策結果，精度低、通信量小、抗干擾強

Apollo 應該是在特徵層面對 objects 進行了融合。每當節點收到新的一幀數據的時候，融合部分就被調用。融合部分的輸入爲 SensorObjects, 輸出爲融合後的 object， 其大體的流程如下圖所示。

這其中，最重要的步驟自然就是融合了，Apollo 依照傳感器的測量結果，根據is_background 標誌位，將 objects 分爲 ForegroundObjects 和 BackgroundObjects 兩類, 融合時只處理 ForegroundObjects。

融合函數位置：obstacle/fusion/probabilistic_fusion/probabilistic_fusion.cc

void FuseForegroundObjects(
      std::vector<PbfSensorObjectPtr> *foreground_objects,
      Eigen::Vector3d ref_point, const SensorType &sensor_type,
      const std::string &sensor_id, double timestamp);

從之前的經驗我們可以看出，傳感器的數據融合有兩部分內容比較重要，即 數據關聯 和 動態預估。

數據關聯

數據關聯的接口定義如下：

  // @brief match sensor objects to global tracks build previously
  // @params[IN] fusion_tracks: global tracks
  // @params[IN] sensor_objects: sensor objects
  // @params[IN] options: matcher options for future use
  // @params[OUT] assignments: matched pair of tracks and measurements
  // @params[OUT] unassigned_tracks: unmatched tracks
  // @params[OUT] unassigned_objects: unmatched objects
  // @params[OUT] track2measurements_dist:minimum match distance to measurements
  // for each track
  // @prams[OUT] measurement2track_dist:minimum match distacne to tracks for
  // each measurement
  // @return nothing
  virtual bool Match(const std::vector<PbfTrackPtr> &fusion_tracks,
                     const std::vector<PbfSensorObjectPtr> &sensor_objects,
                     const TrackObjectMatcherOptions &options,
                     std::vector<TrackObjectPair> *assignments,
                     std::vector<int> *unassigned_fusion_tracks,
                     std::vector<int> *unassigned_sensor_tracks,
                     std::vector<double> *track2measurements_dist,
                     std::vector<double> *measurement2track_dist) = 0;

數據關聯的實現是在:

obstacle/fusion/probabilistic_fusion/pbf_hm_track_object_matcher.cc

bool PbfHmTrackObjectMatcher::Match(
    const std::vector<PbfTrackPtr> &fusion_tracks,
    const std::vector<PbfSensorObjectPtr> &sensor_objects,
    const TrackObjectMatcherOptions &options,
    std::vector<TrackObjectPair> *assignments,
    std::vector<int> *unassigned_fusion_tracks,
    std::vector<int> *unassigned_sensor_objects,
    std::vector<double> *track2measurements_dist,
    std::vector<double> *measurement2track_dist) ;

其實從文件名稱中的”hm”就可以看出一些端倪了，與 Lidar 相似，Fusion 部分的數據關聯還是使用了匈牙利算法（Hungarian Matcher）來進行分配的。

但是在計算關聯距離時，兩者有比較大的不同。Fusion 部分計算的只是兩個 object 中心的幾何距離。我們稍微看一下這裏的代碼，有個細節比較有趣。

Fusion 在計算關聯距離時，使用了以下代碼。

函數位置：obstacle/fusion/probabilistic_fusion/pbf_track_object_distance.cc

float PbfTrackObjectDistance::Compute(
    const PbfTrackPtr &fused_track, const PbfSensorObjectPtr &sensor_object,
    const TrackObjectDistanceOptions &options) {

  //fused_track 已經融合過obj的航跡
  //sensor_object 來自傳感器的，待融合的obj

  //獲取此幀數據來源於的傳感器類型
  const SensorType &sensor_type = sensor_object->sensor_type;
  ADEBUG << "sensor type: " << sensor_type;
  // 獲取上次融合的obj
  PbfSensorObjectPtr fused_object = fused_track->GetFusedObject();
  if (fused_object == nullptr) {
    ADEBUG << "fused object is nullptr";
    return (std::numeric_limits<float>::max)();
  }

  Eigen::Vector3d *ref_point = options.ref_point;
  if (ref_point == nullptr) {
    AERROR << "reference point is nullptr";
    return (std::numeric_limits<float>::max)();
  }

  float distance = (std::numeric_limits<float>::max)();
  //獲取航跡中最近的來自Lidar的obj
  const PbfSensorObjectPtr &lidar_object = fused_track->GetLatestLidarObject();
  //獲取航跡中最近的來自Radar的obj
  const PbfSensorObjectPtr &radar_object = fused_track->GetLatestRadarObject();

  //下面是重點
  if (is_lidar(sensor_type)) {  //如果這次要融合obj是來自源於Lidar
    if (lidar_object != nullptr) {    
    // 如果航跡中已經有來自 Lidar 的obj, 則計算兩者的幾何距離 
      distance =
          ComputeVelodyne64Velodyne64(fused_object, sensor_object, *ref_point);
    } else if (radar_object != nullptr) {
     // 如果航跡中沒有來自 Lidar 的obj, 則計算與 radar obj 的距離,注意這裏 
     // sensor_object 和 fused_object 與上面的位置是相反的。原因是這個函數
     // 在計算距幾何距離時的實現，是以第一個參數爲速度基準，計算v*time_diff
     // 也就是說，當 fused_object 爲 radar 時，以 sensor_object 爲準。  
      distance =
          ComputeVelodyne64Radar(sensor_object, fused_object, *ref_point);
    } else {
      AWARN << "All of the objects are nullptr";
    }
  } else if (is_radar(sensor_type)) { // 如果這次要融合的obj是來源於Radar
    if (lidar_object != nullptr) { 
      // 如果航跡中已經有來自 Lidar 的obj, 則計算兩者的幾何距離 
      distance =
          ComputeVelodyne64Radar(fused_object, sensor_object, *ref_point);
    } else if (radar_object != nullptr) {
     // 如果航跡中沒有來自 Lidar 的obj, 返回 float 的極值
      distance = std::numeric_limits<float>::max();
      //    distance = compute_radar_radar(fused_object, sensor_object,
      //    *ref_point);
    } else {
      AWARN << "All of the objects are nullptr";
    }
  } else {
    AERROR << "fused sensor type is not support";
  }
  return distance;
}

上面這段代碼表明瞭，Fusion 在計算幾何距離時，要求計算的兩個 obj 中至少有一個是來自於 Lidar 的，並且以 Lidar 爲基準來測量距離。

但只靠幾何距離來進行數據關聯，容易出現 Miss Match 的問題。即，當兩條航跡發生交叉時，只靠幾何距離是無法爲 object 關聯合適的航跡的。我現在還不清楚 Apollo 2.0 打算如何規避這個問題。

動態預估

動態預估還是使用的卡爾曼濾波，這部分有兩點比較有趣：

Apollo 的融合部分爲了可擴展性使用的是標準卡爾曼濾波Apollo 融合部分的卡爾曼濾波使用了非簡化的估計誤差協方差矩陣 Pk|k${\mathbf{P}}_{k|k}$ 更新公式

我們對比上圖標準卡爾曼濾波的公式，來說明我爲什麼覺着上述兩點比較有趣。

1. 使用標準卡爾曼濾波

從代碼細節上看，Apollo 使用了標準卡爾曼濾波，並且在更新來自 Lidar 和 Radar 的 object 的時候，Apollo 對這兩者使用了相同的觀測矩陣 \mathbf {H}。

這裏有趣的地方在於，一般而言 Lidar 和 Radar 的觀測矩陣是不同的, 因爲兩者得到的數據不同 。爲了更好地說明這個問題，我建議大家先閱讀一下下面這篇文章 擴展卡爾曼濾波EKF與多傳感器融合。
這個問題已經得到解答，大陸的Radar提供的也是笛卡爾座標系的數據，所以用一個觀測矩陣是可以的

2. 使用了非簡化的估計誤差協方差矩陣 Pk|k${\mathbf{P}}_{k|k}$ 更新公式

我們簡單看一下區別

標準卡爾曼濾波：Pk|k=(I−KkHk)Pk|k−1${\displaystyle {\mathbf{P}}_{k|k}=(\mathbf{I}\mathbf{-}{\mathbf{K}}_{\mathbf{k}}{\mathbf{H}}_{\mathbf{k}}){\mathbf{P}}_{k|k-1}}$
Apollo：Pk|k=(I−KkHk)Pk|k−1(I−KkHk)T+KkRkKTk${\displaystyle {\mathbf{P}}_{k|k}=(\mathbf{I}\mathbf{-}{\mathbf{K}}_{\mathbf{k}}{\mathbf{H}}_{\mathbf{k}}){\mathbf{P}}_{k|k-1}}(\mathbf{I}\mathbf{-}{\mathbf{K}}_{\mathbf{k}}{\mathbf{H}}_{\mathbf{k}}{)}^{\mathbf{T}}+{\mathbf{K}}_k{\mathbf{R}}_k{\mathbf{K}}_k^{\mathrm{T}}$

結合 Wikipedia 上關於卡爾曼濾波的介紹，我先總結下該問題的背景:

• Apollo 使用的估計誤差協方差矩陣 Pk|k${\mathbf{P}}_{k|k}$ 的更新公式是所謂的 Joseph form，而標準卡爾曼濾波通常使用的是簡化版的更新公式
• 簡化版的更新公式計算量小，實踐中應用廣，但只在 卡爾曼增益爲最優 時有效
• 必須使用Joseph form 的兩種情況：
  – 使用了非最優卡爾曼增益
  – 算法精度過低，造成了數值穩定性相關的問題

Apollo 社區回答了兩點原因，一是出於算法精度的考慮；二是由於計算單元的強大（且昂貴），非簡化版的卡爾曼濾波在計算時也不會消耗太久的時間。綜合考慮，Apollo 選擇了 Joseph form 的更新方程。

最後，我們用一個表格對障礙物感知部分做個簡單的小結

模塊入口的選擇

我在分析 Apollo 時將重點放在了模塊的實現和解決方案上，而非代碼的編寫技巧。因此關於代碼只在本篇最後簡單說一下上篇裏提到過的函數入口的問題。

Preception 模塊的入口

Preception 模塊的主入口其實還是很明確的，爲：

modules/perception/main.cc

APOLLO_MAIN(apollo::perception::Perception);

這是一個宏，展開之後爲

int main(int argc, char **argv) {                            
    google::InitGoogleLogging(argv[0]);                        
    google::ParseCommandLineFlags(&argc, &argv, true);         
    signal(SIGINT, apollo::common::apollo_app_sigint_handler); 
    APP apollo_app_;                                           
    ros::init(argc, argv, apollo_app_.Name());                 
    apollo_app_.Spin();                                        
    return 0;                                                  
  }

關於這段代碼的具體含義，強烈建議參考 知行合一2018的文章 Apollo Planning模塊源代碼分析。值得一提的是，Apollo 大部分模塊的都是以類似的形式開始的，分析的方式也類似。

感知模塊的初始化代碼如下所示：

Status Perception::Init() {
  AdapterManager::Init(FLAGS_perception_adapter_config_filename);

  RegistAllOnboardClass();
  /// init config manager
  ConfigManager* config_manager = ConfigManager::instance();
  if (!config_manager->Init()) {
    AERROR << "failed to Init ConfigManager";
    return Status(ErrorCode::PERCEPTION_ERROR, "failed to Init ConfigManager.");
  }
  AINFO << "Init config manager successfully, work_root: "
        << config_manager->work_root();
  //---------------------------注意這段-------------------------------
  const std::string dag_config_path = apollo::common::util::GetAbsolutePath(
      FLAGS_work_root, FLAGS_dag_config_path);   

  if (!dag_streaming_.Init(dag_config_path)) {
    AERROR << "failed to Init DAGStreaming. dag_config_path:"
           << dag_config_path;
 //-----------------------------分割線---------------------------------------
    return Status(ErrorCode::PERCEPTION_ERROR, "failed to Init DAGStreaming.");
  }
  callback_thread_num_ = 5;

  return Status::OK();
}

程序啓動代碼爲：

Status Perception::Start() {
  dag_streaming_.Start();
  return Status::OK();
}

從這兩段中可以看到有向無環圖 ( directed acyclic graph , DAG )的身影。其實在官方文檔裏就有提到，感知模塊的框架是基於 DAG 圖的，每一個功能都以 DAG 中的 sub-node 的形式出現[7]。

The perception framework is a directed acyclic graph (DAG). There are three components in DAG configuration, including sub-nodes, edges and shared data. Each function is implemented as a sub-node in DAG. The sub-nodes that share data have an edge from producer to customer.

DAG 圖由以下幾個 Subnode 構成

障礙物感知：
- LidarProcessSubnode
- RadarProcessSubnode
- FusionSubnode

交通燈檢測

• TLPreprocessorSubnode
• TLProcSubnode

DAG 圖如下圖所示：

那麼到現在爲止，其實各部分的程序實現的入口應該比較明朗了，只要找到各個Subnode的執行代碼即可。

Doxygen

百度提供了 Doxygen 的文檔系統，十分有助於我們瞭解具體各部分代碼之間的關係[8]。

Doxygen是一種開源跨平臺的，以類似JavaDoc風格描述的文檔系統，完全支持C、C++、Java、Objective-C和IDL語言，部分支持PHP、C#。註釋的語法與Qt-Doc、KDoc和JavaDoc兼容。Doxygen可以從一套歸檔源文件開始，生成HTML格式的在線類瀏覽器，或離線的LATEX、RTF參考手冊 Doxygen_百度百科

Apollo 的 Doxygen 的網址
如圖爲障礙物感知 （obstacle） 的 Directory dependency graph。

由圖片可見，障礙物感知調用了 Radar, Lidar 和 Fusion 的相關函數，其入口在 onboard 文件夾中。我們來看下 onboard 文件夾中有哪些文件。

.
├── BUILD
├── fusion_subnode.cc
├── fusion_subnode.h
├── hdmap_input.cc
├── hdmap_input.h
├── hdmap_input_test.cc
├── lidar_process.cc
├── lidar_process.h
├── lidar_process_subnode.cc
├── lidar_process_subnode.h
├── lidar_process_test.cc
├── object_shared_data.h
├── obstacle_perception.cc
├── obstacle_perception.h
├── radar_process_subnode.cc
├── radar_process_subnode.h
└── sensor_raw_frame.h

可以推斷，這些文件就是障礙物感知涉及到的 Subnode 的執行代碼了。我是選取的以下3個文件作爲分析的入口。

• Radar -> radar_process_subnode.cc
• Lidar -> lidar_process_subnode.cc
• Fusion -> fusion_subnode.cc

這裏其實也遺留了一些問題，由於我並不打算進行交通燈感知的分析，直接跳到這裏開始分析障礙物感知部分。因而造成一個問題是， 我不太清楚某些文件的具體作用。

比如 obstacle_perception.cc，這個文件看着很像入口，描述也很像入口, 內容也很像入口，但是和 Subnode 的框架似乎沒什麼關係。

/**
   * @brief The main process to detect, recognize and track objects
   * based on different kinds of sensor data.
   * @param frame Sensor data of one single frame
   * @param out_objects The obstacle perception results
   * @return True if process successfully, false otherwise
   */
  bool Process(SensorRawFrame* frame, std::vector<ObjectPtr>* out_objects);

這個函數中直接調用了障礙物感知的3個核心函數

1. lidar_perception_->Process(velodyne_frame->timestamp_,
                                    velodyne_frame->cloud_, velodyne_pose);
2. radar_detector_->Detect(radar_frame->raw_obstacles_, map_polygons,
                                options, &objects);
3. fusion_->Fuse(multi_sensor_objs, &fused_objects);

再比如 lidar_process.cc，這個文件內容與 lidar_process_subnode.cc 的內容相似，但也不完全相同，也令人感到疑惑。十分希望大家可以給我一些指點。