# ORBmatcher.cpp
### ORBmatcher::ORBmatcher()
建構特徵匹配器
在[Tracking::MonocularInitialization()](#Tracking::MonocularInitialization())，[Tracking::TrackReferenceKeyFrame()](#Tracking::TrackReferenceKeyFrame())，[LocalMapping::SearchInNeighbors()](#LocalMapping::SearchInNeighbors()) 中引用
只有傳遞參數，函式實際沒有執行動作
#### 定義
```
ORBmatcher::ORBmatcher(float nnratio, //最佳與次佳平分的比例
bool checkOri //是否檢查方向
```
### ORBmatcher::SearchForInitialization()
進行特徵匹配
在[Tracking::MonocularInitialization()](#Tracking::MonocularInitialization())中引用
#### 定義
```
int ORBmatcher::SearchForInitialization(Frame &F1, //輸入參考幀
Frame &F2, //輸入當前幀
vector<cv::Point2f> &vbPrevMatched, //輸入參考幀的特徵點
vector<int> &vnMatches12, //輸出匹配關係
int windowSize) //搜索窗口大小
```
#### 函式流程
1. 構建旋轉直方圖
2. 在半徑窗口內搜索當前幀F2中所有的候選匹配特徵點
3. 計算最優與次優候選特徵點
4. 對最優次優結果進行檢查
5. 計算匹配點旋轉角度差所在的直方圖
6. 篩除旋轉直方圖中“非主流”部分
7. 保存最後通過篩選的匹配好的特徵點
#### 函式詳解
1. 構建旋轉直方圖
```
int nmatches=0;
// F1中特徵點和F2中匹配關係， 按照F1特徵點數目分配空間
vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);
// 建構長度為30的旋轉直方圖，HISTO_LENGTH = 30
vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
// 每個bin裡預分配500個，因為使用的是vector不夠的話可以自動擴展容量
rotHist[i].reserve(500);
// const float factor = 1.0f/HISTO_LENGTH; 原作者code
const float factor = HISTO_LENGTH/360.0f;
// 匹配點對距離，注意是按照F2特徵點數目分配空間
vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);
// 從幀2到幀1的反向匹配，注意是按照F2特徵點數目分配空間
vector<int> vnMatches21(F2.mvKeysUn.size(),-1);
// 遍歷幀1中的所有特徵點
for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1<iend1; i1++)
{
cv::KeyPoint kp1 = F1.mvKeysUn[i1];
int level1 = kp1.octave;
// 只使用原始圖像上提取的特徵點
if(level1>0)
continue;
```
2. 在半徑窗口內搜索當前幀F2中所有的候選匹配特徵點
```
// vbPrevMatched 輸入的是參考幀 F1的特徵點
// windowSize = 100，輸入最大最小金字塔層級 均為0
//找到在 以x,y為中心,邊長為2r的方形內且在[minLevel, maxLevel]的特徵點
vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);
// 沒有候選特徵點，跳過
if(vIndices2.empty())
continue;
// 取出參考幀F1中當前遍歷特徵點對應的描述子
cv::Mat d1 = F1.mDescriptors.row(i1);
int bestDist = INT_MAX; // 最佳描述子匹配距離，越小越好
int bestDist2 = INT_MAX;// 次佳描述子匹配距離
int bestIdx2 = -1; // 最佳候選特徵點在F2中的index
```
跳到[Frame::GetFeaturesInArea()](#Frame::GetFeaturesInArea())
3. 計算最優與次優候選特徵點
```
// 遍歷搜索搜索窗口中的所有潛在的匹配候選點，找到最優的和次優的
for(vector<size_t>::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
{
size_t i2 = *vit;
// 取出候選特徵點對應的描述子
cv::Mat d2 = F2.mDescriptors.row(i2);
// 計算兩個特徵點描述子距離
int dist = DescriptorDistance(d1,d2);
if(vMatchedDistance[i2]<=dist)
continue;
// 如果當前匹配距離更小，更新最佳次佳距離
if(dist<bestDist)
{
bestDist2=bestDist;
bestDist=dist;
bestIdx2=i2;
}
else if(dist<bestDist2)
{
bestDist2=dist;
}
}
```
4. 對最優次優結果進行檢查
```
// 對最優次優結果進行檢查，滿足閾值、最優/次優比例，刪除重複匹配
// 即使算出了最佳描述子匹配距離，也不一定保證配對成功。要小於設定閾值
if(bestDist<=TH_LOW)
{
// 最佳距離比次佳距離要小於設定的比例，這樣特徵點辨識度更高
if(bestDist<(float)bestDist2*mfNNratio)
{
// 如果找到的候選特徵點對應F1中特徵點已經匹配過了，說明發生了重複匹配，將原來的匹配也刪掉
if(vnMatches21[bestIdx2]>=0)
{
vnMatches12[vnMatches21[bestIdx2]]=-1;
nmatches--;
}
// 次優的匹配關係，雙向建立
// vnMatches12保存參考幀F1和F2匹配關係，index保存是F1對應特徵點索引，值保存的是匹配好的F2特徵點索引
vnMatches12[i1]=bestIdx2;
vnMatches21[bestIdx2]=i1;
vMatchedDistance[bestIdx2]=bestDist;
nmatches++;
```
5. 計算匹配點旋轉角度差所在的直方圖
```
if(mbCheckOrientation)
{
// 計算匹配特徵點的角度差，這裡單位是角度°，不是弧度
float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;
if(rot<0.0)
rot+=360.0f;
// 前面factor = HISTO_LENGTH/360.0f
// bin = rot / 360.of * HISTO_LENGTH 表示當前rot被分配在第幾個直方圖bin
int bin = round(rot*factor);
// 如果bin 滿了又是一個輪迴
if(bin==HISTO_LENGTH)
bin=0;
assert(bin>=0 && bin<HISTO_LENGTH);
rotHist[bin].push_back(i1);
}
}
}
}
```
6. 篩除旋轉直方圖中“非主流”部分
```
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
// 篩選出在旋轉角度差落在在直方圖區間內數量最多的前三個bin的索引
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i<HISTO_LENGTH; i++)
{
if(i==ind1 || i==ind2 || i==ind3)
continue;
// 剔除掉不在前三的匹配對，因為他們不符合“主流旋轉方向”
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
int idx1 = rotHist[i][j];
if(vnMatches12[idx1]>=0)
{
vnMatches12[idx1]=-1;
nmatches--;
}
}
}
}
```
跳到[ORBmatcher::ComputeThreeMaxima()](#ORBmatcher::ComputeThreeMaxima())
7. 保存最後通過篩選的匹配好的特徵點
```
// 將最後通過篩選的匹配好的特徵點保存到vbPrevMatched
for(size_t i1=0, iend1=vnMatches12.size(); i1<iend1; i1++)
if(vnMatches12[i1]>=0)
vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;
return nmatches;
```
### ORBmatcher::ComputeThreeMaxima()
篩選出在旋轉角度差落在在直方圖區間內數量最多的前三個區域的索引
在[ORBmatcher::SearchForInitialization()](#ORBmatcher::SearchForInitialization())
#### 定義
```
void ORBmatcher::ComputeThreeMaxima(vector<int>* histo, //旋轉方向直方圖
const int L, //直方圖區域數量(30)
int &ind1, //輸出最大的索引
int &ind2, //輸出第二大的索引
int &ind3) //輸出第三大的索引
```
#### 函式流程
1. 計算數量最多的前三個區域的索引
2. 確認前三區域的索引的差距，太大就放棄小的
#### 函式詳解
1. 計算數量最多的前三個區域的索引
```
int max1=0;
int max2=0;
int max3=0;
//遍歷所有區域
for(int i=0; i<L; i++)
{
const int s = histo[i].size();
if(s>max1)
{
max3=max2;
max2=max1;
max1=s;
ind3=ind2;
ind2=ind1;
ind1=i;
}
else if(s>max2)
{
max3=max2;
max2=s;
ind3=ind2;
ind2=i;
}
else if(s>max3)
{
max3=s;
ind3=i;
}
}
```
2. 確認前三區域的索引的差距，太大就放棄小的
```
if(max2<0.1f*(float)max1)
{
ind2=-1;
ind3=-1;
}
else if(max3<0.1f*(float)max1)
{
ind3=-1;
}
```
### ORBmatcher::SearchForTriangulation()
利用基礎矩陣F12極線約束，用BoW加速匹配兩個關鍵幀的未匹配的特徵點，產生新的匹配點對。
具體來說，pKF1圖像的每個特徵點與pKF2圖像同一 node節點的所有特徵點依次匹配，判斷是否滿足對極幾何約束，滿足約束就是匹配的特徵點
在[LocalMapping::CreateNewMapPoints()](#LocalMapping::CreateNewMapPoints())中引用
#### 定義
```
int ORBmatcher::SearchForTriangulation(
KeyFrame *pKF1, //關鍵幀1
KeyFrame *pKF2, //關鍵幀2
cv::Mat F12, //從2到1的基礎矩陣
vector<pair<size_t, size_t> > &vMatchedPairs, //存儲匹配特徵點對，特徵點用其在關鍵幀中的索引表示
const bool bOnlyStereo) //在雙目和rgbd情況下，是否要求特徵點在右圖存在匹配
```
#### 函式流程
1. 計算KF1的相機中心在KF2圖像平面的二維像素坐標
2. 利用BoW加速匹配：只對屬於同一節點(特定層)的ORB特徵進行匹配
2.1：遍歷pKF1和pKF2中的node節點
2.2：遍歷屬於同一node節點(id：f1it->first)下的所有特徵點
2.3：通過特徵點索引idx1在pKF1中取出對應的MapPoint
2.4：通過特徵點索引idx1在pKF1中取出對應的特徵點
2.5：遍歷該node節點下(f2it->first)對應KF2中的所有特徵點
2.6 計算 idx1 與 idx2 在兩個關鍵幀中對應特徵點的描述子距離
2.7 極點e2到kp2的像素距離如果小於閾值th,認為kp2對應的MapPoint距離pKF1相機太近，跳過該匹配點對
2.8 計算特徵點kp2到kp1對應極線的距離是否小於閾值
3. 用旋轉差直方圖來篩掉錯誤匹配對
4. 存儲匹配關係，下標是關鍵幀1的特徵點id，存儲的是關鍵幀2的特徵點id
#### 函式詳解
1. 計算KF1的相機中心在KF2圖像平面的二維像素坐標
```
const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;
// KF1相機光心在世界坐標系坐標Cw
cv::Mat Cw = pKF1->GetCameraCenter();
// KF2相機位姿R2w,t2w,是世界坐標系到相機坐標系
cv::Mat R2w = pKF2->GetRotation();
cv::Mat t2w = pKF2->GetTranslation();
// KF1的相機光心轉化到KF2坐標系中的坐標
cv::Mat C2 = R2w*Cw+t2w;
const float invz = 1.0f/C2.at<float>(2);
// 得到KF1的相機光心在KF2中的坐標，也叫極點，這裡是像素坐標
const float ex =pKF2->fx*C2.at<float>(0)*invz+pKF2->cx;
const float ey =pKF2->fy*C2.at<float>(1)*invz+pKF2->cy;
// Find matches between not tracked keypoints
// Matching speed-up by ORB Vocabulary
// Compare only ORB that share the same node
int nmatches=0;
// 記錄匹配是否成功，避免重複匹配
vector<bool> vbMatched2(pKF2->N,false);
vector<int> vMatches12(pKF1->N,-1);
// 用於統計匹配點對旋轉差的直方圖
vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
//! 原作者代碼是 const float factor = 1.0f/HISTO_LENGTH; 是錯誤的，更改為下面代碼
const float factor = HISTO_LENGTH/360.0f;
```
2. 利用BoW加速匹配：只對屬於同一節點(特定層)的ORB特徵進行匹配
```
// FeatureVector 其實就是一個map類，那就可以直接獲取它的迭代器進行遍歷
// FeatureVector的數據結構類似於：{(node1,feature_vector1) (node2,feature_vector2)...}
// f1it->first對應node編號，f1it->second對應屬於該node的所有特特徵點編號
DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();
// Step 2.1：遍歷pKF1和pKF2中的node節點
while(f1it!=f1end && f2it!=f2end)
{
// 如果f1it和f2it屬於同一個node節點才會進行匹配，這就是BoW加速匹配原理
if(f1it->first == f2it->first)
{
// Step 2.2：遍歷屬於同一node節點(id：f1it->first)下的所有特徵點
for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)
{
// 獲取pKF1中屬於該node節點的所有特徵點索引
const size_t idx1 = f1it->second[i1];
// Step 2.3：通過特徵點索引idx1在pKF1中取出對應的MapPoint
MapPoint* pMP1 = pKF1->GetMapPoint(idx1);
// If there is already a MapPoint skip
// 由於尋找的是未匹配的特徵點，所以pMP1應該為NULL
if(pMP1)
continue;
// 如果mvuRight中的值大於0，表示是雙目，且該特徵點有深度值
const bool bStereo1 = pKF1->mvuRight[idx1]>=0;
if(bOnlyStereo)
if(!bStereo1)
continue;
// Step 2.4：通過特徵點索引idx1在pKF1中取出對應的特徵點
const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];
// 通過特徵點索引idx1在pKF1中取出對應的特徵點的描述子
const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);
int bestDist = TH_LOW;
int bestIdx2 = -1;
// Step 2.5：遍歷該node節點下(f2it->first)對應KF2中的所有特徵點
for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++)
{
// 獲取pKF2中屬於該node節點的所有特徵點索引
size_t idx2 = f2it->second[i2];
// 通過特徵點索引idx2在pKF2中取出對應的MapPoint
MapPoint* pMP2 = pKF2->GetMapPoint(idx2);
// If we have already matched or there is a MapPoint skip
// 如果pKF2當前特徵點索引idx2已經被匹配過或者對應的3d點非空，那麼跳過這個索引idx2
if(vbMatched2[idx2] || pMP2)
continue;
const bool bStereo2 = pKF2->mvuRight[idx2]>=0;
if(bOnlyStereo)
if(!bStereo2)
continue;
// 通過特徵點索引idx2在pKF2中取出對應的特徵點的描述子
const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);
// Step 2.6 計算 idx1 與 idx2 在兩個關鍵幀中對應特徵點的描述子距離
const int dist = DescriptorDistance(d1,d2);
if(dist>TH_LOW || dist>bestDist)
continue;
// 通過特徵點索引idx2在pKF2中取出對應的特徵點
const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
//? 為什麼雙目就不需要判斷像素點到極點的距離的判斷？
// 因為雙目模式下可以左右互匹配恢復三維點
if(!bStereo1 && !bStereo2)
{
const float distex = ex-kp2.pt.x;
const float distey = ey-kp2.pt.y;
// Step 2.7 極點e2到kp2的像素距離如果小於閾值th,認為kp2對應的MapPoint距離pKF1相機太近，跳過該匹配點對
// 作者根據kp2金字塔尺度因子(scale^n，scale=1.2，n為層數)定義閾值th
// 金字塔層數從0到7，對應距離 sqrt(100*pKF2->mvScaleFactors[kp2.octave]) 是10-20個像素
//? 對這個閾值的有效性持懷疑態度
if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave])
continue;
}
// Step 2.8 計算特徵點kp2到kp1對應極線的距離是否小於閾值
if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
{
// bestIdx2，bestDist 是 kp1 對應 KF2中的最佳匹配點 index及匹配距離
bestIdx2 = idx2;
bestDist = dist;
}
}
//有匹配關係
if(bestIdx2>=0)
{
const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
// 記錄匹配結果
vMatches12[idx1]=bestIdx2;
vbMatched2[bestIdx2]=true; // !記錄已經匹配，避免重複匹配。原作者漏掉！
nmatches++;
// 記錄旋轉差直方圖信息
if(mbCheckOrientation)
{
// angle：角度，表示匹配點對的方向差。
float rot = kp1.angle-kp2.angle;
if(rot<0.0)
rot+=360.0f;
int bin = round(rot*factor);
if(bin==HISTO_LENGTH)
bin=0;
assert(bin>=0 && bin<HISTO_LENGTH);
rotHist[bin].push_back(idx1);
}
}
}
f1it++;
f2it++;
}
else if(f1it->first < f2it->first)
{
f1it = vFeatVec1.lower_bound(f2it->first);
}
else
{
f2it = vFeatVec2.lower_bound(f1it->first);
}
}
```
3. 用旋轉差直方圖來篩掉錯誤匹配對
```
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i<HISTO_LENGTH; i++)
{
if(i==ind1 || i==ind2 || i==ind3)
continue;
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
vbMatched2[vMatches12[rotHist[i][j]]] = false; // !清除匹配關係。原作者漏掉！
vMatches12[rotHist[i][j]]=-1;
nmatches--;
}
}
}
```
4. 存儲匹配關係，下標是關鍵幀1的特徵點id，存儲的是關鍵幀2的特徵點id
```
vMatchedPairs.clear();
vMatchedPairs.reserve(nmatches);
for(size_t i=0, iend=vMatches12.size(); i<iend; i++)
{
if(vMatches12[i]<0)
continue;
vMatchedPairs.push_back(make_pair(i,vMatches12[i]));
}
return nmatches;
```
跳到[ORBmatcher::CheckDistEpipolarLine()](#ORBmatcher::CheckDistEpipolarLine())
### ORBmatcher::CheckDistEpipolarLine()
用基礎矩陣檢查極線距離是否符合要求
在[ORBmatcher::SearchForTriangulation()](#ORBmatcher::SearchForTriangulation())中引用
#### 定義
```
bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1, //KF1中特徵點
const cv::KeyPoint &kp2, //KF2中特徵點
const cv::Mat &F12, //從KF2到KF1的基礎矩陣
const KeyFrame* pKF2) //關鍵幀KF2
```
#### 函式流程
1. 求出kp1在pKF2上對應的極線
2. 計算kp2特徵點到極線l2的距離
3. 判斷誤差是否滿足條件。尺度越大，誤差範圍應該越大
#### 函式詳解
1. 求出kp1在pKF2上對應的極線
```
// Epipolar line in second image l2 = x1'F12 = [a b c]
const float a = kp1.pt.x*F12.at<float>(0,0)+kp1.pt.y*F12.at<float>(1,0)+F12.at<float>(2,0);
const float b = kp1.pt.x*F12.at<float>(0,1)+kp1.pt.y*F12.at<float>(1,1)+F12.at<float>(2,1);
const float c = kp1.pt.x*F12.at<float>(0,2)+kp1.pt.y*F12.at<float>(1,2)+F12.at<float>(2,2);
```
2. 計算kp2特徵點到極線l2的距離
```
// 極線l2：ax + by + c = 0
// (u,v)到l2的距離為： |au+bv+c| / sqrt(a^2+b^2)
const float num = a*kp2.pt.x+b*kp2.pt.y+c;
const float den = a*a+b*b;
// 距離無窮大
if(den==0)
return false;
// 距離的平方
const float dsqr = num*num/den;
```
3. 判斷誤差是否滿足條件。尺度越大，誤差範圍應該越大
```
// 金字塔最底層一個像素就佔一個像素，在倒數第二層，一個像素等於最底層1.2個像素（假設金字塔尺度為1.2）
// 3.84 是自由度為1時，服從高斯分佈的一個平方項（也就是這裡的誤差）小於一個像素，這件事發生概率超過95%時的概率 （卡方分佈）
return dsqr < 3.84*pKF2->mvLevelSigma2[kp2.octave];
```
### ORBmatcher::SearchBySim3()
通過Sim3變換，確定pKF1的特徵點在pKF2中的大致區域，同理，確定pKF2的特徵點在pKF1中的大致區域
在該區域內通過描述子進行匹配捕獲pKF1和pKF2之前漏匹配的特徵點，更新vpMatches12（之前使用SearchByBoW進行特徵點匹配時會有漏匹配）
在[LoopClosing::ComputeSim3()](#LoopClosing::ComputeSim3())中引用
#### 定義
```
int ORBmatcher::SearchBySim3(KeyFrame *pKF1, //當前幀
KeyFrame *pKF2, //閉環候選幀
vector<MapPoint*> &vpMatches12, //匹配關係
const float &s12, //pKF2到pKF1的sim變換的尺度
const cv::Mat &R12, //pKF2到pKF1的sim變換的旋轉矩陣
const cv::Mat &t12, //pKF2到pKF1的sim變換的平移矩陣
const float th) //搜索閾值
```
#### 函式流程
1. 變數初始化
2. 紀錄已經匹配的特徵點
3. 通過Sim變換，尋找pKF1中特徵點與pKF2中新的匹配
4. 通過Sim變換，確定pKF2的特徵點在pKF1中的新的匹配
5. 一致性檢查，只有兩邊都有匹配才可以
#### 函式詳解
1. 變數初始化
```
const float &fx = pKF1->fx;
const float &fy = pKF1->fy;
const float &cx = pKF1->cx;
const float &cy = pKF1->cy;
// Camera 1 from world
// 從world到camera的變換
cv::Mat R1w = pKF1->GetRotation();
cv::Mat t1w = pKF1->GetTranslation();
//Camera 2 from world
cv::Mat R2w = pKF2->GetRotation();
cv::Mat t2w = pKF2->GetTranslation();
//Transformation between cameras
cv::Mat sR12 = s12*R12;
cv::Mat sR21 = (1.0/s12)*R12.t();
cv::Mat t21 = -sR21*t12;
const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
const int N1 = vpMapPoints1.size();
const vector<MapPoint*> vpMapPoints2 = pKF2->GetMapPointMatches();
const int N2 = vpMapPoints2.size();
vector<bool> vbAlreadyMatched1(N1,false);// 用於記錄該特徵點是否被處理過
vector<bool> vbAlreadyMatched2(N2,false);// 用於記錄該特徵點是否在pKF1中有匹配
```
2. 紀錄已經匹配的特徵點
```
// 用vpMatches12更新vbAlreadyMatched1和vbAlreadyMatched2
for(int i=0; i<N1; i++)
{
MapPoint* pMP = vpMatches12[i];
if(pMP)
{
vbAlreadyMatched1[i]=true;// 該特徵點已經判斷過
int idx2 = pMP->GetIndexInKeyFrame(pKF2);
if(idx2>=0 && idx2<N2)
vbAlreadyMatched2[idx2]=true;// 該特徵點在pKF1中有匹配
}
}
vector<int> vnMatch1(N1,-1);
vector<int> vnMatch2(N2,-1);
```
3. 通過Sim變換，尋找pKF1中特徵點與pKF2中新的匹配
```
// Transform from KF1 to KF2 and search
// 在該區域內通過描述子進行匹配捕獲pKF1和pKF2之前漏匹配的特徵點，更新vpMatches12
// （之前使用SearchByBoW進行特徵點匹配時會有漏匹配）
for(int i1=0; i1<N1; i1++)
{
MapPoint* pMP = vpMapPoints1[i1];
// 該特徵點已經有匹配點了，直接跳過
if(!pMP || vbAlreadyMatched1[i1])
continue;
if(pMP->isBad())
continue;
// 3.1 通過sim3變換將pKF1的地圖點投影到pKF2的座標
//取出世界坐標系下的地圖點
cv::Mat p3Dw = pMP->GetWorldPos();
// 把pKF1系下的MapPoint從world坐標系變換到camera1坐標系
cv::Mat p3Dc1 = R1w*p3Dw + t1w;
// 再通過Sim3將該MapPoint從camera1變換到camera2坐標系
cv::Mat p3Dc2 = sR21*p3Dc1 + t21;
// Depth must be positive
if(p3Dc2.at<float>(2)<0.0)
continue;
// 投影到camera2圖像平面(u,v)
const float invz = 1.0/p3Dc2.at<float>(2);
const float x = p3Dc2.at<float>(0)*invz;
const float y = p3Dc2.at<float>(1)*invz;
const float u = fx*x+cx;
const float v = fy*y+cy;
// Point must be inside the image
if(!pKF2->IsInImage(u,v))
continue;
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
const float dist3D = cv::norm(p3Dc2);
// Depth must be inside the scale invariance region
if(dist3D<minDistance || dist3D>maxDistance )
continue;
// Compute predicted octave
//3.2 預測該MapPoint對應的特徵點在圖像金字塔哪一層
const int nPredictedLevel = pMP->PredictScale(dist3D,pKF2);
// Search in a radius
// 計算特徵點搜索半徑
const float radius = th*pKF2->mvScaleFactors[nPredictedLevel];
// 3.3 取出該區域內的所有特徵點
const vector<size_t> vIndices = pKF2->GetFeaturesInArea(u,v,radius);
if(vIndices.empty())
continue;
// Match to the most similar keypoint in the radius
const cv::Mat dMP = pMP->GetDescriptor();
int bestDist = INT_MAX;
int bestIdx = -1;
// 3.4 遍歷搜索區域內的所有特徵點，與pMP進行描述子匹配，尋找最佳匹配點
for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
{
const size_t idx = *vit;
const cv::KeyPoint &kp = pKF2->mvKeysUn[idx];
if(kp.octave<nPredictedLevel-1 || kp.octave>nPredictedLevel)
continue;
const cv::Mat &dKF = pKF2->mDescriptors.row(idx);
const int dist = DescriptorDistance(dMP,dKF);
if(dist<bestDist)
{
bestDist = dist;
bestIdx = idx;
}
}
if(bestDist<=TH_HIGH)
{
vnMatch1[i1]=bestIdx;
}
}
```
4. 通過Sim變換，確定pKF2的特徵點在pKF1中的新的匹配
```
// Transform from KF2 to KF1 and search
// 在該區域內通過描述子進行匹配捕獲pKF1和pKF2之前漏匹配的特徵點，更新vpMatches12
// （之前使用SearchByBoW進行特徵點匹配時會有漏匹配）
for(int i2=0; i2<N2; i2++)
{
MapPoint* pMP = vpMapPoints2[i2];
if(!pMP || vbAlreadyMatched2[i2])
continue;
if(pMP->isBad())
continue;
cv::Mat p3Dw = pMP->GetWorldPos();
cv::Mat p3Dc2 = R2w*p3Dw + t2w;
cv::Mat p3Dc1 = sR12*p3Dc2 + t12;
// Depth must be positive
if(p3Dc1.at<float>(2)<0.0)
continue;
const float invz = 1.0/p3Dc1.at<float>(2);
const float x = p3Dc1.at<float>(0)*invz;
const float y = p3Dc1.at<float>(1)*invz;
const float u = fx*x+cx;
const float v = fy*y+cy;
// Point must be inside the image
if(!pKF1->IsInImage(u,v))
continue;
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
const float dist3D = cv::norm(p3Dc1);
// Depth must be inside the scale pyramid of the image
if(dist3D<minDistance || dist3D>maxDistance)
continue;
// Compute predicted octave
const int nPredictedLevel = pMP->PredictScale(dist3D,pKF1);
// Search in a radius of 2.5*sigma(ScaleLevel)
const float radius = th*pKF1->mvScaleFactors[nPredictedLevel];
const vector<size_t> vIndices = pKF1->GetFeaturesInArea(u,v,radius);
if(vIndices.empty())
continue;
// Match to the most similar keypoint in the radius
const cv::Mat dMP = pMP->GetDescriptor();
int bestDist = INT_MAX;
int bestIdx = -1;
for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
{
const size_t idx = *vit;
const cv::KeyPoint &kp = pKF1->mvKeysUn[idx];
if(kp.octave<nPredictedLevel-1 || kp.octave>nPredictedLevel)
continue;
const cv::Mat &dKF = pKF1->mDescriptors.row(idx);
const int dist = DescriptorDistance(dMP,dKF);
if(dist<bestDist)
{
bestDist = dist;
bestIdx = idx;
}
}
if(bestDist<=TH_HIGH)
{
vnMatch2[i2]=bestIdx;
}
}
```
5. 一致性檢查，只有兩邊都有匹配才可以
```
// Check agreement
int nFound = 0;
for(int i1=0; i1<N1; i1++)
{
int idx2 = vnMatch1[i1];
if(idx2>=0)
{
int idx1 = vnMatch2[idx2];
if(idx1==i1)
{
//更新匹配的地圖點
vpMatches12[i1] = vpMapPoints2[idx2];
nFound++;
}
}
}
return nFound;
```
### ORBmatcher::SearchByBoW()普通幀
通過詞袋隊關鍵幀的特徵點進行跟蹤
在[Tracking::TrackReferenceKeyFrame()](#Tracking::TrackReferenceKeyFrame())中引用
#### 定義
```
int ORBmatcher::SearchByBoW(KeyFrame* pKF, //參考關鍵幀
Frame &F, //當前普通幀
vector<MapPoint*> &vpMapPointMatches) //輸出匹配關係
```
#### 函式流程
1. 分別取出屬於同一node的ORB特徵點(只有屬於同一node，才有可能是匹配點)
2. 遍歷關鍵幀中屬於該node的特徵點
3. 遍歷當前普通幀中屬於該node的特徵點
4. 尋找最佳匹配點
5. 根據閾值和角度投票剔除誤匹配
5.1. 第一關篩選：匹配距離必須小於設定閾值
5.2. 第二關篩選：最佳匹配比次佳匹配明顯要好，那麼最佳匹配才真正可以
5.3. 記錄成功匹配特徵點的對應的地圖點(來自關鍵幀)
5.4. 計算匹配點旋轉角度差所在的直方圖
7. 根據方向剔除誤匹配的點
#### 函式詳解
```
// 獲取該關鍵幀的地圖點
const vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches();
// 和普通幀F特徵點的索引一致
vpMapPointMatches = vector<MapPoint*>(F.N,static_cast<MapPoint*>(NULL));
// 取出關鍵幀的詞袋特徵向量
const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec;
int nmatches=0;
// 特徵點角度旋轉差統計用的直方圖
vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
// 將0~360的數轉換到0~HISTO_LENGTH的係數
// !原作者代碼是 const float factor = 1.0f/HISTO_LENGTH; 是錯誤的，更改為下面代碼
const float factor = 1.0f/HISTO_LENGTH;
// const float factor = HISTO_LENGTH/360.0f;
//將屬於同一節點的ORB特徵進行匹配
DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin();
DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin();
DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();
while(KFit != KFend && Fit != Fend)
{
```
1. 分別取出屬於同一node的ORB特徵點(只有屬於同一node，才有可能是匹配點)
```
// 當兩個特徵屬於圖一個node
if(KFit->first == Fit->first)
{
// 關鍵幀的index
const vector<unsigned int> vIndicesKF = KFit->second;
// 普通幀的index
const vector<unsigned int> vIndicesF = Fit->second;
```
2. 遍歷關鍵幀中屬於該node的特徵點
```
for(size_t iKF=0; iKF<vIndicesKF.size(); iKF++)
{
// 關鍵幀該節點中特徵點的索引
const unsigned int realIdxKF = vIndicesKF[iKF];
// 取出KF中該特徵對應的地圖點
MapPoint* pMP = vpMapPointsKF[realIdxKF];
if(!pMP)
continue;
if(pMP->isBad())
continue;
// 取出KF中該特徵對應的描述子
const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF);
int bestDist1=256; // 最好的距離（最小距離）
int bestIdxF =-1 ;
int bestDist2=256; // 次好距離（倒數第二小距離）
```
3. 遍歷當前普通幀中屬於該node的特徵點
```
for(size_t iF=0; iF<vIndicesF.size(); iF++)
{
// 和上面for循環重名了,這裡的 realIdxF 是指普通幀該節點中特徵點的索引
const unsigned int realIdxF = vIndicesF[iF];
// 如果地圖點存在，說明這個點已經被匹配過了，不再匹配，加快速度
if(vpMapPointMatches[realIdxF])
continue;
// 取出F中該特徵對應的描述子
const cv::Mat &dF = F.mDescriptors.row(realIdxF);
```
4. 尋找最佳匹配點
```
// 計算描述子的距離
const int dist = DescriptorDistance(dKF,dF);
// 遍歷，記錄最佳距離、最佳距離對應的索引、次佳距離等
// 如果 dist < bestDist1 < bestDist2，更新bestDist1 bestDist2
if(dist<bestDist1)
{
bestDist2=bestDist1;
bestDist1=dist;
bestIdxF=realIdxF;
}
// 如果bestDist1 < dist < bestDist2，更新bestDist2
else if(dist<bestDist2)
{
bestDist2=dist;
}
}
```
5. 根據閾值和角度投票剔除誤匹配
```
//5.1. 第一關篩選：匹配距離必須小於設定閾值
if(bestDist1<=TH_LOW)
{
//5.2. 第二關篩選：最佳匹配比次佳匹配明顯要好，那麼最佳匹配才真正可以
if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
{
//5.3. 記錄成功匹配特徵點的對應的地圖點(來自關鍵幀)
vpMapPointMatches[bestIdxF]=pMP;
// 這裡的realIdxKF是當前遍歷到的關鍵幀的特徵點id
const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];
//5.4. 計算匹配點旋轉角度差所在的直方圖
if(mbCheckOrientation)
{
// angle：每個特徵點在提取描述子時的旋轉主方向角度，如果圖像旋轉了，這個角度將發生改變
// 所有的特徵點的角度變化應該是一致的，通過直方圖統計得到最準確的角度變化值
float rot = kp.angle-F.mvKeys[bestIdxF].angle; // 該特徵點的角度變化值
if(rot<0.0)
rot+=360.0f;
int bin = round(rot*factor); // 將rot分配到bin組, 四捨五入, 其實就是離散到對應的直方圖組中
if(bin==HISTO_LENGTH)
bin=0;
assert(bin>=0 && bin<HISTO_LENGTH);
rotHist[bin].push_back(bestIdxF); // 直方圖統計
}
nmatches++;
}
}
}
KFit++;
Fit++;
}
else if(KFit->first < Fit->first)
{
// 對齊
KFit = vFeatVecKF.lower_bound(Fit->first);
}
else
{
// 對齊
Fit = F.mFeatVec.lower_bound(KFit->first);
}
}
```
6. 根據方向剔除誤匹配的點
```
if(mbCheckOrientation)
{
// index
int ind1=-1;
int ind2=-1;
int ind3=-1;
// 篩選出在旋轉角度差落在在直方圖區間內數量最多的前三個bin的索引
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i<HISTO_LENGTH; i++)
{
// 如果特徵點的旋轉角度變化量屬於這三個組，則保留
if(i==ind1 || i==ind2 || i==ind3)
continue;
// 剔除掉不在前三的匹配對，因為他們不符合“主流旋轉方向”
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
nmatches--;
}
}
}
return nmatches;
```
### ORBmatcher::SearchByProjection()
將上一幀跟踪的地圖點投影到當前幀，並且搜索匹配點。用於跟踪前一幀
在[Tracking::TrackWithMotionModel()](#Tracking::TrackWithMotionModel())中引用
#### 函式流程
1. 建立旋轉直方圖，用於檢測旋轉一致性
2. 計算當前幀和前一幀的平移向量[知識點1]
3. 對於前一幀的每一個地圖點，通過相機投影模型，得到投影到當前幀的像素坐標
4. 根據相機的前後前進方向來判斷搜索尺度範圍
5. 歷候選匹配點，尋找距離最小的最佳匹配點
6. 計算匹配點旋轉角度差所在的直方圖
7. 進行旋轉一致檢測，剔除不一致的匹配
#### 知識點
1. 座標系轉換
Tcw: 當前幀姿態
Tlw: 上一幀姿態
Tlc: 當前幀相對於上一幀相機的轉換
#### 函式詳解
1. 建立旋轉直方圖，用於檢測旋轉一致性
```
int nmatches = 0;
// Rotation Histogram (to check rotation consistency)
vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
//! 原作者代碼是 const float factor = 1.0f/HISTO_LENGTH; 是錯誤的，更改為下面代碼
// const float factor = HISTO_LENGTH/360.0f;
const float factor = 1.0f/HISTO_LENGTH;
```
2. 計算當前幀和前一幀的平移向量
```
//當前幀的相機位姿
const cv::Mat Rcw = CurrentFrame.mTcw.rowRange(0,3).colRange(0,3);
const cv::Mat tcw = CurrentFrame.mTcw.rowRange(0,3).col(3);
//當前相機坐標系到世界坐標系的平移向量
const cv::Mat twc = -Rcw.t()*tcw;
//上一幀的相機位姿
const cv::Mat Rlw = LastFrame.mTcw.rowRange(0,3).colRange(0,3);
const cv::Mat tlw = LastFrame.mTcw.rowRange(0,3).col(3); // tlw(l)
// vector from LastFrame to CurrentFrame expressed in LastFrame
// 當前幀相對於上一幀相機的平移向量
const cv::Mat tlc = Rlw*twc+tlw;
// 判斷前進還是後退
const bool bForward = tlc.at<float>(2) > CurrentFrame.mb && !bMono; // 非單目情況，如果Z大於基線，則表示相機明顯前進
const bool bBackward = -tlc.at<float>(2) > CurrentFrame.mb && !bMono; // 非單目情況，如果-Z小於基線，則表示相機明顯後退
```
3. 對於前一幀的每一個地圖點，通過相機投影模型，得到投影到當前幀的像素坐標
```
for(int i=0; i<LastFrame.N; i++)
{
MapPoint* pMP = LastFrame.mvpMapPoints[i];
if(pMP)
{
if(!LastFrame.mvbOutlier[i])
{
// 對上一幀有效的MapPoints投影到當前幀坐標系
cv::Mat x3Dw = pMP->GetWorldPos();
//轉換到相機座標系
cv::Mat x3Dc = Rcw*x3Dw+tcw;
//用內參轉換到影像坐標系
const float xc = x3Dc.at<float>(0);
const float yc = x3Dc.at<float>(1);
const float invzc = 1.0/x3Dc.at<float>(2);
if(invzc<0)
continue;
// 投影到當前幀中
float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx;
float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;
if(u<CurrentFrame.mnMinX || u>CurrentFrame.mnMaxX)
continue;
if(v<CurrentFrame.mnMinY || v>CurrentFrame.mnMaxY)
continue;
// 上一幀中地圖點對應二維特徵點所在的金字塔層級
int nLastOctave = LastFrame.mvKeys[i].octave;
// Search in a window. Size depends on scale
// 單目：th = 7，雙目：th = 15
float radius = th*CurrentFrame.mvScaleFactors[nLastOctave]; // 尺度越大，搜索範圍越大
```
4. 根據相機的前後前進方向來判斷搜索尺度範圍
```
// 記錄候選匹配點的id
vector<size_t> vIndices2;
// 以下可以這麼理解，例如一個有一定面積的圓點，在某個尺度n下它是一個特徵點
// 當相機前進時，圓點的面積增大，在某個尺度m下它是一個特徵點，由於面積增大，則需要在更高的尺度下才能檢測出來
// 當相機後退時，圓點的面積減小，在某個尺度m下它是一個特徵點，由於面積減小，則需要在更低的尺度下才能檢測出來
if(bForward) // 前進,則上一幀興趣點在所在的尺度nLastOctave<=nCurOctave
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);
else if(bBackward) // 後退,則上一幀興趣點在所在的尺度0<=nCurOctave<=nLastOctave
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, 0, nLastOctave);
else // 在[nLastOctave-1, nLastOctave+1]中搜索
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave-1, nLastOctave+1);
//是否有候選匹配點
if(vIndices2.empty())
continue;
const cv::Mat dMP = pMP->GetDescriptor();
int bestDist = 256;
int bestIdx2 = -1;
```
5. 歷候選匹配點，尋找距離最小的最佳匹配點
```
for(vector<size_t>::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++)
{
const size_t i2 = *vit;
// 如果該特徵點已經有對應的MapPoint了,則退出該次循環
if(CurrentFrame.mvpMapPoints[i2])
if(CurrentFrame.mvpMapPoints[i2]->Observations()>0)
continue;
if(CurrentFrame.mvuRight[i2]>0)
{
// 雙目和rgbd的情況，需要保證右圖的點也在搜索半徑以內
const float ur = u - CurrentFrame.mbf*invzc;
const float er = fabs(ur - CurrentFrame.mvuRight[i2]);
if(er>radius)
continue;
}
const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);
const int dist = DescriptorDistance(dMP,d);
if(dist<bestDist)
{
bestDist=dist;
bestIdx2=i2;
}
}
// 最佳匹配距離要小於設定閾值
if(bestDist<=TH_HIGH)
{
CurrentFrame.mvpMapPoints[bestIdx2]=pMP;
nmatches++;
```
6. 計算匹配點旋轉角度差所在的直方圖
```
if(mbCheckOrientation)
{
float rot = LastFrame.mvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].angle;
if(rot<0.0)
rot+=360.0f;
int bin = round(rot*factor);
if(bin==HISTO_LENGTH)
bin=0;
assert(bin>=0 && bin<HISTO_LENGTH);
rotHist[bin].push_back(bestIdx2);
}
}
}
}
}
```
7. 進行旋轉一致檢測，剔除不一致的匹配
```
//Apply rotation consistency
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i<HISTO_LENGTH; i++)
{
// 對於數量不是前3個的點對，剔除
if(i!=ind1 && i!=ind2 && i!=ind3)
{
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
CurrentFrame.mvpMapPoints[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
nmatches--;
}
}
}
}
return nmatches;
```
### ORBmatcher::SearchByProjection()局部地圖點
對於每個局部3D點通過投影在小範圍內找到和最匹配的2D點。從而實現Frame對Local MapPoint的跟踪。用於tracking過程中實現當前幀對局部3D點的跟踪。
在[Tracking::SearchLocalPoints()](#Tracking::SearchLocalPoints())中引用
#### 定義
```
int ORBmatcher::SearchByProjection(
Frame &F, // 當前幀
const vector<MapPoint*> &vpMapPoints, // 局部地圖點
const float th) //
```
#### 函式流程
1. 遍歷有效的局部地圖點
2. 設定搜索搜索窗口的大小
3. 通過投影點以及搜索窗口和預測的尺度進行搜索,找出搜索半徑內的候選匹配點索引
4. 尋找候選匹配點中的最佳和次佳匹配點
5. 篩選最佳匹配點
#### 函式詳解
1. 遍歷有效的局部地圖點
```
int nmatches=0;
// 如果 th！=1 (RGBD 相機或者剛剛進行過重定位), 需要擴大範圍搜索
const bool bFactor = th!=1.0;
for(size_t iMP=0; iMP<vpMapPoints.size(); iMP++)
{
//取得局部地圖點
MapPoint* pMP = vpMapPoints[iMP];
// 判斷該點是否要投影
if(!pMP->mbTrackInView)
continue;
if(pMP->isBad())
continue;
// 通過距離預測的金字塔層數，該層數相對於當前的幀
const int &nPredictedLevel = pMP->mnTrackScaleLevel;
```
2. 設定搜索搜索窗口的大小。取決於視角, 若當前視角和平均視角夾角較小時, r取一個較小的值
```
float r = RadiusByViewingCos(pMP->mTrackViewCos);
// 如果需要擴大範圍搜索，則乘以閾值th
if(bFactor)
r*=th;
```
跳到[ORBmatcher::RadiusByViewingCos()](#ORBmatcher::RadiusByViewingCos())
3. 通過投影點以及搜索窗口和預測的尺度進行搜索, 找出搜索半徑內的候選匹配點索引
```
const vector<size_t> vIndices =
F.GetFeaturesInArea(pMP->mTrackProjX,pMP->mTrackProjY, // 該地圖點投影到一幀上的坐標
r*F.mvScaleFactors[nPredictedLevel], // 認為搜索窗口的大小和該特徵點被追踪到時所處的尺度也有關係
nPredictedLevel-1,nPredictedLevel); // 搜索的圖層範圍
// 沒找到候選的,就放棄對當前點的匹配
if(vIndices.empty())
continue;
const cv::Mat MPdescriptor = pMP->GetDescriptor();
// 最優的次優的描述子距離和index
int bestDist=256;
int bestLevel= -1;
int bestDist2=256;
int bestLevel2 = -1;
int bestIdx =-1 ;
```
F.GetFeaturesInArea : 跳到[Frame::GetFeaturesInArea()](#Frame::GetFeaturesInArea())
4. 尋找候選匹配點中的最佳和次佳匹配點
```
for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
{
const size_t idx = *vit;
// 如果Frame中的該興趣點已經有對應的MapPoint了,則退出該次循環
if(F.mvpMapPoints[idx])
if(F.mvpMapPoints[idx]->Observations()>0)
continue;
//如果是雙目數據
if(F.mvuRight[idx]>0)
{
//計算在X軸上的投影誤差
const float er = fabs(pMP->mTrackProjXR-F.mvuRight[idx]);
//超過閾值,說明這個點不行,丟掉.
//這裡的閾值定義是以給定的搜索範圍r為參考,然後考慮到越近的點(nPredictedLevel越大), 相機運動時對其產生的影響也就越大,
//因此需要擴大其搜索空間.
//當給定縮放倍率為1.2的時候, mvScaleFactors 中的數據是: 1 1.2 1.2^2 1.2^3 ...
if(er>r*F.mvScaleFactors[nPredictedLevel])
continue;
}
const cv::Mat &d = F.mDescriptors.row(idx);
// 計算地圖點和候選投影點的描述子距離
const int dist = DescriptorDistance(MPdescriptor,d);
// 尋找描述子距離最小和次小的特徵點和索引
if(dist<bestDist)
{
bestDist2=bestDist;
bestDist=dist;
bestLevel2 = bestLevel;
bestLevel = F.mvKeysUn[idx].octave;
bestIdx=idx;
}
else if(dist<bestDist2)
{
bestLevel2 = F.mvKeysUn[idx].octave;
bestDist2=dist;
}
}
```
5. 篩選最佳匹配點
```
// 最佳匹配距離還需要滿足在設定閾值內
if(bestDist<=TH_HIGH)
{
// 條件1：bestLevel==bestLevel2 表示 最佳和次佳在同一金字塔層級
// 條件2：bestDist>mfNNratio*bestDist2 表示最佳和次佳距離不滿足閾值比例。理論來說 bestDist/bestDist2 越小越好
if(bestLevel==bestLevel2 && bestDist>mfNNratio*bestDist2)
continue;
//保存結果: 為Frame中的特徵點增加對應的MapPoint
F.mvpMapPoints[bestIdx]=pMP;
nmatches++;
}
}
return nmatches;
```
### ORBmatcher::SearchByProjection()sim3
根據Sim3變換，將閉環KF及其共視KF的所有地圖點（不考慮當前KF已經匹配的地圖點）投影到當前KF，生成新的匹配點對。
在[LoopClosing::ComputeSim3()](#LoopClosing::ComputeSim3())中引用
##### 定義
```
int ORBmatcher::SearchByProjection(
KeyFrame* pKF, // 輸入當前關鍵幀
cv::Mat Scw, // 輸入當前關鍵幀和閉環關鍵幀之間的Sim3變換
const vector<MapPoint*> &vpPoints, // 輸入閉環關鍵幀及其共視關鍵幀的地圖點
vector<MapPoint*> &vpMatched, // 輸入當前關鍵幀的已經匹配的地圖點
int th) // 輸入搜索範圍
```
#### 函式流程
1. 分解Sim3變換矩陣
2. 遍歷閉環KF及其共視KF的所有地圖點（不考慮當前KF已經匹配的地圖點）投影到當前KF
2.1. 丟棄壞點，跳過當前KF已經匹配上的地圖點
2.2. 投影到當前KF的圖像坐標並判斷是否有效
2.3. 搜索候選匹配點
2.4. 遍歷候選匹配點，找到最佳匹配點
4. 返回新的成功匹配的點對的數目
#### 函式詳解
1. 分解Sim3變換矩陣
```
// Get Calibration Parameters for later projection
const float &fx = pKF->fx;
const float &fy = pKF->fy;
const float &cx = pKF->cx;
const float &cy = pKF->cy;
// 這裡的尺度在Pc歸一化時會被約掉。可以理解為投影的時候不需要尺度，因為變換到了射線上，尺度無關
// 尺度會在後面優化的時候用到
cv::Mat sRcw = Scw.rowRange(0,3).colRange(0,3);
const float scw = sqrt(sRcw.row(0).dot(sRcw.row(0))); // 計算得到尺度s
cv::Mat Rcw = sRcw/scw; // 保證旋轉矩陣行列式為1
cv::Mat tcw = Scw.rowRange(0,3).col(3)/scw; // 去掉尺度後的平移向量
cv::Mat Ow = -Rcw.t()*tcw; // 世界坐標系下相機光心坐標
// 使用set類型，記錄前面已經成功的匹配關係，避免重複匹配。並去除其中無效匹配關係（NULL）
set<MapPoint*> spAlreadyFound(vpMatched.begin(), vpMatched.end());
spAlreadyFound.erase(static_cast<MapPoint*>(NULL));
```
2. 遍歷閉環KF及其共視KF的所有地圖點（不考慮當前KF已經匹配的地圖點）投影到當前KF
```
int nmatches=0;
// 遍歷閉環KF及其共視KF的所有地圖點
for(int iMP=0, iendMP=vpPoints.size(); iMP<iendMP; iMP++)
{
MapPoint* pMP = vpPoints[iMP];
// 2.1. 丟棄壞點，跳過當前KF已經匹配上的地圖點
if(pMP->isBad() || spAlreadyFound.count(pMP))
continue;
// 2.2. 投影到當前KF的圖像坐標並判斷是否有效
cv::Mat p3Dw = pMP->GetWorldPos();
// Transform into Camera Coords.
cv::Mat p3Dc = Rcw*p3Dw+tcw;
// 深度值必須為正
if(p3Dc.at<float>(2)<0.0)
continue;
// Project into Image
const float invz = 1/p3Dc.at<float>(2);
const float x = p3Dc.at<float>(0)*invz;
const float y = p3Dc.at<float>(1)*invz;
const float u = fx*x+cx;
const float v = fy*y+cy;
// 在圖像範圍內
if(!pKF->IsInImage(u,v))
continue;
// 判斷距離是否在有效距離內
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
// 地圖點到相機光心的向量
cv::Mat PO = p3Dw-Ow;
const float dist = cv::norm(PO);
if(dist<minDistance || dist>maxDistance)
continue;
// Viewing angle must be less than 60 deg
// 觀察角度小於60°
cv::Mat Pn = pMP->GetNormal();
if(PO.dot(Pn)<0.5*dist)
continue;
// 根據當前這個地圖點距離當前KF光心的距離,預測該點在當前KF中的尺度(圖層)
int nPredictedLevel = pMP->PredictScale(dist,pKF);
// Search in a radius
// 根據尺度確定搜索半徑
const float radius = th*pKF->mvScaleFactors[nPredictedLevel];
// 2.3. 搜索候選匹配點
const vector<size_t> vIndices = pKF->GetFeaturesInArea(u,v,radius);
if(vIndices.empty())
continue;
// Match to the most similar keypoint in the radius
const cv::Mat dMP = pMP->GetDescriptor();
int bestDist = 256;
int bestIdx = -1;
// 2.4. 遍歷候選匹配點，找到最佳匹配點
for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
{
const size_t idx = *vit;
if(vpMatched[idx])
continue;
const int &kpLevel= pKF->mvKeysUn[idx].octave;
// 不在一個尺度也不行
if(kpLevel<nPredictedLevel-1 || kpLevel>nPredictedLevel)
continue;
const cv::Mat &dKF = pKF->mDescriptors.row(idx);
const int dist = DescriptorDistance(dMP,dKF);
if(dist<bestDist)
{
bestDist = dist;
bestIdx = idx;
}
}
// 該MapPoint與bestIdx對應的特徵點匹配成功
if(bestDist<=TH_LOW)
{
vpMatched[bestIdx]=pMP;
nmatches++;
}
}
```
3. 返回新的成功匹配的點對的數目
```
return nmatches;
```
### ORBmatcher::RadiusByViewingCos()
根據觀測的角度來計算匹配時的搜索範圍
在[ORBmatcher::SearchByProjection()局部地圖點](#ORBmatcher::SearchByProjection()局部地圖點)中引用
##### 定義
```
float ORBmatcher::RadiusByViewingCos(const float &viewCos) //輸入視角
```
#### 函式流程
1. 根據輸入視角判斷搜索範圍
#### 函式詳解
1. 根據輸入視角判斷搜索範圍
```
// 當視角相差小於3.6°，對應cos(3.6°)=0.998，搜索範圍是2.5，否則是4
if(viewCos>0.998)
return 2.5;
else
return 4.0;
```
### ORBmatcher::Fuse()
將地圖點投影(用關鍵幀的姿態)到關鍵幀中，並判斷是否有重複的地圖點
1. 如果地圖點能匹配關鍵幀的特徵點，並且該點有對應的地圖點，那麼將兩個地圖點合併（選擇觀測數多的）
2. 如果地圖點能匹配關鍵幀的特徵點，並且該點沒有對應的地圖點，那麼為該點添加地圖點
在[LocalMapping::SearchInNeighbors()](#LocalMapping::SearchInNeighbors())中引用
##### 定義
```
int ORBmatcher::Fuse(KeyFrame *pKF, // 相鄰關鍵幀
const vector<MapPoint *> &vpMapPoints, // 當前關鍵幀的地圖點
const float th) // 搜索半徑因子
```
#### 函式流程
1. 判斷地圖點的有效性
2. 得到地圖點投影到關鍵幀的影像坐標
3. 地圖點到關鍵幀相機光心距離需滿足在有效範圍內
4. 地圖點到光心的連線與該地圖點的平均觀測向量之間夾角要小於60°
5. 在投影點附近搜索窗口內找到候選匹配點的索引
6. 遍歷尋找最佳匹配點
7. 找到投影點對應的最佳匹配特徵點，根據是否存在地圖點來融合或新增
#### 函式詳解
```
// 取出當前幀位姿、內參、光心在世界坐標系下坐標
cv::Mat Rcw = pKF->GetRotation();
cv::Mat tcw = pKF->GetTranslation();
const float &fx = pKF->fx;
const float &fy = pKF->fy;
const float &cx = pKF->cx;
const float &cy = pKF->cy;
const float &bf = pKF->mbf;
cv::Mat Ow = pKF->GetCameraCenter();
int nFused=0;
const int nMPs = vpMapPoints.size();
```
1. 判斷地圖點的有效性
```
// 遍歷所有的待投影地圖點
for(int i=0; i<nMPs; i++)
{
MapPoint* pMP = vpMapPoints[i];
if(!pMP)
continue;
// 地圖點無效 或 已經是該幀的地圖點（無需融合），跳過
if(pMP->isBad() || pMP->IsInKeyFrame(pKF))
continue;
// 將地圖點變換到關鍵幀的相機坐標系下
cv::Mat p3Dw = pMP->GetWorldPos();
cv::Mat p3Dc = Rcw*p3Dw + tcw;
// Depth must be positive
// 深度值為負，跳過
if(p3Dc.at<float>(2)<0.0f)
continue;
```
2. 得到地圖點投影到關鍵幀的影像坐標
```
const float invz = 1/p3Dc.at<float>(2);
const float x = p3Dc.at<float>(0)*invz;
const float y = p3Dc.at<float>(1)*invz;
const float u = fx*x+cx;
const float v = fy*y+cy;
// Point must be inside the image
// 投影點需要在有效範圍內
if(!pKF->IsInImage(u,v))
continue;
const float ur = u-bf*invz; //雙目才會用到
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
cv::Mat PO = p3Dw-Ow;
const float dist3D = cv::norm(PO);
```
3. 地圖點到關鍵幀相機光心距離需滿足在有效範圍內
```
if(dist3D<minDistance || dist3D>maxDistance )
continue;
```
4. 地圖點到光心的連線與該地圖點的平均觀測向量之間夾角要小於60°
```
cv::Mat Pn = pMP->GetNormal();
if(PO.dot(Pn)<0.5*dist3D)
continue;
// 根據地圖點到相機光心距離預測匹配點所在的金字塔尺度
int nPredictedLevel = pMP->PredictScale(dist3D,pKF);
```
跳到[MapPoint::PredictScale()](#MapPoint::PredictScale())
5. 在投影點附近搜索窗口內找到候選匹配點的索引
```
// 確定搜索範圍
const float radius = th*pKF->mvScaleFactors[nPredictedLevel];
const vector<size_t> vIndices = pKF->GetFeaturesInArea(u,v,radius);
if(vIndices.empty())
continue;
```
跳到[KeyFrame::GetFeaturesInArea()](#KeyFrame::GetFeaturesInArea())
6. 遍歷尋找最佳匹配點
```
const cv::Mat dMP = pMP->GetDescriptor();
int bestDist = 256;
int bestIdx = -1;
// 遍歷搜索範圍內的features
for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
{
const size_t idx = *vit;
const cv::KeyPoint &kp = pKF->mvKeysUn[idx];
const int &kpLevel= kp.octave;
// 金字塔層級要接近（同一層或小一層），否則跳過
if(kpLevel<nPredictedLevel-1 || kpLevel>nPredictedLevel)
continue;
// 計算投影點與候選匹配特徵點的距離，如果偏差很大，直接跳過
if(pKF->mvuRight[idx]>=0) // 雙目情況
{
const float &kpx = kp.pt.x;
const float &kpy = kp.pt.y;
const float &kpr = pKF->mvuRight[idx];
const float ex = u-kpx;
const float ey = v-kpy;
// 右目數據的偏差也要考慮進去
const float er = ur-kpr;
const float e2 = ex*ex+ey*ey+er*er;
//自由度為3, 誤差小於1個像素,這種事情95%發生的概率對應卡方檢驗閾值為7.82
if(e2*pKF->mvInvLevelSigma2[kpLevel]>7.8)
continue;
}
else // 單目情況
{
//匹配特徵點
const float &kpx = kp.pt.x;
const float &kpy = kp.pt.y;
//投影點
const float ex = u-kpx;
const float ey = v-kpy;
const float e2 = ex*ex+ey*ey;
// 自由度為2的，卡方檢驗閾值5.99（假設測量有一個像素的偏差）
if(e2*pKF->mvInvLevelSigma2[kpLevel]>5.99)
continue;
}
const cv::Mat &dKF = pKF->mDescriptors.row(idx);
//計算距離
const int dist = DescriptorDistance(dMP,dKF);
// 找地圖點在此區域最佳匹配的特徵點
if(dist<bestDist)
{
bestDist = dist;
bestIdx = idx;
}
}
```
7. 找到投影點對應的最佳匹配特徵點，根據是否存在地圖點來融合或新增
```
// 最佳匹配距離要小於閾值
if(bestDist<=TH_LOW)
{
MapPoint* pMPinKF = pKF->GetMapPoint(bestIdx);
if(pMPinKF)
{
// 如果最佳匹配點有對應有效地圖點，選擇被觀測次數最多的那個替換
if(!pMPinKF->isBad())
{
if(pMPinKF->Observations()>pMP->Observations())
pMP->Replace(pMPinKF);
else
pMPinKF->Replace(pMP);
}
}
else
{
// 如果最佳匹配點沒有對應地圖點，添加觀測信息
pMP->AddObservation(pKF,bestIdx);
pKF->AddMapPoint(pMP,bestIdx);
}
nFused++;
}
}
return nFused;
```
Sign in with Wallet
Connect another wallet