基于慣性先驗(yàn)校正圖像灰度的VIO 前端改良方法

徐錦樂(lè)，潘樹國(guó)，高旺，劉宏，王向

(東南大學(xué)儀器科學(xué)與工程學(xué)院,南京 210096)

0 引言

自動(dòng)駕駛等無(wú)人系統(tǒng)需要自主實(shí)現(xiàn)定位、導(dǎo)航和規(guī)劃等功能，因此移動(dòng)載體必須具備在信息缺失或是完全陌生的環(huán)境中同時(shí)進(jìn)行定位和建圖(SLAM)的能力，SLAM 技術(shù)作為自主移動(dòng)載體的必備能力[1]，也成為當(dāng)今移動(dòng)載體高精度定位領(lǐng)域的研究熱點(diǎn)之一[2]，并取得了許多引人注目的成果[3-5].其中，基于相機(jī)和慣性測(cè)量單元(IMU)互補(bǔ)的傳感器，具有成本低和體積小的優(yōu)勢(shì)，視覺(jué)慣性里程計(jì)(VIO)技術(shù)在增強(qiáng)現(xiàn)實(shí)和空中導(dǎo)航等領(lǐng)域的應(yīng)用中成為主流定位手段[6].特別是在地下、室內(nèi)等復(fù)雜環(huán)境中，傳統(tǒng)的定位方法例如全球衛(wèi)星導(dǎo)航系統(tǒng)(GNSS)的性能退化，無(wú)法提供準(zhǔn)確定位[7]，而VIO 可以在一定程度上實(shí)現(xiàn)自主定位，成為一種在封閉空間中使用的定位手段.

VIO 技術(shù)視覺(jué)前端主要分為直接法和特征點(diǎn)法.在封閉環(huán)境導(dǎo)致相機(jī)光照條件不佳的情況下，與直接法相比，特征點(diǎn)法在面對(duì)圖像低質(zhì)量問(wèn)題時(shí)的表現(xiàn)更具魯棒性.特征點(diǎn)法根據(jù)匹配方法的不同又可分為光流匹配和特征描述符匹配[8]，光流法相比于后者運(yùn)行效率更快，但對(duì)照明變化和快速運(yùn)動(dòng)較為敏感，劇烈的照明變化可能會(huì)導(dǎo)致VIO 缺少足夠的視覺(jué)幾何約束來(lái)參與優(yōu)化而出現(xiàn)系統(tǒng)發(fā)散[9].因此，對(duì)VIO 視覺(jué)前端圖像增強(qiáng)的改進(jìn)有重要意義.

目前已有許多圖像增強(qiáng)方法，例如基于Retinex理論的圖像增強(qiáng)算法[10]，包括單尺度增強(qiáng)和多尺度增強(qiáng)[11]，其將原始圖像分解為照度分量和反射分量，由反射分量作為增強(qiáng)結(jié)果；He 等[12]提出的暗通道先驗(yàn)去霧算法，利用暗通道圖復(fù)原清晰圖像，并結(jié)合導(dǎo)向?yàn)V波提升算法效果；經(jīng)典且廣泛應(yīng)用的直方圖均衡化算法等[13].以上算法雖然在圖像增強(qiáng)方面效果非凡，但多數(shù)并不是為視覺(jué)特征跟蹤而專門研究，且特征的穩(wěn)定跟蹤需要依賴在不同場(chǎng)景下對(duì)不同策略進(jìn)行合理的選擇和搭配.Song 等[9]針對(duì)照明改變場(chǎng)景中圖像特征跟蹤困難和圖像噪聲過(guò)大的問(wèn)題，提出一種將連續(xù)圖像增強(qiáng)到相同亮度水平具有約束的塊噪聲去除算法，并在EuRoC 數(shù)據(jù)集驗(yàn)證了其算法的優(yōu)勢(shì).高揚(yáng)等[14]將圖像轉(zhuǎn)換到HSV 空間，對(duì)其中V 通道中的亮度信息進(jìn)行處理，使用伽馬校正和直方圖均衡化加權(quán)融合的方式增強(qiáng)圖像，在改良隨機(jī)抽樣一致性(RANSAC)算法處理后，提高了圖像中特征點(diǎn)提取數(shù)量以及匹配的成功率.尹勝楠等[15]對(duì)自動(dòng)彩色增強(qiáng)(automatic color enhancement，ACE)算法進(jìn)行改進(jìn)，使用快速ACE 算法在提升特征點(diǎn)提取數(shù)量和匹配成功數(shù)量上都取得了較大進(jìn)步.Zhu 等[8]對(duì)前后圖像的特征點(diǎn)鄰域的亮度變化進(jìn)行線性估計(jì)，根據(jù)結(jié)果對(duì)整幅圖像進(jìn)行灰度伽馬校正，在EuRoC 數(shù)據(jù)集上取得了不錯(cuò)的效果.Tomasi 等[16]設(shè)計(jì)了深度卷積神經(jīng)網(wǎng)絡(luò)(CNN)模型，以自適應(yīng)地改變相機(jī)曝光時(shí)間和增益，從而增加了高質(zhì)量特征點(diǎn)的數(shù)量.

以上算法都針對(duì)光照變化提出了適合SLAM 系統(tǒng)的圖像增強(qiáng)處理方法，但沒(méi)有圖像序列信息的前后關(guān)聯(lián)價(jià)值完全利用；如若圖像局部光照變化有別于整體，傳統(tǒng)方法也將無(wú)法有效應(yīng)對(duì).本文提出了一種基于慣性先驗(yàn)校正圖像灰度的方法，其中在VIO 系統(tǒng)中充分利用IMU 信息作為先驗(yàn)信息對(duì)特征點(diǎn)位置進(jìn)行預(yù)測(cè)，并對(duì)預(yù)測(cè)點(diǎn)所在區(qū)域的像素進(jìn)行伽馬校正.由于圖像中明暗變化的非一致性，在避免預(yù)測(cè)點(diǎn)區(qū)域圖像質(zhì)量受光照變化大幅下降的同時(shí)，還需對(duì)圖像中光照過(guò)暗或過(guò)亮而導(dǎo)致細(xì)節(jié)確定的區(qū)域進(jìn)行自適應(yīng)伽馬校正，并根據(jù)幾何約束和圖像相似性剔除誤匹配后，以余下預(yù)測(cè)點(diǎn)作為迭代初值，得到匹配準(zhǔn)確率更高的光流追蹤結(jié)果，作為視覺(jué)幾何約束參與因子圖優(yōu)化(FGO)從而估計(jì)載體運(yùn)動(dòng)軌跡.

1 VIO 前端改良方法

本文的系統(tǒng)框圖如圖1 所示，以已知深度的特征點(diǎn)為被跟蹤點(diǎn)，通過(guò)IMU 預(yù)積分結(jié)果得到相機(jī)位姿的先驗(yàn)估計(jì)；以該結(jié)果預(yù)測(cè)被跟蹤點(diǎn)在當(dāng)前圖像中的像素位置，取預(yù)測(cè)點(diǎn)和被跟蹤點(diǎn)鄰域范圍內(nèi)的像素信息計(jì)算圖像結(jié)構(gòu)相似性衡量指標(biāo)(SSIM)結(jié)果，并根據(jù)結(jié)果選擇不同策略對(duì)預(yù)測(cè)點(diǎn)區(qū)域進(jìn)行灰度變化(伽馬校正)；在L-K 光流金字塔中使用預(yù)測(cè)點(diǎn)像素位置為光流法迭代初值以得到跟蹤效果可靠的特征點(diǎn)對(duì)，將其作為VIO 系統(tǒng)前端的視覺(jué)幾何約束(重投影誤差因子)，與IMU 殘差因子一同進(jìn)入后端優(yōu)化得到系統(tǒng)位姿估計(jì)結(jié)果.

圖1 慣性先驗(yàn)校正圖像灰度的VIO 系統(tǒng)前端改良方法原理框圖

1.1 慣性先驗(yàn)預(yù)測(cè)特征點(diǎn)位置

默認(rèn)情況下，L-K 光流法遵循灰度不變假設(shè)[17]，以前一幀圖像中提取的特征點(diǎn)像素位置本身為初始位置估計(jì)進(jìn)行光流追蹤，這對(duì)相機(jī)運(yùn)動(dòng)狀態(tài)和圖像灰度變化情況要求較為嚴(yán)格.如果相機(jī)劇烈旋轉(zhuǎn)運(yùn)動(dòng)或是光照條件發(fā)生變化造成圖像質(zhì)量大幅下降，都可能出現(xiàn)光流跟蹤失敗的現(xiàn)象，導(dǎo)致匹配成功的特征點(diǎn)數(shù)量減少，使VIO 系統(tǒng)魯棒性降低.而使用慣性信息預(yù)積分結(jié)果作為相機(jī)運(yùn)動(dòng)先驗(yàn)估計(jì)預(yù)測(cè)特征點(diǎn)像素位置的做法，可以在一定程度上彌補(bǔ)L-K 光流法的不足.

通過(guò)對(duì)兩幀圖像時(shí)間內(nèi)的IMU 預(yù)積分，我們可以初步估計(jì)兩個(gè)時(shí)刻間的相機(jī)位姿變化，對(duì)應(yīng)公式如下：

1.2 鄰域像素SSIM 計(jì)算

得到特征點(diǎn)的預(yù)測(cè)位置后選取特征點(diǎn)在兩幀內(nèi)對(duì)應(yīng)的鄰域像素，選取規(guī)則如圖2 所示，其中p與q分別為被跟蹤點(diǎn)與預(yù)測(cè)點(diǎn).

圖2 特征點(diǎn)鄰域選取規(guī)則

選取鄰域像素中尺寸相同且位置對(duì)應(yīng)的矩形圖像區(qū)域，計(jì)算兩圖像矩陣的SSIM 結(jié)果.SSIM 是常用的一種圖像相似性衡量指標(biāo)，給定大小一致的圖像x和y，其SSIM 結(jié)果可由以下公式得到：

式中：l(x,y) 為x、y的灰度相似性；c(x,y)為x、y的對(duì)比度相似性；s(x,y)為x、y的結(jié)構(gòu)相似性；α、β、λ(α ≥0,β ≥0,λ ≥0)分別為以上三者在圖像相似性中占據(jù)的比重，通常取值為 α=β=λ=1 .

灰度相似性、對(duì)比度相似性和結(jié)構(gòu)相似性分別表示為：

式中：μx、μy為特征點(diǎn)在前、后兩幀圖像中鄰域像素的平均灰度值；σx、σy、σxy為特征點(diǎn)在前、后兩幀圖像中鄰域像素的標(biāo)準(zhǔn)差和協(xié)方差；C1、C2、C3為控制參數(shù)，本文方法分別設(shè)置為：

式中：k1=0.01 ；k2=0.03；L取圖像灰度最大值255(為SSIM 默認(rèn)參數(shù)值).特征點(diǎn)鄰域像素相似性越高.特征匹配成功率越高，同時(shí)可以根據(jù)SSIM 結(jié)果對(duì)結(jié)構(gòu)相似性低、對(duì)比度低或紋理缺失的特征點(diǎn)進(jìn)行剔除，當(dāng)s(x,y) 小于某值時(shí)，說(shuō)明對(duì)應(yīng)區(qū)域的結(jié)構(gòu)相似較低，匹配成功幾率小.本文根據(jù)實(shí)驗(yàn)經(jīng)驗(yàn)，在s(x,y) 低于0.3 時(shí)將對(duì)應(yīng)特征點(diǎn)視作異常值剔除，不作為視覺(jué)幾何約束參與系統(tǒng)FGO.

1.3 自適應(yīng)伽馬校正

在篩選保留合適的特征點(diǎn)之后，需要評(píng)估其SSIM 結(jié)果，對(duì)預(yù)測(cè)點(diǎn)所在圖像區(qū)域進(jìn)行自適應(yīng)伽馬校正，灰度值計(jì)算可由下式表示：

式中：I′(x,y) 為校正后灰度值；I(x,y) 為原像素灰度值；γ 為關(guān)于I(x,y) 的函數(shù)，表示伽馬校正數(shù)值，不同情況對(duì)應(yīng)不同處理策略.

多數(shù)場(chǎng)景下，一張圖像不同區(qū)域所需伽馬校正的伽馬數(shù)值往往是不同的，要根據(jù)特征點(diǎn)鄰域灰度變化情況進(jìn)行自適應(yīng)伽馬校正.

1.3.1 光照條件大幅變化情況

在評(píng)估特征點(diǎn)在前后時(shí)刻所處兩鄰域的圖像相似性SSIM 指標(biāo)時(shí)，能夠跟蹤成功的情形往往灰度、對(duì)比度、結(jié)構(gòu)相似性都接近于1.當(dāng)兩者鄰域內(nèi)信息只存在灰度上的差異時(shí)，在SSIM 指標(biāo)中前后圖像對(duì)比度、結(jié)構(gòu)相似性高且灰度相似性較低時(shí)對(duì)當(dāng)前圖像特征點(diǎn)鄰域進(jìn)行伽馬校正，校正結(jié)果可由下式計(jì)算得到：

式中：伽馬校正數(shù)值 γ 設(shè)為 l gμyμx，以指數(shù)形式求得兩幀間平均灰度值的差距.

在一次校正后，為避免區(qū)域邊緣出現(xiàn)分層現(xiàn)象，需要對(duì)校正區(qū)域進(jìn)行取值調(diào)整，保留灰度值得到合理伽馬校正的像素：

式中：I′′(x,y) 為 最終校正后的圖像灰度；G為與特征點(diǎn)鄰域?qū)?yīng)矩陣階數(shù)相等的高斯矩陣；k為增益，取值為G矩陣中最大值的倒數(shù)；m ax 函數(shù)與min 函數(shù)分別對(duì)應(yīng)不同的光照變化情況，意在避免分層現(xiàn)象產(chǎn)生并保留區(qū)域中更亮(暗)的像素，以達(dá)到校正特征點(diǎn)鄰域像素灰度值在圖像序列中避免大幅波動(dòng)的目的.

1.3.2 光照條件不佳情況

在相機(jī)采集圖像的過(guò)程中還存在前后時(shí)刻特征點(diǎn)鄰域都過(guò)亮或過(guò)暗的情況，采取的自適應(yīng)伽馬校正由如下公式表示：

式中，M為對(duì)應(yīng)圖像的灰度值中位數(shù).同樣，使用式(13)對(duì)校正區(qū)域進(jìn)行灰度調(diào)整，避免圖像分層.

1.3.3 對(duì)比度變化情況

在相機(jī)光照條件發(fā)生變化時(shí)，隨著灰度變化采集圖像的對(duì)比度往往也會(huì)出現(xiàn)變化，因此，當(dāng)鄰域像素的對(duì)比度相似性較低而結(jié)構(gòu)相似性較高時(shí)，根據(jù)對(duì)比度進(jìn)行伽馬校正，但只針對(duì)圖像對(duì)比度變低的情況進(jìn)行校正，判斷依據(jù)為 σx是否大于 σy，對(duì)應(yīng)的伽馬校正公式相對(duì)于1.3.1 中略有不同：

式中，c為區(qū)域內(nèi)像素SSIM 結(jié)果中的對(duì)比相似度.同樣使用式(13)對(duì)校正區(qū)域進(jìn)行調(diào)整，對(duì)應(yīng)的灰度校正曲線如圖3 所示：

圖3 自適應(yīng)伽馬校正灰度曲線

綜上所述，根據(jù)SSIM 指標(biāo)結(jié)果選擇不同校正策略的自適應(yīng)伽馬校正流程圖如圖4 所示.

圖4 自適應(yīng)伽馬校正流程圖

需注意的是流程圖中的伽馬校正代表了1.3.1、1.3.2 和1.3.3 中不同光照變化情況下對(duì)應(yīng)的伽馬校正策略.

2 實(shí)驗(yàn)與結(jié)果分析

為驗(yàn)證本文方法實(shí)際效果，從視覺(jué)效果層面將本文方法校正圖像與原始圖像相比較；從耗時(shí)成本層面將本文方法平均耗時(shí)與VINS-Mono 中光流跟蹤平均耗時(shí)進(jìn)行比較；從定位精度層面，在TUM 數(shù)據(jù)集和真實(shí)室內(nèi)外環(huán)境中進(jìn)行實(shí)驗(yàn)，評(píng)估軌跡精度.本文實(shí)驗(yàn)使用的相機(jī)為?？倒I(yè)相機(jī)(型號(hào)：MV-CA016-10UC，分辨率：1 440×1 080)，系統(tǒng)運(yùn)行配置為CPU：IntelCorei7-11700 2.50 GHz，顯卡：NVIDIA GTX1060，內(nèi)存16 GB.

2.1 灰度校正視覺(jué)效果

本文所提方法處理結(jié)果與原始圖像如圖5 所示，抽取EuRoC 數(shù)據(jù)集V1_03_difficult 序列中光照變化幅度較大的5 張連續(xù)圖像進(jìn)行對(duì)比.

圖5 圖像處理前后對(duì)比

由圖5 可以看到，原始圖像的光照強(qiáng)度有較為明顯的變化，整體由亮變暗，但經(jīng)本文算法處理后，圖像在特征豐富區(qū)域的灰度變化較為穩(wěn)定，這有利于特征點(diǎn)的提取與跟蹤；本文方法對(duì)對(duì)比度較弱的區(qū)域也進(jìn)行了一定的圖像增強(qiáng)，這有利于新的特征點(diǎn)的提取.

2.2 特征跟蹤結(jié)果

經(jīng)本文方法處理后，特征提取與匹配結(jié)果得到一定改善，具體體現(xiàn)為相機(jī)大幅旋轉(zhuǎn)后慣性先驗(yàn)?zāi)転楣饬鞲櫶峁┨卣鼽c(diǎn)預(yù)測(cè)像素位置初值；相機(jī)在光照條件大幅變化后經(jīng)灰度校正仍能使特征點(diǎn)遵循光流法中的灰度不變假設(shè).抽取EuRoC 數(shù)據(jù)集V1_03_difficult序列中運(yùn)動(dòng)幅度劇烈、光照變化幅度大以及光線較暗不同情況下特征點(diǎn)跟蹤結(jié)果進(jìn)行對(duì)比如圖6、7所示.

圖6 光照條件大幅變化情況下的特征點(diǎn)跟蹤結(jié)果

圖6 表明，光照條件出現(xiàn)較大變化的情況下，灰度校正能夠減輕光照變化給特征提取與匹配所帶來(lái)的影響，處理圖像對(duì)比原始圖像保有更多跟蹤成功的點(diǎn).

圖7 結(jié)果表明，載體運(yùn)動(dòng)幅度過(guò)大時(shí)，慣性先驗(yàn)信息能夠?yàn)樘卣鼽c(diǎn)光流跟蹤提供更加準(zhǔn)確的初始迭代值，因此相較于原始圖像，特征點(diǎn)依然保持跟蹤穩(wěn)定.

圖7 載體大幅旋轉(zhuǎn)情況下的特征點(diǎn)跟蹤結(jié)果

由以上示例可見，在出現(xiàn)不利于特征提取、跟蹤的情況下，本文方法可以保有部分特征點(diǎn)的有效跟蹤，因此將通過(guò)隨機(jī)一致性檢驗(yàn)的特征點(diǎn)視為跟蹤成功的特征點(diǎn).以其占總體特征點(diǎn)的比例為評(píng)價(jià)標(biāo)準(zhǔn)，并與VINS-Mono 進(jìn)行對(duì)比，依然以V1_03_difficult序列為例，結(jié)果如圖8 所示.

圖8 特征點(diǎn)跟蹤成功率對(duì)比結(jié)果

從以上結(jié)果可得出結(jié)論：相機(jī)光照條件出現(xiàn)較大變化的情況下，本文處理方法能夠減輕相機(jī)運(yùn)動(dòng)、光照變化給特征提取與匹配所帶來(lái)的影響，提高特征點(diǎn)跟蹤成功率.

2.3 灰度校正耗時(shí)成本

由于不同時(shí)刻圖像的信息含量不同、特征點(diǎn)數(shù)量不同，而本文所提方法的時(shí)間復(fù)雜度與特征點(diǎn)數(shù)量以及鄰域大小相關(guān)，因此將本文方法在TUM 數(shù)據(jù)集上運(yùn)行的平均耗時(shí)與VINS-Mono 視覺(jué)前端中普通光流法的平均耗時(shí)進(jìn)行對(duì)比.結(jié)果如表1 所示，普通光流法耗時(shí)相對(duì)較少，但本文所提方法耗時(shí)在100 ms 以內(nèi)(相機(jī)采集頻率為10 Hz)，對(duì)VIO 系統(tǒng)的實(shí)時(shí)運(yùn)行沒(méi)有影響.

表1 TUM 數(shù)據(jù)集特征跟蹤耗時(shí)

2.4 定位精度

為客觀評(píng)估定位精度，本文將VINS-Mono 配置中的直方圖均衡化與回環(huán)檢測(cè)設(shè)置關(guān)閉，使用TUM數(shù)據(jù)集中的VI 數(shù)據(jù)集對(duì)本文方法和VINS-Mono 的定位均方根誤差(RMSE)進(jìn)行比較，定位精度對(duì)比結(jié)果如表2 所示.

表2 TUM 數(shù)據(jù)集定位誤差(RMSE)

在真實(shí)場(chǎng)景下對(duì)定位精度進(jìn)行對(duì)比，使用GNSS/INS 組合導(dǎo)航作為定位基準(zhǔn)，其定位精度為厘米級(jí)，絕對(duì)位置誤差的RMSE 對(duì)比結(jié)果如表3所示，定位軌跡如圖9、10 所示.

表3 真實(shí)環(huán)境定位RMSE

圖9 序列1 軌跡

圖10 序 列2 軌跡

從圖7、10 和表3 中數(shù)據(jù)看出，改進(jìn)視覺(jué)前端后，絕對(duì)位置誤差最大值相較VINS-Mono 更小.

序列1 測(cè)試環(huán)境為室外真實(shí)環(huán)境，運(yùn)動(dòng)軌跡長(zhǎng)度為274.9 m，本文方法與VINS-Mono 的定位絕對(duì)位置誤差對(duì)比如圖11 所示.

圖11 序列1 運(yùn)動(dòng)軌跡絕對(duì)位置誤差

由圖11 可見，經(jīng)過(guò)本文方法改進(jìn)視覺(jué)前端后，在真實(shí)場(chǎng)景運(yùn)行軌跡的絕對(duì)位置誤差整體相較于VINS-Mono 更低，且誤差波動(dòng)相對(duì)更小.

序列2 測(cè)試環(huán)境為室外真實(shí)環(huán)境，運(yùn)動(dòng)軌跡長(zhǎng)度為611.5 m，本文方法與VINS-Mono 的定位絕對(duì)位置誤差對(duì)比如圖12 所示.

圖12 序列2 運(yùn)動(dòng)軌跡絕對(duì)位置誤差(序列中一段時(shí)間載體處于靜止不動(dòng)狀態(tài))

可以看出，本文方法能夠提供更加穩(wěn)定和準(zhǔn)確的視覺(jué)幾何約束進(jìn)入VIO 系統(tǒng)后端優(yōu)化，提升軌跡定位精度.

3 結(jié)束語(yǔ)

本文針對(duì)光照變化場(chǎng)景圖像過(guò)亮、過(guò)暗和灰度大幅變化問(wèn)題，提出了一種基于慣性先驗(yàn)校正圖像灰度的方法，根據(jù)IMU 預(yù)積分的位姿估計(jì)預(yù)測(cè)特征點(diǎn)像素位置并結(jié)合圖像序列前后幀的局部圖像相似性進(jìn)行圖像增強(qiáng)；實(shí)驗(yàn)對(duì)比了處理前后圖像視覺(jué)效果，特征點(diǎn)跟蹤成功率變化和視覺(jué)前端特征跟蹤的耗時(shí)成本；與VINS-Mono 定位精度進(jìn)行比較，在TUM 數(shù)據(jù)集上定位精度比VINS-Mono 平均提升了30.7%，10 組數(shù)據(jù)的軌跡精度提升幅度最大可達(dá)到57.9%；在真實(shí)的室內(nèi)外環(huán)境下進(jìn)行了實(shí)驗(yàn)，軌跡精度與VINS-Mono 相比平均提高了17.7%.