介紹

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

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

自動(dòng)駕駛陷入低谷的原因之一是，感知組件是一個(gè)非常復(fù)雜的問(wèn)題。雖然大多數(shù)團(tuán)隊(duì)都采用基于激光雷達(dá)的感知方式，但仍有人試圖通過(guò)相機(jī)來(lái)感知（Tesla 和 Wayve）。

依賴激光雷達(dá)的解決方案也可以分為兩類：處理點(diǎn)云的傳統(tǒng)計(jì)算機(jī)視覺算法和基于深度學(xué)習(xí)的方法。

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

在本文中，我們將看一看在PCL（一個(gè)開源的點(diǎn)云庫(kù)）的幫助下制作的自動(dòng)駕駛堆棧。

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

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

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

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

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

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

點(diǎn)云抽取

為什么我們可能需要從深度傳感器（可能是一個(gè)或幾個(gè)激光雷達(dá)）中抽取點(diǎn)云？

自動(dòng)駕駛軟件最重要的要求是滿足實(shí)時(shí)操作約束。

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

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

抽取的標(biāo)準(zhǔn)選項(xiàng)包括：

1． 有規(guī)律的

2． （偽）隨機(jī)

3． 格柵下采樣

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

原始點(diǎn)云：

大量點(diǎn)云：

多掃描聚合

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

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

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

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

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

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

／／ 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（）；

  ｝

｝

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

使用這種方法，計(jì)算成本為O（N＊D），其中N是點(diǎ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；

｝

聚合后，如果移動(dòng)的物體看起來(lái)有點(diǎn)模糊，點(diǎn)云會(huì)顯得有些模糊。可以在聚類階段進(jìn)一步解決。在這個(gè)階段，我們需要的是一個(gè)更密集的點(diǎn)云，它可以從多個(gè)幀中積累信息。

地面移除

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

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

1． 用Ransac探測(cè)平面

2． 用Hough變換檢測(cè)平面

3． 基于Floodfill的非平面表面檢測(cè)

讓我們?cè)贓GIN和PCL庫(kù)的幫助下，研究RANSAC的C＋＋實(shí)現(xiàn)。

首先，讓我們定義候選平面。我們將使用基點(diǎ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＿M(jìn)AKE＿ALIGNED＿OPERATOR＿NEW

｝；

然后，我們定義了一個(gè)輔助函數(shù)，它允許我們?cè)邳c(diǎn)云轉(zhuǎn)換為平面坐標(biāo)后，在Z坐標(biāo)上找到滿足條件的所有點(diǎn)的索引。代碼中的注釋給出了實(shí)現(xiàn)的細(xì)節(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實(shí)現(xiàn)如下所示。第一步是基于Z坐標(biāo)對(duì)點(diǎn)進(jìn)行粗略過(guò)濾。此外，我們需要再次抽取點(diǎn)，因?yàn)槲覀儾恍枰�聚集云中的所有點(diǎn)來(lái)驗(yàn)證候選平面。這些操作可以一次完成。

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

迭代結(jié)束后，我們剩下的是最佳候選平面，我們最終使用它來(lái)復(fù)制點(diǎn)云中所有索引不存在于列表中的點(diǎn)的副本。請(qǐng)注意std：：unordered＿set＜＞的用法。它允許執(zhí)行恒定時(shí)間O（1）搜索，而不是對(duì)std：：vector＜＞進(jìn)行的線性O(shè)（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é)果。

在移除地面之前：

地面移除后：

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

可視化的最終對(duì)象：

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

KITTI 0003中的聚類點(diǎn)云：

結(jié)論

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

參考資料：

感謝閱讀！

       原文標(biāo)題 : 自動(dòng)駕駛：基于PCL的激光雷達(dá)感知