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()函数：建立关键帧之间的连接关系