參考鏈接：3D obstacle Perception

1 感知Perception概覽

整個apollo perception傳感器架構(gòu)圖如下：

整個感知模塊的硬件方面包括了多個相機、毫米波雷達（前后）、激光雷達；在算法方面使用了單傳感器、多傳感器融合后的檢測、識別，能夠?qū)φ系K物進行分類、跟蹤和運動軌跡的預測，除此之外還能給出障礙物的運動信息（前進方向、加速度等），給出車道相與本車的相對位置。

各傳感器數(shù)據(jù)流細節(jié)如下圖：

1.1 感知模塊 Input

• 128線激光雷達數(shù)據(jù)
• 16線激光雷達數(shù)據(jù)
• 毫米波雷達數(shù)據(jù)
• 圖像數(shù)據(jù)（相機傳感器）
• 各傳感器外參標定數(shù)據(jù)
• 各相機內(nèi)參標定數(shù)據(jù)
• 車輛加速度和角加速度數(shù)據(jù)

1.2 感知模塊 Output

• 帶航向、速度、分類信息的3D障礙物軌跡
• 紅綠燈檢測和識別

2 3D障礙物感知模塊

3D障礙物模塊主要由三個模塊組成：

• 激光雷達感知
• 毫米波雷達感知
• 障礙物結(jié)果融合

2.1 激光雷達障礙物感知

2.1.1 感知流程

下面的步驟展示了Apollo框架中從激光雷達獲取點云數(shù)據(jù)后的3D障礙物感知處理流程：

• 高精地圖ROI（Region of Interest）區(qū)域過濾
• CNN分割
• MinBox障礙物邊框構(gòu)建
• HM障礙物跟蹤
• 時序類型融合

2.1.1.1 高精地圖ROI過濾

ROI的作用是確定從高精地圖中檢索獲取的可行駛區(qū)域，可行駛區(qū)域包括路面和路口。高精地圖ROI過濾點云中ROI之外的數(shù)據(jù)，移除背景對象，例如道路周圍的建筑物、數(shù)目，只留下之后處理步驟中需要的數(shù)據(jù)。給定一個高精地圖后，點云之間的聯(lián)系可以確定該點云是否屬于ROI內(nèi)部。每個激光點云點都能通過一張汽車周圍2D量化的查詢表（lookup table, LUT）查詢到。

高精地圖ROI過濾輸入數(shù)據(jù)為由激光雷達捕捉到的點云數(shù)據(jù)，輸出由高精地圖定義的ROI范圍內(nèi)的點云切片，其中，高精地圖的定義為一組多邊形，每個多邊形都是一個有序的點的集合。

Apollo高精地圖ROI過濾通常由三個連續(xù)的模塊組成：

• 坐標轉(zhuǎn)換
  在高精地圖ROI過濾中，數(shù)據(jù)交互由一組多邊形定義，每個多邊形實際上是一組在世界坐標系下有序的點。在點云上執(zhí)行一次詢問要求點云和多邊形在同一個坐標系中呈現(xiàn)，因此，Apollo將來自點云的數(shù)據(jù)和高精地圖中的多邊形均轉(zhuǎn)換到激光雷達的相對坐標系下。

• ROI LUT
  為了判斷一個輸入的點是否處于ROI范圍內(nèi)，Apollo采用了一個網(wǎng)格式的LUT，該LUT將ROI量化成一個鳥瞰2D網(wǎng)格。LUT覆蓋了一個矩形區(qū)域，該矩形區(qū)域與高精地圖邊界上方的一般視圖周圍的預定義空間綁定。那么，LUT代表了與ROI關(guān)于每個網(wǎng)格之間的關(guān)系。為了提高計算效率，Apollo使用掃描線算法和位圖編碼來構(gòu)建ROI LUT。

• 帶ROI LUT的點詢問
  基于ROI LUT，每個輸入點之間的聯(lián)系通過一個兩步確認的方式進行詢問。對于點詢問過程：

• 確認件該點是否在ROI LUT范圍內(nèi)
• 詢問該點所在的網(wǎng)格，了解其與ROI的從屬關(guān)系
• 收集所有屬于ROI的點，輸出它們與對應輸入點云的切片

最后，高精地圖ROI過濾使用到的參數(shù)如下（參數(shù)名，用法，默認值）：

• range，ROI LUT相對于激光雷達傳感器的范圍，120米
• cell_size，2D網(wǎng)格尺寸，0.25米
• extend_dist，ROI從多邊形框延長的距離，0米
• no_edge_table，使用邊界框產(chǎn)生多邊形掩碼，false
• set_roi_service，打開感知模塊下激光雷達模組的ROI服務，true

2.1.1.2 CNN分割

通過高精地圖ROI過濾確定周圍環(huán)境后，Apollo獲取了過濾后的點云，該點云只在ROI范圍內(nèi)，然后將這些點云數(shù)據(jù)投入分割模塊中，該模塊將前景障礙物從點云中檢測和分割出來，包括汽車，卡車，自行車，行人等。

該模塊輸入數(shù)據(jù)是一組點云數(shù)據(jù)，輸出一組與ROI中障礙物對應的一組對象。

Apollo使用一個深度卷積神經(jīng)網(wǎng)絡來檢測和分割障礙物，整個CNN分割由一下連續(xù)部分構(gòu)成：

• 通道特征提取
  給定一個點云處理框架，apollo構(gòu)建了一個相對坐標系下的2D方格鳥瞰視圖，在相對坐標系下，每個預定義范圍內(nèi)的點被量化為一個基于網(wǎng)格坐標的點。經(jīng)過量化后，Apollo計算了每個方格中點的8個統(tǒng)計測量值，這些值將會作為輸入通道特征被投入CNN中。8個統(tǒng)計測量值如下：

  • 方格中的最大高度
  • 方格中最高點的強度
  • 方格中所有點的平均高度
  • 方格中所有點的平均強度
  • 方格中點的數(shù)量
  • 方格中心相對于激光雷達的角度
  • 方格中心與激光雷達的距離
  • 表明該方格空/被占用的二進制標志位

• 基于CNN的障礙物預測
  經(jīng)過通道特征提取后，Apollo使用一個深度全卷積神經(jīng)網(wǎng)絡（FCNN）來預測方格各自方格障礙物屬性，包括相對于潛在對象中心（稱為中心偏移）的偏移位移，客觀程度，真實程度以及障礙物高等。

  模型輸入數(shù)據(jù)為一個W×H×C方格的點云數(shù)據(jù)，其中W代表方格列，H代表方格的行，C代表通道特征的數(shù)量

  FCNN由三層組成：

  • 下游編碼層（特征編碼）
  • 上游解碼層（特征解碼）
  • 障礙物屬性預測（預測器）

  特征編碼器將通道特征圖像（點云）作為輸入且對其空間像素進行連續(xù)向下取樣，不斷抽象特征。之后，特征解碼器對編碼的特征圖像向上取樣至2D方格輸入的空間像素，這一過程能恢復特征圖像空間細節(jié)，方便各方格障礙物屬性預測。

  向下和向上取樣是通過帶非線性激活函數(shù)的堆疊卷積/傳遞層實現(xiàn)的。經(jīng)過解碼器后輸出的屬性內(nèi)容包括：中心偏移，客觀程度，真實程度，障礙物高，類概率。

• 障礙物聚類
  經(jīng)過上面步驟后，Apollo獲取了帶五個屬性的各方格的預測信息，即解碼器輸出的屬性內(nèi)容。為了產(chǎn)生障礙物對象，Apollo構(gòu)建了一個有向圖，基于方格中心偏移的預測結(jié)果，搜尋連接的組件作為候選對象簇。

  其中每個方格都是有向圖中的一個節(jié)點，有向邊界基于該方格的中心偏移預測結(jié)果構(gòu)建，每個方格根據(jù)另一個方格指向它的父節(jié)點。

  在生成所有方格的有向圖后，Apollo采用了一個壓縮聯(lián)合查找算法來高效查找構(gòu)成障礙物的組件，每一個組件都是障礙物簇中的候選者。客觀性表示一個方格是一個有效對象的概率。因此Apollo定義非對象方格的概率在0.5以下，最終過濾掉每個候選簇中的空的方格和非對象方格，余下構(gòu)成障礙物的邊界框。

  一個對象簇可由多個相鄰連接的方格組成，且這些方格的根節(jié)點可能是相鄰的。

  類概率是一個對象簇中所有障礙物（交通工具、行人、自行車人等）的概率總和。根據(jù)最大平均概率，障礙物的類型由對象簇最終分類結(jié)果決定。

• 后處理
  經(jīng)過聚類后，Apollo獲得了一組對象簇，每一個對象簇都包含幾個方格。在后處理步驟中，Apollo首先計算每個候選簇檢測結(jié)果的置信分和對象高，計算由每個簇中方格的真實程度和對象高的平均值構(gòu)成。之后，Apollo移除每個對象簇中相對于預測對象高過高的點，收集有效方格中的點。最后，Apollo移除低置信率或只有少量點的候選簇，輸出最終的障礙物分割。

  cnn分割模塊的參數(shù)路徑于：modules/perception/production/data/perception/lidar/models/cnnseg/velodyne128/cnnseg_param.conf，其中包括的參數(shù)和用法如下（參數(shù)名，用法，默認值）：

• objectness_thresh，在障礙物聚類步驟中過濾客觀程度的閾值，0.5
• model_type，網(wǎng)絡類型例如RTNet表示tensorRT加速網(wǎng)絡，RTNet
• confidence_thresh，后處理步驟中過濾候選簇的檢測置信分數(shù)閾值，0.1
• confidence_range，在相對于激光雷達距離的高質(zhì)量檢測的信任范圍，85米
• height_thresh，后處理步驟中如果點的高度超過預測對象高的過濾閾值，0.5米
• min_pts_num，后處理步驟中過濾候選簇點數(shù)量的閾值，3
• ground_detector，地標檢測類型，SpatioTemporalGroundDetector
• gpu_id，CNN障礙物預測網(wǎng)絡中使用的GPU id，0
• roi_filter，高精地圖下ROI過濾器類型，HdmapROIFilter
• network_param，不同caffe輸入和輸出blob類型，由層預定義
• feature_param{width}，2D網(wǎng)格中X（列）軸的方格數(shù)量，864
• feature_param{width}，2D網(wǎng)格中Y（行）軸的方格數(shù)量，864
• feature_param{min_height}，相對激光雷達的最小高度，-5米
• feature_param{max_height}，相對激光雷達的最小高度，5米
• feature_param{use_intensity_feature}，打開輸入通道強度特征，false
• feature_param{use_constant_feature}，打開輸入通道恒定特征，false
• feature_param{point_cloud_range}，2D網(wǎng)格相對于激光雷達的范圍，90米

2.1.1.3 MinBox障礙物邊框構(gòu)建

該模塊為檢測到的障礙物構(gòu)建邊界框。由于遮擋或距離對激光雷達的影響，點云形成障礙物可能存在稀疏或只覆蓋了障礙物表面的一部分。因此，邊界框構(gòu)建器的工作就是恢復完整的邊界框并給出邊界框點。邊界框的主要目的是估計障礙物的名稱（例如，交通工具），即便點云數(shù)據(jù)是稀疏的。同樣地，邊界框也被用于可視化障礙物。

MinBox算法的思想是在給定多邊形點的邊界后尋找全部區(qū)域。例如，當給定兩個點（形成一條邊界后），Apollo映射其他多邊形點到AB上，建立擁有最大距離的交點對。形成的邊界是邊界框中的一條。然后同樣的方法獲取邊界框的其他邊界。最后通過迭代多邊形的所有邊界，Apollo選擇出一個帶6條邊界的邊界框，選擇擁有最小面積的解作為最終邊界框。

2.1.1.4 HM障礙物跟蹤

該模塊是對分割步驟分割出的障礙物進行跟蹤。通常，該功能通過綜合當前檢測結(jié)果和存在的跟蹤表，形成并更新跟蹤表。如果障礙物不再存在，則刪除舊的跟蹤表；如果存在新檢測的障礙物而跟蹤表上沒有，則產(chǎn)生新的跟蹤表。經(jīng)過關(guān)聯(lián)后，將會對新的跟蹤表上障礙物的運動狀態(tài)進行更新。

HM障礙物跟蹤使用匈牙利算法進行檢測和檢測的關(guān)聯(lián)，使用魯棒卡爾曼濾波（RKF）對運動進行估計。

• 檢測和跟蹤關(guān)聯(lián)
  當把檢測結(jié)果與現(xiàn)有的跟蹤表進行關(guān)聯(lián)時，Apollo構(gòu)建了一個二分圖，且使用匈牙利算法，通過衡量最短距離來查找最優(yōu)的檢測和跟蹤匹配。

• 計算距離關(guān)聯(lián)矩陣
  第一步是先建立距離的關(guān)聯(lián)矩陣。給定檢測對象和跟蹤對象之間的距離會根據(jù)一系列關(guān)聯(lián)特征進行計算，部分HM跟蹤器的距離計算特征有：位置距離（運動），方向距離（運動），邊界框大小距離（外觀），點數(shù)量距離（外觀），直方圖距離（外觀）。除此之外，還有一些重要的距離權(quán)重參數(shù)也被使用，這些參數(shù)結(jié)合了上面提到的關(guān)聯(lián)特征。最終將所有特征和參數(shù)都納入最終的距離測量。

• 通過匈牙利算法進行二分圖匹配
  給定關(guān)聯(lián)距離矩陣后，Apollo構(gòu)建了一個二分圖，并使用匈牙利算法計算最小距離查找出最佳檢測和跟蹤匹配。它解決了O(n^3)時間復雜度內(nèi)的分配問題。為了提高計算性能，匈牙利算法是在將原始二分圖切割成子圖后，通過刪除距離大于合理最大距離閾值的頂點來實現(xiàn)的。

• 跟蹤運動估計
  經(jīng)過檢測和跟蹤關(guān)聯(lián)后，HM對象跟蹤使用魯棒卡爾曼濾波（RKF）和很俗運動模型對當前跟蹤表上的對象進行運動狀態(tài)估計。運動狀態(tài)包括信念錨點和信念速度，與3D位置和3D速度對應。為了克服可能存在的由不完美檢測導致的干擾，魯棒統(tǒng)計技術(shù)被加入到跟蹤濾波算法中。

• 觀察冗余
  作為濾波算法輸入的速度測量是從一系列冗余觀察中挑選出來的，這些觀測值包括錨點漂移，邊界框中心漂移，邊界框角點漂移。冗余觀察增強了濾波測量的穩(wěn)定性，因為所有觀察結(jié)果同時失效出現(xiàn)的概率遠遠低于單個觀察失效。

• 突破
  高斯濾波算法假定噪聲來自高斯分布。然而，這一假設在一個運動估計問題中卻可能不成立，因為測量噪聲可能來自一個厚尾分布。Apollo在過濾過程中使用一個突破閾值來中和更新增益的高估。

• 根據(jù)關(guān)聯(lián)質(zhì)量更新
  原始卡爾曼濾波在更新狀態(tài)時沒有區(qū)分測量質(zhì)量的好壞，然而測量質(zhì)量是過濾過程中一個有益的指標且能夠被估計。例如，關(guān)聯(lián)步驟中的計算距離就可以是一個測量結(jié)果的合理評估。根據(jù)關(guān)聯(lián)質(zhì)量更新濾波算法的狀態(tài)能夠增加運動估計問題中的魯棒性和平滑性。

  一個HM對象跟蹤器的高抽象工作流包括：啟動和預處理，預測和匹配，更新和收集。

  HM對象跟蹤器工作流的主要點包括：

• 構(gòu)建被跟蹤對象，將它們轉(zhuǎn)換到世界坐標
• 預測現(xiàn)有跟蹤表的狀態(tài)并且將檢測結(jié)果與它們匹配
• 更新更新后的跟蹤表的運動狀態(tài)，并收集跟蹤結(jié)果

2.1.1.5 時序類型融合

為平滑障礙物類型、減少整個軌跡中的類型轉(zhuǎn)換，Apollo采用了一個基于線性鏈條件隨機場的時序類型融合算法（a sequential type fusion algorithm based on a linear-chain Conditional Random Field (CRF)），表述如下：



其中一元項對單個節(jié)點操作，二元節(jié)點對每條邊界操作。

一元項中的概率是基于CNN預測的類概率輸出。二元項中的狀態(tài)轉(zhuǎn)移概率由t-1時刻至t時刻障礙物類型轉(zhuǎn)移建模，建模過程是從大量障礙物軌跡中進行統(tǒng)計學習。特別地，Apollo也使用學習后的混淆矩陣來表示從預測類型到實際類型的概率變化，以此優(yōu)化原始基于CNN的類概率。

利用維特比算法（the Viterbi algorithm），連續(xù)的障礙物類型可以通過下面的公式優(yōu)化：

2.1.2 實現(xiàn)細節(jié)

文件順序下的實現(xiàn)流程：

segmentation_component.cc
|—lidar_obstacle_segmentation.cc
| └pointcloud_preprocessor.cc（點云預處理）
| └map_manager.cc（ROI高精地圖獲取）
| └hdmap_input.cc + hdmap.cc + hdmap_impl.cc
| └cnn_segmentation.cc（CNN分割）
| └feature_generator.cc

2.1.2.1 segmentation_component.cc

• bool SegmentationComponent::Init()
  讀取配置文件，做一些變量初始化，創(chuàng)建writer結(jié)點，再初始化一些算法插件。

• bool SegmentationComponent::InitAlgorithmPlugin()
  初始化分割組件segmentor_，計算lidar到novatel的坐標變換矩陣

• bool SegmentationComponent::Proc()
  新建一個writer結(jié)點寫到channel里的out_message，調(diào)用InitAlgorithmPlugin()函數(shù)，將該函數(shù)處理的結(jié)果寫入channel

• bool SegmentationComponent::InternalProc()
  該函數(shù)在前半段主要是做一些變量的更新（時間戳，out_message更新等）

  然后計算了傳感器（激光雷達）到障礙物的坐標變換矩陣、傳感器（激光雷達）到novatel的坐標變換矩陣，所有矩陣存儲在變量lidar2world_trans_中

  最后調(diào)用了segmentor_的Process方法，進入激光雷達分割（lidar_obstacle_segmentation.cc）

2.1.2.2 lidar_obstacle_segmentation.cc

• bool LidarObstacleSegmentation::Init()
  除了配置文件讀取、賦值，還對cloud_preprocessor_, map_manager_, segmentor_, builder_, filter_bank_進行了初始化

• LidarProcessResult LidarObstacleSegmentation::Process()
  該函數(shù)是一個重載函數(shù)，另一個請查閱文件，這里只說被調(diào)用的一個

  該函數(shù)首先通過cloud_preprocessor_調(diào)用了Preprocess()函數(shù)對點云進行預處理，緊接著調(diào)用ProcessCommon()函數(shù)，執(zhí)行ROI地圖過濾、分割、邊界框構(gòu)建、過濾等工作。

• LidarProcessResult LidarObstacleSegmentation::ProcessCommon()
  函數(shù)主要做了以下工作：

• 高精地圖ROI過濾
• CNN點云分割

2.1.2.3 pointcloud_preprocessor.cc

• bool PointCloudPreprocessor::Init()
  讀取配置文件，變量賦值

• bool PointCloudPreprocessor::Preprocess()
  首先對要處理的數(shù)據(jù)做了一些指針非空判斷；然后開始對點云進行過濾，包括點云點中x, y, z任一維度是否為Nan，是否超過最大值范圍（1e3/1000）；接著將汽車周圍一圈（范圍可修改）的零散的點云去掉；最后還可以限制點云高度，將高度超過某一范圍的點云過濾掉。

  經(jīng)過濾后，調(diào)用TransformCloud()函數(shù)，計算了點云在世界坐標系下的每個點云的坐標。

• bool PointCloudPreprocessor::TransformCloud()
  將點云從激光雷達坐標系轉(zhuǎn)換到世界坐標系

2.1.2.4 map_manager.cc

• bool MapManager::Init()
  讀取配置文件，變量賦值和更新；初始化hdmap_input_變量

• bool MapManager::QueryPose()
  該函數(shù)沒寫，但應該更新汽車pose，重新計算變換矩陣，如果時間差很小可以不更新。

• bool MapManager::Update()
  該函數(shù)首先對將要處理的數(shù)據(jù)進行指針非空判斷、變量重置等工作。

  緊接著調(diào)用QueryPose()函數(shù)對汽車pose進行更新。

  最后調(diào)用hdmap_input_對象的GetRoiHDMapStruct()方法，細節(jié)見2.1.2.5；執(zhí)行完該函數(shù)后 ，ROI高精地圖過濾部分算是完成了。

2.1.2.5 hdmap_input.cc + hdmap.cc + hdmap_impl.cc

(部分函數(shù))

• bool HDMapInput::GetRoiHDMapStruct()
  首先判斷判斷指針是否非空；接著調(diào)用hdmap_對象的GetRoadBoundaries()方法

• int HDMap::GetRoadBoundaries()
  直接跳轉(zhuǎn)到impl_對象的GetRoadBoundaries()方法

• int HDMapImpl::GetRoadBoundaries()
  (該方法是一個重載函數(shù))

  首先判斷要處理的數(shù)據(jù)指針是否非空，重置了存儲路面邊界的road_boundaries和存儲路口的junctions變量指針

  緊接著調(diào)用函數(shù)GetRoads()獲取，該函數(shù)首先獲取了道路信息，然后對每條道都進行id判斷，最后通過id來獲取最終所需的道路（我不知道，我亂講的，反正大概這個意思，我沒看，我什么都不知道）；在獲取到道路信息后，對每條道路都判斷是否有路口信息，如果有，則將路口信息取出來。總之，經(jīng)過該函數(shù)后，最終獲取到了路面信息和路口信息。

  在獲取到路面信息和路口信息后，由于這兩類信息分別存儲在兩個變量中，因此緊接著調(diào)用MergeBoundaryJunction()函數(shù)，將存儲路面和路口信息的變量整合到hdmap_struct_ptr指向的變量中；最后調(diào)用 GetRoadBoundaryFilteredByJunctions()利用路口信息過濾掉多余的路面信息。

2.1.2.6 cnn_segmentation.cc

• bool CNNSegmentation::Init()
  讀取cnn分割、推理模型的配置文件，變量賦值，初始化特征生成器feature_generator_、推理inference_、點云映射方格point2grid_

  最后調(diào)用了函數(shù)InitClusterAndBackgroundSegmentation()對聚類和背景分割做初始化

• bool CNNSegmentation::InitClusterAndBackgroundSegmentation()
  初始化了背景檢測器ground_detector_、ROI過濾器roi_filter_、spp網(wǎng)絡spp_engine_、線程優(yōu)化器worker_

  在這里進行了奇怪的一個功能：背景過濾。按理說不應該放在初始化模塊，畢竟初始化模塊還沒接收點云數(shù)據(jù)，所以很奇怪。雖然不知道目的是什么，但我們先放個**？**在這里

• void CNNSegmentation::MapPointToGrid()
  首先計算2D方格中每米有多少個cell；然后對每個點云點進行過濾，超過范圍（+5 -5）則舍棄。

  然后調(diào)用函數(shù)GroupPc2Pixel()，將點云映射到2D網(wǎng)格中。這個函數(shù)一開始沒看明白，后來發(fā)現(xiàn)它并不是將點云的三維數(shù)據(jù)x, y, z變成二維數(shù)據(jù)，而是用x, y這二維數(shù)據(jù)計算出了一個pos_x, pos_y（類似于對數(shù)據(jù)編碼，后面會解碼），并且后面會用這兩個值做一些過濾工作，最終投入網(wǎng)絡進行推理分割。在計算出pos_x和pos_y后，如果pos_x和pos_y的值分別超出范圍(0, 5)，則被過濾掉；沒被過濾掉點使用公式pos_y * width_ + pos_x計算出一個值，存儲到向量point2grid_中，其中point2grid_是一個長度等于所有點云長度的一個向量。

  映射到2D網(wǎng)格后，開始調(diào)用feature_generator_的Generate()方法，利用原始點云original_cloud_和上一步計算的point2grid_生成特征。

  特征生成后，再調(diào)用inference_對象的推理方法Infer()，進行推理

  最后調(diào)用GetObjectsFromSppEngine()函數(shù)，對分割后的對象進行MinBox聚類處理，處理完畢后該函數(shù)執(zhí)行完畢，返回布爾真。

• void CNNSegmentation::GetObjectsFromSppEngine()

• bool CNNSegmentation::GetConfigs()

• bool CNNSegmentation::Segment()
  首先判斷傳入的數(shù)據(jù)指針是否非空，接著判斷激光雷達坐標系下的點云和世界坐標系下的點云指針是否非空，指針非空，再判斷激光點云數(shù)量是否為0，激光點云和世界點云的點數(shù)量是否相同（應該相同）

  然后調(diào)用函數(shù)MapPointToGrid()將點云映射到方格中

  最后進行spp推理，獲取障礙物對象

2.1.2.7 feature_generator.cc

(部分函數(shù))

• void FeatureGenerator::GenerateCPU()
  在接收到original_cloud_和point2grid_后，首先創(chuàng)建了一個864864的向量max_height_data_，并初始化，用于記錄每個cell（一共864864個cell，每個cell包含多個點云點）中最高的點云點的值；然后做了一些內(nèi)存分配。

  接著建立了一個for循環(huán)，循環(huán)次數(shù)為original_cloud_中包含的點云數(shù)量，循環(huán)內(nèi)部對original_cloud_和point2grid_同時進行處理，其中original_cloud_和point2grid_中的點是一一對應的。接著開始遍歷每個點云點，舍掉original_cloud_對應的point2grid_中值為-1的點，記錄每個cell中最高點云的高度于max_height_data_中，并計算了每個cell的平均高度于mean_height_data_中。

2.2 毫米波雷達障礙物感知

考慮來自毫米被雷達的數(shù)據(jù)，可以使用下面基礎的處理步驟。

首先，跟蹤ID應該被擴展，因為Apollo需要全局ID進行障礙物關(guān)聯(lián)。初始毫米波雷達提供只有8位的ID，因此這很難判斷兩幅相連幀中帶相同ID的兩個對象在跟蹤歷史中是否指的是同一個對象，尤其是在經(jīng)歷掉幀后。Apollo使用毫米波雷達提供的測量狀態(tài)來處理這個問題。同時，Apollo為一個遠離的對象分配一個新的跟蹤ID，該遠離的對象在之前幀中擁有和另一個對象相同的ID。

其次，使用假正過濾器來移除噪聲。Apollo利用毫米波雷達數(shù)據(jù)設置了一些閾值來對可能是噪聲的結(jié)果進行過濾。然后，將毫米波雷達數(shù)據(jù)作為統(tǒng)一的對象格式構(gòu)建對象。Apollo通過標定將對象轉(zhuǎn)換成世界坐標系下的對象。原始毫米波雷達提供對象的相對加速度，因此Apollo采用來自定位的主車速度。Apollo使用這兩個指標定義被檢測對象的絕對速度。

最后，高精地圖ROI過濾器被用于獲取感興趣的對象。只有在ROI范圍內(nèi)的對象將被用于融合算法中。

2.3 障礙物結(jié)果融合

傳感器融合模組的作用是融合激光雷達跟蹤結(jié)果和毫米波雷達檢測結(jié)果。Apollo首先根據(jù)障礙物跟蹤ID將傳感器識別結(jié)果和融合結(jié)果進行匹配。然后計算未被匹配的識別結(jié)果和融合結(jié)果之間的關(guān)聯(lián)矩陣以尋求最佳匹配結(jié)果。

對于被匹配的傳感器結(jié)果，使用自適應卡爾曼濾波（AKF）器更新相應的融合對象。對于未被匹配的傳感器結(jié)果，創(chuàng)建一個新的融合對象。移除所有舊的不匹配的融合對象。

2.3.1 融合對象管理

Apollo有發(fā)布傳感器（publish-sensor）的概念。給定的毫米波雷達結(jié)果是緩存的。給定的激光雷達結(jié)果觸發(fā)融合操作。傳感器融合輸出的頻率和發(fā)布傳感器的頻率是相同的。Apollo的發(fā)布傳感器是激光雷達。傳感器結(jié)果根據(jù)傳感器時間戳投入到融合處理管道。Apollo保管所有傳感器的結(jié)果。對象存活時間是為Apollo中不同傳感器對象設定的。一個對象如果在至少一個傳感器中存在，那么該對象的狀態(tài)將保持活躍。Apollo感知模組提供汽車小范圍內(nèi)的激光雷達和毫米波雷達融合結(jié)果和長距離范圍內(nèi)的單毫米波雷達結(jié)果。

2.3.2 傳感器結(jié)果與融合列表關(guān)聯(lián)

當關(guān)聯(lián)傳感器結(jié)果到融合列表時，Apollo首先匹配相同傳感器下的相同跟蹤ID，然后構(gòu)建一個二分圖，使用最小距離損失的匈牙利算法，查找未被匹配的傳感器結(jié)果和融合列表之間的最佳結(jié)果-融合匹配。這里使用的匈牙利算法和HM對象跟蹤使用的匈牙利算法相同。距離損失是由傳感器結(jié)果和融合結(jié)果之間對應錨點的歐氏距離計算得出。

2.3.3 運動融合

Apollo使用帶恒定加速運動模型的自適應卡爾曼濾波（AKF）來估計當前對象的運動。運動狀態(tài)包括信念錨點，信念速度，信念加速度，分別和3D位置，3D速度，3D加速度對應。Apollo只使用來自傳感器的位置和速度作為基礎。在運動融合中，Apollo捕捉所有傳感器結(jié)果的狀態(tài)，通過卡爾曼濾波計算加速度。Apollo提供激光雷達跟蹤器和毫米波雷達檢測器數(shù)據(jù)中的位置和速度的不確定性。Apollo將所有狀態(tài)和不確定性投入自適應卡爾曼濾波器（AKF）中，獲取融合結(jié)果。Apollo使用過濾過程中的突破閾值來中和更新增益中的過度估計。

apollo感知硬件架構(gòu)圖

apollo感知硬件數(shù)據(jù)流細節(jié)圖

總結(jié)

以上是生活随笔為你收集整理的「Apollo」Apollo感知汇总的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。