0 引言

ORB-SLAM2学习笔记7详细了解了System主类和多线程和ORB-SLAM2学习笔记8详细了解了图像特征点提取和描述子的生成，本文在此基础上，继续学习ORB-SLAM2中的图像帧，也就是Frame类，该类中主要包含设置相机参数、利用双目计算深度及特征点反投影到3D地图点等函数。

请添加图片描述

1 Frame类

1.1 构造和重载函数

首先设置一个无参的Frame构造函数，也方便后续的单目、双目和RBGD相机的有参的Frame重载函数，而且C++允许一个类有多个同名的重载函数，可以通过多个同名重载函数的参数和返回值来进一步区分，但是三者的主要功能类似，都是提取并矫正特征点，然后把特征点划分到网格中等：

//无参的构造函数默认为空
Frame::Frame()
{}

1.1.1 双目相机

双目相机Frame的重载函数，和其他两者的不同：双目使用SAD双目立体匹配算法恢复深度。输入参数主要是左目图像，右目图像，时间戳，左目图像特征点提取器句柄，右目图像特征点提取器句柄，ORB字典句柄，相机内参矩阵，相机去畸变参数，相机基线长度和焦距的乘积及远点和近点的深度区分阈值：

// 为双目相机准备的构造函数Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth):mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),mpReferenceKF(static_cast<KeyFrame*>(NULL))
{// Step 1 帧的ID 自增mnId=nNextId++;// Step 2 计算图像金字塔的参数 //获取图像金字塔的层数mnScaleLevels = mpORBextractorLeft->GetLevels();//这个是获得层与层之前的缩放比mfScaleFactor = mpORBextractorLeft->GetScaleFactor();//计算上面缩放比的对数, NOTICE log=自然对数，log10=才是以10为基底的对数 mfLogScaleFactor = log(mfScaleFactor);//获取每层图像的缩放因子mvScaleFactors = mpORBextractorLeft->GetScaleFactors();//同样获取每层图像缩放因子的倒数mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();//高斯模糊的时候，使用的方差mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();//获取sigma^2的倒数mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();// ORB extraction// Step 3 对左目右目图像提取ORB特征点, 第一个参数0-左图， 1-右图。为加速计算，同时开了两个线程计算thread threadLeft(&Frame::ExtractORB,		//该线程的主函数this,						//当前帧对象的对象指针0,						//表示是左图图像imLeft);					//图像数据//对右目图像提取ORB特征，参数含义同上thread threadRight(&Frame::ExtractORB,this,1,imRight);//等待两张图像特征点提取过程完成threadLeft.join();threadRight.join();//mvKeys中保存的是左图像中的特征点，这里是获取左侧图像中特征点的个数N = mvKeys.size();//如果左图像中没有成功提取到特征点那么就返回，也意味这这一帧的图像无法使用if(mvKeys.empty())return;// Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正// 实际上由于双目输入的图像已经预先经过矫正,所以实际上并没有对特征点进行任何处理操作UndistortKeyPoints();// Step 5 计算双目间特征点的匹配，只有匹配成功的特征点会计算其深度,深度存放在 mvDepth // mvuRight中存储的应该是左图像中的点所匹配的在右图像中的点的横坐标（纵坐标相同）ComputeStereoMatches();// 初始化本帧的地图点mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));   // 记录地图点是否为外点，初始化均为外点falsemvbOutlier = vector<bool>(N,false);// This is done only for the first Frame (or after a change in the calibration)//  Step 5 计算去畸变后图像边界，将特征点分配到网格中。这个过程一般是在第一帧或者是相机标定参数发生变化之后进行if(mbInitialComputations){//计算去畸变后图像的边界ComputeImageBounds(imLeft);// 表示一个图像像素相当于多少个图像网格列（宽）mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);// 表示一个图像像素相当于多少个图像网格行（高）mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);//给类的静态成员变量复制fx = K.at<float>(0,0);fy = K.at<float>(1,1);cx = K.at<float>(0,2);cy = K.at<float>(1,2);// 猜测是因为这种除法计算需要的时间略长，所以这里直接存储了这个中间计算结果invfx = 1.0f/fx;invfy = 1.0f/fy;//特殊的初始化过程完成，标志复位mbInitialComputations=false;}// 双目相机基线长度mb = mbf/fx;// 将特征点分配到图像网格中 AssignFeaturesToGrid();    
}

1.1.2 RGBD相机

RGBD相机Frame的重载函数，和其他两者的不同：RGBD相机自带深度值。输入参数主要是对RGB图像灰度化之后得到的灰度图像，深度图像，时间戳，特征点提取器句柄，ORB特征点词典的句柄，相机内参矩阵，相机去畸变参数，相机基线长度和焦距的乘积及远点和近点的深度区分阈值：

// 为RGBD相机准备的帧构造函数
Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth):mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)
{// Step 1 帧的ID 自增mnId=nNextId++;// Step 2 计算图像金字塔的参数 //获取图像金字塔的层数mnScaleLevels = mpORBextractorLeft->GetLevels();//获取每层的缩放因子mfScaleFactor = mpORBextractorLeft->GetScaleFactor();    //计算每层缩放因子的自然对数mfLogScaleFactor = log(mfScaleFactor);//获取各层图像的缩放因子mvScaleFactors = mpORBextractorLeft->GetScaleFactors();//获取各层图像的缩放因子的倒数mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();//TODO 也是获取这个不知道有什么实际含义的sigma^2mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();//计算上面获取的sigma^2的倒数mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();/** 3. 提取彩色图像(其实现在已经灰度化成为灰度图像了)的特征点 \n Frame::ExtractORB() */// ORB extraction// Step 3 对图像进行提取特征点, 第一个参数0-左图， 1-右图ExtractORB(0,imGray);//获取特征点的个数N = mvKeys.size();//如果这一帧没有能够提取出特征点，那么就直接返回了if(mvKeys.empty())return;// Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正UndistortKeyPoints();// Step 5 获取图像的深度，并且根据这个深度推算其右图中匹配的特征点的视差ComputeStereoFromRGBD(imDepth);// 初始化本帧的地图点mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));// 记录地图点是否为外点，初始化均为外点falsemvbOutlier = vector<bool>(N,false);// This is done only for the first Frame (or after a change in the calibration)//  Step 5 计算去畸变后图像边界，将特征点分配到网格中。这个过程一般是在第一帧或者是相机标定参数发生变化之后进行if(mbInitialComputations){//计算去畸变后图像的边界ComputeImageBounds(imGray);// 表示一个图像像素相当于多少个图像网格列（宽）mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);// 表示一个图像像素相当于多少个图像网格行（高）mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);//给类的静态成员变量复制fx = K.at<float>(0,0);fy = K.at<float>(1,1);cx = K.at<float>(0,2);cy = K.at<float>(1,2);// 猜测是因为这种除法计算需要的时间略长，所以这里直接存储了这个中间计算结果invfx = 1.0f/fx;invfy = 1.0f/fy;//特殊的初始化过程完成，标志复位mbInitialComputations=false;}// 计算假想的基线长度 baseline= mbf/fx// 后面要对从RGBD相机输入的特征点,结合相机基线长度,焦距,以及点的深度等信息来计算其在假想的"右侧图像"上的匹配点mb = mbf/fx;// 将特征点分配到图像网格中 AssignFeaturesToGrid();
}

1.1.3 单目相机

单目相机Frame的重载函数，和其他两者的不同：单目不获取深度，直接把相应变量赋值为-1。输入参数主要是灰度图，时间戳，特征点提取器句柄，ORB特征点词典的句柄，相机内参矩阵，相机去畸变参数，相机基线长度和焦距的乘积及远点和近点的深度区分阈值：

// 单目帧构造函数Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth):mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),mTimeStamp(timeStamp), mK(K.clone()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)
{// Frame ID// Step 1 帧的ID 自增mnId=nNextId++;// Step 2 计算图像金字塔的参数 // Scale Level Info//获取图像金字塔的层数mnScaleLevels = mpORBextractorLeft->GetLevels();//获取每层的缩放因子mfScaleFactor = mpORBextractorLeft->GetScaleFactor();//计算每层缩放因子的自然对数mfLogScaleFactor = log(mfScaleFactor);//获取各层图像的缩放因子mvScaleFactors = mpORBextractorLeft->GetScaleFactors();//获取各层图像的缩放因子的倒数mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();//获取sigma^2mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();//获取sigma^2的倒数mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();// ORB extraction// Step 3 对这个单目图像进行提取特征点, 第一个参数0-左图， 1-右图ExtractORB(0,imGray);//求出特征点的个数N = mvKeys.size();//如果没有能够成功提取出特征点，那么就直接返回了if(mvKeys.empty())return;// Step 4 用OpenCV的矫正函数、内参对提取到的特征点进行矫正 UndistortKeyPoints();// Set no stereo information// 由于单目相机无法直接获得立体信息，所以这里要给右图像对应点和深度赋值-1表示没有相关信息mvuRight = vector<float>(N,-1);mvDepth = vector<float>(N,-1);// 初始化本帧的地图点mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));// 记录地图点是否为外点，初始化均为外点falsemvbOutlier = vector<bool>(N,false);// This is done only for the first Frame (or after a change in the calibration)//  Step 5 计算去畸变后图像边界，将特征点分配到网格中。这个过程一般是在第一帧或者是相机标定参数发生变化之后进行if(mbInitialComputations){// 计算去畸变后图像的边界ComputeImageBounds(imGray);// 表示一个图像像素相当于多少个图像网格列（宽）mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);// 表示一个图像像素相当于多少个图像网格行（高）mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);//给类的静态成员变量复制fx = K.at<float>(0,0);fy = K.at<float>(1,1);cx = K.at<float>(0,2);cy = K.at<float>(1,2);// 猜测是因为这种除法计算需要的时间略长，所以这里直接存储了这个中间计算结果invfx = 1.0f/fx;invfy = 1.0f/fy;//特殊的初始化过程完成，标志复位mbInitialComputations=false;}//计算 baslinemb = mbf/fx;// 将特征点分配到图像网格中 AssignFeaturesToGrid();
}

1.2 成员函数

成员函数	类型	定义
`void Frame::AssignFeaturesToGrid()`	`public`	将提取的`ORB`特征点分配到图像网格中
`void Frame::ExtractORB(int flag, const cv::Mat &im)`	`public`	提取图像的`ORB`特征点，提取的关键点存放在`mvKeys`，描述子存放在`mDescriptors`
`void Frame::SetPose(cv::Mat Tcw)`	`public`	设置相机姿态
`void Frame::UpdatePoseMatrices()`	`public`	根据`Tcw`计算`mRcw`、`mtcw`和`mRwc`、`mOw`
`bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)`	`public`	判断地图点是否在视野中
`vector<size_t> Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel) const`	`public`	找到在以`x,y`为中心,半径为r的圆形内且金字塔层级在[minLevel, maxLevel]的特征点
`bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)`	`public`	计算某个特征点所在网格的网格坐标，如果找到特征点所在的网格坐标，记录在`nGridPosX`,`nGridPosY`里，返回`true`，没找到返回`false`
`void Frame::ComputeBoW()`	`public`	计算当前帧特征点对应的词袋`Bow`，主要是`mBowVec` 和 `mFeatVec`
`void Frame::UndistortKeyPoints()`	`public`	用内参对特征点去畸变，结果报存在`mvKeysUn`中
`void Frame::ComputeImageBounds(const cv::Mat &imLeft)`	`public`	计算去畸变图像的边界
`void Frame::ComputeStereoMatches()`	`public`	双目匹配函数
`void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth)`	`public`	计算`RGBD`图像的立体深度信息
`cv::Mat Frame::UnprojectStereo(const int &i)`	`public`	当某个特征点的深度信息或者双目信息有效时,将它反投影到三维世界坐标系中

其中特征点提取在上一篇已经详细学习了，所以特征点提取后的基础上，还需要对特征点去畸变、双目匹配、特征点分配到网格中等关键问题进一步学习：

1.2.1 特征点去畸变

首先判断第一个畸变系数 $K 1$ 是不是为0，如果不为0，说明需要去畸变，该参数在配置文件中，如EuRoc.yaml；

其次，去畸变调用的是OpenCV中的cv::undistortPoints接口，但除了相机内参和畸变系数的输入，还需要先把特征点的调整为2通道；

最后，遍历所有特征点，并把去畸变后的特征点坐标赋值给原始的未去畸变的特征点坐标。

/*** @brief 用内参对特征点去畸变，结果报存在mvKeysUn中* */
void Frame::UndistortKeyPoints()
{// Step 1 如果第一个畸变参数为0，不需要矫正。第一个畸变参数k1是最重要的，一般不为0，为0的话，说明畸变参数都是0//变量mDistCoef中存储了opencv指定格式的去畸变参数，格式为：(k1,k2,p1,p2,k3)if(mDistCoef.at<float>(0)==0.0){mvKeysUn=mvKeys;return;}// Step 2 如果畸变参数不为0，用OpenCV函数进行畸变矫正// Fill matrix with points// N为提取的特征点数量，为满足OpenCV函数输入要求，将N个特征点保存在N*2的矩阵中cv::Mat mat(N,2,CV_32F);//遍历每个特征点，并将它们的坐标保存到矩阵中for(int i=0; i<N; i++){//然后将这个特征点的横纵坐标分别保存mat.at<float>(i,0)=mvKeys[i].pt.x;mat.at<float>(i,1)=mvKeys[i].pt.y;}// Undistort points// 函数reshape(int cn,int rows=0) 其中cn为更改后的通道数，rows=0表示这个行将保持原来的参数不变//为了能够直接调用opencv的函数来去畸变，需要先将矩阵调整为2通道（对应坐标x,y） mat=mat.reshape(2);cv::undistortPoints(	mat,				//输入的特征点坐标mat,				//输出的校正后的特征点坐标覆盖原矩阵mK,					//相机的内参数矩阵mDistCoef,			//相机畸变参数矩阵cv::Mat(),			//一个空矩阵，对应为函数原型中的RmK); 				//新内参数矩阵，对应为函数原型中的P//调整回只有一个通道，回归我们正常的处理方式mat=mat.reshape(1);// Fill undistorted keypoint vector// Step 存储校正后的特征点mvKeysUn.resize(N);//遍历每一个特征点for(int i=0; i<N; i++){//根据索引获取这个特征点//注意之所以这样做而不是直接重新声明一个特征点对象的目的是，能够得到源特征点对象的其他属性cv::KeyPoint kp = mvKeys[i];//读取校正后的坐标并覆盖老坐标kp.pt.x=mat.at<float>(i,0);kp.pt.y=mat.at<float>(i,1);mvKeysUn[i]=kp;}
}

1.2.2 特征点网格分配

为了加速匹配，ORB-SLAM2在对特征点进行预处理后，会将特征点分配到48行64列的网格中，在程序中也就是一个二维数组来表示。

// 将提取的ORB特征点分配到图像网格中void Frame::AssignFeaturesToGrid()
{// Step 1  给存储特征点的网格数组 Frame::mGrid 预分配空间// FRAME_GRID_COLS = 64，FRAME_GRID_ROWS=48int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);//开始对mGrid这个二维数组中的每一个vector元素遍历并预分配空间for(unsigned int i=0; i<FRAME_GRID_COLS;i++)for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)mGrid[i][j].reserve(nReserve);// Step 2 遍历每个特征点，将每个特征点在mvKeysUn中的索引值放到对应的网格mGrid中for(int i=0;i<N;i++){//从类的成员变量中获取已经去畸变后的特征点const cv::KeyPoint &kp = mvKeysUn[i];//存储某个特征点所在网格的网格坐标，nGridPosX范围：[0,FRAME_GRID_COLS], nGridPosY范围：[0,FRAME_GRID_ROWS]int nGridPosX, nGridPosY;// 计算某个特征点所在网格的网格坐标，如果找到特征点所在的网格坐标，记录在nGridPosX,nGridPosY里，返回true，没找到返回falseif(PosInGrid(kp,nGridPosX,nGridPosY))//如果找到特征点所在网格坐标，将这个特征点的索引添加到对应网格的数组mGrid中mGrid[nGridPosX][nGridPosY].push_back(i);}
}

1.2.3 双目匹配

ORB-SLAM2双目特征点的匹配用的是SAD双目立体视觉匹配算法，主要的过程：（输入两帧立体矫正后的图像img_left 和 img_right 对应的ORB特征点集）

行特征点统计. 统计img_right每一行上的ORB特征点集，便于使用立体匹配思路(行搜索/极线搜索）进行同名点搜索, 避免逐像素的判断；
粗匹配. 根据步骤1的结果，对img_left第i行的ORB特征点pi，在img_right的第i行上的ORB特征点集中搜索相似ORB特征点, 得到qi；
精确匹配. 以点qi为中心，半径为r的范围内，进行块匹配（归一化SAD），进一步优化匹配结果；
亚像素精度优化. 步骤3得到的视差为uchar/int类型精度，并不一定是真实视差，通过亚像素差值（抛物线插值)获取float精度的真实视差；
最优视差值/深度选择. 通过胜者为王算法（WTA）获取最佳匹配点；
删除离群点(outliers). 块匹配相似度阈值判断，归一化SAD最小，并不代表就一定是正确匹配，比如光照变化、弱纹理等会造成误匹配。

最后输出：稀疏特征点视差图/深度图（亚像素精度）mvDepth 匹配结果 mvuRight。

// 双目匹配函数void Frame::ComputeStereoMatches()
{// 为匹配结果预先分配内存，数据类型为float型// mvuRight存储右图匹配点索引// mvDepth存储特征点的深度信息mvuRight = vector<float>(N,-1.0f);mvDepth = vector<float>(N,-1.0f);// orb特征相似度阈值  -> mean ～= (max  + min) / 2const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2;// 金字塔底层（0层）图像高 nRowsconst int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;// 二维vector存储每一行的orb特征点的列坐标的索引，为什么是vector，因为每一行的特征点有可能不一样，例如// vRowIndices[0] = [1，2，5，8, 11]   第1行有5个特征点,他们的列号（即x坐标）分别是1,2,5,8,11// vRowIndices[1] = [2，6，7，9, 13, 17, 20]  第2行有7个特征点.etcvector<vector<size_t> > vRowIndices(nRows, vector<size_t>());for(int i=0; i<nRows; i++) vRowIndices[i].reserve(200);// 右图特征点数量，N表示数量 r表示右图，且不能被修改const int Nr = mvKeysRight.size();// Step 1. 行特征点统计。 考虑用图像金字塔尺度作为偏移，左图中对应右图的一个特征点可能存在于多行，而非唯一的一行for(int iR = 0; iR < Nr; iR++) {// 获取特征点ir的y坐标，即行号const cv::KeyPoint &kp = mvKeysRight[iR];const float &kpY = kp.pt.y;// 计算特征点ir在行方向上，可能的偏移范围r，即可能的行号为[kpY + r, kpY -r]// 2 表示在全尺寸(scale = 1)的情况下，假设有2个像素的偏移，随着尺度变化，r也跟着变化const float r = 2.0f * mvScaleFactors[mvKeysRight[iR].octave];const int maxr = ceil(kpY + r);const int minr = floor(kpY - r);// 将特征点ir保证在可能的行号中for(int yi=minr;yi<=maxr;yi++)vRowIndices[yi].push_back(iR);}// 下面是 粗匹配 + 精匹配的过程// 对于立体矫正后的两张图，在列方向(x)存在最大视差maxd和最小视差mind// 也即是左图中任何一点p，在右图上的匹配点的范围为应该是[p - maxd, p - mind], 而不需要遍历每一行所有的像素// maxd = baseline * length_focal / minZ// mind = baseline * length_focal / maxZconst float minZ = mb;const float minD = 0;			// 最小视差为0，对应无穷远 const float maxD = mbf/minZ;    // 最大视差对应的距离是相机的焦距// 保存sad块匹配相似度和左图特征点索引vector<pair<int, int> > vDistIdx;vDistIdx.reserve(N);// 为左图每一个特征点il，在右图搜索最相似的特征点irfor(int iL=0; iL<N; iL++) {const cv::KeyPoint &kpL = mvKeys[iL];const int &levelL = kpL.octave;const float &vL = kpL.pt.y;const float &uL = kpL.pt.x;// 获取左图特征点il所在行，以及在右图对应行中可能的匹配点const vector<size_t> &vCandidates = vRowIndices[vL];if(vCandidates.empty()) continue;// 计算理论上的最佳搜索范围const float minU = uL-maxD;const float maxU = uL-minD;// 最大搜索范围小于0，说明无匹配点if(maxU<0) continue;// 初始化最佳相似度，用最大相似度，以及最佳匹配点索引int bestDist = ORBmatcher::TH_HIGH;size_t bestIdxR = 0;const cv::Mat &dL = mDescriptors.row(iL);// Step 2. 粗配准。左图特征点il与右图中的可能的匹配点进行逐个比较,得到最相似匹配点的描述子距离和索引for(size_t iC=0; iC<vCandidates.size(); iC++) {const size_t iR = vCandidates[iC];const cv::KeyPoint &kpR = mvKeysRight[iR];// 左图特征点il与待匹配点ic的空间尺度差超过2，放弃if(kpR.octave<levelL-1 || kpR.octave>levelL+1)continue;// 使用列坐标(x)进行匹配，和stereomatch一样const float &uR = kpR.pt.x;// 超出理论搜索范围[minU, maxU]，可能是误匹配，放弃if(uR >= minU && uR <= maxU) {// 计算匹配点il和待匹配点ic的相似度distconst cv::Mat &dR = mDescriptorsRight.row(iR);const int dist = ORBmatcher::DescriptorDistance(dL,dR);//统计最小相似度及其对应的列坐标(x)if( dist<bestDist ) {bestDist = dist;bestIdxR = iR;}}}// Step 3. 图像块滑动窗口用SAD(Sum of absolute differences，差的绝对和)实现精确匹配. if(bestDist<thOrbDist) {// 如果刚才匹配过程中的最佳描述子距离小于给定的阈值// 计算右图特征点x坐标和对应的金字塔尺度const float uR0 = mvKeysRight[bestIdxR].pt.x;const float scaleFactor = mvInvScaleFactors[kpL.octave];// 尺度缩放后的左右图特征点坐标const float scaleduL = round(kpL.pt.x*scaleFactor);			const float scaledvL = round(kpL.pt.y*scaleFactor);const float scaleduR0 = round(uR0*scaleFactor);// 滑动窗口搜索, 类似模版卷积或滤波// w表示sad相似度的窗口半径const int w = 5;// 提取左图中，以特征点(scaleduL,scaledvL)为中心, 半径为w的图像块patchcv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1);IL.convertTo(IL,CV_32F);// 图像块均值归一化，降低亮度变化对相似度计算的影响IL = IL - IL.at<float>(w,w) * cv::Mat::ones(IL.rows,IL.cols,CV_32F);//初始化最佳相似度int bestDist = INT_MAX;// 通过滑动窗口搜索优化，得到的列坐标偏移量int bestincR = 0;//滑动窗口的滑动范围为（-L, L）const int L = 5;// 初始化存储图像块相似度vector<float> vDists;vDists.resize(2*L+1); // 计算滑动窗口滑动范围的边界，因为是块匹配，还要算上图像块的尺寸// 列方向起点 iniu = r0 - 最大窗口滑动范围 - 图像块尺寸// 列方向终点 eniu = r0 + 最大窗口滑动范围 + 图像块尺寸 + 1// 此次 + 1 和下面的提取图像块是列坐标+1是一样的，保证提取的图像块的宽是2 * w + 1// ! 源码： const float iniu = scaleduR0+L-w; 错误// scaleduR0：右图特征点x坐标const float iniu = scaleduR0-L-w;const float endu = scaleduR0+L+w+1;// 判断搜索是否越界if(iniu<0 || endu >= mpORBextractorRight->mvImagePyramid[kpL.octave].cols)continue;// 在搜索范围内从左到右滑动，并计算图像块相似度for(int incR=-L; incR<=+L; incR++) {// 提取右图中，以特征点(scaleduL,scaledvL)为中心, 半径为w的图像快patchcv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1);IR.convertTo(IR,CV_32F);// 图像块均值归一化，降低亮度变化对相似度计算的影响IR = IR - IR.at<float>(w,w) * cv::Mat::ones(IR.rows,IR.cols,CV_32F);// sad 计算，值越小越相似float dist = cv::norm(IL,IR,cv::NORM_L1);// 统计最小sad和偏移量if(dist<bestDist) {bestDist = dist;bestincR = incR;}//L+incR 为refine后的匹配点列坐标(x)vDists[L+incR] = dist; 	}// 搜索窗口越界判断if(bestincR==-L || bestincR==L)continue;// Step 4. 亚像素插值, 使用最佳匹配点及其左右相邻点构成抛物线来得到最小sad的亚像素坐标// 使用3点拟合抛物线的方式，用极小值代替之前计算的最优是差值//    \                 / <- 由视差为14，15，16的相似度拟合的抛物线//      .             .(16)//         .14     .(15) <- int/uchar最佳视差值//              . //           （14.5）<- 真实的视差值//   deltaR = 15.5 - 16 = -0.5// 公式参考opencv sgbm源码中的亚像素插值公式// 或论文<<On Building an Accurate Stereo Matching System on Graphics Hardware>> 公式7const float dist1 = vDists[L+bestincR-1];	const float dist2 = vDists[L+bestincR];const float dist3 = vDists[L+bestincR+1];const float deltaR = (dist1-dist3)/(2.0f*(dist1+dist3-2.0f*dist2));// 亚像素精度的修正量应该是在[-1,1]之间，否则就是误匹配if(deltaR<-1 || deltaR>1)continue;// 根据亚像素精度偏移量delta调整最佳匹配索引float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR);float disparity = (uL-bestuR);if(disparity>=minD && disparity<maxD) {// 如果存在负视差，则约束为0.01if( disparity <=0 ) {disparity=0.01;bestuR = uL-0.01;}// 根据视差值计算深度信息// 保存最相似点的列坐标(x)信息// 保存归一化sad最小相似度// Step 5. 最优视差值/深度选择.mvDepth[iL]=mbf/disparity;mvuRight[iL] = bestuR;vDistIdx.push_back(pair<int,int>(bestDist,iL));}   }}// Step 6. 删除离群点(outliers)// 块匹配相似度阈值判断，归一化sad最小，并不代表就一定是匹配的，比如光照变化、弱纹理、无纹理等同样会造成误匹配// 误匹配判断条件  norm_sad > 1.5 * 1.4 * mediansort(vDistIdx.begin(),vDistIdx.end());const float median = vDistIdx[vDistIdx.size()/2].first;const float thDist = 1.5f*1.4f*median;for(int i=vDistIdx.size()-1;i>=0;i--) {if(vDistIdx[i].first<thDist)break;else {// 误匹配点置为-1，和初始化时保持一直，作为error codemvuRight[vDistIdx[i].second]=-1;mvDepth[vDistIdx[i].second]=-1;}}
}

补充—视差的计算原理三角测量法：

如下图，假设有一点 $P$ 可以同时投影至两个相机，可以根据简单的三角形相似原理，计算点 $P$ 的3D坐标，其中就包括深度 $Z$ ，已知：

$T$ ：基线，表示左右相机光心的距离
$f$ ：焦距
$x^l, x^r$ ：点 $P$ 在左右相机的投影位置
$x_l, x_r$ ：像平面左边缘到 $P$ 投影位置的距离

根据三角形 $\Delta Px^lx^r$ 与 $\Delta PO_lO_r$ 相似，则：
$\begin{gather} \frac{x^lx^r}{T}=\frac{Z-f}{Z} \\ \frac{T-(x_l-x_r)}{T}=\frac{Z-f}{Z} \\ Z=\frac{f·T}{x_l-x_r} \\ Z=\frac{f·T}{d} \end{gather}$
而 $d=x_l-x_r$ 被称为视差， $Z$ 也就是想要求得双目深度。

请添加图片描述

1.2.4 RGBD相机深度计算

ORB-SLAM2中对于RGBD特征点,根据深度信息构造虚拟右目图像：

//计算RGBD图像的立体深度信息
void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth)	//参数是深度图像
{/** 主要步骤如下:.对于彩色图像中的每一个特征点:<ul>  */// mvDepth直接由depth图像读取`//这里是初始化这两个存储“右图”匹配特征点横坐标和存储特征点深度值的vectormvuRight = vector<float>(N,-1);mvDepth = vector<float>(N,-1);//开始遍历彩色图像中的所有特征点for(int i=0; i<N; i++){/** <li> 从<b>未矫正的特征点</b>提供的坐标来读取深度图像拿到这个点的深度数据 </li> *///获取校正前和校正后的特征点const cv::KeyPoint &kp = mvKeys[i];const cv::KeyPoint &kpU = mvKeysUn[i];//获取其横纵坐标，注意 NOTICE 是校正前的特征点的const float &v = kp.pt.y;const float &u = kp.pt.x;//从深度图像中获取这个特征点对应的深度点//NOTE 从这里看对深度图像进行去畸变处理是没有必要的,我们依旧可以直接通过未矫正的特征点的坐标来直接拿到深度数据const float d = imDepth.at<float>(v,u);///** <li> 如果获取到的深度点合法(d>0), 那么就保存这个特征点的深度,并且计算出等效的\在假想的右图中该特征点所匹配的特征点的横坐标 </li>* \n 这个横坐标的计算是 x-mbf/d* \n 其中的x使用的是<b>矫正后的</b>特征点的图像坐标*/if(d>0){//那么就保存这个点的深度mvDepth[i] = d;//根据这个点的深度计算出等效的、在假想的右图中的该特征点的横坐标//TODO 话说为什么要计算这个嘞，计算出来之后有什么用?可能是为了保持计算一致mvuRight[i] = kpU.pt.x-mbf/d;}//如果获取到的深度点合法}//开始遍历彩色图像中的所有特征点
}

1.3 成员变量

成员变量	类型	定义
`mbInitialComputations`	`public`	`static`变量，是否需要为Frame类的相机参数赋值，初始化为false,第一次为相机参数赋值后变为false
`float fx, float fy, float cx, float cy, float invfx, float invfy`	`public`	`static`变量，相机内参
`cv::Mat mK`	`public`	相机内参矩阵
`float mb`	`public`	相机基线`baseline`,相机双目间的距离
`float mbf`	`public`	相机基线`baseline`与焦距的乘积

以上的部分成员变量从配置文件中读取，比如EuRoc.yaml中读取：

Camera.fx: 435.2046959714599
Camera.fy: 435.2046959714599
Camera.cx: 367.4517211914062
Camera.cy: 252.2008514404297Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0Camera.width: 752
Camera.height: 480# Camera frames per second 
Camera.fps: 20.0# stereo baseline times fx
Camera.bf: 47.90639384423901# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1# Close/Far threshold. Baseline times.
ThDepth: 35
...

最后，Frame类大多与相机相关的参数，而且整个系统内的所有Frame对象共享同一份相机参数。

2 Frame类的用途

在ORB-SLAM2中，Frame类是用于表示相机帧的数据结构。它在视觉ORB-SLAM2中起着重要的作用。下面是Frame类的主要用途：

存储图像信息：Frame类包含了相机捕获的图像以及与该图像相关的元数据，如时间戳、相机内参等。这些信息对于后续的特征提取、特征匹配和姿态估计等步骤非常重要；
特征提取和描述子计算：Frame类通过调用ORB特征提取器，从图像中提取关键点（特征点）并计算对应的描述子。这些特征点和描述子会用于后续的特征匹配和地图构建；
特征匹配：Frame类还负责进行特征匹配，将当前帧提取的特征点与之前帧或地图中的特征点进行匹配。这有助于确定相机的运动，并用于后续的姿态估计和地图更新；
相机姿态估计：通过匹配当前帧和之前帧或地图中的特征点，Frame类可以估计相机的姿态（即相机的位置和方向）；
地图构建：Frame类还用于地图的构建。通过将当前帧的特征点与地图中已有的特征点进行匹配，可以更新地图并添加新的地图点。

总之，Frame类在ORB-SLAM2中承担了存储图像数据、特征提取、特征匹配、姿态估计和地图构建等多个功能，除了少数被选为KeyFrame的帧以外，大部分Frame对象的作用仅在于Tracking线程内追踪当前帧位姿，不会对LocalMapping线程和LoopClosing线程产生任何影响，在mLastFrame和mCurrentFrame更新之后就被系统销毁了。

Reference:

https://github.com/raulmur/ORB_SLAM2
https://github.com/electech6/ORB_SLAM2_detailed_comments/tree/master
https://docs.opencv.org/3.4/da/d54/group__imgproc__transform.html#ga55c716492470bfe86b0ee9bf3a1f0f7e
https://blog.csdn.net/u012507022/article/details/51446891
https://zhuanlan.zhihu.com/p/484509910

须知少时凌云志，曾许人间第一流。

⭐️👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍👍🌔