QQ群「3D視覺研習社」，群號：574432628

一 前言

本文承接ORB-SLAM3 細讀單目初始化過程(上)，ORBSLAM3單目視覺有很多知識點需要展開和深入，初始化過程是必然要經歷的，而網上資料不夠系統，因此本文主旨是從程式碼實現出發，把初始化過程系統化，建立起知識樹，以把零碎的知識點串聯起來，方便快速學習提升自己。注意，本文雖然從程式碼出發，但並非講全部程式碼細節，如有需要建議直接看原始碼，地址是：https://github.com/UZ-SLAMLab/ORB_SLAM3，我自己稍微做了點修改，可以跑資料集的版本，可以參考一下，地址是：https://github.com/shanpenghui/ORB_SLAM3_Fixed

二 初始化主要函式

ORBSLAM單目視覺SLAM的追蹤器介面是函式TrackMonocular，呼叫了GrabImageMonocular，其下面有2個主要的函式：Frame::Frame()和Tracking::Track()，本文和上篇都是按照以下框架流程來分解單目初始化過程，上篇記錄了Frame::Frame()，本文就記錄Tracking::Track()。

1 Tracking作用

ORB-SLAM3的Tracking部分作用論文已提及，包含輸入當前幀、初始化、相機位姿跟蹤、區域性地圖跟蹤、關鍵幀處理、姿態更新與儲存等，如圖。

2 兩個主要函式

單目地圖初始化函式是Tracking::MonocularInitialization，其主要是呼叫以下兩個函式完成了初始化過程，ORBmatcher::SearchForInitialization和KannalaBrandt8::ReconstructWithTwoViews，前者用於參考幀和當前幀的特徵點匹配，後者利用構建的虛擬相機模型，針對不同相機計算基礎矩陣和單應性矩陣，選取最佳的模型來恢復出最開始兩幀之間的相對姿態，並進行三角化得到初始地圖點。

三 ORBmatcher::SearchForInitialization

這個旋轉的角度哪來的呢？就是在計算描述子的時候算的，呼叫函式IC_Angle,程式碼是：ORBextractor.cc#L475

keypoint->angle = IC_Angle(image, keypoint->pt, umax);

感興趣的同學想知道為什麼要這麼麻煩的選取最優3個角度，請從旋轉不變性開始理解，原理參見：3-5-3如何保證描述子旋轉不變性?

（https://blog.csdn.net/shanpenghui/article/details/109809723#t20）

四 KannalaBrandt8::ReconstructWithTwoView1 畸變校正

利用魚眼模型，對兩幀影象的特徵點進行畸變校正，程式碼見KannalaBrandt8.cpp#L219。要注意的是，魚眼模型的特殊性在於只考慮徑向畸變，忽略切向畸變，所以其p_ipi值都是0。想要深入理解魚眼模型的同學可以參考這篇文章《魚眼相機成像模型》

(https://blog.csdn.net/u010128736/article/details/52864024)。ORB-SLAM3中對不同模型相機的畸變校正做了區分，當相機模型是針孔的時候，用的畸變校正引數是mDistCoef，當相機模型是魚眼的時候，用的是虛擬出的相機類，程式碼參見mpCamera = new KannalaBrandt8(vCamCalib)，為避免重複校正，用了個條件限制，就是在函式Frame::UndistortKeyPoints中判斷mDistCoef.at<float>(0)==0.0，程式碼參見Frame.cc#L734，因為在用魚眼相機模型的時候，mDistCoef沒有賦值，都是0。

cv::fisheye::undistortPoints(vPts1,vPts1,K,D,R,K);cv::fisheye::undistortPoints(vPts2,vPts2,K,D,R,K);

2 位姿估計

主要由函式TwoViewReconstruction::Reconstruct完成，涉及到的知識點又多又關鍵的，包括對極約束、八點法、歸一化、直接線性變換、卡方檢驗、重投影等，先從主要流程開始理解。

利用隨機種子DUtils::Random::SeedRandOnceTwoViewReconstruction.cc#L79，在所有匹配特徵點對中隨機選擇8對匹配特徵點為一組for(size_t j=0; j<8; j++)TwoViewReconstruction.cc#L86，用於估計H矩陣和F矩陣。將當前幀和參考幀中的特徵點座標進行歸一化。TwoViewReconstruction::NormalizeTwoViewReconstruction.cc#L753用DLT方法求解F矩陣 TwoViewReconstruction::ComputeF21TwoViewReconstruction.cc#L273對給定的F矩陣打分,需要使用到卡方檢驗的知識 TwoViewReconstruction::CheckFundamentalTwoViewReconstruction.cc#L395利用得到的最佳模型（選擇得分較高的矩陣值，單應矩陣H或者基礎矩陣F）估計兩幀之間的位姿，程式碼中對應著函式ReconstructH或ReconstructF。其中，分兩個步驟。第一是利用基礎矩陣F和本質矩陣E的關係，計算出四組解。第二是呼叫的函式CheckRT作用是用R，t來對特徵匹配點三角化，並根據三角化結果判斷R,t的合法性。最終可以得到最優解的條件是位於相機前方的3D點個數最多並且三角化視差角必須大於最小視差角。2.2.4 基礎矩陣Fundamental 代數推導

有了以上的示意，我們嘗試用數學公式描述極點、極線和極平面之間的關係。看了好幾篇文章，感覺還是視覺十四講裡面的代數推導比較明晰，我就直接參考過來，當做記錄了，其他比較雜亂，記錄在《SLAM 學習筆記 本質矩陣E、基礎矩陣F、單應矩陣H的推導》（https://blog.csdn.net/shanpenghui/article/details/110133454），感興趣的同學可以看看。

設以第一個相機作為座標系三維空間的點：

2.2.6 結尾

由於知識有限，加上篇幅限制，就不再展開了，這裡可以參考另外幾篇比較好的文章，有比較詳細的推導過程，想深入研究的童鞋可以看看。

1、SLAM入門之視覺里程計(3)：兩檢視對極約束 基礎矩陣

2、SLAM基礎知識總結（https://blog.csdn.net/MyArrow/article/details/53704339）

五、總結

單目方案的初始化過程再梳理一下：

對極約束是原理基礎，從物理世界出發描述了整個視覺相機成像、資料來源以及相互關係的根本問題，其中印象最深的是把搜尋匹配點的範圍縮小成一段極線，大大加速了匹配過程。八點法從求解的角度出發，用公式描述了獲得我們想要的解的最小條件，提供了有力的數學基礎。歸一化使影象進行縮放，而縮放尺度是為了讓噪聲對於影象的影響在一個數量級上，從而減少噪聲對影象的影響。直接線性轉換則從諸多的測量值中（超過8點的N個匹配點，超定方程）算出了最優的解，使我們基本得到了想要的解。在已經有的粗解基礎上利用統計學方法進行分析，篩選出優質的點（符合機率模型的內點）來構成我們最終使用的一個投影的最優解，利用兩幀影象上匹配點對進行相互投影，綜合判斷內外點，從而最小化誤差。篩選出內外點之後，對兩個模型進行打分，選出最優模型，然後透過三角化測量進行深度估計，最終完成初始化過程。

至此，單目的初始化過程（基於基礎矩陣F）就完啦，內容較多，希望不對的地方多多指教，相互學習，共同成長。以上僅是個人見解，如有紕漏望各位指出，謝謝。

參考：

1.對極幾何及單應矩陣https://blog.csdn.net/u012936940/article/details/80723609

2.2D-2D：對極約束https://blog.csdn.net/u014709760/article/details/88059000

3.多檢視幾何

https://blog.csdn.net/weixin_43847162/article/details/89363281

4.SVD分解及線性最小二乘問題

https://www.cnblogs.com/houkai/p/6656894.html

5.矩陣SVD分解（理論部分II——利用SVD求解最小二乘問題）

https://zhuanlan.zhihu.com/p/64273563

6.奇異值分解(SVD)原理詳解及推導

https://blog.csdn.net/zhongkejingwang/article/details/43053513

7.最小二乘解（Least-squares Minimization ）

https://blog.csdn.net/kokerf/article/details/72437294

8.卡方檢驗 （Chi-square test / Chi-square goodness-of-fit test)

https://blog.csdn.net/zfcjhdq/article/details/83512680?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-3.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-3.control

9.樣本標準差與自由度 n-1 卡方分佈關係的證明

https://blog.csdn.net/robert_chen1988/article/details/90640917

10.證明殘差平方和除隨機項方差服從卡方分佈

https://www.docin.com/p-1185555448.html

11.本質矩陣最佳化分解的相對位姿估計

http://www.doc88.com/p-6931350248387.html

12.單目移動機器人的相對位姿估計方法

https://www.doc88.com/p-7744747222946.html

13.三角化求深度值（求三位座標）

https://blog.csdn.net/michaelhan3/article/details/89483148

備註：作者也是我們「3D視覺從入門到精通」特邀嘉賓：一個超乾貨的3D視覺學習社群