本發(fā)明涉及圖像的技術領域，更具體地，涉及基于色彩圖像和紅外圖像融合的即時定位與建圖系統(tǒng)。

背景技術：

實時（即時）的定位與建圖（slam）在機器人、無人車、無人機和增強現(xiàn)實（ar）應用中發(fā)揮著重要的作用。對于機器人和無人車，slam不可或缺，因為需要slam模塊在制定控制策略之前感知周圍環(huán)境。對于近年來很熱門話題——虛擬現(xiàn)實和增強現(xiàn)實，slam提供了他們所需的前提條件——重建環(huán)境3d地圖。

然而當前基于色彩圖像的slam系統(tǒng)都有一個共同的缺點，即它們只能在光照充足的環(huán)境中工作。當面對低光照環(huán)境時，現(xiàn)有技術的視覺方法將失去它們的功能，因為它們不能獲得足夠的色彩信息的特征或紋理來支撐系統(tǒng)的正常運行。紅外相機可以通過接受周圍場景的紅外熱輻射信息來獲取物體的溫度并進一步得到對應的紅外熱圖像，該紅外信息不直接依賴于光照強度，因此近年來紅外相機開始被用在物體表面3d溫度圖像的建立以及定位與建圖中。

雖然對于不能獲得足夠的色彩信息的低光照環(huán)境來說，紅外傳感器可以是很好的信息源，但是單獨使用紅外傳感器進行定位會存在一個比較明顯的問題，即基于紅外傳感器的定位系統(tǒng)需要周圍環(huán)境的場景中存在一定的溫度差異，對溫度差異較小的環(huán)境定位系統(tǒng)會由于信息不足而失效。因此使用單獨的紅外傳感器進行定位和建圖會具有比較小適用范圍，一般用于特定的場景，如變電站、機房控制室等特殊環(huán)境。

技術實現(xiàn)要素：

本發(fā)明為克服上述現(xiàn)有技術所述的至少一種缺陷，提供基于色彩圖像和紅外圖像融合的即時定位與建圖系統(tǒng)，通過將環(huán)境的色彩信息和紅外信息融合在一起實現(xiàn)即時的定位與建圖。

本發(fā)明的技術方案是：基于色彩圖像和紅外圖像融合的即時定位與建圖系統(tǒng)，其中，包括定位模塊、建圖模塊、優(yōu)化模塊三大部分，

定位模塊基于已經(jīng)被創(chuàng)建出來的包含紅外特征地圖點和色彩特征地圖點的地圖來進行，并且通過獲取的色彩圖像和紅外圖像來不斷更新已經(jīng)被創(chuàng)建的地圖用于后續(xù)定位；

建圖模塊創(chuàng)建同時包含色彩圖像特征和紅外圖像特征的地圖點，并在定位完成時根據(jù)新的數(shù)據(jù)源不斷的更新現(xiàn)有地圖，以及對地圖點中錯誤的壞點進行剔除；

回環(huán)優(yōu)化模塊通過回環(huán)探測的方式對創(chuàng)建好的全局地圖進行優(yōu)化，在進行回環(huán)探測時同時使用色彩圖像和紅外圖像進行回環(huán)探測。

本發(fā)明與僅能提供色彩圖像的光學相機不同，通過使用一些當前的雙模式相機可以為同一場景提供色彩圖像和熱圖像。色彩和熱圖像中的像素具有直接對應性，并且由相機的硬件來直接實現(xiàn)對應性的校正，因此基于這種圖像像素之間的對應關系，我們可以通過將環(huán)境的色彩信息和紅外信息融合在一起實現(xiàn)即時的定位與建圖。

提出了一個新的單目即時定位與建圖（slam）框架，與大多數(shù)現(xiàn)有的視覺slam方法（嚴重依賴于照明條件）不同，我們提出的框架通過融合色彩信息與紅外信息來實現(xiàn)，因此有著更強的光照魯棒性，可以適用于更具有挑戰(zhàn)意義的場景，如室內(nèi)或室外的低光照、低紋理環(huán)境。

具體的，色彩信息和紅外信息的融合被放在圖像的前端進行，即在色彩圖像和紅外圖像之間尋找圖像像素之間的相關性，并且通過圖像像素之間的相關性完成信息融合后再執(zhí)行之后的定位、建圖、優(yōu)化。

所述的定位模塊中，每次達到新的位置時，首先獲取周圍場景的紅外圖像和色彩圖像，之后通過現(xiàn)有地圖以及當前的紅外圖像和色彩圖像定位出位置；定位成功后，新的地圖點將通過新的紅外圖像和色彩圖像來創(chuàng)建。

所述的建圖模塊中，每當新的圖像傳入時，在定位完成后，通過新的色彩圖像和紅外圖像信息來創(chuàng)建新的地圖點對現(xiàn)有地圖進行更新，同時不斷檢查現(xiàn)有地圖中是否存在壞點并且及時將壞點剔除。

所述的回環(huán)優(yōu)化模塊中，對現(xiàn)有地圖進行優(yōu)化，從而減低系統(tǒng)整體誤差；回環(huán)優(yōu)化時首先使用色彩圖像和紅外圖像進行回環(huán)探測，探測到回環(huán)后再基于找到的回環(huán)來計算系統(tǒng)存在的誤差，最后將誤差分散到回環(huán)上。

還包括初始化模塊，用于實現(xiàn)對系統(tǒng)的初始化，即構建已知的初始化地圖；初始化通過基于圖像序列的緩存來實現(xiàn)，當新的圖像幀傳入時通過在緩存中查找參考幀來嘗試進行成功的初始化或者更新現(xiàn)有緩存。

所述的初始化模塊中，基于緩沖區(qū)的初始化方法，該方法不是僅使用一個先前幀作為參考幀，而是在現(xiàn)有圖像的緩存隊列中查找并嘗試初始化或更新緩存，直到初始化成功。

與現(xiàn)有技術相比，有益效果是：本發(fā)明提出了一種基于緩沖區(qū)的初始化方法。該方法不是僅使用一個先前幀作為參考幀，而是在現(xiàn)有圖像的緩存隊列中查找并嘗試初始化或更新緩存，直到初始化成功。我們提出的初始化算法與當前其他的即時定位與建圖系統(tǒng)初始化算法，相比具有速度和效率上的雙重優(yōu)勢。在特征匹配時，與基于色彩圖像的特征匹配算法相比，我們在特征匹配中將環(huán)境的色彩信息和紅外信息結(jié)合在一起考慮，通過信息的相互精煉從而剔除錯誤的匹配，提高特征匹配的準確度。

附圖說明

圖1表示整套算法的結(jié)構圖。

圖2信息融合示意圖。

圖3初始化示意圖。

圖4初始化圖。

圖5系統(tǒng)運行結(jié)果。

具體實施方式

附圖僅用于示例性說明，不能理解為對本專利的限制；為了更好說明本實施例，附圖某些部件會有省略、放大或縮小，并不代表實際產(chǎn)品的尺寸；對于本領域技術人員來說，附圖中某些公知結(jié)構及其說明可能省略是可以理解的。附圖中描述位置關系僅用于示例性說明，不能理解為對本專利的限制。

光學相機可以獲取周圍環(huán)境的rgb圖像，這些rgb圖像包含了周圍環(huán)境的色彩信息，同時對于同一個場景紅外相機獲取不直接依賴于光照強度的紅外圖像。因此，將兩部分的信息結(jié)合起來將可以提供更加豐富的信息來源，使得實現(xiàn)的定位系統(tǒng)具有更強的適應能力和魯棒性。然而兩種圖像分別來源于不同的傳感器，并且具有不同的成像原理，如何使用兩種來源于不同傳感器并且具有不同成像原理的圖像來實現(xiàn)融合來實現(xiàn)定位是一個極大的問題。

在實現(xiàn)過程過我們將信息的融合放在了圖像的前端，即針對于在同一個場景中同時獲取rgb和紅外圖像，這兩張圖像經(jīng)過相機底層硬件的矯正校正，可使得像素之間存在直接的對應關系。這種設計大大的簡化了系統(tǒng)的復雜性，同時由于信息融合在前端已經(jīng)完成，因此對于后端的定位和優(yōu)化算法可以使用統(tǒng)一的算法框架來實現(xiàn)，因此不僅降低的系統(tǒng)的復雜程度，同時也具備了很高的實時性和可靠性。

在融合前過程中，首先在rgb圖像和紅外圖像中進行特征探測，之后對于探測得到的特征點，我們計算其描述子。最后通過比較特征之間的描述子之間距離來分別得到rgb圖像和紅外圖像的最終匹配結(jié)果。

由于紅外圖像和rgb圖像之間的像素經(jīng)過相機的校正已經(jīng)存在了直接的對應關系，因此可在匹配完成后，將紅外圖像中的特征探測和匹配結(jié)果直接包含到rgb圖像的探測和匹配結(jié)果中。rgb和紅外圖像特征匹配及融合說明參見圖2。

在融合過程中，由于結(jié)合了rgb與紅外信息，在弱光照條件下，rgb圖像黑暗部分中無法匹配和探測到的特征點在紅外圖像中可以被探測和匹配到；而在溫度差異小的情況下，特征的探測和匹配主要在rgb圖像中完成。以此可實現(xiàn)優(yōu)于單純rgb或紅外圖像的特征探測及匹配算法效果的融合定位。包含紅外地圖點、色彩圖像地圖點以及系統(tǒng)運行估計的結(jié)果如圖5。

為了保證整個定位與重建工作的實時性和穩(wěn)定性，將計算過程分為定位、重建、優(yōu)化三大模塊，分別用于實現(xiàn)當前位置定位、稀疏點云重建、累計誤差優(yōu)化三個重要功能，在具體實現(xiàn)過程中，分別對應三個線程，并行執(zhí)行，提高系統(tǒng)的實時性。系統(tǒng)結(jié)構如圖1所示。

其中，定位模塊主要使用獲取到的rgb圖像和紅外圖像對當前位置進行定位。在定位開始前通過執(zhí)行嚴格的初始化操作，初始化通過使用當前圖像幀在之前存在的圖像序列緩存中尋找引用幀來嘗試創(chuàng)建基于rgb特征和紅外特征的初始化全局地圖，初始化流程如圖4。初始化完成后，每次捕獲新的rgb與紅外圖像后，完成如下步驟獲得當前時刻的坐標位置：1）通過預估上一次的運動，提供一個初始狀態(tài)的位姿估計；2）通過初始位姿與全局地圖之間的關系，從全局地圖中提取出一個用于優(yōu)化初始位姿的本地地圖；3）通過當前圖像的像平面特征點(2d)和本地地圖中地圖點(3d)之間的3d-2d匹配關系，結(jié)合bundleadjustment來優(yōu)化位姿。位姿估計如圖3。

建圖模塊主要用于實現(xiàn)對周圍環(huán)境的稀疏點云重建，并通過新定位到的地圖點更新全局地圖。該模塊主要包含新點創(chuàng)建和壞點刪除兩部分功能。在定位模塊定位出當前的位姿后新點創(chuàng)建時，我們對當前幀進行檢查，如果當前幀和之前幀之間滿足距離和視差的條件，當前幀將會被創(chuàng)建為關鍵幀，并且其中的特征點需要對匹配到的特征點會被創(chuàng)建為新的地圖點進行檢查，判斷是否應該被創(chuàng)建為地圖點。新創(chuàng)建的關鍵幀和地圖點將被包含到全局地圖中，用以更新地圖。

同時如果特征點被新建為地圖點，將用于更新當前已經(jīng)存在的地圖，并用于之后的定位操作。為了保持地圖的準確性和有效性，對每一個新建的地圖點，會實時的檢測其有效性來判斷其是否為壞點。如果一個新建的地圖點在后續(xù)幀中無法被連續(xù)的多次觀測到，那么它將被判斷為壞點，并且從地圖中剔除。

優(yōu)化模塊主要實現(xiàn)對回環(huán)的探測，以及累計誤差在回環(huán)上的分散。累計誤差是影響實時定位與重建精度的一個很重要的原因。隨著里程數(shù)目的增加，定位過程中的誤差會不斷的累計和放大。當獲得新的圖像幀時，通過圖像特征詞典來計算當前幀與之前幀之間的相似度，并通過相似度來判斷當前幀與之前幀之間是否存在回環(huán)。如果找到回環(huán)，即執(zhí)行基于回環(huán)的優(yōu)化過程。在優(yōu)化的過程中，當前幀和回環(huán)幀之間首先會計算相似轉(zhuǎn)換，根據(jù)轉(zhuǎn)換關系，求得累計誤差，并將此誤差分散到整個回環(huán)上。由于優(yōu)化會動態(tài)調(diào)整之前定位的位姿，因此之前地圖中創(chuàng)建的地圖點和關鍵幀信息都需要做重新的調(diào)整，因此整個優(yōu)化信息都將用于更新地圖，同時對整個地圖的信息進行更新同步。

如圖5中，a表示紅外地圖點，b表示色彩圖像地圖點，c表示運動軌跡。

顯然，本發(fā)明的上述實施例僅僅是為清楚地說明本發(fā)明所作的舉例，而并非是對本發(fā)明的實施方式的限定。對于所屬領域的普通技術人員來說，在上述說明的基礎上還可以做出其它不同形式的變化或變動。這里無需也無法對所有的實施方式予以窮舉。凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進等，均應包含在本發(fā)明權利要求的保護范圍之內(nèi)。