流形和非流形方法對配準算法的影響分析

裴 東，高文輝，任 琪，但唐旭

(西北師范大學 物理與電子工程學院/甘肅省智能信息技術與應用工程研究中心，甘肅 蘭州 730070)

在計算機視覺、移動機器人等領域中，由于硬件條件以及環境因素受限，常使得激光雷達等傳感器獲得的點云信息出現殘缺、錯位，以及在三維物體模型構建中需要對模型進行拼接等操作，這些都需要用到點云配準，而Besl[1]提出的ICP算法是解決點云配準問題的常用方法[2-3].ICP算法是2組點云按照一定的約束條件，通過KD-Tree搜索算法[4-5]找到一組點云在另外一組點云中的最近鄰點，然后將點到點的誤差度量作為目標函數，也可以將點到線[6]或點到面[7]的誤差度量作為目標函數，通過ICP算法計算出2組點云在空間中的變換位姿，將不同坐標系的點云數據合并到同一個坐標系中.ICP算法對初值比較敏感，每次配準都要對點云數據進行全局搜索查找對應點，而且點云稀疏區域的配準容易受到密集區域的影響，這將會造成計算量過大以及ICP噪聲和離群值等問題.針對傳統的ICP算法的缺點，研究者在ICP算法的基礎上進行了改進，提出了PICP(Probability ICP)[8]、MICP(Modified ICP)[9]和 CICP(Cluster ICP)[10]以及引入熵[11]等方法.

ICP算法求解方式主要有2種，一種是利用線性代數的方式求解；另一種是利用非線性優化的方式求解，即最小二乘法.最小二乘法的解法通常為迭代法，包括最速下降法、Newton法、Gauss-Newton(GN)法和Levenberg-Marquardt(LM)法.最速下降法過于貪心，容易走出鋸齒路線從而增加迭代次數，牛頓法需要計算目標函數的海森矩陣H，在問題規模較大時海森矩陣的計算將會耗費更多資源.使用GN法和LM法時需要求解雅可比矩陣J，但歐拉角求解雅可比矩陣的常用方法是將歐拉角分步驟轉換為繞x軸、y軸和z軸旋轉的3個旋轉矩陣，然后分別對3個旋轉矩陣做相應的變換從而達到間接求解雅可比矩陣的目的.

1 問題描述

?pi∈P,?qi∈Q|=qi-Tpi==0,

(1)

其中，T為第i對配準點pi和qi之間的變換矩陣.變換T由平移t和旋轉R兩種操作構成.變換過程如下

(2)

為求出旋轉矩陣R和平移向量t，需要構建配準點Tpi即Rpi+t和qi之間的偏差ep，下標p表示兩個點云數據集點與點的配準方式

ep=qi-(Rpi+t).

(3)

為方便計算，取ep的二范數的平方項作為點到點的損失函數，求出損失函數取得最小時R和t的值.

上述變換矩陣T的求解過程是利用點到點之間的距離作為誤差度量.由于2組點云表示的點不可能是相同位置的點，所以點到點的距離作為誤差度量會引入隨機誤差.為了減小這種隨機誤差帶來的影響，采用點到面的距離.當使用點到面的距離時，誤差度量是每個源點與其對應目標點所在切平面之間的距離.取源數據集P中的任意一點pi和目標數據集Q中與之對應的點qi，點到面之間的距離誤差可以用en來表示，下標n表示2個點云數據集點到面的配準方式

(4)

其中li為qi所在區域的單位法向量.取en的平方項作為點到面的損失函數，求出使損失函數取得最小值時R和t的值.(3)式和(4)式分別給出了ICP中涉及到的點到點配準以及點到面配準所用到的損失函數.對于兩類損失函數，都需要求解出使它們各自取得最小值時的R和t.

2 問題求解

下面將從流形和非流形不同的解決方法中分別介紹點到點和點到面對應的損失函數中t和R的求解方法.

為求使損失函數取得最小時t和R的值，首先將誤差e進行一階泰勒展開

e(T+ΔT)≈e(T)+J(T)TΔT,

(5)

求出損失函數關于ΔT的導數，并令其為零

J(T)Te(T)+J(T)TJ(T)ΔT=0,

(6)

令

(7)

則迭代步長

(8)

得到GN法的實現流程如算法1所示.

算法1Gauss-newton method

begin

k=0

H=JTJ;G=JTe

while(notfound)and(k

begin

k=k+1;SolveHΔT=-G

T=T+ΔT

end

end

LM法如算法2所示.

算法2Levenberg-Marquardt method

begin

λ=0.01;v0=2;k=0

H=JTJ;G=JTe

while(notfound)and(k

begin

k=k+1;Solve (H+λI)ΔT=-G

Tnew=T+ΔT

ρ=ΔC/ΔS

ifρ>0

T=Tnew

else

λ=λ*v;v=*v

end

end

第1節介紹了點到點以及點到面的損失函數.位姿變換T可以用平移t(tx,ty,tz)和旋轉R(α,β,γ)來表示.平移向量中的參數tx,ty和tz分別表示3維坐標系中x,y,z3個坐標軸方向上的偏移量.旋轉矩陣的參數α,β和γ分別表示俯仰角、偏航角以及翻滾角.

2.1 非流形方法

非流形的求解方式是求解雅可比矩陣的常用方式.需要將α,β和γ轉換成相應的旋轉矩陣Rx(α),Ry(β)和Rz(γ)，分別對旋轉矩陣Rx(α),Ry(β)和Rz(γ)中的α,β和γ進行求導，得到

然后通過一定的運算規則求解出雅可比矩陣.本小節將介紹非流形算法中點到點以及點到面的距離為誤差度量時雅可比矩陣的推導過程以及ICP 迭代方式中如何采用非流形的方式求解雅克比矩陣.

2.2 流形方法

下面將介紹 ICP 迭代方式中如何采用流形算法.假設物體表面有一點τ，在τ點處的切空間就是其自身的局部坐標系空間.向量空間中的元素映射到李代數空間，該過程用(·)^表示.李代數空間的元素經過指數運算映射到李群空間中.同理，李群空間中的元素經過對數運算映射到李代數空間.

2.2.1 點到點 2.1節中非流形的 ICP 是在切空間對歐拉角進行的分解變換求導等操作，流形中對雅可比矩陣J的求解放在李群流形空間中.在李群空間中，假設對三維點p進行了旋轉，得到Rp，如果對Rp中的R進行求導，可以看成對R進行了一次左擾動ΔR.設左擾動ΔR對應的李代數為θ,則結果相對于擾動的變換率為

所以流形中的雅可比矩陣

J=[-I3-(Rp)^].

(16)

流形中點到點的GN法中的誤差ep和非流形解法相似.在求取J和e后，對每次迭代求H和G，同樣地，H和G的求解方式參考(7)式.而后求取迭代步長ΔT.此時迭代步長ΔT為切空間中的步長，需要將其轉換到流形空間

將得到的變換步長ΔTi與變換矩陣Ti相乘得到下一次的變換矩陣最終得到變換矩陣

2.2.2 點到面 對比流形中點到點與非流形點到面的 GN法，流形中點到面的高斯牛頓法求解變換矩陣T所需要的雅可比矩陣J的求解形式為

J=[-lTI3-lT(Rp)^].

(19)

3 實驗分析

下面將在斯坦福數據集中通過平移和旋轉，分別對流形和非流形中點到點及點到面的 LM以及 GN法進行實驗.通過流形和非流形的方式先對KIT數據集中的相鄰兩幀數據進行實驗，而后對前1 550幀數據進行位姿計算，通過位姿還原傳感器的運動軌跡.所有的程序在配置為Intel(R)Core(TM)i5-7500 CPU @ 3.40GHz 和 16 GB RAM 的計算機上使用Matlab 2020a 編寫.

3.1 斯坦福數據中的對比

使用斯坦福三維掃描庫比較 ICP 非流形和流形算法性能差異.先對原始數據使用方形網格過濾器進行降采樣以提高運行速度，其中方形網格長度為 0.002.然后對源點云集做一次位姿變換產生目標點云集.迭代的初始位姿為4×4的單位陣.迭代次數最大為 50，根據數據集尺度的大小確定閾值，去除K-D樹中歐氏距離大于該閾值的配準點.當迭代步長范數小于1×10-6時，停止迭代.為方便標記，使用英文大寫字母表示下面用到的一些專業名稱，流形(M)、非流形(N)、點到面(S)、點到點(P)、GN法(G)、LM法(L).例如 ICP 非流形點到點GN法可以簡寫為 NPG，依此類比.

將掃描庫中的一幀點云數據集作為目標點云，如圖1(a)所示，然后將其繞x軸順時針旋轉30°，將變換后的點云為源點云圖1(b).經過ICP 求解出位姿，經過位姿變換的源點云與目標點云的配準結果如圖1(c)所示.

圖1 ICP 配準結果

圖2給出了ICP算法的迭代偏差.從圖2a和2b可以看出使用非流形的迭代偏差與使用流形的迭代偏差相似.使用點到點的求解方式比使用點到面的求解方式在該實例中收斂更快.

對流形和非流形中點到點與點到面的GN和LM法分別在同樣點云數據集位姿變換中進行了實驗，得到8種算法的計算時間和迭代次數,如圖3所示.結果表明,非流形和流形中點到點迭代次數為9次， 少于點到面的迭代次數12次. 而同等條件下，流形完成迭代所用的時長比較流形完成迭代所用的時長短.點到點相比點到面消耗的時間更短.算法的計算誤差包括旋轉誤差和平移誤差.旋轉誤差弧度Δθ為

圖2 ICP迭代偏差

圖3 使用斯坦福數據集ICP算法不同方法的迭代次數與執行時長

Δt==tTrue-tICP=2,

(21)

即平移誤差Δt為平移向量偏差的二范數.

從圖4中可以看出，使用點到面的計算誤差接近于零.流形和非流形的計算誤差結果相似.實驗結果表明使用點到點作為配準點云計算偏差小于使用點到面作為計算配準點云偏差；使用流形的方式進行迭代要比使用非流形的方式效果好.

圖4 使用斯坦福數據集ICP算法不同方法的計算誤差

3.2 KITTI數據中的對比

使用 KITTI數據集來對比算法使用流形與流形的性能.因為 KITTI 的數據不成均勻分布的狀態，所以與對 3.1 節中斯坦數據集的處理過程不同，使用隨機采樣的方式對原始數據集進行降采樣.本實驗的采樣率為3%.采樣的原始數據集和目標數據集分別為間隔兩幀的激光數據.同樣地，設定最大迭代次數為50次，每次迭代去除大于K-D tree 配準點歐式距離中大于設定閾值(2 m)的數據點.當迭代步長小于0.001 的時候停止迭代.由于點到點的ICP 配準魯棒性較差，本小節僅使用點到面的方式對流形和非流形的計算方法進行比較.4種方式分別為NSG,NSL,MSG和MSL.

取KITTI 數據集中的一幀數據以及與其間隔2幀的數據分別作為源點云和目標點云.2幀數據的圖像如圖5所示.4種算法完成配準后的圖像相似,所以使用經過MSL 配準后的圖像來展現配準的結果.2種方式的迭代次數相同，但SG和SL的迭代時間流形相比非流形分別提高了53%和49%,如圖6所示.

從圖7中可以看到4種算法的平移誤差近似相等，旋轉誤差GN略高于LM.實驗數據表明流形和非流形的迭代次數及計算誤差近似相等，但算法消耗的時長明顯減少.

圖5 KITTI兩幀點云數據ICP配準結果

圖6 使用KITTI數據集ICP算法不同方法的迭代次數與執行時長

圖7 使用KITTI數據集ICP算法不同方法的計算誤差

3.3 KIT數據還原軌跡對比

上述實驗過程僅進行了2幀點云之間的配準，下面使用 KITTI數據集中的 1 550 幀3D 激光點云數據進行配準測試.經過測試發現 NSG 和 MSG 兩種的魯棒性能夠保證每一幀點云的完整匹配，所以流形和非流形對比將使用點到面的 GN 算法.為了保證配準精度，將原始的點云數據進行降采樣時采用隨機抽取10%點云的方式，其余的參數配置與 3.2 節中一致.

圖8為使用ICP迭代計算得到的位姿繪制的軌跡和誤差.可以看出，流形和非流形兩種方式得到的軌跡之間的偏差較小，原因是兩種迭代方式在進行隨機降采樣后的點云數據并不一致，這將會導致計算得到的位姿不一樣.由圖8(b)可以看出流形與非流形的計算得到的位姿因為誤差不斷累積，導致最終計算出的軌跡和真實軌跡均有較大差距.非流形和流形迭代所消耗的時長分別為12 614 s,5 421 s.非流形平均每幀時長為8.1381 s，而流形平均每幀時長為3.4974 s.經過計算得出ICP 迭代中雅克比的計算使用流形比使用非流形的方式效率提高了約57%.

圖8 ICP算法解算后的軌跡對比

4 結論

文中針對傳統的最小二乘法在數據量較大的情況下配準速度慢的問題，提出了一種基于流形求解雅可比矩陣的最小二乘法.該方法通過改變目標函數關于變量的求導思路，將成同構關系向量空間中的元素通過李代數空間映射到李群流形空間中，通過對流行空間進行擾動的方式解出所求矩陣的導數，進而實現對迭代的優化.3個實驗驗證優化算法的有效性，前2個實驗分別在斯坦福和KITTI數據集中進行算法性能的對比，第3個實驗對比多幀數據配準時二者的執行效率和計算精度.實驗結果表明，在點云數據相同的情況下流形的ICP算法較非流形的ICP算法迭代偏差與迭代次數基本一致，但迭代效率有顯著提升.文中提出的ICP流形迭代對初始位姿有較高的要求，如果2幀點云數據位姿偏差較大很容易陷入局部最優解.后續將針對改變點云的分布對ICP的影響進行研究.