SLAM或VO中兩幀間恢復Rt的方法彙總

2021-08-29 23:39:56 字數 3531 閱讀 1985

a.對極約束

參考幀和當前幀的二維畫素點通過特徵點法拿到了匹配關係,那麼從各自光心出發的兩條射線交點即3d點,p和兩個光心構成了極平面,e1e1

e1\ e2e2

e2為極點,l1l1

l1\ l2l2

l2是極線(epipolar line)。

對極幾何必須要有正確的匹配關係,否則當前幀無法找到參考幀p1p1

p1對應的點在p2p2

p2,那樣的話就要用極線搜尋了。

對極幾何求r t的步驟是:

1.根據匹配的畫素點對和內參求出基礎矩陣f或者本質矩陣e。e=t^ r,只和運動有關,所以是我們求的本質,平移和旋轉各3自由度,單目尺度歧義性自由度-1,所以e有5個自由度,一般用八點法,8對點求;f還包括內參f=k

−t∗e

∗k−1

f=k^*e*k^

f=k−t∗

e∗k−

1.2.根據e或者r恢復r t。這裡一般用svd分解恢復出4個解,然後取點在相機前方的那個解。

b.單應矩陣

@輸入:參考幀和當前幀的2d點座標對(一般4對),內參

@輸出:運動恢復r t

homography描述兩個平面之間的對映關係,自由度為8。可以通過4對不共線的點得出。一般配合ransac。

退化(degenerate):特徵點共面or純旋轉,基礎矩陣的自由度下降(比如平移那塊沒了),就出現了退化。

orb中h 和f都算,取好的。

深度濾波器:用多次三角測量讓深度估計收斂

直接法時候沒有特徵點匹配,所以需要用極限搜尋,如dso中immaturepoint::traceon()。dso**中沒有過多提及,深度濾波器部分可以看svo比較詳細。

@輸入:*frame_ref, *frame_curr, r, t, 參考幀點畫素位置和深度估計區間, 內參

@輸出:當前幀對應點畫素位置,點的深度和不確定度。

這時候的前提是已知rt(可以是估計出來的,也可以是靜止模型、勻速模型、隨機模型之類的),思路是:

這時候只知道參考幀的點畫素座標和深度區間,也知道當前幀的極線,p1p1

p1深度區間投過來後限定了極線的搜尋範圍,於是在範圍內用塊匹配之類的方法評估ncc(normalized cross correlation)找到相似度最高的點,如果只把這個點當作最終結果是不行的,很容易陷入區域性極值。所以需要用濾波的方法多次搜尋直到收斂。所以過程是:

1.假設畫素的深度滿足高斯分布,通過極限搜尋確定當前幀畫素點對應位置

2.根據幾何關係三角化計算深度和幾何不確定性(dso還加上了光度不確定性)

3.根據深度和不確定性融合進上一次的估計中,dso中是gn迭代,直到深度收斂。

@輸入:參考幀和當前幀的2d點座標對,恢復出的r t

@輸出:空間點的位置(即參考幀和當前幀點對應的深度s1 s2)

在用2d-2d的方法恢復出rt後,還需要用運動估計點的空間位置。

理論上兩個射線會交於點p,但無論是匹配誤差還是運動估計誤差等雜訊都會導致無法相交,所以一般用最小二乘的思路解點的位置。最後求得兩幀系下的位置深度s1和s2。三角測量必須要有一定的平移。(csdn裡面 \^{}打不出反協號)

s 1∗

x1νx

1=0=

s2∗x

1ν∗r

∗s2+

x1νt

s_1*x_1 ^ \nu x_1=0=s_2 * x_1 ^\nu *r * s_2+x_1^\nu t

s1​∗x1

ν​x1

​=0=

s2​∗

x1ν​

∗r∗s

2​+x

1ν​t

@輸入:參考幀下的特徵點畫素座標和3d座標,匹配到當前幀的2d點畫素座標,內參

@輸出:運動估計r t

perspective-n-point,當知道了n個空間點和其投影位置的情況下求解位姿。3d-2d比2d-2d簡單的多,最少只需要3對點,不過他需要特徵點的3d位置,可以由三角化or rgb-d or lidar提供,因此在雙目和rgb-d中就很方便,單目中要先初始化後才能用pnp。需要注意的是:3d點的位置應該是在參考幀座標系下的,實際上等價於在世界座標系下已知(因為參考幀對世界系的t已知),而當前幀的空間座標是不知道的,只知道參考幀特徵點對應的2d畫素座標,否則就成了3d-3d icp了。

pnp有很多方法,一般先用p3p/epnp估計乙個初值,然後用非線性優化的方法即ba refine:

當前已知p1 p2座標和p在左圖下的3d點,這時候ba優化重投影誤差,不斷更新r t,讓p在右圖的投影p2^盡量接近p2,即可用g-n l-m等優化方法求出r t。(這裡可以看看其中的雅可比推導),ba的目標函式:

ξ ∗=

arg⁡

min⁡ξ1

2∑∥u

i−1s

ikex

p(ξν

)pi∥

22\xi^* = \mathop_ \ \ \frac \sum \| u_i - \frackexp(\xi^\nu)p_i \|^2_2

ξ∗=arg

minξ​2

1​∑∥

ui​−

si​1

​kex

p(ξν

)pi​

∥22​

即重投影誤差,u是當前幀畫素座標即p2,pi是參考幀下的3d點齊次座標,這裡的1/s實際上就是在k投影前將當前系下估計出的3d座標歸一化,所以變數只有李代數。

@輸入:配對好的一組3d點對,即那坨點雲分別在參考幀下和當前幀下的座標

@輸出:運動恢復r t

這時候不用輸入相機內參,3d-3d和相機模型無關,所以lidar也可以直接用,視覺和lidar的icp區別是:視覺可以通過特徵點去匹配2d點然後找到對應的3d點對,但是lidar有時比較難匹配特徵點對,就認為距離最近的兩個點是同乙個點,即最近鄰,當然這個點也是有一定的幾何特徵的特徵點;當然lidar也可以用強度匹配之類的。

ba的目標函式:p p』為兩對輸入,變數為李代數

ξ ∗=

arg⁡

min⁡ξ1

2∑∥p

i−ex

p(ξν

)pi′

∥22\xi^* = \mathop_ \ \ \frac \sum \| p_i - exp(\xi^\nu)p_i ' \|^2_2

ξ∗=arg

minξ​2

1​∑∥

pi​−

exp(

ξν)p

i′​∥

22​

vue兩個元件間值的傳遞或修改方式

1 可以用公共的父元件來實現 2 可以在store.js裡面在設定公共變數 3 也可以用本地儲存localstorage.setitem 和localstorage.getitem 通過修改本地儲存的值和獲取修改後的值來實現 4 就是父子元件間的值的傳遞與修改props,這裡需要注意的是子元件裡面不...

陣列中兩兩異或大於m的個數

給定整數m以及n各數字a1,a2,an,將數列a中所有元素兩兩異或,共能得到n n 1 2個結果,請求出這些結果中大於m的有多少個 3.adigit 1,mdigit 0時,字典中第k位為0,異或結果為1,與對應分支所有數異或,結果都會大於m,第k位為1,異或結果為0,遞迴獲得結果 4.adigit...

python中兩個不同shape的陣列間運算規則

宣告 本部落格討論的陣列間運算是指四則運算,如 a b a b a b a b,不包括 a.dot b 等運算,由於 numpy 和 tensorflow 中都遵循相同的規則,本部落格以 numpy 為例。眾所周知,相同 shape 的兩個陣列間運算是指兩個陣列的對應元素相加。我們經常會碰到一些不同...