顧及桿狀物和車道線的城市道路場景輕量化快速點云自動配準

趙輝友，吳學群，夏永華

（昆明理工大學 國土資源工程學院，云南 昆明 600093）

1 引言

隨著智慧城市和數字化城市的迅猛發展，對各大場景三維信息獲取的效率和精度有著更高的要求，出現了許多可以獲取三維信息的方法，如傅里葉變換輪廓術（Fourier Transform Profilometry，FTP），相移測量輪廓術（Phase Shifting Profilometry，PSP），調制測量輪廓術（Modulation Measurement Profilometry，MMP），空間相位檢測法（Spatial Phase Detection，SPD），鎖相環輪廓法（Phase Lock Loop Profilometry，PLLP），計算莫爾輪廓術（Computer-Generated Moire Profilometry，CGMP），激光雷達距離測量等（Light Detection and Ranging Measurement，Li-DAR）［1-4］。其中FTP 和PSP 應用較為廣泛，但對不同物體表面反射十分敏感，測量精度低，成像慢，效率低，對于復雜動態場景的適應還有待提高；LiDAR 是集成電子機械控制以及計算機技術等信息手段的一門新興技術，主要通過發射和接受激光束，由于光速非?？欤w行時間非常短，因此速度快，精準，成像快。車載移動激光掃描（Mobile Laser Scanning，MLS）能夠快速高效的獲取大場景三維激光點云數據，具有測量精度準、視野廣和效率高等優勢［5］，且不受光照變化影響，能準確獲得深度信息和檢測周圍環境。但車載MLS 在獲取數據時，受掃描角度、動態目標和場景地物之間等遮擋的限制［6］，造成數據不完整，密度不均勻，需要在同一路段進行多次掃描的方式來采集數據，對區域空間信息進行缺漏補測。然而，在城市環境中衛星信號閉塞敏感，多路徑效應可能導致位置誤差大和較大的姿態誤差積累，系統所依賴的定位精度逐漸降低［7］，同一區域重訪車載點云之間存在偏差。為了整合目標場景地物完整的三維幾何紋理信息，需要將多次掃描點云通過點云配準技術使其處于同一坐標系，為后續三維重建做準備。

迄今，Besl等［8］1992 年提出的迭代最近點算法（Iterative Closest Point，ICP）最為經典，但ICP 效率低，需要局部初始粗配準，否則會陷入局部最優導致配準失敗。為了滿足MLS 在實際工程項目的需求，許多專家和學者提出了點云配準算法相關的優化方法［9］。Gressin等［10］對點云進行劃分，對每一段點云使用ICP 完成點云配準，最后合并點云實現配準；但配準誤差會導致塊之間出現偏差，增加誤差積累，約20 cm 的精度。Lu等［11］利用旋轉投影統計（Rotation Projection Statistics，RoPS）來提取關鍵點，并使用幾何一致性方法（Sample Consensus Initial Alignment，SAC-IA）確定關鍵點對應關系，在戶外點云上配準效果較好，在一百六十萬個點配準精度3 cm；但其需要計算整體大量特征和描述，且需要的存儲空間大。Yang等［12］先使用自適應距離聚類算法對點云截面進行聚類，后利用擬合線或圓柱的空間連續性來提取特征線和特征三角形，然后根據相似度進行三角形匹配，并根據幾何約束拒絕不匹配的三角形。最后，該方法通過構造加權無向圖的最小生成樹來配準多視點云，精度達到5 cm 范圍內；但對長度距離、高度和半徑閾值較為敏感。Gonzále等［13］利用反射強度信息，保留反射強度高的交通標志，以交通標志為基元使用ICP 進行配準，配準精度為2 cm；但該方法對采集數據的強度特征要求較高，且強度反射距離也會造成偏差和點稀疏，影響配準精度。李鵬等［14］提出了基于虛擬特征點的擬合算法，以端點擬合、直線擬合完成點云粗配準；但卻只適用于規則幾何的被測物，一千萬個點耗費近67 s，配準精度達到20 cm。閆利等［15］利用遺傳算法進行全局搜索，尋找最優對應關系，再使用ICP 完成車載激光點云的精配準；但在局部搜索尋優花費時間較長，計算復雜度高，一千三百萬點迭代了300次，花費了208 s，配準精度達到5 cm。孫培芪等［16］利用尺度不變特征轉換（Scale Invariant Fea-ture Transformation，SIFT）算法提取關鍵特征點，后使用特征點法向量距離與其夾角進行約束，但提取特征點時間消耗長，在三萬點的模型中花費了37 s，且不適用于大模型。譚舸等［17］利用激光跟蹤儀來輔助實現點云配準，精度達到了1.66 mm；但只適用水平掃描固定物體，對平行地面值影響較大，且存在多站掃描誤差積累，四站花費時間300 s。Li等［18］先提取桿狀物，后使用蒙特卡洛定位（Monte Carlo Localization，MCL）和ICP 進行數據和地圖匹配，在室外大場景數據平均絕對誤差小于0.2 m；但依賴于桿狀物的位置精度，且只提取了固定高度的桿狀物。劉如飛等［19］提取配準基元的SIFT 關鍵點，應用四點全等集算法粗配準和雙向KD 樹改進的ICP算法精配準，完成多期道路點云的配準，配準精度在5 cm 范圍；但沒有考慮同名特征點的配對正確性，且存在誤差積累，600 m 路段耗時近100 s。

綜上，在城市場景中點云配準仍存在對噪聲點敏感，提取表面幾何特征少或提取不完全，配準計算復雜度較高、配準效率低和魯棒性弱等局限性。本文根據城市道路場景中桿狀物和車道線的重復性和長期穩定性，且同名特征配對準確性較高，為保證提取的準確性，利用改進方法準確提取桿狀物和車道線作為目標特征，后利用改進的ICP 算法和法向量約束，將桿狀物作和車道線作為配準基元，利用SAC-IA 算法剔除錯誤點對，并使用雙向KD 樹快速對應的特征點關系，降低了計算復雜度和存儲占用，加快配準速度和提高精度，增強魯棒性。

2 點云配準基本原理

本文首先得到地面點和非地面點；利用地面點反射強度值和密度分割法，將車道線根據格網灰度統計轉為灰度圖，通過方差閾值準確提取車道線；同時為保證桿狀物的準確提取，使用RANSAC 圓柱擬合前先使用包圍盒格網投影，進行遍歷。后提出以桿狀物和車道線特征為配準基元的面到面ICP 改進配準算法，主要使用了Levenberg-Marquardt算法進行非線性迭代優化，并引入Cauchy損失函數作為加權函數來抵抗噪聲和離群點，以面的法向量和特征法向量雙重約束，得到兩個點云的最優配準關系，算法流程如圖1所示。

圖1 本文算法流程Fig.1 Algorithm flow of this article

2.1 桿狀物配準基元提取原理

桿狀物在城市中屬于豐富且相對穩定和規整的地物類型，具有較為顯著的對象特征，比較適合作為配準的基元。桿狀物的準確提取對后續配準也是重要的，為避免在提取桿狀物時誤將建筑物立面當作桿狀對象提取，造成桿狀物的誤分，本文先對布料模型濾波［20］后的非地面點云使用K 均值［21］無監督分類，分類后對場景點云行道樹，路燈，標識牌和電線桿打上標簽，提取行道樹的樹干、路燈、標識牌和電線桿為桿狀目標。為確保桿狀物提取效果，首先對分類后的點云使用包盒法［22］建立空間格網，利用高程閾值將高于桿狀物的多余點云去除；再用隨機抽樣一致性算法（Random Sample Consensus，RANSAC）［23］對 格網俯瞰圖進行遍歷，進行先驗選取符合圓柱擬合的格網，并以樹干為中心設置圓柱半徑，對半徑鄰域搜索圓弧狀點集；最后通過判別格網密度、最小支持點數量和垂直于立面的角度閾值，對格網內符合要求的視為桿狀點云。城市路燈的燈頭部會有水平延伸區域，路燈頭部和標示牌的牌部點云數量較少，設置不同的密度和最小支持點云數量閾值，即可精確提取桿狀點云集。

根據點云中坐標極值構建邊長為L的m×n格網，將點云數據劃分入規則的二維格網中，m和n計算方式如式（1）所示：

其中：Xmax，Xmin，Ymax和Ymin為點云數值極值，L為格網寬度，根據點云密度設置，本文設為1 m。

采用RANSAC 算法搜索擬合圓提取圓柱點云，設置半徑緩沖區范圍為0.05～0.5 m 來提取圓柱點云。

圖2 樹桿和路燈投影提取示意圖Fig.2 Schematic diagram of tree pole and street lamp projection extraction

其中：ωR和ωM分別是路面和道路標記出現的概率；μR和μM是相應的平均水平。

網格單元灰度值計算如式（4）所示：

最后，選擇最佳強度閾值：

點的空間密度定義如式（9）所示：

其中：N(p)表示點p的局部鄰域，dN是鄰域的大小，并且pi(xi，yi，zi)是鄰域內的點。通過這樣的定義，位于一點附近的點越多，該點的空間密度越高。因此，噪聲的空間密度低于道路標記點的空間密度。在計算點的空間密度后，將空間密度低于閾值ρSD的點視為噪聲并進一步濾除。圖3 展示了強度值歸一化統計圖，根據車道線的強度值較小，綠色表示車道線統計；圖4 展示出了在空間密度分割之后利用Otsu 的灰度格網圖（彩圖見期刊電子版）。

圖3 密度分割反射強度統計圖Fig.3 Density split reflection intensity statistical chart

圖4 Otsu 提取的車道線格網灰度圖Fig.4 Grayscale map of the lane line grid extracted by Otsu

2.3 L-M 改進的面到面ICP 算法

傳統的ICP 算法由于采用歐氏距離最近點作為對應點，容易受到噪聲干擾，會引入許多錯誤點對，直接用最小二乘優化計算得到的解會受一定的影響；利用奇異值分解法（SVD）［8］分解的特征值對噪聲比較敏感，影響配準的精準性導致誤差大。因此，本文利用概率模型構建平面到平面的距離［25］來搜索相應的點，改進的ICP 基于概率模型附加的簡化步驟。保持算法的其余部分不變，降低計算的復雜度和加快配準速度，仍使用歐式距離而不是概率度量來計算對應關系。這樣做是為了允許在查找最近點時使用KD 樹，并因此保持ICP 相對于其他完全概率技術的主要優點，速度快和利于應用。概率函數的最小化可用最速下降法、牛頓法等方法進行非線性優化，但這些方法計算量大，收斂慢。為此，本文使用Levenberg-Marquardt（L-M）算法［26］優化的ICP 平面到平面算法進行全局精配準。不僅降低了對初始位置的要求，而且加快了配準的收斂速度。

設一個非線性方程f(x)，最基本的非線性最小二乘法，將目標函數f(x)在x附近使用泰勒公式一階展開，得到式（10）：

其中：J(x)是f(x)關于x的導數，Δx為下降量，為使f(x)+J(x)Δx2最小，需求適合的Δx。

為目標函數Δx*展開求導并令其為0 得到式（12）：

那么可以看出是一個高斯-牛頓法的正規方程，L-M算法則在其加入一個正定對角矩陣，變為：

其中：I為單位矩陣，λ為一個正實數，用來控制步長，在每次迭代的時候，除了要更新誤差函數f(x)在第k次迭代值x和在該處的一階偏導數值以外，還要更新λ的值調節收斂步長。

其中：Τopt 為更新的剛性變換矩陣，Τ=R·t，是L-M 優化E(Τ)每次迭代求得的剛性矩陣。

為了改善相對于點到平面的性能并增加模型的對稱性，使用點到面來激發概率模型。本質上，每個測量點僅提供沿著其表面法線的約束。為了構建這種模型，認為每個采樣點沿其局部平面有高的協方差分布，并且在表面法線方向具有非常低的協方差。如果點的表面法線為ni，則協方差矩陣變為：

其中：α表示沿著法線的協方差的一個小的常數。這對應非常高的置信度并知道沿著法線的位置，但是不確定其在平面中的位置。

其中：si，ti為ai和bi相對應點的法向量，Rx為變換基向量的旋轉矩陣。

再將式（18）和式（19）代入到式（17）計算出變換剛性矩陣Τopt。即L-M 優化的平面到平面ICP 算法構建完成。圖5 提供了在極端情況下算法的效果圖示（彩圖見期刊電子版。在這種情況下，沿著綠色掃描的垂直部分的所有點都不正確地與紅色掃描中的單個點相關聯。由于曲面方向不一致，平面到平面將自動忽略這些匹配；每個對應最終求和的協方差矩陣將是各向同性的，并且相對于精確定義的對應協方差矩陣，將形成對目標函數的非常小的貢獻。這種行為的另一種觀點是作為每個對應的軟約束。不一致的匹配允許紅色掃描點沿著水平軸移動，而綠色掃描點可沿著豎直方向自由移動。因此，不正確的對應關系形成了對整體比對的非常弱且無信息的約束。

圖5 平面到平面匹配示意圖Fig.5 Schematic diagram of plane-to-plane matching

圖6 研究區域示意圖Fig.6 Schematic diagram of the study area

圖7 L001 區域點云彩色圖Fig.7 L001 area point cloud color map

為減小點云配對過程噪聲和離群值的影響，利用Cauchy 損失函數［27］作為加權函數來抵抗噪聲和離群點。改進的ICP 算法如下：

其中：ρCauchy為Cauchy 損失函數權重，k為損失參數，通常情況下k取1.345，r為優化時的殘差。

將L-M 優化得到的Τ=R·t中的R，代入到式（19）和式（20）可以求得和，再代入求得更新變換矩陣Τopt，并求出，以此迭代使得小于設置歐式距離閾值τ或者使變換更新矩陣小于設定的閾值?，達到其中一個條件即停止迭代。

3 實驗與分析

3.1 數據準備

3.2 數據分析與預處理

依據第二節原理，本文對地面濾波過后的點云進行噪點濾除，得到去噪后的地面點和非地面點如下圖，再對其分別提取出車道線和桿狀物作為配準基元，更能有效快速完成配準。如圖8 所示為從地面點中提取車道線流程，圖8（a）中可以看出車道線與地面點的強度值不同，能看出車道線特征，但還有少量混合，為了準確提取，先進性密度分割，如圖8（b）所示，看起來更加準確；圖8（c）展示了根據強度值將分割的車道線轉為灰度圖，可以明顯看出車道線，再利用Otsu 算法分割出車道線，如圖8（d）所示。圖9 為非地面點的桿狀物提取，先將非地面點進行分類，并打上標簽，如圖9（b）所示；后只保留有桿狀物的地類，以免建筑物立面和其他地物的干擾，如圖9（c），并加快速度準確提取桿狀物，圖9（d）為提取的桿狀物。圖10 展示了提取的桿狀物和車道線俯視圖，紅色為桿狀物，白色為車道線；圖11 為利用SAC-IA 進行初始估計，從圖中可看出許多的同名點配對，也有誤配對，但也得到了初始變換矩陣，為后續精配準做準備（彩圖見期刊電子版）。

圖8 車道線提取效果圖Fig.8 Lane line extraction rendering

圖9 桿狀物提取效果圖Fig.9 Pole-like extraction rendering

圖10 配準基元的提取結果圖Fig.10 Extraction result of registration primitives

圖11 SAC-IA 算法初始估計配對Fig.11 Initial estimation pairing of SAC-IA algorithm

3.3 各算法配準對比實驗與分析

實驗將本文算法與點到點ICP 算法［8］、點到面PP-ICP 算法［29］、FGR 算法［30］、ISS-ICP 算法［31］和以桿狀物為配準基元的ICP 算法進行試驗比較。實驗選擇迭代次數閾值均為200 次，變換矩陣誤差閾值為10-5m，均方根誤差（RMSE）閾值為10-5m，滿足其中一個閾值則停止迭代。本次實驗的評價指標選用迭代所耗時間和配準誤差［32］，采用點對的歐式距離RMSE 作為配準誤差精度進行評估，單位為米。選用配準算法所耗時間作為配準效率進行評估，單位為s。由式（21）可得RMSE：

其中：si=(six，siy，siz)為源點云點的坐標，ti=(tix，tiy，tiz)為待配準點云的坐標，n為配準的點對數量。

各算法的配準結果如圖12 所示，表1 時間為整體流程算法總時間，ICP、PP-ICP 和FGR 三種算法利用的全局降采樣的點數量；ISS+ICP，Pole-ICP 和本文算法則是使用各自提取特征點數量，相對于ICP，PP-ICP 和FGR 三種算法數量較少，點對距離大，初始RMSE 就偏大。如圖12（a）所示，將L001 區域數據利用平移和旋轉分別得到不同位置的兩片點云，重疊度為10%，再進行點云配準實驗。結合表1 和圖12 可以分析得出，點云數量較大時，傳統ICP 算法配準效率低，融合效果不佳，使用鄰近點對的搜索方式導致算法運算時間長，消耗了186.231 s，收斂速度慢，達到了迭代閾值次數，且對噪聲較為敏感，魯棒性較差，可從圖12（b）可以看出明顯位置偏差；點到面ICP 算法配準結果誤差減少許多，雖然比傳統ICP 算法收斂快，配準效率提高了24 s，用了162.224 s，但也達到了迭代閾值次數，從圖12（c）仍可看出效果不太好，在局部放大圖中依舊有明顯位置偏差。FGR 算法的配準效果明顯有了改善，且配準精度較高，能達到9 cm，從圖12（d）可以看出基本融合，但輸電線依舊可看出有偏差；由于在計算較大模型時，花費了大量時間計算快速點特征直方圖（FPFH）［33］和特征描述子，配準時間較長，用了859.095 s；ISS+ICP 算法比前三種算法配準精度與效率明顯提高，配準誤差為0.146 85 s，但在ISS 算法提取優良點時較慢，且主要為邊角點，從圖12（e）主圖可看出融合效果還有待提高；受到噪點的影響，后續配準收斂也慢，迭代了90 次變換矩陣才達到誤差閾值，耗時51.749 s。本文方法配準結果如圖 12（g）所示。從整體情況看，本文算法實現了高度融合，桿狀物、車道線和其他地物配準結果較好，只花了19.622 s，比ICP，PP-ICP，FGR 和ISS-ICP 算法的配準效率分別快了166 s，142 s，830 s 和32 s，且只用了20 次迭代就達到了RMSE 誤差閾值，配準精度遠高于其他四種方法，達到了1.987 7×10-5m。為證明加入車道線和改進ICP 的配準提高效果，本文還與桿狀物作為配準基元的點到點ICP 方法作比較。雖然多了計算車道線提取的時間，但可以明顯看出本文算法迭代次數較少，且收斂較快，配準效率也略有提高，且配準精度從4.861 2×10-5m 提高到了1.987 7×10-5m，從圖12（f）主圖可看出比本文算法主圖融合效果略差，證明了本文算法具有較強的魯棒性。

表1 各算法的配準精度和運行時間Tab.1 Registration accuracy and run time for each algorithm

圖12 不同算法的配準結果和局部放大圖Fig.12 Registration results of different algorithms

圖13 為六種算法的迭代收斂性對比圖，初始RMSE 不一致是由于點云數量的不同。從圖13中可看出本文算法收斂很快，呈現陡崖是下降收斂，Pole-ICP 算法雖然開始收斂很快，但后續陷入了局部最優，出現緩慢收斂，本文采樣概率非線性優化配準，并不會陷入局部最優解，其他四種算法從圖可看出收斂緩慢。

圖13 六種算法收斂速度對比Fig.13 Comparison of convergence speeds of six algorithms

4 結論

針對車載MLS 獲取城市場景出現不同時期位置偏差，且數據具有密度不均、目標多樣和離散點多等特點，點云配準方法仍存在計算復雜度較高、配準效率低和魯棒性弱等局限性。本文根據城市道路場景中桿狀物和車道線長期優異的穩定性和重復性，提取桿狀物和車道線作為配準基元。利用改進的面到面ICP 算法，并使用L-M 非線性迭代法優化和加入Cauchy 損失權重函數抗噪，利用SAC-IA 算法剔除錯誤點對，使用雙向KD 樹保證特征點對應的關系，加快配準速度和提高精度，增強穩定性。最后使用城市場景的點云數據，與五種點云配準方法進行對比實驗。結果表明，在初始重疊度較低的城市環境情況下，本文配準方法相比于傳統配準方法效率、配準精度和魯棒性顯著提高，在大面積點云用了不到20 秒，只需迭代20 次，精度可達1.9877×10-5米，實現了輕量化的快速準確點云配準，對三維重建具有重要意義。