ORB-SLAM 代碼筆記(五)Frame類


Frame類的成員變量主要包含從攝像頭獲取的圖像的

1. 特征點信息(關鍵點+描述字)

2. 尺寸不變特征所用金字塔信息,這些都定義在ORBextractor對象中

3. 詞袋模型參數,用於跟蹤失敗情況下重定位

4. 相機參數,深度閾值

5. 當前幀的id,時間戳,相對世界坐標系位姿,參考關鍵幀,特征點對應地圖點及其是否是外點

6. 特征點網格分配情況,以及當前幀相對世界坐標的位姿(包括位姿矩陣的更新,以及相機光心坐標)。

 

每獲取一幀圖像,mnId++,因此30fps的相機,一秒構建30個Frame對象。

雙目,RGB-D和單目對應重載的構造函數。

雙目構建的Frame對象:

提取特征加入雙線程同步提取,0,1代表左目和右目,兩張圖片提取的特征點會放在不同的vector中,對於單目和RGB-D來說,右目不使用。

thread threadLeft(&Frame::ExtractORB,this,0,imLeft);
thread threadRight(&Frame::ExtractORB,this,1,imRight);
threadLeft.join();
threadRight.join();

向量mvKeys中存放N個提取出的左圖關鍵點,mDescriptor中存放提取出的左圖描述子,右圖的放在mvKeysRight和mDescriptorsRight中;

然后進行畸變矯正(由OpenCV完成),然后進行雙目匹配(記錄左圖中特征點對應的右圖坐標,以及恢復出的深度);

初始化地圖點及其外點;

  mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
  mvbOutlier = vector<bool>(N,false);

最后將關鍵點分布到64*48分割而成的網格中(目的是加速匹配以及均勻化關鍵點分布)。

RGBD構建的Frame對象:

和雙目類似,但是不需要雙目匹配,只需恢復出右圖中深度>0的橫坐標和深度即可。

單目構建的Frame對象:

和雙目類似,但是不包含匹配信息:

mvuRight = vector<float>(N,-1);
mvDepth = vector<float>(N,-1);

 Frame類還提供了一些與自己關聯緊密的函數,用於其他模塊:

1. 設置當前幀的姿態,並更新當前幀相機在世界坐標系下的位姿,中心點位置;

2. 判斷一個MapPoint是否在視角范圍內:

bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
{  // 注意這里的MapPoint是從SearchLocalPoint傳遞進來的,具備一定信息量
    pMP->mbTrackInView = false;
    cv::Mat P = pMP->GetWorldPos(); 

    const cv::Mat Pc = mRcw*P+mtcw; // 這里的R,t是經過初步的優化后的
    const float &PcX = Pc.at<float>(0);
    const float &PcY = Pc.at<float>(1);
    const float &PcZ = Pc.at<float>(2);

    if(PcZ<0.0f)
        return false;

    // Project in image and check it is not outside
    const float invz = 1.0f/PcZ;
    const float u=fx*PcX*invz+cx;
    const float v=fy*PcY*invz+cy;

    if(u<mnMinX || u>mnMaxX)
        return false;
    if(v<mnMinY || v>mnMaxY)
        return false;

    // Check distance is in the scale invariance region of the MapPoint
    // 每一個地圖點都是對應於若干尺度的金字塔提取出來的,具有一定的有效深度,如果相對當前幀的深度超過此范圍,返回False
    const float maxDistance = pMP->GetMaxDistanceInvariance();
    const float minDistance = pMP->GetMinDistanceInvariance();
    // 世界坐標系下,相機到3D點P的向量, 向量方向由相機指向3D點P
    const cv::Mat PO = P-mOw;
    const float dist = cv::norm(PO);

    if(dist<minDistance || dist>maxDistance)
        return false;

    // Check viewing angle
    // 每一個地圖點都有其平均視角,是從能夠觀測到地圖點的幀位姿中計算出
    // 如果當前幀的視角和其平均視角相差太大,返回False
    cv::Mat Pn = pMP->GetNormal();// |Pn|=1

    const float viewCos = PO.dot(Pn)/dist; //  = P0.dot(Pn)/(|P0|*|Pn|); |P0|=dist 

    if(viewCos<viewingCosLimit)
        return false;

    // Predict scale in the image
    // 根據深度預測尺度(對應特征點在一層)
    const int nPredictedLevel = pMP->PredictScale(dist,this);

    // Data used by the tracking
    // 標記該點將來要被投影
    pMP->mbTrackInView = true;
    pMP->mTrackProjX = u;
    pMP->mTrackProjXR = u - mbf*invz; //該3D點投影到雙目右側相機上的橫坐標
    pMP->mTrackProjY = v;
    pMP->mnTrackScaleLevel = nPredictedLevel;
    pMP->mTrackViewCos = viewCos;

    return true;
}
View Code

3. 在某塊區域內獲取特幀點:

vector<size_t> Frame::GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel, const int maxLevel) const;

其中,minLevel和maxLevel考察特征點是從圖像金字塔的哪一層提取出來的。

4. 將當前幀的描述子矩陣(可以轉換成向量),轉換成詞袋模型向量(DBoW2::BowVector mBowVec; DBoW2::FeatureVector mFeatVec;):

void Frame::ComputeBoW(){...}

5. 將特征點坐標(給id即可)反投影到3D地圖點(世界坐標):

cv::Mat Frame::UnprojectStereo(const int &i){}

 

最后,是Frame類中最重要的一個算法,雙目匹配以及恢復特征點的深度

圖像金字塔:0層是原始圖。對0層高斯核卷積后,降采樣(刪除所有偶數行和偶數列)即可得到高斯金字塔第1層;插入0用高斯卷積恢復成原始大小,與0層相減,得到0層拉普拉斯金字塔,對應的是0層高斯金字塔在濾波降采樣過程中丟失的信息,在其上可以提取特征。然后不斷降采樣,獲得不同層的高斯金字塔與拉普拉斯金字塔,提取出的特征對應的尺度與金字塔的層數是正相關的。層數越高,對應的尺度越大,尺度不確定性越高。通過對圖像的這種處理,我們可以提取出尺寸不變的的特征。但是在特征匹配時,需要考慮到提取特征點時對應的層數(尺度)。

在匹配左右幀的特征點時,雖然已經經過了極線矯正,但是不能僅僅搜索極線對應的同一行像素點,而應該根據右目提取特征點時的尺度(金字塔層數),確定一個極線附近的掃描范圍r,這個帶狀范圍內均包含這個特征信息。

const float r = 2.0f*mvScaleFactors[mvKeysRight[iR].octave];

1. 對左目相機每個特征點,通過描述子在右目帶狀搜索區域找到匹配點;

2. 通過SAD匹配提高像素匹配修正量bestincR;

3. 做拋物線擬合找谷底得到亞像素匹配deltaR;

4. 剔除SAD匹配偏差較大的匹配特征點。


免責聲明!

本站轉載的文章為個人學習借鑒使用,本站對版權不負任何法律責任。如果侵犯了您的隱私權益,請聯系本站郵箱yoyou2525@163.com刪除。



 
粵ICP備18138465號   © 2018-2025 CODEPRJ.COM