SVO代碼（一）從頭到尾

閱讀SVO代碼過程中主要參考的下面兩篇博客:
svo： semi-direct visual odometry 論文解析
SVO詳細解讀

基於幀間4x4的圖像塊的灰度不變形來優化相機位姿，這與直接法很像，直接法使用的是一個像素點，接着使用8x8的光流進一步優化關鍵點的位置，經過這一步特徵點的位置就很精確了，最後通過構造重投影誤差優化相機的位姿和路標點的深度，這與特徵點法很像，SVO結合了直接法和特徵點法，因此，稱它爲半直接法

---

生成新feature的地方只有三個地方：

• processSecondFrame()->addSecondFrame()
• processFrame()->reprojectMap()->reprojectCell()
• initializeSeeds()->detect()

---

1、初始化

1、processFirstFrame()不向frame中添加feature，只負責將檢測到的feature存到px_ref_（feature的像素座標）、f_ref_（feature對應的單位向量）中

feature的屬性：

FeatureType type; //feature的類型：corner or edgelet
Frame* frame;     //feature所在的frame
Vector2d px;      //在0層的像素座標系上座標
Vector3d f;       //在單位圓上的向量，並非歸一化平面座標
int level;        //feature在哪一個層檢測到的
Point* point;     //這個feature對應的MapPoint座標
Vector2d grad;    //edgelet的梯度，歸一化後的

addFirstFrame()中將px_ref_轉存到px_cur_的目的是：爲addSecondFrame()中的trackKlt()初始化一個好的值

setKeyframe()是將普通幀設置成關鍵幀，這裏跟orb-slam中不一樣，並沒有新生成一個keyframe對象，而是通過設置關鍵幀標誌位is_keyframe_來實現，此外，也爲該關鍵幀選取5個有代表性keypoints，在後面用於processFrame()->reprojectMap()->getCloseKeyframes()中得到有共視關係的關鍵幀

2、在processSecondFrame中使用multi-scale Lucas-Kanade算法計算稀疏光流，進行跟蹤

• 如果跟蹤成功的角點數超過50個並且disparity的中值大於50個像素長度，則計算單應矩陣，三角化生成新的MapPoints
• 調整scale，orb-slam中也有這步，在這修改以使用里程計恢復初始尺度
• 然後就可以向frame_cur和frame_ref中添加feature(都是位於0層，有些feature在其它層提取，也不加考慮了？？），同時向point添加能觀測到這個point的feature

使用twoViewBA()優化第二個關鍵幀的位姿(第一個關鍵幀固定)和MapPoints的位置，這裏使用的是重投影誤差
將new_frame_（第二個關鍵幀）加入到深度濾波器

---

2、運動估計

1、Sparse Model-based Image Alignment
圖像的稀疏對齊操作的是兩個普通的幀: last_frame_和 new_frame_，從max_level_=4到max_level_=2中的每一層進行遍歷優化T_cur_from_ref，下面是優化的過程：

• 預計算雅可比矩陣（使用 inverse compositional，故只需計算一次），precomputeReferencePatches()遍歷參考幀（last_frame_）中的每一個feature，即使這個feature不是在這一層提取的也這麼遍歷（這樣沒問題嗎！？）
• 計算殘差，computeResiduals()根據位姿變換將last_frame_上的feature（普通幀的feature哪裏來的，看下面），投影到當前幀像素平面上，計算亞像素值用於計算殘差，這裏使用的是最小化“像素塊”灰度值差值

2、Feature Alignment
上一步已經調整過T_cur_from_ref，那麼就可以進行feature對齊了，爲了準確（因爲關鍵幀中的point都是比較準確的），使用的<當前幀>和<與當前有共視關係的關鍵幀>，調整的是當前幀feature（初始位置：由共視關鍵幀對應的point投影得到）的位置
1）使用創建關鍵幀時生成的5個keyPoints投影進行尋找，當前幀與Map中存儲的關鍵幀有共視的關鍵幀，取前10個共視最好的關鍵幀
2）遍歷每一個柵格(grid_size=30）中每一個地圖點，grid_.cells中的每一個cell包括兩個成員變量：point（由關鍵幀觀測到）、point在當前幀上的投影，注意這裏不含feature，因爲這一步只是尋找point而已，feature要在下一步通過getCloseViewObs()確定
3）針對每一個地圖點，通過優化尋找直接法匹配，即使用findMatchDirect()

• 使用getCloseViewObs()，選出夾角最小的那個關鍵幀作爲參考關鍵幀，以及對應的feature: ref_ftr_
• 計算從參考關鍵幀到當前幀的仿射變換，爲什麼？這是因爲如果幀間圖像發生了旋轉，我們還用同樣的窗口，將出錯，這等價於給fast（其實是圖像塊）加上旋轉不變性！！
• 確定search_level_，爲什麼？這是因爲當前幀很大可能與參考關鍵幀的距離比較遠，所以feature的大小很有可能發生變化，這就等價於給fast角點（其實是圖像塊）加上尺度不變性？！

4）仿射變換矩陣是A_ref_cur，表示從當前幀的<0層>轉換到參考關鍵幀的<0層>的仿射變換，warpAffine()用於將在當前幀的search_level_層上取到的10x10的圖像塊，投影到feature<提取層>對應的參考關鍵幀ref_ftr_->frame->img_pyr_[ref_ftr_->level]上，然後就可以使用align2D()或者align1D（edgelet特徵）進行優化，對齊feature，使用的同樣是 inverse compositional，可能是因爲太好用了吧.
5）根據上一步的結果篩選point
6）如果findMatchDirect()匹配成功了，則向當前幀添加feature，通過下面代碼生成新的feature，使用的層是search_level_，search_level_是針對每一個point在findMatchDirect()確定

Feature* new_feature = new Feature(frame.get(), it->px, matcher_.search_level_);

3、Pose and Structure Refinement
1）optimizeGaussNewton()使用重投影誤差優化當前幀的位姿frame->T_f_w_，觀測是上一步生成featrue在當前幀歸一化平面上的座標，頂點當前幀的位姿（沒有structrue）.
使用的redescending M-estimators魯棒核函數，好處如下：

The redescending M-estimators are slightly more efficient than the Huber estimator for several symmetric, wider tailed distributions

優化結束後，計算位姿的協方差. 假設，在對應的層數上，測量值的協方差都爲1個像素（orb-slam中不是這麼做的，而是根據特徵點在哪個層，就乘以相應的尺度，尺度越大，不確定性越大），即測量值滿足方差爲1的高斯分佈，由高斯牛頓可得：
$J\xi=\delta \ \ \ \ \ \Rightarrow \ \ \ \ \ \xi=(J^TJ)^{-1}J^T\delta$
由協方差的傳遞律可得，$\xi$ 的協方差爲:
$P_{\xi}=((J^TJ)^{-1}J^T)P_{\delta}((J^TJ)^{-1}J^T)^T=(J^TJ)^{-1}J^TJ(J^TJ)^{-T}=(J^TJ)^{-1}$
這也是g2o裏面計算狀態變量的協方差，使用computeMarginals()函數，注意，它計算返回的是協方差而不是信息矩陣.
2）使用optimizeStructure()優化point的位置

---

3、深度濾波

1、DepthFilter新進來一個關鍵幀，則需要生成新的seed（種子）

• 新的關鍵幀到來時，則中斷updateSeeds()，因爲這時有很大可能正在處理普通幀，而不是關鍵幀，所以就丟掉了，即，關鍵幀優先
• setExistingFeatures()設置包含feature的柵格爲佔據狀態，這時的feature只來自與相鄰關鍵幀匹配得到，所以不需要通過深度濾波器再優化了
• 使用feature_detector_->detect()檢測新的feature，然後加入到seeds_中

seed的屬性如下，每一個seed包括一個新生成的feature

Seed::Seed(Feature* ftr, float depth_mean, float depth_min) :
    batch_id(batch_counter),
    id(seed_counter++),
    ftr(ftr),                  //對應的feature
    a(10),
    b(10),
    mu(1.0/depth_mean),        //均值，深度的倒數
    z_range(1.0/depth_min),    //深度範圍爲當前幀的最近的深度的倒數
	    sigma2(z_range*z_range/36) //方差
{}

2、使用updateSeeds()遍歷每一個seed

1）首先檢測該seed在當前幀中是否可見，
2）接着使用findEpipolarMatchDirect()處理這個seed，

• 對於邊緣特徵（Feature::EDGELET），如果把梯度仿射過來後，梯度的方向與極線方向的夾角大於45度，就認爲沿着極線找，圖塊像素也不會變化很大，就不搜索了，直接返回false
• 沿着極線方向搜索，在當前幀上找到與ref_ftr對應的圖像塊最匹配的圖像塊，接着使用align2D()進一步優化，得到更爲精準的px_cur_，而後就可以使用seed的對應的ref_ftr與px_cur_進行三角化，得到3D point，也就得到了待估計的深度（depth）

3）上一步得到深度，接下來使用updateSeed()得到深度的協方差，這裏要參考論文:《 Video-based, Real-Time Multi View Stereo》
4）如果協方差小於閾值，就認爲收斂了，它就不再是種子點，而是candidate點，使用回調函數，加入到候選點隊列中

seed_converged_cb_(point, it->sigma2);

候選point類型如下

typedef pair<Point*, Feature*> PointCandidate;
typedef list<PointCandidate> PointCandidateList;

5）如果第2步匹配成功，則得到px_cur_，如果當前幀是關鍵幀的話，就將matcher_.px_cur_所在的柵格設置爲佔據狀態

---

<完>
@leatherwang

---