ORB-SLAM2的源码阅读（三）：Frame类

看到网上介绍ORB-SLAM2，基本上介绍一个系统的框架，或者按照线程，Tracking,Local Mapping, Loop Closing，介绍大致流程，但是细节讲的不是很多，只能说懂个大概原理。LZ还是自己慢慢阅读吧，虽然这样的代码注释有点啰嗦，请高手勿喷^_^

在ORB-SLAM2中，头文件中作者都写的很清楚各个函数的定义，这个LZ就不在赘述了。

#ifndef FRAME_H
#define FRAME_H

#include<vector>

#include "MapPoint.h"
#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
#include "ORBVocabulary.h"
#include "KeyFrame.h"
#include "ORBextractor.h"

#include <opencv2/opencv.hpp>

namespace ORB_SLAM2
{
#define FRAME_GRID_ROWS 48
#define FRAME_GRID_COLS 64

class MapPoint;
class KeyFrame;

class Frame
{
public:
Frame();

// Copy constructor.
Frame(const Frame &frame);

// Constructor for stereo cameras.
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);

// Constructor for RGB-D cameras.
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);

// Constructor for Monocular cameras.
Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);

// Extract ORB on the image. 0 for left image and 1 for right image.
void ExtractORB(int flag, const cv::Mat &im);

// Compute Bag of Words representation.
void ComputeBoW();

// Set the camera pose.
void SetPose(cv::Mat Tcw);

// Computes rotation, translation and camera center matrices from the camera pose.
void UpdatePoseMatrices();

// Returns the camera center.
inline cv::Mat GetCameraCenter(){
return mOw.clone();
}

// Returns inverse of rotation
inline cv::Mat GetRotationInverse(){
return mRwc.clone();
}

// Check if a MapPoint is in the frustum of the camera
// and fill variables of the MapPoint to be used by the tracking
bool isInFrustum(MapPoint* pMP, float viewingCosLimit);

// Compute the cell of a keypoint (return false if outside the grid)
bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);

vector<size_t> GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel=-1, const int maxLevel=-1) const;

// Search a match for each keypoint in the left image to a keypoint in the right image.
// If there is a match, depth is computed and the right coordinate associated to the left keypoint is stored.
void ComputeStereoMatches();

// Associate a "right" coordinate to a keypoint if there is valid depth in the depthmap.
void ComputeStereoFromRGBD(const cv::Mat &imDepth);

// Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
cv::Mat UnprojectStereo(const int &i);

public:
// Vocabulary used for relocalization.
ORBVocabulary* mpORBvocabulary;

// Feature extractor. The right is used only in the stereo case.
ORBextractor* mpORBextractorLeft, *mpORBextractorRight;

// Frame timestamp.
double mTimeStamp;

// Calibration matrix and OpenCV distortion parameters.
cv::Mat mK;
static float fx;
static float fy;
static float cx;
static float cy;
static float invfx;
static float invfy;
cv::Mat mDistCoef;

// Stereo baseline multiplied by fx.
float mbf;

// Stereo baseline in meters.
float mb;

// Threshold close/far points. Close points are inserted from 1 view.
// Far points are inserted as in the monocular case from 2 views.
float mThDepth;

// Number of KeyPoints.
int N;

// Vector of keypoints (original for visualization) and undistorted (actually used by the system).
// In the stereo case, mvKeysUn is redundant as images must be rectified.
// In the RGB-D case, RGB images can be distorted.
std::vector<cv::KeyPoint> mvKeys, mvKeysRight;
std::vector<cv::KeyPoint> mvKeysUn;

// Corresponding stereo coordinate and depth for each keypoint.
// "Monocular" keypoints have a negative value.
std::vector<float> mvuRight;
std::vector<float> mvDepth;

// Bag of Words Vector structures.
DBoW2::BowVector mBowVec;
DBoW2::FeatureVector mFeatVec;

// ORB descriptor, each row associated to a keypoint.
cv::Mat mDescriptors, mDescriptorsRight;

// MapPoints associated to keypoints, NULL pointer if no association.
std::vector<MapPoint*> mvpMapPoints;

// Flag to identify outlier associations.
std::vector<bool> mvbOutlier;

// Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints.
static float mfGridElementWidthInv;
static float mfGridElementHeightInv;
std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];

// Camera pose.
cv::Mat mTcw;

// Current and Next Frame id.
static long unsigned int nNextId;
long unsigned int mnId;

// Reference Keyframe.
KeyFrame* mpReferenceKF;

// Scale pyramid info.
int mnScaleLevels;
float mfScaleFactor;
float mfLogScaleFactor;
vector<float> mvScaleFactors;
vector<float> mvInvScaleFactors;
vector<float> mvLevelSigma2;
vector<float> mvInvLevelSigma2;

// Undistorted Image Bounds (computed once).
static float mnMinX;
static float mnMaxX;
static float mnMinY;
static float mnMaxY;

static bool mbInitialComputations;

private:

// Undistort keypoints given OpenCV distortion parameters.
// Only for the RGB-D case. Stereo must be already rectified!
// (called in the constructor).
void UndistortKeyPoints();

// Computes image bounds for the undistorted image (called in the constructor).
void ComputeImageBounds(const cv::Mat &imLeft);

// Assign keypoints to the grid for speed up feature matching (called in the constructor).
void AssignFeaturesToGrid();

// Rotation, translation and camera center
cv::Mat mRcw;
cv::Mat mtcw;
cv::Mat mRwc;
cv::Mat mOw; //==mtwc
};

}// namespace ORB_SLAM

#endif // FRAME_H

#include "Frame.h"
#include "Converter.h"
#include "ORBmatcher.h"
#include <thread>

namespace ORB_SLAM2
{

long unsigned int Frame::nNextId=0;
bool Frame::mbInitialComputations=true;
float Frame::cx, Frame::cy, Frame::fx, Frame::fy, Frame::invfx, Frame::invfy;
float Frame::mnMinX, Frame::mnMinY, Frame::mnMaxX, Frame::mnMaxY;
float Frame::mfGridElementWidthInv, Frame::mfGridElementHeightInv;

Frame::Frame()
{}

//Copy Constructor
Frame::Frame(const Frame &frame)
:mpORBvocabulary(frame.mpORBvocabulary), mpORBextractorLeft(frame.mpORBextractorLeft), mpORBextractorRight(frame.mpORBextractorRight),
mTimeStamp(frame.mTimeStamp), mK(frame.mK.clone()), mDistCoef(frame.mDistCoef.clone()),
mbf(frame.mbf), mb(frame.mb), mThDepth(frame.mThDepth), N(frame.N), mvKeys(frame.mvKeys),
mvKeysRight(frame.mvKeysRight), mvKeysUn(frame.mvKeysUn),  mvuRight(frame.mvuRight),
mvDepth(frame.mvDepth), mBowVec(frame.mBowVec), mFeatVec(frame.mFeatVec),
mDescriptors(frame.mDescriptors.clone()), mDescriptorsRight(frame.mDescriptorsRight.clone()),
mvpMapPoints(frame.mvpMapPoints), mvbOutlier(frame.mvbOutlier), mnId(frame.mnId),
mpReferenceKF(frame.mpReferenceKF), mnScaleLevels(frame.mnScaleLevels),
mfScaleFactor(frame.mfScaleFactor), mfLogScaleFactor(frame.mfLogScaleFactor),
mvScaleFactors(frame.mvScaleFactors), mvInvScaleFactors(frame.mvInvScaleFactors),
mvLevelSigma2(frame.mvLevelSigma2), mvInvLevelSigma2(frame.mvInvLevelSigma2)
{
for(int i=0;i<FRAME_GRID_COLS;i++)
for(int j=0; j<FRAME_GRID_ROWS; j++)
mGrid[i][j]=frame.mGrid[i][j];

if(!frame.mTcw.empty())
SetPose(frame.mTcw);
}

//双目构建的Frame对象

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))
{
// Frame ID
mnId=nNextId++;

// Scale Level Info
mnScaleLevels = mpORBextractorLeft->GetLevels();
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
mfLogScaleFactor = log(mfScaleFactor);
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

// ORB extraction
//提取特征加入双线程同步提取，0,1代表左目和右目
//两张提取的特征点会放在不同的vector中
//对单目和RGBD来说，右目不用，以左为准
thread threadLeft(&Frame::ExtractORB,this,0,imLeft);
thread threadRight(&Frame::ExtractORB,this,1,imRight);
//该函数在线程执行完成是返回
//在调用这个函数之后，线程对象变为不可连接的并且可以被安全地销毁
threadLeft.join();
threadRight.join();

// N为特征点的数量
N = mvKeys.size();

//如果提取的特征点数目为0，则直接返回
if(mvKeys.empty())
return;

UndistortKeyPoint
16956
s();

ComputeStereoMatches();

//初始化地图点及其外点
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
mvbOutlier = vector<bool>(N,false);

// This is done only for the first Frame (or after a change in the calibration)
if(mbInitialComputations)
{
ComputeImageBounds(imLeft);

mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/(mnMaxX-mnMinX);
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/(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();
}

//RGBD构建的Frame对象，基本上和双目类似,只需要恢复出右图深度为正的深度即可
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)
{
// Frame ID
mnId=nNextId++;

// Scale Level Info
mnScaleLevels = mpORBextractorLeft->GetLevels();
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
mfLogScaleFactor = log(mfScaleFactor);
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

// ORB extraction
ExtractORB(0,imGray);

N = mvKeys.size();

if(mvKeys.empty())
return;

UndistortKeyPoints();

ComputeStereoFromRGBD(imDepth);

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

// This is done only for the first Frame (or after a change in the calibration)
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;
}

mb = mbf/fx;

AssignFeaturesToGrid();
}

//单目构建的Frame类，和双目类似，但是不包含匹配信息
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
mnId=nNextId++;

// Scale Level Info
mnScaleLevels = mpORBextractorLeft->GetLevels();
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
mfLogScaleFactor = log(mfScaleFactor);
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

// ORB extraction
ExtractORB(0,imGray);

N = mvKeys.size();

if(mvKeys.empty())
return;

UndistortKeyPoints();

// Set no stereo information
mvuRight = vector<float>(N,-1);
mvDepth = vector<float>(N,-1);

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

// This is done only for the first Frame (or after a change in the calibration)
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;
}

mb = mbf/fx;

AssignFeaturesToGrid();
}

//将关键点分布到64*48分割而成的网格中，为了加速匹配和均匀化关键点分布
void Frame::AssignFeaturesToGrid()
{
//这里为什么要乘以0.5f？
int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);
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);

for(int i=0;i<N;i++)
{
const cv::KeyPoint &kp = mvKeysUn[i];

int nGridPosX, nGridPosY;
if(PosInGrid(kp,nGridPosX,nGridPosY))
mGrid[nGridPosX][nGridPosY].push_back(i);
}
}

//对输入的图像提取0RB的特征
void Frame::ExtractORB(int flag, const cv::Mat &im)
{
if(flag==0)
(*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors);
else
(*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);
}

void Frame::SetPose(cv::Mat Tcw)
{
mTcw = Tcw.clone();
UpdatePoseMatrices();
}

void Frame::UpdatePoseMatrices()
{
mRcw = mTcw.rowRange(0,3).colRange(0,3);
mRwc = mRcw.t();
mtcw = mTcw.rowRange(0,3).col(3);
mOw = -mRcw.t()*mtcw;
}

//设置当前帧的姿态，并更新当前帧相机在世界坐标系下的位姿，中心点位置；
// 判断一个MapPoint是否在视角范围内
bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
{
//注意这里的MapPoint是从SearchLocalPoint传递进来的，具备一定信息量
pMP->mbTrackInView = false;

// 3D in absolute coordinates
cv::Mat P = pMP->GetWorldPos();

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

// Check positive depth
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); //|pn| = 1

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

//在某块区域内获取特征点
//minLevel和maxLevel考察特征点是从图像金字塔的哪一层提取出来的
vector<size_t> Frame::GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel, const int maxLevel) const
{
vector<size_t> vIndices;
vIndices.reserve(N);

const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));
if(nMinCellX>=FRAME_GRID_COLS)
return vIndices;

const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
if(nMaxCellX<0)
return vIndices;

const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
if(nMinCellY>=FRAME_GRID_ROWS)
return vIndices;

const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
if(nMaxCellY<0)
return vIndices;

const bool bCheckLevels = (minLevel>0) || (maxLevel>=0);

for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
{
for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
{
const vector<size_t> vCell = mGrid[ix][iy];
if(vCell.empty())
continue;

for(size_t j=0, jend=vCell.size(); j<jend; j++)
{
const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
if(bCheckLevels)
{
if(kpUn.octave<minLevel)
continue;
if(maxLevel>=0)
if(kpUn.octave>maxLevel)
continue;
}

const float distx = kpUn.pt.x-x;
const float disty = kpUn.pt.y-y;

if(fabs(distx)<r && fabs(disty)<r)
vIndices.push_back(vCell[j]);
}
}
}

return vIndices;
}

//判断特征点是否在区域内
bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &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
if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS)
return false;

return true;
}

//将当前帧的描述子矩阵（可以转换成向量）转换成词袋模型向量
void Frame::ComputeBoW()
{
if(mBowVec.empty())
{
vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);
}
}

//通过OpenCV给定的畸变参数给定未畸变的关键点
//只针对RGBD的例子，双目的例子一定是要进行校正过的
//在构造函数中，我们可以看到该函数被调用
void Frame::UndistortKeyPoints()
{
//判断是否给定校正参数
if(mDistCoef.at<float>(0)==0.0)
{
mvKeysUn=mvKeys;
return;
}

// Fill matrix with points
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
//使用OpenCV进行校正
mat=mat.reshape(2);
cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
mat=mat.reshape(1);

// Fill undistorted keypoint vector
// 重新存储已经去畸变后的特征点坐标
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;
}
}

//计算图像的边界
void Frame::ComputeImageBounds(const cv::Mat &imLeft)
{
if(mDistCoef.at<float>(0)!=0.0)
{
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
{
mnMinX = 0.0f;
mnMaxX = imLeft.cols;
mnMinY = 0.0f;
mnMaxY = imLeft.rows;
}
}

//进行立体匹配
//找到关键点在左右两幅图之间的匹配
//如果存在匹配，计算出深度并存储于左图关键点相关的右图中关键点的坐标
void Frame::ComputeStereoMatches()
{
//先对要存储的信息进行初始化
mvuRight = vector<float>(N,-1.0f);
mvDepth = vector<float>(N,-1.0f);

//这里在matcher中设定的阈值，TH_HIGH = 100， TH_LOW = 50，这个阈值怎么来的，具体还不是很清楚
const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2;
//获得原始图像的行数
const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;

//Assign keypoints to row table
//构造n个vector<sizt_t>类型的vector
vector<vector<size_t> > vRowIndices(nRows,vector<size_t>());

//每行最多装200个特征点
for(int i=0; i<nRows; i++)
vRowIndices[i].reserve(200);

// 右图特征点的数量
const int Nr = mvKeysRight.size();

//https://www.cnblogs.com/shang-slam/p/6393419.html参考这个网址的解释
//在匹配左右帧的特征点时，虽然已经过了极线矫正，但是不能仅仅搜索极线对应的同一行像素点
//而应该根据右目提取特征点时的尺度（金字塔层数），确定一个极线附近的扫描范围r，
//这个带状范围内均包含这个特征信息，这个就是r计算的算法
for(int iR=0; iR<Nr; iR++)
{
const cv::KeyPoint &kp = mvKeysRight[iR];
const float &kpY = kp.pt.y;
const float r = 2.0f*mvScaleFactors[mvKeysRight[iR].octave];
const int maxr = ceil(kpY+r);
const int minr = floor(kpY-r);

for(int yi=minr;yi<=maxr;yi++)
vRowIndices[yi].push_back(iR);
}

// Set limits for search
//双目视觉中基线的长度，单位是m
const float minZ = mb;
const float minD = 0;
const float maxD = mbf/minZ;

// For each left keypoint search a match in the right image
vector<pair<int, int> > vDistIdx;
vDistIdx.reserve(N);

for(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;

const vector<size_t> &vCandidates = vRowIndices[vL];

if(vCandidates.empty())
continue;

const float minU = uL-maxD;
const float maxU = uL-minD;

if(maxU<0)
continue;

int bestDist = ORBmatcher::TH_HIGH;
size_t bestIdxR = 0;

const cv::Mat &dL = mDescriptors.row(iL);

// Compare descriptor to right keypoints
//通过描述子进行特征点匹配，得到每个特征点最佳匹配点scaleduR
for(size_t iC=0; iC<vCandidates.size(); iC++)
{
const size_t iR = vCandidates[iC];
const cv::KeyPoint &kpR = mvKeysRight[iR];

if(kpR.octave<levelL-1 || kpR.octave>levelL+1)
continue;

const float &uR = kpR.pt.x;

if(uR>=minU && uR<=maxU)
{
const cv::Mat &dR = mDescriptorsRight.row(iR);
const int dist = ORBmatcher::DescriptorDistance(dL,dR);

if(dist<bestDist)
{
bestDist = dist;
bestIdxR = iR;
}
}
}

// Subpixel match by correlation
//通过SAD滑窗得到匹配修正量bestincR
if(bestDist<thOrbDist)
{
// coordinates in image pyramid at keypoint scale
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);

// sliding window search
const int w = 5;
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;
const int L = 5;
vector<float> vDists;
vDists.resize(2*L+1);

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++)
{
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);

float dist = cv::norm(IL,IR,cv::NORM_L1);
if(dist<bestDist)
{
bestDist =  dist;
bestincR = incR;
}

vDists[L+incR] = dist;
}

if(bestincR==-L || bestincR==L)
continue;

//(bestincR,dist) (bestincR-1,dist) (bestincR+1,dist)三个点拟合出抛
//物线，得到亚像素修正量deltaR
// Sub-pixel match (Parabola fitting)
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));

if(deltaR<-1 || deltaR>1)
continue;

// Re-scaled coordinate
//最终匹配点位置为：scaleduR0 + bestincR + deltaR
float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR);

float disparity = (uL-bestuR);

if(disparity>=minD && disparity<maxD)
{
if(disparity<=0)
{
disparity=0.01;
bestuR = uL-0.01;
}
mvDepth[iL]=mbf/disparity;
mvuRight[iL] = bestuR;
vDistIdx.push_back(pair<int,int>(bestDist,iL));
}
}
}

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
{
mvuRight[vDistIdx[i].second]=-1;
mvDepth[vDistIdx[i].second]=-1;
}
}
}

//RGBD计算立体匹配，只要判断下深度
void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth)
{
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];

const float &v = kp.pt.y;
const float &u = kp.pt.x;

const float d = imDepth.at<float>(v,u);

if(d>0)
{
mvDepth[i] = d;
mvuRight[i] = kpU.pt.x-mbf/d;
}
}
}

//将特征点坐标（给id即可）反投影到3D地图点（世界坐标）
//单纯的二维像素点是没办法投影到3D空间中去的，因为缺少对应的尺度，
//但是在已知深度的情况下,则可确定对应的尺度，最后获得3D中点坐标
cv::Mat Frame::UnprojectStereo(const int &i)
{
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();
}

} //namespace ORB_SLAM

里面有些涉及到其他类就没有具体解释，还有就是LZ也不明白。。。

反正代码还能怎么着？多敲！多瞧呗！O(∩_∩)O哈哈~