文章目錄
第二階段：根據匹配計算兩幀之間的位姿
本質矩陣不能處理平面場景，單應矩陣不能處理非平面的場景,分單應矩陣和本質矩陣來分別恢復R和T
-
1 結構化匹配上的特徵點對，並且歸一化
Initializer::Normalize（）
-
2 在所有匹配特徵點對中隨機選擇8對匹配特徵點爲一組，用於FindHomography和FindFundamental求解
-
3 調用多線程分別用於計算fundamental 和homography 矩陣
（Initializer::FindFundamental（） and Initializer::FindHomography（），返回模型分數）
-
4 計算得分比例，選取某個模型
-
5 從H矩陣或F矩陣中恢復R,t（ReconstructH(）or ReconstructF(））
ORB_SLAM2/src/Tracking.cpp--void Tracking::MonocularInitialization()
...
cv::Mat Rcw; // Current Camera Rotation
cv::Mat tcw; // Current Camera Translation
vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
//調用Initialize函數，並行地計算基礎矩陣和單應性矩陣，選取其中一個模型，恢復出最開始兩幀之間的相對姿態以及點雲
if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
....
ORB_SLAM2/src/Initializer.cpp--bool Initializer::Initialize()
...
// 用current frame,也就是用SLAM邏輯上的第二幀來初始化整個SLAM，得到最開始兩幀之間的R t,以及點雲
bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
{
// Fill structures with current keypoints and matches with reference frame
// Reference Frame: 1, Current Frame: 2
// Frame2 特徵點
mvKeys2 = CurrentFrame.mvKeysUn;
// mvMatches12記錄匹配上的特徵點對
mvMatches12.clear();
mvMatches12.reserve(mvKeys2.size());
// mvbMatched1記錄每個特徵點是否有匹配的特徵點，
// 這個變量後面沒有用到，後面只關心匹配上的特徵點
mvbMatched1.resize(mvKeys1.size());
// 步驟1：組織特徵點對
//typedef pair<int,int> Match;
// vector<Match> mvMatches12;< Match的數據結構是pair,mvMatches12只記錄Reference到Current匹配上的特徵點對
for(size_t i=0, iend=vMatches12.size();i<iend; i++)
{
if(vMatches12[i]>=0)
{
mvMatches12.push_back(make_pair(i,vMatches12[i]));
//vector<bool> mvbMatched1;記錄Reference Frame的每個特徵點在Current Frame是否有匹配的特徵點
mvbMatched1[i]=true;
}
else
mvbMatched1[i]=false;
}
// 匹配上的特徵點的個數
const int N = mvMatches12.size();
// Indices for minimum set selection
// 新建一個容器vAllIndices，生成0到N-1的數作爲特徵點的索引
vector<size_t> vAllIndices;
vAllIndices.reserve(N);
vector<size_t> vAvailableIndices;
for(int i=0; i<N; i++)
{
vAllIndices.push_back(i);
}
// Generate sets of 8 points for each RANSAC iteration
// 步驟2：在所有匹配特徵點對中隨機選擇8對匹配特徵點爲一組，共選擇mMaxIterations組
// 用於FindHomography和FindFundamental求解
// mMaxIterations:200
mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));//二維容器，外層容器的大小爲迭代次數，內層容器大小爲每次迭代算H或F矩陣需要的點
DUtils::Random::SeedRandOnce(0);
for(int it=0; it<mMaxIterations; it++)
{
vAvailableIndices = vAllIndices;
// Select a minimum set
for(size_t j=0; j<8; j++)
{
// 產生0到N-1的隨機數
int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
// idx表示哪一個索引對應的特徵點被選中
int idx = vAvailableIndices[randi];
mvSets[it][j] = idx;
// randi對應的索引已經被選過了，從容器中刪除
// randi對應的索引用最後一個元素替換，並刪掉最後一個元素
vAvailableIndices[randi] = vAvailableIndices.back();
vAvailableIndices.pop_back();
}
}
// Launch threads to compute in parallel a fundamental matrix and a homography
// 步驟3：調用多線程分別用於計算fundamental matrix和homography
vector<bool> vbMatchesInliersH, vbMatchesInliersF;
float SH, SF; // score for H and F
cv::Mat H, F; // H and F
// ref是引用的功能:http://en.cppreference.com/w/cpp/utility/functional/ref
// 計算homograpy並打分
thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
// 計算fundamental matrix並打分
thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
// Wait until both threads have finished
threadH.join();
threadF.join();
// Compute ratio of scores
// 步驟4：計算得分比例，選取某個模型
float RH = SH/(SH+SF);
// Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
// 步驟5：從H矩陣或F矩陣中恢復R,t
if(RH>0.40)
return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
else //if(pF_HF>0.6)
return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
return false;
}
ORB_SLAM實現的是自動初始化，也就是，無論場景平面，還是普通場景，都能完成初始化工作。其做法是同時計算適用於平面場景的單應性矩陣（H）和適用於非平面場景的基礎矩陣(F),
首先，由抽樣點對，計算出H（歸一化4點法）和F矩陣（歸一化八點法）；通過若干次RANSAC抽樣，計算出最優的H和F矩陣；然後，通過比較H和F矩陣，獲得評價分數，選擇最優的矩陣，恢復出幀間位姿。
但如果兩個模型分值都不高（意味着沒有足夠的局內點），就重新選擇第二幀，重新匹配並嘗試初始化。
一旦選擇好模型，我們就可以獲得相應的運動狀態。如果選擇單應矩陣，按照Faugeras等人發表的論文中提到的方法，提取8種運動假設，該方法提出用cheriality check測試來選擇有效解。
然而，如果在低視差的情況下，這些測試就會失效，因爲雲點很容易在相機的前面或後面移動，會導致選解錯誤。我們提出的方法是直接按這8種解將二維點三角化，然後檢查是否有一種解可以使得所有的雲點都位於兩個相機的前面，且重投影誤差較小。如果沒有一個最優的解，我們就不執行初始化，否則重新選擇第二幀，重新匹配並嘗試初始化。
特徵點歸一化-Normalize()
將mvKeys1和mvKey2歸一化到均值爲0，一階絕對矩爲1，歸一化矩陣分別爲T1、T2
在計算H 或者 F矩陣的時候需要對特徵點進行座標變換，稱之爲歸一化。
計算單應矩陣時變換特徵點的座標會得到更好的效果，包括座標的平移和尺度縮放，並且這一步驟必須放在DLT之前。DLT之後再還原到原座標系。書本指出歸一化與條件數確切的說是DTL矩陣A的第一個和倒數第二個奇異值的比例有關。有充分證據表明在精確數據和無限精度的算術運算條件下，歸一化並不起作用，但是有噪聲存在時解將偏離其正確結果。
步驟
參考自《計算機視覺中的多視圖幾何》 3.4.4
ORB_SLAM2/src/Initializer.cpp--void Initializer::Normalize()
...
void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)
{
float meanX = 0;
float meanY = 0;
const int N = vKeys.size();
vNormalizedPoints.resize(N);
for(int i=0; i<N; i++)
{
meanX += vKeys[i].pt.x;
meanY += vKeys[i].pt.y;
}
meanX = meanX/N;
meanY = meanY/N;
float meanDevX = 0;
float meanDevY = 0;
// 將所有vKeys點減去中心座標，使x座標和y座標均值分別爲0
for(int i=0; i<N; i++)
{
vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;
vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;
meanDevX += fabs(vNormalizedPoints[i].x);
meanDevY += fabs(vNormalizedPoints[i].y);
}
meanDevX = meanDevX/N;
meanDevY = meanDevY/N;
float sX = 1.0/meanDevX;
float sY = 1.0/meanDevY;
// 將x座標和y座標分別進行尺度縮放，使得x座標和y座標的一階絕對矩分別爲1
for(int i=0; i<N; i++)
{
vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
}
// |sX 0 -meanx*sX|
// |0 sY -meany*sY|
// |0 0 1 |
T = cv::Mat::eye(3,3,CV_32F);
T.at<float>(0,0) = sX;
T.at<float>(1,1) = sY;
T.at<float>(0,2) = -meanX*sX;
T.at<float>(1,2) = -meanY*sY;
}
...
解除歸一化
單應矩陣
基礎矩陣
計算Homography，獲取模型評分
ORB_SLAM2/src/Initializer.cpp--void Initializer::FindHomography()
...
/**
* @brief 計算單應矩陣
*
* 假設場景爲平面情況下通過前兩幀求取Homography矩陣(current frame 2 到 reference frame 1),並得到該模型的評分
*/
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{
...
//1.特徵點歸一化
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
cv::Mat T2inv = T2.inv();
//2.求解單應矩陣
cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
// 恢復原始的均值和尺度（解除歸一化）
H21i = T2inv*Hn*T1;
H12i = H21i.inv();
// 3.利用重投影誤差爲當次RANSAC的結果評分
currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
...
}
...
1.將特徵點歸一化
見特徵點歸一化
2.求解單應性矩陣（歸一化四點法）
單應性矩陣的模型：
《計算機視覺中的多視圖幾何》P53-P55
ORB_SLAM2/src/Initializer.cpp--cv::Mat Initializer::ComputeH21()
...
cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)
{
const int N = vP1.size();
cv::Mat A(2*N,9,CV_32F); // 2N*9
...
...
cv::Mat u,w,vt;
//cv::SVDecomp():https://docs.opencv.org/3.1.0/d2/de8/group__core__array.html#gab477b5b7b39b370bb03e75b19d2d5109
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
return vt.row(8).reshape(0, 3); // v的最後一列
}
...
3.Homography模型評分
重投影誤差(圖像平面和目的圖像平面之間的透視變換矩陣H）:
顯然估計出的單應矩陣Hˆ爲使得誤差最小時H的值。
由於兩幅圖像中的測量點x,x′都有誤差，假設估計的變換爲Hˆ，它的逆變換爲Hˆ-1。則此時的幾何誤差就是 (將兩點x,y之間的歐氏距離記作d(x,y))：
ORB_SLAM2/src/Initializer.cpp--float Initializer::CheckHomography()
...
float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma)
計算Fundamental，獲取模型評分
ORB_SLAM2/src/Initializer.cpp--void Initializer::FindFundamental()
...
/**
* @brief 計算基礎矩陣
*
* 假設場景爲非平面情況下通過前兩幀求取Fundamental矩陣(current frame 2 到 reference frame 1),並得到該模型的評分
*/
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{
...
//1. 歸一化
Normalize(mvKeys1,vPn1, T1);
Normalize(mvKeys2,vPn2, T2);
cv::Mat T2t = T2.t();
...
...
//2.計算F矩陣
cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
//解除歸一化
F21i = T2t*Fn*T1;
// 3.利用重投影誤差爲當次RANSAC的結果評分
currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
...
}
1.將特徵點歸一化
見特徵點歸一化
2.求解基礎矩陣（歸一化八點法）
ORB_SLAM2/src/Initializer.cpp--cv::Mat Initializer::ComputeF21()
...
cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1,const vector<cv::Point2f> &vP2)
{
const int N = vP1.size();
cv::Mat A(N,9,CV_32F); // N*9
for(int i=0; i<N; i++)
{
const float u1 = vP1[i].x;
const float v1 = vP1[i].y;
const float u2 = vP2[i].x;
const float v2 = vP2[i].y;
A.at<float>(i,0) = u2*u1;
A.at<float>(i,1) = u2*v1;
A.at<float>(i,2) = u2;
A.at<float>(i,3) = v2*u1;
A.at<float>(i,4) = v2*v1;
A.at<float>(i,5) = v2;
A.at<float>(i,6) = u1;
A.at<float>(i,7) = v1;
A.at<float>(i,8) = 1;
}
cv::Mat u,w,vt;
cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
cv::Mat Fpre = vt.row(8).reshape(0, 3); // v的最後一列
cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
w.at<float>(2)=0; // 秩2約束，將第3個奇異值設爲0
return u*cv::Mat::diag(w)*vt;
}
3.Fundamental模型評分
ORB_SLAM2/src/Initializer.cpp--float Initializer::CheckFundamental()
...
float Initializer::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma)
計算Essential
ORB_SLAM是先計算基礎矩陣F，然後通過相機內參計算E，沒有直接計算E;直接求解E可以使用8點法，或者5點法
《計算機視覺中的多視圖幾何》P173
ORB_SLAM2/src/Initializer.cpp--bool Initializer::ReconstructF()
...
// Compute Essential Matrix from Fundamental Matrix
cv::Mat E21 = K.t()*F21*K;
...
爲什麼Ax=0的解是V的最後一列
《計算機視覺中的多視圖幾何》P412-P413
Homograph 矩陣分解恢復R，t
Motion and structure from motion in a piecewise planar environment
ORB_SLAM2/src/Initializer.cpp--Initializer::ReconstructH()
...
bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{
...
// We recover 8 motion hypotheses using the method of Faugeras et al.
// Motion and structure from motion in a piecewise planar environment.
// International Journal of Pattern Recognition and Artificial Intelligence, 1988
cv::Mat U,w,Vt,V;
//1.求解H的分解矩陣，得到8種運動假設
cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);
V=Vt.t();
...
...
// Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax)
// We reconstruct all hypotheses and check in terms of triangulated points and parallax
// d'=d2和d'=-d2分別對應8組(R t)
//2.進行cheirality check,選出最優的
for(size_t i=0; i<8; i++)
{
float parallaxi;
vector<cv::Point3f> vP3Di;
vector<bool> vbTriangulatedi;
int nGood = CheckRT(vR[i],vt[i],mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K,vP3Di, 4.0*mSigma2, vbTriangulatedi, parallaxi);
...
}
Essential 矩陣分解恢復R，t
/**
* @brief 從F恢復R t
*
* 度量重構
* 1. 由Fundamental矩陣結合相機內參K，得到Essential矩陣: \f$ E = k'^T F k \f$
* 2. SVD分解得到R t
* 3. 進行cheirality check, 從四個解中找出最合適的解
*
* @see Multiple View Geometry in Computer Vision - Result 9.19 p259
*/
bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
{
...
// 1.Compute Essential Matrix from Fundamental Matrix
cv::Mat E21 = K.t()*F21*K;
cv::Mat R1, R2, t;
// 2. Recover the 4 motion hypotheses
// 雖然這個函數對t有歸一化，但並沒有決定單目整個SLAM過程的尺度
// 因爲CreateInitialMapMonocular函數對3D點深度會縮放，然後反過來對 t 有改變
// F矩陣通過結合內參可以得到Essential矩陣，分解E矩陣將得到4組解
//這4組解分別爲[R1,t],[R1,-t],[R2,t],[R2,-t]
DecomposeE(E21,R1,R2,t);
cv::Mat t1=t;
cv::Mat t2=-t;
// 3. Reconstruct with the 4 hyphoteses and check
vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;
vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;
float parallax1,parallax2, parallax3, parallax4;
int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D1, 4.0*mSigma2, vbTriangulated1, parallax1);
int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);
int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);
int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);
...
}
參考
https://www.zhihu.com/question/50385799/answer/120902345
https://blog.csdn.net/zhubaohua_bupt/article/details/78560966
ORB-SLAM: A Versatile and Accurate Monocular SLAM System
http://webdiis.unizar.es/~raulmur/orbslam/
https://github.com/raulmur/ORB_SLAM2
https://en.cppreference.com/w/cpp/algorithm/fill
《計算機視覺中的多視圖幾何》