试图学会ORB-SLAM2(3)——Frame类

ORB-SLAM系列有关键帧KeyFrame的概念，但当图像刚刚传入的时候，都是先构造成为普通帧Frame的(以单目为例，具体的构造在System里的GrabImageMonocular函数中)，之后会对是否设为关键帧判断，但特征提取等操作都是直接在Frame里完成的。

一些成员变量

变量名	访问控制	简单解释
ORBVocabulary* mpORBvocabulary	public	ORB词典，主要在重定位中使用
ORBextractor* mpORBextractorLeft ORBextractor *mpORBextractorRight	public	ORB特征提取器，其中右提取器只有双目才会用到
double mTimeStamp	public	时间戳，就是当前帧的时间
cv::Mat mDistCoef	public	畸变参数矩阵
float mThDepth	public	近远点的判断阈值，主要在双目和RGBD三角化中使用
std::vector<cv::KeyPoint> mvKeys std::vector<cv::KeyPoint> mvKeysRight std::vector<cv::KeyPoint> mvKeysUn	public	左目特征点(未校正畸变) 右目特征点(未校正畸变) 校正后
std::vector<float> mvuRight	public	双目中右图的特征点
std::vector<float> mvDepth	public	对应的深度
DBoW2::BowVector mBowVec DBoW2::FeatureVector mFeatVec	public	特征向量 所有特征点在图像中的索引
cv::Mat mDescriptors, mDescriptorsRight	public	左右目对应的特征点描述子
std::vector<MapPoint*> mvpMapPoints	public	特征点对应的地图点，没有的话就是空指针
std::vector<bool> mvbOutlier	public	观测不到的外点，主要在优化中使用
static float mfGridElementWidthInv static float mfGridElementHeightInv	public	后边把图像分成小格之后，判断图像在哪个格子
std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS]	public	把图像分成48行64列的小格
cv::Mat mTcw	public	相机姿态，w 到 c的变换矩阵
static long unsigned int nNextId long unsigned int mnId	public	下一帧的ID和当前帧的ID
KeyFrame* mpReferenceKF	public	普通帧的位姿会以参考关键帧为基准，这里便是普通帧对应的参考关键帧
int mnScaleLevels float mfScaleFactor float mfLogScaleFactor vector<float> mvScaleFactors vector<float> mvInvScaleFactors	public	图像金字塔相关参数
vector<float> mvLevelSigma2 vector<float> mvInvLevelSigma2	public	依旧是图像金字塔信息，这个主要是判断出来特征点在哪一层时使用
static bool mbInitialComputations	public	判断是否进行了初始化计算(图像边界)
cv::Mat mRcw, mtcw, mRwc, mOw	private	相机位姿相关的变量

成员函数

拷贝构造函数Frame::Frame(const Frame &frame)

因为ORB-SLAM2中会频繁地出现用一帧直接给另一帧赋值的操作，因此在Frame类中设计了拷贝构造函数，利用列表初始化的方法，直接给成员变量赋值。
mGrid变量是通过对其遍历进行的逐个复制，还有就是如果这个帧没有位姿信息，就把mTcw变量设为当前帧的位姿。

为不同传感器准备的构造函数Frame

传感器类型	参数
双目	const cv::Mat &imLeft 左目图像 const cv::Mat &imRight 右目图像 const double &timeStamp 时间戳 ORBextractor* extractorLeft 左目特征提取器 ORBextractor* extractorRight 右目特征提取器 ORBVocabulary* voc ORB字典 cv::Mat &K 相机内参矩阵 cv::Mat &distCoef 去畸变参数 const float &bf 基线长 const float &thDepth 远近点区分阈值
RGBD	const cv::Mat &imGray 灰度图像 const cv::Mat &imDepth 深度图像 const double &timeStamp 时间戳 ORBextractor* extractor 特征提取器 ORBVocabulary* voc ORB字典 cv::Mat &K 相机内参矩阵 cv::Mat &distCoef 去畸变参数 const float &bf 基线长 const float &thDepth 远近点区分阈值
单目	const cv::Mat &imGray 灰度图像 const double &timeStamp 时间戳 ORBextractor* extractor 特征提取器 ORBVocabulary* voc ORB字典 cv::Mat &K 相机内参矩阵 cv::Mat &distCoef 去畸变参数 const float &bf 基线长 const float &thDepth 远近点区分阈值

三种传感器对应的构造函数大体流程相同，这里以单目为例进行分析，先梳理一下大概流程，涉及到的函数后边再看。
首先是进行一个帧ID的自增，帧的ID也就是帧的序号，之后会通过ID来把帧的一些特征与序号进行连接；之后是获取得到图像金字塔的参数，这里的参数都是在配置文件中设置的，所以这里只需要调用函数(获取这些参数的函数都在ORBextractor类中)得到值并赋值给对应的成员变量即可：

//获取图像金字塔的层数
mnScaleLevels = mpORBextractorLeft->GetLevels();
//获取每层的缩放因子
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();    
//计算每层缩放因子的自然对数
mfLogScaleFactor = log(mfScaleFactor);
//获取各层图像的缩放因子
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
//获取各层图像的缩放因子的倒数
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
//这个主要用在后面反推特征点在金字塔的第几层
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
//计算上面获取的sigma^2的倒数
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

• 随后对图像进行ORB特征点的提取：

ExtractORB(0,imGray);
//获取特征点的个数
N = mvKeys.size();
//如果这一帧没有能够提取出特征点，那么就直接返回了
if(mvKeys.empty())
	return;

• 利用OpenCV的去畸变函数，对特征点去畸变，这里需要注意的是只是对特征点进行去畸变处理

UndistortKeyPoints();
// 单目是没有右目图像和深度信息的，这里就把对应的成员变量都设为-1了
mvuRight = vector<float>(N,-1);
mvDepth = vector<float>(N,-1);

• 再初始化本帧的地图点(数目设为和特征点数目一致)

// 初始化本帧的地图点
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
// 记录地图点是否为外点，初始化均为外点false
mvbOutlier = vector<bool>(N,false);

• 计算去畸变后的图像边界

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

//计算 basline
mb = mbf/fx;

• 最后把特征点分配到网格中，会在特征匹配中使用，能够加速匹配

AssignFeaturesToGrid();

构造函数中用到的函数1，提取特征点ExtractORB

ExtractORB其实也只是个入口，特征点的提取都在ORBextract类中，这个下一篇文章再分析。
ORB-SLAM2这里是通过仿函数实现的，用到了Frame类的成员变量mpORBextractorLeft，提取出来的特征点和描述子也分别保存在类成员变量mvKeys和mDescriptors中。

// 判断是左图还是右图
if(flag==0)
	// 左图的话就套使用左图指定的特征点提取器，并将提取结果保存到对应的变量中 
	// 这里使用了仿函数来完成，重载了括号运算符 ORBextractor::operator() 
	(*mpORBextractorLeft)(im,				//待提取特征点的图像
						  cv::Mat(),		//掩摸图像, 实际没有用到
						  mvKeys,			//输出变量，用于保存提取后的特征点
						  mDescriptors);	//输出变量，用于保存特征点的描述子
else
	// 右图的话就需要使用右图指定的特征点提取器，并将提取结果保存到对应的变量中 
	(*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);

构造函数中用到的函数2，特征点去畸变UndistortKeyPoints

• 首先判断畸变参数是不是0，因为一般第一个参数最重要，所以直接就判断第一个

if(mDistCoef.at<float>(0)==0.0)
{
	// 为0说明不需要校正，那就直接把mvKeys当作校正好的就可以
	mvKeysUn=mvKeys;
	return;
}

• 不是0说明需要校正，这里用的是OpenCV自带的函数cv::undistortPoints

// OpenCV官方
[https://docs.opencv.org/3.4/da/d54/group__imgproc__transform.html#ga7dfb72c9cf9780a347fbe3d1c47e5d5a]
void cv::undistortPoints(InputArray 	src,
						 OutputArray 	dst,
 						 InputArray 	cameraMatrix,
						 InputArray 	distCoeffs,
						 InputArray 	R = noArray(),
						 InputArray 	P = noArray() 
						)		

为了能直接使用这个函数，先把特征点坐标保存成N*2的形式

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

随后又用了一个reshape函数，来改变图像的通道数(也是为了能直接使用OpenCV自带的函数)。图像通道数我个人的理解就是一个点能放几个数，比如这里我们输入的是灰度图像，那么就是通道数为1，如果是RGB图像的话，应该就是3

• 疑惑：看参数里是可以N*2通道为1的输入的啊，为啥非得转一下，是OpenCV版本不同么？
  当然我们在校正后也把通道数为2的图像又转回来了

mat=mat.reshape(2);
cv::undistortPoints(	
					mat,				//输入的特征点坐标
					mat,				//输出的校正后的特征点坐标覆盖原矩阵
					mK,					//相机的内参数矩阵
					mDistCoef,			//相机畸变参数矩阵
					cv::Mat(),			//一个空矩阵，对应为函数原型中的R
					mK); 				//新内参数矩阵，对应为函数原型中的P
//调整回只有一个通道，回归我们正常的处理方式
mat=mat.reshape(1);

• 最后就是把校正后的点都存在mvKeysUn，畸变校正就结束了，再强调一遍，这里只是对特征点进行了畸变校正

构造函数中用到的函数3，图像边界去畸变ComputeImageBounds

前边是对特征点进行了去畸变处理，这里的作用我感觉是为了防止去畸变后的特征点不在图像里了，所以把图像的边界也进行了去畸变处理(其实只是对四个角进行操作，然后再画成一个规则的矩形)

void Frame::ComputeImageBounds(const cv::Mat &imLeft)	
{
    // 如果畸变参数不为0，用OpenCV函数进行畸变矫正
    if(mDistCoef.at<float>(0)!=0.0)
	{
        // 保存矫正前的图像四个边界点坐标： (0,0) (cols,0) (0,rows) (cols,rows)
        cv::Mat mat(4,2,CV_32F);
        mat.at<float>(0,0)=0.0;         //左上
		mat.at<float>(0,1)=0.0;
        mat.at<float>(1,0)=imLeft.cols; //右上
		mat.at<float>(1,1)=0.0;
		mat.at<float>(2,0)=0.0;         //左下
		mat.at<float>(2,1)=imLeft.rows;
        mat.at<float>(3,0)=imLeft.cols; //右下
		mat.at<float>(3,1)=imLeft.rows;

        // Undistort corners
		// 和前面校正特征点一样的操作，将这几个边界点作为输入进行校正
        mat=mat.reshape(2);
        cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
        mat=mat.reshape(1);

		//校正后的四个边界点已经不能够围成一个严格的矩形，因此在这个四边形的外侧加边框作为坐标的边界
        mnMinX = min(mat.at<float>(0,0),mat.at<float>(2,0));//左上和左下横坐标最小的
        mnMaxX = max(mat.at<float>(1,0),mat.at<float>(3,0));//右上和右下横坐标最大的
        mnMinY = min(mat.at<float>(0,1),mat.at<float>(1,1));//左上和右上纵坐标最小的
        mnMaxY = max(mat.at<float>(2,1),mat.at<float>(3,1));//左下和右下纵坐标最小的
    }
    else
    {
        // 如果畸变参数为0，就直接获得图像边界
        mnMinX = 0.0f;
        mnMaxX = imLeft.cols;
        mnMinY = 0.0f;
        mnMaxY = imLeft.rows;
    }
}

构造函数中用到的函数4，特征点分配到网格AssignFeaturesToGrid

之前提取得到的特征点坐标都是相对于整个图像的，ORB-SLAM2为了之后能加速特征匹配的过程，这里采用了一种把特征点分格化的操作，用到的就是之前的成员变量mGrid，每个格子对应的是特征点的索引

• 先是对每个格子的空间进行预分配，因为后边会不断的pushback特征点索引进来，所以就先给每个格子预分配一个空间

//给存储特征点的网格数组 Frame::mGrid 预分配空间
// FRAME_GRID_COLS = 64，FRAME_GRID_ROWS=48
int 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);

• 个人理解：这里用的空间是0.5*N/网格数，个人想到最合理的解释就是为了节省一定的内存空间，因为可能有些格子里没有那么多，然后vector的capacity又是两倍两倍增长的，如果这个空间够用就不增加，不够用就增加到N/网格数，以此类推，这样的空间分配确实比较合理
• 下一步就是把特征点索引放到对应的网格了，这里用到了另一个成员函数PosInGrid

for(int i=0;i<N;i++)
{
	//从类的成员变量中获取已经去畸变后的特征点
	const cv::KeyPoint &kp = mvKeysUn[i];

	//存储某个特征点所在网格的网格坐标
	int nGridPosX, nGridPosY;
	if(PosInGrid(kp,nGridPosX,nGridPosY))
	//如果找到特征点所在网格坐标，将这个特征点的索引添加到对应网格的数组mGrid中
	mGrid[nGridPosX][nGridPosY].push_back(i);
}

判断特征点是否在对应的网格中PosInGrid

这个函数的逻辑非常简单，里面用到了几个成员变量，首先就是去畸变后的图像边界，然后就是mfGridElementWidthInv和mfGridElementHeightInv，这个表示一个像素占了多少格，特征点的坐标是像素坐标，乘一下这个变量再取整就是对应的格数了

bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)
{
	// 计算特征点x,y坐标落在哪个网格内，网格坐标为posX，posY
    posX = round((kp.pt.x-mnMinX)*mfGridElementWidthInv);
    posY = round((kp.pt.y-mnMinY)*mfGridElementHeightInv);

    //Keypoint's coordinates are undistorted, which could cause to go out of the image
    // 因为特征点进行了去畸变，而且前面计算是round取整，所以有可能得到的点落在图像网格坐标外面
    // 如果网格坐标posX，posY超出了[0,FRAME_GRID_COLS] 和[0,FRAME_GRID_ROWS]，表示该特征点没有对应网格坐标，返回false
    if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS)
        return false;
	// 计算成功返回true
    return true;
}

跟踪线程用到的函数1，判断地图点是否在当前帧视野中isInFrustum

先来看一下这个函数的使用环境，在跟踪线程中，我们可能需要利用局部地图点来进行更多的匹配，那么能把地图点和一帧进行匹配的基本条件就是当前帧能观测到这个地图点

// Project (this fills MapPoint variables for matching)
// 判断地图点是否在在当前帧视野内
if(mCurrentFrame.isInFrustum(pMP,0.5))
{
	// 观测到该点的帧数加1
	pMP->IncreaseVisible();
	// 只有在视野范围内的地图点才参与之后的投影匹配
	nToMatch++;
}

• 首先就是获取地图点的三维坐标，并把坐标转到当前相机坐标系下

// 获得这个地图点的世界坐标
cv::Mat P = pMP->GetWorldPos(); 
// 3D in camera coordinates
// 根据当前帧(粗糙)位姿转化到当前相机坐标系下的三维点Pc
const cv::Mat Pc = mRcw*P+mtcw; 
const float &PcX = Pc.at<float>(0);
const float &PcY = Pc.at<float>(1);
const float &PcZ = Pc.at<float>(2);

• 随后分别利用几个条件进行判断
• 1 深度值必须为正

 if(PcZ<0.0f)
 	return false;

• 2 把地图点投影到当前真的像素坐标下，那么像素坐标必须在图像有效范围内

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;

• 3 计算地图点到相机光心的距离，必须在有效范围内(这里使用了地图点类的一个概念，简单的说就是一个地图点最远和最近分别可以被多远看到)

// 得到认为的可靠距离范围:[0.8f*mfMinDistance, 1.2f*mfMaxDistance]
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();

// 得到当前地图点距离当前帧相机光心的距离,注意P，mOw都是在同一坐标系下才可以
const cv::Mat PO = P-mOw;
//取模就得到了距离
const float dist = cv::norm(PO);

//如果不在有效范围内，认为投影不可靠
if(dist<minDistance || dist>maxDistance)
	return false;

• 4 计算当前相机指向地图点向量和地图点的平均观测方向夹角，小于60°才能进入下一步(这里也是用到了地图点里的一个概念，就是平均观测方向，我理解就是我根据所有观测到地图点的帧，那么我能得到一个所有地图点观测向量的平均，这样如果这个点到当前帧的观测与这个平均相差太大，就认为这个不可观测到了)

cv::Mat Pn = pMP->GetNormal();

// 计算当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值，注意平均观测方向为单位向量
const float viewCos = PO.dot(Pn)/dist;

//夹角要在60°范围内，否则认为观测方向太偏了，重投影不可靠，返回false
if(viewCos<viewingCosLimit)
	return false;

• 之后又调用地图点的函数，来预测当前地图点在当前帧的哪一层

const int nPredictedLevel = pMP->PredictScale(dist,		//这个点到光心的距离
											  this);	//当前帧

• 最后给当前地图点的一些成员变量赋值，具体的含义在之后地图点的类里再说

// 通过置位标记 MapPoint::mbTrackInView 来表示这个地图点要被投影 
pMP->mbTrackInView = true;	
// 该地图点投影在当前图像（一般是左图）的像素横坐标
pMP->mTrackProjX = u;	
// bf/z其实是视差，相减得到右图（如有）中对应点的横坐标
pMP->mTrackProjXR = u - mbf*invz; 
// 该地图点投影在当前图像（一般是左图）的像素纵坐标									
pMP->mTrackProjY = v;				
// 根据地图点到光心距离，预测的该地图点的尺度层级
pMP->mnTrackScaleLevel = nPredictedLevel;		
// 保存当前相机指向地图点向量和地图点的平均观测方向夹角的余弦值
pMP->mTrackViewCos = viewCos;

如果完成了上述所有步骤，那么说明在视野内了，就返回true

跟踪线程用到的函数2，计算词袋向量ComputeBoW

在跟踪线程中很多地方可能会用到词袋向量，包括KeyFrame类中也会有这个函数，函数就是把描述子向量转换到词袋向量，存在对应的成员变量里

if(mBowVec.empty())
{
// 将描述子mDescriptors转换为DBOW要求的输入格式
vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
// 将特征点的描述子转换成词袋向量mBowVec以及特征向量mFeatVec
mpORBvocabulary->transform(vCurrentDesc,	//当前的描述子vector
						   mBowVec,			//输出，词袋向量，记录的是单词的id及其对应权重TF-IDF值
						   mFeatVec,		//输出，记录node id及其对应的图像 feature对应的索引
						   4);				//4表示从叶节点向前数的层数
}

Converter::toDescriptorVector就是把描述子从cv::Mat转换到了std::vector，然后用DBoW2自带的transform函数进行词袋向量的转换

跟踪线程用到的函数3，把双目立体信息反投影到三维坐标UnprojectStereo

函数的使用场景有双目初始化StereoInitialization、更新上一帧位姿UpdateLastFrame和创造新的关键帧CreateNewKeyFrame，这些函数中双目情况都涉及生成有效的新的地图点，因此需要把地图点的三维坐标更新一下

• 先获取深度

const float z = mvDepth[i];

• 深度值为正的话就反投影，否则就返回

if(z>0)
{
	//获取像素坐标，注意这里是矫正后的特征点的坐标
	const float u = mvKeysUn[i].pt.x;
	const float v = mvKeysUn[i].pt.y;
	//计算在当前相机坐标系下的坐标
	const float x = (u-cx)*z*invfx;
	const float y = (v-cy)*z*invfy;
	//生成三维点（在当前相机坐标系下）
	cv::Mat x3Dc = (cv::Mat_<float>(3,1) << x, y, z);
	//然后计算这个点在世界坐标系下的坐标，这里是对的，但是公式还是要斟酌一下。首先变换成在没有旋转的相机坐标系下，最后考虑相机坐标系相对于世界坐标系的平移
	return mRwc*x3Dc+mOw;
}
else
	// 如果深度值不合法，那么就返回一个空矩阵,表示计算失败
	return cv::Mat();

双目匹配ComputeStereoMatches(注意是左右目的匹配)

这个函数的目的是为左目图像上的每一个特征点在右目图像上找到对应的匹配点。

• 首先是声明一些后边可能会用到的变量

// 为匹配结果预先分配内存，数据类型为float型
mvuRight = vector<float>(N,-1.0f);
mvDepth = vector<float>(N,-1.0f);
// orb特征相似度阈值  -> mean ～= (max  + min) / 2
const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2;
// 金字塔底层（0层）图像高 nRows
const 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个特征点.etc
vector<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();

• 行特征点统计，找出对应的匹配点可能存在的行数

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

• 进行匹配，这里需要注意的一个逻辑是因为右目是右边的相机得到的图像，那么左目的特征点只可能存在于右目相机对应坐标的左侧，这个范围就应该是最小视差0和最大视差基线之间

    const float minZ = mb;
    const float minD = 0;			// 最小视差为0，对应无穷远 
    const float maxD = mbf/minZ;    // 最大视差对应的距离是相机的基线

• 在粗匹配之前还会有一些准备工作

// 初始化最佳相似度，用最大相似度，以及最佳匹配点索引
int bestDist = ORBmatcher::TH_HIGH;
size_t bestIdxR = 0;
const cv::Mat &dL = mDescriptors.row(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的相似度dist
		const cv::Mat &dR = mDescriptorsRight.row(iR);
		const int dist = ORBmatcher::DescriptorDistance(dL,dR);

		//统计最小相似度及其对应的列坐标(x)
		if( dist<bestDist ) {
			bestDist = dist;
			bestIdxR = iR;}
	}
}

• 图像块滑动窗口用SAD(Sum of absolute differences，差的绝对和)实现精确匹配
  SAD算法：立体匹配中常用的一种算法，简单来说就是在左右各确定一个像素块，然后计算这两个像素块内像素之差的绝对值的和
  

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的图像块patch
	cv::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的图像快patch
	cv::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;

• 然后利用最佳匹配点和相邻坐标进行亚像素插值

// 使用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>> 公式7

const 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.01
	if( disparity <=0 ) {
	disparity=0.01;
	bestuR = uL-0.01;
}

• 最优视差值/深度选择

// 根据视差值计算深度信息
// 保存最相似点的列坐标(x)信息
// 保存归一化sad最小相似度
mvDepth[iL]=mbf/disparity;
mvuRight[iL] = bestuR;
vDistIdx.push_back(pair<int,int>(bestDist,iL));

• 最后把离群点删除

// 块匹配相似度阈值判断，归一化sad最小，并不代表就一定是匹配的，比如光照变化、弱纹理、无纹理等同样会造成误匹配
// 误匹配判断条件  norm_sad > 1.5 * 1.4 * median
sort(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 code
		mvuRight[vDistIdx[i].second]=-1;
		mvDepth[vDistIdx[i].second]=-1;
	}
}

RGBD图像深度信息计算ComputeStereoFromRGBD

就是RGBD模式下，根据灰度图像和深度图像把立体深度信息计算了出来

void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth)	//参数是深度图像
{
    // mvDepth直接由depth图像读取`
	//这里是初始化这两个存储“右图”匹配特征点横坐标和存储特征点深度值的vector
    mvuRight = vector<float>(N,-1);
    mvDepth = vector<float>(N,-1);

	//开始遍历彩色图像中的所有特征点
    for(int i=0; i<N; i++)
    {
		//获取校正前和校正后的特征点
        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);
        if(d>0)
        {
			//那么就保存这个点的深度
            mvDepth[i] = d;
			//根据这个点的深度计算出等效的、在假想的右图中的该特征点的横坐标
			//TODO 话说为什么要计算这个嘞，计算出来之后有什么用?可能是为了保持计算一致
            mvuRight[i] = kpU.pt.x-mbf/d;
        }//如果获取到的深度点合法
    }//开始遍历彩色图像中的所有特征点
}