ORBSLAM2源码学习（6） ORBmatcher类
还是先上代码再放总结
#include "ORBmatcher.h"
#include
#include
#include
#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
#include
using namespace std;
namespace ORB_SLAM2
{
const int ORBmatcher::TH_HIGH = 100;
const int ORBmatcher::TH_LOW = 50;
const int ORBmatcher::HISTO_LENGTH = 30;
ORBmatcher::ORBmatcher(float nnratio, bool checkOri): mfNNratio(nnratio), mbCheckOrientation(checkOri)
{
}
// 通过投影，对Local MapPoint进行跟踪
// 将Local MapPoint投影到当前帧中, 由此增加当前帧的MapPoints
int ORBmatcher::SearchByProjection(Frame &F, const vector &vpMapPoints, const float th)
{
int nmatches=0;
const bool bFactor = th!=1.0;
for(size_t iMP=0; iMPmbTrackInView)
continue;
if(pMP->isBad())
continue;
// 通过距离预测的金字塔层数
const int &nPredictedLevel = pMP->mnTrackScaleLevel;
// The size of the window will depend on the viewing direction
// 搜索范围, 若当前视角和平均视角夹角接近时, r取一个较小的值
float r = RadiusByViewingCos(pMP->mTrackViewCos);
if(bFactor)
r*=th;
// 通过投影点投影到当前帧,以及搜索窗口和预测的尺度进行搜索, 找出附近的点
const vector 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
// 遍历在设置范围内找到的点
for(vector::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)
{
const float er = fabs(pMP->mTrackProjXR-F.mvuRight[idx]);
if(er>r*F.mvScaleFactors[nPredictedLevel])
continue;
}
const cv::Mat &d = F.mDescriptors.row(idx);
const int dist = DescriptorDistance(MPdescriptor,d);
// 根据描述子寻找描述子距离最小和次小的特征点
if(distmfNNratio*bestDist2)
continue;
F.mvpMapPoints[bestIdx]=pMP; // 为Frame中的点增加对应的MapPoint
nmatches++;
}
}
return nmatches;
}
// 视角和平均观测视角越小，搜索范围越小
float ORBmatcher::RadiusByViewingCos(const float &viewCos)
{
if(viewCos>0.998)
return 2.5;
else
return 4.0;
}
// 判断点到极线的距离是否合适
// 计算kp2特征点到极线的距离：
// 极线l：ax + by + c = 0
// (u,v)到l的距离为： |au+bv+c| / sqrt(a^2+b^2)
bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const KeyFrame* pKF2)
{
// Epipolar line in second image l = x1'F12 = [a b c]
// 求出kp1在pKF2上对应的极线
const float a = kp1.pt.x*F12.at(0,0)+kp1.pt.y*F12.at(1,0)+F12.at(2,0);
const float b = kp1.pt.x*F12.at(0,1)+kp1.pt.y*F12.at(1,1)+F12.at(2,1);
const float c = kp1.pt.x*F12.at(0,2)+kp1.pt.y*F12.at(1,2)+F12.at(2,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;
return dsqr<3.84*pKF2->mvLevelSigma2[kp2.octave];
}
// 通过bow对关键帧(pKF)和普通帧(F)中的特征点进行快速匹配
// 对属于同一node的特征点通过描述子距离进行匹配
// 根据匹配，用pKF中特征点对应的MapPoint更新F中特征点对应的MapPoints
// 通过距离阈值、比例阈值和角度投票进行剔除误匹配
// 注意是对关键帧的特征点进行匹配
int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector &vpMapPointMatches)
{
const vector vpMapPointsKF = pKF->GetMapPointMatches();
vpMapPointMatches = vector(F.N,static_cast(NULL));
const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec;
int nmatches=0;
vector rotHist[HISTO_LENGTH];
for(int i=0;ifirst == Fit->first)
{
const vector vIndicesKF = KFit->second; // KF中的点
const vector vIndicesF = Fit->second; // F中的点
// 遍历KF中属于该node的特征点
for(size_t iKF=0; iKFisBad())
continue;
const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF); // 取出KF中该特征对应的描述子
int bestDist1=256; // 最好的距离
int bestIdxF =-1 ;
int bestDist2=256; // 倒数第二好距离
// 遍历F中属于该node的特征点，找到了最佳匹配点
for(size_t iF=0; iF(bestDist1)(bestDist2))
{
// 更新特征点的MapPoint
vpMapPointMatches[bestIdxF]=pMP;
const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];
if(mbCheckOrientation)
{
// 所有的特征点的角度变化应该是一致的，通过直方图统计得到最准确的角度变化值
float rot = kp.angle-F.mvKeys[bestIdxF].angle;// 该特征点的角度变化值
if(rot<0.0)
rot+=360.0f;
int bin = round(rot*factor);// 将旋转角度分配到组
if(bin==HISTO_LENGTH)
bin=0;
assert(bin>=0 && binfirst < Fit->first)
{
KFit = vFeatVecKF.lower_bound(Fit->first);
}
else
{
Fit = F.mFeatVec.lower_bound(KFit->first);
}
}
// 根据方向剔除误匹配的点
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
// 计算rotHist中最大的三个的index
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i(NULL);
nmatches--;
}
}
}
return nmatches;
}
// 根据Sim3变换，将每个vpPoints投影到pKF上，并根据尺度确定一个搜索区域，
// 根据该MapPoint的描述子与该区域内的特征点进行匹配
int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector &vpPoints, vector &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
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;
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 spAlreadyFound(vpMatched.begin(), vpMatched.end());
spAlreadyFound.erase(static_cast(NULL));
int nmatches=0;
// For each Candidate MapPoint Project and Match
// 遍历所有的MapPoints
for(int iMP=0, iendMP=vpPoints.size(); iMPisBad() || spAlreadyFound.count(pMP))
continue;
// Get 3D Coords.
cv::Mat p3Dw = pMP->GetWorldPos();
// Transform into Camera Coords.
cv::Mat p3Dc = Rcw*p3Dw+tcw;
// Depth must be positive
if(p3Dc.at(2)<0.0)
continue;
// Project into Image
// 将地图点投影到图像中（像素）
const float invz = 1/p3Dc.at(2);
const float x = p3Dc.at(0)*invz;
const float y = p3Dc.at(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(distmaxDistance)
continue;
// Viewing angle must be less than 60 deg
// 检查视角
cv::Mat Pn = pMP->GetNormal();
if(PO.dot(Pn)<0.5*dist)
continue;
// 预测尺度
int nPredictedLevel = pMP->PredictScale(dist,pKF);
// Search in a radius
// 根据尺度确定搜索半径
const float radius = th*pKF->mvScaleFactors[nPredictedLevel];
// 得到搜索范围内的特征点
const vector 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;
// 遍历搜索区域内所有特征点，与该MapPoint的描述子进行匹配
for(vector::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(kpLevelnPredictedLevel)
continue;
const cv::Mat &dKF = pKF->mDescriptors.row(idx);
// 计算描述子距离
const int dist = DescriptorDistance(dMP,dKF);
// 记录
if(dist &vbPrevMatched, vector &vnMatches12, int windowSize)
{
int nmatches=0;
vnMatches12 = vector(F1.mvKeysUn.size(),-1);
vector rotHist[HISTO_LENGTH];
for(int i=0;i vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);
vector vnMatches21(F2.mvKeysUn.size(),-1);
for(size_t i1=0, iend1=F1.mvKeysUn.size(); i10)
continue;
// 在另一帧中设置搜索范围并获取范围内的特征点
vector vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);
if(vIndices2.empty())
continue;
// 取出第一针中点的描述子
cv::Mat d1 = F1.mDescriptors.row(i1);
int bestDist = INT_MAX;
int bestDist2 = INT_MAX;
int bestIdx2 = -1;
// 遍历第二帧中候选特征点，提取描述子计算距离，记录最好的和次好的距离和索引
for(vector::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=0)
{
vnMatches12[vnMatches21[bestIdx2]]=-1;
nmatches--;
}
vnMatches12[i1]=bestIdx2;
vnMatches21[bestIdx2]=i1;
vMatchedDistance[bestIdx2]=bestDist;
nmatches++;
if(mbCheckOrientation)
{
float rot = F1.mvKeysUn[i1].angle-F2.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=0)
{
vnMatches12[idx1]=-1;
nmatches--;
}
}
}
}
//Update prev matched
for(size_t i1=0, iend1=vnMatches12.size(); i1=0)
vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;
return nmatches;
}
// 通过词包，对关键帧的特征点进行跟踪，该函数用于闭环检测时两个关键帧间的特征点匹配
// 是关键帧之间的匹配
int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches12)
{
// 分别获取特征点、特征点的词向量、对应的地图点、描述子
const vector &vKeysUn1 = pKF1->mvKeysUn;
const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
const vector vpMapPoints1 = pKF1->GetMapPointMatches();
const cv::Mat &Descriptors1 = pKF1->mDescriptors;
const vector &vKeysUn2 = pKF2->mvKeysUn;
const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;
const vector vpMapPoints2 = pKF2->GetMapPointMatches();
const cv::Mat &Descriptors2 = pKF2->mDescriptors;
vpMatches12 = vector(vpMapPoints1.size(),static_cast(NULL));
vector vbMatched2(vpMapPoints2.size(),false);
vector rotHist[HISTO_LENGTH];
for(int i=0;ifirst == f2it->first) // 分别取出属于同一node的ORB特征点
{
// 遍历KF1中属于该node的特征点
for(size_t i1=0, iend1=f1it->second.size(); i1second[i1];
MapPoint* pMP1 = vpMapPoints1[idx1];
if(!pMP1)
continue;
if(pMP1->isBad())
continue;
const cv::Mat &d1 = Descriptors1.row(idx1);
int bestDist1=256;
int bestIdx2 =-1 ;
int bestDist2=256;
// 遍历KF2中属于该node的特征点，找到最佳匹配点
for(size_t i2=0, iend2=f2it->second.size(); i2second[i2];
MapPoint* pMP2 = vpMapPoints2[idx2];
if(vbMatched2[idx2] || !pMP2)
continue;
if(pMP2->isBad())
continue;
const cv::Mat &d2 = Descriptors2.row(idx2);
int dist = DescriptorDistance(d1,d2);
if(dist(bestDist1)(bestDist2))
{
vpMatches12[idx1]=vpMapPoints2[bestIdx2];
vbMatched2[bestIdx2]=true;
if(mbCheckOrientation)
{
float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].angle;
if(rot<0.0)
rot+=360.0f;
int bin = round(rot*factor);
if(bin==HISTO_LENGTH)
bin=0;
assert(bin>=0 && binfirst < f2it->first)
{
f1it = vFeatVec1.lower_bound(f2it->first);
}
else
{
f2it = vFeatVec2.lower_bound(f1it->first);
}
}
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i(NULL);
nmatches--;
}
}
}
return nmatches;
}
// 利用基本矩阵F12，在两个关键帧之间未匹配的特征点中产生新的3d点
int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
vector > &vMatchedPairs, const bool bOnlyStereo)
{
const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;
// Compute epipole in second image
// 计算KF1的相机中心在KF2图像平面的坐标，即极点坐标
cv::Mat Cw = pKF1->GetCameraCenter();
cv::Mat R2w = pKF2->GetRotation();
cv::Mat t2w = pKF2->GetTranslation();
cv::Mat C2 = R2w*Cw+t2w;
const float invz = 1.0f/C2.at(2);
// 得到KF1的相机光心在KF2中的坐标
const float ex =pKF2->fx*C2.at(0)*invz+pKF2->cx;
const float ey =pKF2->fy*C2.at(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 vbMatched2(pKF2->N,false);
vector vMatches12(pKF1->N,-1);
vector rotHist[HISTO_LENGTH];
for(int i=0;ifirst == f2it->first)
{
// 遍历该node节点下的所有特征点
for(size_t i1=0, iend1=f1it->second.size(); i1second[i1];
// 通过特征点索引idx1在pKF1中取出对应的MapPoint
MapPoint* pMP1 = pKF1->GetMapPoint(idx1);
// If there is already a MapPoint skip
if(pMP1)
continue;
const bool bStereo1 = pKF1->mvuRight[idx1]>=0;
if(bOnlyStereo)
if(!bStereo1)
continue;
// 通过特征点索引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;
for(size_t i2=0, iend2=f2it->second.size(); i2second[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);
// 计算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;
if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave])
continue;
}
// 计算特征点kp2到kp1极线的距离是否小于阈值
if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
{
bestIdx2 = idx2;
bestDist = dist;
}
}
if(bestIdx2>=0)
{
const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
vMatches12[idx1]=bestIdx2;
vbMatched2[bestIdx2]=true;
nmatches++;
if(mbCheckOrientation)
{
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 && binfirst < f2it->first)
{
f1it = vFeatVec1.lower_bound(f2it->first);
}
else
{
f2it = vFeatVec2.lower_bound(f1it->first);
}
}
if(mbCheckOrientation)
{
int ind1=-1;
int ind2=-1;
int ind3=-1;
ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
for(int i=0; i &vpMapPoints, const float th)
{
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();
// 遍历所有的MapPoints
for(int i=0; iisBad() || pMP->IsInKeyFrame(pKF))
continue;
// 地图点转换至相机坐标系
cv::Mat p3Dw = pMP->GetWorldPos();
cv::Mat p3Dc = Rcw*p3Dw + tcw;
// Depth must be positive
// 检查深度
if(p3Dc.at(2)<0.0f)
continue;
const float invz = 1/p3Dc.at(2);
const float x = p3Dc.at(0)*invz;
const float y = p3Dc.at(1)*invz;
const float u = fx*x+cx;
const float v = fy*y+cy; // 得到MapPoint在图像上的投影坐标
// 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);
// Depth must be inside the scale pyramid of the image
// 尺度必须合适
if(dist3DmaxDistance )
continue;
// Viewing angle must be less than 60 deg
// 检查视角
cv::Mat Pn = pMP->GetNormal();
if(PO.dot(Pn)<0.5*dist3D)
continue;
// 预测尺度
int nPredictedLevel = pMP->PredictScale(dist3D,pKF);
// Search in a radius
// 根据MapPoint的尺度，从而确定搜索范围
const float radius = th*pKF->mvScaleFactors[nPredictedLevel];
// 获得范围内的点
const vector 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;
// 遍历这些点
for(vector::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(kpLevelnPredictedLevel)
continue;
// 计算MapPoint投影的坐标与这个区域特征点的距离，如果偏差很大，直接跳过特征点匹配
if(pKF->mvuRight[idx]>=0)
{
// Check reprojection error in stereo
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;
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;
if(e2*pKF->mvInvLevelSigma2[kpLevel]>5.99)
continue;
}
const cv::Mat &dKF = pKF->mDescriptors.row(idx);
const int dist = DescriptorDistance(dMP,dKF);
if(distGetMapPoint(bestIdx);
if(pMPinKF) // 如果这个点有对应的MapPoint
{
if(!pMPinKF->isBad()) // 如果这个MapPoint不是bad，选择被观测次数多的点
{
if(pMPinKF->Observations()>pMP->Observations())
pMP->Replace(pMPinKF);
else
pMPinKF->Replace(pMP);
}
}
else // 如果这个点没有对应的MapPoint则添加
{
pMP->AddObservation(pKF,bestIdx);
pKF->AddMapPoint(pMP,bestIdx);
}
nFused++;
}
}
return nFused;
}
// 投影MapPoints到KeyFrame中，并判断是否有重复的MapPoints
// Scw为世界坐标系到pKF相机坐标系的Sim3变换，用于将世界坐标系下的vpPoints变换到相机坐标系
int ORBmatcher::Fuse(KeyFrame *pKF, cv::Mat Scw, const vector &vpPoints, float th, vector &vpReplacePoint)
{
// 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
// 将Sim3转化为SE3并分解
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;
cv::Mat tcw = Scw.rowRange(0,3).col(3)/scw;
cv::Mat Ow = -Rcw.t()*tcw;
// Set of MapPoints already found in the KeyFrame
const set spAlreadyFound = pKF->GetMapPoints();
int nFused=0;
const int nPoints = vpPoints.size();
// For each candidate MapPoint project and match
// 遍历所有的MapPoints
for(int iMP=0; iMPisBad() || spAlreadyFound.count(pMP))
continue;
// Get 3D Coords.
cv::Mat p3Dw = pMP->GetWorldPos();
// Transform into Camera Coords.
cv::Mat p3Dc = Rcw*p3Dw+tcw;
// Depth must be positive
if(p3Dc.at(2)<0.0f)
continue;
// Project into Image
const float invz = 1.0/p3Dc.at(2);
const float x = p3Dc.at(0)*invz;
const float y = p3Dc.at(1)*invz;
const float u = fx*x+cx;
const float v = fy*y+cy;// 得到MapPoint在图像上的投影坐标
// Point must be inside the image
if(!pKF->IsInImage(u,v))
continue;
// Depth must be inside the scale pyramid of the image
const float maxDistance = pMP->GetMaxDistanceInvariance();
const float minDistance = pMP->GetMinDistanceInvariance();
cv::Mat PO = p3Dw-Ow;
const float dist3D = cv::norm(PO);
if(dist3DmaxDistance)
continue;
// Viewing angle must be less than 60 deg
cv::Mat Pn = pMP->GetNormal();
if(PO.dot(Pn)<0.5*dist3D)
continue;
// Compute predicted scale level
const int nPredictedLevel = pMP->PredictScale(dist3D,pKF);
// Search in a radius
// 计算搜索范围
const float radius = th*pKF->mvScaleFactors[nPredictedLevel];
// pKF在该区域内的特征点
const vector 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 = INT_MAX;
int bestIdx = -1;
for(vector::const_iterator vit=vIndices.begin(); vit!=vIndices.end(); vit++)
{
const size_t idx = *vit;
const int &kpLevel = pKF->mvKeysUn[idx].octave;
if(kpLevelnPredictedLevel)
continue;
const cv::Mat &dKF = pKF->mDescriptors.row(idx);
int dist = DescriptorDistance(dMP,dKF);
if(distGetMapPoint(bestIdx);
// 如果这个MapPoint已经存在，则替换，
// 先记录下来，之后调用Replace函数来替换
if(pMPinKF)
{
if(!pMPinKF->isBad())
vpReplacePoint[iMP] = pMPinKF;
}
else// 如果这个MapPoint不存在，直接添加
{
pMP->AddObservation(pKF,bestIdx);
pKF->AddMapPoint(pMP,bestIdx);
}
nFused++;
}
}
return nFused;
}
// 通过Sim3变换，确定pKF1的特征点在pKF2中的大致区域，同理，确定pKF2的特征点在pKF1中的大致区域
// 在该区域内通过描述子进行匹配捕获pKF1和pKF2之前漏匹配的特征点，更新vpMatches12
int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector &vpMatches12,
const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th)
{
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 vpMapPoints1 = pKF1->GetMapPointMatches();
const int N1 = vpMapPoints1.size();
const vector vpMapPoints2 = pKF2->GetMapPointMatches();
const int N2 = vpMapPoints2.size();
vector vbAlreadyMatched1(N1,false);
vector vbAlreadyMatched2(N2,false);
// 用vpMatches12更新vbAlreadyMatched1和vbAlreadyMatched2
for(int i=0; iGetIndexInKeyFrame(pKF2);
if(idx2>=0 && idx2 vnMatch1(N1,-1);
vector vnMatch2(N2,-1);
// Transform from KF1 to KF2 and search
// 通过Sim变换，确定pKF1的特征点在pKF2中的大致区域，
// 在该区域内通过描述子进行匹配pKF1和pKF2之前漏匹配的特征点，更新vpMatches12
for(int i1=0; i1isBad())
continue;
cv::Mat p3Dw = pMP->GetWorldPos();
cv::Mat p3Dc1 = R1w*p3Dw + t1w; // 把pKF1系下的MapPoint从world坐标系变换到camera1坐标系
cv::Mat p3Dc2 = sR21*p3Dc1 + t21; // 再通过Sim3将该MapPoint从camera1变换到camera2坐标系
// Depth must be positive
if(p3Dc2.at(2)<0.0)
continue;
// 投影到camera2图像平面
const float invz = 1.0/p3Dc2.at(2);
const float x = p3Dc2.at(0)*invz;
const float y = p3Dc2.at(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(dist3DmaxDistance )
continue;
// Compute predicted octave
// 预测该MapPoint对应的尺度
const int nPredictedLevel = pMP->PredictScale(dist3D,pKF2);
// Search in a radius
// 计算搜索半径
const float radius = th*pKF2->mvScaleFactors[nPredictedLevel];
// 取出该区域内的所有特征点
const vector 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;
// 遍历搜索区域内的所有特征点，与pMP进行描述子匹配
for(vector::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.octavenPredictedLevel)
continue;
const cv::Mat &dKF = pKF2->mDescriptors.row(idx);
const int dist = DescriptorDistance(dMP,dKF);
if(distisBad())
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(2)<0.0)
continue;
const float invz = 1.0/p3Dc1.at(2);
const float x = p3Dc1.at(0)*invz;
const float y = p3Dc1.at(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(dist3DmaxDistance)
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 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::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.octavenPredictedLevel)
continue;
const cv::Mat &dKF = pKF1->mDescriptors.row(idx);
const int dist = DescriptorDistance(dMP,dKF);
if(dist=0)
{
int idx1 = vnMatch2[idx2];
if(idx1==i1) // KF1中的某个点和KF2中的某个点之间互相都能匹配上
{
vpMatches12[i1] = vpMapPoints2[idx2];
nFound++;
}
}
}
return nFound;
}
// 通过投影，对上一帧的特征点进行跟踪
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)
{
int nmatches = 0;
// Rotation Histogram (to check rotation consistency)
vector rotHist[HISTO_LENGTH];
for(int i=0;i(2)>CurrentFrame.mb && !bMono;
const bool bBackward = -tlc.at(2)>CurrentFrame.mb && !bMono;
for(int i=0; iGetWorldPos();
cv::Mat x3Dc = Rcw*x3Dw+tcw;
// 投影至当前帧
const float xc = x3Dc.at(0);
const float yc = x3Dc.at(1);
const float invzc = 1.0/x3Dc.at(2);
if(invzc<0)
continue;
float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx;
float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;
if(uCurrentFrame.mnMaxX)
continue;
if(vCurrentFrame.mnMaxY)
continue;
int nLastOctave = LastFrame.mvKeys[i].octave;
// Search in a window. Size depends on scale
float radius = th*CurrentFrame.mvScaleFactors[nLastOctave]; // 尺度越大，搜索范围越大
vector vIndices2;
if(bForward)
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);
else if(bBackward)
vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, 0, nLastOctave);
else
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;
// 遍历满足条件的特征点
for(vector::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++)
{
// 如果该特征点已经有对应的MapPoint了,则退出该次循环
const size_t i2 = *vit;
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=0 && bin(NULL);
nmatches--;
}
}
}
}
return nmatches;
}
// 将KF中的特征点投影到当前帧中进行匹配，增加当前帧的地图点，即跟踪kF
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set &sAlreadyFound, const float th , const int ORBdist)
{
int nmatches = 0;
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 Ow = -Rcw.t()*tcw;
// Rotation Histogram (to check rotation consistency)
vector rotHist[HISTO_LENGTH];
for(int i=0;i vpMPs = pKF->GetMapPointMatches();
for(size_t i=0, iend=vpMPs.size(); iisBad() && !sAlreadyFound.count(pMP))
{
//Project
cv::Mat x3Dw = pMP->GetWorldPos();
cv::Mat x3Dc = Rcw*x3Dw+tcw;
const float xc = x3Dc.at(0);
const float yc = x3Dc.at(1);
const float invzc = 1.0/x3Dc.at(2);
const float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx;
const float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;
if(uCurrentFrame.mnMaxX)
continue;
if(vCurrentFrame.mnMaxY)
continue;
// Compute predicted scale level
cv::Mat PO = x3Dw-Ow;
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(dist3DmaxDistance)
continue;
int nPredictedLevel = pMP->PredictScale(dist3D,&CurrentFrame);
// Search in a window
const float radius = th*CurrentFrame.mvScaleFactors[nPredictedLevel];
const vector 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;
for(vector::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(distmvKeysUn[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, const int L, int &ind1, int &ind2, int &ind3)
{
int max1=0;
int max2=0;
int max3=0;
for(int i=0; imax1)
{
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;
}
}
int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b)
{
const int *pa = a.ptr();
const int *pb = b.ptr();
int dist=0;
for(int i=0; i<8; i++, pa++, pb++)
{
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;
}
} //namespace ORB_SLAM
代码有点长。。。
总结下里面几个重要的函数，其实很多步骤大体相同
- int ORBmatcher::SearchByProjection(Frame &F, const vector
- 首先判断点是否需要投影及是否是坏点
- 由距离预测金字塔层数
- 由视角计算搜索半径，当前视角越接近平均视角，则搜索半径越小
- 将地图点投影至当前帧，根据2、3步的值找到一个范围内的所有特征点
- 遍历范围内的点：计算点和地图点描述子的距离，距离最好距离和次好距离及对应索引
- 若最好距离满足阈值且和次好距离有明显区别，则为当前帧添加地图点int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector
- 只对属于同一节点的特征点进行匹配
- 对属于同一节点的关键帧和普通帧的特征点遍历计算描述子距离，记录最好距离和次好距离
- 若最好距离满足阈值且明显优于次好距离，则更新特征点对应的匹配点（地图点）
- 上一步得到一对匹配的点，再计算两个点的角度变化值，分配到容器中，即统计所有匹配后的点的角度变化值，不同角度值分配到不同组中
- 计算所有角度变化组中数量最大的三个组，若前面匹配好的点的角度变化在这三个组中则保留，否则剔除，即认为特征点的角度变化应该是一致的
- int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector
- 分解相似变换矩阵计算尺度和旋转、平移
- 对于每一个地图点，投影到像素坐标上，检查：是否是坏点、是否已匹配、深度是否为正、是否在图像内、距离是否合适、视角大小
- 距离预测尺度à确定搜索半径à得到一个范围内的特征点
- 遍历这些特征点，计算其和地图点的描述子距离，记录相关信息
- 最好距离满足阈值，则添加匹配
- int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector
&vnMatches12, int windowSize); 初始化时的搜索匹配，两个普通帧间的匹配
- 在第二帧中设置搜索范围，获取范围内的特征点
- 遍历范围内的点，计算描述子距离，记录相关信息
- 根据最好距离和次好距离以及阈值的关系，记录匹配，记录特征点角度变化
- 根据角度变化剔除不好的匹配
- int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector
&vpMatches12); 通过BoW对两个关键帧之间的特征点进行匹配
- 都是对四叉树中属于相同节点的特征点才考虑匹配，首先对KF1中某个节点下的特征点，遍历KF2中对应节点的所有特征点，计算描述子距离并记录信息
- 根据距离和阈值关系添加匹配，记录角度变化
- 根据角度变化剔除误匹配
- int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,vector
- 同样是对同一节点中的特征点进行遍历，跳过已经有对应地图点的点，因为这个函数的目的是为了三角化进行的搜索匹配
- 计算描述子距离，检查特征点距离极线的距离，满足要求的记录信息
- 进行角度变化的统计和检查
- int ORBmatcher::Fuse(KeyFrame *pKF, const vector
&vpMapPoints, const float th); 将地图点投影到关键帧，判断是否有重复的地图点
- 遍历地图点，将其转换至相机坐标系检查深度，投影并检查是否在图像内，检查尺度和视角范围是否合适
- 根据距离预测尺度从而确定搜索半径，获得范围内的特征点
- 遍历这些点，计算其和地图点投影后的坐标误差，误差太大的跳过，之后计算描述子距离，记录信息
- 对于最佳匹配，如果某个点已经有了地图点，则选择观测次数多的那个点，舍弃观测次数少的点，即两点融合；如果没有对应的地图点，则添加地图点，添加观测关系。
- int ORBmatcher::Fuse(KeyFrame *pKF, cv::Mat Scw, const vector
&vpPoints, float th, vector &vpReplacePoint); 同上
- 分解相似变换矩阵，得到相关量
- 对每一个地图点投影到相机系和图像系，检查深度、视角等信息
- 由距离预测尺度进而确定搜索半径，在指定区域内获取特征点
- 遍历这些特征点，计算其和地图点的描述子距离，计录最好距离和索引
- 如果索引对应的地图点不存在则新添加一个，存在的话则标记其将要被替换
- int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector
- 这个函数将两帧互相进行了匹配，根据相似变换确定将KF1中的特征点投影到KF2中，设定搜索范围，对范围内的特征点进行匹配；同样将KF2中的点投影到KF1中进行了匹配，这两个过程相同
- 最后有一个检查的条件，某一对匹配点在KF1至KF2的匹配及KF2至KF1的匹配都存在时才认为是有效的匹配
- int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono); 通过投影对上一帧对应的地图点进行跟踪
- 将上一帧的有效的点投影至当前帧，根据相机的前进或后退，在当前帧的不同层的金字塔图像中获取指定范围的特征点
- 对于范围内的没有地图点的特征点，计算其和上一帧对应点的描述子距离，记录信息，并统计角度变化
- 根据角度变化剔除误匹配
- int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set
- 对于关键帧中每一个不是坏点且没有被匹配的地图点：投影至当前帧检查距离等信息
- 在预测范围内获取当前帧的特征点，遍历这些点计算其和KF中点的描述子距离并记录信息，统计角度变化，最后根据角度剔除误匹配
上面的总结是每个函数内部大体做了什么，有些多余，因为很多函数匹配步骤大致相似，基本思路都是根据距离预测尺度，进而根据尺度确定一个合理的搜索范围，之后对这个范围内的点进行匹配。同一类的只需要仔细看一个函数即可，没必要都看一遍，重要的是搞清楚在什么情况下调用这些不同的函数。