Apollo的感知融合模塊解析

Apollo的感知融合模塊解析

• 下文主要對百度Apollo的感知模塊的fusion部分進行細緻深入的瞭解，我將結合代碼、流程圖等一起分析，儘可能的將我的認知記錄下來，分享大家。需要注意的是，Apollo版本迭代很快，我這裏的分析是根據2.5_release版本進行分析的，如果後續版本有所變動，以實際代碼爲準。

導讀

• 下文我將按照如下流程對模塊進行分析：
  
  1. 文字簡述
  2. 流程圖
  3. 用函數表達流程
  4. 局部細節分析
  5. 總結

分析

文字簡述

• Apollo的融合模塊入口自然是在perception.cc裏面，這個應不必多說。在perception.cc裏面註冊了兩種融合方法：RegisterFactoryAsyncFusionSubnode()和RegisterFactoryFusionSubnode()。這兩種方法在底層實現上大同小異，只是上層的數據流程有所區別。在2.5版本的啓動腳本可看到，目前啓用的是dag_streaming_lowcost.config，在這個文件內，啓動的是FusionSubnode，而不是AsyncFusionSubnode。
• 在底層實現上，Apollo使用的HM（匈牙利算法）參考，對各個傳感器提取到的各個Object進行匹配，最後將匹配後的數據用kalman進行融合，分配對應的track_id，將處理後的數據分類存儲，爲新的匹配對象進行創建跟蹤對象（track_id），並保存。當然，收尾還是要移除跟蹤失敗的object。

流程圖

• 單純的用大段文字總結不是我的風格，也不便於理解，我還是喜歡用清晰的流程圖來反應程序流程，後續再結合代碼分析具體細節，能夠更直觀的重現代碼開發的過程，也可以更好的幫助讀者理解。

  • Apollo之fusion模塊整體流程
    

  • Apollo的fusion模塊的數據緩存架構
    

  • FuseFrame流程細節圖
    

代碼流程速覽

1. 每幀單獨融合

    for (size_t i = 0; i < frames.size(); ++i) {
        FuseFrame(frames[i]);
      }

2. 將Obj按照前景、背景分揀

    DecomposeFrameObjects(objects, &foreground_objects, &background_objects);

3. 前景融合

    FuseForegroundObjects(&foreground_objects, ref_point, frame->sensor_type,
                          frame->sensor_id, frame->timestamp);

   1. 匈牙利算法匹配

      matcher_->Match(tracks, *foreground_objects, options, &assignments,
                      &unassigned_tracks, &unassigned_objects,
                      &track2measurements_dist, &measurement2tracks_dist);

      1. IdAssign

         • 根據fusion_tracks和sensor_objects的交集，將數據分組成unassigned_fusion_tracks和unassigned_sensor_objects，也是HM前的預處理。

      2. 計算關聯矩陣

         • 這是HM之前最重要的步驟，直接影響後面融合的效果。這裏的HM求得的最優匹配關係就是匹配和最小，而每一條匹配的邊長是多少，就是這裏的關聯矩陣求得的值。如果以後想要對匹配結果優化，這裏是一個很重要的地方，這個關聯繫數能否正確的反應匹配關係？

      3. HM匹配

         1. 拆分成多個二分圖（最小連同區），分別求解最優匹配

           ComputeConnectedComponents(association_mat, max_dist, &fusion_components, &sensor_components);

         1. 對每個子二分圖求解最優匹配

         MinimizeAssignment(loc_mat, &fusion_idxs, &sensor_idxs);

   2. Update

      • 根據前文HM求得的匹配對，使用kalman進行融合，得到PbfTrack，將跟蹤目標、未跟蹤上的等數據分類處理。

      UpdateAssignedTracks(&tracks, *foreground_objects, assignments,       track2measurements_dist);
      
      UpdateUnassignedTracks(&tracks, unassigned_tracks, track2measurements_dist,
                             sensor_type, sensor_id, timestamp);

   3. CreateNewTracks

      • 爲新的跟蹤目標創建PbfTrack，這裏需要注意的是，代碼邏輯僅僅允許是Camera檢測並跟蹤上的的Obj纔可以創建新的PbfTrack，也就是說，Camera的置信度比較高。

      if (FLAGS_use_navigation_mode) {
        if (is_camera(sensor_type)) {
          CreateNewTracks(*foreground_objects, unassigned_objects);
        }
      } else {
        CreateNewTracks(*foreground_objects, unassigned_objects);
      }
      }

4. 移除丟失的跟蹤

    track_manager_->RemoveLostTracks();

局部分析

1. 預處理

   • 前面幾篇大致的講解了Apollo的基本架構，每個傳感器爲一個subnode，在每個subnode裏面，Apollo對其數據也做了一定的預處理，這裏我們以camera爲例：

     //-- @Zuo yolo
     detector_->Multitask(img, CameraDetectorOptions(), &objects, &mask);
     
     //-- @Zuo obj 2d -> 3d
     converter_->Convert(&objects);
     
     //-- @Zuo 根據外參將obj從相機座標系轉換到車輛座標系
     transformer_->Transform(&objects);
     
     tracker_->Associate(img, timestamp, &objects);
     
     //-- @Zuo 使用kalman進行跟蹤
     filter_->Filter(timestamp, &objects);

     • camera裏面的預處理流程大致爲：yolo提取目標 -> 2d目標轉爲3d目標 -> 根據傳感器外參從傳感器座標系轉換到車輛座標系 -> 目標跟蹤（dlf/kcf） -> 使用kalman對目標位置進行修正。

2. 觸發傳感器

   • 進入融合函數ProbabilisticFusion::Fuse()後，可以看到有一個publish_sensor_id_的存在，這裏是什麼意思呢？這也是ProbabilisticFusion和async_fusion的最大區別，前者需要事先設定一個觸發傳感器，預先將每一幀數據分類存儲，直到觸發傳感器傳來數據，則將事先存儲好的數據取出進行融合。而後者則是不管傳感器類型，來一幀就融一幀。

     if (GetSensorType(multi_sensor_objects[i].sensor_type) == publish_sensor_id_)

3. 關係矩陣

   • 在調用HM匹配前，調用了一個函數ComputeAssociationMat，這是用來計算track_objs和sensor_objs之間兩兩的匹配度。這一步至關重要，直接關係到後續的HM融合的效果好壞。前面我說了，在Apollo的整個體系中，使用HM求解的匹配關係是核心，而這裏的HM是求解最小匹配度，而這個匹配度就是ComputeAssociationMat求得的。如何去求匹配度呢，很顯然，最直接的方法就是用兩個obj的中心距離來反映，但是這樣畢竟比較粗糙，我們的世界是三維的，obj還會有速度、朝向等參數，所以Apollo目前使用的方法也是將這一系列的參數按照不同的權重加進來，一起求解得到最後兩兩之間的匹配度。目前來說，2.5版本的這個方法是有bug的，所以說，後續如果對容和效果不滿意，這裏應該是重點。

4. 如何區分同類型的不同設備

   • 在Apollo中，如果有相同類型的不同設備，系統是否會對其數據做區分？怎麼區分？其實，稍微看了Apollo的都清楚，在Apollo裏面有sensor_type和sensor_id，很明顯，這裏是用來區分傳感器的類型和設備號的。但是仔細看代碼，我們發現目前，在整體的框架中，整個Apollo中用的是std::string GetSensorType(SensorType sensor_type)來獲取sensor_id，這很奇葩，完全想不通爲什麼要這麼做。我這裏做的修改是，在PbfSensorFrame、SensorObjects等之前轉換的過程中，直接使用Obj的sensor_id，而不是上述奇葩方法。

5. 傳感器偏愛

   • 整個fusion流程走一遍之後就知道，Apollo對Camera是非常之偏愛，對Radar是非常之苛刻。例如：

     • Radar不能創建新的Obj

     if (FLAGS_use_navigation_mode) {
        if (is_camera(sensor_type)) {
            CreateNewTracks(*foreground_objects, unassigned_objects);
        }
     } else {
        CreateNewTracks(*foreground_objects, unassigned_objects);
     }

     • 在AbleToPublish()中，對Radar檢測到的數據做大量的條件檢測才能允許發佈。

6. 數據類型

   • 在Apollo裏面主要用到下面幾種數據類型來管理目標：Object、SensorObjects、PbfSensorObject、PbfSensorFrame。
     
     • Object：是其它類型的基類型，爲最初Yolo檢測類型出來的原始數據存儲方式。主要包括一些幾何參數等。
     • SensorObjects：是Object的集合，用來管理同一幀下相同傳感器的所有Object集合。用std::vector來管理Object。因爲它是跟着傳感器走的，所以，肯定包含了傳感器的相關參數，如傳感器外參等。
     • PbfSensorObject：是用於管理跟蹤Object。它雖然也是在Object的基礎上進行了包裝，但是可以理解爲是跟Object同一級別的，不過它的應用場景是在跟蹤裏面的數據存儲。相應的，它擴展了相關功能，如invisible_period，用來將超過一定時間的Obj從跟蹤隊列中刪除。
     • PbfSensorFrame：是在PbfSensorObject的基礎上擴展的，他和PbfSensorObject的關係，類似於SensorObjects和Object的關係。用來管理同一傳感器的同一幀的所有被跟蹤上的Obj。也是用std::vector來管理PbfSensorObject，並且多了傳感器外參等。

7. 數據結構

   • 這個其實沒什麼好說的，Apollo裏面的數據結構風格很統一，不外乎map+queue、map+deque、map+vector這幾種方式之一。

8. 變量名解釋

   • idx = ind = index
   • CIPV = closest in-path vehicle

總結

• 在整個融合體系中，從數據分類的角度看，就兩大塊——tracks和sensors。前者爲已經跟蹤上的Obj，後者爲待融合的Obj。
• 融合的關鍵是HM，而HM的關鍵是求所有Sensor_object和Track_object兩兩之間的匹配分數。這個匹配分數怎麼求才能更加真實的反應兩者之間的匹配緊密度？這可能是後續優化的關鍵點。
• 在用HM求得匹配關係後，根據Kalman進行兩兩融合，然後再更新Tracks，至此，整個Fusion模塊基本完結。