ORB-SLAM2目錄：
一步步帶你看懂orbslam2源碼–總體框架(一)
一步步帶你看懂orbslam2源碼–orb特徵點提取(二)
一步步帶你看懂orbslam2源碼–單目初始化(三)
一步步帶你看懂orbslam2源碼–單應矩陣/基礎矩陣,求解R,t(四)
一步步帶你看懂orbslam2源碼–單目初始化(五)
回顧:
上一節我們主要講解了對極約束的原理以及F矩陣的求解,單應矩陣的原理以及單應矩陣的求解,RANSAC隨機採樣一致性算法,閾值選擇原理,score計算方式,模型選擇策略,並且對創建Frame剩餘的部分源碼進行補充說明.本章節主要將講解如何從單應矩陣和基礎矩陣中分解出相機位姿和三維空間點座標,這一部分東北大學的吳博在"ORB-SLAM代碼詳細解讀"PPT中已經進行了詳細推導,筆者這次的主要目的是根據吳博的推導過程再講解一遍,並且完善其中的計算過程,最後還給出了其在orbslam2源碼中的實現方式.
理論環節
由上一講中,我們已經知道單應矩陣的形式:
問題轉換爲:我們如何從中分解出和,這也是本章節的主要內容,可以將式(1)做進一步變換:
令,對進行奇異值分解:,則
因爲和爲單位正交矩陣,所以考慮
將式(3)變換爲:
令
則式(4)變爲:
於是,原問題轉換爲:如何從式子(5)中的分解出和,從而可以得到:
注意,這是正交矩陣的性質,引入是爲了說明與,與存在符號取反的可能. 進一步的:
取:
則:
注意到,由於是平面的單位法向量,所以
所以式(7)變爲:
式(9)也可以等價爲:
將式(10)兩邊同時乘以,式(11)兩邊同時乘以可得:
由上式下式可得:從而消去,以此類推可得:
由於旋轉矩陣具有保範性,即:
對式(13)中的第一式左右兩邊取範數,可得:
進一步有:
以此類推,可得:
令:
則式子(14)可以簡寫爲:
由因爲該方程必有非零解,所以:
則:
即
因爲:,所以對於式子(15)可以分爲以下三種情況:
因此我們必須找到解同時滿足以上三種情況,結合式子(14)分析,這三種情況均可以得到:,對於其他解均能找到反例.現對第一種情況使用反例證明:
如果:或,根據式子(14)可得:
可以推出:與相矛盾.
故問題轉換爲:求情況下,對應的和.即:
情況一(針對式子(16)):
將代入式子(14)可得:
可得:
式子(22)變成:
經過化簡可得:
將代入式子(11)可得:
該式子的含義爲向量經過旋轉之後還是向量,注意到,即y軸只能繞y軸旋轉後,還是y軸本身,注意,此處定義順時針旋轉方向爲正.
故:
將式子(24)和式子(23)代入式子(13)的第三個可得:
可以解得:
將式子(24)代入式子(9)中可得:
所以:
進一步可得:
因此:
所以上述情況一:總共可分獲得4組不同的解,分別取,
情況二(針對式子(17)):
將代入式子(14)可得:
經過化簡可得:
將式子(27)代入式子(9)中可得:
所以:
進一步可得:
因此:
所以上述情況二:總共可分獲得2組不同的解,分別取
情況三(針對式子(18)): 未定義,無法解
情況四(針對式子(19)):
將代入式子(14)可得:
可得:
式子(29)變成:
經過化簡可得:
將代入式子(11)可得:
該式子的含義爲向量經過旋轉之後爲原來的相反向量,注意到,爲何旋轉矩陣的最後兩列會乘以負號呢?其實筆者此處也不太理解~~希望讀者們如果懂的話,麻煩私信指導下我,感激不盡!!.
故:
將式子(30)和式子(31)代入式子(13)的第三個可得:
可以解得:
將式子(31)代入式子(9)中可得:
所以:
進一步可得:
因此:
所以上述情況四:總共同樣可獲得4組不同的解,分別取,
情況五和情況六最後這塊筆者並沒有取細看,貌似源碼中也沒有實現這塊,就直接把吳博的結論放在這裏了
情況五(針對式子(20)):
同理可以解得:
情況六(針對式子(21)):
根據公式(7):
根據可得:
即:
取垂直於法向量的向量,代入:,則:
根據household變換:
源碼分析環節
終於把所有公式都敲完了…真是心累…寫篇好博客真心太難…
接下來,讓我們來看下orbslam2源碼中是如何利用單應矩陣和基礎矩陣進行相機位姿估計以及三角化測量點的吧.
(1)單應矩陣恢復
首先看下函數接口以及傳參的意義吧.
//vbMatchesInliersH: true--inliners false--outliners
//H:單應矩陣 ,mK:相機內參
//R21,t21:待求相機位姿
//vP3D:三角化測量後的空間三維點座標
//vbTriangulated:true--成功三角化測量 false--測量失敗
//1.0:最小視差爲1° 50:三角化測量成功點最低閾值
ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
系統成功使用單應矩陣進行相機恢復具備如下條件:
- 單應矩陣的三個奇異值必須要有足夠的差異
- 成功三角化測量的內點的個數必須足夠突出,遠好過第二好,且個數要大於最低閾值
- 要有足夠的視差
成功三角化測量的內點必須具備如下條件:
- 有足夠的視差
- 在參考幀和當前幀的相機前方
- 重投影誤差小
實現源碼如下:
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)
{
int N=0;
for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)//計算inliers的個數
if(vbMatchesInliers[i])
N++;
// 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 invK = K.inv();
cv::Mat A = invK*H21*K;
cv::Mat U,w,Vt,V;
cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);
V=Vt.t();
float s = cv::determinant(U)*cv::determinant(Vt);
float d1 = w.at<float>(0);
float d2 = w.at<float>(1);
float d3 = w.at<float>(2);
//單應矩陣的三個奇異值必須要有足夠的差異
if(d1/d2<1.00001 || d2/d3<1.00001)
{
return false;
}
vector<cv::Mat> vR, vt, vn;
vR.reserve(8);
vt.reserve(8);
vn.reserve(8);
//n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1
float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
float x1[] = {aux1,aux1,-aux1,-aux1};
float x3[] = {aux3,-aux3,aux3,-aux3};
//case d'=d2 對應情況一中的四組解
float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);
float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};
// 計算旋轉矩陣 R‘，計算ppt中公式18
// | ctheta 0 -aux_stheta| | aux1|
// Rp = | 0 1 0 | tp = | 0 |
// | aux_stheta 0 ctheta | |-aux3|
// | ctheta 0 aux_stheta| | aux1|
// Rp = | 0 1 0 | tp = | 0 |
// |-aux_stheta 0 ctheta | | aux3|
// | ctheta 0 aux_stheta| |-aux1|
// Rp = | 0 1 0 | tp = | 0 |
// |-aux_stheta 0 ctheta | |-aux3|
// | ctheta 0 -aux_stheta| |-aux1|
// Rp = | 0 1 0 | tp = | 0 |
// | aux_stheta 0 ctheta | | aux3|
for(int i=0; i<4; i++)
{
cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
Rp.at<float>(0,0)=ctheta;
Rp.at<float>(0,2)=-stheta[i];
Rp.at<float>(2,0)=stheta[i];
Rp.at<float>(2,2)=ctheta;
cv::Mat R = s*U*Rp*Vt;
vR.push_back(R);
cv::Mat tp(3,1,CV_32F);
tp.at<float>(0)=x1[i];
tp.at<float>(1)=0;
tp.at<float>(2)=-x3[i];
tp*=d1-d3;
// 這裏雖然對t有歸一化，並沒有決定單目整個SLAM過程的尺度
// 因爲CreateInitialMapMonocular函數對3D點深度會縮放，然後反過來對 t 有改變
cv::Mat t = U*tp;
vt.push_back(t/cv::norm(t));
cv::Mat np(3,1,CV_32F);
np.at<float>(0)=x1[i];
np.at<float>(1)=0;
np.at<float>(2)=x3[i];
cv::Mat n = V*np;
if(n.at<float>(2)<0)
n=-n;
vn.push_back(n);
}
//case d'=-d2 對應情況四中的四組解
float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);
float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);
float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};
for(int i=0; i<4; i++)
{
cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
Rp.at<float>(0,0)=cphi;
Rp.at<float>(0,2)=sphi[i];
Rp.at<float>(1,1)=-1;
Rp.at<float>(2,0)=sphi[i];
Rp.at<float>(2,2)=-cphi;
cv::Mat R = s*U*Rp*Vt;
vR.push_back(R);
cv::Mat tp(3,1,CV_32F);
tp.at<float>(0)=x1[i];
tp.at<float>(1)=0;
tp.at<float>(2)=x3[i];
tp*=d1+d3;
cv::Mat t = U*tp;
vt.push_back(t/cv::norm(t));
cv::Mat np(3,1,CV_32F);
np.at<float>(0)=x1[i];
np.at<float>(1)=0;
np.at<float>(2)=x3[i];
cv::Mat n = V*np;
if(n.at<float>(2)<0)
n=-n;
vn.push_back(n);
}
int bestGood = 0;
int secondBestGood = 0;
int bestSolutionIdx = -1;
float bestParallax = -1;
vector<cv::Point3f> bestP3D;
vector<bool> bestTriangulated;
// 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
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);
//保存最好和次最好
if(nGood>bestGood)
{
secondBestGood = bestGood;
bestGood = nGood;
bestSolutionIdx = i;
bestParallax = parallaxi;
bestP3D = vP3Di;
bestTriangulated = vbTriangulatedi;
}
else if(nGood>secondBestGood)
{
secondBestGood = nGood;
}
}
//最好必須遠大於次最好,且要有足夠的視差,且內點數要大於最低閾值
if(secondBestGood<0.75*bestGood && bestParallax>=minParallax && bestGood>minTriangulated && bestGood>0.9*N)
{
vR[bestSolutionIdx].copyTo(R21);
vt[bestSolutionIdx].copyTo(t21);
vP3D = bestP3D;
vbTriangulated = bestTriangulated;
return true;
}
return false;
}
其中,讓我們來看看CheckRT()這個函數吧,這個函數將計算由單應矩陣分解出來的R和t的優劣,並且返回成功三角化測量點的數量,同時計算出特徵點的三維點座標(即在世界座標系下的座標,即參考幀座標系下的座標),這個時候不知道讀者們能否想起來,當時在分解單應矩陣的時候,進行這步操作呢?實際上這步操作已經將從像素座標到像素座標之間的單應矩陣變換轉換爲從相機座標到相機座標的單應矩陣變換了,那麼分解出來的自然也就是相機座標之間的旋轉變換了.主要是爲了方便求出兩個相機光心的座標.所以在後續計算可能中還需要乘以K轉換爲像素座標之間的變換關係.
以下爲該函數的代碼:其主要思路是計算視差(即角度),三維點座標,重投影誤差,並以此計算成功三角測量的點的數量nGood.
int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,
const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers,
const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float ¶llax)
{
// Calibration parameters
const float fx = K.at<float>(0,0);
const float fy = K.at<float>(1,1);
const float cx = K.at<float>(0,2);
const float cy = K.at<float>(1,2);
vbGood = vector<bool>(vKeys1.size(),false);
vP3D.resize(vKeys1.size());
vector<float> vCosParallax;
vCosParallax.reserve(vKeys1.size());
// Camera 1 Projection Matrix K[I|0]
cv::Mat P1(3,4,CV_32F,cv::Scalar(0));
K.copyTo(P1.rowRange(0,3).colRange(0,3));
cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);//幀1的光心的世界座標
// Camera 2 Projection Matrix K[R|t]
cv::Mat P2(3,4,CV_32F);
R.copyTo(P2.rowRange(0,3).colRange(0,3));
t.copyTo(P2.rowRange(0,3).col(3));
P2 = K*P2;
cv::Mat O2 = -R.t()*t;//幀2的光心的世界座標(即在幀1中的座標)
int nGood=0;
for(size_t i=0, iend=vMatches12.size();i<iend;i++)//遍歷 幀1和幀2 所有的匹配點
{
if(!vbMatchesInliers[i])
continue;
const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];
const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];
cv::Mat p3dC1;
Triangulate(kp1,kp2,P1,P2,p3dC1);
//如果3d座標無限大,則跳過
if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2)))
{
vbGood[vMatches12[i].first]=false;
continue;
}
// Check parallax
cv::Mat normal1 = p3dC1 - O1;
float dist1 = cv::norm(normal1);
cv::Mat normal2 = p3dC1 - O2;
float dist2 = cv::norm(normal2);
float cosParallax = normal1.dot(normal2)/(dist1*dist2);//三角化的 前方交會角 的餘弦值
// Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)
if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)
continue;
// Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)
cv::Mat p3dC2 = R*p3dC1+t;
//acos(0.99998)=0.3624°,如果 z<0 且 角度變化小於0.3624° , 則捨棄
if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)
continue;
// Check reprojection error in first image
float im1x, im1y;
float invZ1 = 1.0/p3dC1.at<float>(2);
im1x = fx*p3dC1.at<float>(0)*invZ1+cx;
im1y = fy*p3dC1.at<float>(1)*invZ1+cy;
//計算3d點在第一幀中,投影像素座標與實際像素座標的距離平方
float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);
if(squareError1>th2)//如果重投影誤差太大,捨棄
continue;
// Check reprojection error in second image
float im2x, im2y;
float invZ2 = 1.0/p3dC2.at<float>(2);
im2x = fx*p3dC2.at<float>(0)*invZ2+cx;
im2y = fy*p3dC2.at<float>(1)*invZ2+cy;
float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);
if(squareError2>th2)
continue;
//至此,3d點成功通過篩選流程,保存 cosParallax 和 p3dC1,統計點的數量
vCosParallax.push_back(cosParallax);
vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));
nGood++;
if(cosParallax<0.99998)
vbGood[vMatches12[i].first]=true;
}
if(nGood>0)//有合格的3d點才進行
{
sort(vCosParallax.begin(),vCosParallax.end());//默認升序
size_t idx = min(50,int(vCosParallax.size()-1));//最大取50
parallax = acos(vCosParallax[idx])*180/CV_PI;//轉換爲角度,求出所有3d點中的 最小前方交會角 的角度
}
else
parallax=0;
return nGood;
}
其中,計算視差:cosParallax的原理如下圖所示:
再看看Triangulate(kp1,kp2,P1,P2,p3dC1);函數的原理吧,字有點醜…原諒筆者實在是不想在敲公式了…
(2)基礎矩陣恢復
這裏我們主要看看如何從基礎矩陣F分解出R,t,更準確說是從本質矩陣E,因爲源碼中將E轉換爲了F再進行分解的.代碼如下:
void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
{
cv::Mat u,w,vt;
cv::SVD::compute(E,w,u,vt);
u.col(2).copyTo(t);
t=t/cv::norm(t);
cv::Mat W(3,3,CV_32F,cv::Scalar(0));
W.at<float>(0,1)=-1;
W.at<float>(1,0)=1;
W.at<float>(2,2)=1;
R1 = u*W*vt;
if(cv::determinant(R1)<0)
R1=-R1;
R2 = u*W.t()*vt;
if(cv::determinant(R2)<0)
R2=-R2;
}
視覺SLAM十四講是這樣講的…偷個懶…
如若感興趣關於R,t爲何如此取值,可參考博文:https://www.cnblogs.com/FangLai-you/p/10472755.html
關於此處筆者並沒有去過多關注.
總結
- 單應矩陣如何分解R,t的理論推導以及源碼實現
- 基礎矩陣/本質矩陣的理論及其源碼實現
參考文獻:
- 高翔–視覺SLAM十四講
- 吳博-ORB-SLAM代碼詳細解讀
PS:
- 如果您覺得我的博客對您有所幫助，歡迎關注我的博客。此外，歡迎轉載我的文章，但請註明出處鏈接。
- 本博客僅代表個人觀點,不一定完全正確,如有出錯之處,也歡迎批評指正.
上一章節:一步步帶你看懂orbslam2源碼–單目初始化(三)
下一章節:一步步帶你看懂orbslam2源碼–單目初始化(五)