# KeyFrame.cpp

### KeyFrame::KeyFrame()
創建關鍵幀
在[Tracking::CreateInitialMapMonocular()](#Tracking::CreateInitialMapMonocular())，[Tracking::CreateNewKeyFrame()](#Tracking::CreateNewKeyFrame())中引用

#### 定義

KeyFrame::KeyFrame(Frame &F,                 //輸入幀
                   Map *pMap, 
                   KeyFrameDatabase *pKFDB)

#### 函式流程
1. 獲取ID
2. 將輸入幀的特徵點根據網格進行分配
3. 設置當前關鍵幀的姿態

#### 函式詳解
1. 獲取ID

mnId=nNextId++;

2. 將輸入幀的特徵點根據網格進行分配

mGrid.resize(mnGridCols);
//遍歷網格列
for(int i=0; i<mnGridCols;i++)
{
    mGrid[i].resize(mnGridRows);
    
    //遍歷網格行
    for(int j=0; j<mnGridRows; j++)
        mGrid[i][j] = F.mGrid[i][j];
}

3. 設置當前關鍵幀的姿態

SetPose(F.mTcw);

### KeyFrame::AddMapPoint()
將地圖點加入到當前關鍵幀中的相對印索引值中
在[Tracking::CreateInitialMapMonocular()](#Tracking::CreateInitialMapMonocular())中引用
#### 定義

void KeyFrame::AddMapPoint(MapPoint *pMP,     //地圖點
                           const size_t &idx) //地圖點在關鍵幀中的索引

#### 函式流程
1. 將地圖點加入到當前關鍵幀中的相對印索引值中

#### 函式詳解
1. 將地圖點加入到當前關鍵幀中的相對印索引值中

unique_lock<mutex> lock(mMutexFeatures);
mvpMapPoints[idx]=pMP;

### KeyFrame::ComputeSceneMedianDepth()
對當前關鍵幀下所有地圖點的深度進行由小到大的排序，返回距離頭部其中1/q處的深度值作為當前場景的平均深度。
在[Tracking::CreateInitialMapMonocular()](#Tracking::CreateInitialMapMonocular())，[LocalMapping::CreateNewMapPoints()](#LocalMapping::CreateNewMapPoints())中引用

#### 定義

float KeyFrame::ComputeSceneMedianDepth(const int q) //中值

#### 函式流程
1. 計算變換矩陣
2. 計算所有地圖點深度
3. 計算平均深度

#### 函式詳解
1. 計算變換矩陣

vector<MapPoint*> vpMapPoints;
cv::Mat Tcw_;
{
    unique_lock<mutex> lock(mMutexFeatures);
    unique_lock<mutex> lock2(mMutexPose);
    vpMapPoints = mvpMapPoints;
    Tcw_ = Tcw.clone();
}

vector<float> vDepths;
vDepths.reserve(N);
// 提取深度需要轉換的數值
cv::Mat Rcw2 = Tcw_.row(2).colRange(0,3);
Rcw2 = Rcw2.t();
float zcw = Tcw_.at<float>(2,3);

2. 計算所有地圖點深度

//遍歷每一個地圖點
for(int i=0; i<N; i++)
{
    //如果三維點存在
    if(mvpMapPoints[i])
    {
        MapPoint* pMP = mvpMapPoints[i];
        cv::Mat x3Dw = pMP->GetWorldPos();
        float z = Rcw2.dot(x3Dw)+zcw; // (R*x3Dw+t)的第三行，即z
        vDepths.push_back(z);
    }
}

sort(vDepths.begin(),vDepths.end());

3. 計算平均深度

return vDepths[(vDepths.size()-1)/q];

### KeyFrame::TrackedMapPoints()
關鍵幀觀測到的地圖點的數目
在[Tracking::CreateInitialMapMonocular()](#Tracking::CreateInitialMapMonocular())，與[Tracking::NeedNewKeyFrame()](#Tracking::NeedNewKeyFrame())中引用。

#### 定義

int KeyFrame::TrackedMapPoints(const int &minObs) //最小觀測數目

#### 函式流程
1. 計算有多少成功跟蹤的特徵點

#### 函式詳解
1. 計算有多少成功跟蹤的特徵點

unique_lock<mutex> lock(mMutexFeatures);

int nPoints=0;
const bool bCheckObs = minObs>0;

//遍歷當前幀中所有特徵點
for(int i=0; i<N; i++)
{
    MapPoint* pMP = mvpMapPoints[i];
    if(pMP)
    {
        if(!pMP->isBad())
        {
            if(bCheckObs)
            {
                // 该MapPoint是一个高质量的MapPoint
                if(mvpMapPoints[i]->Observations()>=minObs)
                    nPoints++;
            }
            else
                nPoints++;
        }
    }
}

return nPoints;

### KeyFrame::UnprojectStereo()
在雙目與RGBD模式下將特徵點反投影到空間中，得到世界座標系下的三維點
在[Tracking::UpdateLastFrame()](#Tracking::UpdateLastFrame())中引用

#### 定義

cv::Mat KeyFrame::UnprojectStereo(int i)    //輸入特徵點索引

#### 函式流程
1. 由2維影像投影到相機坐標系
2. 由相機坐標系到世界坐標系

#### 函式詳解
1. 由2維影像投影到相機坐標系

const float z = mvDepth[i];
if(z>0)
{
    //2維影像投影到相機坐標系
    const float u = mvKeys[i].pt.x;
    const float v = mvKeys[i].pt.y;
    const float x = (u-cx)*z*invfx;
    const float y = (v-cy)*z*invfy;
    cv::Mat x3Dc = (cv::Mat_<float>(3,1) << x, y, z);

2. 由相機坐標系到世界坐標系

    unique_lock<mutex> lock(mMutexPose);
    //相機坐標系到世界坐標系
    return Twc.rowRange(0,3).colRange(0,3)*x3Dc+Twc.rowRange(0,3).col(3);
}
else
    return cv::Mat();

### KeyFrame::UpdateConnections()
更新關鍵幀之間的連接關係
在[Tracking::CreateInitialMapMonocular()](#Tracking::CreateInitialMapMonocular())，[LocalMapping::ProcessNewKeyFrame()](#LocalMapping::ProcessNewKeyFrame())，[LocalMapping::SearchInNeighbors()](#LocalMapping::SearchInNeighbors())，[LoopClosing::CorrectLoop()](#LoopClosing::CorrectLoop()) 中引用

#### 函式流程
1. 獲得該關鍵幀的所有地圖點
2. 通過地圖點被關鍵幀觀測來間接統計關鍵幀之間的共視程度
3. 找到對應權重最大的關鍵幀（共視程度最高的關鍵幀）
4. 如果沒有超過閾值的權重，則對權重最大的關鍵幀建立連接
5. 對滿足共視程度的關鍵幀對更新連接關係及權重（從大到小）
6. 更新生成樹的連接

#### 函式詳解
1. 獲得該關鍵幀的所有地圖點

// 關鍵幀-權重，權重為其它關鍵幀與當前關鍵幀共視地圖點的個數，也稱為共視程度
map<KeyFrame*,int> KFcounter; 
vector<MapPoint*> vpMP;

{
    // 獲得該關鍵幀的所有地圖點
    unique_lock<mutex> lockMPs(mMutexFeatures);
    vpMP = mvpMapPoints;
}

2. 通過地圖點被關鍵幀觀測來間接統計關鍵幀之間的共視程度

// 統計每一個地圖點都有多少關鍵幀與當前關鍵幀存在共視關係，統計結果放在KFcounter
for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
{
    MapPoint* pMP = *vit;

    if(!pMP)
        continue;

    if(pMP->isBad())
        continue;

    // 對於每一個地圖點，observations記錄了可以觀測到該地圖點的所有關鍵幀
    map<KeyFrame*,size_t> observations = pMP->GetObservations();

    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        // 除去自身，自己與自己不算共視
        if(mit->first->mnId==mnId)
            continue;
        // 這裡的操作非常精彩！
        // map[key] = value，當要插入的鍵存在時，會覆蓋鍵對應的原來的值。如果鍵不存在，則添加一組鍵值對
        // mit->first 是地圖點看到的關鍵幀，同一個關鍵幀看到的地圖點會累加到該關鍵幀計數
        // 所以最後KFcounter 第一個參數表示某個關鍵幀，第2個參數表示該關鍵幀看到了多少當前幀的地圖點，也就是共視程度
        KFcounter[mit->first]++;
    }
}

// This should not happen
// 沒有共視關係，直接退出 
if(KFcounter.empty())
    return;

// If the counter is greater than threshold add connection
// In case no keyframe counter is over threshold add the one with maximum counter
int nmax=0; // 記錄最高的共視程度
KeyFrame* pKFmax=NULL;
// 至少有15個共視地圖點才會添加共視關係
int th = 15;

3. 找到對應權重最大的關鍵幀（共視程度最高的關鍵幀）

// vPairs記錄與其它關鍵幀共視幀數大於th的關鍵幀
// pair<int,KeyFrame*>將關鍵幀的權重寫在前面，關鍵幀寫在後面方便後面排序
vector<pair<int,KeyFrame*> > vPairs;
vPairs.reserve(KFcounter.size());
for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
{
    if(mit->second>nmax)
    {
        nmax=mit->second;
        pKFmax=mit->first;
    }

    // 建立共視關係至少需要大於等於th個共視地圖點
    if(mit->second>=th)
    {
        // 對應權重需要大於閾值，對這些關鍵幀建立連接
        vPairs.push_back(make_pair(mit->second,mit->first));
        // 對方關鍵幀也要添加這個信息
        // 更新KFcounter中該關鍵幀的mConnectedKeyFrameWeights
        // 更新其它KeyFrame的mConnectedKeyFrameWeights，更新其它關鍵幀與當前幀的連接權重
        (mit->first)->AddConnection(this,mit->second);
    }
}

4. 如果沒有超過閾值的權重，則對權重最大的關鍵幀建立連接

if(vPairs.empty())
{
    // 如果每個關鍵幀與它共視的關鍵幀的個數都少於th，
    // 那就只更新與其它關鍵幀共視程度最高的關鍵幀的mConnectedKeyFrameWeights
    // 這是對之前th這個閾值可能過高的一個補丁
    vPairs.push_back(make_pair(nmax,pKFmax));
    pKFmax->AddConnection(this,nmax);
}

5. 對滿足共視程度的關鍵幀對更新連接關係及權重（從大到小）

// vPairs裡存的都是相互共視程度比較高的關鍵幀和共視權重，接下來由大到小進行排序
sort(vPairs.begin(),vPairs.end());         // sort函數默認升序排列
// 將排序後的結果分別組織成為兩種數據類型
list<KeyFrame*> lKFs;
list<int> lWs;
for(size_t i=0; i<vPairs.size();i++)
{
    // push_front 後變成了從大到小順序
    lKFs.push_front(vPairs[i].second);
    lWs.push_front(vPairs[i].first);
}

{
    unique_lock<mutex> lockCon(mMutexConnections);

    // mspConnectedKeyFrames = spConnectedKeyFrames;
    // 更新當前幀與其它關鍵幀的連接權重
    // ?bug 這裡直接賦值，會把小於閾值的共視關係也放入mConnectedKeyFrameWeights，會增加計算量
    // ?但後續主要用mvpOrderedConnectedKeyFrames來取共視幀，對結果沒影響
    mConnectedKeyFrameWeights = KFcounter;
    mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
    mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

6. 更新生成樹的連接

    if(mbFirstConnection && mnId!=0)
    {
        // 初始化該關鍵幀的父關鍵幀為共視程度最高的那個關鍵幀
        mpParent = mvpOrderedConnectedKeyFrames.front();
        // 建立雙向連接關係，將當前關鍵幀作為其子關鍵幀
        mpParent->AddChild(this);
        mbFirstConnection = false;
    }
}

### KeyFrame::GetFeaturesInArea()
獲取某個特徵點的鄰域中的特徵點id,其實這個和Frame.cpp中的GetFeaturesInArea函數基本上都是一致的;
在[ORBmatcher::SearchByProjection()局部地圖點](#ORBmatcher::SearchByProjection()局部地圖點)，[ORBmatcher::Fuse()](#ORBmatcher::Fuse())中引用

#### 定義

vector<size_t> KeyFrame::GetFeaturesInArea(const float &x, // 特徵點坐標x
                                           const float &y, // 特徵點坐標y
                                           const float &r  // 邊長（半徑）
                                           ) const

#### 函式流程

#### 函式詳解

vector<size_t> vIndices;
vIndices.reserve(N);

// 計算要搜索的cell的範圍

// floor向下取整，mfGridElementWidthInv 為每個像素佔多少個格子
const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));
if(nMinCellX>=mnGridCols)
    return vIndices;

// ceil向上取整
const int nMaxCellX = min((int)mnGridCols-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
if(nMaxCellX<0)
    return vIndices;

const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
if(nMinCellY>=mnGridRows)
    return vIndices;

const int nMaxCellY = min((int)mnGridRows-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
if(nMaxCellY<0)
    return vIndices;

// 遍歷每個cell,取出其中每個cell中的點,並且每個點都要計算是否在鄰域內
for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
{
    for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
    {
        const vector<size_t> vCell = mGrid[ix][iy];
        for(size_t j=0, jend=vCell.size(); j<jend; j++)
        {
            const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
            const float distx = kpUn.pt.x-x;
            const float disty = kpUn.pt.y-y;

            if(fabs(distx)<r && fabs(disty)<r)
                vIndices.push_back(vCell[j]);
        }
    }
}

return vIndices;

### KeyFrame::EraseMapPointMatch()ID
根據關鍵幀ID刪除與地圖點的匹配關係
在[MapPoint::SetBadFlag()](#MapPoint::SetBadFlag())中引用

#### 定義

void KeyFrame::EraseMapPointMatch(const size_t &idx) //輸入關鍵幀ID

#### 函式流程
1. 根據關鍵幀ID刪除與地圖點的匹配關係

#### 函式詳解
1. 根據關鍵幀ID刪除與地圖點的匹配關係

unique_lock<mutex> lock(mMutexFeatures);
mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);

### KeyFrame::GetBestCovisibilityKeyFrames()
得到與該關鍵幀連接的前N個關鍵幀(按大到小排序)
在[LocalMapping::SearchInNeighbors()](#LocalMapping::SearchInNeighbors())，[KeyFrameDatabase::DetectLoopCandidates()](#KeyFrameDatabase::DetectLoopCandidates())中引用

#### 定義

vector<KeyFrame*> KeyFrame::GetBestCovisibilityKeyFrames(const int &N) //要得到幾個關鍵幀

#### 函式流程
1. 回傳前N個關鍵幀

#### 函式詳解
1. 回傳前N個關鍵幀

unique_lock<mutex> lock(mMutexConnections);
if((int)mvpOrderedConnectedKeyFrames.size()<N)
    return mvpOrderedConnectedKeyFrames;
else
    return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(),mvpOrderedConnectedKeyFrames.begin()+N);

### KeyFrame::GetConnectedKeyFrames()
得到與該關鍵幀連接的關鍵幀
在[KeyFrameDatabase::DetectLoopCandidates()](#KeyFrameDatabase::DetectLoopCandidates())，[LoopClosing::DetectLoop()](#LoopClosing::DetectLoop())中引用

#### 函式流程
1. 得到與該關鍵幀連接的關鍵幀

#### 函式詳解
1. 得到與該關鍵幀連接的關鍵幀

unique_lock<mutex> lock(mMutexConnections);
set<KeyFrame*> s;
for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin();mit!=mConnectedKeyFrameWeights.end();mit++)
    s.insert(mit->first);
return s;

mConnectedKeyFrameWeights : 在[KeyFrame::UpdateConnections()](#KeyFrame::UpdateConnections())中更新