首先会看到long unsigned int类型的数据：
long占四个字节；
int的尺寸和平台有关系：
①在16位的系统中，int 占据2个字节
②在32位系统中，占用4个字节

在这里插入图片描述

关键帧——初始化函数

初始化：关键帧数据库，地图，帧ID，网格用于快速匹配，位姿。

KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB):
    mnFrameId(F.mnId),  mTimeStamp(F.mTimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),
    mfGridElementWidthInv(F.mfGridElementWidthInv), mfGridElementHeightInv(F.mfGridElementHeightInv),
    mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0),
    mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0),
    fx(F.fx), fy(F.fy), cx(F.cx), cy(F.cy), invfx(F.invfx), invfy(F.invfy),
    mbf(F.mbf), mb(F.mb), mThDepth(F.mThDepth), N(F.N), mvKeys(F.mvKeys), mvKeysUn(F.mvKeysUn),
    mvuRight(F.mvuRight), mvDepth(F.mvDepth), mDescriptors(F.mDescriptors.clone()),
    mBowVec(F.mBowVec), mFeatVec(F.mFeatVec), mnScaleLevels(F.mnScaleLevels), mfScaleFactor(F.mfScaleFactor),
    mfLogScaleFactor(F.mfLogScaleFactor), mvScaleFactors(F.mvScaleFactors), mvLevelSigma2(F.mvLevelSigma2),
    mvInvLevelSigma2(F.mvInvLevelSigma2), mnMinX(F.mnMinX), mnMinY(F.mnMinY), mnMaxX(F.mnMaxX),
    mnMaxY(F.mnMaxY), mK(F.mK), mvpMapPoints(F.mvpMapPoints), mpKeyFrameDB(pKFDB),
    mpORBvocabulary(F.mpORBvocabulary), mbFirstConnection(true), mpParent(NULL), mbNotErase(false),
    mbToBeErased(false), mbBad(false), mHalfBaseline(F.mb/2), mpMap(pMap)
{
    mnId=nNextId++;

    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];
    }

    SetPose(F.mTcw);
}

关键帧——计算词袋（BoW bag of words）

首先将二维矩阵表述的描述子矩阵，按照行排列，逐行变为描述子向量；
其次通过词袋模型，将所有描述子分散到树的第四层上，便于查找。

/**
 * @brief Bag of Words Representation
 *
 * 计算mBowVec，并且将描述子分散在第4层上，即mFeatVec记录了属于第i个node的ni个描述子
 * @see ProcessNewKeyFrame()
 */
void KeyFrame::ComputeBoW()
{
	// 此处判断在Frame.cpp中是否已经计算了帧的词袋
    if(mBowVec.empty() || mFeatVec.empty())
    {
        vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
        // Feature vector associate features with nodes in the 4th level (from leaves up)
        // We assume the vocabulary tree has 6 levels, change the 4 otherwise
        mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);
    }
}

关键帧（位姿）——设置位姿

此处涉及到同一个变量，在多线程中的读写，因此此函数内部需要加锁。
计算世界坐标系到相机坐标系的平移向量：

$R_{cw}\cdot P_{w} + t_{cw} = P_{c}$ $P_{w} = R_{cw}^{-1} \cdot P_{c} -R_{cw}^{-1} \cdot t_{cw}$
$P_{w} = T_{wc} \cdot P_{c} = R_{wc} \cdot P_{c} + t_{wc}$
当 $P_{c} = [0, 0, 0]_{}^{T}$ 时，上下两式相等，可以等到世界坐标到相机坐标的平移向量 $t_{wc} = -R_{cw}^{-1} \cdot t_{cw}$ 同时由于旋转矩阵 $R_{cw}$ 与 $R_{wc}$ 是反对称矩阵，因此其转置就是求逆。

$T_{wc}$ 与T_{cw}的换算关系：

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));
    // center为相机坐标系（左目）下，立体相机中心的坐标
    // 立体相机中心点坐标与左目相机坐标之间只是在x轴上相差mHalfBaseline,
    // 因此可以看出，立体相机中两个摄像头的连线为x轴，正方向为左目相机指向右目相机
    cv::Mat center = (cv::Mat_<float>(4,1) << mHalfBaseline, 0 , 0, 1);
    // 世界坐标系下，左目相机中心到立体相机中心的向量，方向由左目相机指向立体相机中心
    Cw = Twc*center;
}

关键帧（位姿）——得到位姿

位姿：相机坐标系到世界坐标系的变换矩阵 $T_{cw}$ ：
$T_{cw} \cdot P_{w} = P_{c}$

cv::Mat KeyFrame::GetPose()
{
    unique_lock<mutex> lock(mMutexPose);
    return Tcw.clone();
}

关键帧（位姿）——得到位姿的逆

其实并不是 $T_{cw}$ 进行 $T_{cw}^{-1}$ 运算，而是为了得到世界坐标到相机坐标的变换矩阵 $T_{wc}$ 。

cv::Mat KeyFrame::GetPoseInverse()
{
    unique_lock<mutex> lock(mMutexPose);
    return Twc.clone();
}

关键帧（位姿）——得到相机中心

指：相机坐标原点对应的世界坐标系下的坐标，同时也可以作为世界坐标到相机坐标的平移向量。

cv::Mat KeyFrame::GetCameraCenter()
{
    unique_lock<mutex> lock(mMutexPose);
    return Ow.clone();
}

关键帧（位姿）——得到立体相机中心

指：双目相机基线中心对应的世界坐标系下的坐标值。

cv::Mat KeyFrame::GetStereoCenter()
{
    unique_lock<mutex> lock(mMutexPose);
    return Cw.clone();
}

关键帧（位姿）——得到旋转矩阵

指： $T_{cw}$ 的前三维构成的3X3矩阵。

cv::Mat KeyFrame::GetRotation()
{
    unique_lock<mutex> lock(mMutexPose);
    return Tcw.rowRange(0,3).colRange(0,3).clone();
}

关键帧（位姿）——得到平移矩阵

指： $T_{cw}$ 第3列形成的1X3的向量。

cv::Mat KeyFrame::GetTranslation()
{
    unique_lock<mutex> lock(mMutexPose);
    return Tcw.rowRange(0,3).col(3).clone();
}

关键帧（共视图）——为关键帧添加连接

需要理解std::map<KeyFrame*,int> mConnectedKeyFrameWeights; ///< 与该关键帧连接的关键帧与权重其中std::map<>形成键/值对（关键帧/权重）。哈希表是不可重复的，链表是可重复的。
在这里插入图片描述

/**
 * @brief 为关键帧之间添加连接
 * 
 * 更新了mConnectedKeyFrameWeights
 * @param pKF    关键帧
 * @param weight 权重，该关键帧与pKF共同观测到的3d点数量
 */
void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
{
    {
        unique_lock<mutex> lock(mMutexConnections);
        // std::map::count函数只可能返回0或1两种情况
        if(!mConnectedKeyFrameWeights.count(pKF)) // count函数返回0，mConnectedKeyFrameWeights中没有pKF，之前没有连接
            mConnectedKeyFrameWeights[pKF]=weight;
        else if(mConnectedKeyFrameWeights[pKF]!=weight) // 之前连接的权重不一样，更新键值（weight）
            mConnectedKeyFrameWeights[pKF]=weight;
        else
            return;
    }
	// 按照权重对连接的关键帧进行排序
    UpdateBestCovisibles();
}

关键帧（共视图）——跟新连接的关键帧排序

利用sort函数对pair数据进行排序；针对vector<pair<int,KeyFrame*> >使用sort函数，默认对first进行升序排列，从小到大。得到排序后的权重序列，以及根据权重序列排列的关键帧序列。
由于map结构没有sort函数，需要将元素取出放入一个pair组成的vector中，排序后放入这两个向量中mvpOrderedConnectedKeyFrames 和 mvOrderedWeights 。

/**
 * @brief 按照权重对连接的关键帧进行排序
 * 
 * 更新后的变量存储在mvpOrderedConnectedKeyFrames和mvOrderedWeights中
 */
void KeyFrame::UpdateBestCovisibles()
{
    unique_lock<mutex> lock(mMutexConnections);
    // http://stackoverflow.com/questions/3389648/difference-between-stdliststdpair-and-stdmap-in-c-stl
    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(mConnectedKeyFrameWeights.size());
    // 取出所有连接的关键帧，mConnectedKeyFrameWeights的类型为std::map<KeyFrame*,int>，而vPairs变量将共视的3D点数放在前面，利于排序
    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());
}

关键帧（共视图）——得到与此关键帧连接的关键帧集合

从已经存储好的连接关键帧的键值对中，将关键帧取出来，放入集合中。

/**
 * @brief 得到与该关键帧连接的关键帧
 * @return 连接的关键帧
 */
set<KeyFrame*> KeyFrame::GetConnectedKeyFrames()
{
    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;
}

关键帧（共视图）——得到共视关键帧（已经排序的关键帧序列）

跟新连接的关键帧排序函数中已经将此序列的关键帧提取出来；

/**
 * @brief 得到与该关键帧连接的关键帧(已按权值排序)
 * @return 连接的关键帧
 */
vector<KeyFrame*> KeyFrame::GetVectorCovisibleKeyFrames()
{
    unique_lock<mutex> lock(mMutexConnections);
    return mvpOrderedConnectedKeyFrames;
}

关键帧（共视图）——得到与该关键帧连接的前N个关键帧

这个是否不应该使用权值排序后的关键帧序列，排序将打乱其时间顺序，怎么确定前N个关键帧哪？
如果按照越靠近该关键帧，其权重越大的思路考虑，使用排序后的关键帧序列，取前N个关键帧，仍然有效。

/**
 * @brief 得到与该关键帧连接的前N个关键帧(已按权值排序)
 * 
 * 如果连接的关键帧少于N，则返回所有连接的关键帧
 * @param N 前N个
 * @return 连接的关键帧
 */
vector<KeyFrame*> KeyFrame::GetBestCovisibilityKeyFrames(const int &N)
{
    unique_lock<mutex> lock(mMutexConnections);
    if((int)mvpOrderedConnectedKeyFrames.size()<N)
        return mvpOrderedConnectedKeyFrames;
    else
        return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(),mvpOrderedConnectedKeyFrames.begin()+N);
}

关键帧（共视图）——得到连接的关键帧中权值大于w的关键帧

需要理解#include <algorithm>中的upper_bound函数。此函数使用条件：数组有序，并且是非降序列。如果是降序，则需要使用其扩展用法lower_bound(迭代器_1, 迭代器_2, x, compare)或者upper_bound(迭代器_1, 迭代器_2, x, compare)
理解vector的反向迭代器（rbegin, rend）与（begin， end）的区别。

/**
 * @brief 得到与该关键帧连接的权重大于等于w的关键帧
 * @param w 权重
 * @return 连接的关键帧
 */
vector<KeyFrame*> KeyFrame::GetCovisiblesByWeight(const int &w)
{
    unique_lock<mutex> lock(mMutexConnections);

    if(mvpOrderedConnectedKeyFrames.empty())
        return vector<KeyFrame*>();

    // http://www.cplusplus.com/reference/algorithm/upper_bound/
    // 从mvOrderedWeights找出第一个大于w的那个迭代器
    // 这里应该使用lower_bound，因为lower_bound是返回小于等于，而"upper_bound只能返回第一个大于的(这个解释是错误的)upper_bound在扩展应用情况下返回的是第一个小于w的迭代器"：：：：：此处原始用法是正确的。
    vector<int>::iterator it = upper_bound(mvOrderedWeights.begin(),mvOrderedWeights.end(),w,KeyFrame::weightComp);
    if(it==mvOrderedWeights.end() && *mvOrderedWeights.rbegin()<w)
        return vector<KeyFrame*>();
    else
    {
        int n = it-mvOrderedWeights.begin();
        return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin()+n);
    }
}

关键帧（共视图）——得到该关键帧与pKF的权重

首先确定pKF关键帧是否存在连接关系；
其次从map的键值对中取出权重值。

/**
 * @brief 得到该关键帧与pKF的权重
 * @param  pKF 关键帧
 * @return     权重
 */
int KeyFrame::GetWeight(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexConnections);
    if(mConnectedKeyFrameWeights.count(pKF))
        return mConnectedKeyFrameWeights[pKF];
    else
        return 0;
}

关键帧（地图点）——增加地图点

讲mappoint.cpp中相关函数计算的地图点，作为关键帧的属性添加进来。

/**
 * @brief Add MapPoint to KeyFrame
 * @param pMP MapPoint
 * @param idx MapPoint在KeyFrame中的索引
 */
void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    mvpMapPoints[idx]=pMP;
}

关键帧（地图点）——根据ID擦除地图点与关键帧的匹配

其中用到了static_cast mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);此ID下的地图点重置为空。

void KeyFrame::EraseMapPointMatch(const size_t &idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
}

关键帧（地图点）——根据地图点删除与关键帧的匹配

首先利用地图与关键帧的映射关系（在mappoint.cpp中的函数），找到对应的ID，然后将此ID下的地图点重置为空。

void KeyFrame::EraseMapPointMatch(MapPoint* pMP)
{
    int idx = pMP->GetIndexInKeyFrame(this);
    if(idx>=0)
        mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
}

关键帧（地图点）——根据关键帧ID匹配地图点

void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP)
{
    mvpMapPoints[idx]=pMP;
}

关键帧（地图点）——获取地图点

    // MapPoints associated to keypoints
    std::vector<MapPoint*> mvpMapPoints;

从上述变量中取出所有的地图点。

set<MapPoint*> KeyFrame::GetMapPoints()
{
    unique_lock<mutex> lock(mMutexFeatures);
    set<MapPoint*> s;
    for(size_t i=0, iend=mvpMapPoints.size(); i<iend; i++)
    {
        if(!mvpMapPoints[i])
            continue;
        MapPoint* pMP = mvpMapPoints[i];
        if(!pMP->isBad())
            s.insert(pMP);
    }
    return s;
}

关键帧（地图点）——跟踪地图点

minObs代表最少被观测到MapPoint的关键帧数量。返回一共多少个关键帧观测到此MapPoint。

/**
 * @brief 关键帧中，大于等于minObs的MapPoints的数量
 * minObs就是一个阈值，大于minObs就表示该MapPoint是一个高质量的MapPoint
 * 一个高质量的MapPoint会被多个KeyFrame观测到，
 * @param  minObs 最小观测
 */
int KeyFrame::TrackedMapPoints(const int &minObs)
{
    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;
}

关键帧（地图点）——获取关键帧的地图点

mvpMapPoints地图点是跟踪、局部地图等输入；
返回的是地图点向量。

/**
 * @brief Get MapPoint Matches
 *
 * 获取该关键帧的MapPoints
 */
vector<MapPoint*> KeyFrame::GetMapPointMatches()
{
    unique_lock<mutex> lock(mMutexFeatures);
    return mvpMapPoints;
}

关键帧（地图点）——获取指定ID的地图点

仅仅返回对应的地图点。

MapPoint* KeyFrame::GetMapPoint(const size_t &idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    return mvpMapPoints[idx];
}

关键帧（生长树）——增加子树

将当前帧插入到子树集合中

void KeyFrame::AddChild(KeyFrame *pKF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    mspChildrens.insert(pKF);
}

关键帧（生长树）——删除子树

void KeyFrame::EraseChild(KeyFrame *pKF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    mspChildrens.erase(pKF);
}

关键帧（生长树）——更改父节点

void KeyFrame::ChangeParent(KeyFrame *pKF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    mpParent = pKF;
    pKF->AddChild(this);
}

关键帧（生长树）——获得所有子树节点set

set<KeyFrame*> KeyFrame::GetChilds()
{
    unique_lock<mutex> lockCon(mMutexConnections);
    return mspChildrens;
}

关键帧（生长树）——得到该帧父节点

KeyFrame* KeyFrame::GetParent()
{
    unique_lock<mutex> lockCon(mMutexConnections);
    return mpParent;
}

关键帧（生长树）——关键帧是否存在在子树中

判断某一帧是否含有某一个关键帧子节点

bool KeyFrame::hasChild(KeyFrame *pKF)
{
    unique_lock<mutex> lockCon(mMutexConnections);
    return mspChildrens.count(pKF);
}

雪天枫

发布了44 篇原创文章 · 获赞 8 · 访问量 6734

私信关注

从ORB-SLAM2的源码工程中学习C++编程——关键帧