ORB-SLAM筆記——(6)LocalMapping

到上一篇爲止算是把Tracking的主體部分介紹得差不多了，那麼這一次開始說LocalMapping線程。

當下主流的Keyframe-based SLAM或VO方法，都是用當前最新的觀測和已經儲存的地圖信息進行匹配，從而實現定位功能的。那麼問題來了：我們存什麼在地圖裏？什麼時候應該向地圖裏存東西？內存是有限的我們肯定不能無限地擴大地圖，那又該怎麼刪以維持有限的地圖大小呢？

以上問題總結起來就是我們如何去維護地圖，既能保證定位精度，同時又不會無限佔用內存。那麼在orb-slam中，這些問題全部交由LocalMapping線程來處理，全部工作包括：處理新關鍵幀，地圖點檢查剔除，生成新地圖點，Local BA，關鍵幀剔除。

ProcessNewKeyFrame()

當Tracking線程判斷當前幀是一幀關鍵幀時，會首先調用Tracking::CreateNewKeyFrame()去更新這一幀的相關信息。然後，函數內部會再通過mpLocalMapper->InsertKeyFrame(pKF)，將這一幀加入到建圖線程中的新關鍵幀隊列（mlNewKeyFrames）中。當前線程會定時通過CheckNewKeyFrames()檢查Tracking線程是否送來了新關鍵幀。若有，取最早送進隊列的關鍵幀進行處理，具體流程如下：

1. 計算當前關鍵幀的Bag of Words：mpCurrentKeyFrame->ComputeBoW()

2. 在TrackLocalMap階段，我們已經在局部地圖中找出了一些與當前幀能匹配上的地圖點。用當前幀對應特徵去更新地圖點的法向向量（normal vector）和深度，並選出一個最佳描述子。

UpdateNormalAndDepth()

void MapPoint::UpdateNormalAndDepth()
{
    map<KeyFrame*,size_t> observations;
    KeyFrame* pRefKF;
    cv::Mat Pos;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        if(mbBad)
            return;
        observations=mObservations;
        pRefKF=mpRefKF;
        Pos = mWorldPos.clone();
    }

    if(observations.empty())
        return;

    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
    int n=0;
    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        cv::Mat Owi = pKF->GetCameraCenter();
        cv::Mat normali = mWorldPos - Owi;
        normal = normal + normali/cv::norm(normali);
        n++;
    }

    cv::Mat PC = Pos - pRefKF->GetCameraCenter();
    const float dist = cv::norm(PC);
    const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
    const float levelScaleFactor =  pRefKF->mvScaleFactors[level];
    const int nLevels = pRefKF->mnScaleLevels;

    {
        unique_lock<mutex> lock3(mMutexPos);
        mfMaxDistance = dist*levelScaleFactor;
        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];
        mNormalVector = normal/n;
    }
}

更新該點的平均法向量（點到相機的方向向量），以及點的尺度不變的深度範圍。

ComputeDistinctiveDescriptors()

void MapPoint::ComputeDistinctiveDescriptors()
{
    // Retrieve all observed descriptors
    vector<cv::Mat> vDescriptors;

    map<KeyFrame*,size_t> observations;

    {
        unique_lock<mutex> lock1(mMutexFeatures);
        if(mbBad)
            return;
        observations=mObservations;
    }

    if(observations.empty())
        return;

    vDescriptors.reserve(observations.size());

    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;

        if(!pKF->isBad())
            vDescriptors.push_back(pKF->mDescriptors.row(mit->second));
    }

    if(vDescriptors.empty())
        return;

    // Compute distances between them
    const size_t N = vDescriptors.size();

    float Distances[N][N];
    for(size_t i=0;i<N;i++)
    {
        Distances[i][i]=0;
        for(size_t j=i+1;j<N;j++)
        {
            int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
            Distances[i][j]=distij;
            Distances[j][i]=distij;
        }
    }

    // Take the descriptor with least median distance to the rest
    int BestMedian = INT_MAX;
    int BestIdx = 0;
    for(size_t i=0;i<N;i++)
    {
        vector<int> vDists(Distances[i],Distances[i]+N);
        sort(vDists.begin(),vDists.end());
        int median = vDists[0.5*(N-1)];

        if(median<BestMedian)
        {
            BestMedian = median;
            BestIdx = i;
        }
    }

    {
        unique_lock<mutex> lock(mMutexFeatures);
        mDescriptor = vDescriptors[BestIdx].clone();
    }
}

一個MapPoint對應n個觀測會有n個描述子，顯然我們需要選出一個足夠有代表性的作爲該地圖點的描述子，不然當我們去匹配一個地圖點時，和多個描述子進行對比可還了得，失去了“描述”二字的意義。在orb-slam中，具體做法是計算兩兩描述子的距離，然後找出那個，和剩餘描述子距離的中值最小的那個描述子，即爲我們想要的。這其實相當於找了一個有平均最小距離的描述子吧。

3. 在做完點的更新後，需要更新該幀在covisibility graph和essential graph中，和其他關鍵幀的連接關係。

MapPointCulling()

上一步結束之後，我們得到了新關鍵幀和之前老關鍵幀的種種聯繫。基於這些信息，在orb-slam中，便可以對地圖點進行管理。因爲每一個新的關鍵幀都會對應產生一些新的地圖點，那麼是不是這些地圖點都可以直接隨着關鍵幀的生成一股腦地插入到map中呢？答案顯然是不能的。其最直接的理由在於，如果該點只是被當前關鍵幀發現，但在後面的連續追蹤過程中幾乎不能被觀測或匹配到，並且基於該點，當前關鍵幀可以找出的共視關鍵幀非常少。那麼這樣看來，即使當時在創建關鍵幀該點被找出來了，也不能成爲進入地圖的理由，應當即使刪除掉。那麼現在我們看一下orb-slam中是如何具體操作的：

首先，在localmapping線程中，orb維護了一個叫做mlpRecentAddedMapPoints的list，這裏面存放了地圖中比較新的那些地圖點的索引。每當有新的mappoint生成時，都會將對應指針push到list中。每次調用mappointculling()，都會遍歷該list，並且根據一系列條件判斷該點是否應該被刪去。先在這裏放上代碼：

void LocalMapping::MapPointCulling()
{
    // Check Recent Added MapPoints
    list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
    const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;

    int nThObs;
    if(mbMonocular)
        nThObs = 2;
    else
        nThObs = 3;
    const int cnThObs = nThObs;

    while(lit!=mlpRecentAddedMapPoints.end())
    {
        MapPoint* pMP = *lit;
        if(pMP->isBad())
        {
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(pMP->GetFoundRatio()<0.25f )
        {
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
        {
            pMP->SetBadFlag();
            lit = mlpRecentAddedMapPoints.erase(lit);
        }
        else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
            lit = mlpRecentAddedMapPoints.erase(lit);
        else
            lit++;
    }
}

這一部分的代碼還是非常清楚的，步驟是：

1. 已經是壞點的MapPoints直接從檢查鏈表中刪除;

2. 通過GetFoundRatio()返回的(mnFound)/mnVisible的比值，也就是實際能夠發生匹配的幀數與可以發生匹配的幀數的比值。如果該值<0.25，將該點置爲壞點並從list中刪去該點；

3. 如果連續該點從生成以後經歷了兩個以上的關鍵幀，但觀測數卻<=cnThObs，則將該點置爲壞點並從list中刪去該點；

4. 如果該點從生成以後經歷了三個以上的關鍵幀，並且沒有被當作壞點，則停止對它進行更多的判斷，認爲它是一個mappoint，並從list中刪除索引。

CreateNewMapPoints()

遍歷covisibilitygraph中共視程度最高的10或20（單目模式下）個關鍵幀。針對每一幀，一開始會先計算兩幀之間基線的長度，如果基線太短，認爲不利用三角化，直接放棄對其進行後續操作。若基線滿足條件，通過ComputeF12()計算兩幀之間的基礎矩陣F並利用matcher.SearchForTriangulation()以及F，採用極限約束的方法找到當前幀在共視幀的匹配。對這些匹配進行三角化，並對scale的一致性進行查驗，以下是這部分的代碼：

//Check scale consistency
cv::Mat normal1 = x3D-Ow1;
float dist1 = cv::norm(normal1);

cv::Mat normal2 = x3D-Ow2;
float dist2 = cv::norm(normal2);

if(dist1==0 || dist2==0)
    continue;

const float ratioDist = dist2/dist1;
const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave];

/*if(fabs(ratioDist-ratioOctave)>ratioFactor)
    continue;*/
if(ratioDist*ratioFactor<ratioOctave || ratioDist>ratioOctave*ratioFactor)
    continue;

代碼中最後的if判斷若滿足，則表明尺度一致性不滿足。

以上滿足的情況下認爲三角化成功，生成mappoint並計算描述子，normal vector和depth。最後將其加到map以及上一部分提到的mlpRecentAddedMapPoints這個list中。

BA+KeyFrameCulling()

在做完以上步驟後，會先check一下有沒有新的關鍵幀進來並且有沒有stoprequest，若沒有則可以進行接下的操作。

首先進行的是一個局部優化，這裏不同於我們上一篇博客討論tracklocalmap()中的BA。這裏優化的對象有當前關鍵幀Ki、共視圖中的相鄰關鍵幀Kc和這些關鍵幀觀測到的地圖點，另外可以觀測到這些地圖點但是不與當前幀相鄰的關鍵幀僅作爲約束條件而不作爲優化的對象。

在進行過BA之後，我們在localmapping這個線程的最後，需要對map中的關鍵幀進行維護，防止關鍵幀個數無限地增長。這裏的策略非常簡單：如果某一關鍵幀，它所觀測到的90%的地圖點都能被至少其他三個關鍵幀所觀測到，那麼則認爲這一幀是冗餘的應該刪去。

那麼這些就是localmapping中比較核心的思想以及實現方式了！

最後，以上全部爲個人學習心得，如有理解錯誤或是出入較大的地方歡迎評論區指正，如需轉發，請標明轉載出處。

ORB-SLAM筆記——(6)LocalMapping

ProcessNewKeyFrame()

UpdateNormalAndDepth()

ComputeDistinctiveDescriptors()

MapPointCulling()

CreateNewMapPoints()

BA+KeyFrameCulling()

ORB-SLAM筆記——(4)Tracking線程整體分析

ORB-SLAM筆記——(5)Track Local Map分析

ORB-SLAM筆記——(6)LocalMapping

SVO源碼閱讀筆記——motion estimation概述

三角測量的一些基礎理論

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結