ORB-SLAM2特征匹配
特征匹配本质上就是 两幅图像的特征点匹配，以及对应地图点的匹配。
1 单目初始化匹配只进行了2d-2d的特征点匹配，然后通过对极几何计算初始位姿，没有3D点（三角化后才有）。
2 地图点投影匹配、Sim(3)相似变换匹配、词袋匹配时都已经有了初始匹配帧的位姿和3D点对应信息，匹配完后续会通过后端优化位姿
1 单目初始化特征匹配
在ORB-SLAM2的单目纯视觉初始化中，认为参与初始化的两帧是比较接近的，即在第1帧提取的特征点坐标对应的第2帧中的位置附近画一个正方形(或圆形)，匹配点应该落在这个圆中。
1.1 快速确定候选匹配点
提取完特征点后会直接将特征点划分到不同的网格中并记录在mGrid里。如图所示，在搜索时是以网格为单位进行的，代码中的网格尺寸默认是64像素×48像素，这样圆内包含的网格数目相比像素数目就大大减少。
但是这种方法也会引起一些问题，假如初始场景变化比较大，这种方法可能匹配效果较差，导致初始化失败！
(1) 源码解析
/**
* @brief 找到在 以x,y为中心,半径为r的圆形内且金字塔层级在[minLevel, maxLevel]的特征点
*
* @param[in] x 特征点坐标x
* @param[in] y 特征点坐标y
* @param[in] r 搜索半径
* @param[in] minLevel 最小金字塔层级
* @param[in] maxLevel 最大金字塔层级
* @return vector<size_t> 返回搜索到的候选匹配点id
*/
vector<size_t> Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel) const
{
// 存储搜索结果的vector
vector<size_t> vIndices;
vIndices.reserve(N);
// step 1
// 1.1 左边界有多少网格----即x-mnMinX-r这一部分由多少个网格组成
// mfGridElementWidthInv一个像素占多少网格(<1) 解析见下面
// mnMinX、mnMinY是去畸变的图像边界
const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));
if(nMinCellX>=FRAME_GRID_COLS)
return vIndices;
// 1.2 右边界到达多少个网格----即x-mnMinX+r这一部分由多少个网格组成
const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
if(nMaxCellX<0)
return vIndices;
// 1.3 上边界
const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
if(nMinCellY>=FRAME_GRID_ROWS)
return vIndices;
// 1.4 下边界
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); // 逻辑或
// Step 2 遍历圆形区域内的所有网格，寻找满足条件的候选特征点，并将其index放到输出里
for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
{
for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
{
// 2.1 获取这个网格内的所有特征点在 Frame::mvKeysUn 中的索引
const vector<size_t> vCell = mGrid[ix][iy];
if(vCell.empty())
continue;
// 2.2 如果这个网格中有特征点，那么遍历这个图像网格中所有的特征点
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;
}
// 2.3 通过检查，计算候选特征点到圆中心的距离，查看是否是在这个圆形区域之内
// x和y即这个圆心，也即上一帧的特征点坐标
const float distx = kpUn.pt.x-x;
const float disty = kpUn.pt.y-y;
// 不超出方向搜索区域
// if(distx*distx + disty*disty < r*r) 改为圆形区域
if(fabs(distx)<r && fabs(disty)<r)
// vCell[j]是特征点索引
vIndices.push_back(vCell[j]);
}
}
}
return vIndices;
}
(2) mfGridElementWidthInv 与 mfGridElementHeightInv
#define FRAME_GRID_ROWS 48
#define FRAME_GRID_COLS 64
// mnMaxX等图像边界由下面函数求解(去畸变) FRAME_GRID_COLS是预定义的值
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
mnMaxX最大列像素 - mnMinX最小列像素 = 图像宽度(多少像素)
(mnMaxX-mnMinX)/FRAME_GRID_COLS就是一个网格占多少像素(>1)
FRAME_GRID_COLS/(mnMaxX-mnMinX)就是一个像素占多少网格(<1)
(3) 去畸变图像的边界mnMinX、mnMaxX、mnMinY、mnMaxY
/**
* @brief 计算去畸变图像的边界
*
* @param[in] imLeft 需要计算边界的图像
*/
void Frame::ComputeImageBounds(const cv::Mat &imLeft)
{
// 如果相机畸变参数不为0，用OpenCV函数进行畸变矫正
if(mDistCoef.at<float>(0)!=0.0)
{
// 1 保存矫正前的图像四个边界点坐标 CV_32F
// 这里创建的是单通道 4*2矩阵，表示单精度浮点数类型
cv::Mat mat(4,2,CV_32F); // 4个坐标8个数，所以4*2矩阵
// 1.1 (0,0)左上角
mat.at<float>(0,0)=0.0; mat.at<float>(0,1)=0.0;
// 1.2 (cols,0)右上角
mat.at<float>(1,0)=imLeft.cols; mat.at<float>(1,1)=0.0;
// 1.3 (0,rows)左下角
mat.at<float>(2,0)=0.0; mat.at<float>(2,1)=imLeft.rows;
// 1.4 (cols,rows)右下角
mat.at<float>(3,0)=imLeft.cols; mat.at<float>(3,1)=imLeft.rows;
// Undistort corners
// https://docs.opencv.org/4.x/d3/d63/classcv_1_1Mat.html#a4eb96e3251417fa88b78e2abd6cfd7d8
// 第一个参数：通道数 第二个：行数 这里2指的是通道数 矩阵维度4*1 相当于成了4个点
mat=mat.reshape(2);
// https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga55c716492470bfe86b0ee9bf3a1f0f7e
// 参数： 输入图像 去畸变图像 mK即相机内参矩阵 mDistCoef相机畸变系数 cv::Mat()外参矩阵，这里为空，默认使用恒等变换
// 最后mk指畸变后新的内参矩阵
cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
mat=mat.reshape(1);
//2 校正后的四个边界点已经不能够围成一个严格的矩形，因此在这个四边形的外侧加边框作为坐标的边界
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));//左下和右下纵坐标最小的,1));
}
else
{
// 如果畸变参数为0，无需去畸变
mnMinX = 0.0f;
mnMaxX = imLeft.cols;
mnMinY = 0.0f;
mnMaxY = imLeft.rows;
}
}
(4) cv::Mat::reshape
cv::Mat::reshape(int cn, int rows = 0)
arg1:cn 通道数
arg2:rows如果不填，默认和原矩阵行维度保持一致
int main()
{
cv::Mat m(4, 2, CV_8UC1);
for (int x = 0; x < 2; x++)
{
for (int y = 0; y < 4; y++)
{
m.at<uchar>(y, x) = x + y;
}
}
// 输出原始矩阵的值
for (int row = 0; row < m.rows; ++row)
{
for (int col = 0; col < m.cols; ++col)
{
// 必须把uchar值强制转换为int
std::cout << static_cast<int>(m.at<uchar>(row, col)) << ' ';
}
cout << endl;
}
cout << endl;
// 将矩阵改为2通道 行维不设置则默认为原来的行维
m = m.reshape(2);
// 矩阵变为4*1
for (int row = 0; row < m.rows; ++row)
{
for (int col = 0; col < m.cols; ++col)
{
cv::Vec2b pixel = m.at<cv::Vec2b>(row, col);
std::cout << "Pixel at (" << row << ", " << col << "): "
<< static_cast<int>(pixel[0]) << ", "
<< static_cast<int>(pixel[1]) << ", "
<< std::endl;
}
}
}
1.2 方向一致性检验
原理是统计两张图像所有匹配对中两个特征，点主方向的差，构建一个直方图。由于两张图像整体发生了运动，因此特征点匹配对主方向整体会有一个固定一致的变化。通常直方图中前三个最大的格子里就是正常的匹配点对，那些误匹配的特征点对此时就会暴露出来，落在直方图之外的其他格子里，这些就是需要剔除的错误匹配。
/**
* @brief 单目初始化中用于参考帧和当前帧的特征点匹配
* 步骤
* Step 1 构建旋转直方图
* Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点
* Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
* Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
* Step 5 计算匹配点旋转角度差所在的直方图
* Step 6 筛除旋转直方图中“非主流”部分
* Step 7 将最后通过筛选的匹配好的特征点保存
* @param[in] F1 初始化参考帧
* @param[in] F2 当前帧
* @param[in & out] vbPrevMatched 本来存储的是参考帧的所有特征点坐标，该函数更新为匹配好的当前帧的特征点坐标
* @param[in & out] vnMatches12 保存参考帧F1中特征点匹配情况，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
* @param[in] windowSize 搜索窗口
* @return int 返回成功匹配的特征点数目
*/
int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize)
{
// 总的匹配数
int nmatches=0;
// F1中特征点和F2中匹配关系，注意是按照F1特征点数目分配空间，值全为-1
// vector构造函数，(数目，数值)
vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);
// Step 1 构建旋转直方图，HISTO_LENGTH = 30
// 创建一个数组，每个元素都是vector<int>
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;
// 匹配的特征点对距离，按照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;
// 只使用原始图像层级(level1=0)上的特征点
if(level1>0)
continue;
// Step 2 在半径窗口内搜索当前帧F2中所有的候选匹配特征点
// GetFeaturesInArea返回搜索到的帧2的候选匹配点索引vector 参数 x,y,r=100,minl=maxl=0
vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);
// 没有帧1的候选匹配点，直接跳过
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
// Step 3 遍历搜索搜索窗口中的所有潜在的匹配候选点，找到最优的和次优的
for(vector<size_t>::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
{
size_t i2 = *vit;
// 取出候选特征点对应的描述子 i2即索引，因为vit是迭代器指针，解引用vector的值
cv::Mat d2 = F2.mDescriptors.row(i2);
// DescriptorDistance函数：计算描述子距离--汉明距离 参数：两个描述子矩阵
int dist = DescriptorDistance(d1,d2);
// 描述子距离>=INT_MAX = vMatchedDistance[i2]
if(vMatchedDistance[i2]<=dist)
continue;
// 描述子距离<=INT_MAX
if(dist<bestDist)
{
bestDist2=bestDist;
bestDist=dist;
bestIdx2=i2; // 记录最佳匹配点的索引
}
else if(dist<bestDist2)
{
bestDist2=dist;
}
}
// Step 4 对最优次优结果进行检查，满足阈值、最优/次优比例，删除重复匹配
// 即使算出了最佳描述子匹配距离，也不一定保证配对成功。要小于设定阈值
if(bestDist<=TH_LOW)
{
// 最佳距离比次佳距离要小于设定的比例，这样特征点辨识度更高 mfNNratio=0.6
if(bestDist<(float)bestDist2*mfNNratio)
{
// 如果找到的候选特征点对应F1中特征点已经匹配过了，说明发生了重复匹配，将原来的匹配也删掉
// vnMatches21初始化帧2每一个特征点索引值都对应-1
if(vnMatches21[bestIdx2]>=0)
{
// vnMatches21[bestIdx2] = i1 即帧1中的特征点索引 设置为-1 也就是说明误匹配
vnMatches12[vnMatches21[bestIdx2]]=-1;
nmatches--; // 总匹配数-1
}
// i1为帧1中的特征点索引 bestIdx2帧2中的特征点索引
vnMatches12[i1]=bestIdx2;
vnMatches21[bestIdx2]=i1; // 互相标记，这是两者值不在是-1 两者有匹配点
vMatchedDistance[bestIdx2]=bestDist; // 距离索引标记
nmatches++;
// Step 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 HISTO_LENGTH=30
// bin = rot / 360.of * HISTO_LENGTH 表示当前rot被分配在第几个直方图bin // 因为是取整，所bin会有0、1、2...29
int bin = round(rot*factor); // 四舍五入取整
// 如果bin 满了又是一个轮回
if(bin==HISTO_LENGTH)
bin=0; // 一共30个数
assert(bin>=0 && bin<HISTO_LENGTH); // 0~29为正常数据
// 把每一个特征点加到对应的直方图区间
rotHist[bin].push_back(i1);
}
}
}
}
// Step 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--;
}
}
}
}
//Update prev matched
// Step 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;
}
// 描述子矩阵: 特征点数*32 每个数都是8位uchar 每一行都代表了一个特征点的描述子
descriptors.create(nkeypoints, 32, CV_8U); // nkeypoints是行数，32是列数
// CV_8U矩阵元素的格式 32*8=256
1.2.1 关于旋转直方图作用！
这里没有采用ORB的匹配方式，利用了最近邻FLANN方法来进行特征匹配，主要是为了对比旋转直方图作用前后的一个效果。很明显少了很多的错误匹配！
无直方图
有直方图
直方图结果
1.3 汉明距离计算
int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b)
{
const int *pa = a.ptr<int32_t>();
const int *pb = b.ptr<int32_t>();
int dist=0;
for(int i=0; i<8; i++, pa++, pb++)
{
// 这将两个描述符中对应整数进行异或操作，得到一个结果v，表示两个描述符在这个位置上的差异。
unsigned int v = *pa ^ *pb;
v = v - ((v >> 1) & 0x55555555);
v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
}
return dist;
}
1.2 ComputeThreeMaxima
void ORBmatcher::ComputeThreeMaxima(vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3)
{
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;
}
}
if(max2<0.1f*(float)max1)
{
ind2=-1;
ind3=-1;
}
else if(max3<0.1f*(float)max1)
{
ind3=-1;
}
}
2 地图点投影匹配
2.1 投影匹配原理
这里本质就是把一帧对应的地图点投影到另一帧，投影即空间的位姿变换。代码中很多都是已知一帧的地图点，获取其世界坐标系下的坐标，然后利用位姿变换Tcw将其投影到目标帧中，转为为像素坐标系下坐标，利用GetFeaturesInArea函数获取待匹配特征点集合，最后计算描述子距离，获取最佳匹配。
一开始肯定会疑惑，要用当前帧的位姿去投影匹配，它是怎么得到的？
// 比如在恒速模型跟踪过程
ORBmatcher matcher(0.9,true);
UpdateLastFrame(); // 计算位姿速度，然后利用上一帧的位姿，对当前帧的位姿进行一个初始的估计
mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
在整个ORB-SLAM2的代码中，一共有四种地图点投影匹配函数(重载)
- 应用在局部地图跟踪
// 函数重载————地图点投影匹配
int SearchByProjection(Frame &F, const std::vector<MapPoint*> &vpMapPoints, const float th=3);
- 应用在恒速模型跟踪
int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono);
- 应用在重定位跟踪
int SearchByProjection(Frame &CurrentFrame, KeyFrame* pKF, const std::set<MapPoint*> &sAlreadyFound, const float th, const int ORBdist);
- 应用在闭环检测
int SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const std::vector<MapPoint*> &vpPoints, std::vector<MapPoint*> &vpMatched, int th);
2.2 恒速跟踪模型中的地图点投影匹配
2.3.1 算法原理
首先判断相机是否有明显的前进或后退
T
c
w
=
[
R
c
w
t
c
w
O
1
]
T
c
w
−
1
=
[
R
c
w
⊤
−
R
c
w
⊤
t
c
w
O
1
]
T
l
w
=
[
R
l
w
t
l
w
O
1
]
\begin{aligned} &\boldsymbol{T_\mathrm{cw}} =\left[\begin{array}{cc}\boldsymbol{R}_\mathrm{cw}&\boldsymbol{t}_\mathrm{cw}\\\boldsymbol{O}&1\end{array}\right] \\ &\boldsymbol{T}_{\mathrm{cw}}^{-1} =\left[\begin{array}{cc}\boldsymbol{R_\mathrm{cw}^\top}&-\boldsymbol{R_\mathrm{cw}^\top t_\mathrm{cw}}\\\boldsymbol{O}&1\end{array}\right] \\ &\boldsymbol{T}_{\mathrm{lw}} =\begin{bmatrix}\boldsymbol{R_\mathrm{lw}}&\boldsymbol{t_\mathrm{lw}}\\\boldsymbol{O}&1\end{bmatrix} \end{aligned}
Tcw=[RcwOtcw1]Tcw−1=[Rcw⊤O−Rcw⊤tcw1]Tlw=[RlwOtlw1]
主要是通过
t
l
c
t_\mathrm{lc}
tlc来判断的
T
l
c
=
T
lw
T
c
w
−
1
=
[
R
l
w
t
l
w
O
1
]
[
R
c
w
⊤
−
R
c
w
⊤
t
c
w
O
1
]
=
[
R
l
w
R
c
w
⊤
−
R
l
w
R
c
w
⊤
t
c
w
+
t
l
w
0
1
]
\begin{aligned} T_{\mathrm{lc}}& =T_\text{ lw}{ T _ { c w }^{-1}} \\ &=\begin{bmatrix}\boldsymbol{R}_\mathrm{lw}&\boldsymbol{t}_\mathrm{lw}\\\boldsymbol{O}&1\end{bmatrix}\begin{bmatrix}\boldsymbol{R}_\mathrm{cw}^\top&-\boldsymbol{R}_\mathrm{cw}^\top\boldsymbol{t}_\mathrm{cw}\\\boldsymbol{O}&1\end{bmatrix} \\ &=\left[\begin{array}{c|c}R_\mathrm{lw}R_\mathrm{cw}^\top&-R_\mathrm{lw}R_\mathrm{cw}^\top t_\mathrm{cw}+t_\mathrm{lw}\\0&1\end{array}\right] \end{aligned}
Tlc=T lwTcw−1=[RlwOtlw1][Rcw⊤O−Rcw⊤tcw1]=[RlwRcw⊤0−RlwRcw⊤tcw+tlw1]
t l c = − R l w R c w ⊤ t c w + t l w t_\mathrm{lc}=-R_\mathrm{lw}R_\mathrm{cw}^\top t_\mathrm{cw}+t_\mathrm{lw} tlc=−RlwRcw⊤tcw+tlw
const bool bForward = tlc.at<float>(2) > CurrentFrame.mb && !bMono;
const bool bBackward = -tlc.at<float>(2) > CurrentFrame.mb && !bMono;
然后根据前面判断的相机运动方向（前进或后退），确定搜索候选匹配特征点的尺度范围。
2.3.2 代码分析
/**
* @brief 将上一帧跟踪的地图点投影到当前帧，并且搜索匹配点。用于跟踪前一帧——————恒速模型跟踪
* @param[in] CurrentFrame 当前帧
* @param[in] LastFrame 上一帧
* @param[in] th 搜索范围阈值，默认单目为7，双目15
* @param[in] bMono 是否为单目
* @return int 成功匹配的数量
*/
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)
{
int nmatches = 0;
// Rotation Histogram (to check rotation consistency)
// Step 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;
// Step 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)
// 当前帧相对于上一帧相机的平移向量
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小于基线，则表示相机明显后退
// Step 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]; // 尺度越大，搜索范围越大
// 记录候选匹配点的id
vector<size_t> vIndices2;
// Step 4 根据相机的前后前进方向来判断搜索尺度范围。
// 以下可以这么理解，例如一个有一定面积的圆点，在某个尺度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;
// Step 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++;
// Step 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);
}
}
}
}
}
//Apply rotation consistency
// Step 7 进行旋转一致检测，剔除不一致的匹配
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;
}
2.3 局部地图跟踪模型中的地图点投影匹配
实际上就是把从局部关键帧获取的有效地图点投影到当前帧上，在投影点附近利用GetFeaturesInArea函数获取可能的特征点索引区域，然后计算描述子距离，选择最佳的特征点，建立特征点与地图点间关系。
/**
* @brief 通过投影地图点到当前帧，对Local MapPoint进行跟踪
* @param[in] F 当前帧
* @param[in] vpMapPoints 局部地图点，来自局部关键帧
* @param[in] th 搜索范围
* @return int 成功匹配的数目
*/
int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints, const float th)
{
int nmatches=0; // 总的匹配数
const bool bFactor = th!=1.0; // 如果 th！=1 (RGBD 相机或者刚刚进行过重定位), 需要扩大范围搜索
// Step 1 遍历有效地图点
for(size_t iMP=0; iMP<vpMapPoints.size(); iMP++)
{
MapPoint* pMP = vpMapPoints[iMP]; // 第iMP个局部地图点
if(!pMP->mbTrackInView)
continue;
if(pMP->isBad())
continue;
const int &nPredictedLevel = pMP->mnTrackScaleLevel; // 通过距离预测的金字塔层数，该层数相对于当前的帧
// The size of the window will depend on the viewing direction
// Step 2 设定搜索搜索窗口的大小。取决于视角, 若当前视角和平均视角夹角较小时, r取一个较小的值
float r = RadiusByViewingCos(pMP->mTrackViewCos);
if(bFactor)
r*=th;
// Step 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();
int bestDist=256;
int bestLevel= -1;
int bestDist2=256;
int bestLevel2 = -1;
int bestIdx =-1 ;
// Get best and second matches with near keypoints
// Step 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;
}
}
// Apply ratio to second match (only if best and second are in the same scale level)
// Step 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;
}
2.4 重定位跟踪模型中的地图点投影匹配
/**
* @brief 通过投影的方式将关键帧中未匹配的地图点投影到当前帧中,进行匹配，并通过旋转直方图进行筛选
*
* @param[in] CurrentFrame 当前帧
* @param[in] pKF 参考关键帧
* @param[in] sAlreadyFound 已经找到的地图点集合，不会用于PNP
* @param[in] th 匹配时搜索范围，会乘以金字塔尺度
* @param[in] ORBdist 匹配的ORB描述子距离应该小于这个阈值
* @return int 成功匹配的数量
*/
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set<MapPoint*> &sAlreadyFound, const float th , const int ORBdist)
{
int nmatches = 0;
const cv::Mat Rcw = CurrentFrame.mTcw.rowRange(0,3).colRange(0,3); // 当前帧位姿Tcw
const cv::Mat tcw = CurrentFrame.mTcw.rowRange(0,3).col(3);
const cv::Mat Ow = -Rcw.t()*tcw; // twc
// Step 1 建立旋转直方图，用于检测旋转一致性
vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
const float factor = HISTO_LENGTH/360.0f;
const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches(); // 获取关键帧匹配好的全部地图点
// Step 2 遍历关键帧中的每个地图点，通过相机投影模型，得到投影到当前帧的像素坐标
for(size_t i=0, iend=vpMPs.size(); i<iend; i++)
{
MapPoint* pMP = vpMPs[i]; // 关键帧的第i个地图点
if(pMP)
{
// 地图点存在 并且 不在已有地图点集合里
if(!pMP->isBad() && !sAlreadyFound.count(pMP))
{
//Project
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);
const float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx; // 像素坐标系
const float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;
if(u<CurrentFrame.mnMinX || u>CurrentFrame.mnMaxX) // 像素越界
continue;
if(v<CurrentFrame.mnMinY || v>CurrentFrame.mnMaxY)
continue;
// Compute predicted scale level
cv::Mat PO = x3Dw-Ow; // 相机光心与地图点的向量，由O指向P
float dist3D = cv::norm(PO); // 长度
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
// Depth must be inside the scale pyramid of the image
if(dist3D<minDistance || dist3D>maxDistance)
continue;
int nPredictedLevel = pMP->PredictScale(dist3D,&CurrentFrame); // 预测地图点对应特征点所在金字塔的尺度
// Search in a window
// 搜索半径和尺度相关
const float radius = th*CurrentFrame.mvScaleFactors[nPredictedLevel];
// Step 3 搜索候选匹配点
const vector<size_t> vIndices2 = CurrentFrame.GetFeaturesInArea(u, v, radius, nPredictedLevel-1, nPredictedLevel+1);
if(vIndices2.empty())
continue;
const cv::Mat dMP = pMP->GetDescriptor();
int bestDist = 256;
int bestIdx2 = -1;
// Step 4 遍历候选匹配点，寻找距离最小的最佳匹配点
for(vector<size_t>::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
{
const size_t i2 = *vit;
if(CurrentFrame.mvpMapPoints[i2])
continue;
const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);
const int dist = DescriptorDistance(dMP,d);
if(dist<bestDist)
{
bestDist=dist;
bestIdx2=i2;
}
}
if(bestDist<=ORBdist)
{
CurrentFrame.mvpMapPoints[bestIdx2]=pMP;
nmatches++;
// Step 5 计算匹配点旋转角度差所在的直方图
if(mbCheckOrientation)
{
float rot = pKF->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);
}
}
}
}
}
// Step 6 进行旋转一致检测，剔除不一致的匹配
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)
{
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
CurrentFrame.mvpMapPoints[rotHist[i][j]]=NULL;
nmatches--;
}
}
}
}
return nmatches;
}
2.5 闭环检测中的地图点投影匹配
换汤不换药
/**
* @brief 根据Sim3变换，将闭环KF及其共视KF的所有地图点（不考虑当前KF已经匹配的地图点）投影到当前KF，生成新的匹配点对
*
* @param[in] pKF 当前KF
* @param[in] Scw 当前KF和闭环KF之间的Sim3变换
* @param[in] vpPoints 闭环KF及其共视KF的地图点
* @param[in] vpMatched 当前KF的已经匹配的地图点
* @param[in] th 搜索范围
* @return int 返回新的成功匹配的点对的数目
*/
int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector<MapPoint*> &vpPoints, vector<MapPoint*> &vpMatched, int th)
{
// 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;
// Decompose Scw
// Step 1 分解Sim变换矩阵
// 这里的尺度在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 of MapPoints already found in the KeyFrame
// 使用set类型，记录前面已经成功的匹配关系，避免重复匹配。并去除其中无效匹配关系（NULL）
set<MapPoint*> spAlreadyFound(vpMatched.begin(), vpMatched.end());
spAlreadyFound.erase(static_cast<MapPoint*>(NULL));
int nmatches=0;
// For each Candidate MapPoint Project and Match
// Step 2 遍历闭环KF及其共视KF的所有地图点（不考虑当前KF已经匹配的地图点）投影到当前KF
for(int iMP=0, iendMP=vpPoints.size(); iMP<iendMP; iMP++)
{
MapPoint* pMP = vpPoints[iMP];
// Discard Bad MapPoints and already found
// Step 2.1 丢弃坏点，跳过当前KF已经匹配上的地图点
if(pMP->isBad() || spAlreadyFound.count(pMP))
continue;
// Get 3D Coords.
// Step 2.2 投影到当前KF的图像坐标并判断是否有效
cv::Mat p3Dw = pMP->GetWorldPos();
// Transform into Camera Coords.
cv::Mat p3Dc = Rcw*p3Dw+tcw;
// Depth must be positive
// 深度值必须为正
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;
// Point must be inside the image
// 在图像范围内
if(!pKF->IsInImage(u,v))
continue;
// Depth must be inside the scale invariance region of the point
// 判断距离是否在有效距离内
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];
// Step 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;
// Step 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++;
}
}
// Step 3 返回新的成功匹配的点对的数目
return nmatches;
}
3 Sim(3)变换投影匹配
在闭环线程中，闭环候选帧和当前关键帧最早是通过词袋进行搜索匹配的。这是因为它们间隔的时间比较远，没有先验信息，这时用词袋搜索是最合适的。
但是词袋搜索会导致漏匹配。而成功的闭环需要在闭环候选帧和当前帧之间尽可能多地建立更多的匹配关系，这时可以利用初步估计的Sim(3)位姿进行相互投影匹配，忽略已经匹配的特征点，只在尚未匹配的特征点中挖掘新的匹配关系。
3.1 相互投影匹配
第1步，先统计它们之间已经匹配好的特征点对，目的是在后续投影中跳过这些已经匹配好的特征点对，从剩下的未匹配的特征点中寻找新的匹配关系。
第2步，把KF1的地图点用
Sim21变换投影到KF2图像上，在投影点附近一定的范围内寻找候选匹配点，从中选择描述子距离最近的点作为最佳匹配点。第3步，把KF2的地图点用
Sim12变换投影到KF1图像上，在投影点附近一定的范围内寻找候选匹配，点，从中选择描述子距离最近的点作为最佳匹配点。第4步，找出同时满足第2、3步要求的特征点匹配对，也就是在两次相互匹配中同时出现的匹配点对，作为最终可靠的新的匹配结果。
S = [ s R t O 1 ] S=\begin{bmatrix}sR&t\\\boldsymbol{O}&1\end{bmatrix} S=[sROt1]
S − 1 = [ 1 s R ⊤ − 1 s R ⊤ t O 1 ] S^{-1}=\begin{bmatrix}\frac1s\boldsymbol{R}^\top&-\frac1s\boldsymbol{R}^\top t\\\boldsymbol{O}&1\end{bmatrix} S−1=[s1R⊤O−s1R⊤t1]
3.2 源码分析
/**
* @brief 通过Sim3变换，搜索两个关键帧中更多的匹配点对
* （之前使用SearchByBoW进行特征点匹配时会有漏匹配）
* @param[in] pKF1 当前帧
* @param[in] pKF2 闭环候选帧
* @param[in] vpMatches12 i表示匹配的pKF1 特征点索引，vpMatches12[i]表示匹配的pKF2中地图点，null表示没有匹配
* @param[in] s12 pKF2 到 pKF1 的Sim 变换中的尺度
* @param[in] R12 pKF2 到 pKF1 的Sim 变换中的旋转矩阵
* @param[in] t12 pKF2 到 pKF1 的Sim 变换中的平移向量
* @param[in] th 搜索窗口的倍数
* @return int 新增的匹配点对数目
*/
int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint*> &vpMatches12,
const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th)
{
// Step 1： 准备工作：获取内参，Sim3相似变换矩阵
const float &fx = pKF1->fx;
const float &fy = pKF1->fy;
const float &cx = pKF1->cx;
const float &cy = pKF1->cy;
// Camera 1 from world
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(); // 关键帧pKF1中地图点数目
const vector<MapPoint*> vpMapPoints2 = pKF2->GetMapPointMatches();
const int N2 = vpMapPoints2.size(); // 关键帧pKF2中地图点数目
// Step 2：记录已经匹配的特征点——————有的特征点已经匹配好了地图点，那就不管哪些特征点，只搜索哪些没有对应地图点的
vector<bool> vbAlreadyMatched1(N1,false);
vector<bool> vbAlreadyMatched2(N2,false);
for(int i=0; i<N1; i++)
{
MapPoint* pMP = vpMatches12[i]; // i是pKF1中特征点索引，vpMatches12[i]是匹配的pKF2中地图点
if(pMP)
{
vbAlreadyMatched1[i]=true;
int idx2 = pMP->GetIndexInKeyFrame(pKF2); // 获取这个地图点在pKF2中的特征点id
if(idx2>=0 && idx2<N2) // 特征点索引不越界
vbAlreadyMatched2[idx2]=true;
}
}
vector<int> vnMatch1(N1,-1);
vector<int> vnMatch2(N2,-1);
// Transform from KF1 to KF2 and search
// Step 3：通过Sim变换，寻找 pKF1 中特征点和 pKF2 中的新的匹配(从1到2)
// 之前使用SearchByBoW进行特征点匹配时会有漏匹配
for(int i1=0; i1<N1; i1++)
{
MapPoint* pMP = vpMapPoints1[i1]; // 关键帧1的特征点对应的地图点，利用sim3相似变换将其投影到关键帧2，获得匹配
if(!pMP || vbAlreadyMatched1[i1]) // 删除已经匹配
continue;
if(pMP->isBad())
continue;
cv::Mat p3Dw = pMP->GetWorldPos(); // 地图点世界系坐标
cv::Mat p3Dc1 = R1w*p3Dw + t1w; // 关键帧1系下坐标----相机坐标系
cv::Mat p3Dc2 = sR21*p3Dc1 + t21; // 关键帧2系下坐标---
// Depth must be positive
if(p3Dc2.at<float>(2)<0.0) // 深度为正
continue;
const float invz = 1.0/p3Dc2.at<float>(2); // 归一化坐标系(关键帧2系)
const float x = p3Dc2.at<float>(0)*invz;
const float y = p3Dc2.at<float>(1)*invz;
const float u = fx*x+cx; // 像素坐标系(关键帧2系)
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 预测地图点对应特征点所在的图像金字塔尺度层数
const int nPredictedLevel = pMP->PredictScale(dist3D,pKF2);
// Search in a radius 搜索半径
const float radius = th*pKF2->mvScaleFactors[nPredictedLevel];
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; // 对应id
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]; // 去畸变的关键帧2的特征点
if(kp.octave<nPredictedLevel-1 || kp.octave>nPredictedLevel) // 不能不在预测尺度区间
continue;
const cv::Mat &dKF = pKF2->mDescriptors.row(idx); // 获取关键帧2对应特征点idx的描述子
const int dist = DescriptorDistance(dMP,dKF); // 欧式距离
if(dist<bestDist)
{
bestDist = dist; // 最好距离
bestIdx = idx; // 关键帧2的特征点索引
}
}
if(bestDist<=TH_HIGH)
{
vnMatch1[i1]=bestIdx; // i1是关键帧1的特征点索引 bestIdx = idx; 关键帧2的特征点索引
}
}
// Transform from KF2 to KF2 and search
// Step 4：通过Sim变换，寻找 pKF2 中特征点和 pKF1 中的新的匹配(从2到1)
// 和上面基本一样，只不过这里是取出2的地图点利用sim3相似变换投影到1中
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;
}
}
// Check agreement
// Step 5： 一致性检查,只有在两次互相匹配中都出现才能够认为是可靠的匹配
int nFound = 0;
for(int i1=0; i1<N1; i1++)
{
int idx2 = vnMatch1[i1]; // 这里想法还是很妙的，1的结果就是2的索引，2的结果是1的索引 一开始的想法还是不一样
if(idx2>=0)
{
int idx1 = vnMatch2[idx2];
if(idx1==i1)
{
vpMatches12[i1] = vpMapPoints2[idx2]; // 关键帧1的特征索引 关键帧2的地图点
nFound++;
}
}
}
return nFound;
}
4 词袋匹配
词袋（Bag of Words,.BoW)，其中的Words表示的是图像中的局部信息，如特征点。在词袋中的单词是没有顺序的，也就是我们只关心某张图像中有没有出现某个单词，出现了多少次，而不关心到底是在图像哪个位置出现的，也不关心单词出现的先后顺序，这样就简化了词袋模型的表达方式，节省了存储空间，可以实现高效索引。
词袋模型中有两个概率，一个是直接索引，另一个是倒排索引。这两个概率分别对应了词袋的两个主要应用：加速特征匹配和闭环检测。
1 为什么要用词袋进行特征匹配？
因为在没有任何先验信息的情况下，如果想要对两张图像中提取的特征点进行匹配，则通常只能用暴力匹配的方法，这样不仅非常慢，而且很容易出现错误匹配。而通过词袋搜索匹配，只需要比较同一个节点下的特征点，因为同一个节点下的特征点通常都是比较相似的，这相当于提前对相似的特征点进行了区域划分，不仅提高了搜索效率，也能诚少很多错误匹配。但是有一个很大的弊端就是能够成功匹配的特征点对较少！
2 为什么可以用于闭环检测？
闭环检测的核心就是判断两张图像是不是同一个场景，本质就是判断图像的相似性。词袋使用图像特征的集合作为单词，只关心图像中这些单词出现的频率，不关心单词出现的位置，而且通常采用的是二进制图像特征，对光照变化比较鲁棒，使用词袋进行闭环检测更符合人类的感知方式。
4.1 字典
词袋模型通过训练图像提取特征点并通过K-means算法不断聚类，最后构建了字典树。分支数 K K K表示每个节点被分为多少类，深度L表示有多少层，第0层是根节点，最底层是叶子节点，即单词。每一个单词都会被赋予一个权重，这个单词出现的频率越高，说明它越没有辨识度，权重就更小；出现的次数越少，权重越大。就像是平庸的人很多，所以工资低；人才很少，工资高。
4.1.1 保存字典
① 代码
// 把训练好的字典保存为txt格式
template<class TDescriptor, class F>
void TemplatedVocabulary<TDescriptor,F>::saveToTextFile(const std::string &filename) const
{
fstream f;
f.open(filename.c_str(),ios_base::out); // 输出流
// m_k字典分支数 m_L字典层数 m_scoring评分方式 m_weighting权重计算方式
f << m_k << " " << m_L << " " << " " << m_scoring << " " << m_weighting << endl;
for(size_t i=1; i<m_nodes.size();i++) // 遍历每个节点
{
const Node& node = m_nodes[i];
f << node.parent << " "; // 当前节点的父节点
if(node.isLeaf()) // 当前节点是不是叶子节点
f << 1 << " "; // 是为1
else
f << 0 << " ";
// 存储当前节点对应的256位描述子，用32个无符号8位数描述，即每个数介于0~255
// 最后存储当前节点的权重
f << F::toString(node.descriptor) << " " << (double)node.weight << endl;
}
f.close();
}
② ORB字典格式
每个节点10个分支 一共是6层,可以看出，只有叶子节点才有相应的权重，非叶子节点为0.
10 6 0 0
0 0 252 188 188 242 169 109 85 143 187 191 164 25 222 255 72 27 129 215 237 16 58 111 219 51 219 211 85 127 192 112 134 34 0 # 1+1+32+1所以一行共35个数
0 0 93 125 221 103 180 14 111 184 112 234 255 76 215 115 153 115 22 196 124 110 233 240 249 46 237 239 101 20 104 243 66 33 0
......
1 0 244 52 157 119 168 111 85 154 178 187 167 24 215 115 73 27 129 214 108 58 56 101 219 2 201 243 117 60 192 113 198 34 0
......
41 1 212 52 57 252 8 239 85 123 188 238 225 24 247 247 33 25 149 185 93 34 57 71 114 31 209 227 115 56 208 97 7 115 7.50133
4.1.2 加载字典
总的节点数，按上面图总第四层level = 4的叶子节点应该是
3
4
3^4
34.
K
L
+
1
−
1
K
−
1
\frac{K^{L+1}-1}{K-1}
K−1KL+1−1
template<class TDescriptor, class F>
bool TemplatedVocabulary<TDescriptor,F>::loadFromTextFile(const std::string &filename)
{
ifstream f;
f.open(filename.c_str());
if(f.eof())
return false;
m_words.clear();
m_nodes.clear();
string s;
getline(f,s);
stringstream ss;
ss << s;
ss >> m_k; // 读取分支数
ss >> m_L; // 深度
int n1, n2;
ss >> n1; // 评分方式
ss >> n2; // 权重计算方式
if(m_k<0 || m_k>20 || m_L<1 || m_L>10 || n1<0 || n1>5 || n2<0 || n2>3)
{
std::cerr << "Vocabulary loading failure: This is not a correct text file!" << endl;
return false;
}
m_scoring = (ScoringType)n1; // 枚举类型，转化为相应的评分方式
m_weighting = (WeightingType)n2;
createScoringObject();
// 总的nodes数 = (k^(L+1)-1)/(k-1)
int expected_nodes =
(int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1));
m_nodes.reserve(expected_nodes);
m_words.reserve(pow((double)m_k, (double)m_L + 1)); // 单词数 = K^(L+1) 有点问题
m_nodes.resize(1);
m_nodes[0].id = 0;
while(!f.eof())
{
string snode;
getline(f,snode);
stringstream ssnode;
ssnode << snode;
int nid = m_nodes.size();
m_nodes.resize(m_nodes.size()+1);
m_nodes[nid].id = nid;
int pid ;
ssnode >> pid;
m_nodes[nid].parent = pid;
m_nodes[pid].children.push_back(nid);
int nIsLeaf;
ssnode >> nIsLeaf;
stringstream ssd;
for(int iD=0;iD<F::L;iD++)
{
string sElement;
ssnode >> sElement;
ssd << sElement << " ";
}
F::fromString(m_nodes[nid].descriptor, ssd.str());
ssnode >> m_nodes[nid].weight;
if(nIsLeaf>0)
{
int wid = m_words.size();
m_words.resize(wid+1);
m_nodes[nid].word_id = wid;
m_words[wid] = &m_nodes[nid];
}
else
{
m_nodes[nid].children.reserve(m_k);
}
}
return true;
}
4.1.3 权重类型和评分方式
/// Weighting type
enum WeightingType
{
TF_IDF,
TF,
IDF,
BINARY
};
/// Scoring type
enum ScoringType
{
L1_NORM,
L2_NORM,
CHI_SQUARE,
KL,
BHATTACHARYYA,
DOT_PRODUCT,
};
4.2 词袋向量—相似度检验
在ORB-SLAM2中，对于新的一帧图像，会利用上面的离线字典为当前图像在线生成词袋向量。
第1步，对新的一帧图像先提取ORB特征点，特征点描述子和离线字典中的一致。
第2步，对于每个特征点的描述子，从离线创建好的字典树中自上而下开始寻找自己的位置，从根节点开始，用该描述子和每个节点的描述子计算汉明距离，选择汉明距离最小的节点作为自己所在的节点，一直遍历到叶子节点。最终把叶子的单词
id和权重等属性赋予这个特征点。树状结构的遍历是很快的！
class BowVector: // 一帧图像的全部特征点被分为了单词和对应的权重，相同的单词会不断累计权重
std::map<WordId, WordValue> // 词袋向量
// WordId指特征点对应单词的id WordValue指对应这个单词的权重
// 更新BowVector中单词的权重，就是说这一张图像中又出现了属于同一个单词的特征点
void BowVector::addWeight(WordId id, WordValue v)
{
BowVector::iterator vit = this->lower_bound(id);
if(vit != this->end() && !(this->key_comp()(id, vit->first)))
{
vit->second += v;
}
else
{
this->insert(vit, BowVector::value_type(id, v));
}
}
4.3 特征向量—加速特征匹配
特征向量的存在就是为了加速特征匹配存在的，上面通过不停的遍历已经得到了当前帧的特征点对于与哪一个叶子节点。这里又定义了一个变量level up，即下面所示。我们知道当前特征点所属叶子节点，减去level up，即可找到其隶属的NodeId节点。在进行特征匹配时候，都是比较同一个NodeId节点下的特征点，这样子可以加速特征匹配！
class FeatureVector: // 特征向量
public std::map<NodeId, std::vector<unsigned int> >
// 其中，NodeId并不是该叶子节点的直接父节点ID,而是距离叶子节点深度为level up的节点的ID。在进行特征匹配时，搜索该单词的匹配点时，搜索范围是和它具有同样Nodeld的所有子节点。所以，搜索范围的大小是根据level up确定的，level up值越大，搜索范围越广，搜索速度越慢；level up值越小，搜索范围越小，搜索速度越快，但能够匹配的特征就越少。
// std::vector<unsigned int>实际存储的是处于NodeId节点下的所有特征点在图像中的索引
关于DBOW2中源码将图像帧的描述子矩阵vector如何转换词袋向量和特征向量
// 转换函数
template<class TDescriptor, class F>
void TemplatedVocabulary<TDescriptor,F>::transform(
const std::vector<TDescriptor>& features,
BowVector &v, FeatureVector &fv, int levelsup) const
{
...
// normalize
LNorm norm;
bool must = m_scoring_object->mustNormalize(norm);
typename vector<TDescriptor>::const_iterator fit;
if(m_weighting == TF || m_weighting == TF_IDF)
{
unsigned int i_feature = 0;
for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)
{
WordId id;
NodeId nid;
WordValue w;
// w is the idf value if TF_IDF, 1 if TF
transform(*fit, id, w, &nid, levelsup);
if(w > 0) // not stopped
{
v.addWeight(id, w);
fv.addFeature(nid, i_feature);
}
}
if(!v.empty() && !must)
{
// unnecessary when normalizing
const double nd = v.size();
for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++)
vit->second /= nd;
}
}
else // IDF || BINARY
{
unsigned int i_feature = 0;
for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)
{
WordId id;
NodeId nid;
WordValue w;
// w is idf if IDF, or 1 if BINARY
transform(*fit, id, w, &nid, levelsup);
if(w > 0) // not stopped
{
v.addIfNotExist(id, w);
fv.addFeature(nid, i_feature);
}
}
} // if m_weighting == ...
if(must) v.normalize(norm);
}
4.4 词袋特征匹配源码
优点1：因为只需要在同一个节点下搜索候选匹配点，不需要地图点投影，所以匹配效率很高
优点2：不需要位姿即可匹配，比地图点投影匹配方法的应用场景更广泛，比如可以用于跟踪丢失重定位、闭环检测等场景
缺点：比较依赖字典，能够成功匹配到的特征对较少，适用于粗糙的特征匹配来估计初始位姿
4.4.1 普通帧计算两个向量
// 计算当前帧特征点对应的词袋Bow，主要是mBowVec 和 mFeatVec
void Frame::ComputeBoW()
{
// 判断是否以前已经计算过了，计算过了就跳过
if(mBowVec.empty())
{
// 实际上就是把描述子矩阵转换了vector，每一个元素都是一个特征点对应的描述子
vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
// 将特征点的描述子转换成词袋向量mBowVec以及特征向量mFeatVec
mpORBvocabulary->transform(vCurrentDesc, //当前的描述子vector
mBowVec, // 输出，词袋向量，记录的是单词的id及其对应权重TF-IDF值
mFeatVec, // 输出，记录node id及其对应的图像 feature对应的索引
4); // 4表示从叶节点向前数的层数 levelsup
}
}
4.4.2 关键帧计算两个向量
// Bag of Words Representation 计算词袋表示
void KeyFrame::ComputeBoW()
{
// 只有当词袋向量或者节点和特征序号的特征向量为空的时候执行
if(mBowVec.empty() || mFeatVec.empty())
{
// 那么就从当前帧的描述子中转换得到词袋信息
vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4); // levels up = 4
}
}
4.4.3 SearchByBoW
① 用于tracking中重定位、参考关键帧跟踪
/*
* @brief 通过词袋，对关键帧的特征点进行跟踪匹配，然后把关键帧pKF的地图点与帧中相对应，记录在vpMapPointMatches
* @param pKF 关键帧(指针)
* @param F 当前普通帧(引用)
* @param vpMapPointMatches F中特征点对应的地图点，NULL表示未匹配
* @return 成功匹配的数量
*/
int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)
{
const vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches(); // 取出关键帧的地图点
vpMapPointMatches = vector<MapPoint*>(F.N,static_cast<MapPoint*>(NULL)); // 初始化普通帧的地图点指针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);
// const float factor = 1.0f/HISTO_LENGTH; 原代码错误
const float factor = HISTO_LENGTH/360.0f;
// We perform the matching over ORB that belong to the same vocabulary node (at a certain level)
DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin(); // 关键帧特征向量map容器迭代器
DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin(); // 普通帧特征向量map容器迭代器
DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();
while(KFit != KFend && Fit != Fend)
{
// Step 1：分别取出属于同一node的ORB特征点(只有属于同一node，才有可能是匹配点)
// first 元素就是node id，遍历
if(KFit->first == Fit->first)
{
const vector<unsigned int> vIndicesKF = KFit->second; // 同一node节点特征点索引vector
const vector<unsigned int> vIndicesF = Fit->second;
// Step 2：遍历KF中属于该node的特征点
for(size_t iKF=0; iKF<vIndicesKF.size(); iKF++)
{
const unsigned int realIdxKF = vIndicesKF[iKF];
MapPoint* pMP = vpMapPointsKF[realIdxKF]; // PKF第realIdxKF个特征点对应的地图点
if(!pMP) // 如果这个特征点没有对应的地图点，就没必要去匹配，因为后续是为了3d-2d投影优化
continue;
if(pMP->isBad())
continue;
const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF); // 取出KF中该特征对应的描述子
int bestDist1=256; // 最佳距离
int bestIdxF =-1 ; // 最佳距离对应F中特征点的索引
int bestDist2=256; // 次佳距离
// Step 3：遍历F中属于该node的特征点，寻找最佳匹配点
for(size_t iF=0; iF<vIndicesF.size(); iF++)
{
const unsigned int realIdxF = vIndicesF[iF];
if(vpMapPointMatches[realIdxF]) // 帧F的特征点以对应上地图点
continue;
const cv::Mat &dF = F.mDescriptors.row(realIdxF);
const int dist = DescriptorDistance(dKF,dF); // 计算描述子距离
if(dist<bestDist1)
{
bestDist2=bestDist1; // 最佳距离更新
bestDist1=dist;
bestIdxF=realIdxF;
}
else if(dist<bestDist2)
{
bestDist2=dist;
}
}
// Step 4：根据阈值 和 角度投票剔除误匹配
// Step 4.1：第一关筛选：匹配距离必须小于设定阈值
if(bestDist1<=TH_LOW)
{
// Step 4.2：第二关筛选：最佳匹配比次佳匹配明显要好，那么最佳匹配才真正靠谱 mfNNratio=0.6
if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
{
// Step 4.3：记录成功匹配特征点的对应的地图点(来自关键帧)
vpMapPointMatches[bestIdxF]=pMP;
// 这里的realIdxKF是当前遍历到的关键帧的特征点id
const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];
// Step 4.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++;
}
// 这里才是词袋匹配的关键帧————确保对应上NodeId
else if(KFit->first < Fit->first)
{
KFit = vFeatVecKF.lower_bound(Fit->first);
}
else
{
Fit = F.mFeatVec.lower_bound(KFit->first);
}
}
// Step 5 根据方向剔除误匹配的点
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;
}
② 用于闭环LoopClosing::ComputeSim3
/* 函数重载
* @brief 通过词袋，对关键帧的特征点进行跟踪，该函数用于闭环检测时两个关键帧间的特征点匹配 基本上和上面的函数一致
* @param pKF1 KeyFrame1
* @param pKF2 KeyFrame2
* @param vpMatches12 pKF2中与pKF1匹配的MapPoint，vpMatches12[i]表示匹配的地图点，null表示没有匹配，i表示匹配的pKF1 特征点索引
* @return 成功匹配的数量
*/
int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12)
扫一扫
9816
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
