當前位置：首頁 > 人文社科 > 生活经验 >内容正文

生活经验

svo_note

發布時間：2023/11/27 生活经验 30 豆豆

生活随笔收集整理的這篇文章主要介紹了 svo_note 小編覺得挺不錯的,現在分享給大家,幫大家做個參考.

SVO論文筆記

- 1.frame overviews
- 2. Motion Estimate Thread
- - 2.1 Sparse Model-based Image Alignment 基于稀疏點亮度的位姿預估
  - 2.2 Relaxation Through Feature Alignment 基于圖塊的特征點匹配
  - 2.3 Pose and Structure Refinement
- 3 Mapping Thread
- - 3.1 depth-filter
  - 3.2 初始化
  - 參考：

1.frame overviews

SVO的整體框架如鏈接，可以看到整個SVO的結構是由兩個線程構成的，第一個線程有三個主要的操作，a. Sparse Model-based Image Alignment; b. Feature Alignment; c. Pose&Structure Refinement。第二個線程主要是map的過程，一旦有一幀被定義為keyframe，就提取其中的feature，并初始化深度濾波器，如果不是keyframe，則一直更新深度濾波器，如果某個feature的深度收斂了，則生成3D點放入Map中作為landmark。

2. Motion Estimate Thread

該線程中主要求解當前幀的位姿(Sparse Model-based Image Alignment; Feature Alignment)和local BA(Pose&Structure Refinement). svo 方法中motion estimation的步驟可以簡單概括如下:

對稀疏的特征塊使用direct method 配準，獲取相機位姿；
通過獲取的位姿預測參考幀中的特征塊在當前幀中的位置，由于深度估計的不準導致獲取的位姿也存在偏差，從而使得預測的特征塊位置不準。由于預測的特征塊位置和真實位置很近，所以可以使用牛頓迭代法對這個特征塊的預測位置進行優化。
特征塊的預測位置得到優化，說明之前使用直接法預測的有問題。利用這個優化后的特征塊預測位置，再次使用直接法，對相機位姿(pose)以及特征點位置（structure）進行優化

2.1 Sparse Model-based Image Alignment 基于稀疏點亮度的位姿預估

該部分類似于光流，不過求解的不是對應關系，而是相對位姿。

直接法具體過程如下：
??step1. 準備工作。假設相鄰幀之間的位姿 $T_{k,k?1}$ 已知，一般初始化為上一相鄰時刻的位姿或者假設為單位矩陣。通過之前多幀之間的特征檢測以及深度估計，我們已經知道第k-1幀中特征點位置以及它們的深度。
??step2. 重投影。知道 $I_{k?1}$ 中的某個特征在圖像平面的位置(u,v)，以及它的深度d，能夠將該特征投影到三維空間 $p_{k?1}$ ，該三維空間的坐標系是定義在 $I_{k?1}$ 攝像機坐標系的。所以，我們要將它投影到當前幀 $I_{k}$ 中，需要位姿轉換 $T_{k,k?1}$ ，得到該點在當前幀坐標系中的三維坐標 $p_k$ 。最后通過攝像機內參數，投影到 $I_{k}$ 的圖像平面 $(u', v')$ ，完成重投影。
??step3. 迭代優化更新位姿。按理來說對于空間中同一個點，被極短時間內的相鄰兩幀拍到，它的亮度值應該沒啥變化。但由于位姿是假設的一個值，所以重投影的點不準確，導致投影前后的亮度值是不相等的。不斷優化位姿使得這個殘差最小，就能得到優化后的位姿 $T_{k,k?1}$ 。
??將上述過程公式化如下：通過不斷優化位姿 $T_{k,k?1}$ 最小化殘差損失函數。其優化函數為:
$T_{k?1}^k=argmin?_?ρ[δI(T,u)]du$
其中:

$ρ[?]=0.5∥?∥^ 2$ ，可見整個過程是一個最小二乘法的問題；
$δI ( T , u ) = I _k(π ( T* π ^{? 1} ( u , d u ) )) ? I k ? 1 ( u )$ ，這個殘差對比的是圖像位置上的像素值的灰度，其中u ∈ ? ,?表示即可以在k-1幀圖像上看到，又可以通過投影在k幀上看到
對于比對灰度值的算法來說，一般都會用一個patch size中的全部像素的灰度進行對比，因此下面的公式中都不僅僅使用一點像素，而是使用多點像素進行求解的，但是在這個過程中，作者為了提高計算的速度，并沒有進行patch的投影，算是以量取勝吧

NOTE:公式中 $π ^{? 1} ( u , d u )$ 為根據圖像位置和深度逆投影到三維空間，第二步 $T* π ^{? 1} ( u , d u )$ 將三維坐標點旋轉平移到當前幀坐標系下，第三步 $π ( T* π ^{? 1} ( u , d u ) )$ 再將三維坐標點投影回當前幀圖像坐標。當然在優化過程中，殘差的計算方式不止這一種形式：有前向(forwards)，逆向(inverse)之分，并且還有疊加式(additive)和構造式(compositional)之分。這方面可以讀讀光流法方面的論文，Baker的大作《Lucas-Kanade 20 Years On: A Unifying Framework》。選擇的方式不同，在迭代優化過程中計算雅克比矩陣的時候就有差別，一般為了減小計算量，都采用的是inverse compositional algorithm。

優化目標函數：

把上述的notation帶入到優化函數中就可以得到
$T_{k?1}^k=argmin∑_{i∈?}1/2∥δI(T_{k?1}^k,u_i)∥^2$
其中雅克比矩陣為圖像殘差對李代數的求導，可以通過鏈式求導得到：
$J=?δI(ξ,ui)?ξ=?Ik?1(a)?a∣a=ui.?π(b)?b∣b=pi.?T(ξ)?ξ∣ξ=0.pi=?I?u.?u?b.?b?ξJ=\frac{\partial\delta I(\xi,u_i)}{\partial\xi} = \frac{\partial I_{k-1}(a)}{\partial a}|_{a=u_{i}}.\frac{\partial π(b)}{\partial b}|_{b=p_{i}}.\frac{\partial T(\xi)}{\partial \xi}|_{\xi=0}.p_i = \frac{\partial I}{\partial u}. \frac{\partial u}{\partial b}. \frac{\partial b}{\partial \xi}$
對于上一幀的每一個特征點，都進行這樣的計算，在自己本來的層數上，取那個特征點左上角的 4x4 圖塊。如果特征點映射回原來的層數時，坐標不是整數，就進行插值，其實，本來提取特征點的時候，在這一層特征點坐標就應該是整數。把圖塊往這一幀的圖像上的對應的層數投影，然后計算雅克比和殘差。計算殘差時，因為投影的位置并不剛好是整數的像素，所以會在投影點附近插值，獲取與投影圖塊對應的圖塊。最后，得到一個巨大的雅克比矩陣，以及殘差矩陣。但是為了節省存儲空間，提前就轉換成了 H 矩陣 $ H =J *J ^ T$ 。

上面的非線性最小化二乘問題，可以用高斯牛頓迭代法求解，位姿的迭代增量ξ(李代數)可以通過下述方程計算：
$JTJξ=?JTδI(0)Hξ=?JTδI(0)ξ=?H?1JTδI(0)J^TJξ =-J^T \delta I(0) \\ H \xi=-J^T \delta I(0) \\ \xi =-H^{-1}J^T \delta I(0) \\$
然后，得到 $T(ξ)T(\xi)$ ，逆矩陣得到 $T(ψ)T(\psi)$ ，再更新出 $T_{k,k-1}$ 。然后，在新的位置上，再從像素點坐標，投影出新的點 $p_{k-1}$ 。每一層迭代 30 次。因為這種 inverse-compositional 方法，用這種近似的思想，雅克比就可以不用再重新計算了。（因為重新投影出新的 p 點的位置，這個過程沒有在殘差公式里面表現出來。）這樣子逐層下去，重復之前的步驟。

對于每一個圖塊的每一個像素，它的雅克比計算如下。
$?I?u\frac{\partial I}{\partial u}$ ，是這個像素插值點在圖像上的梯度，就是水平右邊的像素點減去水平左邊的像素點，豎直下邊的像素點減去豎直上邊的像素點。如下圖所示。

$?u?b\frac{\partial u}{\partial b}$ 算的是投影雅克比
$\left[ \begin{matrix} b_x/b_z \\ b_y/b_z \\b_z/b_z \end{matrix} \right] =\left[ \begin{matrix} f_x &0 &c_x\\ 0 &f_y &c_y\\0 &0 &1 \end{matrix} \right] * \left[ \begin{matrix} b_x/b_z \\ b_y/b_z \\1 \end{matrix} \right] =\left[ \begin{matrix} f_xb_x/b_z +u_0\\ f_yb_y/b_z +v_0\\1 \end{matrix} \right]$
$?u?b=[fx/bz0?fx.bx/bz20fy/bz?fy.by/bz2]\frac{\partial u}{\partial b}= \left[ \begin{matrix} f_x/b_z & 0 &-f_x.b_x/b^2_z\\ 0 &f_y/b_z & -f_y.b_y/b^2_z \end{matrix} \right]$

$?b?ξ=?T(ξ)p?ξ=[I?p∧]=?(δ?∧+δρ)?[δρδ?]\frac{\partial b}{\partial \xi} =\frac{\partial T(\xi)p}{\partial \xi} =[I \quad -p^{\wedge}] =\frac{\partial (\delta \phi^{\wedge} + \delta \rho)}{\partial [\delta \rho \quad\delta \phi]}$
$\left[ \begin{matrix} 1 & 0 & 0 & 0 & p_z&-p_y\\ 0 & 1 & 0 & -p_z& 0 & p_x\\ 0 & 0 & 1 & p_y & -p_x& 0 \end{matrix} \right]$
為了方便計算，雖然 $=T(\xi) p$ ，但因為 $T(ξ)T(\xi)$ 是一個很小的擾動，所以可以認為 $=T(\xi) p$ ,

$?u?b=?u?T(ξ)p≈?u?p\frac{\partial u}{\partial b}=\frac{\partial u}{\partial {T(\xi) p}}\approx\frac{\partial u}{\partial p}$

所以，后兩項就可以相乘，統一用 p 來表示了。可以認為$ f =f_x=f_y$ 。

因此，

$?u?b=?b?ξ=[fx/bz0?fx.bx/bz20fy/bz?fy.by/bz2]?[1000pz?py010?pz0px001py?px0]\frac{\partial u}{\partial b} =\frac{\partial b}{\partial \xi} = \left[ \begin{matrix} f_x/b_z & 0 &-f_x.b_x/b^2_z\\ 0 &f_y/b_z & -f_y.b_y/b^2_z \end{matrix} \right] * \left[ \begin{matrix} 1 & 0 & 0 & 0 & p_z&-p_y\\ 0 & 1 & 0 & -p_z& 0 & p_x\\ 0 & 0 & 1 & p_y & -p_x& 0 \end{matrix} \right]$
$\left[ \begin{matrix} f_x/p_z & 0 & -f_x/p^2_z & -f_xp_xp_y/p^2_z & f_x+f_xp^2_x/p^2_z &-f_xp_y/p_z\\ 0 & f_y/p_z & -f_y/p^2_z & - f_y -f_yp^2_y/p^2_z& -f_yp_yp_y/p^2_z & f_yp_x/p_z \end{matrix} \right]$

$f(f_x 、f_y)$
對于每一個特征點，根據像素位置算出 $?I?u\frac{\partial I}{\partial u}$ ，再根據反投影出來的空間點位置 p 算出上式的左邊項，根據特征點所在的層數算出 f 。然后，相乘，就得到了一行雅克比矩陣。再根據 $H=J J ^ T $，$ J^TJξ =-J^T \delta I(0)$加到 H 矩陣和殘差矩陣上。

最后，在優化出 $ξ\xi$ 后，更新如下，
$Tk,k?1=Tk,k?1Tk?1,k?1=Tk,k?1Tψ=Tk,k?1T(ξ)?1T_{k,k-1}=T_{k,k-1}T_{k-1,k-1}=T_{k,k-1}T_{\psi}=T_{k,k-1}T(\xi)^{-1}$
注釋：但是在程序里面，直接就是 $Tk,k?1=Tk,k?1T(ξ)T_{k,k-1}=T_{k,k-1}T(\xi)$ 。可能是為了加快計算，認為 $T(ξ)?1≈Tk,k?1T(\xi)^{-1} \approx T_{k,k-1}$ 。在 sparse_align.cpp 的 307行

T_curnew_from_ref = T_curold_from_ref * SE3::exp(-x_); 這個地方， 為什么不是 T_curnew_from_ref = T_curold_from_ref *(SE3::exp(x_)).inverse()**需要以后研究一下。

到這里，我們已經能夠估計位姿了，但是這個位姿肯定不是完美的。導致重投影預測的特征點在中的位置并不和真正的吻合，也就是還會有殘差的存在。如下圖所示：

圖中灰色的特征塊為真實位置，藍色特征塊為預測位置。幸好，他們偏差不大，可以構造殘差目標函數，和上面直接法類似，不過優化變量不再是相機位姿，而是像素的位置 $(u', v')$ ，通過迭代對特征塊的預測位置進行優化。這就是svo中提到的Feature Alignment。

2.2 Relaxation Through Feature Alignment 基于圖塊的特征點匹配

通過第一步的幀間匹配能夠得到當前幀相機的位姿，但是這種frame to frame估計位姿的方式不可避免的會帶來累計誤差從而導致漂移。所以，應該通過已經建立好的地圖模型，來進一步約束當前幀的位姿。

地圖模型通常來說保存的就是三維空間點，因為每一個Key frame通過深度估計能夠得到特征點的三維坐標，這些三維坐標點通過特征點在Key Frame中進行保存。所以SVO地圖上保存的是Key Frame 以及還未插入地圖的KF中的已經收斂的3d點坐標（這些3d點坐標是在世界坐標系下的），也就是說地圖map不需要自己管理所有的3d點，它只需要管理KF就行了。

那選取KF的標準是啥？KF中保存了哪些東西？

比如，當新的幀new frame和相鄰KF的平移量超過場景深度平均值的12%時(比如四軸上升)，new frame就會被當做KF，它會被立即插入地圖。同時，又在這個新的KF上檢測新的特征點作為深度估計的seed，這些seed會不斷融合新的new frame進行深度估計。

但是，如果有些seed點3d點位姿通過深度估計已經收斂了，怎么辦？

map用一個point_candidates來保存這些尚未插入地圖中的點。所以map這個數據結構中保存了兩樣東西，以前的KF的3d點以及新的尚未插入地圖的KF中已經收斂的3d點。

通過地圖我們保存了很多三維空間點，很明顯，每一個new frame都是可能看到地圖中的某些點的。由于new frame的位姿通過上一步的直接法已經計算出來了，按理來說這些被看到的地圖上的點可以被投影到這個new frame中，即圖中的藍色方框塊。上圖中分析了，所有位姿誤差導致這個方框塊在new frame中肯定不是真正的特征塊所處的位置。所以需要Feature Alignment來找到地圖中特征塊在new frame中應該出現的位置，根據這個位置誤差為進一步的優化做準備。基于光度不變性假設，特征塊在以前參考幀中的亮度應該和new frame中的亮度差不多。所以可以重新構造一個殘差，對特征預測位置進行優化：
$ui′=argmin12∥Ik(ui′)?AiIr(ui)∥2u^′_i=argmin \frac{1}{2}∥I_k(u^′_i)?A_iI_r(u_i)∥^2$
注意這里的優化變量是像素位置，這過程就是光流法跟蹤。并且注意，光度誤差的前一部分是當前圖像中的亮度值，后一部分不是 $I_{k?1}$ 而是 $i_r$ ，即它是根據投影的3d點追溯到的這個3d點所在的key frame中的像素值，而不是相鄰幀。由于是特征塊對比并且3d點所在的KF可能離當前幀new frame比較遠，所以光度誤差和前面不一樣的是還加了一個仿射變換，需要對KF幀中的特征塊進行旋轉拉伸之類仿射變換后才能和當前幀的特征塊對比。
??這時候的迭代量計算方程和之前是一樣的，只不過雅克比矩陣變了，這里的雅克比矩陣很好計算：
$KaTeX parse error: Undefined control sequence: \matrix at position 12: J= \left[ \?m?a?t?r?i?x?{ \frac{\part…$
就是圖像橫縱兩個方向的梯度嘛。
??通過這一步我們能夠得到優化后的特征點預測位置，它比之前通過相機位姿預測的位置更準，所以反過來，我們利用這個優化后的特征位置，能夠進一步去優化相機位姿以及特征點的三維坐標。所以位姿估計的最后一步就是Pose and Structure Refinement。

2.3 Pose and Structure Refinement

在一開始的直接法匹配中，我們是使用的光度誤差，這里由于優化后的特征位置和之前預測的特征位置存在差異，這個能用來構造新的優化目標函數。
$Tw,k=argmin12∑i(∥ui?π(Tw,k,wpi)∥)2T_{w,k}=argmin \frac{1}{2}∑_i (∥u_i?π(T_{w,k},_wp_i)∥)^2$
這個部分就是一個BA，不過論文也說了，最后一步也會有一個local BA，修正位姿和3D點

上式中誤差變成了像素重投影以后位置的差異（不是像素值的差異），優化變量還是相機位姿，雅克比矩陣大小為2×6(橫縱坐標u，v分別對六個李代數變量求導)。這一步是就叫做motion-only Bundler Adjustment。同時根據根據這個誤差定義，我們還能夠對獲取的三維點的坐標(x,y,z)進行優化，還是上面的誤差像素位置誤差形式，只不過優化變量變成三維點的坐標，這一步叫Structure -only Bundler Adjustment，優化過程中雅克比矩陣大小為2×32×3(橫縱坐標u，vu，v分別對點坐標(x,y,z)變量求導)。

Discussion
論文討論了該方法中的前幾個步驟，討論了如果使用傳統的LK光流法的話，時間會增加，同時較大距離的跟蹤需要較大的patch以及金字塔的應用，同時因為會存在跟丟的情況，因此需要外點檢測，而在SVO的方法中，因為有feature alignment的步驟，使得特征點的位置還是比較準確的，因此能保證沒有外點。

3 Mapping Thread

該線程中主要的工作就是判斷當前幀是否可以作為key frame，之后進行深度濾波器的迭代以及初始化工作。下面主要說明深度濾波器的相關推導

3.1 depth-filter

最基本的深度估計就是三角化，這是多視角幾何的基礎內容(可以參看圣經Hartly的《Multiple View Geometry in Computer Vision》中的第十二章structure computation；可以參看我的相應博客)。我們知道通過兩幀圖像的匹配點就可以計算出這一點的深度值，如果有多幅圖像，那就能計算出這一點的多個深度值。這就像對同一個狀態變量我們進行了多次測量，因此，可以用貝葉斯估計來對多個測量值進行融合，使得估計的不確定性縮小。如下圖所示：

一開始深度估計的不確定性較大(淺綠色部分)，通過三角化得到一個深度估計值以后，能夠極大的縮小這個不確定性(墨綠色部分)。
??在這里，先簡單介紹下svo中的三角化計算深度的過程，主要是極線搜索確定匹配點。在參考幀Ir中，我們知道了一個特征的圖像位置，假設它的深度值在[dmin,dmax][dmin,dmax]之間，那么根據這兩個端點深度值，我們能夠計算出他們在當前幀Ik中的位置，如上圖中草綠色圓圈中的線段。確定了特征出現的極線段位置，就可以進行特征搜索匹配了。如果極線段很短，小于兩個像素，那直接使用上面求位姿時提到的Feature Alignment光流法就可以比較準確地預測特征位置。如果極線段很長，那分兩步走，第一步在極線段上間隔采樣，對采樣的多個特征塊一一和參考幀中的特征塊匹配，用Zero mean Sum of Squared Differences 方法對各采樣特征塊評分，那個得分最高，說明他和參考幀中的特征塊最匹配。第二步就是在這個得分最高點附近使用Feature Alignment得到次像素精度的特征點位置。像素點位置確定了，就可以三角化計算深度了。得到一個新的深度估計值以后，用貝葉斯概率模型對深度值更新。在LSD slam中，假設深度估計值服從高斯分布，用卡爾曼濾波(貝葉斯的一種)來更新深度值。這種假設中，他認為深度估計值效果很棒，很大的概率出現在真實值(高斯分布均值)附近。而SVO的作者采用的是Vogiatzis的論文《Video-based, real-time multi-view stereo》提到的概率模型，而在深度濾波器中，論文把真實深度的分布看做一個正態分布+均勻分布的模型

這個概率模型是一個高斯分布加上一個設定在最小深度dmindmin和最大深度dmaxdmax之間的均勻分布。這個均勻分布的意義是假設會有一定的概率出現錯誤的深度估計值。有關這個概率模型來由更嚴謹的論證去看看Vogiatzis的論文。同時，有關這個概率模型遞推更新的過程具體可以看Vogiatzis在論文中提到的Supplementary material，論文中告知了下載地址。知道了這個貝葉斯概率模型的遞推過程，程序就可以實現深度值的融合了，結合supplementary material去看svo代碼中的updateSeeds(frame)這個程序就容易了，整個程序里的那些參數的計算遞歸過程的推導，我簡單截個圖，這部分我也沒細看(公式19是錯誤的，svo作者指出了)，現在有幾篇博客對該部分進行了推導。詳見盧彥斌：svo原理解析,東北大學孫志明：svo的Supplementary matterial 推導過程](https://blog.csdn.net/u013004597/article/details/52069741)

在深度估計的過程中，除了計算深度值外，這個深度值的不確定性也是需要計算的，它在很多地方都會用到，如極線搜索中確定極線的起始位置和長度，如用貝葉斯概率更新深度的過程中用它來確定更新權重(就像卡爾曼濾波中協方差矩陣扮演的角色)，如判斷這個深度點是否收斂了，如果收斂就插入地圖等等。SVO的作者Forster作為第二作者發表的《REMODE: Probabilistic, Monocular Dense Reconstruction in Real Time》中對由于特征定位不準導致的三角化深度誤差進行了分析，如上圖。
它是通過假設特征點定位差一個像素偏差，來計算深度估計的不確定性。具體推導見原論文，簡單的幾何關系。

3.2 初始化

? SVO的初始化過程：它假設前兩個關鍵幀所拍到的特征點在一個平面上(四軸飛行棋對地面進行拍攝)，然后估計單應性H矩陣，并通過三角化來估計初始特征點的深度值。SVO初始化時triangulation的方法具體代碼是vikit/math_utils.cpp里的triangulateFeatureNonLin()函數，使用的是中點法，關于這個三角化代碼算法的推導見github issue。

還有就是SVO適用于攝像頭垂直向下的情況（也就是無人機上,垂直向上也可以,朝著一面墻也可以），為什么呢？

1.初始化的時候假設的是平面模型
2.KF的選擇是個極大的限制，除了KF的選擇原因外攝像頭水平朝前運動的時候，SVO中的深度濾波做的不好，這個討論可以看看github issue，然而在我的測試中，不知道修改了哪些參數，稍微改動了部分代碼，發現前向運動，并且對著非平面SVO也是很溜的。

參考：

[1] http://blog.csdn.net/heyijia0327
[2] https://blog.csdn.net/heyijia0327/article/details/51083398
[3] https://blog.csdn.net/u013004597/article/details/52069741
[4] SVO論文翻譯總結
[5] http://rpg.ifi.uzh.ch/svo_pro.html
[6] https://www.cnblogs.com/wxt11/p/7097250.html

總結

以上是生活随笔為你收集整理的svo_note的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。

svo_note

上一篇：狗狗驱虫一次多少钱
下一篇：【Boost】noncopyable：不