一種基於點線綜合特徵的改進同步定位與建圖方法

2023-10-11 23:16:15 1

1.本發明屬於機器人定位與建圖領域，具體涉及一種基於點線綜合特徵的改進同步定位與建圖方法。


背景技術：

2.隨著同步定位與建圖(slam,simultaneous localization and mapping)技術的發展和深入研究，單純基於點特徵的視覺slam的局限性慢慢表露出來，尤其是在缺少點特徵的室內弱紋理環境下更為明顯。而線特徵作為在人工環境中出現最多的特徵，非常適合作為點特徵在弱紋理環境下的補充特徵，並且線特徵所包含的環境信息更加豐富，對於環境信息的結構化的表達效果更好。線特徵的使用提高了在室內弱紋理場景下可靠的定位與建圖能力，對提升弱紋理場景下機器人定位與建圖的精度和魯棒性具有重要意義。


技術實現要素：

3.為了解決上述問題，本發明提出了一種基於點線綜合特徵的改進同步定位與建圖方法，在傳統視覺slam算法的基礎上增加線特徵作為補充特徵，改進重投影誤差模型，構建基於點線仿射不變量的點線綜合特徵殘差約束，建立基於點線綜合特徵的後端優化方程，提升視覺slam算法在弱紋理場景下的精度和魯棒性問題。
4.本發明的技術方案如下：
5.一種基於點線綜合特徵的改進同步定位與建圖方法，包括以下步驟：
6.步驟1、在slam前端，提取圖像的特徵點得到點特徵，然後進行點特徵匹配；
7.步驟2、繼續在slam前端，進行線特徵的提取，構建線特徵描述符，再將提取的線特徵進行匹配；
8.步驟3、對點特徵和線特徵經過匹配之後，在slam後端，改進線特徵的重投影模型，基於點線綜合特徵對相機的位姿進行估計；
9.步驟4、構建基於點線仿射不變性的點線綜合特徵約束，由得到的點線重投影約束構建基本的後端點線特徵緊耦合約束方程，進而得到最優的相機位姿估計和特徵坐標估計，最後生成地圖。
10.進一步地，步驟1的具體過程如下：
11.步驟1.1、使用harris算法進行角點的提取；
12.首先，構建一個沿著圖像xy兩方向移動的像素值變化函數e(x,y)：
[0013][0014][0015]
其中，(x,y)表示像素點的位置，m為局部結構張量，ω為高斯函數，
i為圖像的像素值隨位置變化的函數，μ和ν分別為x、y軸的像素位移量；
[0016]
然後，定義r為每個角點的harris響應函數，r表示為：
[0017]
r＝det(m)-α[tr(m)]2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0018]
其中，det(m)為矩陣m的特徵值，tr(m)為矩陣m的跡，α為權重係數；
[0019]
最後，根據求解的r值進行角點響應閾值判斷，令thr表示角點檢測閾值，r＞thr時區域判定為角點；
[0020]
步驟1.2、採用光流法進行點特徵的匹配；
[0021]
首先進行圖像金字塔的構建來滿足圖像的尺度不變性，隨後根據光流法的灰度不變假設來追蹤幀間的特徵點灰度值，由於幀間位移量微小，並且同一特徵點短時間的灰度值保持不變，所以在參考幀定位一個區域的灰度值，並以此區域在下一幀一定範圍內尋找到與參考幀特徵點附近區域灰度值相近的區域，最後進行點特徵的追蹤匹配。
[0022]
進一步地，步驟2的具體過程如下：
[0023]
步驟2.1、圖像的線特徵提取選擇edlines直線檢測器；其過程描述如下：
[0024]
步驟2.1.1、邊緣提取；使用ed邊緣檢測器對灰度圖像進行邊緣特徵提取；
[0025]
步驟2.1.2、合併線段；將邊緣提取得到的連續邊緣像素組成的邊緣線段分割成為一個或者多個線段，其基本思想為按照順序遍歷像素點，並使用最小二乘法將像素擬合為直線直至誤差超過閾值，當誤差超過閾值後，結束擬合併生成新的線段；
[0026]
步驟2.1.3、剔除誤檢量；構造一個包含區域中所有點的矩形，然後將矩形中所有水平線方向角度與矩形方向角度偏差小於經驗值τ的點稱為對齊點；假設一個矩形內總的像素點數為n，對齊點數量為k；誤檢剔除採用helm-holtz原理構建nfa，nfa模型如公式(4)所示：
[0027][0028]
公式中，i表示像素點的下標，其範圍為從k到n，n代表圖像像素值，p代表任意一個像素點為對齊點的概率；設置nfa的閾值為ξ，若nfa＞ξ，則保留直線段檢測的結果；否則，丟棄該直線段的檢測結果；
[0029]
步驟2.2、構建線特徵描述符，再將提取的線特徵進行匹配；
[0030]
線特徵的匹配採用lbd描述子對線特徵進行描述，並完成參考圖像和查詢圖像之間的線特徵匹配，線特徵匹配包括三個步驟：
[0031]
步驟2.2.1、查找候選匹配對；檢查參考圖像和查詢圖像的線矢量linevec，比較它們的一元集合屬性和局部外觀相似性，若沒有通過測試，則認為是不匹配的；
[0032]
一元集合屬性用linevec的方向直方圖衡量；計算參考圖像和查詢圖像linevec的方向直方圖，分別用hr、h
t
表示，若||h
r-hq||＜th
l
，則認為是一組候選匹配，其中th
l
為閾值，若||h
r-hq||≥th
l
，則需要判斷局部外觀相似性；
[0033]
局部外觀相似性用線描述符的距離度量；對於linevec的每個直線，生成一個lbd描述符向量；當對參考圖像和查詢圖像的兩組linevec做匹配時，計算參考圖像和查詢圖像的所有描述符之間的距離，其中，距離的最小值為d
min
，若d
min
＜ths，則被選為候選匹配，若dmin
＞ths，則認為線特徵未匹配成功；其中，ths為局部外觀相似的容忍度；
[0034]
步驟2.2.2、構建線段關係圖；對於一組候選匹配，假設內部含k條曲線，構建一個一致性得分矩陣a來表示線段關係圖，a的維數為k
×
k；設該組候選匹配內，li、lj分別表示在參考圖像和查詢圖像內的兩條線段，矩陣a的元素a
ij
計算為其中，di、d
p
、d
θ
為幾何相似性，為局部外觀相似性；
[0035]
步驟2.2.3、生成匹配結果；基於矩陣a，用指標向量x表示該簇，問題構建為如下問題：
[0036]
x
*
＝argmax(x
t
ax)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0037]
線特徵的匹配從匹配簇的結果中搜索得到。
[0038]
進一步地，步驟3的具體過程如下：
[0039]
步驟3.1、基於點特徵，利用傳統視覺裡程計的方法對相機位姿進行估計，其具體過程如下：
[0040]
步驟3.1.1、採用對極約束恢復單目尺度和初始相機位姿；
[0041]
步驟3.1.2、採用三角測量對於空間的二維坐標進行深度恢復，進而得到三維空間中的特徵坐標；
[0042]
步驟3.1.3、根據步驟3.1.2所求解的特徵三維坐標以及對應的二維平面坐標，採用pnp算法進行幀間位姿的求解，由此獲得連續的幀間相機位姿和關鍵的特徵點坐標；
[0043]
步驟3.1.4、在得到關鍵的特徵點坐標後，根據icp算法進行3d-3d匹配對的位姿求解，最終可得到相機坐標系相對於世界坐標系的位姿關係矩陣r和t；
[0044]
步驟3.2、對線特徵的重投影誤差模型進行改進；改進的線特徵重投影誤差模型採用投影線段與像素平面線段中點的垂足距離和兩線段的夾角作為優化變量；改進的線特徵重投影方程和過程如下：
[0045]
步驟3.2.1、對於空間中的某條線段l，用其首末兩端的坐標(x1,y1,z1)、(x2,y2,z2)表示該線段，在普呂克坐標下l為6
×
1維的向量；
[0046]
步驟3.2.2、將線段l投影到圖像平面，得到圖像上的投影線段lc：
[0047][0048]
其中，k為線段的投影矩陣，q為普呂克坐標繫到相機坐標系的轉換關係，f
x
、fy、c
x
、cy為相機內參，r
cw
表示兩個坐標系間的旋轉矩陣，t
cw
為平移向量，[]
×
表示向量的反對稱矩陣，lc為3
×
1的向量，其三個分量分別用l1,l2,l3表示；
[0049]
步驟3.2.3、線特徵重投影誤差e
l
計算：令l表示將lc通過位姿參數投影到下一幀得到的線段，l
′
表示lc通過幀間匹配得到的下一幀的線段；若(x
l,s
,y
l,s
)、(x
l,e
,y
l,e
)表示線段l的起點和終點，k
l
、k
l
′
分別為l和l
′
的斜率，計算線段l
′
的中點(x
l
′
,m
,y
l
′
,m
)到投影線l的垂足距離d以及兩條線之間的夾角θ作為重投影誤差：
[0050]el
＝[d,θ]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0051][0052][0053]
步驟3.2.4、加入線特徵，利用線特徵約束優化對r和t的估計；以當前幀的位姿作為優化的狀態變量，通過最小化點線特徵的重投影殘差函數進行優化，優化過程表示為：
[0054][0055]
其中，i、j代表點特徵、線特徵在相鄰關鍵幀中的匹配對集合，e
pi
為點特徵的重投影誤差，e
lj
為線特徵的重投影誤差，ρ
p
(
·
)、ρ
l
(
·
)為huber核函數，用於平衡e
pi
、e
lj
增長過快對優化過程的影響。
[0056]
進一步地，步驟4的具體過程如下：
[0057]
步驟4.1、基於點線特徵的重投影誤差約束構建步驟如下：
[0058]
步驟4.1.1、對滑窗內幀的點特徵和線特徵進行跟蹤，範圍篩選為有共面的點特徵和線特徵，選出被觀測到幀數最多的點特徵和線特徵之後，根據點特徵強度選出點特徵強度最強的點特徵，根據線特徵長度選出最長的兩條線特徵；
[0059]
步驟4.1.2、在選取出的點特徵和線特徵所有共面幀上進行約束構建，根據仿射變換中面積比例不變的特性，約束的殘差方程採用第i幀點特徵和線特徵投影到第j幀之後點和線特徵所構成的三角形面積之比ki，與在第j幀上根據光流追蹤到的點線特徵構建的三角形面積比例kj之差構建，以此來構建幀內點線約束特徵e
pl
，方程如下：
[0060]
i幀和j幀之間的變換矩陣t為：
[0061]
t＝[r，t]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0062]
其中，r表示旋轉矩陣，t表示平移矩陣；
[0063]
點特徵坐標以及其變換為：
[0064]
(uj,vj,1)
t
＝r(ui,vi,1)
t
+t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0065]
其中，(ui,vi)表示第i幀點特徵的位置，(uj,vj)表示第j幀點特徵的位置；
[0066]
線特徵參數化形式和變換表示為：
[0067][0068]
其中，ii表示第i幀線特徵，ij表示第j幀線特徵，n
c,j
表示第j幀圖像中線特徵ij上一點與原點組成平面的法向量，d
c,j
表示第j幀圖像中線特徵ij的方向向量；
[0069]
點線特徵所構建三角形面積比例表示為：
[0070][0071]
其中，s1、s2為一張圖中由點線特徵構成的兩個三角形；
[0072]
基於公式(14)，ki、kj表示為：
[0073][0074]
其中，s
1,i
、s
2,i
表示第i幀圖像中點線特徵構成的兩個三角形，s
1,j
、s
2,j
表示第j幀圖像中點線特徵構成的兩個三角形；
[0075]
點線綜合約束特徵方程為：
[0076]epl
＝k
i-kjꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0077]
代價方程c_fun表示為：
[0078][0079]
其中，ρ(
·
)為cauchyloss函數，其形式為ρ(x)＝log(x+1)，表示點線融合的觀測協方差矩陣；
[0080]
步驟4.2、融合點線綜合特徵構建後端緊耦合後端優化函數；緊耦合後端優化函數的構建描述為：
[0081]
由點特徵的重投影誤差e
l
和線特徵的重投影誤差e
p
，構建基本的點線特徵緊耦合約束方程c
p+l
，即融合點線綜合特徵的緊耦合後端優化函數：
[0082][0083]
γ(x)＝log(x+1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(19)
[0084]
其中，和為點和線的重投影誤差，h表示點特徵個數的下標，j表示線特徵個數的下標，p
k,h
、l
k,j
表示點、線的觀測協方差矩陣，γ(
·
)為cauchyloss函數；通過ceres優化器求解優化c
p+l
，得到最優的相機位姿估計和特徵坐標估計，相機位姿估計實現目標的定位，利用特徵坐標估計生成地圖。
[0085]
本發明所帶來的有益技術效果：
[0086]
通過在slam前端採用線特徵作為補充特徵，在slam後端建立了基於點線綜合特徵的緊耦合優化函數，增強了視覺slam算法在弱紋理場景下的魯棒性問題，提升了機器人在室內弱紋理場景下定位和建圖的精度和可靠性。
附圖說明
[0087]
圖1為本發明基於點線綜合特徵的改進同步定位與建圖方法的流程圖；
[0088]
圖2為本發明基於點線仿射不變性構建點線綜合約束的原理圖。
具體實施方式
[0089]
下面結合附圖以及具體實施方式對本發明作進一步詳細說明：
[0090]
如圖1所示，一種基於點線綜合特徵的改進同步定位與建圖方法，包括以下步驟：
[0091]
步驟1、在slam前端，提取圖像的特徵點得到點特徵，然後進行點特徵匹配。具體過程如下：
[0092]
步驟1.1、使用常用的harris算法進行角點的提取；
[0093]
首先，構建一個沿著圖像xy兩方向移動的像素值變化函數e(x,y)：
[0094][0095][0096]
其中，(x,y)表示像素點的位置，m為局部結構張量，ω為高斯函數，i為圖像的像素值隨位置變化的函數，μ和ν分別為x、y軸的像素位移量。
[0097]
然後，定義r為每個角點的harris響應函數，r可以表示為：
[0098]
r＝det(m)-α[tr(m)]2ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(3)
[0099]
其中，det(m)為矩陣m的特徵值，tr(m)為矩陣m的跡，α為權重係數，一般取0.04～0.06。
[0100]
最後，根據求解的r值進行角點響應閾值判斷，令thr表示角點檢測閾值，r＞thr時區域判定為角點。
[0101]
步驟1.2、採用光流法進行點特徵的匹配；
[0102]
首先進行圖像金字塔的構建來滿足圖像的尺度不變性，隨後根據光流法的灰度不變假設來追蹤幀間的特徵點灰度值，由於幀間位移量微小，並且同一特徵點短時間的灰度值保持不變，所以可在參考幀定位一個區域的灰度值，並以此區域在下一幀一定範圍內尋找到與參考幀特徵點附近區域灰度值相近的區域，最後進行特徵點的追蹤匹配。
[0103]
步驟2、繼續在slam前端，進行線特徵的提取，構建線特徵描述符，再將提取的線特徵進行匹配。具體過程如下：
[0104]
步驟2.1、圖像的線特徵提取選擇edlines直線檢測器。其過程描述如下：
[0105]
步驟2.1.1、邊緣提取。使用edge drawing(ed,邊緣檢測器)對灰度圖像進行邊緣特徵提取。
[0106]
步驟2.1.2、合併線段。將邊緣提取得到的連續邊緣像素組成的邊緣線段分割成為一個或者多個線段，其基本思想為按照順序遍歷像素點，並使用最小二乘法將像素擬合為直線直至誤差超過一定閾值(1個像素誤差)，當誤差超過閾值後，結束擬合併生成新的線段。
[0107]
步驟2.1.3、剔除誤檢量。構造一個包含區域中所有點的矩形，然後將矩形中所有水平線方向角度與矩形方向角度偏差小於τ(τ為經驗值)的點稱為對齊點。假設一個矩形內總的像素點數為n，對齊點數量為k。誤檢剔除採用helm-holtz原理構建nfa(number of false alarms)，nfa模型如公式(4)所示：
[0108][0109]
公式中，i表示像素點的下標，其範圍為從k到n，n代表圖像像素值，p代表任意一個像素點為對齊點的概率。設置一個nfa的閾值ξ，若nfa＞ξ，則保留直線段檢測的結果；否則，丟棄該直線段的檢測結果。
[0110]
步驟2.2、構建線特徵描述符，再將提取的線特徵進行匹配；
[0111]
線特徵的匹配採用lbd(line band discriptor)描述子對線特徵進行描述，並完成參考圖像和查詢圖像之間的線特徵匹配，線特徵的匹配包括三個步驟：
[0112]
步驟2.2.1、查找候選匹配對。檢查參考圖像和查詢圖像的線矢量(linevec)，比較它們的一元集合屬性和局部外觀相似性，若沒有通過測試，則認為是不匹配的。
[0113]
一元集合屬性用linevec的方向直方圖衡量。計算參考圖像和查詢圖像linevec的方向直方圖，分別用hr、h
t
表示，若||h
r-hq||＜th
l
，則認為是一組候選匹配，其中th
l
為閾值，為經驗值，一般選取0.4。若||h
r-hq||≥th
l
，則需要判斷局部外觀相似性。
[0114]
局部外觀相似性用線描述符的距離度量。對於linevec的每個直線，生成一個lbd描述符向量。當對參考圖像和查詢圖像的兩組linevec做匹配時，計算參考圖像和查詢圖像的所有描述符之間的距離，其中，距離的最小值為d
min
，若d
min
＜ths，則被選為候選匹配，若d
min
＞ths，則認為線特徵未匹配成功。其中，ths為局部外觀相似的容忍度，為經驗值，一般取ths＝0.35。
[0115]
步驟2.2.2、構建線段關係圖。對於一組候選匹配(假設內部含k條曲線)，構建一個一致性得分矩陣a來表示線段關係圖，a的維數為k
×
k。設該組候選匹配內，li、lj分別表示在參考圖像和查詢圖像內的兩條線段，矩陣a的元素a
ij
可以計算為其中，di、d
p
、d
θ
為幾何相似性，為局部外觀相似性。
[0116]
步驟2.2.3、生成匹配結果。基於矩陣a，線特徵匹配問題就轉換成了需要匹配簇的最優化問題，即用指標向量x表示該簇，問題構建為如下問題：
[0117]
x
*
＝argmax(x
t
ax)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)
[0118]
線特徵的匹配可以從匹配簇的結果中搜索得到。
[0119]
步驟3、對點特徵和線特徵經過匹配之後，在slam後端，改進線特徵的重投影模型，基於點線綜合特徵對相機的位姿進行估計。具體過程如下：
[0120]
步驟3.1、基於點特徵，利用傳統視覺裡程計的方法對相機位姿進行估計，其具體過程如下：
[0121]
步驟3.1.1、採用對極約束恢復單目尺度和初始相機位姿；
[0122]
步驟3.1.2、採用三角測量對於空間的二維坐標進行深度恢復，進而得到三維空間中的特徵坐標；
[0123]
步驟3.1.3、根據步驟3.1.2所求解的特徵三維坐標以及對應的二維平面坐標，採用pnp(perspective-n-point)算法進行幀間位姿的求解，由此獲得連續的幀間相機位姿和關鍵的特徵點坐標。
[0124]
步驟3.1.4、在得到關鍵的特徵點坐標後，根據icp(iterative closest point)算法進行3d-3d匹配對的位姿求解，最終可得到相機坐標系相對於世界坐標系的位姿關係矩陣r和t。
[0125]
步驟3.2、對線特徵的重投影誤差模型進行改進。
[0126]
傳統線特徵重投影誤差模型採用投影線段與像素平面線段端點的垂足距離作為優化變量，改進的線特徵重投影誤差模型採用投影線段與像素平面線段中點的垂足距離和兩線段的夾角作為優化變量。改進的線特徵重投影方程和過程如下：
[0127]
步驟3.2.1、對於空間中的某條線段l，用其首末兩端的坐標(x1,y1,z1)、(x2,y2,z2)表示該線段，在普呂克坐標下l為6
×
1維的向量。
[0128]
步驟3.2.2、將線段l投影到圖像平面，得到圖像上的投影線段lc：
[0129][0130]
其中，k為線段的投影矩陣，q為普呂克坐標繫到相機坐標系的轉換關係，f
x
、fy、c
x
、cy為相機內參，r
cw
表示兩個坐標系間的旋轉矩陣，t
cw
為平移向量，[]
×
表示向量的反對稱矩陣，lc為3
×
1的向量，其三個分量分別用l1,l2,l3表示。
[0131]
步驟3.2.3、線特徵重投影誤差e
l
計算：令l表示將lc通過位姿參數投影到下一幀得到的線段，l
′
表示lc通過幀間匹配得到的下一幀的線段。若(x
l,s
,y
l,s
)、(x
l,e
,y
l,e
)表示線段l的起點和終點，k
l
、k
l
′
分別為l和l
′
的斜率，計算線段l
′
的中點(x
l
′
,m
,y
l
′
,m
)到投影線l的垂足距離d以及兩條線之間的夾角θ作為重投影誤差：
[0132]el
＝[d,θ]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(7)
[0133][0134][0135]
步驟3.2.4、加入線特徵，利用線特徵約束優化對r和t的估計。以當前幀的位姿作為優化的狀態變量，通過最小化點線特徵的重投影殘差函數進行優化，優化過程可以表示為：
[0136][0137]
其中，i、j代表點特徵、線特徵在相鄰關鍵幀中的匹配對集合，e
pi
為點特徵的重投影誤差，e
lj
為線特徵的重投影誤差，ρ
p
(
·
)、ρ
l
(
·
)為huber核函數，用於平衡e
pi
、e
lj
增長過快對優化過程的影響。
[0138]
步驟4、構建基於點線仿射不變性的點線綜合特徵約束，由得到的點線重投影約束構建基本的後端點線特徵緊耦合約束方程。具體過程如下：
[0139]
在slam的滑窗優化過程中，每幀圖像幀之間的優化誤差和噪聲影響會不斷累積進而造成滑窗優化方向產生偏移。為解決該問題，根據點線特徵的仿射不變性在滑窗幀內構建短期的迴環約束，即得到基於點線特徵的重投影誤差約束。
[0140]
步驟4.1、基於點線特徵的重投影誤差約束構建步驟如下：
[0141]
步驟4.1.1、對滑窗內幀的點特徵和線特徵進行跟蹤，範圍篩選為有共面(最少三個共面幀以上)的點特徵和線特徵，選出被觀測到幀數最多的點特徵和線特徵之後，根據點特徵強度選出點特徵強度最強的點特徵，根據線特徵長度選出最長的兩條線特徵。
[0142]
步驟4.1.2、在選取出的點特徵和線特徵所有共面幀上進行約束構建，根據仿射變換中面積比例不變的特性，約束的殘差方程採用第i幀點特徵和線特徵投影到第j幀之後點和線特徵所構成的三角形面積之比ki，與在第j幀上根據光流追蹤到的點線特徵構建的三角形面積比例kj之差構建，以此來構建幀內點線約束特徵e
pl
，圖2為幀間點線特徵約束，具
體方程如下：
[0143]
i幀和j幀之間的變換矩陣t為：
[0144]
t＝[r，t]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(11)
[0145]
其中，r表示旋轉矩陣，t表示平移矩陣。
[0146]
點特徵坐標以及其變換為：
[0147]
(uj,vj,1)
t
＝r(ui,vi,1)
t
+t
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)
[0148]
其中，(ui,vi)表示第i幀點特徵的位置，(uj,vj)表示第j幀點特徵的位置。
[0149]
線特徵參數化形式和變換表示為：
[0150][0151]
其中，ii表示第i幀線特徵，ij表示第j幀線特徵，n
c,j
表示第j幀圖像中線特徵ij上一點與原點組成平面的法向量，d
c,j
表示第j幀圖像中線特徵ij的方向向量。
[0152]
點線特徵所構建三角形面積比例可表示為：
[0153][0154]
其中，s1、s2為一張圖中由點線特徵構成的兩個三角形。
[0155]
基於公式(14)，ki、kj可以表示為：
[0156][0157]
其中，s
1,i
、s
2,i
表示第i幀圖像中點線特徵構成的兩個三角形，s
1,j
、s
2,j
表示第j幀圖像中點線特徵構成的兩個三角形。
[0158]
點線綜合約束特徵方程為：
[0159]epl
＝k
i-kjꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(16)
[0160]
代價方程(cost function)c_fun可以表示為：
[0161][0162]
其中，ρ(
·
)為cauchyloss函數，其形式為ρ(x)＝log(x+1)，表示點線融合的觀測協方差矩陣。通過公式(16)的約束，能有效約束短期內視覺約束造成的累積誤差，對於後端迴環檢測起到了有效的補充作用。
[0163]
步驟4.2、融合點線綜合特徵構建後端緊耦合優化函數。
[0164]
緊耦合後端優化函數的構建可以描述為：
[0165]
由點特徵的重投影誤差e
l
和線特徵的重投影誤差e
p
，構建基本的點線特徵緊耦合約束方程c
p+l
，即融合點線綜合特徵的緊耦合後端優化函數：
[0166][0167]
γ(x)＝log(x+1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(19)
[0168]
其中，和為點和線的重投影誤差，h表示點特徵個數的下標，j表示點特徵個數的下標，p
k,h
、l
k,j
表示點、線的觀測協方差矩陣，γ(
·
)為cauchyloss函數。選擇ceres優
化器求解優化函數c
p+l
，可以得到最優的相機位姿估計和特徵坐標估計，得到相機位姿估計即實現了目標的定位，利用特徵坐標估計生成地圖。
[0169]
當然，上述說明並非是對本發明的限制，本發明也並不僅限於上述舉例，本技術領域的技術人員在本發明的實質範圍內所做出的變化、改型、添加或替換，也應屬於本發明的保護範圍。