本公開(kāi)屬于機(jī)器視覺(jué)領(lǐng)域，尤其是涉及一種同步定位和建圖方法。
背景技術(shù)：
：現(xiàn)階段機(jī)器人和/或自動(dòng)駕駛交通工具的移動(dòng)大多依靠人工路徑規(guī)劃，機(jī)器人和/或自動(dòng)駕駛交通工具的自主導(dǎo)航能力依賴(lài)于同步定位與建圖技術(shù)(SimultaneousLocalizationandMapping，以下簡(jiǎn)稱(chēng)為SLAM)。其旨在解決當(dāng)機(jī)器人和/或自動(dòng)駕駛交通工具進(jìn)入未知工作環(huán)境時(shí)，利用傳感器信息，對(duì)周?chē)h(huán)境高效率且準(zhǔn)確地進(jìn)行構(gòu)建(Mapping)，同時(shí)得到設(shè)備在空間中的位置與姿態(tài)(Localization)。除了可以應(yīng)用在機(jī)器人領(lǐng)域外，虛擬現(xiàn)實(shí)與增強(qiáng)現(xiàn)實(shí)設(shè)備的空間追蹤以及自動(dòng)駕駛也同樣可以使用SLAM技術(shù)。SLAM問(wèn)題自提出以來(lái)已經(jīng)過(guò)去了近30年，這期間SLAM問(wèn)題所用的傳感器與計(jì)算方法都發(fā)生了巨大的變化。現(xiàn)在的SLAM技術(shù)大多采用視覺(jué)為主的傳感器，包括單目傳感器，多目傳感器，以及彩色圖像及深度信息(RGB-D)的傳感器等?，F(xiàn)階段主流的基于視覺(jué)的SLAM方案可根據(jù)優(yōu)化方法分為兩類(lèi)，一類(lèi)為使用濾波器的SLAM方法，另一類(lèi)的是使用圖優(yōu)化的SLAM方法。使用濾波器的SLAM方案模型構(gòu)建比較簡(jiǎn)單，但誤差會(huì)逐漸累計(jì)不可修復(fù)。在這類(lèi)方法中，Davison等人在文獻(xiàn)“A.Davison,I.Reid,andN.Molton.MonoSLAM:Real-TimeSingleCameraSLAM.IEEETransactionsonPatternAnalysisandMachineIntelligence,pp.1052-1067,2007.”中提出的MonoSLAM方案是基于擴(kuò)展卡爾曼濾波器(簡(jiǎn)稱(chēng)EKF)的單目傳感器SLAM方案，通過(guò)在圖像中構(gòu)建路標(biāo)點(diǎn)并使用EKF對(duì)SLAM進(jìn)行模型構(gòu)建而求解。是基于濾波器的SLAM中比較優(yōu)秀的方案。使用圖優(yōu)化方法的SLAM方案由于要構(gòu)建姿態(tài)圖往往運(yùn)算量比較大。在這類(lèi)方法中，Artal等人在文獻(xiàn)“R.Artal,J.Montiel,andJ.Tardos.ORB-SLAM:AVersatileandAccurateMonocularSLAMSystem.WorkshoponResearch,EducationandDevelopmentofUnmannedAerialSystems,pp241-248,2015.”中提出的ORB-SLAM是目前基于圖優(yōu)化方法的單目視覺(jué)SLAM最優(yōu)方案。ORB-SLAM通過(guò)對(duì)圖像提取ORB特征描述符(一種提取速度極快的特征描述符)，可以達(dá)到很高的速度。并且通過(guò)完善的圖優(yōu)化操作，得到的地圖也具有很高的精確度。以上兩種方法是目前視覺(jué)SLAM采用濾波器或圖優(yōu)化方法中比較有代表性的方案，也是實(shí)際使用中比較主流的方案。但現(xiàn)有方法仍有一定不足。首先單目SLAM方案難以估計(jì)現(xiàn)實(shí)世界的尺度，往往難以構(gòu)建出準(zhǔn)確規(guī)模的地圖；其次SLAM方案生成的一般是三維點(diǎn)云地圖，由空間中的散點(diǎn)構(gòu)成，在空間上不具備連續(xù)性，無(wú)法用于導(dǎo)航；另外基于圖優(yōu)化方案的SLAM會(huì)將每幀圖像加入姿態(tài)圖進(jìn)行優(yōu)化，會(huì)導(dǎo)致姿態(tài)圖過(guò)大優(yōu)化速度慢。技術(shù)實(shí)現(xiàn)要素：本公開(kāi)的實(shí)施例關(guān)于一種同步定位與建圖方法，包括基于幀間運(yùn)動(dòng)的大小從多個(gè)圖像幀中選出關(guān)鍵幀序列，基于關(guān)鍵幀序列的幀間匹配結(jié)果將多個(gè)圖像幀導(dǎo)入姿態(tài)圖，基于姿態(tài)圖構(gòu)建一個(gè)或多個(gè)有效檢測(cè)回環(huán)以用于進(jìn)行捆集調(diào)整以及基于所捆集調(diào)整的姿態(tài)圖修正多個(gè)圖像幀。在一些實(shí)施例中，多個(gè)圖像幀包括彩色圖像和深度圖像兩者。在一些實(shí)施例中，將修正的多個(gè)圖像幀變換為三維點(diǎn)云并拼接為點(diǎn)云地圖。在一些實(shí)施例中，基于八叉樹(shù)建圖將點(diǎn)云地圖轉(zhuǎn)化為三維柵格地圖。在一些實(shí)施例中，關(guān)鍵幀序列包括多個(gè)圖像幀的第一幀。在一些實(shí)施例中，基于幀間運(yùn)動(dòng)的大小選出關(guān)鍵幀序列包括當(dāng)圖像幀與關(guān)鍵幀序列中的最后一幀之間幀間運(yùn)動(dòng)的值在第一幀間運(yùn)動(dòng)閾值以上且在第二幀間運(yùn)動(dòng)閾值以下時(shí)，將該圖像幀添加入關(guān)鍵幀序列作為新的最后一幀。在一些實(shí)施例中，當(dāng)圖像幀與關(guān)鍵幀序列中的最后一幀之間幀間運(yùn)動(dòng)的值小于第一幀間運(yùn)動(dòng)閾值或大于第二幀間運(yùn)動(dòng)閾值時(shí)，丟棄該圖像幀并導(dǎo)入下一圖像幀。在一些實(shí)施例中，姿態(tài)圖包括代表相機(jī)姿態(tài)的狀態(tài)變量的節(jié)點(diǎn)和代表不同姿態(tài)運(yùn)動(dòng)變換關(guān)系的連接節(jié)點(diǎn)的邊。在一些實(shí)施例中，姿態(tài)圖基于每一圖像幀與其前后幀之間的運(yùn)動(dòng)變換關(guān)系而形成為線狀。在一些實(shí)施例中，構(gòu)建檢測(cè)回環(huán)包括在具有至少一個(gè)可匹配特征點(diǎn)的非相鄰節(jié)點(diǎn)之間建立運(yùn)動(dòng)變換關(guān)系。本公開(kāi)的實(shí)施例引入深度圖像準(zhǔn)確估計(jì)環(huán)境的尺度，通過(guò)八叉樹(shù)建圖建立三維彩色柵格地圖用于導(dǎo)航，并且提取關(guān)鍵幀加入姿態(tài)圖，有助于壓縮圖優(yōu)化規(guī)模從而提高優(yōu)化速度和/或效率。該方法相比于同類(lèi)算法有20％-50％的效率提升，通過(guò)引入深度圖像信息獲得了比較準(zhǔn)確的現(xiàn)實(shí)環(huán)境地圖尺度，以及使用八叉樹(shù)建圖得到的彩色三維柵格地圖可以直接用于導(dǎo)航。附圖說(shuō)明本公開(kāi)提供了附圖以便于所公開(kāi)內(nèi)容的進(jìn)一步理解，附圖構(gòu)成本申請(qǐng)的一部分，但僅僅是用于圖示出體現(xiàn)發(fā)明概念的一些發(fā)明的非限制性示例，而不是用于做出任何限制。圖1是根據(jù)本公開(kāi)一些實(shí)施例的同步定位與建圖方法的流程圖。圖2分別是根據(jù)本公開(kāi)一些實(shí)施例的同步定位與建圖方法所建立的地圖在優(yōu)化之前與之后的三維點(diǎn)云地圖。圖3是根據(jù)本公開(kāi)一些實(shí)施例建立柵格地圖的示意圖。圖4是根據(jù)本公開(kāi)一些實(shí)施例在姿態(tài)圖中構(gòu)建檢測(cè)回環(huán)的示意圖。具體實(shí)施方式下文將使用本領(lǐng)域技術(shù)人員向本領(lǐng)域的其它技術(shù)人員傳達(dá)他們工作的實(shí)質(zhì)所通常使用的術(shù)語(yǔ)來(lái)描述本公開(kāi)的發(fā)明概念。然而，這些發(fā)明概念可體現(xiàn)為許多不同的形式，因而不應(yīng)視為限于本文中所述的實(shí)施例。提供這些實(shí)施例是為了使本公開(kāi)內(nèi)容更詳盡和完整，并且向本領(lǐng)域的技術(shù)人員完整傳達(dá)其包括的范圍。也應(yīng)注意這些實(shí)施例不相互排斥。來(lái)自一個(gè)實(shí)施例的組件、步驟或元素可假設(shè)成在另一實(shí)施例中可存在或使用。在不脫離本公開(kāi)的實(shí)施例的范圍的情況下，可以用多種多樣的備選和/或等同實(shí)現(xiàn)方式替代所示出和描述的特定實(shí)施例。本申請(qǐng)旨在覆蓋本文論述的實(shí)施例的任何修改或變型。對(duì)于本領(lǐng)域的技術(shù)人員而言明顯可以?xún)H使用所描述的方面中的一些方面來(lái)實(shí)踐備選實(shí)施例。本文出于說(shuō)明的目的，在實(shí)施例中描述了特定的數(shù)字、材料和配置，然而，領(lǐng)域的技術(shù)人員在沒(méi)有這些特定細(xì)節(jié)的情況下，也可以實(shí)踐備選的實(shí)施例。在其它情況下，可能省略或簡(jiǎn)化了眾所周知的特征，以便不使說(shuō)明性的實(shí)施例難于理解。此外，下文為有助于理解說(shuō)明性的實(shí)施例，將各種操作依次描述為了多個(gè)離散的操作；然而，所描述的順序不應(yīng)當(dāng)被認(rèn)為是意味著這些操作必須依賴(lài)于該順序執(zhí)行。而是不必以所呈現(xiàn)的順序來(lái)執(zhí)行這些操作。下文中的“在一些實(shí)施例中”，“在一個(gè)實(shí)施例中”等短語(yǔ)可以或可以不指相同的實(shí)施例。術(shù)語(yǔ)“包括”、“具有”和“包含”是同義的，除非上下文中以其它方式規(guī)定。短語(yǔ)“A和/或B”意味著(A)、(B)或(A和B)。短語(yǔ)“A/B”意味著(A)、(B)或(A和B)，類(lèi)似于短語(yǔ)“A和/或B”。短語(yǔ)“A、B和C中的至少一個(gè)”意味著(A)、(B)、(C)、(A和B)、(A和C)、(B和C)或(A、B和C)。短語(yǔ)“(A)B”意味著(B)或(A和B)，即A是可選的。參考圖1所示的方法流程圖，在一些實(shí)施例中，同步定位和建圖需要先取得一系列的多個(gè)圖像幀作為輸入，如步驟S101所示。該多個(gè)圖像幀包括彩色圖像和深度圖像，尤其是可以包括在時(shí)間上同步采集的彩色與深度圖像序列。本領(lǐng)域技術(shù)人員將認(rèn)識(shí)到可以使用同一時(shí)刻的彩色圖像與深度圖像生成該時(shí)刻的點(diǎn)云地圖，如可選的步驟S102所示。由于彩色及深度圖都是二維圖像，對(duì)圖像建立二維笛卡爾坐標(biāo)系，則可以用(u,v)來(lái)表示圖像上任意一個(gè)像素點(diǎn)p。對(duì)彩色圖像中的任意一點(diǎn)可以提取出該點(diǎn)對(duì)應(yīng)的顏色信息，即r,g,b顏色分量，也可以對(duì)深度圖像中的該點(diǎn)提取出深度值d。對(duì)于x,y,z分量，利用小孔相機(jī)模型依靠求解u,v,d來(lái)獲得。圖像中的信息u,v,d關(guān)于空間點(diǎn)P的x,y,z的關(guān)系為：S·uvd=C(R·xyz+t)---(1)]]>其中C為相機(jī)的內(nèi)參矩陣，由相機(jī)標(biāo)定得出。相機(jī)可以包括彩色相機(jī)和/或深度相機(jī)。S是縮放矩陣，默認(rèn)為單位陣。R為旋轉(zhuǎn)矩陣，t為位移矩陣。對(duì)于單張圖片來(lái)說(shuō)，不存在視角的旋轉(zhuǎn)和位移，所以在單幀圖像轉(zhuǎn)換時(shí)忽略這兩個(gè)量。由此，可以將二維的圖像信息轉(zhuǎn)化到真實(shí)的三維世界坐標(biāo)系中。對(duì)圖像中所有的點(diǎn)全部進(jìn)行變換，得到上述的點(diǎn)云。在可選的步驟S103、S104和S105中，可以利用可選步驟S102中得到的初步點(diǎn)云，對(duì)所有圖像提取SIFT特征描述符(可替換為其他特征描述符，如ORB等)，該步驟可以使用本領(lǐng)域技術(shù)人員熟知的計(jì)算機(jī)視覺(jué)庫(kù)OPENCV中的SIFT特征描述符提取函數(shù)來(lái)進(jìn)行，本處不作詳述。然后使用近似最近鄰法(簡(jiǎn)稱(chēng)為FLANN)對(duì)每相鄰兩幀圖像進(jìn)行特征匹配，隨后使用隨機(jī)一致性估計(jì)(簡(jiǎn)稱(chēng)為RANSAC)估計(jì)兩幀圖像之間拍攝相機(jī)的姿態(tài)變化，下文中又將兩幀之間相機(jī)的姿態(tài)變化簡(jiǎn)稱(chēng)為“幀間運(yùn)動(dòng)”，相機(jī)的姿態(tài)變化可以包括軌跡變化、俯仰變化、平移變化和/或旋轉(zhuǎn)變化之類(lèi)的任何相機(jī)與參考系相對(duì)姿態(tài)的變化。以上可選步驟利用得到的幀間運(yùn)動(dòng)估計(jì)對(duì)單幅點(diǎn)云進(jìn)行拼接，可以得到初步的點(diǎn)云地圖。該初步的點(diǎn)云地圖是用于與經(jīng)過(guò)下文所述的優(yōu)化步驟后的最終點(diǎn)云地圖進(jìn)行比較，以進(jìn)一步例示本公開(kāi)實(shí)施例方法取得的效果。在步驟S106中，根據(jù)幀間運(yùn)動(dòng)估計(jì)大小來(lái)從一系列的輸入圖像幀中選取合適的關(guān)鍵幀，從而得到一個(gè)關(guān)鍵幀的序列。設(shè)關(guān)鍵幀序列為F，則可先將第一幀f0放入序列，之后導(dǎo)入圖像序列的每一幀圖像i，并計(jì)算每幀i與關(guān)鍵幀序列F中最后一幀(即，最近一幀)的幀間運(yùn)動(dòng)大小T。若T＜Tmin則認(rèn)為圖像i距離前一個(gè)關(guān)鍵幀過(guò)近，放棄并開(kāi)始導(dǎo)入下一幀。若T＞Tmax則認(rèn)為圖像i運(yùn)動(dòng)過(guò)遠(yuǎn)，同樣放棄并開(kāi)始導(dǎo)入下一幀。Tmin與Tmax為所設(shè)定的第一閾值和第二閾值，分別代表關(guān)鍵幀間運(yùn)動(dòng)的最小運(yùn)動(dòng)距離與最大運(yùn)動(dòng)距離。若兩幀圖像無(wú)法匹配，則認(rèn)為圖像i存在問(wèn)題，如模糊和/或大面積遮擋，則同樣拋棄并開(kāi)始導(dǎo)入下一幀。剩余的情況，即幀間運(yùn)動(dòng)大小為第一閾值和第二閾值之間時(shí)，則將圖像i定義為關(guān)鍵幀F(xiàn)i并加入關(guān)鍵幀序列F末尾。由此，最終得到的關(guān)鍵幀序列F內(nèi)將包含所需要的所有關(guān)鍵幀。在步驟S107中，將關(guān)鍵幀的姿態(tài)信息加入姿態(tài)圖。首先構(gòu)建一個(gè)由節(jié)點(diǎn)(vertices)與邊(edge)組成的姿態(tài)圖，節(jié)點(diǎn)代表相機(jī)姿態(tài)的狀態(tài)變量，而連接兩個(gè)節(jié)點(diǎn)的邊代表不同位姿下圖像之間的約束關(guān)系，即運(yùn)動(dòng)變換關(guān)系。在上一步中，通過(guò)計(jì)算可以得到相機(jī)在幀間匹配后得到的運(yùn)動(dòng)估計(jì)旋轉(zhuǎn)矩陣R與位移矩陣t。若將相機(jī)的姿態(tài)表示為P，也就是姿態(tài)圖中的節(jié)點(diǎn)，將姿態(tài)之間的變換關(guān)系表示為T(mén)，也就是姿態(tài)圖中的邊，那么有：Pi=R3×3t3×1O1×31iTi,j=R3×3t3×1O1×31i,j---(2)]]>同時(shí)設(shè)相機(jī)第一幀圖像姿態(tài)為P0，且為4×4大小的單位矩陣。則隨著圖像序列的導(dǎo)入有：Pj＝Ti,jPi(3)即第i幀圖像利用與下一幀圖像的幀間匹配結(jié)果T，將下一幀也就是第j幀關(guān)鍵幀的節(jié)點(diǎn)信息(姿態(tài))加入姿態(tài)圖，同時(shí)T也作為兩個(gè)節(jié)點(diǎn)之間的約束條件，如圖4中T1,2和/或T2,3所示。圖中的Pi代表姿態(tài)節(jié)點(diǎn)，Ti,j代表不同姿態(tài)之間的運(yùn)動(dòng)變換約束，如P1代表第一幀圖像的攝像機(jī)位姿，T1,2代表第一幀圖像與第二幀圖像之間的運(yùn)動(dòng)關(guān)系約束。當(dāng)全部圖像導(dǎo)入姿態(tài)圖后，即可獲得每幀與其前后幀相關(guān)的姿態(tài)圖，該姿態(tài)圖可理解為線狀的，如上述可以完成姿態(tài)圖的初步構(gòu)建。相機(jī)運(yùn)動(dòng)過(guò)程中，如果有某些場(chǎng)景的圖像與之前某時(shí)刻的圖像匹配結(jié)果較為接近，則在某種程度上可以認(rèn)為相機(jī)回到了之前來(lái)過(guò)的某個(gè)位置。這種情況下通過(guò)使用這組相似場(chǎng)景之間的運(yùn)動(dòng)變換得到對(duì)應(yīng)相機(jī)姿態(tài)之間的運(yùn)動(dòng)變換，可以利用該運(yùn)動(dòng)變換對(duì)之前所估計(jì)的相機(jī)姿態(tài)進(jìn)行修正，使修正后的各姿態(tài)軌跡更接近真實(shí)值。在本公開(kāi)的實(shí)施例中，利用這種較為匹配的非相鄰圖像幀之間的運(yùn)動(dòng)變換建立運(yùn)動(dòng)變換關(guān)系T以用于結(jié)果的優(yōu)化，在姿態(tài)圖中這表現(xiàn)為線狀姿態(tài)圖中原非相鄰節(jié)點(diǎn)之間回環(huán)狀結(jié)構(gòu)的建立，建立回環(huán)結(jié)構(gòu)的非相鄰節(jié)點(diǎn)雖然在時(shí)間上不連續(xù)，但相互具有相匹配的至少一個(gè)特征點(diǎn)。如圖4中所示，P為不同幀對(duì)應(yīng)的姿態(tài)，T為不同姿態(tài)之間的運(yùn)動(dòng)變換，圖中姿態(tài)P1，P2，P3在沒(méi)有加入回環(huán)檢測(cè)T1,3時(shí)，之間的約束只有T1,2和T1,3，即如上文所述的線狀?，F(xiàn)假設(shè)非相鄰圖像幀的姿態(tài)P1與姿態(tài)P3中有一定數(shù)目可以匹配的特征點(diǎn)，則可以新添加一條運(yùn)動(dòng)變換約束T1,3，即構(gòu)成了檢測(cè)回環(huán)。類(lèi)似地，可以如圖4所示建立任意數(shù)量的檢測(cè)回環(huán)以用于后續(xù)的優(yōu)化步驟。如步驟S108所示，對(duì)姿態(tài)圖進(jìn)行捆集調(diào)整，得到優(yōu)化后的姿態(tài)圖。在根據(jù)應(yīng)用要求已構(gòu)建足夠數(shù)量的檢測(cè)回環(huán)之后，可以對(duì)姿態(tài)圖進(jìn)行全局的姿態(tài)優(yōu)化，對(duì)于全局姿態(tài)的優(yōu)化被稱(chēng)為捆集調(diào)整(Bundleadjustment)。可以利用非線性最小二乘法來(lái)實(shí)現(xiàn)全局誤差的最小。這個(gè)過(guò)程使用Levenberg-Marquardt(LM)法來(lái)實(shí)現(xiàn)迭代策略。由于邊中存在誤差，使得所有邊得到的數(shù)據(jù)并不一致，于是優(yōu)化的是整個(gè)過(guò)程中的不一致性誤差：minE=Σi,j||xi*-Ti,jxj*||22---(4)]]>x(t+1)*=x(t)*-η*▿xE---(5)]]>這里代表對(duì)優(yōu)化變量xi的估計(jì)值，若上式收斂，就可以在優(yōu)化結(jié)束后獲得最小的誤差E，即最接近真實(shí)值的姿態(tài)圖。在本領(lǐng)域中存在多種用于捆集調(diào)整的方法，其中一種通過(guò)使用Kümmerle等人在文“R.Kümmerle,G.Grisetti,H.Strasdat,K.KonoligeandW.Burgard,“G2o:Ageneralframeworkforgraphoptimization,”IEEEInternationalConferenceonRoboticsandAutomation,pp.3607-3613,2011.”中提出的圖優(yōu)化工具g2o(GeneralFrameworkforGraphOptimization)來(lái)進(jìn)行，此處不再贅述。在步驟S109中，利用捆集調(diào)整后的姿態(tài)圖對(duì)彩色/深度圖像幀，即點(diǎn)云進(jìn)行重新拼接，以得到最終準(zhǔn)確的點(diǎn)云地圖。設(shè)每一幀圖像生成的點(diǎn)云為pcli。通過(guò)下式可以利用較準(zhǔn)確的相機(jī)姿態(tài)Pi將所有幀的點(diǎn)云合并到同一個(gè)坐標(biāo)系中，得到最終環(huán)境的點(diǎn)云圖像pclmap。pcloi＝Pi×pcli(6)pclmap=Σi=0Npcloi---(7)]]>可選地，在步驟S110-S111中，使用八叉樹(shù)建圖工具將點(diǎn)云地圖轉(zhuǎn)化為可用于導(dǎo)航的三維柵格地圖。使用本領(lǐng)域中用于對(duì)點(diǎn)云進(jìn)行處理的Octomap工具將地圖轉(zhuǎn)化為三維柵格地圖，同時(shí)根據(jù)真實(shí)世界的色彩對(duì)柵格單位方格進(jìn)行染色。完成轉(zhuǎn)化后，本方法可以得到例如如圖3所示的最終的三維彩色柵格地圖。本公開(kāi)實(shí)施例的同步定位與建圖方法還在中央處理器為i5-45903.4GHzCPU、內(nèi)存為8G、OS為Ubuntu操作系統(tǒng)的運(yùn)行環(huán)境中進(jìn)行了測(cè)試，在測(cè)試中用來(lái)構(gòu)建地圖的彩色與深度測(cè)試序列cafe來(lái)自于NYUv2數(shù)據(jù)集，該數(shù)據(jù)庫(kù)由N.Silberman等人在文獻(xiàn)“N.Silberman,D.Hoiem,andP.Kohli,IndoorSegmentationandSupportInferencefromRGBDImages,EuropeanConferenceonComputerVision,pp746-760,2012”中提出，為750對(duì)RGB彩色圖像與深度圖像，分辨率均為640*480，幀率為30Hz。將完整序列導(dǎo)入本公開(kāi)構(gòu)建的SLAM系統(tǒng)，并根據(jù)包括可選步驟的上述步驟生成初步的點(diǎn)云地圖和優(yōu)化后的點(diǎn)云地圖，之后對(duì)比點(diǎn)云地圖的細(xì)節(jié)準(zhǔn)確程度，生成速度以及穩(wěn)定性。圖2為展示本公開(kāi)優(yōu)化前后的的點(diǎn)云地圖。首先在整體上兩幅地圖規(guī)模近似，都對(duì)序列中的場(chǎng)景進(jìn)行了比較完整的構(gòu)建。但在細(xì)節(jié)部分可以發(fā)現(xiàn)兩幅點(diǎn)云地圖的準(zhǔn)確程度有一定差距。如圖中被框出部分，左圖中視覺(jué)里程計(jì)的部分出現(xiàn)了明顯的重疊與模糊，尤其是在木架部分，明顯出現(xiàn)多條木架的重疊。墻面云點(diǎn)分布較為分散，不能準(zhǔn)確的分布在同一個(gè)平面中，反映到圖片中即為墻面亮度較低，空洞較多。而右圖的本RGBDSLAM方法得到的點(diǎn)云地圖，由于優(yōu)化后的點(diǎn)云圖誤差較小，點(diǎn)云分布均勻平整，沒(méi)有重影出現(xiàn)；并且在多幀圖像的點(diǎn)云疊加后，小圖中頂部黑板上方的菜單文字依然清晰可辨。在速度與穩(wěn)定性上還可與另外一種使用彩色與深度信息源的RGBDSLAMv2進(jìn)行對(duì)比。如以下表1所示，本公開(kāi)的SLAM方案的運(yùn)行速度(幀率)高于RGBDSLAMv2，為5.7fps，對(duì)比的RGBDSLAMv2的幀率僅為3.2fps。同時(shí)本公開(kāi)的SLAM方法可以完整的運(yùn)行整個(gè)圖像序列，而作為對(duì)比的RGBDSLAMv2方法只能運(yùn)行到序列的第400幀就出現(xiàn)錯(cuò)誤不能繼續(xù)執(zhí)行運(yùn)算，證明的本公開(kāi)具有較好的穩(wěn)定性，能夠以較快速度建立準(zhǔn)確的可用于導(dǎo)航的三維柵格地圖。表格1算法運(yùn)行速度比較方法序列幀數(shù)耗時(shí)每幀平均耗時(shí)處理幀率本SLAM方法750132.7s176ms5.7fpsRGBD-SLAMv2400126.0s315ms3.2fps本文中的部分方法步驟和流程可能需要由計(jì)算機(jī)執(zhí)行，從而以硬件、軟件、固件及其任何組合的方式來(lái)實(shí)施，并且可以包括計(jì)算機(jī)可執(zhí)行的指令。該計(jì)算機(jī)可執(zhí)行的指令可以以計(jì)算機(jī)程序產(chǎn)品的形式存儲(chǔ)在機(jī)器可讀介質(zhì)上或者以從遠(yuǎn)程服務(wù)器下載的方式進(jìn)行提供，并由通用計(jì)算機(jī)、專(zhuān)用計(jì)算機(jī)和/或其他可編程數(shù)據(jù)處理裝置的一個(gè)或多個(gè)處理器讀取和執(zhí)行以實(shí)現(xiàn)方法步驟和流程中指明的功能/動(dòng)作。機(jī)器可讀介質(zhì)包括但不限于軟盤(pán)、光盤(pán)、壓縮盤(pán)、磁光盤(pán)、只讀存儲(chǔ)器ROM、隨機(jī)存取存儲(chǔ)器RAM、可擦可編程ROM(EPROM)、電可擦可編程ROM(EEPROM)、存儲(chǔ)卡、閃存和/或電、光、聲以及其他形式的傳播信號(hào)(例如載波、紅外信號(hào)、數(shù)字信號(hào)等)。另外需注意，本文中的術(shù)語(yǔ)“和/或”可表示“和”、“或”、“異或”、“一個(gè)”、“一些但不是全部”、“兩者皆不”和/或“兩者皆是”，但在此方面并無(wú)限制。本文雖然已經(jīng)示出和描述了本公開(kāi)的具體實(shí)施例，但對(duì)本領(lǐng)域技術(shù)人員顯然可以在不脫離所附權(quán)利要求書(shū)范圍的情況下進(jìn)行眾多改變、變化和修改。另外，在上述具體實(shí)施方式中，可看到各種特征在單個(gè)實(shí)施例中組合在一起以便簡(jiǎn)化公開(kāi)內(nèi)容。此公開(kāi)方式不應(yīng)解釋為反映要求保護(hù)的實(shí)施方式需要比每個(gè)權(quán)利要求項(xiàng)明確所述的具有更多特征。相反，如權(quán)利要求所反映的一樣，本公開(kāi)的主題依賴(lài)的是比單個(gè)公開(kāi)實(shí)施方式所有特征更少的特征。因此，權(quán)利要求書(shū)的每個(gè)權(quán)利要求項(xiàng)本身保持為單獨(dú)的完整的實(shí)施例。綜上，本領(lǐng)域技術(shù)人員將認(rèn)識(shí)到在不脫離本公開(kāi)的范圍和精神的情況下，可在更廣闊的各方面中進(jìn)行改變和修改。所附權(quán)利要求書(shū)在其范圍內(nèi)涵蓋了落入本公開(kāi)真實(shí)范圍和精神內(nèi)的所有此類(lèi)改變、變化和修改。當(dāng)前第1頁(yè)1 2 3