ORB-SLAM 代码笔记（五）Frame类

Frame类的成员变量主要包含从摄像头获取的图像的

1. 特征点信息（关键点+描述字）

2. 尺寸不变特征所用金字塔信息，这些都定义在ORBextractor对象中

3. 词袋模型参数，用于跟踪失败情况下重定位

4. 相机参数，深度阈值

5. 当前帧的id，时间戳，相对世界坐标系位姿，参考关键帧，特征点对应地图点及其是否是外点

6. 特征点网格分配情况，以及当前帧相对世界坐标的位姿（包括位姿矩阵的更新，以及相机光心坐标）。

每获取一帧图像，mnId++，因此30fps的相机，一秒构建30个Frame对象。

双目，RGB-D和单目对应重载的构造函数。

双目构建的Frame对象：

提取特征加入双线程同步提取，0,1代表左目和右目，两张图片提取的特征点会放在不同的vector中，对于单目和RGB-D来说，右目不使用。

thread threadLeft(&Frame::ExtractORB,this,,imLeft);
thread threadRight(&Frame::ExtractORB,this,,imRight);
threadLeft.join();
threadRight.join();

向量mvKeys中存放N个提取出的左图关键点，mDescriptor中存放提取出的左图描述子，右图的放在mvKeysRight和mDescriptorsRight中；

然后进行畸变矫正（由OpenCV完成），然后进行双目匹配（记录左图中特征点对应的右图坐标，以及恢复出的深度）；

初始化地图点及其外点；

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

最后将关键点分布到64*48分割而成的网格中（目的是加速匹配以及均匀化关键点分布）。

RGBD构建的Frame对象：

和双目类似，但是不需要双目匹配，只需恢复出右图中深度>0的横坐标和深度即可。

单目构建的Frame对象：

和双目类似，但是不包含匹配信息：

mvuRight = vector<float>(N,-);
mvDepth = vector<float>(N,-);

Frame类还提供了一些与自己关联紧密的函数，用于其他模块：

1. 设置当前帧的姿态，并更新当前帧相机在世界坐标系下的位姿，中心点位置；

2. 判断一个MapPoint是否在视角范围内：

bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
{  // 注意这里的MapPoint是从SearchLocalPoint传递进来的，具备一定信息量
    pMP->mbTrackInView = false;
    cv::Mat P = pMP->GetWorldPos(); 

    const cv::Mat Pc = mRcw*P+mtcw; // 这里的R，t是经过初步的优化后的
    const float &PcX = Pc.at<float>();
    const float &PcY = Pc.at<float>();
    const float &PcZ = Pc.at<float>();

    if(PcZ<0.0f)
        return false;

    // Project in image and check it is not outside
    const float invz = 1.0f/PcZ;
    const float u=fx*PcX*invz+cx;
    const float v=fy*PcY*invz+cy;

    if(u<mnMinX || u>mnMaxX)
        return false;
    if(v<mnMinY || v>mnMaxY)
        return false;

    // Check distance is in the scale invariance region of the MapPoint
    // 每一个地图点都是对应于若干尺度的金字塔提取出来的，具有一定的有效深度，如果相对当前帧的深度超过此范围，返回False
    const float maxDistance = pMP->GetMaxDistanceInvariance();
    const float minDistance = pMP->GetMinDistanceInvariance();
    // 世界坐标系下，相机到3D点P的向量, 向量方向由相机指向3D点P
    const cv::Mat PO = P-mOw;
    const float dist = cv::norm(PO);

    if(dist<minDistance || dist>maxDistance)
        return false;

    // Check viewing angle
    // 每一个地图点都有其平均视角，是从能够观测到地图点的帧位姿中计算出
    // 如果当前帧的视角和其平均视角相差太大，返回False
    cv::Mat Pn = pMP->GetNormal();// |Pn|=1

    const float viewCos = PO.dot(Pn)/dist; //  = P0.dot(Pn)/(|P0|*|Pn|); |P0|=dist 

    if(viewCos<viewingCosLimit)
        return false;

    // Predict scale in the image
    // 根据深度预测尺度（对应特征点在一层）
    const int nPredictedLevel = pMP->PredictScale(dist,this);

    // Data used by the tracking
    // 标记该点将来要被投影
    pMP->mbTrackInView = true;
    pMP->mTrackProjX = u;
    pMP->mTrackProjXR = u - mbf*invz; //该3D点投影到双目右侧相机上的横坐标
    pMP->mTrackProjY = v;
    pMP->mnTrackScaleLevel = nPredictedLevel;
    pMP->mTrackViewCos = viewCos;

    return true;
}

3. 在某块区域内获取特帧点：

vector<size_t> Frame::GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel, const int maxLevel) const；

其中，minLevel和maxLevel考察特征点是从图像金字塔的哪一层提取出来的。

4. 将当前帧的描述子矩阵（可以转换成向量），转换成词袋模型向量（DBoW2::BowVector mBowVec； DBoW2::FeatureVector mFeatVec；）：

void Frame::ComputeBoW(){...}

5. 将特征点坐标（给id即可）反投影到3D地图点（世界坐标）：

cv::Mat Frame::UnprojectStereo(const int &i){}

最后，是Frame类中最重要的一个算法，双目匹配以及恢复特征点的深度

图像金字塔：0层是原始图。对0层高斯核卷积后，降采样（删除所有偶数行和偶数列）即可得到高斯金字塔第1层；插入0用高斯卷积恢复成原始大小，与0层相减，得到0层拉普拉斯金字塔，对应的是0层高斯金字塔在滤波降采样过程中丢失的信息，在其上可以提取特征。然后不断降采样，获得不同层的高斯金字塔与拉普拉斯金字塔，提取出的特征对应的尺度与金字塔的层数是正相关的。层数越高，对应的尺度越大，尺度不确定性越高。通过对图像的这种处理，我们可以提取出尺寸不变的的特征。但是在特征匹配时，需要考虑到提取特征点时对应的层数（尺度）。

在匹配左右帧的特征点时，虽然已经经过了极线矫正，但是不能仅仅搜索极线对应的同一行像素点，而应该根据右目提取特征点时的尺度（金字塔层数），确定一个极线附近的扫描范围r，这个带状范围内均包含这个特征信息。

const float r = 2.0f*mvScaleFactors[mvKeysRight[iR].octave];

1. 对左目相机每个特征点，通过描述子在右目带状搜索区域找到匹配点；

2. 通过SAD匹配提高像素匹配修正量bestincR；

3. 做抛物线拟合找谷底得到亚像素匹配deltaR；

4. 剔除SAD匹配偏差较大的匹配特征点。