來(lái)源：公眾號(hào)|計(jì)算機(jī)視覺工坊（系投稿）

作者：?jiǎn)滩凰?/p>

「3D視覺工坊」技術(shù)交流群已經(jīng)成立，目前大約有12000人，方向主要涉及3D視覺、CV&深度學(xué)習(xí)、SLAM、三維重建、點(diǎn)云后處理、自動(dòng)駕駛、CV入門、三維測(cè)量、VR/AR、3D人臉識(shí)別、醫(yī)療影像、缺陷檢測(cè)、行人重識(shí)別、目標(biāo)跟蹤、視覺產(chǎn)品落地、視覺競(jìng)賽、車牌識(shí)別、硬件選型、學(xué)術(shù)交流、求職交流、ORB-SLAM系列源碼交流、深度估計(jì)等。工坊致力于干貨輸出，不做搬運(yùn)工，為計(jì)算機(jī)視覺領(lǐng)域貢獻(xiàn)自己的力量！歡迎大家一起交流成長(zhǎng)~

添加小助手微信：CV_LAB，備注學(xué)校/公司+姓名+研究方向即可加入工坊一起學(xué)習(xí)進(jìn)步。

QQ群「3D視覺研習(xí)社」，群號(hào)：574432628

一 前言

本文承接ORB-SLAM3 細(xì)讀單目初始化過程(上)，ORBSLAM3單目視覺有很多知識(shí)點(diǎn)需要展開和深入，初始化過程是必然要經(jīng)歷的，而網(wǎng)上資料不夠系統(tǒng)，因此本文主旨是從代碼實(shí)現(xiàn)出發(fā)，把初始化過程系統(tǒng)化，建立起知識(shí)樹，以把零碎的知識(shí)點(diǎn)串聯(lián)起來(lái)，方便快速學(xué)習(xí)提升自己。注意，本文雖然從代碼出發(fā)，但并非講全部代碼細(xì)節(jié)，如有需要建議直接看源代碼，地址是：https://github.com/UZ-SLAMLab/ORB_SLAM3，我自己稍微做了點(diǎn)修改，可以跑數(shù)據(jù)集的版本，可以參考一下，地址是：https://github.com/shanpenghui/ORB_SLAM3_Fixed

二 初始化主要函數(shù)

ORBSLAM單目視覺SLAM的追蹤器接口是函數(shù)TrackMonocular，調(diào)用了GrabImageMonocular，其下面有2個(gè)主要的函數(shù)：Frame::Frame()和Tracking::Track()，本文和上篇都是按照以下框架流程來(lái)分解單目初始化過程，上篇記錄了Frame::Frame()，本文就記錄Tracking::Track()。

1 Tracking作用

ORB-SLAM3的Tracking部分作用論文已提及，包含輸入當(dāng)前幀、初始化、相機(jī)位姿跟蹤、局部地圖跟蹤、關(guān)鍵幀處理、姿態(tài)更新與保存等，如圖。

2 兩個(gè)主要函數(shù)

單目地圖初始化函數(shù)是Tracking::MonocularInitialization，其主要是調(diào)用以下兩個(gè)函數(shù)完成了初始化過程，ORBmatcher::SearchForInitialization和KannalaBrandt8::ReconstructWithTwoViews，前者用于參考幀和當(dāng)前幀的特征點(diǎn)匹配，后者利用構(gòu)建的虛擬相機(jī)模型，針對(duì)不同相機(jī)計(jì)算基礎(chǔ)矩陣和單應(yīng)性矩陣，選取最佳的模型來(lái)恢復(fù)出最開始兩幀之間的相對(duì)姿態(tài)，并進(jìn)行三角化得到初始地圖點(diǎn)。

三 ORBmatcher::SearchForInitialization

這個(gè)函數(shù)的主要作用是構(gòu)建旋轉(zhuǎn)角度直方圖，選取最優(yōu)的三個(gè)Bin，也就是占據(jù)概率最大的三個(gè)Bin，如圖（數(shù)字3被異形吃掉了^-^）。因?yàn)楫?dāng)前幀會(huì)提取到諸多特征點(diǎn)，每一個(gè)都可以作為圖像旋轉(zhuǎn)角度的測(cè)量值，我們希望能在諸多的角度值中，選出最能代表當(dāng)前幀的旋轉(zhuǎn)角度的測(cè)量值，這就是為什么要在旋轉(zhuǎn)角度直方圖中選最優(yōu)的3個(gè)Bin的原因。

這個(gè)旋轉(zhuǎn)的角度哪來(lái)的呢？就是在計(jì)算描述子的時(shí)候算的，調(diào)用函數(shù)IC_Angle,代碼是：ORBextractor.cc#L475

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

感興趣的同學(xué)想知道為什么要這么麻煩的選取最優(yōu)3個(gè)角度，請(qǐng)從旋轉(zhuǎn)不變性開始理解，原理參見：3-5-3如何保證描述子旋轉(zhuǎn)不變性?

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

四 KannalaBrandt8::ReconstructWithTwoView

1 畸變校正

利用魚眼模型，對(duì)兩幀圖像的特征點(diǎn)進(jìn)行畸變校正，代碼見KannalaBrandt8.cpp#L219。要注意的是，魚眼模型的特殊性在于只考慮徑向畸變，忽略切向畸變，所以其p_ipi值都是0。想要深入理解魚眼模型的同學(xué)可以參考這篇文章《魚眼相機(jī)成像模型》

(https://blog.csdn.net/u010128736/article/details/52864024)。ORB-SLAM3中對(duì)不同模型相機(jī)的畸變校正做了區(qū)分，當(dāng)相機(jī)模型是針孔的時(shí)候，用的畸變校正參數(shù)是mDistCoef，當(dāng)相機(jī)模型是魚眼的時(shí)候，用的是虛擬出的相機(jī)類，代碼參見mpCamera = new KannalaBrandt8(vCamCalib)，為避免重復(fù)校正，用了個(gè)條件限制，就是在函數(shù)Frame::UndistortKeyPoints中判斷mDistCoef.at<float>(0)==0.0，代碼參見Frame.cc#L734，因?yàn)樵谟敏~眼相機(jī)模型的時(shí)候，mDistCoef沒有賦值，都是0。

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

2 位姿估計(jì)

主要由函數(shù)TwoViewReconstruction::Reconstruct完成，涉及到的知識(shí)點(diǎn)又多又關(guān)鍵的，包括對(duì)極約束、八點(diǎn)法、歸一化、直接線性變換、卡方檢驗(yàn)、重投影等，先從主要流程開始理解。

1. 利用隨機(jī)種子DUtils::Random::SeedRandOnceTwoViewReconstruction.cc#L79，在所有匹配特征點(diǎn)對(duì)中隨機(jī)選擇8對(duì)匹配特征點(diǎn)為一組for(size_t j=0; j<8; j++)TwoViewReconstruction.cc#L86，用于估計(jì)H矩陣和F矩陣。

2. 將當(dāng)前幀和參考幀中的特征點(diǎn)坐標(biāo)進(jìn)行歸一化。TwoViewReconstruction::NormalizeTwoViewReconstruction.cc#L753

3. 用DLT方法求解F矩陣 TwoViewReconstruction::ComputeF21TwoViewReconstruction.cc#L273

4. 對(duì)給定的F矩陣打分,需要使用到卡方檢驗(yàn)的知識(shí) TwoViewReconstruction::CheckFundamentalTwoViewReconstruction.cc#L395

5. 利用得到的最佳模型（選擇得分較高的矩陣值，單應(yīng)矩陣H或者基礎(chǔ)矩陣F）估計(jì)兩幀之間的位姿，代碼中對(duì)應(yīng)著函數(shù)ReconstructH或ReconstructF。其中，分兩個(gè)步驟。第一是利用基礎(chǔ)矩陣F和本質(zhì)矩陣E的關(guān)系，計(jì)算出四組解。第二是調(diào)用的函數(shù)CheckRT作用是用R，t來(lái)對(duì)特征匹配點(diǎn)三角化，并根據(jù)三角化結(jié)果判斷R,t的合法性。最終可以得到最優(yōu)解的條件是位于相機(jī)前方的3D點(diǎn)個(gè)數(shù)最多并且三角化視差角必須大于最小視差角。

2.2.4 基礎(chǔ)矩陣Fundamental 代數(shù)推導(dǎo)

有了以上的示意，我們嘗試用數(shù)學(xué)公式描述極點(diǎn)、極線和極平面之間的關(guān)系?？戳撕脦灼恼拢杏X還是視覺十四講里面的代數(shù)推導(dǎo)比較明晰，我就直接參考過來(lái)，當(dāng)做記錄了，其他比較雜亂，記錄在《SLAM 學(xué)習(xí)筆記 本質(zhì)矩陣E、基礎(chǔ)矩陣F、單應(yīng)矩陣H的推導(dǎo)》（https://blog.csdn.net/shanpenghui/article/details/110133454），感興趣的同學(xué)可以看看。

設(shè)以第一個(gè)相機(jī)作為坐標(biāo)系三維空間的點(diǎn)：

2.2.6 結(jié)尾

由于知識(shí)有限，加上篇幅限制，就不再展開了，這里可以參考另外幾篇比較好的文章，有比較詳細(xì)的推導(dǎo)過程，想深入研究的童鞋可以看看。

1、SLAM入門之視覺里程計(jì)(3)：兩視圖對(duì)極約束 基礎(chǔ)矩陣

2、SLAM基礎(chǔ)知識(shí)總結(jié)（https://blog.csdn.net/MyArrow/article/details/53704339）

五、總結(jié)

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

1. 對(duì)極約束是原理基礎(chǔ)，從物理世界出發(fā)描述了整個(gè)視覺相機(jī)成像、數(shù)據(jù)來(lái)源以及相互關(guān)系的根本問題，其中印象最深的是把搜索匹配點(diǎn)的范圍縮小成一段極線，大大加速了匹配過程。

2. 八點(diǎn)法從求解的角度出發(fā)，用公式描述了獲得我們想要的解的最小條件，提供了有力的數(shù)學(xué)基礎(chǔ)。

3. 歸一化使圖像進(jìn)行縮放，而縮放尺度是為了讓噪聲對(duì)于圖像的影響在一個(gè)數(shù)量級(jí)上，從而減少噪聲對(duì)圖像的影響。

4. 直接線性轉(zhuǎn)換則從諸多的測(cè)量值中（超過8點(diǎn)的N個(gè)匹配點(diǎn)，超定方程）算出了最優(yōu)的解，使我們基本得到了想要的解。

5. 在已經(jīng)有的粗解基礎(chǔ)上利用統(tǒng)計(jì)學(xué)方法進(jìn)行分析，篩選出優(yōu)質(zhì)的點(diǎn)（符合概率模型的內(nèi)點(diǎn)）來(lái)構(gòu)成我們最終使用的一個(gè)投影的最優(yōu)解，利用兩幀圖像上匹配點(diǎn)對(duì)進(jìn)行相互投影，綜合判斷內(nèi)外點(diǎn)，從而最小化誤差。

6. 篩選出內(nèi)外點(diǎn)之后，對(duì)兩個(gè)模型進(jìn)行打分，選出最優(yōu)模型，然后通過三角化測(cè)量進(jìn)行深度估計(jì)，最終完成初始化過程。

至此，單目的初始化過程（基于基礎(chǔ)矩陣F）就完啦，內(nèi)容較多，希望不對(duì)的地方多多指教，相互學(xué)習(xí)，共同成長(zhǎng)。以上僅是個(gè)人見解，如有紕漏望各位指出，謝謝。

參考：

1.對(duì)極幾何及單應(yīng)矩陣https://blog.csdn.net/u012936940/article/details/80723609

2.2D-2D：對(duì)極約束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)原理詳解及推導(dǎo)

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

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

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

8.卡方檢驗(yàn) （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.樣本標(biāo)準(zhǔn)差與自由度 n-1 卡方分布關(guān)系的證明

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

10.證明殘差平方和除隨機(jī)項(xiàng)方差服從卡方分布

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

11.本質(zhì)矩陣優(yōu)化分解的相對(duì)位姿估計(jì)

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

12.單目移動(dòng)機(jī)器人的相對(duì)位姿估計(jì)方法

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

13.三角化求深度值（求三位坐標(biāo)）

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

備注：作者也是我們「3D視覺從入門到精通」特邀嘉賓：一個(gè)超干貨的3D視覺學(xué)習(xí)社區(qū)

本文僅做學(xué)術(shù)分享，如有侵權(quán)，請(qǐng)聯(lián)系刪文。