本發(fā)明涉及導(dǎo)航技術(shù)領(lǐng)域，尤其涉及一種基于分層架構(gòu)的視覺slam后端優(yōu)化方法。

背景技術(shù)：

同步定位與地圖構(gòu)建(simultaneouslocalizationandmapping,slam)最早于1988年提出，最開始的時(shí)候用于描述機(jī)器人在未知環(huán)境的未知地點(diǎn)中同步的進(jìn)行地圖構(gòu)建和自身定位。機(jī)器人通過(guò)傳感器獲取的環(huán)境數(shù)據(jù)對(duì)位置環(huán)境進(jìn)行地圖創(chuàng)建，然后根據(jù)當(dāng)前觀測(cè)到的環(huán)境特征與地圖中的特征進(jìn)行匹配從而進(jìn)行自身的定位。根據(jù)使用的傳感器不同，slam主要可以分為視覺slam和激光slam。由于圖像具有豐富的顏色信息和紋理信息，再加上近年來(lái)計(jì)算能力的提升，基于圖像的視覺slam成為研究熱點(diǎn)。

在slam算法問(wèn)題中，可以分為前端和后端兩個(gè)部分。前端的主要作用是計(jì)算幀與幀間的相對(duì)關(guān)系。包括特征點(diǎn)提取、特征點(diǎn)的匹配、利用匹配點(diǎn)計(jì)算位姿。后端的作用主要是對(duì)前端的輸出結(jié)果進(jìn)行優(yōu)化，得到最優(yōu)的位姿估計(jì)。傳統(tǒng)的視覺slam方法中，后端優(yōu)化一般分為兩部分，一個(gè)是基于當(dāng)前時(shí)刻圖像幀的局部?jī)?yōu)化，另一個(gè)是當(dāng)運(yùn)動(dòng)載體回到運(yùn)動(dòng)初始點(diǎn)或者行駛過(guò)的路徑時(shí)進(jìn)行回環(huán)檢測(cè)，利用回環(huán)檢測(cè)結(jié)果，從回環(huán)起點(diǎn)到終點(diǎn)進(jìn)行一個(gè)整體優(yōu)化。

傳統(tǒng)slam方法的缺點(diǎn)在于，局部小窗口的優(yōu)化對(duì)定位誤差的矯正效果有限，當(dāng)運(yùn)動(dòng)路線較長(zhǎng)時(shí)，局部小窗的優(yōu)化方式仍然避免不了大的位姿漂移。閉環(huán)整體優(yōu)化可以大幅度矯正這種漂移，然而當(dāng)slam技術(shù)運(yùn)用在車輛上時(shí)，由于車輛行駛路線的不確定性，在行駛過(guò)程中未必能夠形成閉環(huán)，況且由于車輛行駛路線較長(zhǎng)，局部誤差累計(jì)到足夠大時(shí)，閉環(huán)檢測(cè)對(duì)誤差的矯正能力也會(huì)受限。

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

鑒于上述的分析，本發(fā)明旨在提供一種基于分層架構(gòu)的視覺slam后端優(yōu)化方法，用以解決現(xiàn)有技術(shù)存在位姿漂移、傳統(tǒng)slam優(yōu)化方法依賴閉環(huán)檢測(cè)的問(wèn)題。

本發(fā)明的目的主要是通過(guò)以下技術(shù)方案實(shí)現(xiàn)的：

一種基于分層架構(gòu)的視覺slam后端優(yōu)化方法，包括以下步驟：

步驟s1：建立底層優(yōu)化層，對(duì)關(guān)鍵幀進(jìn)行優(yōu)化，得到當(dāng)前關(guān)鍵幀的最佳位置；

步驟s2：建立高層優(yōu)化層，對(duì)關(guān)聯(lián)幀單元進(jìn)行優(yōu)化，得到關(guān)聯(lián)幀單元的最佳位置。

所述步驟s1包括以下步驟：

步驟s101：當(dāng)生成新的關(guān)鍵幀時(shí)，將歷史關(guān)鍵幀中與當(dāng)前關(guān)鍵幀存在連接關(guān)系的關(guān)鍵幀作為待優(yōu)化關(guān)鍵幀，并放入待優(yōu)化關(guān)鍵幀列表，將待優(yōu)化關(guān)鍵幀對(duì)應(yīng)的所有地圖點(diǎn)作為待優(yōu)化地圖點(diǎn)；

步驟s102：取出所有能夠觀察到待優(yōu)化地圖點(diǎn)的關(guān)鍵幀，將待優(yōu)化關(guān)鍵幀以外的其他關(guān)鍵幀全部固定，作為優(yōu)化參考；

步驟s103：使用圖優(yōu)化的方法，將待優(yōu)化關(guān)鍵幀和待優(yōu)化地圖點(diǎn)作為優(yōu)化頂點(diǎn)，所有待優(yōu)化地圖點(diǎn)和能觀察到待優(yōu)化地圖點(diǎn)的關(guān)鍵幀的投影關(guān)系作為優(yōu)化邊，進(jìn)行優(yōu)化迭代，得到最終的優(yōu)化結(jié)果，并將優(yōu)化過(guò)的關(guān)鍵幀設(shè)置標(biāo)記；

步驟s104：從關(guān)鍵幀緩存列表起點(diǎn)開始，取出未被標(biāo)記的關(guān)鍵幀，進(jìn)行優(yōu)化。

所述圖優(yōu)化的方法是通過(guò)g2o的開源c++框架來(lái)建立的，g2o中定義好了圖優(yōu)化的優(yōu)化步驟和優(yōu)化流程，用戶在使用g2o時(shí)，只需建立誤差函數(shù)以及優(yōu)化邊。

所建立的誤差函數(shù)為待優(yōu)化關(guān)鍵幀對(duì)應(yīng)的三維點(diǎn)投影在圖像平面上時(shí)，與對(duì)應(yīng)的圖像中的二維特征點(diǎn)的位置的誤差，稱為重投影誤差函數(shù)，如下式所示：

式中為圖像中的特征點(diǎn)二維坐標(biāo)；k為相機(jī)內(nèi)參矩陣；rk為待優(yōu)化關(guān)鍵幀相對(duì)于世界坐標(biāo)系的三維旋轉(zhuǎn)矩陣；tk為待優(yōu)化關(guān)鍵幀相對(duì)于世界坐標(biāo)系的三維平移向量；pⁱ為待優(yōu)化空間三維點(diǎn)坐標(biāo)。

進(jìn)一步地，從關(guān)鍵幀緩存列表中的第一幀開始，逐幀檢查關(guān)鍵幀是否被標(biāo)記，將未被標(biāo)記的關(guān)鍵幀取出，放入待優(yōu)化關(guān)鍵幀列表；

在待優(yōu)化關(guān)鍵幀列表中，從未被標(biāo)記幀開始向前數(shù)兩幀，向后數(shù)兩幀，這五幀固定第一幀和最后一幀，優(yōu)化中間的三幀；倘若最后一幀也為未標(biāo)記幀，則放棄該幀，固定第一幀，優(yōu)化后三幀。

所述步驟s2包括以下步驟：

步驟s201：將一定數(shù)量的關(guān)鍵幀劃分為一個(gè)關(guān)聯(lián)幀單元，確定關(guān)聯(lián)幀單元的大??；

步驟s202：通過(guò)關(guān)聯(lián)幀單元內(nèi)的關(guān)鍵幀建立單元位姿與單元間共視點(diǎn)的關(guān)系；

步驟s203：將關(guān)聯(lián)幀單元的單元位姿和地圖點(diǎn)位姿分別作為優(yōu)化頂點(diǎn)，通過(guò)圖優(yōu)化的方法對(duì)關(guān)聯(lián)幀單元進(jìn)行優(yōu)化。

進(jìn)一步地，取一個(gè)尺寸固定大小的窗口，窗口不斷地在待優(yōu)化關(guān)鍵幀列表中滑動(dòng)，取出窗口內(nèi)的關(guān)聯(lián)幀單元進(jìn)行優(yōu)化，窗口滑動(dòng)的間隔有一定的重疊。

進(jìn)一步地，計(jì)算每一個(gè)關(guān)聯(lián)幀單元內(nèi)關(guān)鍵幀相對(duì)于關(guān)聯(lián)幀單元的相對(duì)位姿rkr、tkr，并使其保持不變，進(jìn)行優(yōu)化時(shí)，將關(guān)聯(lián)幀單元位姿和單元間共視點(diǎn)位姿作為優(yōu)化頂點(diǎn)，通過(guò)關(guān)鍵幀與三維點(diǎn)的位置關(guān)系，構(gòu)建關(guān)聯(lián)幀單元與三維點(diǎn)的位置關(guān)系，得到誤差函數(shù)。

關(guān)聯(lián)幀單元位姿與關(guān)聯(lián)幀單元內(nèi)關(guān)鍵幀位姿的關(guān)系如下式所示：

其中tk為關(guān)聯(lián)幀單元內(nèi)關(guān)鍵幀位姿，tr為關(guān)聯(lián)幀單元位姿，tkr代表單元內(nèi)關(guān)鍵幀相對(duì)于單元的六自由度位姿變換，rr、tr為待優(yōu)化單元位姿；t表示關(guān)鍵幀相對(duì)于世界坐標(biāo)系的六自由度三維變換矩陣，包括旋轉(zhuǎn)和平移。

關(guān)聯(lián)幀單元的誤差函數(shù)為：

通過(guò)圖優(yōu)化的方法求出關(guān)聯(lián)幀單元的誤差函數(shù)最小時(shí)對(duì)應(yīng)的rr和tr的值，得到關(guān)聯(lián)幀單元的最佳位置。

本發(fā)明有益效果如下：

本發(fā)明提出了一種基于通用分層架構(gòu)的視覺slam優(yōu)化方法，從多個(gè)層次多個(gè)尺度對(duì)slam定位結(jié)果進(jìn)行在線優(yōu)化，且計(jì)算量不會(huì)隨著優(yōu)化層的增加而顯著增加，相較于傳統(tǒng)slam優(yōu)化方法，本方法不依賴回環(huán)檢測(cè)的方式來(lái)進(jìn)行偏差矯正，即使沒有形成閉環(huán)也可以顯著的優(yōu)化定位結(jié)果。同時(shí)本方法也支持回環(huán)檢測(cè)，在本方法的基礎(chǔ)上進(jìn)行回環(huán)檢測(cè)會(huì)得到更好的定位效果，提高了slam定位結(jié)果的魯棒性，通過(guò)將slam運(yùn)行過(guò)程中產(chǎn)生的關(guān)鍵幀進(jìn)行逐層分塊優(yōu)化的方法來(lái)減小誤差，減少了由于多幀誤差積累等原因造成的slam地圖和定位結(jié)果漂移的現(xiàn)象，提高了slam定位結(jié)果和地圖的準(zhǔn)確性和穩(wěn)定性。

本發(fā)明的其他特征和優(yōu)點(diǎn)將在隨后的說(shuō)明書中闡述，并且，部分的從說(shuō)明書中變得顯而易見，或者通過(guò)實(shí)施本發(fā)明而了解。本發(fā)明的目的和其他優(yōu)點(diǎn)可通過(guò)在所寫的說(shuō)明書、權(quán)利要求書、以及附圖中所特別指出的結(jié)構(gòu)來(lái)實(shí)現(xiàn)和獲得。

附圖說(shuō)明

附圖僅用于示出具體實(shí)施例的目的，而并不認(rèn)為是對(duì)本發(fā)明的限制，在整個(gè)附圖中，相同的參考符號(hào)表示相同的部件。

圖1分層優(yōu)化流程圖；

圖2優(yōu)化單元及優(yōu)化窗口尺寸示意圖；

圖3是單元內(nèi)外點(diǎn)示意圖；

圖4是底層優(yōu)化層優(yōu)化窗口示意圖；

具體實(shí)施方式

下面結(jié)合附圖來(lái)具體描述本發(fā)明的優(yōu)選實(shí)施例，其中，附圖構(gòu)成本申請(qǐng)一部分，并與本發(fā)明的實(shí)施例一起用于闡釋本發(fā)明的原理。

本發(fā)明實(shí)施例提供了一種基于分層架構(gòu)的視覺slam后端優(yōu)化方法，該方法流程圖如圖1所示，提出的后端分層優(yōu)化架構(gòu)總體上分為底層和上層，底層的任務(wù)是將當(dāng)前關(guān)鍵幀與臨近的關(guān)鍵幀和地圖點(diǎn)建立連接，通過(guò)最小化重投影誤差來(lái)得到當(dāng)前關(guān)鍵幀的最佳位置；上層的任務(wù)是將固定數(shù)量的關(guān)鍵幀綁定作為一個(gè)獨(dú)立的局部關(guān)聯(lián)幀單元，將多個(gè)局部關(guān)聯(lián)幀單元建立連接，通過(guò)最小化重投影誤差來(lái)得到關(guān)聯(lián)幀單元的最佳位置。

將程序運(yùn)行時(shí)的第一個(gè)圖像幀作為關(guān)鍵幀，在以后的關(guān)鍵幀選取中，若某時(shí)刻的圖像幀與上一個(gè)關(guān)鍵幀所提取到的特征點(diǎn)的重復(fù)率小于一定閾值(在本實(shí)施例中為0.25～0.3)，則把該時(shí)刻的圖像幀作為關(guān)鍵幀。

在當(dāng)前關(guān)鍵幀中可以提取若干二維特征點(diǎn)，每個(gè)二維特征點(diǎn)都在三維世界中對(duì)應(yīng)一個(gè)三維地圖點(diǎn)，這些三維地圖點(diǎn)也稱為當(dāng)前關(guān)鍵幀對(duì)應(yīng)的三維地圖點(diǎn)。若當(dāng)前關(guān)鍵幀所對(duì)應(yīng)的三維地圖點(diǎn)與某一個(gè)關(guān)鍵幀所對(duì)應(yīng)的三維地圖點(diǎn)中存在重合的地圖點(diǎn)，則認(rèn)為當(dāng)前幀與該關(guān)鍵幀存在連接關(guān)系。所述將當(dāng)前關(guān)鍵幀與臨近的關(guān)鍵幀和地圖點(diǎn)建立連接，是指找出與當(dāng)前關(guān)鍵幀存在連接關(guān)系的所有關(guān)鍵幀，與當(dāng)前幀一起通過(guò)最小化重投影誤差的方式來(lái)得到當(dāng)前關(guān)鍵幀的最佳位置。

具體地，所述基于分層架構(gòu)的視覺slam后端優(yōu)化方法包括以下步驟：

步驟s1：建立底層優(yōu)化層，對(duì)關(guān)鍵幀進(jìn)行優(yōu)化，得到當(dāng)前關(guān)鍵幀的最佳位置。

在車輛行駛的過(guò)程中，slam前端會(huì)不斷生成新的關(guān)鍵幀，從所生成的第一幀關(guān)鍵幀一直到當(dāng)前關(guān)鍵幀，按照時(shí)間序列存入關(guān)鍵幀緩存列表，提取出與當(dāng)前關(guān)鍵幀存在連接關(guān)系的關(guān)鍵幀和地圖點(diǎn)，通過(guò)固定一部分關(guān)鍵幀的位姿來(lái)優(yōu)化其余的關(guān)鍵幀和地圖點(diǎn)，將被優(yōu)化過(guò)的關(guān)鍵幀做上標(biāo)記。

具體地，所述步驟s1包括以下步驟：

步驟s101：當(dāng)生成新的關(guān)鍵幀時(shí)，將歷史關(guān)鍵幀中與當(dāng)前關(guān)鍵幀存在連接關(guān)系的關(guān)鍵幀作為待優(yōu)化關(guān)鍵幀，并放入待優(yōu)化關(guān)鍵幀列表，然后將待優(yōu)化關(guān)鍵幀對(duì)應(yīng)的所有地圖點(diǎn)作為待優(yōu)化地圖點(diǎn)。

步驟s102：取出所有能夠觀察到待優(yōu)化地圖點(diǎn)的關(guān)鍵幀，將待優(yōu)化關(guān)鍵幀以外的其他關(guān)鍵幀全部固定，作為優(yōu)化參考。

步驟s103：使用圖優(yōu)化的方法，將待優(yōu)化關(guān)鍵幀和待優(yōu)化地圖點(diǎn)作為優(yōu)化頂點(diǎn)，所有待優(yōu)化地圖點(diǎn)和能觀察到待優(yōu)化地圖點(diǎn)的關(guān)鍵幀的投影關(guān)系作為優(yōu)化邊，進(jìn)行一定次數(shù)優(yōu)化迭代，得到最終的優(yōu)化結(jié)果，并將優(yōu)化過(guò)的關(guān)鍵幀設(shè)置標(biāo)記。

所述投影關(guān)系是指本發(fā)明領(lǐng)域中通用的透視投影相機(jī)模型所建立的投影關(guān)系；

理論上，優(yōu)化迭代的次數(shù)越多，優(yōu)化效果越好，但是耗費(fèi)時(shí)間也越長(zhǎng)，所以優(yōu)化過(guò)程中需要平衡優(yōu)化精度和消耗時(shí)間，經(jīng)過(guò)試驗(yàn)，優(yōu)化迭代的次數(shù)為10-20時(shí)能夠滿足要求；

所述圖優(yōu)化的方法是通過(guò)g2o的開源c++框架來(lái)建立的，g2o中定義好了圖優(yōu)化的優(yōu)化步驟和優(yōu)化流程，用戶在使用g2o時(shí)，只需建立誤差函數(shù)以及優(yōu)化邊；

在本實(shí)施例中，所建立的誤差函數(shù)即為待優(yōu)化關(guān)鍵幀對(duì)應(yīng)的三維點(diǎn)投影在圖像平面上時(shí)，與對(duì)應(yīng)的圖像中的二維特征點(diǎn)的位置的誤差，稱為重投影誤差函數(shù)，如下式所示：

式中為圖像中的特征點(diǎn)二維坐標(biāo)；k為相機(jī)內(nèi)參矩陣；rk為待優(yōu)化關(guān)鍵幀相對(duì)于世界坐標(biāo)系的三維旋轉(zhuǎn)矩陣，是一個(gè)3*3的正交矩陣，表示關(guān)鍵幀相對(duì)于世界坐標(biāo)系的三自由度旋轉(zhuǎn)；tk為待優(yōu)化關(guān)鍵幀相對(duì)于世界坐標(biāo)系的三維平移向量，是一個(gè)3*1的向量，表示關(guān)鍵幀相對(duì)于世界坐標(biāo)系的三自由度平移；pⁱ為待優(yōu)化空間三維點(diǎn)坐標(biāo)。

步驟s104：從關(guān)鍵幀緩存列表起點(diǎn)開始，取出未被標(biāo)記的關(guān)鍵幀，進(jìn)行優(yōu)化。

由于車輛的不斷向前行駛，會(huì)不斷的產(chǎn)生新的關(guān)鍵幀，而步驟s101～s103中的優(yōu)化過(guò)程需要一定時(shí)間才能完成，因此在優(yōu)化的過(guò)程中產(chǎn)生的關(guān)鍵幀有可能會(huì)被跳過(guò)，所以從關(guān)鍵幀緩存列表起點(diǎn)開始，將未被標(biāo)記的關(guān)鍵幀取出，進(jìn)行優(yōu)化；

具體地，從關(guān)鍵幀緩存列表中的第一幀開始，逐幀檢查關(guān)鍵幀是否被標(biāo)記，將未被標(biāo)記的關(guān)鍵幀取出，放入待優(yōu)化關(guān)鍵幀列表，如圖2所示，在待優(yōu)化關(guān)鍵幀列表中，從未被標(biāo)記幀開始向前數(shù)兩幀，向后數(shù)兩幀，這五幀固定第一幀和最后一幀，優(yōu)化中間的三幀；倘若最后一幀也為未標(biāo)記幀，則放棄該幀，固定第一幀，優(yōu)化后三幀；優(yōu)化過(guò)程同步驟s103相同。

步驟s2：建立高層優(yōu)化層，對(duì)關(guān)聯(lián)幀單元進(jìn)行優(yōu)化，得到關(guān)聯(lián)幀單元的最佳位置。

如圖3所示，高層優(yōu)化層隨著程序的運(yùn)行不斷地創(chuàng)建新的高層優(yōu)化層，每層高層優(yōu)化層中以一定數(shù)量的關(guān)鍵幀綁定作為一個(gè)單獨(dú)的優(yōu)化單元，作為關(guān)聯(lián)幀單元，隨著層數(shù)的增加，關(guān)聯(lián)幀單元內(nèi)的關(guān)鍵幀的數(shù)量也在不斷地增加。

具體地，所述步驟s2包括以下子步驟：

步驟s201：根據(jù)一定數(shù)量規(guī)律將一定數(shù)量的關(guān)鍵幀劃分為一個(gè)關(guān)聯(lián)幀單元，確定關(guān)聯(lián)幀單元的大?。?/p>

所述一定數(shù)量規(guī)律是指關(guān)聯(lián)幀單元中關(guān)鍵幀數(shù)量n與所在層數(shù)l呈指數(shù)關(guān)系，n＝2^l+1，采用指數(shù)映射的關(guān)系可以避免相鄰高層優(yōu)化層中關(guān)聯(lián)幀單元內(nèi)待優(yōu)化關(guān)鍵幀的數(shù)量過(guò)于接近，從而避免產(chǎn)生因計(jì)算資源分布不均而導(dǎo)致的優(yōu)化阻滯的現(xiàn)象；

本實(shí)施例中，關(guān)聯(lián)幀單元中關(guān)鍵幀的數(shù)量為4，根據(jù)實(shí)驗(yàn)效果得知，此時(shí)的優(yōu)化效果較好。

步驟s202：通過(guò)關(guān)聯(lián)幀單元內(nèi)的關(guān)鍵幀建立單元位姿與單元間共視點(diǎn)的關(guān)系；

如圖4所示，將只有關(guān)聯(lián)幀單元內(nèi)的關(guān)鍵幀可以觀察到的地圖點(diǎn)稱為單元內(nèi)點(diǎn)，將相鄰關(guān)聯(lián)幀單元間的關(guān)鍵幀所共同觀察到的地圖點(diǎn)稱為單元間共視點(diǎn)；

所述關(guān)聯(lián)幀單元的單元位姿用處于關(guān)聯(lián)幀單元中間位置的關(guān)鍵幀的位姿來(lái)表示，單元位姿與關(guān)聯(lián)幀單元內(nèi)的關(guān)鍵幀的位姿存在相對(duì)位置關(guān)系，關(guān)聯(lián)幀單元內(nèi)的關(guān)鍵幀與單元間共視點(diǎn)之間存在投影關(guān)系，因此，通過(guò)關(guān)聯(lián)幀單元內(nèi)的關(guān)鍵幀建立單元位姿與單元間共視點(diǎn)的關(guān)系；

步驟s203：將關(guān)聯(lián)幀單元的單元位姿和地圖點(diǎn)位姿分別作為優(yōu)化頂點(diǎn)，通過(guò)圖優(yōu)化的方法對(duì)關(guān)聯(lián)幀單元進(jìn)行優(yōu)化；

在進(jìn)行優(yōu)化時(shí)，關(guān)聯(lián)幀單元作為整體進(jìn)行優(yōu)化，關(guān)聯(lián)幀單元位姿發(fā)生改變后，單元內(nèi)點(diǎn)、單元內(nèi)關(guān)鍵幀位姿都要隨之發(fā)生改變，取一個(gè)尺寸固定大小的窗口，窗口不斷地在待優(yōu)化關(guān)鍵幀列表中滑動(dòng)，取出窗口內(nèi)的關(guān)聯(lián)幀單元進(jìn)行優(yōu)化，為了使優(yōu)化后的關(guān)聯(lián)幀單元位姿和地圖點(diǎn)保持連續(xù)而不發(fā)生斷層的現(xiàn)象，窗口滑動(dòng)的間隔有一定的重疊；

由于單元間共視點(diǎn)的數(shù)量只由相鄰關(guān)聯(lián)幀單元臨近的幾幀關(guān)鍵幀決定，數(shù)量基本固定，不隨著關(guān)聯(lián)幀單元內(nèi)關(guān)鍵幀數(shù)量的增加而增加，并且每次優(yōu)化的窗口大小也是一定的，所以優(yōu)化時(shí)的計(jì)算量不會(huì)隨著關(guān)聯(lián)幀單元內(nèi)關(guān)鍵幀的增加而增加，隨著優(yōu)化層的層數(shù)增加，優(yōu)化的計(jì)算量基本保持不變；

計(jì)算每一個(gè)關(guān)聯(lián)幀單元內(nèi)關(guān)鍵幀相對(duì)于關(guān)聯(lián)幀單元的相對(duì)位姿rkr、tkr，并使其保持不變，進(jìn)行優(yōu)化時(shí)，將關(guān)聯(lián)幀單元位姿和單元間共視點(diǎn)位姿作為優(yōu)化頂點(diǎn)，通過(guò)關(guān)鍵幀與三維點(diǎn)的位置關(guān)系，構(gòu)建關(guān)聯(lián)幀單元與三維點(diǎn)的位置關(guān)系，得到誤差函數(shù)。關(guān)聯(lián)幀單元位姿與關(guān)聯(lián)幀單元內(nèi)關(guān)鍵幀位姿的關(guān)系如下式所示：

其中tk為關(guān)聯(lián)幀單元內(nèi)關(guān)鍵幀位姿，tr為關(guān)聯(lián)幀單元位姿，tkr代表單元內(nèi)關(guān)鍵幀相對(duì)于單元的六自由度位姿變換，rr、tr為待優(yōu)化單元位姿。

t為三維變換矩陣，是一個(gè)4*4的矩陣，表示關(guān)鍵幀相對(duì)于世界坐標(biāo)系的六自由度變換，包括旋轉(zhuǎn)和平移。其左上角的3*3個(gè)元素由r構(gòu)成，右上角的3*1個(gè)元素由t構(gòu)成，第四行的四個(gè)元素中前三個(gè)為0，最后一個(gè)為1，最終可以寫成的形式。

將式(2-2)帶入(2-1)中，可得：

式(2-3)為關(guān)聯(lián)幀單元內(nèi)一個(gè)關(guān)鍵幀的重投影誤差函數(shù)，因?yàn)閮?yōu)化的是一個(gè)整體單元，需要將單元內(nèi)所有關(guān)鍵幀的重投影誤差考慮在內(nèi)，因此關(guān)聯(lián)幀單元總的誤差函數(shù)為：

通過(guò)圖優(yōu)化的方法求出誤差函數(shù)(2-4)最小時(shí)對(duì)應(yīng)的變量值，即rr，tr，得到關(guān)聯(lián)幀單元的最佳位置。

綜上所述，本發(fā)明實(shí)施例提供了一種基于分層架構(gòu)的視覺slam后端優(yōu)化方法，通過(guò)將slam運(yùn)行過(guò)程中產(chǎn)生的關(guān)鍵幀進(jìn)行逐層分塊優(yōu)化的方法來(lái)減小誤差，減少了由于多幀誤差積累等原因造成的slam地圖和定位結(jié)果漂移的現(xiàn)象，提高了slam定位結(jié)果和地圖的準(zhǔn)確性和穩(wěn)定性，通過(guò)將本發(fā)明方法和目前效果最好的slam方法得到的定位結(jié)果進(jìn)行比較，證明了本發(fā)明的有效性。

本領(lǐng)域技術(shù)人員可以理解，實(shí)現(xiàn)上述實(shí)施例方法的全部或部分流程，可以通過(guò)計(jì)算機(jī)程序來(lái)指令相關(guān)的硬件來(lái)完成，所述的程序可存儲(chǔ)于計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)中。其中，所述計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)為磁盤、光盤、只讀存儲(chǔ)記憶體或隨機(jī)存儲(chǔ)記憶體等。

以上所述，僅為本發(fā)明較佳的具體實(shí)施方式，但本發(fā)明的保護(hù)范圍并不局限于此，任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，可輕易想到的變化或替換，都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。