ORB-SLAM（五）KeyFrame類

KeyFrame類利用Frame類來構造。對於什么樣的Frame可以認為是關鍵幀以及何時需要加入關鍵幀，是實現在tracking模塊中的。

由於KeyFrame中一部分數據會被多個線程訪問修改，因此需要在這些成員中加線程鎖，保證同一時間只有一個線程有訪問權。涉及線程安全的有：

關鍵幀位姿的設置（lock(mMutexPose)）；

關鍵幀間連接關系的設置（lock(mMutexConnections)）；

關鍵幀對應地圖點的操作（lock(mMutexFeatures)），包括通過地圖點計算相連關鍵幀之間的權重。

1. 設置相機位姿參數：設置KeyFrame中成員變量mTcw，mTwc，Ow（左目相機中心坐標），和Cw（雙目相機baseline中點坐標），相機坐標Z朝北，X朝東，Y朝地。

並給出了get函數獲取姿態參數。

注意

• 這里的Ow等價於twc，表示當前相機光心在世界坐標系下的三維坐標
• Tcw直接求逆計算量比較大，一般矩陣求逆在實現時都會用等價的矩陣表達式去表示，這里Ow就對應Tcw^-1中的平移向量-R^Tt.

void KeyFrame::SetPose(const cv::Mat &Tcw_)
{
    unique_lock<mutex> lock(mMutexPose);
    Tcw_.copyTo(Tcw);
    cv::Mat Rcw = Tcw.rowRange(0,3).colRange(0,3);
    cv::Mat tcw = Tcw.rowRange(0,3).col(3);
    cv::Mat Rwc = Rcw.t();
    Ow = -Rwc*tcw;
    
    Twc = cv::Mat::eye(4,4,Tcw.type());
    Rwc.copyTo(Twc.rowRange(0,3).colRange(0,3));
    Ow.copyTo(Twc.rowRange(0,3).col(3));
    cv::Mat center = (cv::Mat_<float>(4,1) << mHalfBaseline, 0 , 0, 1);
    Cw = Twc*center;
}

2. 為關鍵幀之間添加連接，通過關鍵幀之間的weight連接，weight指的是兩個關鍵幀之間共同觀測到的地圖點：（注意這里都是接口函數，真實的建立連接關系使用的是void KeyFrame::UpdateConnections()函數）

使用的數據結構是：

std::map<KeyFrame*,int> mConnectedKeyFrameWeights

每一個關鍵幀都會維護一個自己的map，其中記錄了與其他關鍵幀之間的weight。每次為當前關鍵幀添加新的連接關鍵幀后，都需要根據weight對map結構重新排序，

UpdateBestCovisibles();

並更新這兩個向量：

mvpOrderedConnectedKeyFrames
mvOrderedWeights

由於map結構沒有sort函數，需要將元素取出放入一個pair組成的vector中，排序后放入上面這兩個向量中

vector<pair<int,KeyFrame*> > vPairs;
vPairs.reserve(mConnectedKeyFrameWeights.size());
for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
    vPairs.push_back(make_pair(mit->second,mit->first));

sort(vPairs.begin(),vPairs.end());
list<KeyFrame*> lKFs; // keyframe
list<int> lWs; // weight
for(size_t i=0, iend=vPairs.size(); i<iend;i++)
{
    lKFs.push_front(vPairs[i].second);
    lWs.push_front(vPairs[i].first);
}

mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

還有幾個相關的API：

set<KeyFrame*> KeyFrame::GetConnectedKeyFrames();
vector<KeyFrame*> KeyFrame::GetVectorCovisibleKeyFrames();

vector<KeyFrame*> KeyFrame::GetBestCovisibilityKeyFrames(const int &N)；
vector<KeyFrame*> KeyFrame::GetCovisiblesByWeight(const int &w)；

int KeyFrame::GetWeight(KeyFrame *pKF)；

都是返回連接的關鍵幀；

前兩個返回所有連接的關鍵幀，區別在於一個未排序(set)，一個排序(vector)。// set是關聯容器 vector是順序容器

中間兩個返回滿足一定閾值（前N個，權重大於等於w）的關鍵幀，最后一個返回指定幀與當前幀間的權重。

3. 當前幀對應的地圖點的指針均存放在mvpMapPoints（mvp代表：member、vector、pointer）向量中，通過對mvpMapPoints操作封裝，可以得到以下API：

void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx);
void KeyFrame::EraseMapPointMatch(const size_t &idx);
void KeyFrame::EraseMapPointMatch(MapPoint* pMP);
void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP);

// 注意區別下面兩個
set<MapPoint*> KeyFrame::GetMapPoints();
vector<MapPoint*> KeyFrame::GetMapPointMatches();
MapPoint* KeyFrame::GetMapPoint(const size_t &idx);

// 返回高質量MapPoints（被至少minObs個關鍵幀觀察到）的數量，其中會判斷MapPoint的Observations()屬性，對比給出的閾值
int KeyFrame::TrackedMapPoints(const int &minObs);

mvpMapPoints初始化在Frame.cpp中：

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

其中有N個空指針，因此有的位置上的MapPoint並沒有指向實際的地圖點（雖然對應有特征點，有索引idx，但是是外點），獲取時需要注意。

4. UpdateConnections()函數：建立關鍵幀之間的連接關系