本發(fā)明涉及一種同步定位與地圖創(chuàng)建(SLAM，simultaneouslocalizationandmapping)方法，尤其涉及一種能增強(qiáng)多環(huán)境下攝像跟蹤性能的基于視覺測量的實(shí)時(shí)SLAM方法。
背景技術(shù)：
：隨著技術(shù)的進(jìn)步和硬件價(jià)格的下降，密度方法越來越普及，密集繪圖和SLAM的應(yīng)用越來越廣泛。傳統(tǒng)的Kinec融合算法，是最早的系統(tǒng)之一，重建了實(shí)時(shí)場景，有了很高的準(zhǔn)確性。雖然體積表示對于計(jì)劃任務(wù)量是一個(gè)有用的指標(biāo)，但是這個(gè)算法還是有很多的局限性。而且這個(gè)算法的一個(gè)顯著特征就是僅僅在攝像動(dòng)作跟蹤方面就利用了深層次的信息。這種評級算法，在以下場景容易出現(xiàn)錯(cuò)誤：照相機(jī)偏移較大或者缺乏3D深層次信息。比如，在朝向一面墻或者在沒有3D特征的情況下，攝像跟蹤的性能會(huì)比較差。技術(shù)實(shí)現(xiàn)要素：本發(fā)明的目的在于針對上述技術(shù)存在的缺陷，提出了一種能夠增強(qiáng)多環(huán)境下攝像跟蹤性能的基于視覺測量的實(shí)時(shí)SLAM方法，該方法在現(xiàn)有的小型GPU基礎(chǔ)上，結(jié)合ICP和FOVIS實(shí)現(xiàn)RGB-D視覺測量方法，通過采用改進(jìn)型的RGB-D攝像機(jī)位置追蹤系統(tǒng)，產(chǎn)生高質(zhì)量的密集的彩色地圖，進(jìn)而生成高質(zhì)量的彩色表面模型，并具有魯棒性；本發(fā)明能夠在不使用關(guān)鍵幀的情況下，完成實(shí)時(shí)的彩色表面模型重建；同時(shí)，能夠提升處理的速度，可以在具有挑戰(zhàn)性的環(huán)境下進(jìn)行跟蹤，而且能夠低延遲地實(shí)時(shí)執(zhí)行，尤其在室內(nèi)條件下，效果很好。本發(fā)明是通過如下技術(shù)方案實(shí)現(xiàn)的，一種基于視覺測量的實(shí)時(shí)SLAM方法，其特征在于：該方法包括以下步驟：步驟1：實(shí)現(xiàn)CUDA：CUDA為一種并行計(jì)算相機(jī)變換框架；步驟1.1：圖像預(yù)處理：在GPU上，對給定的RGB圖和深度圖的序列，進(jìn)行預(yù)處理，獲得連續(xù)的RGB深度圖對；步驟1.2：圖像的預(yù)計(jì)算：在現(xiàn)有的開源的OpenCVCPU應(yīng)用的基礎(chǔ)上，只選擇灰度圖中具有最小梯度的對應(yīng)點(diǎn)集進(jìn)行優(yōu)化；步驟1.3：迭代的轉(zhuǎn)化估計(jì)：首先采用步驟1.2中預(yù)計(jì)算所得出的有最小梯度的對應(yīng)點(diǎn)集來迭代填充有效的對應(yīng)點(diǎn)集，所述有效的對應(yīng)點(diǎn)集初始為空，逐漸增加；然后求解增量變換和連接這些變換的線性系統(tǒng)，這兩個(gè)步驟循環(huán)進(jìn)行，通過迭代和逐步求解，得到優(yōu)化后的轉(zhuǎn)化估計(jì)；步驟2：視覺測量估計(jì)：整合了FOVIS視覺測量系統(tǒng)、RGB-D和ICP系統(tǒng)視覺測量方法；步驟2.1：FOVIS系統(tǒng)：對于滿足一定條件的轉(zhuǎn)化估計(jì)，使用FOVIS和基于錯(cuò)誤測量的估計(jì)量之間進(jìn)行動(dòng)態(tài)切換的方式，即FOVIS切換；對于不支持平移的組件，選擇FOVIS估計(jì)量的方式；步驟2.2：RGB-D和ICP：為了在運(yùn)動(dòng)估計(jì)中，整合顏色信息和深度信息，需要尋找一個(gè)能最小化RGB-D和ICP代價(jià)和的運(yùn)動(dòng)參數(shù)；步驟3：顏色信息融合：在GPU內(nèi)存中，使用一種和TSDF維度相同的獨(dú)立的顏色域，來表示從第1幀到第n幀的顏色信息，表示為Cn(p)。其中p代表3D坐標(biāo)，n代表幀數(shù)；由于存在表面信息域和顏色信息域的一一映射，我們可以根據(jù)原始的KinectFusion方法在表面點(diǎn)p處進(jìn)行光線投影，從而找到p處的顏色信息；步驟4：偽顏色消除：邊界的著色通常是不準(zhǔn)確的，同時(shí)邊界的深度也是不連續(xù)的。邊緣的著色問題可以通過兩種策略來解決：一是，不斷更新深度不連續(xù)的表面；二是，利用相機(jī)角度來加權(quán)顏色更新，完成實(shí)時(shí)彩色表面模型的重建。所述的步驟1.1中RGB深度圖對的計(jì)算過程為：針對每一副深度圖，首先把原始的傳感器值轉(zhuǎn)換為度量深度圖，然后定義一個(gè)灰度圖，構(gòu)建一個(gè)高斯內(nèi)核來進(jìn)行降采樣，得到一個(gè)四層的灰度和深度金字塔，最后，計(jì)算給定的RGB圖和深度圖對的偏導(dǎo)數(shù)。所述的步驟2.1中FOVIS切換的條件為：假設(shè)一個(gè)估計(jì)量有一個(gè)增量的轉(zhuǎn)化矩陣T，其中TF是FOVIS相機(jī)轉(zhuǎn)換矩陣，TO是估計(jì)的相機(jī)轉(zhuǎn)換矩陣，如果|||tF||2-||tO||2|＞μ，就使用TF作為估計(jì)的增量轉(zhuǎn)化，否則使用TO，然后使用該轉(zhuǎn)化來計(jì)算相機(jī)位置。本發(fā)明有益效果是：1、本發(fā)明提供的基于實(shí)時(shí)測量的SLAM方法是在現(xiàn)有的小型GPU基礎(chǔ)上，結(jié)合ICP和FOVIS實(shí)現(xiàn)RGB-D視覺測量方法，該測量方法采用改進(jìn)型的RGB-D攝像機(jī)位置追蹤系統(tǒng)，產(chǎn)生高質(zhì)量的密集的彩色地圖，進(jìn)而生成高質(zhì)量的彩色表面模型，并具有魯棒性。2、本發(fā)明提出了對現(xiàn)有RGB-D的視覺測量算法的GPU實(shí)現(xiàn)，能夠提升處理的速度，可以在具有挑戰(zhàn)性的環(huán)境下進(jìn)行跟蹤，而且能夠低延遲地實(shí)時(shí)執(zhí)行，尤其在室內(nèi)條件下，效果很好。3、本發(fā)明不同于KinectFusion算法，使用了多種深度信息來進(jìn)行相機(jī)運(yùn)動(dòng)跟蹤。并且通過在KinectFusion算法的基礎(chǔ)上，加入相機(jī)位置估計(jì)，克服了傳統(tǒng)視覺估計(jì)算法在幀之間的相機(jī)位移很大或是在不能捕捉到3D深度特征(遇到墻面)的情況下容易出現(xiàn)錯(cuò)誤的問題。附圖說明圖1是本發(fā)明所述方法的流程圖。具體實(shí)施方式下面結(jié)合附圖對本發(fā)明做進(jìn)一步說明。一種基于視覺測量的實(shí)時(shí)SLAM方法，該方法包括以下步驟：如附圖1所示，步驟1：實(shí)現(xiàn)CUDA：CUDA為一種求解相機(jī)變換的并行計(jì)算框架，CUDA需要對給定的兩個(gè)連續(xù)的RGB深度圖對和計(jì)算能夠最大化這兩個(gè)對間一致性的相機(jī)變換，本發(fā)明選擇一種四層金字塔的模式來迭代求解相機(jī)變換。步驟1.1：圖像預(yù)處理：在GPU上，對給定的RGB圖和深度圖的序列，進(jìn)行預(yù)處理，獲得連續(xù)的RGB深度圖對；RGB深度圖對的計(jì)算過程為：針對每一幅深度圖，首先把原始的傳感器值轉(zhuǎn)換為度量深度圖M，因?yàn)镃UDA應(yīng)用中，只使用了彩色圖像的灰度值，然后定義了灰度圖I，構(gòu)建一個(gè)5*5的高斯內(nèi)核來進(jìn)行降采樣，得到一個(gè)四層的灰度和深度金字塔；一個(gè)GPU線程處理一個(gè)像素；最后，對于RGB圖和深度圖對[In+1,Mn+1]計(jì)算偏導(dǎo)數(shù)和步驟1.2：圖像的預(yù)計(jì)算：在現(xiàn)有的開源的OpenCVCPU應(yīng)用的基礎(chǔ)上，只選擇灰度圖中具有最小梯度的對應(yīng)點(diǎn)集進(jìn)行優(yōu)化；在OpenCV版本中，這種選擇用一種二值模板來實(shí)現(xiàn)；本發(fā)明實(shí)現(xiàn)了這種優(yōu)化算法，而且在模板上面加上了有意義的點(diǎn)。步驟1.3：迭代的轉(zhuǎn)化估計(jì)：首先用步驟1.2中預(yù)計(jì)算所得出的有最小梯度的對應(yīng)點(diǎn)集來迭代填充有效的對應(yīng)點(diǎn)集，該有效的對應(yīng)點(diǎn)集初始為空，逐漸增加；然后求解增量變換和連接這些變換的線性系統(tǒng)，這兩個(gè)步驟循環(huán)進(jìn)行；對于幀n而言，設(shè)相機(jī)轉(zhuǎn)化矩陣Tn為Tn=Rntn0001,]]>其中Rn代表一個(gè)3*3的旋轉(zhuǎn)矩陣，tn代表一個(gè)3*1的平移向量；在進(jìn)行迭代之前，計(jì)算Tn到圖像上的投影，和利用求得的投影關(guān)系，可以進(jìn)行對應(yīng)點(diǎn)的填充，進(jìn)而求得優(yōu)化的相機(jī)轉(zhuǎn)化估計(jì)其中其中[ω]×是斜對稱矩陣，[ω]×代表一個(gè)3*3的旋轉(zhuǎn)矩陣，u代表一個(gè)3*1的平移向量；通過迭代和逐步求解，可以得到優(yōu)化后的轉(zhuǎn)化估計(jì)。步驟2：測量估計(jì)：本發(fā)明整合了兩種系統(tǒng)：(i)基于ICP和RGB-D位置估計(jì)系統(tǒng)的FOVIS視覺測量系統(tǒng)和(ii)綜合了KinectFusion的原始的ICP估計(jì)算子和Steinbruecker的RGB-D的視覺測量系統(tǒng)。步驟2.1：FOVIS系統(tǒng)：本發(fā)明選擇了一種策略，能夠使我們能夠根據(jù)某些錯(cuò)誤測量，在FOVIS和別的估計(jì)量之間進(jìn)行動(dòng)態(tài)切換?？紤]到缺乏3D特征，ICP估計(jì)量會(huì)在走廊和其他平面區(qū)域進(jìn)行滑動(dòng)，因此，本發(fā)明選擇了一種保守的切換策略：如果估計(jì)不出平移，就選擇FOVIS估計(jì)量，因?yàn)镕OVIS估計(jì)量很少會(huì)像ICP估計(jì)量一樣進(jìn)行平移。FOVIS切換條件為：假設(shè)可以通過一個(gè)估計(jì)量(例如ICP)，來計(jì)算增量的轉(zhuǎn)化矩陣T，其中TF是FOVIS相機(jī)轉(zhuǎn)換矩陣，TO是估計(jì)的相機(jī)轉(zhuǎn)換矩陣，如果|||tF||2-||tO||2|＞μ，就使用TF作為估計(jì)的增量轉(zhuǎn)化，否則使用TO；經(jīng)驗(yàn)上，μ＝0.03m的設(shè)定能夠獲得很好的實(shí)驗(yàn)效果；計(jì)算完相機(jī)轉(zhuǎn)換矩陣之后，就可以利用該相機(jī)轉(zhuǎn)換矩陣來計(jì)算相機(jī)位置。某些情況下，只依賴FOVIS相機(jī)轉(zhuǎn)換矩陣就可以進(jìn)行深度重建；但是對于幾何特征豐富的場景，只使用FOVIS相機(jī)轉(zhuǎn)化矩陣而不使用ICP估計(jì)量是不明智的；然而，過多地依賴ICP估計(jì)量會(huì)增加突發(fā)錯(cuò)誤發(fā)生的可能性。步驟2.2：RGB-D和ICP測量系統(tǒng)：為了在運(yùn)動(dòng)估計(jì)中，整合顏色信息和深度信息，需要尋找一個(gè)能最小化RGB-D和ICP代價(jià)和的運(yùn)動(dòng)參數(shù)E，其中，RGB-D和ICP的代價(jià)和定義為E＝Eicp+ωErgbd，其中ω代表一個(gè)權(quán)重，經(jīng)驗(yàn)上把這個(gè)權(quán)重設(shè)為0.1來反映ICP和RGB-D代價(jià)的差異。其中ICP的代價(jià)是當(dāng)前點(diǎn)和模型中的對應(yīng)點(diǎn)的距離其中代表第n幀中的第k個(gè)頂點(diǎn)，vk，nk是模型中對應(yīng)的頂點(diǎn)和法線，而T代表從當(dāng)前幀到模型幀的轉(zhuǎn)換估計(jì)。為了簡化概念，我們對上述概念進(jìn)行簡化處理并進(jìn)行了線性化，公式如下：而顏色代價(jià)跟Ergbd的形式是類似的，可以獲得Jrgbd和rrgbd。通過求解正則方法，可以求得這個(gè)最小二乘法問題。其中而和可以通過樹型結(jié)構(gòu)簡化算法在GPU上求解。步驟3：顏色信息融合：在GPU內(nèi)存中，使用一種和TSDF維度相同的獨(dú)立的顏色域，來表示從第1幀到第n幀的顏色信息，表示為Cn(p)。其中p代表3D坐標(biāo)，n代表幀數(shù)；由于存在表面信息域和顏色信息域的一一映射，我們可以根據(jù)原始的KinectFusion方法在表面點(diǎn)p處進(jìn)行光線投影，從而找到p處的顏色信息；步驟4：偽顏色消除：邊界的著色通常是不準(zhǔn)確的，同時(shí)邊界的深度也是不連續(xù)的。邊緣的著色問題可以通過兩種策略來解決：一是，不斷更新深度不連續(xù)的表面；二是，利用相機(jī)角度來加權(quán)顏色更新，完成實(shí)時(shí)彩色表面模型的重建。在判定邊界上的點(diǎn)的過程中，如果一些鄰近點(diǎn)超過給定的距離，則判定為邊界點(diǎn)：假設(shè)有7x7的鄰域，在處理每一幀的過程中，會(huì)計(jì)算深度圖的每個(gè)點(diǎn)的法向量，然后利用計(jì)算得到的法向量角度來加權(quán)顏色更新，這種方法好于在同一個(gè)角度進(jìn)行加權(quán)。表1-3是本發(fā)明的實(shí)驗(yàn)結(jié)果，列出了本發(fā)明所述方法跟別的方法的對比結(jié)果。表1實(shí)測數(shù)據(jù)平移的均方根誤差m/s表1列出了使用均方根誤差(RMSE)的結(jié)果，其中均方根誤差是一種更有利的度量接近動(dòng)作捕捉系統(tǒng)的噪聲誤差的方法。表中列舉了各種算法多組的實(shí)驗(yàn)結(jié)果，從表中數(shù)據(jù)可知，幀之間的誤差大小接近于動(dòng)作捕捉的噪聲水平。表2每種算法在FR1/ROOM數(shù)據(jù)集的計(jì)算性能表2列出了每種算法在FR1/ROOM數(shù)據(jù)集上的計(jì)算性能。其中，測試平臺(tái)使用了IntelCorei7-3960XCPU，主頻是3.30GHz，RAM是16GB，使用的是Ubuntu12.04。結(jié)果表明，各種算法都有能力處理30Hz的幀率，并且最大執(zhí)行時(shí)間都在33ms以下。表3各種數(shù)據(jù)集每種算法的實(shí)測數(shù)據(jù)的絕對軌跡跟蹤誤差表3列出各種數(shù)據(jù)集上的每種算法的絕對軌跡跟蹤錯(cuò)誤，最好的結(jié)果用黑體+下劃線標(biāo)出。結(jié)果表明，對于平均平移誤差而言，在ICP+RGB-D數(shù)據(jù)集上排名第一，在FOVIS/ICP+RGB-D數(shù)據(jù)集上排名第二。對于最大平移誤差而言，在ICP+RGB-D或是FOVIS/ICP+RGB-D上排名第二，而在FOVIS或是FOVIS/RGB-D上總是排名第一。以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)的所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。當(dāng)前第1頁1 2 3