介紹

自動駕駛是現(xiàn)代技術(shù)中一個相對較新且非常迷人的領域。在2004年的DARPA Grand Challenge期間公開展示，并在2007年轉(zhuǎn)向更具挑戰(zhàn)性的城市環(huán)境，自那以后，工業(yè)界和學術(shù)界一直在追求自動駕駛。

這些應用程序在個人自動駕駛汽車、自動出租車、運輸、送貨等方面都有所不同，但這項技術(shù)還沒有成熟。

自動駕駛陷入低谷的原因之一是，感知組件是一個非常復雜的問題。雖然大多數(shù)團隊都采用基于激光雷達的感知方式，但仍有人試圖通過相機來感知（Tesla 和 Wayve）。

依賴激光雷達的解決方案也可以分為兩類：處理點云的傳統(tǒng)計算機視覺算法和基于深度學習的方法。

神經(jīng)網(wǎng)絡有望以較高的平均精度解決感知問題，然而，如果我們想在最壞的情況下證明合理的準確性，這是不夠的。

在本文中，我們將看一看在PCL（一個開源的點云庫）的幫助下制作的自動駕駛堆棧。

首先，我們將堅持系統(tǒng)級的測試驅(qū)動開發(fā)（TDD），以確保在第一次現(xiàn)場部署之前對我們的整個代碼進行徹底測試。

為此，我們需要一個數(shù)據(jù)集來運行代碼。卡爾斯魯厄理工學院（Karlsruhe Institute of Technology）和芝加哥豐田理工學院（Toyota Technology Institute）2012年的經(jīng)典數(shù)據(jù)集Kitti將非常適合這一目的。這是首批收集的大規(guī)模高質(zhì)量數(shù)據(jù)集之一，可作為自動駕駛領域計算機視覺算法的基準。

Kitti跟蹤由21個同步PNG圖像序列、Velodyne激光雷達掃描和來自RT3003 GPS－IMU模塊的NMEA記錄組成。

數(shù)據(jù)集的一個重要特征是傳感器之間的徹底相互校準，包括矩陣“Tr＿imu＿velo”，它是從GPS－imu坐標到Velodyne激光雷達坐標的轉(zhuǎn)換。

感知管道的架構(gòu)如下所示。

讓我們分別討論每一個組件，深入挖掘他們的C＋＋實現(xiàn)。

點云抽取

為什么我們可能需要從深度傳感器（可能是一個或幾個激光雷達）中抽取點云？

自動駕駛軟件最重要的要求是滿足實時操作約束。

第一個要求是處理管道要跟上激光雷達掃描采樣的速率。在現(xiàn)實生活中，掃描速度可能從10到25次／秒不等，這導致最大延遲為100毫秒到40毫秒不等。如果某些操作導致延遲超過100 ms（對于每秒10次掃描的速度），要么會發(fā)生幀丟失，要么管道的總延遲將開始任意增長。這里的解決方案之一是丟掉一些點，而不是丟失整個幀。這將逐漸降低準確性指標（召回率和精度），并保持管道實時運行。

第二個要求是系統(tǒng)的總體延遲或反應時間。同樣，總延遲應該被限制在至少100或200毫秒。對于自動駕駛來說，500ms甚至1秒的反應時間是不可接受的。因此，在算法設計開始時，首先采用抽取的方法處理少量的點是有意義的。

抽取的標準選項包括：

1． 有規(guī)律的

2． （偽）隨機

3． 格柵下采樣

常規(guī)下采樣速度很快，但可能會導致點云上的鋸齒模式。隨機或偽隨機下采樣也很快，但可能會導致不可預測的小對象完全消失。像PCL的pcl：：VoxelGrid＜＞類一樣的格柵下采樣是智能和自適應的，但需要額外的計算和內(nèi)存。

原始點云：

大量點云：

多掃描聚合

多掃描聚合是指當車相對于地面移動時，將多個歷史激光雷達掃描記錄到共同坐標系的過程。通用的坐標系統(tǒng)可以是局部導航框架或當前的激光雷達傳感器坐標。我們將以后者為例。

這個階段在理論上是可選的，但在實踐中是非常重要的。問題是，后續(xù)的聚類階段依賴于LiDAR點的密度，如果密度不夠，可能會產(chǎn)生過聚類的影響。過聚類意味著任何對象（汽車、公共汽車、建筑墻等）都可以被分割成幾個部分。

就其本身而言，這可能不是一個檢測障礙的問題，然而，對于感知－跟蹤－聚類的下游模塊來說，這是一個實質(zhì)性的挑戰(zhàn)。跟蹤器可能會不準確地關(guān)聯(lián)對象的各個部分，這最終導致車輛突然剎車。我們絕對不希望聚類中的小錯誤在下游組件中造成雪崩式的錯誤。

多次連續(xù)掃描（5到10次）的聚合成比例地增加了落在每個物體上的激光雷達點的密度，并促進了精確的聚類。汽車運動的一個很好的特點是，汽車能夠從不同的視角觀察同一物體，激光雷達掃描模式覆蓋物體的不同部分。

讓我們看看執(zhí)行聚合的代碼。

第一階段是保留一個限制長度的隊列，其中包含歷史點云以及后續(xù)掃描儀的姿勢轉(zhuǎn)換。請注意，我們?nèi)绾问褂脧腞T3003 GPS－IMU模塊獲得的平移速度［Vx，Vy］和旋轉(zhuǎn)速度Wz來構(gòu)造姿勢變換。

／／ We accumulate the incoming scans along with their localization metadata

／／ into a deque to perform subsequent aggregation．

｛

  Transform3f next＿veh＿pose＿vs＿curr ＝ Transform3f：：Identity（）；

if （gpsimu＿ptr）

｛

    float frame＿interval＿sec ＝ 0．1f；

    ／／ First， we need to calculate yaw change given the yaw rate

    ／／ （angular speed over Z axis） and the time inteval between frames．

    float angle＿z ＝ gpsimu＿ptr－＞wz ＊ frame＿interval＿sec；

    auto rot ＝ Eigen：：AngleAxisf（angle＿z， Eigen：：Vector3f：：UnitZ（））；
           next＿veh＿pose＿vs＿curr．rotate（rot）；

    ／／ Second， we need a translation transform to the next frame

    ／／ given the speed of the ego－vehicle and the frame interval．

    next＿veh＿pose＿vs＿curr．translate（Eigen：：Vector3f（
              gpsimu＿ptr－＞vf ＊ frame＿interval＿sec，
              gpsimu＿ptr－＞vl ＊ frame＿interval＿sec，

       0．0f
           ））；

  ｝

／／ Since later we want to aggregate all scans into the coordinate

／／ frame of the last scans， we need the inverse transform．

auto curr＿veh＿pose＿vs＿next ＝ next＿veh＿pose＿vs＿curr．inverse（）；

／／ Put the resulting pair of the cloud and the transform into a queue．

auto cloud＿and＿metadata ＝ CloudAndMetadata｛decimated＿cloud＿ptr， curr＿veh＿pose＿vs＿next｝；

m＿queue．push＿back（cloud＿and＿metadata）；

while （m＿queue．size（） ＞ m＿params－＞m＿num＿clouds）

｛

      m＿queue．pop＿front（）；

  ｝

｝

在第二階段，我們從最新的掃描時間向后遍歷隊列，進行聚合，并將聚合轉(zhuǎn)換應用到每個歷史幀。

使用這種方法，計算成本為O（N＊D），其中N是點的數(shù)量，D是歷史的深度（掃描的數(shù)量）。

／／ We accumulate the transforms starting from the latest back in time and

／／ transform each historical point cloud into the coordinates of the current frame．

auto aggregated＿cloud＿ptr ＝ std：：make＿shared＜pcl：：PointCloud＜pcl：：PointXYZI＞ ＞（）；

Eigen：：Matrix4f aggragated＿transform ＝ Eigen：：Matrix4f：：Identity（）；

for （int i ＝ m＿queue．size（）－1； i ＞＝ 0； i－－）

｛

    constauto＆ cloud＿and＿metadata ＝ m＿queue［i］；

    constauto＆ cloud＿ptr ＝ cloud＿and＿metadata．cloud＿ptr；

    constauto＆ trans ＝ cloud＿and＿metadata．transform＿to＿next；

    pcl：：PointCloud＜pcl：：PointXYZI＞：：Ptr transformed＿cloud＿ptr；

    if （i �。� m＿queue．size（）－1）

｛

      aggragated＿transform ＊＝ trans．matrix（）；

      transformed＿cloud＿ptr ＝ std：：make＿shared＜pcl：：PointCloud＜pcl：：PointXYZI＞ ＞（）；

      pcl：：transformPointCloud（＊cloud＿ptr， ＊transformed＿cloud＿ptr， aggragated＿transform）；

  ｝

else

  ｛

    ／／ For the current scan no need to transform
           transformed＿cloud＿ptr ＝ cloud＿ptr；

  ｝

／／ Concatenate the transformed point cloud into the aggregate cloud

  ＊aggregated＿cloud＿ptr ＋＝ ＊transformed＿cloud＿ptr；

｝

聚合后，如果移動的物體看起來有點模糊，點云會顯得有些模糊�？梢栽诰垲愲A段進一步解決。在這個階段，我們需要的是一個更密集的點云，它可以從多個幀中積累信息。

地面移除

感知堆棧的目的是提供有關(guān)動態(tài)對象和靜止障礙物的信息。汽車應該在道路上行駛，通常路面不被視為障礙物。

因此，我們可以移除所有從路面反射的激光雷達點。要做到這一點，我們首先將地面檢測為平面或曲面，并移除表面周圍或下方約10厘米的所有點。有幾種方法可以檢測點云上的地面：

1． 用Ransac探測平面

2． 用Hough變換檢測平面

3． 基于Floodfill的非平面表面檢測

讓我們在EGIN和PCL庫的幫助下，研究RANSAC的C＋＋實現(xiàn)。

首先，讓我們定義候選平面。我們將使用基點加法向量的形式。

／／ A plane is represented with a point on the plane （base＿point）

／／ and a normal vector to the plane．

struct Plane

｛

    Eigen：：Vector3f base＿point；

    Eigen：：Vector3f normal；

    EIGEN＿MAKE＿ALIGNED＿OPERATOR＿NEW

｝；

然后，我們定義了一個輔助函數(shù)，它允許我們在點云轉(zhuǎn)換為平面坐標后，在Z坐標上找到滿足條件的所有點的索引。代碼中的注釋給出了實現(xiàn)的細節(jié)。

／／ This helper function finds indices of points that are considered inliers，

／／ given a plane description and a condition on distance from the plane．

std：：vector＜size＿t＞ find＿inlier＿indices（

  const pcl：：PointCloud＜pcl：：PointXYZ＞：：Ptr＆ input＿cloud＿ptr，

  const Plane＆ plane，

  std：：function＜bool（float）＞ condition＿z＿fn）

｛

  typedef Eigen：：Transform＜float， 3， Eigen：：Affine， Eigen：：DontAlign＞ Transform3f；

  auto base＿point ＝ plane．base＿point；

  auto normal ＝ plane．normal；

  ／／ Before rotation of the coordinate frame we need to relocate the point cloud to

  ／／ the position of base＿point of the plane．

  Transform3f world＿to＿ransac＿base ＝ Transform3f：：Identity（）；

  world＿to＿ransac＿base．translate（－base＿point）；

  auto ransac＿base＿cloud＿ptr ＝ std：：make＿shared＜pcl：：PointCloud＜pcl：：PointXYZ＞ ＞（）；

  pcl：：transformPointCloud（＊input＿cloud＿ptr， ＊ransac＿base＿cloud＿ptr， world＿to＿ransac＿base）；

  ／／ We are going to use a quaternion to determine the rotation transform

  ／／ which is required to rotate a coordinate system that plane＇s normal

  ／／ becomes aligned with Z coordinate axis．

  auto rotate＿to＿plane＿quat ＝ Eigen：：Quaternionf：：FromTwoVectors（
            normal，
             Eigen：：Vector3f：：UnitZ（

  ）．normalized（）；

  ／／ Now we can create a rotation transform and align the cloud that

  ／／ the candidate plane matches XY plane

  Transform3f ransac＿base＿to＿ransac ＝ Transform3f：：Identity（）；

  ransac＿base＿to＿ransac．rotate（rotate＿to＿plane＿quat）；

  auto aligned＿cloud＿ptr ＝ std：：make＿shared＜pcl：：PointCloud＜pcl：：PointXYZ＞ ＞（）；

  pcl：：transformPointCloud（＊ransac＿base＿cloud＿ptr， ＊aligned＿cloud＿ptr， ransac＿base＿to＿ransac）；

  ／／ Once the point cloud is transformed into the plane coordinates，

  ／／ We can apply a simple criterion on Z coordinate to find inliers．

  std：：vector＜size＿t＞ indices；

  for （size＿t i＿point ＝ 0； i＿point ＜ aligned＿cloud＿ptr－＞size（）； i＿point＋＋）

｛

      constauto＆ p ＝ （＊aligned＿cloud＿ptr）［i＿point］；

     if （condition＿z＿fn（p．z））

   ｛

          indices．push＿back（i＿point）；

     ｝

   ｝

   return indices；

｝

最后，主要的Ransac實現(xiàn)如下所示。第一步是基于Z坐標對點進行粗略過濾。此外，我們需要再次抽取點，因為我們不需要聚集云中的所有點來驗證候選平面。這些操作可以一次完成。

接下來，我們開始迭代。在C＋＋標準庫的 std：：mt19937偽隨機生成器的幫助下，每次迭代采樣3個隨機點。對于每個三元組，我們計算平面并確保其法線指向上方。然后我們使用相同的輔助函數(shù)find＿inlier＿index來計算內(nèi)點的數(shù)量。

迭代結(jié)束后，我們剩下的是最佳候選平面，我們最終使用它來復制點云中所有索引不存在于列表中的點的副本。請注意std：：unordered＿set＜＞的用法。它允許執(zhí)行恒定時間O（1）搜索，而不是對std：：vector＜＞進行的線性O（N）搜索。

／／ This function performs plane detection with RANSAC sampling of planes

／／ that lie on triplets of points randomly sampled from the cloud．

／／ Among all trials the plane that is picked is the one that has the highest

／／ number of inliers． Inlier points are then removed as belonging to the ground．

auto remove＿ground＿ransac（

    pcl：：PointCloud＜pcl：：PointXYZ＞：：Ptr input＿cloud＿ptr）

｛

  ／／ Threshold for rough point dropping by Z coordinate （meters）

  constfloat rough＿filter＿thr ＝ 0．5f；

  ／／ How much to decimate the input cloud for RANSAC sampling and inlier counting

  constsize＿t decimation＿rate ＝ 10；

  ／／ Tolerance threshold on the distance of an inlier to the plane （meters）

  constfloat ransac＿tolerance ＝ 0．1f；

  ／／ After the final plane is found this is the threshold below which all

  ／／ points are discarded as belonging to the ground．

  constfloat remove＿ground＿threshold ＝ 0．2f；

  ／／ To reduce the number of outliers （non－ground points） we can roughly crop

  ／／ the point cloud by Z coordinate in the range （－rough＿filter＿thr， rough＿filter＿thr）．

  ／／ Simultaneously we perform decimation of the remaining points since the full

  ／／ point cloud is excessive for RANSAC．

  std：：mt19937：：result＿type decimation＿seed ＝ 41；

  std：：mt19937 rng＿decimation（decimation＿seed）；

  auto decimation＿gen ＝ std：：bind（

       std：：uniform＿int＿distribution＜size＿t＞（0， decimation＿rate）， rng＿decimation）；

  auto filtered＿ptr ＝ std：：make＿shared＜pcl：：PointCloud＜pcl：：PointXYZ＞ ＞（）；

  for （constauto＆ p ： ＊input＿cloud＿ptr）

｛

       if （（p．z ＞ －rough＿filter＿thr） ＆＆ （p．z ＜ rough＿filter＿thr））
            ｛

           ／／ Use random number generator to avoid introducing patterns

          ／／ （which are possible with structured subsampling

          ／／ like picking each Nth point）．

           if （decimation＿gen（） ＝＝ 0）
               ｛
                        filtered＿ptr－＞push＿back（p）；
                 ｝
            ｝
        ｝

  ／／ We need a random number generator for sampling triplets of points．

  std：：mt19937：：result＿type sampling＿seed ＝ 42；

  std：：mt19937 sampling＿rng（sampling＿seed）；

  auto random＿index＿gen ＝ std：：bind（

       std：：uniform＿int＿distribution＜size＿t＞（0， filtered＿ptr－＞size（））， sampling＿rng）；

  ／／ Number of RANSAC trials

  constsize＿t num＿iterations ＝ 25；

  ／／ The best plane is determined by a pair of （number of inliers， plane specification）

  typedefstd：：pair＜size＿t， Plane＞ BestPair；

  auto best ＝ std：：unique＿ptr＜BestPair＞（）；

  for （size＿t i＿iter ＝ 0； i＿iter ＜ num＿iterations； i＿iter＋＋）
      ｛

       ／／ Sample 3 random points．

      ／／ pa is special in the sense that is becomes an anchor － a base＿point of the plane
              Eigen：：Vector3f pa ＝ （＊filtered＿ptr）［random＿index＿gen（）］．getVector3fMap（）；
              Eigen：：Vector3f pb ＝ （＊filtered＿ptr）［random＿index＿gen（）］．getVector3fMap（）；
              Eigen：：Vector3f pc ＝ （＊filtered＿ptr）［random＿index＿gen（）］．getVector3fMap（）；

       ／／ Here we figure out the normal to the plane which can be easily calculated
             ／／ as a normalized cross product．
              auto vb ＝ pb － pa；
              auto vc ＝ pc － pa；
              Eigen：：Vector3f normal ＝ vb．cross（vc）．normalized（）；

      ／／ Flip the normal if points down
             if （normal．dot（Eigen：：Vector3f：：UnitZ（）） ＜ 0）
            ｛
                   normal ＝ －normal；
              ｝
      

      Plane plane｛pa， normal｝；

    ／／ Call find＿inlier＿indices to retrieve inlier indices．
          ／／ We will need only the number of inliers．
          auto inlier＿indices ＝ find＿inlier＿indices（filtered＿ptr， plane，
                ［ransac＿tolerance］（float z） －＞ bool ｛
                         return （z ＞＝ －ransac＿tolerance） ＆＆ （z ＜＝ ransac＿tolerance）；
                  ｝）；

          ／／ If new best plane is found， update the best
          bool found＿new＿best ＝ false；
          if （best）
         ｛
                  if （inlier＿indices．size（） ＞ best－＞first）
                ｛
                          found＿new＿best ＝ true；
                  ｝
           ｝
          else
         ｛
                ／／ For the first trial update anyway
                found＿new＿best ＝ true；
           ｝

   if （found＿new＿best）
        ｛
                  best ＝ std：：unique＿ptr＜BestPair＞（new BestPair｛inlier＿indices．size（）， plane｝）；
          ｝

  ｝

  ／／ For the best plane filter out all the points that are

 ／／ below the plane ＋ remove＿ground＿threshold．

  pcl：：PointCloud＜pcl：：PointXYZ＞：：Ptr cloud＿no＿ground＿ptr；
if （best）
      ｛
                cloud＿no＿ground＿ptr ＝ std：：make＿shared＜pcl：：PointCloud＜pcl：：PointXYZ＞ ＞（）；
                auto inlier＿indices ＝ find＿inlier＿indices（input＿cloud＿ptr， best－＞second，
                     ［remove＿ground＿threshold］（float z） －＞ bool ｛
                            return z ＜＝ remove＿ground＿threshold；
                       ｝）；
                std：：unordered＿set＜size＿t＞ inlier＿set（inlier＿indices．begin（）， inlier＿indices．end（））；
                for （size＿t i＿point ＝ 0； i＿point ＜ input＿cloud＿ptr－＞size（）； i＿point＋＋）
              ｛
                       bool extract＿non＿ground ＝ true；
                       if （（inlier＿set．find（i＿point） ＝＝ inlier＿set．end（）） ＝＝ extract＿non＿ground）
                     ｛
                             constauto＆ p ＝ （＊input＿cloud＿ptr）［i＿point］；
                             cloud＿no＿ground＿ptr－＞push＿back（p）；
                       ｝
                ｝
           ｝
          else
         ｛
               cloud＿no＿ground＿ptr ＝ input＿cloud＿ptr；
           ｝

    return cloud＿no＿ground＿ptr；

｝

讓我們看看地面移除的結(jié)果。

在移除地面之前：

地面移除后：

移除地面后，我們準備對剩余的點進行聚類，并通過凸包提取來壓縮對象元數(shù)據(jù)。這兩個階段應該有自己的文章。我將在即將到來的第二部分中介紹它們的實現(xiàn)。同時下面是聚類的最終結(jié)果——凸包提取。

可視化的最終對象：

凸包絕對是任何跟蹤器都渴望接受作為其輸入的元數(shù)據(jù)類型。它們在RAM使用方面更加緊湊，并且比定向邊界框更準確地表示對象的邊界。

KITTI 0003中的聚類點云：

結(jié)論

我相信，在生活質(zhì)量和整體生產(chǎn)力方面，自動駕駛將是人類的一次飛躍。

參考資料：

感謝閱讀！

       原文標題 : 自動駕駛：基于PCL的激光雷達感知