徐錦樂,潘樹國,高旺,劉宏,王向
(東南大學(xué)儀器科學(xué)與工程學(xué)院,南京 210096)
自動(dòng)駕駛等無人系統(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ī)和慣性測量單元(IMU)互補(bǔ)的傳感器,具有成本低和體積小的優(yōu)勢,視覺慣性里程計(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)的性能退化,無法提供準(zhǔn)確定位[7],而VIO 可以在一定程度上實(shí)現(xiàn)自主定位,成為一種在封閉空間中使用的定位手段.
VIO 技術(shù)視覺前端主要分為直接法和特征點(diǎn)法.在封閉環(huán)境導(dǎo)致相機(jī)光照條件不佳的情況下,與直接法相比,特征點(diǎn)法在面對圖像低質(zhì)量問題時(shí)的表現(xiàn)更具魯棒性.特征點(diǎn)法根據(jù)匹配方法的不同又可分為光流匹配和特征描述符匹配[8],光流法相比于后者運(yùn)行效率更快,但對照明變化和快速運(yùn)動(dòng)較為敏感,劇烈的照明變化可能會(huì)導(dǎo)致VIO 缺少足夠的視覺幾何約束來參與優(yōu)化而出現(xiàn)系統(tǒng)發(fā)散[9].因此,對VIO 視覺前端圖像增強(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ù)并不是為視覺特征跟蹤而專門研究,且特征的穩(wěn)定跟蹤需要依賴在不同場景下對不同策略進(jìn)行合理的選擇和搭配.Song 等[9]針對照明改變場景中圖像特征跟蹤困難和圖像噪聲過大的問題,提出一種將連續(xù)圖像增強(qiáng)到相同亮度水平具有約束的塊噪聲去除算法,并在EuRoC 數(shù)據(jù)集驗(yàn)證了其算法的優(yōu)勢.高揚(yáng)等[14]將圖像轉(zhuǎn)換到HSV 空間,對其中V 通道中的亮度信息進(jìn)行處理,使用伽馬校正和直方圖均衡化加權(quán)融合的方式增強(qiáng)圖像,在改良隨機(jī)抽樣一致性(RANSAC)算法處理后,提高了圖像中特征點(diǎn)提取數(shù)量以及匹配的成功率.尹勝楠等[15]對自動(dòng)彩色增強(qiáng)(automatic color enhancement,ACE)算法進(jìn)行改進(jìn),使用快速ACE 算法在提升特征點(diǎn)提取數(shù)量和匹配成功數(shù)量上都取得了較大進(jìn)步.Zhu 等[8]對前后圖像的特征點(diǎn)鄰域的亮度變化進(jìn)行線性估計(jì),根據(jù)結(jié)果對整幅圖像進(jìn)行灰度伽馬校正,在EuRoC 數(shù)據(jù)集上取得了不錯(cuò)的效果.Tomasi 等[16]設(shè)計(jì)了深度卷積神經(jīng)網(wǎng)絡(luò)(CNN)模型,以自適應(yīng)地改變相機(jī)曝光時(shí)間和增益,從而增加了高質(zhì)量特征點(diǎn)的數(shù)量.
以上算法都針對光照變化提出了適合SLAM 系統(tǒng)的圖像增強(qiáng)處理方法,但沒有圖像序列信息的前后關(guān)聯(lián)價(jià)值完全利用;如若圖像局部光照變化有別于整體,傳統(tǒng)方法也將無法有效應(yīng)對.本文提出了一種基于慣性先驗(yàn)校正圖像灰度的方法,其中在VIO 系統(tǒng)中充分利用IMU 信息作為先驗(yàn)信息對特征點(diǎn)位置進(jìn)行預(yù)測,并對預(yù)測點(diǎn)所在區(qū)域的像素進(jìn)行伽馬校正.由于圖像中明暗變化的非一致性,在避免預(yù)測點(diǎn)區(qū)域圖像質(zhì)量受光照變化大幅下降的同時(shí),還需對圖像中光照過暗或過亮而導(dǎo)致細(xì)節(jié)確定的區(qū)域進(jìn)行自適應(yīng)伽馬校正,并根據(jù)幾何約束和圖像相似性剔除誤匹配后,以余下預(yù)測點(diǎn)作為迭代初值,得到匹配準(zhǔn)確率更高的光流追蹤結(jié)果,作為視覺幾何約束參與因子圖優(yōu)化(FGO)從而估計(jì)載體運(yùn)動(dòng)軌跡.
本文的系統(tǒng)框圖如圖1 所示,以已知深度的特征點(diǎn)為被跟蹤點(diǎn),通過IMU 預(yù)積分結(jié)果得到相機(jī)位姿的先驗(yàn)估計(jì);以該結(jié)果預(yù)測被跟蹤點(diǎn)在當(dāng)前圖像中的像素位置,取預(yù)測點(diǎn)和被跟蹤點(diǎn)鄰域范圍內(nèi)的像素信息計(jì)算圖像結(jié)構(gòu)相似性衡量指標(biāo)(SSIM)結(jié)果,并根據(jù)結(jié)果選擇不同策略對預(yù)測點(diǎn)區(qū)域進(jìn)行灰度變化(伽馬校正);在L-K 光流金字塔中使用預(yù)測點(diǎn)像素位置為光流法迭代初值以得到跟蹤效果可靠的特征點(diǎn)對,將其作為VIO 系統(tǒng)前端的視覺幾何約束(重投影誤差因子),與IMU 殘差因子一同進(jìn)入后端優(yōu)化得到系統(tǒng)位姿估計(jì)結(jié)果.
圖1 慣性先驗(yàn)校正圖像灰度的VIO 系統(tǒng)前端改良方法原理框圖
默認(rèn)情況下,L-K 光流法遵循灰度不變假設(shè)[17],以前一幀圖像中提取的特征點(diǎn)像素位置本身為初始位置估計(jì)進(jìn)行光流追蹤,這對相機(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ù)測特征點(diǎn)像素位置的做法,可以在一定程度上彌補(bǔ)L-K 光流法的不足.
通過對兩幀圖像時(shí)間內(nèi)的IMU 預(yù)積分,我們可以初步估計(jì)兩個(gè)時(shí)刻間的相機(jī)位姿變化,對應(yīng)公式如下:
得到特征點(diǎn)的預(yù)測位置后選取特征點(diǎn)在兩幀內(nèi)對應(yīng)的鄰域像素,選取規(guī)則如圖2 所示,其中p與q分別為被跟蹤點(diǎn)與預(yù)測點(diǎn).
圖2 特征點(diǎn)鄰域選取規(guī)則
選取鄰域像素中尺寸相同且位置對應(yīng)的矩形圖像區(qū)域,計(jì)算兩圖像矩陣的SSIM 結(jié)果.SSIM 是常用的一種圖像相似性衡量指標(biāo),給定大小一致的圖像x和y,其SSIM 結(jié)果可由以下公式得到:
式中:l(x,y) 為x、y的灰度相似性;c(x,y)為x、y的對比度相似性;s(x,y)為x、y的結(jié)構(gòu)相似性;α、β、λ(α ≥0,β ≥0,λ ≥0)分別為以上三者在圖像相似性中占據(jù)的比重,通常取值為 α=β=λ=1 .
灰度相似性、對比度相似性和結(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é)果對結(jié)構(gòu)相似性低、對比度低或紋理缺失的特征點(diǎn)進(jìn)行剔除,當(dāng)s(x,y) 小于某值時(shí),說明對應(yīng)區(qū)域的結(jié)構(gòu)相似較低,匹配成功幾率小.本文根據(jù)實(shí)驗(yàn)經(jīng)驗(yàn),在s(x,y) 低于0.3 時(shí)將對應(yīng)特征點(diǎn)視作異常值剔除,不作為視覺幾何約束參與系統(tǒng)FGO.
在篩選保留合適的特征點(diǎn)之后,需要評估其SSIM 結(jié)果,對預(yù)測點(diǎn)所在圖像區(qū)域進(jìn)行自適應(yīng)伽馬校正,灰度值計(jì)算可由下式表示:
式中:I′(x,y) 為校正后灰度值;I(x,y) 為原像素灰度值;γ 為關(guān)于I(x,y) 的函數(shù),表示伽馬校正數(shù)值,不同情況對應(yīng)不同處理策略.
多數(shù)場景下,一張圖像不同區(qū)域所需伽馬校正的伽馬數(shù)值往往是不同的,要根據(jù)特征點(diǎn)鄰域灰度變化情況進(jìn)行自適應(yīng)伽馬校正.
1.3.1 光照條件大幅變化情況
在評估特征點(diǎn)在前后時(shí)刻所處兩鄰域的圖像相似性SSIM 指標(biāo)時(shí),能夠跟蹤成功的情形往往灰度、對比度、結(jié)構(gòu)相似性都接近于1.當(dāng)兩者鄰域內(nèi)信息只存在灰度上的差異時(shí),在SSIM 指標(biāo)中前后圖像對比度、結(jié)構(gòu)相似性高且灰度相似性較低時(shí)對當(dāng)前圖像特征點(diǎn)鄰域進(jìn)行伽馬校正,校正結(jié)果可由下式計(jì)算得到:
式中:伽馬校正數(shù)值 γ 設(shè)為 l gμyμx,以指數(shù)形式求得兩幀間平均灰度值的差距.
在一次校正后,為避免區(qū)域邊緣出現(xiàn)分層現(xiàn)象,需要對校正區(qū)域進(jìn)行取值調(diào)整,保留灰度值得到合理伽馬校正的像素:
式中:I′′(x,y) 為 最終校正后的圖像灰度;G為與特征點(diǎn)鄰域?qū)?yīng)矩陣階數(shù)相等的高斯矩陣;k為增益,取值為G矩陣中最大值的倒數(shù);m ax 函數(shù)與min 函數(shù)分別對應(yīng)不同的光照變化情況,意在避免分層現(xiàn)象產(chǎn)生并保留區(qū)域中更亮(暗)的像素,以達(dá)到校正特征點(diǎn)鄰域像素灰度值在圖像序列中避免大幅波動(dòng)的目的.
1.3.2 光照條件不佳情況
在相機(jī)采集圖像的過程中還存在前后時(shí)刻特征點(diǎn)鄰域都過亮或過暗的情況,采取的自適應(yīng)伽馬校正由如下公式表示:
式中,M為對應(yīng)圖像的灰度值中位數(shù).同樣,使用式(13)對校正區(qū)域進(jìn)行灰度調(diào)整,避免圖像分層.
1.3.3 對比度變化情況
在相機(jī)光照條件發(fā)生變化時(shí),隨著灰度變化采集圖像的對比度往往也會(huì)出現(xiàn)變化,因此,當(dāng)鄰域像素的對比度相似性較低而結(jié)構(gòu)相似性較高時(shí),根據(jù)對比度進(jìn)行伽馬校正,但只針對圖像對比度變低的情況進(jìn)行校正,判斷依據(jù)為 σx是否大于 σy,對應(yīng)的伽馬校正公式相對于1.3.1 中略有不同:
式中,c為區(qū)域內(nèi)像素SSIM 結(jié)果中的對比相似度.同樣使用式(13)對校正區(qū)域進(jìn)行調(diào)整,對應(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 中不同光照變化情況下對應(yīng)的伽馬校正策略.
為驗(yàn)證本文方法實(shí)際效果,從視覺效果層面將本文方法校正圖像與原始圖像相比較;從耗時(shí)成本層面將本文方法平均耗時(shí)與VINS-Mono 中光流跟蹤平均耗時(shí)進(jìn)行比較;從定位精度層面,在TUM 數(shù)據(jù)集和真實(shí)室內(nèi)外環(huán)境中進(jìn)行實(shí)驗(yàn),評估軌跡精度.本文實(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.
本文所提方法處理結(jié)果與原始圖像如圖5 所示,抽取EuRoC 數(shù)據(jù)集V1_03_difficult 序列中光照變化幅度較大的5 張連續(xù)圖像進(jìn)行對比.
圖5 圖像處理前后對比
由圖5 可以看到,原始圖像的光照強(qiáng)度有較為明顯的變化,整體由亮變暗,但經(jīng)本文算法處理后,圖像在特征豐富區(qū)域的灰度變化較為穩(wěn)定,這有利于特征點(diǎn)的提取與跟蹤;本文方法對對比度較弱的區(qū)域也進(jìn)行了一定的圖像增強(qiáng),這有利于新的特征點(diǎn)的提取.
經(jīng)本文方法處理后,特征提取與匹配結(jié)果得到一定改善,具體體現(xiàn)為相機(jī)大幅旋轉(zhuǎn)后慣性先驗(yàn)?zāi)転楣饬鞲櫶峁┨卣鼽c(diǎn)預(yù)測像素位置初值;相機(jī)在光照條件大幅變化后經(jīng)灰度校正仍能使特征點(diǎn)遵循光流法中的灰度不變假設(shè).抽取EuRoC 數(shù)據(jù)集V1_03_difficult序列中運(yùn)動(dòng)幅度劇烈、光照變化幅度大以及光線較暗不同情況下特征點(diǎn)跟蹤結(jié)果進(jìn)行對比如圖6、7所示.
圖6 光照條件大幅變化情況下的特征點(diǎn)跟蹤結(jié)果
圖6 表明,光照條件出現(xiàn)較大變化的情況下,灰度校正能夠減輕光照變化給特征提取與匹配所帶來的影響,處理圖像對比原始圖像保有更多跟蹤成功的點(diǎn).
圖7 結(jié)果表明,載體運(yùn)動(dòng)幅度過大時(shí),慣性先驗(yàn)信息能夠?yàn)樘卣鼽c(diǎn)光流跟蹤提供更加準(zhǔn)確的初始迭代值,因此相較于原始圖像,特征點(diǎn)依然保持跟蹤穩(wěn)定.
圖7 載體大幅旋轉(zhuǎn)情況下的特征點(diǎn)跟蹤結(jié)果
由以上示例可見,在出現(xiàn)不利于特征提取、跟蹤的情況下,本文方法可以保有部分特征點(diǎn)的有效跟蹤,因此將通過隨機(jī)一致性檢驗(yàn)的特征點(diǎn)視為跟蹤成功的特征點(diǎn).以其占總體特征點(diǎn)的比例為評價(jià)標(biāo)準(zhǔn),并與VINS-Mono 進(jìn)行對比,依然以V1_03_difficult序列為例,結(jié)果如圖8 所示.
圖8 特征點(diǎn)跟蹤成功率對比結(jié)果
從以上結(jié)果可得出結(jié)論:相機(jī)光照條件出現(xiàn)較大變化的情況下,本文處理方法能夠減輕相機(jī)運(yùn)動(dòng)、光照變化給特征提取與匹配所帶來的影響,提高特征點(diǎn)跟蹤成功率.
由于不同時(shí)刻圖像的信息含量不同、特征點(diǎn)數(shù)量不同,而本文所提方法的時(shí)間復(fù)雜度與特征點(diǎn)數(shù)量以及鄰域大小相關(guān),因此將本文方法在TUM 數(shù)據(jù)集上運(yùn)行的平均耗時(shí)與VINS-Mono 視覺前端中普通光流法的平均耗時(shí)進(jìn)行對比.結(jié)果如表1 所示,普通光流法耗時(shí)相對較少,但本文所提方法耗時(shí)在100 ms 以內(nèi)(相機(jī)采集頻率為10 Hz),對VIO 系統(tǒng)的實(shí)時(shí)運(yùn)行沒有影響.
表1 TUM 數(shù)據(jù)集特征跟蹤耗時(shí)
為客觀評估定位精度,本文將VINS-Mono 配置中的直方圖均衡化與回環(huán)檢測設(shè)置關(guān)閉,使用TUM數(shù)據(jù)集中的VI 數(shù)據(jù)集對本文方法和VINS-Mono 的定位均方根誤差(RMSE)進(jìn)行比較,定位精度對比結(jié)果如表2 所示.
表2 TUM 數(shù)據(jù)集定位誤差(RMSE)
在真實(shí)場景下對定位精度進(jìn)行對比,使用GNSS/INS 組合導(dǎo)航作為定位基準(zhǔn),其定位精度為厘米級(jí),絕對位置誤差的RMSE 對比結(jié)果如表3所示,定位軌跡如圖9、10 所示.
表3 真實(shí)環(huán)境定位RMSE
圖9 序列1 軌跡
圖10 序 列2 軌跡
從圖7、10 和表3 中數(shù)據(jù)看出,改進(jìn)視覺前端后,絕對位置誤差最大值相較VINS-Mono 更小.
序列1 測試環(huán)境為室外真實(shí)環(huán)境,運(yùn)動(dòng)軌跡長度為274.9 m,本文方法與VINS-Mono 的定位絕對位置誤差對比如圖11 所示.
圖11 序列1 運(yùn)動(dòng)軌跡絕對位置誤差
由圖11 可見,經(jīng)過本文方法改進(jìn)視覺前端后,在真實(shí)場景運(yùn)行軌跡的絕對位置誤差整體相較于VINS-Mono 更低,且誤差波動(dòng)相對更小.
序列2 測試環(huán)境為室外真實(shí)環(huán)境,運(yùn)動(dòng)軌跡長度為611.5 m,本文方法與VINS-Mono 的定位絕對位置誤差對比如圖12 所示.
圖12 序列2 運(yùn)動(dòng)軌跡絕對位置誤差(序列中一段時(shí)間載體處于靜止不動(dòng)狀態(tài))
可以看出,本文方法能夠提供更加穩(wěn)定和準(zhǔn)確的視覺幾何約束進(jìn)入VIO 系統(tǒng)后端優(yōu)化,提升軌跡定位精度.
本文針對光照變化場景圖像過亮、過暗和灰度大幅變化問題,提出了一種基于慣性先驗(yàn)校正圖像灰度的方法,根據(jù)IMU 預(yù)積分的位姿估計(jì)預(yù)測特征點(diǎn)像素位置并結(jié)合圖像序列前后幀的局部圖像相似性進(jìn)行圖像增強(qiáng);實(shí)驗(yàn)對比了處理前后圖像視覺效果,特征點(diǎn)跟蹤成功率變化和視覺前端特征跟蹤的耗時(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%.