ORBSLAM2之單目初始化(2)

第二階段:根據匹配計算兩幀之間的位姿

本質矩陣不能處理平面場景,單應矩陣不能處理非平面的場景,分單應矩陣和本質矩陣來分別恢復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的第一個和倒數第二個奇異值的比例有關。有充分證據表明在精確數據和無限精度的算術運算條件下,歸一化並不起作用,但是有噪聲存在時解將偏離其正確結果。

步驟
(1)1.meanX=(i=0Nui)/N,meanY=(i=0Nvi)/N 1.計算座標均值:meanX=(\sum\limits_{i=0}^{N} u_i)/N,meanY=(\sum\limits_{i=0}^{N} v_i)/N \tag{1}

(2)2.meanDevX=(i=0NuimeanX)/NmeanDevY=(i=0NvimeanX)/N 2.計算一階絕對矩和對應倒數:\\meanDevX=(\sum\limits_{i=0}^{N} |u_i-meanX|)/N\\meanDevY=(\sum\limits_{i=0}^{N} |v_i-meanX|)/N \tag{2}

(3)sX=1/meanDevX,sY=1/meanDevY sX=1/meanDevX,sY=1/meanDevY \tag{3}

(4)3.ui,=(uimeanX)sX,vi,=(vimeanY)sY[ui,vi,1]=[sX0meanXsX0sYmeanYsY001][uivi1] 3.計算標準化後的座標:u_i^,=(u_i-meanX)*sX,v_i^,=(v_i-meanY)*sY \\ 矩陣形式:\\ \begin{bmatrix} u_i^, \\ v_i^, \\ 1 \end{bmatrix}=\begin{bmatrix} sX &amp; 0 &amp; -meanX*sX \\ 0 &amp; sY &amp; -meanY*sY \\ 0 &amp; 0 &amp; 1 \end{bmatrix} \begin{bmatrix} u_i \\ v_i \\ 1 \end{bmatrix} \tag{4}

(5)4.T=[sX0meanXsX0sYmeanYsY001] 4.返回標準化矩陣:T=\begin{bmatrix} sX &amp; 0 &amp; -meanX*sX \\ 0 &amp; sY &amp; -meanY*sY \\ 0 &amp; 0 &amp; 1 \end{bmatrix} \tag{5}
參考自《計算機視覺中的多視圖幾何》 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;
}

...

解除歸一化

單應矩陣
x1,=Tx1,x2,=T,x2x1,,x2,x1,x2x1,=Hx2,Tx1=HT,x2x1=T1HT,x2 x_1^,=Tx_1,x_2^,=T^,x_2\\ x_1^,,x_2^,是歸一化後的座標數據,x_1,x_2是原始數據\\ 根據單應矩陣模型:x_1^,=Hx_2^,\\ \longrightarrow Tx_1=HT^,x_2\\ \longrightarrow x_1=T^{-1}HT^,x_2
基礎矩陣
x1,=Tx1,x2,=T,x2x1,,x2,x1,x2(x1,)TFx2,=0(Tx1)TFT,x2=0x1TTTFT,x2=0 x_1^,=Tx_1,x_2^,=T^,x_2\\ x_1^,,x_2^,是歸一化後的座標數據,x_1,x_2是原始數據\\ 根據基礎矩陣性質:(x_1^,)^TFx_2^,=0\\ \longrightarrow (Tx_1)^TFT^,x_2=0\\ \longrightarrow x_1^TT^TFT^,x_2=0

計算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.求解單應性矩陣(歸一化四點法)

單應性矩陣的模型:
(1)[x,y,1]=λ[h1h2h3h4h5h6h7h8h9][xy1] \begin{bmatrix} x^, \\ y^, \\ 1 \end{bmatrix} =\lambda\begin{bmatrix} h_1 &amp; h_2 &amp; h_3 \\ h_4 &amp; h_5 &amp; h_6 \\ h_7 &amp; h_8 &amp; h_9 \end{bmatrix} \begin{bmatrix} x \\ y \\ 1 \end{bmatrix} \tag{1}

(2)x,=λHx3xi,Hxixi,×Hxi=0Ah=0A=[000xy1xy,yy,y,xy1000xx,yx,x,]h=[h1h2h3h4h5h6h7h8h9], \longrightarrow x^,=\lambda Hx \tag{2} \\ 這是一個齊次矢量方程,因此3維矢量x_i^,和Hx_i不相等,但是具有相同的方向,利用叉乘性質\\ \longrightarrow x_i^,\times Hx_i=0 \\ \longrightarrow Ah=0 \\ 其中A=\begin{bmatrix} 0 &amp; 0 &amp; 0 &amp; -x &amp; -y &amp; -1 &amp; xy^,&amp; yy^, &amp; y^,\\ -x &amp; -y &amp; -1 &amp; 0 &amp; 0 &amp; 0 &amp; xx^,&amp; yx^, &amp; x^, \end{bmatrix} \\ h=\begin{bmatrix} h_1 &amp; h_2 &amp; h_3 &amp; h_4 &amp; h_5 &amp; h_6 &amp; h_7&amp; h_8 &amp; h_9\\ \end{bmatrix}^, \\

《計算機視覺中的多視圖幾何》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):
si[xi,yi,1]=H[xiyi1]:i(xi,h11xi+h12yi+h13h31xi+h32yi+h33)2+(yi,h21xi+h22yi+h23h31xi+h32yi+h33)2 s_i\begin{bmatrix} x_i^, \\ y_i^, \\ 1 \end{bmatrix}=H \begin{bmatrix} x_i \\ y_i \\ 1 \end{bmatrix} \\ 重投影投影誤差: \sum \limits_{i}(x^,_i-\frac{h_{11}x_i+h_{12}y_i+h_{13}}{h_{31}x_i+h_{32}y_i+h_{33}})^2+(y^,_i-\frac{h_{21}x_i+h_{22}y_i+h_{23}}{h_{31}x_i+h_{32}y_i+h_{33}})^2
顯然估計出的單應矩陣Hˆ爲使得誤差最小時H的值。

由於兩幅圖像中的測量點x,x′都有誤差,假設估計的變換爲Hˆ,它的逆變換爲Hˆ-1。則此時的幾何誤差就是 (將兩點x,y之間的歐氏距離記作d(x,y)):
i=0d(x,,H^xi)2+d(xi,H^1xi,)2 \sum\limits_{i=0}^{}d(x^,,\hat Hx_i)^2+d(x_i,\hat H^{-1}x^,_i)^2-對稱轉移誤差

scoreH=i=0Nρ(THx,H^x2/σ2)+ρ(THxH^1x,2/σ2) scoreH=\sum\limits_{i=0}^{N}\rho(T_H-||x^,-\hat Hx||^2/\sigma^2)+\rho(T_H-||x-\hat H^{-1}x^,||^2/\sigma^2)\\

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模型評分

scoreF=i=0Nρ(TFx,Fx2/σ2) scoreF=\sum\limits_{i=0}^{N}\rho(T_F-||x^,Fx||^2/\sigma^2)

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點法
F=KTEK1E=KTFK F=K^{-T}EK^{-1} \\ E=K^TFK
《計算機視覺中的多視圖幾何》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

ORB-SLAM2源碼中文詳解

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

ORB-SLAM2源碼中文註釋

ORB-SLAM2源碼中文詳解

《計算機視覺中的多視圖幾何》

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章