基于BFGS修正的高斯牛頓光束法平差解算方法

趙帥華 李言言 曹健,? 曹喜信

基于BFGS修正的高斯牛頓光束法平差解算方法

趙帥華1李言言2曹健1,?曹喜信1

1. 北京大學(xué)軟件與微電子學(xué)院, 北京 102600; 2. 德國(guó)慕尼黑工業(yè)大學(xué), 慕尼黑 80333; ? 通信作者, E-mail: caojian@ss.pku.edu.cn

針對(duì)高斯牛頓(Gauss-Newton, GN)方法求解光束法平差模型時(shí)對(duì)初值準(zhǔn)確度要求高、應(yīng)用場(chǎng)景受限的問(wèn)題, 提出基于擬牛頓法 BFGS (Broyden-Fletcher-Goldfarb-Shanno)修正的高斯牛頓算法——BFGS-GN 法。當(dāng)高斯牛頓法的信息矩陣失去正定性后, 使用 BFGS 算法對(duì)法方程進(jìn)行補(bǔ)充修正, 可從根本上消除高斯牛頓方法對(duì)初值敏感的數(shù)學(xué)缺陷。在數(shù)據(jù)集上的實(shí)驗(yàn)結(jié)果表明, BFGS-GN 算法對(duì)不同類(lèi)型的初值具有魯棒性, 在初值較好的情況下, 所提方法與高斯牛頓法具有相同的精度和迭代效率; 在初值較差的情況下, 高斯牛頓方法因發(fā)散而失效, BFGS-GN 算法仍可以收斂到較高的精度。

光束法平差; 高斯牛頓; BFGS 算法; 初值魯棒

光束法平差(bundle adjustment, BA)是三維重建和視覺(jué) SLAM (simultaneous localization and mapp-ing)的核心技術(shù), 主要應(yīng)用于相機(jī)位姿和路標(biāo)點(diǎn)的后端優(yōu)化。獲得相機(jī)內(nèi)參矩陣、外參矩陣、影像特征點(diǎn)和空間 3D 物點(diǎn)坐標(biāo)初值后, 可以使用 BA 模型對(duì)以上參數(shù)進(jìn)行整體優(yōu)化, 消除誤差, 并獲得精確的視覺(jué)信息[1]。

在視覺(jué)重建中, BA 模型的目標(biāo)是獲得更加精確的相機(jī)位姿和空間路標(biāo)點(diǎn)。圍繞這一目標(biāo), 針對(duì)光束法平差的研究分為以下幾個(gè)方向。

1)使用其他質(zhì)量更高的傳感器獲得更高質(zhì)量的初值。Zhang 等[2]著眼于使用標(biāo)靶來(lái)獲得精度更高的初值, 以保證優(yōu)化過(guò)程中的問(wèn)題收斂, 但在提高初值精度時(shí), 往往需要精度更高的采集設(shè)備和更多的人工預(yù)處理。

2)建立包含更多約束內(nèi)容的光束法平差模型。Dabeer 等[3]在 BA 模型中融合路標(biāo)點(diǎn)特征和車(chē)道線特征來(lái)優(yōu)化自動(dòng)駕駛車(chē)輛的姿態(tài)信息。Yang 等[4]提出點(diǎn)面 BA 模型來(lái)處理紋理缺乏場(chǎng)景中機(jī)器人的狀態(tài)優(yōu)化問(wèn)題。Wu 等[5]利用計(jì)算機(jī)并行計(jì)算方式, 通過(guò)使用多核 CPU 和 GPU 進(jìn)行算法加速。Zhao 等[6]用影像間的視差角等角度信息代替直角坐標(biāo), 降低雅克比矩陣(Jacobian matrix)的稀疏性。

3)借助深度學(xué)習(xí)方法求解 BA 問(wèn)題。這種方法往往需要基于數(shù)據(jù)特征進(jìn)行精巧的設(shè)計(jì)來(lái)增強(qiáng)訓(xùn)練過(guò)程的收斂性, 且參數(shù)估計(jì)部分仍然采用傳統(tǒng)方法, 通用性不強(qiáng)。如 Tang 等[7]設(shè)計(jì) BA-Net, 通過(guò)學(xué)習(xí)到的量度特征來(lái)衡量重投影誤差、優(yōu)化場(chǎng)景深度和相機(jī)運(yùn)動(dòng)。

4)直接研究 BA 模型的數(shù)值優(yōu)化解。這是更加基礎(chǔ)性的研究方向, 其目標(biāo)是以各種質(zhì)量的傳感器采集的數(shù)據(jù)為基礎(chǔ), 提高求解方法的準(zhǔn)確度和可拓展性。本文詳細(xì)介紹此方向的研究。

光束法平差模型的本質(zhì)是非線性最小二乘求解問(wèn)題, 通過(guò)多次迭代, 使 3D 路標(biāo)點(diǎn)的重投影誤差最小。在數(shù)值優(yōu)化問(wèn)題的諸多解法中, 牛頓法因具備二階收斂速度而得到廣泛的應(yīng)用, 但在迭代求解的過(guò)程中, 牛頓法因計(jì)算復(fù)雜度過(guò)高而無(wú)法應(yīng)用在數(shù)據(jù)量較大的 BA 模型求解中。這種情況下, 高斯牛頓方法通過(guò)忽略牛頓方法中產(chǎn)生的高階導(dǎo)數(shù), 極大地降低計(jì)算復(fù)雜度, 因此普遍應(yīng)用于工程問(wèn)題中。但是, 在忽略高階項(xiàng)的同時(shí)也為高斯牛頓帶來(lái)問(wèn)題: 當(dāng)輸入模型的初始值偏離真值較遠(yuǎn)時(shí), 此方法極易發(fā)散, 無(wú)法滿足收斂要求。

LM (Levenberg-Marquardt)方法[8]通過(guò)為高斯牛頓增加阻尼項(xiàng)來(lái)實(shí)現(xiàn)問(wèn)題可解, 但由于阻尼的具體數(shù)值是通過(guò)不斷嘗試得到的, 因此很難獲得精準(zhǔn)的下降方向, 并導(dǎo)致迭代中的下降速度較慢。CG (Conjugate Gradient)方法[9]則是在高斯牛頓方法的數(shù)學(xué)模型基礎(chǔ)上, 使用共軛梯度的方式求解方程, 但由于信息矩陣的條件數(shù)較大, 會(huì)影響矩陣的收斂速度。CGBA 方法[10]通過(guò)分解雅克比矩陣, 能夠降低矩陣的條件數(shù), 提高收斂速度, 但基于共軛梯度方式的解算方法通常無(wú)法收斂到更高精度的局部最小點(diǎn)。

BFGS (Broyden-Fletcher-Goldfarb-Shanno)方法[11]是一種常見(jiàn)的擬牛頓方法, 具有良好的正定保持特性, 因此是數(shù)值優(yōu)化領(lǐng)域常用的優(yōu)化方法。sBFGS-BA 是利用 BFGS 的正定保持特性提出的一種新解算方法[12], 但每步迭代都需計(jì)算海森矩陣(Hessian matrix)的高階信息, 無(wú)法實(shí)現(xiàn)實(shí)時(shí)運(yùn)行。本文通過(guò)分析高斯牛頓方法對(duì)初值敏感的原因發(fā)現(xiàn), 在解算過(guò)程中, 是由于近似海森矩陣失去正定特點(diǎn), 才導(dǎo)致下降方向無(wú)法求解。本文引入 BFGS 方法迭代求解目標(biāo)方程的高階導(dǎo)信息, 更加精準(zhǔn)的表現(xiàn)求解目標(biāo)中的海森矩陣, 在增加少量可控計(jì)算量的基礎(chǔ)上, 可以極大地提高方法的魯棒性。在較差初值的情況下, 本文方法也可以獲得較好的下降方向, 從根本上解決基于高斯牛頓方法的光束法平差方法對(duì)初值敏感的問(wèn)題。

1 基于高斯牛頓方法的光束法平差的缺陷分析

1.1 光束法平差模型

可以將 BA 問(wèn)題描述為

其中,是所有未知參數(shù)的集合;是一個(gè)協(xié)方差矩陣, 使用同一個(gè)傳感器進(jìn)行觀測(cè)時(shí), 我們認(rèn)為誤差之間是相互獨(dú)立的, 故通常將作為一個(gè)對(duì)角陣存在, 實(shí)際中通常取單位矩陣。通過(guò)對(duì)目標(biāo)方程的解算, 可以獲得最終優(yōu)化的結(jié)果。

1.2 高斯牛頓方法對(duì)初值敏感的原因

牛頓法是求解無(wú)約束優(yōu)化問(wèn)題最早的算法之一, 主要思路是使用迭代點(diǎn)處的一階導(dǎo)數(shù)和二階導(dǎo)數(shù)對(duì)目標(biāo)方程進(jìn)行近似, 然后將二次模型的極小點(diǎn)作為新的迭代點(diǎn), 不斷重復(fù), 達(dá)到滿足的精度為止。對(duì)式(3)進(jìn)行二階泰勒展開(kāi):

可以看出, 在迭代求解過(guò)程中, 缺少高階導(dǎo)數(shù)信息的高斯牛頓算法必須滿足 Hessian 矩陣正定的條件, 并且要求式(7)中雅克比矩陣必須是列滿秩矩陣, 否則算法失效。Jacobian 矩陣是相機(jī)參數(shù)與觀測(cè)的三維特征點(diǎn)之間的一種表示方式, 隨輸入?yún)?shù)的變化而變化[13]。因此, 對(duì) Jacobian 矩陣的要求就轉(zhuǎn)化成對(duì)觀測(cè)初值的要求, 當(dāng)初值不好時(shí), 優(yōu)化問(wèn)題通常無(wú)法求解, 這就是高斯牛頓方法容易發(fā)散的根本原因。

2 基于 BFGS 修正的高斯牛頓解算方法

針對(duì)高斯牛頓法存在的問(wèn)題, 我們結(jié)合 BFGS算法進(jìn)行修正, 繼而提出更加魯棒的 BA 模型解算方法。

2.1 BFGS算法的正定保持特點(diǎn)

BFGS 是一種常用的擬牛頓方法, 如式(8)所示:

由于是正定矩陣, 利用 Cuchy-Schwarz 公式進(jìn)行分解可得

因此, 在式(12)等號(hào)成立時(shí), 有

在式(12)等號(hào)不成立并嚴(yán)格小于時(shí), 有

2.2 BFGS-GN算法

對(duì)于第一次迭代的修正方法是: 在開(kāi)始求解矩陣時(shí), 增加阻尼項(xiàng)0:

式(17)和(18)中,+1是新得到的海森矩陣, ||.||表示L2 范數(shù)距離。

修正后的通用形式的矩陣可以定義如下:

式(19)得到的+1是一個(gè)精確的矩陣, 可以更好地模擬Hessian矩陣,+1是一個(gè)稠密矩陣, 通過(guò)式(8)獲得?；贐FGS修正的GN算法如下。

算法 BFGS-GN

已知: 相機(jī)位姿、路標(biāo)點(diǎn)和特征點(diǎn)

我們將非零元素分為兩類(lèi): 對(duì)角元素和非對(duì)角元素。若將它們都保留在增益矩陣+1中, 可以獲得更好的實(shí)驗(yàn)結(jié)果。因此, 通過(guò)濾波器的Hadamard乘法, 可以得到+1的稀疏矩陣, 記為:

3 實(shí)驗(yàn)與討論

3.1 實(shí)驗(yàn)數(shù)據(jù)

為了驗(yàn)證本文提出的 BFGS-GN 方法的有效性, 使用 4 個(gè)數(shù)據(jù)集進(jìn)行實(shí)驗(yàn), 將實(shí)驗(yàn)結(jié)果與傳統(tǒng)的高斯牛頓解算方法進(jìn)行比較①代碼地址: https://github.com/shzhao/BFGS-GN。。為了確保公平性, 兩個(gè)算法在同樣的計(jì)算平臺(tái)及軟件環(huán)境下編寫(xiě)和運(yùn)行: MATLAB-2016b 和華碩 3.4 GHz Windows 系統(tǒng), 對(duì)參數(shù)進(jìn)行同樣的初始化操作。采用同樣的標(biāo)準(zhǔn)進(jìn)行限制: 最大迭代次數(shù) 100 次。

測(cè)試 4 個(gè)數(shù)據(jù)集由高質(zhì)量攝像機(jī)拍攝的近景及航空攝影測(cè)量圖像組成。表 1 列出數(shù)據(jù)集的整體參數(shù), 包含數(shù)據(jù)類(lèi)型、圖像大小、影像張數(shù)、投影點(diǎn)數(shù)量和 3D 點(diǎn)的個(gè)數(shù)等信息。第 1 個(gè)數(shù)據(jù)集是使用Cannon 相機(jī)在 350m 的高度拍攝的 63 幅野外圖像, 產(chǎn)生 4961 個(gè) 3D 點(diǎn)和 23150 個(gè)投影點(diǎn)。第 2 個(gè)數(shù)據(jù)集是將專(zhuān)業(yè)的光度測(cè)量攝像機(jī) DMC 安裝在 1000m米高的機(jī)載結(jié)構(gòu)上, 拍攝的 90 幅航拍圖像和 37705個(gè)投影。第 3 個(gè)數(shù)據(jù)集是近景數(shù)據(jù)集, 圖像來(lái)自公共機(jī)器人數(shù)據(jù)集合, 其中的 51652 個(gè)投影使用開(kāi)源軟件 L2SIFT 提取。第 4 個(gè)數(shù)據(jù)集是從無(wú)人機(jī)(UAV)平臺(tái)上采集的 468 幅分辨率為 5616×3744 的大學(xué)校園圖像。

提取高質(zhì)量的圖像投影點(diǎn)后, 使用基于相對(duì)姿態(tài)估計(jì)的算法, 計(jì)算兩個(gè)相鄰攝像機(jī)之間的相對(duì)姿態(tài)。在獲得兩個(gè)相鄰攝像機(jī)的所有相對(duì)姿態(tài)后, 通過(guò)將相對(duì)姿態(tài)轉(zhuǎn)換為第一攝像頭坐標(biāo)的方式, 將本地坐標(biāo)參考系中的相機(jī)姿態(tài)初始化。然后, 通過(guò)交點(diǎn)算法實(shí)現(xiàn) 3D 點(diǎn)的初始化。將每個(gè)數(shù)據(jù)集整理為 4 個(gè) txt 文件, 分別是相機(jī)的內(nèi)參文件、每張影像的位姿信息、每張影像的特征點(diǎn)文件和特征點(diǎn)對(duì)應(yīng)的空間路標(biāo)點(diǎn)文件。最后, 將攝像機(jī)姿態(tài)和路標(biāo)點(diǎn)位置輸入基于高斯牛頓法的光束法平差方法和基于 BFGS 修正的高斯牛頓方法中。統(tǒng)一使用優(yōu)化參數(shù)計(jì)算得到的預(yù)測(cè)點(diǎn)和真實(shí)點(diǎn)間的均方誤差(MSE)來(lái)衡量算法的準(zhǔn)確性。

3.2 實(shí)驗(yàn)結(jié)果與討論

實(shí)驗(yàn)結(jié)果分為兩種情況。如圖 2 所示, 對(duì)于數(shù)據(jù)集 DunHuan 和 Village, 高斯牛頓方法(GN)和本文提出的基于 BFGS 修正的高斯牛頓方法(BFGS-GN)具有相同的收斂精度和收斂速度。本文方法在中間迭代點(diǎn)處下降更快速(圖 2(a)), 圖 2(b)中兩種方法取得相同的收斂效果。說(shuō)明在數(shù)據(jù)集初值較好的情況下, 兩種方法均能很好地模擬真實(shí) Hessian 矩陣, 達(dá)到較好的收斂效果。

如圖 3 所示, 對(duì)于數(shù)據(jù)集 Magala 和 College, 使用高斯牛頓方法優(yōu)化相機(jī)姿態(tài)和 3D 點(diǎn)坐標(biāo)時(shí), 迭代過(guò)程經(jīng)過(guò)振蕩后, 最終的結(jié)果出現(xiàn)發(fā)散。BFGS-GN 方法仍然可以將數(shù)據(jù)優(yōu)化到較高的精度。從圖3 可以看出, 高斯牛頓方法出現(xiàn)發(fā)散的原因在于, 數(shù)據(jù)集的初始值距離真實(shí)值較遠(yuǎn), 所以高斯牛頓方法中的近似 Hessian 矩陣不能很好地表示真實(shí)的Hessian 矩陣, 并且在優(yōu)化的過(guò)程中失去正定性。BFGS-GN 方法通過(guò)對(duì)高斯牛頓的目標(biāo)方程進(jìn)行修正, 可以更加準(zhǔn)確地逼近真實(shí) Hessian 矩陣, 并且始終有能力使矩陣在迭代過(guò)程中保持正定特性。對(duì)于Malaga 數(shù)據(jù)集, BFGS-GN 方法可以收斂到 0.512; 對(duì)于 College 數(shù)據(jù)集, 僅需 7 步就能使 MSE 收斂到0.352。

表1 測(cè)試數(shù)據(jù)集的整體參數(shù)

圖2 Dunhuan 和 Village 數(shù)據(jù)集收斂情況

測(cè)試數(shù)據(jù)集的初始誤差、迭代次數(shù)和最終收斂精度如表 2 所示, 其中的第 3 和第 4 列分別表示使用初始未知參數(shù)計(jì)算的均方誤差和使用最優(yōu)參數(shù)計(jì)算的收斂后的均方誤差, 以像素作為統(tǒng)一的單位。此外, 我們還記錄了迭代次數(shù)。

從整體上看, 與基于高斯牛頓的光束法平差方法相比, 對(duì)于初值較好的數(shù)據(jù), BFGS-GN 方法具有不弱于高斯牛頓的迭代效率和收斂精度。對(duì)于擁有較差初值的數(shù)據(jù), BFGS-GN 方法可以順利地收斂, 解決了基于高斯牛頓的光束法平差方法因?qū)Τ踔得舾卸鴳?yīng)用場(chǎng)景受限的問(wèn)題。

表2 高斯牛頓和BFGS-GN優(yōu)化的收斂結(jié)果

4 結(jié)語(yǔ)

高斯牛頓法是光束法平差中的重要方法, 因收斂精度高和超線性收斂速度而得到廣泛應(yīng)用。但是, 高斯牛頓法由于理論上的缺陷, 使得其對(duì)相機(jī)姿態(tài)和空間路標(biāo)點(diǎn)的初始值具有較高要求, 當(dāng)初始值遠(yuǎn)離真值時(shí), 算法極容易發(fā)散, 喪失優(yōu)化功能。本文通過(guò)對(duì)高斯牛頓方法初值敏感原因進(jìn)行探索, 提出基于 BFGS 算法修正的高斯牛頓方法(BFGS-GN 方法)。在每次迭代前, BFGS-GN 方法會(huì)對(duì)高斯牛頓信息矩陣的正定性進(jìn)行判斷, 在不滿足正定規(guī)則時(shí), 對(duì)高斯牛頓的目標(biāo)方程進(jìn)行修正。

實(shí)驗(yàn)結(jié)果表明, 對(duì)于高斯牛頓法不能實(shí)現(xiàn)收斂的數(shù)據(jù)集, BFGS-GN 算法可以正常地收斂, 并獲得較高收斂精度; 對(duì)于高斯牛頓法可以收斂的數(shù)據(jù)集, BFGS-GN 算法同樣可以獲得相同的精度和迭代效率。因此, 本文提出的方法可以很好地解決高斯牛頓方法對(duì)于初值敏感的問(wèn)題, 在保證收斂精度的前提下, 可以增加算法的魯棒性, 有效地拓展應(yīng)用場(chǎng)景。

[1] Yan Lei, Chen Rui, Sun Huabo, et al. A novel bundle adjustment method with additional ground control point constraint. Remote Sensing Letters, 2017, 8(1): 68–77

[2] Zhang Chunsen, Zhu Shihuan, Zang Yufu, et al. GPS-supported bundle adjustment method of UAV by con-sidering exposure delay. Acta Geodaetica et Cartogra-phica Sinica, 2017, 46(5): 565–572

[3] Dabeer O, Gowaikar R, Grzechnik S K, et al. An end-to-end system for crowdsourced 3D maps for autono-mous vehicles: the mapping component // IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Vancouver, 2017: 634–641

[4] Yang S C, Song Y, Kaess M, et al. Pop-up SLAM: semantic monocular plane SLAM for low-texture environments // IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Daejeon, 2016: 1222–1229

[5] Wu Changchang, Agarwal S, Curless B, et al. Mul-ticore bundle adjustment // The 24th IEEE Confe-rence on Computer Vision and Pattern Recognition (CVPR). Providence, RI, 2011: 3057–3064

[6] Zhao Liang, Huang Shoudong, Sun Yanbiao, et al. ParallaxBA: bundle adjustment using parallax angle feature parametrization. The International Journal of Robotics Research, 2015, 34(4/5): 493–516

[7] Tang Chengzhou, Tan Ping. BA-Net: dense bundle adjustment networks [EB/OL]. (2019–08–25) [2019–12–01]. https://arxiv.org/abs/1806.04807

[8] Madsen K, Tingle O, Nielsen H B, et al. Methods for non-linear least squares problems. 2nd ed. Lyngb: Technical University of Denmark, 2004

[9] Hager W W, Zhang H. A survey of nonlinear con-jugate gradient methods. Pacific Journal of Optimi-zation, 2006, 2(1): 35–58

[10] Byr?d M, Astrom K. Conjugate gradient bundle ad-justment // European Conference on Computer Vision. Heraklion, 2010: 114–127

[11] Zhou Weijun, Chen Xiaojun. Global convergence of a new hybrid Gauss-Newton structured BFGS method for nonlinear least squares problems. Siam Journal on Optimization, 2010, 20(5): 2422–2441

[12] Li Yanyan, Fan Shiyue, Sun Yanbiao, et al. Bundle adjustment method using sparse BFGS solution. Remote Sensing Letters, 2018, 9(8): 789–798

[13] 高翔, 張濤. 視覺(jué) SLAM 十四講: 從理論到實(shí)踐. 北京: 電子工業(yè)出版社, 2017: 248–253

A BFGS-Corrected Gauss-Newton Solver for Bundle Adjustment

ZHAO Shuaihua1, LI Yanyan2, CAO Jian1,?, CAO Xixin1

1. School of Software and Microelectronics, Peking University, Beijing 102600; 2. Technische Universit?t München, Munich 80333; ? Corresponding author, E-mail: caojian@ss.pku.edu.cn

Aiming at the problem that the Gauss-Newton (GN) method is sensitive to the initial information matrixin the Bundle Adjustment (BA) model, which leads to limited application scenarios, the paper proposes a novel method BFGS-GN using BFGS (Broyden-Fletcher-Goldfarb-Shanno) algorithm to improve the traditional Gauss-Newton method. When the information matrix of the Gauss-Newton method loses positive definiteness, BFGS algorithm can be used to modify the normal equations, which fundamentally eliminates the mathematical defect that the Gauss-Newton method is sensitive to initial values. Experimental results demonstrate that proposed method is robust to different types of initials. The same accuracy and the number of iterations as GN can be obtained when the initial values are good. As for bad inputs, GN-based BA method cannot work but BFGS-GN can converge to a minimum.

bundle adjustment; Gauss-Newton;BFGS algorithm;initial value robustness

10.13209/j.0479-8023.2020.098

國(guó)家重點(diǎn)研發(fā)計(jì)劃(2018YFE0203801)資助

2019-12–10;

2020–02–04