# Frame.cpp
### Frame::Frame()單目
初始化幀的類
在[Tracking::GrabImageMonocular()](#Tracking::GrabImageMonocular())中引用
#### 定義

Frame::Frame(const cv::Mat &imGray,     //影像
             const double &timeStamp,   //時間戳
             ORBextractor* extractor,   //ORB特徵提取器的句柄
             ORBVocabulary* voc,        //ORB詞袋的句柄
             cv::Mat &K,                //相機內參
             cv::Mat &distCoef,         //相機去畸變參數
             const float &bf,           //baseline*f
             const float &thDepth)      //深度閾值

#### 函式流程
1. 獲取幀的ID
2. 計算特徵金字塔的參數
3. 提取特徵點
4. 對特徵點進行矯正
5. 初始化矯正後的數值
6. 將特徵點分配到影像網格中

#### 流程詳解
1. 獲取幀的ID

mnId=nNextId++;

nNextId宣告在frame.h

//類的靜態變數，在系統初始化的時候就被宣告 
static long unsigned int nNextId;

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

3. 提取特徵點

ExtractORB(0,imGray);

//特徵點數量
N = mvKeys.size();

//沒有特徵點就結束
if(mvKeys.empty())
    return;

跳至 [Frame::ExtractORB](#Frame::ExtractORB)

4. 對特徵點進行矯正

UndistortKeyPoints();

// Set no stereo information
//由於是單目，因此深度及右相機設為-1
mvuRight = vector<float>(N,-1);
mvDepth = vector<float>(N,-1);

//初始化地圖點
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
//紀錄是否為外點的地圖點
mvbOutlier = vector<bool>(N,false);

跳到[Frame::UndistortKeyPoints()](#Frame::UndistortKeyPoints())

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
mb = mbf/fx;

跳至[Frame::ComputeImageBounds()](#Frame::ComputeImageBounds())

6. 將特徵點分配到影像網格中

AssignFeaturesToGrid();

跳至[ Frame::AssignFeaturesToGrid()]( Frame::AssignFeaturesToGrid())

### Frame::Frame()雙目
初始化幀的類，與單目相似，僅多出雙目匹配的步驟。

#### 定義

Frame::Frame(const cv::Mat &imLeft,        //左影像
             const cv::Mat &imRight,       //右影像
             const double &timeStamp,      //時間戳
             ORBextractor* extractorLeft,  //左ORB特徵提取器的句柄
             ORBextractor* extractorRight, //右ORB特徵提取器的句柄
             ORBVocabulary* voc,           //ORB詞袋的句柄
             cv::Mat &K,                   //相機內參
             cv::Mat &distCoef,            //相機去畸變參數
             const float &bf,              //baseline*f
             const float &thDepth)         //深度閾值

#### 函式流程
1. 獲取幀的ID
2. 計算特徵金字塔的參數
3. 提取特徵點
4. 對特徵點進行矯正
5. 計算雙目特徵點的匹配
6. 初始化矯正後的數值
7. 將特徵點分配到影像網格中

#### 流程詳解
1. 獲取幀的ID

// Frame ID
mnId=nNextId++; // Step1 ID自增

2. 計算特徵金字塔的參數

// Scale Level Info
// 獲取圖像金字塔的層數
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();

3. 提取特徵點

// ORB extraction
// 對左目右目圖像提取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;

4. 對特徵點進行矯正

// 用OpenCV的矯正函數、內參對提取到的特徵點進行矯正
// 實際上由於雙目輸入的圖像已經預先經過矯正,所以實際上並沒有對特徵點進行任何處理操作
UndistortKeyPoints();

5. 計算雙目特徵點的匹配

// 計算雙目見特徵點的匹配，只有匹配成功的特徵點才會計算深度，深度存放在mvDepth中;
ComputeStereoMatches();
// 初始化本幀的地圖點
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));    
// 記錄地圖點是否為外點，初始化均為外點false
mvbOutlier = vector<bool>(N,false);

跳到[Frame::ComputeStereoMatches()](#Frame::ComputeStereoMatches())

6. 初始化矯正後的數值

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

7. 將特徵點分配到影像網格中

// 將特徵點分配到圖像網格中
AssignFeaturesToGrid();

### Frame::ExtractORB()
提取特徵點
在[Frame::Frame()單目](#Frame::Frame()單目)中引用

#### 定義

void Frame::ExtractORB(int flag,          // 0:左圖，0:右圖
                       const cv::Mat &im) // 圖像

#### 函式流程
1. 根據相機種類提取特徵點

#### 流程詳解
1. 根據相機種類提取特徵點

if(flag==0)
    (*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors); //使用仿函數重載
else
    (*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);

mpORBextractorLeft 宣告為:

    ORBextractor* mpORBextractorLeft, *mpORBextractorRight;

ORBextractor又為 ORBextractor類(定義在ORBextractor類.h裡)，這裡用operator()進行重載
跳至 [ORBextractor::operator()](#ORBextractor::operator())

### Frame::UndistortKeyPoints()
對特徵點進行校正
在[Frame::Frame()](#Frame::Frame())中引用
#### 函式流程
1. 確認是否要去畸變
2. 去畸變校正
3. 儲存校正後的特徵點

#### 流程詳解
1. 確認是否要去畸變

//當k1為0代表不用去畸變，因為k1是最重要的參數
if(mDistCoef.at<float>(0)==0.0)
{
    mvKeysUn=mvKeys;
    return;
}

2. 去畸變校正

// Fill matrix with points
// N為提取的特徵點數量，將N個特徵點保存在N*2的mat中
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
// 調整mat的通道為2，矩陣的行列形狀不變
mat=mat.reshape(2);
cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK); // 用openc進行校正
mat=mat.reshape(1);

3. 儲存校正後的特徵點

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

### Frame::ComputeImageBounds
計算去畸變後的邊界
在[Frame::Frame()](#Frame::Frame())中引用
#### 定義

void Frame::ComputeImageBounds(const cv::Mat &imLeft    //輸入影像
                              ) 

#### 函式流程
1. 確認是否要去畸變
2. 當要去畸變就計算去畸變後的邊界
3. 當不要去畸變就保留原本的邊界

#### 流程詳解
1. 確認是否要去畸變

if(mDistCoef.at<float>(0)!=0.0)
{

2. 當要去畸變就計算去畸變後的邊界

    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));//左下和右下縱坐標最小的
}

3. 當不要去畸變就保留原本的邊界

else
{
    mnMinX = 0.0f;
    mnMaxX = imLeft.cols;
    mnMinY = 0.0f;
    mnMaxY = imLeft.rows;
}

### Frame::AssignFeaturesToGrid()
在[Frame::Frame()](#Frame::Frame())中引用

#### 函式流程
1. 分配空間給影像網格
2. 將對應特徵點分配至對應的網格

#### 流程詳解
1. 分配空間給影像網格

//FRAME_GRID_COLS = 64, FRAME_GRID_ROWS = 48
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);

2. 將對應特徵點分配至對應的網格

//遍歷所有特徵點
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);  //將特徵點放入對應網格
}

跳至[Frame::PosInGrid()](#Frame::PosInGrid())

### Frame::PosInGrid()

在[Frame::AssignFeaturesToGrid()](#Frame::AssignFeaturesToGrid())中引用
#### 定義

bool Frame::PosInGrid(const cv::KeyPoint &kp, //輸入的特徵點
                      int &posX,              //輸出特徵點在網格的X座標
                      int &posY)              //輸出特徵點在網格YX座標 

#### 函式流程
1. 計算特徵點在網格的座標
2. 判斷特徵點是否在有效範圍內

#### 流程詳解
1. 計算特徵點在網格的座標

posX = round((kp.pt.x-mnMinX)*mfGridElementWidthInv);
posY = round((kp.pt.y-mnMinY)*mfGridElementHeightInv);

2. 判斷特徵點是否在有效範圍內

//當超出邊界時，返回false
if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS)
    return false;

//有效，返回true
return true;

### Frame::ComputeBoW()
計算當前幀特徵點對應的詞袋Bow，主要是mBowVec 和 mFeatVec
在[Tracking::CreateInitialMapMonocular()](#Tracking::CreateInitialMapMonocular())，[Tracking::TrackReferenceKeyFrame()](#Tracking::TrackReferenceKeyFrame())，[LocalMapping::ProcessNewKeyFrame()](#LocalMapping::ProcessNewKeyFrame()) 中引用

#### 函式流程
1. 將描述子轉換為詞袋的輸入格式
2. 將特徵點的描述子轉換成詞袋向量

#### 流程詳解
1. 將描述子轉換為詞袋的輸入格式

// 判斷是否以前已經計算過了，計算過了就跳過
if(mBowVec.empty())
{
    // 將描述子mDescriptors轉換為DBOW要求的輸入格式
    vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);

2. 將特徵點的描述子轉換成詞袋向量

    // 將特徵點的描述子轉換成詞袋向量mBowVec以及特徵向量mFeatVec
    mpORBvocabulary->transform(
        vCurrentDesc,   //當前的描述子vector
        mBowVec,        //輸出，詞袋向量，記錄的是單詞的id及其對應權重TF-IDF值
        mFeatVec,       //輸出，記錄node id及其對應的圖像 feature對應的索引
        4);             //4表示從葉節點向前數的層數
}

跳到Thirdparty: [TemplatedVocabulary<TDescriptor,F>::transform()](#TemplatedVocabulary<TDescriptor,F>::transform)

### Frame::UpdatePoseMatrices()
根據Tcw計算mRcw、mTcw 與 mRwc、mOw
mOw: 當前相機光心在世界座標系下的座標
mTcw: 世界坐標系到相機座標系的變換矩陣
mRcw: 世界坐標系到相機座標系的旋轉矩陣
mtcw: 世界坐標系到相機座標系的平移矩陣
mRwc: 相機座標系到世界坐標系的旋轉矩陣

在[Tracking::CreateNewKeyFrame()](#Tracking::CreateNewKeyFrame())中引用

#### 函式流程
1. 從變換矩陣中提取旋轉矩陣
2. 計算相機座標系到世界坐標系的旋轉矩陣
3. 從變換矩陣中提取平移矩陣
4. 計算當前相機光心在世界座標系下的座標

#### 流程詳解
1. 從變換矩陣中提取旋轉矩陣

mRcw = mTcw.rowRange(0,3).colRange(0,3);

2. 計算相機座標系到世界坐標系的旋轉矩陣

mRwc = mRcw.t();

3. 從變換矩陣中提取平移矩陣

mtcw = mTcw.rowRange(0,3).col(3);

4. 計算當前相機光心在世界座標系下的座標

mOw = -mRcw.t()*mtcw;

### Frame::ComputeStereoMatches()
雙目圖像特徵點匹配,用於雙目相機輸入圖像預處理
作用：輸入雙目圖像和ORB特徵點，輸出匹配結果和深度。
在[Frame::Frame()雙目](#Frame::Frame()雙目)中引用

#### 函式流程
1. 行特徵點統計
2. 取出要匹配搜索的特徵點
3. 粗配準
4. SAD精匹配
5. 亞像素插值
6. 最優視差值/深度選擇
7. 刪除離群點

#### 流程詳解

mvuRight = vector<float>(N,-1.0f);
// mvDepth存儲特徵點的深度信息
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;

//Assign keypoints to row table
// 二維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個特徵點
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();

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

2. 取出要匹配搜索的特徵點

// 根據視差公式計算兩個特徵點匹配搜索的範圍
// 對於立體矯正後的兩張圖，在列方向(x)存在最大視差maxd和最小視差mind
// 也即是左圖中任何一點p，在右圖上的匹配點的範圍為應該是[p - maxd, p - mind], 而不需要遍歷每一行所有的像素
// maxd = baseline * length_focal / minZ
// mind = baseline * length_focal / maxZ
const float minZ = mb;
const float minD = 0;
const float maxD = mbf/minZ;

// 保存sad塊匹配相似度和左圖特徵點索引，精確匹配使用
vector<pair<int, int> > vDistIdx;
vDistIdx.reserve(N);

// 為左圖每一個特徵點il，在右圖搜索最相似的特徵點ir
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;

    // 獲取左圖特徵點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);

3. 粗配準

    // Compare descriptor to right keypoints
// 粗配準. 左圖特徵點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;
            }
        }
    }

4. SAD精匹配

// 精匹配: 滑動窗口匹配,根據匹配點周圍5✖5窗口尋找精確匹配
    //  如果剛才匹配過程中的最佳描述子距離小於給定的閾值，代表還不錯
    if(bestDist<thOrbDist)
    {
        // coordinates in image pyramid at keypoint scale
        // 計算右圖特徵點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);

        // sliding window search
        // 滑動窗口搜索, 類似模版卷積或濾波
        // 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 - 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;
        //滑動窗口下邊界
        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);

            float dist = cv::norm(IL,IR,cv::NORM_L1);
            // 統計最小sad和偏移量
            if(dist<bestDist)
            {
                bestDist =  dist;   //最小sad
                bestincR = incR;    //偏移量
            }

            //L+incR 為refine後的匹配點列坐標(x)
            vDists[L+incR] = dist;
        }

        // 搜索窗口越界判斷
        if(bestincR==-L || bestincR==L)
            continue;

5. 亞像素插值

        // Sub-pixel match (Parabola fitting)
// 亞像素插值, 使用最佳匹配點及其左右相鄰點構成拋物線
        // 使用3點擬合拋物線的方式，用極小值代替之前計算的最優視差值
        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;

        // Re-scaled coordinate
        // 根據亞像素精度偏移量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;
            }

6. 最優視差值/深度選擇

            // 根據視差值計算深度信息
            // 保存最相似點的列坐標(x)信息
            // 保存歸一化sad最小相似度
            mvDepth[iL]=mbf/disparity;
            mvuRight[iL] = bestuR;
            vDistIdx.push_back(pair<int,int>(bestDist,iL));
        }
    }
}

7. 刪除離群點

// 塊匹配相似度閾值判斷，歸一化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;
    }
}

### Frame::GetFeaturesInArea()
找到在 以x,y為中心,邊長為2r的方形內且在[minLevel, maxLevel]的特徵點
在[ORBmatcher::SearchForInitialization()](#ORBmatcher::SearchForInitialization())中引用

#### 定義

vector<size_t> Frame::GetFeaturesInArea(const float &x,     //特徵點坐標x
                                        const float &y,     //特徵點坐標y
                                        const float &r,     //搜索半徑 
                                        const int minLevel, //最小金字塔層級
                                        const int maxLevel)  //最大金字塔層級
                                        const

#### 函式流程
1. 計算半徑為r圓左右上下邊界所在的網格列和行的id
2. 遍歷圓形區域內的所有網格，尋找滿足條件的候選特徵點

#### 流程詳解
1. 計算半徑為r圓左右上下邊界所在的網格列和行的id

// 存储搜索结果的vector
vector<size_t> vIndices;
vIndices.reserve(N);

// 查找半徑為r的圓左側邊界所在網格列坐標：
// (mnMaxX-mnMinX)/FRAME_GRID_COLS：表示列方向每個網格可以平均分得幾個像素（大於1）
// mfGridElementWidthInv=FRAME_GRID_COLS/(mnMaxX-mnMinX) 是上面倒數，表示每個像素可以均分幾個網格列（小於1）
// (x-mnMinX-r)，可以看做是從圖像的左邊界mnMinX到半徑r的圓的左邊界區域佔的像素列數
// 兩者相乘，就是求出那個半徑為r的圓的左側邊界在哪個網格列中
// 保證nMinCellX 結果大於等於0
const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv)); //nMinCellX表示圓左側邊界所在柵格數（列）

// 如果最終求得的圓的左邊界所在的網格列超過了設定了上限，那麼就說明計算出錯，找不到符合要求的特徵點，返回空vector
if(nMinCellX>=FRAME_GRID_COLS)
    return vIndices;

// 計算圓所在的右邊界網格列索引
const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));    //nMaxCellX表示圓右側邊界所在柵格數（列）
// 如果計算出的圓右邊界所在的網格不合法，說明該特徵點不好，直接返回空vector
if(nMaxCellX<0)
    return vIndices;

// 後面的操作也都是類似的，計算出這個圓上下邊界所在的網格行的id
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);

2. 遍歷圓形區域內的所有網格，尋找滿足條件的候選特徵點

for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
{
    for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
    {
        // 獲取這個網格內的所有特徵點在 Frame::mvKeysUn 中的索引
        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)
            {   
                // cv::KeyPoint::octave中表示的是從金字塔的哪一層提取的數據
                // 保證特徵點是在金字塔層級minLevel和maxLevel之間，不是的話跳過
                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;

            // 如果x方向和y方向的距離都在指定的半徑之內，存儲其index為候選特徵點
            if(fabs(distx)<r && fabs(disty)<r)
                vIndices.push_back(vCell[j]);
        }
    }
}

return vIndices;

### Frame::isInFrustum()
判斷一個點是否在視野內
在[Tracking::SearchLocalPoints()](#Tracking::SearchLocalPoints())中引用
#### 定義

bool Frame::isInFrustum(MapPoint *pMP,         //輸入當前地圖點
                        float viewingCosLimit) //輸入視角和平均視角的方向閾值

#### 函式流程
1. 獲得這個地圖點的世界坐標
2. 檢查深度值，如果在有效距離範圍內才能繼續下一步
3. 將這個地圖點投影到當前幀的相機坐標系下，判斷是否在圖像有效邊界中，如果在有效距離範圍內才能繼續下一步
4. 計算地圖點到相機中心的距離，如果在有效距離範圍內才能繼續下一步
5. 計算當前相機指向地圖點向量和地圖點的平均觀測方向夾角，小於60°才能進入下一步
6. 根據地圖點到光心的距離來預測一個尺度（仿照特徵點金字塔層級）
7. 記錄計算得到的一些參數

#### 流程詳解
1. 獲得這個地圖點的世界坐標

// mbTrackInView是決定一個地圖點是否進行重投影的標誌
// 這個標誌的確定要經過多個函數的確定，isInFrustum()只是其中的一個驗證關卡。這裡默認設置為否
pMP->mbTrackInView = false;

cv::Mat P = pMP->GetWorldPos(); 

// 根據當前幀(粗糙)姿態轉換到當前相機坐標系下的三維點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);

2. 檢查深度值，如果在有效距離範圍內才能繼續下一步

if(PcZ<0.0f)
    return false;

3. 將這個地圖點投影到當前幀的相機坐標系下，判斷是否在圖像有效邊界中，如果在有效距離範圍內才能繼續下一步

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;

4. 計算地圖點到相機中心的距離，如果在有效距離範圍內才能繼續下一步

 // 得到認為的可靠距離範圍:[0.8f*mfMinDistance, 1.2f*mfMaxDistance]
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();

// 得到當前地圖點距離當前幀相機光心的距離,注意P，mOw都是在同一坐標系下才可以
//  P : 當前地圖點在世界坐標系下坐標
//  mOw：當前相機光心在世界坐標系下坐標
const cv::Mat PO = P-mOw; //指向當前地圖點的向量

//取模就得到了距離
const float dist = cv::norm(PO);

//如果不在有效範圍內，認為投影不可靠
if(dist<minDistance || dist>maxDistance)
    return false;

pMP->GetMaxDistanceInvariance() : 跳到[MapPoint::GetMaxDistanceInvariance()](#MapPoint::GetMaxDistanceInvariance())
pMP->GetMinDistanceInvariance() : 跳到[MapPoint::GetMinDistanceInvariance()](#MapPoint::GetMinDistanceInvariance())

5. 計算當前相機指向地圖點向量和地圖點的平均觀測方向夾角，小於60°才能進入下一步

//平均觀測方向
cv::Mat Pn = pMP->GetNormal();

// 計算當前相機指向地圖點向量和地圖點的平均觀測方向夾角的餘弦值，注意平均觀測方向為單位向量
const float viewCos = PO.dot(Pn)/dist;

//夾角要在60°範圍內，否則認為觀測方向太偏了，重投影不可靠，返回false
if(viewCos<viewingCosLimit)
    return false;

6. 根據地圖點到光心的距離來預測一個尺度（仿照特徵點金字塔層級）

// Predict scale in the image
const int nPredictedLevel = pMP->PredictScale(dist,		//這個點到光心的距離
                                              this);	//給出這個幀                   

7. 記錄計算得到的一些參數

// 通過置位標記 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
return true;