orb-slam系列 LocalMapping线程 开始(八)
程序员文章站
2022-03-16 23:46:25
...
总体架构
void LocalMapping::Run()
{
mbFinished = false;
// Loopclosing中的关键帧是LocalMapping发送过来的，LocalMapping是Tracking中发过来的
// 在LocalMapping中通过InsertKeyFrame将关键帧插入闭环检测队列mlpLoopKeyFrameQueue
// 闭环检测队列mlpLoopKeyFrameQueue中的关键帧不为空
while(1)
{
// Tracking will see that Local Mapping is busy 设置此时不能接受新的关键帧
SetAcceptKeyFrames(false);
// Check if there are keyframes in the queue 检测在队列中是否存在新的关键帧,如果存在进行局部地图构建
if(CheckNewKeyFrames())
{
// BoW conversion and insertion in Map
ProcessNewKeyFrame();
// Check recent MapPoints 检测上一关键帧进行时新添加的局部地图点
MapPointCulling();
// Triangulate new MapPoints 添加新的地图点
CreateNewMapPoints();
if(!CheckNewKeyFrames()) // 如果当前关键帧是当前关键帧集中的最后一个关键帧
{
// Find more matches in neighbor keyframes and fuse point duplications
SearchInNeighbors();
}
mbAbortBA = false;
if(!CheckNewKeyFrames() && !stopRequested()) // 如果当前关键帧为当前关键帧集中的最后一个关键帧,则进行局部BA优化 并检测是否存在冗余关键帧
{
// Local BA 在地图中存在关键帧数量大于2
if(mpMap->KeyFramesInMap()>2)
Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
// Check redundant local Keyframes 检测冗余关键帧
KeyFrameCulling();
}
// 将关键帧加入回环检测线程
mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
}
else if(Stop())
{
// Safe area to stop
while(isStopped() && !CheckFinish())
{
usleep(3000);//3ms
}
if(CheckFinish())
break;
}
ResetIfRequested();
// Tracking will see that Local Mapping is busy 设置此时可以进行新关键帧的插入
SetAcceptKeyFrames(true);
if(CheckFinish())
break;
usleep(3000);//3ms
}
SetFinish();
}
ProcessNewKeyFrame();
//如果有新的关键帧,对于新关键帧的操作
/***********************************************
1 从新关键帧队列mlNewKeyFrames中取一新关键帧并将其从新关键帧列表中删除(避免重复操作新关键帧)
2 计算新关键帧的BOW向量和Feature向量
3 将该关键帧对应的地图点与该关键帧关联,并更新该地图点的平均观测方向和描述子
4 更新Covisibility图
5 将关键帧插入全局地图中
***********************************************/
void LocalMapping::ProcessNewKeyFrame()
{
{
//取新关键帧列表中的一帧 并讲该帧从关键帧列表中删除
unique_lock<mutex> lock(mMutexNewKFs);
mpCurrentKeyFrame = mlNewKeyFrames.front();
mlNewKeyFrames.pop_front();
}
// Compute Bags of Words structures 计算该关键帧的BOW向量和Feature向量
mpCurrentKeyFrame->ComputeBoW();
// Associate MapPoints to the new keyframe and update normal and descriptor
// 得到与该关键帧相关的地图点 将地图点和新关键帧关联,并更新normal(平均观测方向)和描述子
const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
for(size_t i=0; i<vpMapPointMatches.size(); i++)
{
MapPoint* pMP = vpMapPointMatches[i]; //遍历每个地图点
if(pMP)
{
if(!pMP->isBad())//地图点是好的
{
//如果该地图点不在当前关键帧中,那么关联该关键帧 并更新改关键点的平均观测方向 计算最优描述子(单目)
if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))
{
pMP->AddObservation(mpCurrentKeyFrame, i);
pMP->UpdateNormalAndDepth();
pMP->ComputeDistinctiveDescriptors();
}
//将该地图点添加到新添加地图点的容器中 mlpRecentAddedMapPoints
else // this can only happen for new stereo points inserted by the Tracking (非单目)
{
mlpRecentAddedMapPoints.push_back(pMP);
}
}
}
}
// Update links in the Covisibility Graph 更新 Covisibility 图 的边
mpCurrentKeyFrame->UpdateConnections();
// Insert Keyframe in Map 将该关键帧插入全局地图中
mpMap->AddKeyFrame(mpCurrentKeyFrame);
}
mlNewKeyFrames 来自于 insertkeyframe() 函数
观测到的地图点 加入mlpRecentAddedMapPoints
UpdateConnections() 函数
/**********************************************************************************************************************
* 函数属性：KeyFrame类成员函数UpdateConnections()
* 函数功能：
* 1. 首先获得该关键帧的所有MapPoint点，统计观测到这些3d点的每个关键点与其它所有关键帧之间的共视程度(共视程度是指两个帧之间存在共同的地图点数量)
* 对每一个找到的关键帧(共视程度大于15)，建立一条边，边的权重是该关键帧与当前关键帧公共3d点的个数。
* 2. 并且该权重必须大于一个阈值，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）
* 3. 对这些连接按照权重从大到小进行排序，以方便将来的处理
* 更新完covisibility图之后，如果没有初始化过，则初始化为连接权重最大的边（与其它关键帧共视程度最高的那个关键帧），类似于最大生成树
* 4. 更新关联关键帧及权重
* 5. 更新父关键帧为关联关键帧权重最大帧
* 函数参数：NULL
* 备注：NULL
**********************************************************************************************************************/
void KeyFrame::UpdateConnections()
{
map<KeyFrame*,int> KFcounter; //关键帧-权重，权重为其它关键帧与当前关键帧共视3d点的个数
vector<MapPoint*> vpMP;//存储该关键帧的地图点
{
unique_lock<mutex> lockMPs(mMutexFeatures);
vpMP = mvpMapPoints;
}
//For all map points in keyframe check in which other keyframes are they seen
//Increase counter for those keyframes
// 通过3D点间接统计可以观测到这些3D点的所有关键帧之间的共视程度
// 即统计每一个关键帧都有多少关键帧与它存在共视关系，统计结果放在KFcounter
for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(!pMP)
continue;
if(pMP->isBad())
continue;
map<KeyFrame*,size_t> observations = pMP->GetObservations();//得到可以看到pMP地图点的所有关键帧以及地图点在这些关键帧中的索引
for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
{
if(mit->first->mnId==mnId) //如果查找到的关键帧就是本关键帧那么将跳过本次查找
continue;
KFcounter[mit->first]++;//否则被查找到的关键帧的权重+1
}
}
// This should not happen
if(KFcounter.empty())
return;
//If the counter is greater than threshold add connection
//In case no keyframe counter is over threshold add the one with maximum counter
int nmax=0; //存储权重的最大值
KeyFrame* pKFmax=NULL;//权重最大值所对应的关键帧
int th = 15;
vector<pair<int,KeyFrame*> > vPairs;//vPairs记录与其它关键帧共视帧数大于th的关键帧 权重-关键帧
vPairs.reserve(KFcounter.size());
for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
{
if(mit->second>nmax)
{
nmax=mit->second;
pKFmax=mit->first;
}
if(mit->second>=th) //如果权重大于阈值
{
vPairs.push_back(make_pair(mit->second,mit->first)); //将权重大于th的关键帧存入vPairs中
(mit->first)->AddConnection(this,mit->second); //并增加本关键帧与该查找到的关键帧建立联系
}
}
if(vPairs.empty()) //如果没有关键帧的权重大于阈值，则将权重最大的关键帧与本关键帧建立联系
{
vPairs.push_back(make_pair(nmax,pKFmax));
pKFmax->AddConnection(this,nmax);
}
sort(vPairs.begin(),vPairs.end());
list<KeyFrame*> lKFs; //排序后的关联关键帧
list<int> lWs; //排序后的权重
for(size_t i=0; i<vPairs.size();i++)
{
lKFs.push_front(vPairs[i].second);
lWs.push_front(vPairs[i].first);
}
{
unique_lock<mutex> lockCon(mMutexConnections);
// mspConnectedKeyFrames = spConnectedKeyFrames;
mConnectedKeyFrameWeights = KFcounter; //将关联关键帧及其权重存储
mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end()); //存储排序后的关联关键帧
mvOrderedWeights = vector<int>(lWs.begin(), lWs.end()); //存储排序后的权重
if(mbFirstConnection && mnId!=0)
{
mpParent = mvpOrderedConnectedKeyFrames.front(); //将权重最大的关键帧存储到变量中
mpParent->AddChild(this);
mbFirstConnection = false; //证明不是第一次建立链接
}
}
}
1）提取该帧所有地图点，每个地图点的共视帧看到一次，关键帧 观测数+1
2）找到观测到大于阈值地图点数目的关键帧，存入vPairs，当前帧与大于阈值的关键帧，建立联系。
3）排序，最大权重为父帧。
MapPointCulling
// 检测新添加的局部地图点是否满足条件 筛选出好的地图点
// 筛选条件:地图点是否是好的,地图点的查找率大于0.25, 该地图点第一关键帧(第一次观察到改地图点的帧id)与当前帧id相隔距离, 该关键点的被观察到的次数
void LocalMapping::MapPointCulling()
{
// Check Recent Added MapPoints 检测当前新添加的地图点
list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;
int nThObs; //所有观察到该地图点的关键帧数量的阈值
if(mbMonocular)
nThObs = 2;
else
nThObs = 3;
const int cnThObs = nThObs;
while(lit!=mlpRecentAddedMapPoints.end())
{
MapPoint* pMP = *lit;
if(pMP->isBad()) //如果该地图点是坏的,那么就擦除该地图点
{
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(pMP->GetFoundRatio()<0.25f ) //该地图点的查找率如果小于0.25 那么也将其删除,并将该地图点设置成坏的地图点
{
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
//如果当前帧与该地图点第一观察关键帧相隔大于等于2并且观察到该地图点的关键帧数量小于3 则认为该地图点是坏的,擦除该局部地图点
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
{
pMP->SetBadFlag();
lit = mlpRecentAddedMapPoints.erase(lit);
}
else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3) //如果当前关键帧与第一次观察到该地图点的关键帧相隔大于等于3帧 则擦除该地图点
lit = mlpRecentAddedMapPoints.erase(lit);
else //否则判定该地图点是好的
lit++;
}
}
1）mlpRecentAddedMapPoints中取出地图点
2）地图点的查找率如果小于0.25（没有价值）
当前帧与该地图点第一观察关键帧相隔大于等于2并且观察到该地图点的关键帧数量小于3（没有价值）
当前关键帧与第一次观察到该地图点的关键帧相隔大于等于3帧（过多冗余）
CreateNewMapPoints
//建立新的地图点
/*
* 步骤: 1. 在当前关键帧的共视关键帧中找到共视程度最高的nn帧相邻帧vpNeighKFs
* 2. 遍历相邻关键帧vpNeighKFs,将当前关键帧和共视关键帧进行三角测量和对极约束
* 3. 对每次匹配得到的地图点(追踪线程未追踪到的地图点)进行筛选,看是否满足三角测量和对极约束 并根据对极约束计算三维点坐标(单目),对于双目和rgbd直接可以得到3D坐标
* 判断该匹配点是否是好的地图点 1> 两帧中的深度都是正的 2> 地图点在两帧的重投影误差 3> 检测地图点的尺度连续性
* 4. 如果三角化是成功的,那么建立新的地图点,并设置地图点的相关属性(a.观测到该MapPoint的关键帧 b.该MapPoint的描述子 c.该MapPoint的平均观测方向和深度范围),
* 然后将地图点加入当前关键帧和全局地图中
* 备注: 注意对于单目相机的地图点三维坐标的确立需要根据三角测量来确定.
*/
void LocalMapping::CreateNewMapPoints()
{
// Retrieve neighbor keyframes in covisibility graph
int nn = 10;
if(mbMonocular)// 单目相机
nn=20;
const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);//取与该关键帧关联性最强的前nn个关键帧
ORBmatcher matcher(0.6,false);
cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation(); //当前关键帧的旋转矩阵
cv::Mat Rwc1 = Rcw1.t();
cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation(); //当前关键帧的平移矩阵
cv::Mat Tcw1(3,4,CV_32F);
Rcw1.copyTo(Tcw1.colRange(0,3));
tcw1.copyTo(Tcw1.col(3));
cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter(); //当前关键帧的相机中心点
//当前关键帧的相机参数
const float &fx1 = mpCurrentKeyFrame->fx;
const float &fy1 = mpCurrentKeyFrame->fy;
const float &cx1 = mpCurrentKeyFrame->cx;
const float &cy1 = mpCurrentKeyFrame->cy;
const float &invfx1 = mpCurrentKeyFrame->invfx;
const float &invfy1 = mpCurrentKeyFrame->invfy;
//高斯金字塔的缩放比例*1.5
const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;
int nnew=0; //新添加的地图点的数量
// Search matches with epipolar restriction and triangulate 对极约束 和 三角测量
for(size_t i=0; i<vpNeighKFs.size(); i++)
{
if(i>0 && CheckNewKeyFrames())
return;
KeyFrame* pKF2 = vpNeighKFs[i];
// Check first that baseline is not too short 检测基线是否太短
cv::Mat Ow2 = pKF2->GetCameraCenter();
cv::Mat vBaseline = Ow2-Ow1;
const float baseline = cv::norm(vBaseline);
if(!mbMonocular) //如果不是单目,检测基线的长度,如果基线满足要求则不需要进行三角测量计算地图点深度
{
if(baseline<pKF2->mb)
continue;
}
else //如果是单目 则检测基线深度比 因为单目的深度是不确定的
{
const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2); //计算当前关键帧的场景深度
const float ratioBaselineDepth = baseline/medianDepthKF2; //基线/场景深度 称为基线深度比
if(ratioBaselineDepth<0.01) //如果基线深度比小于0.01 则搜索下一个关联最强关键帧
continue;
}
// Compute Fundamental Matrix 计算当前帧和关联关键帧之间的基础矩阵Fundamental matrix 对极约束
cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);
// Search matches that fullfil epipolar constraint 两帧之间进行特征点匹配
// 存储在追踪线程下没有被建立的地图点(没有从三种追踪方式追踪到的地图点)
vector<pair<size_t,size_t> > vMatchedIndices;
// 匹配两关键帧中未追踪到的特征点
matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);
//对重新获得的匹配点 进行地图点计算
cv::Mat Rcw2 = pKF2->GetRotation();
cv::Mat Rwc2 = Rcw2.t();
cv::Mat tcw2 = pKF2->GetTranslation();
cv::Mat Tcw2(3,4,CV_32F);
Rcw2.copyTo(Tcw2.colRange(0,3));
tcw2.copyTo(Tcw2.col(3));
const float &fx2 = pKF2->fx;
const float &fy2 = pKF2->fy;
const float &cx2 = pKF2->cx;
const float &cy2 = pKF2->cy;
const float &invfx2 = pKF2->invfx;
const float &invfy2 = pKF2->invfy;
// Triangulate each match 对每对匹配通过三角化生成3D点,和 Triangulate函数差不多
const int nmatches = vMatchedIndices.size();
for(int ikp=0; ikp<nmatches; ikp++)
{
const int &idx1 = vMatchedIndices[ikp].first;
const int &idx2 = vMatchedIndices[ikp].second;
const cv::KeyPoint &kp1 = mpCurrentKeyFrame->mvKeysUn[idx1];
const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];
bool bStereo1 = kp1_ur>=0;// 单目的右眼坐标为-1
const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
const float kp2_ur = pKF2->mvuRight[idx2];
bool bStereo2 = kp2_ur>=0;
// Check parallax between rays 两匹配帧中特征点的归一化坐标系坐标
cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0);
cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0);
// 两特征点在世界坐标系下的坐标分别为 Rwc1 * xn1 + twc1 ,Rwc2 * xn2 + twc2 twc1和twc2分别为两相机中心
// 两特征点的夹角cos值为: (Rwc1 * xn1 + twc1 - twc1)(Rwc2 * xn2 + twc2 - twc2)/(norm(Rwc1 * xn1 + twc1 - twc1)norm(Rwc2 * xn2 + twc2 - twc2))
// 计算两特征点的方向夹角
cv::Mat ray1 = Rwc1*xn1;
cv::Mat ray2 = Rwc2*xn2;
const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));
float cosParallaxStereo = cosParallaxRays+1;
float cosParallaxStereo1 = cosParallaxStereo;
float cosParallaxStereo2 = cosParallaxStereo;
if(bStereo1)
cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1]));
else if(bStereo2)
cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2]));
cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);
// 匹配点三维坐标
cv::Mat x3D;
// 计算匹配点三维坐标,,即确定深度,如果为单目则用三角测量方式,对应第一个if ,如果两帧中一帧为双目或rgbd帧,则直接可以得到三维点坐标
if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998)) // 两帧都是单目帧 并且两相机下两特征点夹角不为平行
{
// Linear Triangulation Method 三角化线性方法
// Trianularization: 已知匹配特征点对{x x'} 和 各自相机矩阵{P P'}, 估计三维点 X
// x' = P'X x = PX
// 它们都属于 x = aPX模型
// |X|
// |x| |p1 p2 p3 p4 | |Y| |x| |--p0--||.|
// |y| = a |p5 p6 p7 p8 | |Z| ===> |y| = a|--p1--||X|
// |z| |p9 p10 p11 p12| |1| |z| |--p2--||.|
// 采用DLT的方法：x叉乘PX = 0
// |yp2 - p1| |0|
// |p0 - xp2| X = |0|
// |xp1 - yp0| |0|
// 两个点:
// |yp2 - p1 | |0|
// |p0 - xp2 | X = |0| ===> AX = 0
// |y'p2' - p1' | |0|
// |p0' - x'p2'| |0|
// 变成程序中的形式：
// |xp2 - p0 | |0|
// |yp2 - p1 | X = |0| ===> AX = 0
// |x'p2'- p0'| |0|
// |y'p2'- p1'| |0|
// xn1和xn2已经是相机归一化平面的坐标,所以这里的P就相当与T
cv::Mat A(4,4,CV_32F);
A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);
cv::Mat w,u,vt;
cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
x3D = vt.row(3).t();
if(x3D.at<float>(3)==0)
continue;
// Euclidean coordinates
x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
}
else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2) // 第一帧为双目或rgbd帧
{
x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);
}
else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1) // 第二帧为双目或rgbd帧
{
x3D = pKF2->UnprojectStereo(idx2);
}
else
continue; //No stereo and very low parallax
cv::Mat x3Dt = x3D.t();
//Check triangulation in front of cameras 检验该地图点在第一关键帧中的深度是否大于0
float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at<float>(2);
if(z1<=0)
continue;
//检验该地图点在第二关键帧中的深度是否大于0
float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);
if(z2<=0)
continue;
//Check reprojection error in first keyframe 检测在第一帧中该第地图点的重投影误差
const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];
const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);
const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);
const float invz1 = 1.0/z1;
if(!bStereo1) // 第一帧为单目帧 https://baike.baidu.com/item/%E5%8D%A1%E6%96%B9%E5%88%86%E5%B8%83
{
float u1 = fx1*x1*invz1+cx1;
float v1 = fy1*y1*invz1+cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
// 卡方分布定义:有n个独立变量符合标准正态分布 那么这n个*变量的平方和就符合卡方分布
if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1) // 概率95% *度为2 的情况下 要求误差小于5.991 参考卡方分布的相关知识
continue;
}
else // 第一帧为双目帧
{
float u1 = fx1*x1*invz1+cx1;
float u1_r = u1 - mpCurrentKeyFrame->mbf*invz1;
float v1 = fy1*y1*invz1+cy1;
float errX1 = u1 - kp1.pt.x;
float errY1 = v1 - kp1.pt.y;
float errX1_r = u1_r - kp1_ur;
if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)// 概率95% *度为3 的情况下 要求误差小于7.8 参考卡方分布的相关知识
continue;
}
//Check reprojection error in second keyframe 检测在第二帧中该第地图点的重投影误差
const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];
const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at<float>(0);
const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at<float>(1);
const float invz2 = 1.0/z2;
if(!bStereo2)
{
float u2 = fx2*x2*invz2+cx2;
float v2 = fy2*y2*invz2+cy2;
float errX2 = u2 - kp2.pt.x;
float errY2 = v2 - kp2.pt.y;
if((errX2*errX2+errY2*errY2)>5.991*sigmaSquare2)
continue;
}
else
{
float u2 = fx2*x2*invz2+cx2;
float u2_r = u2 - mpCurrentKeyFrame->mbf*invz2;
float v2 = fy2*y2*invz2+cy2;
float errX2 = u2 - kp2.pt.x;
float errY2 = v2 - kp2.pt.y;
float errX2_r = u2_r - kp2_ur;
if((errX2*errX2+errY2*errY2+errX2_r*errX2_r)>7.8*sigmaSquare2)
continue;
}
//Check scale consistency 检查尺度的连续性
//检测该地图点离相机光心的距离
cv::Mat normal1 = x3D-Ow1;
float dist1 = cv::norm(normal1);
cv::Mat normal2 = x3D-Ow2;
float dist2 = cv::norm(normal2);
if(dist1==0 || dist2==0)
continue;
// ratioDist是不考虑金字塔尺度下的距离比例
const float ratioDist = dist2/dist1;
// 金字塔尺度因子的比例
const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave];
/*if(fabs(ratioDist-ratioOctave)>ratioFactor)
continue;*/
// ratioDist*ratioFactor < ratioOctave 或 ratioDist/ratioOctave > ratioFactor表明尺度变化是不连续的
//ratioDist ratioOctave 应该是相等的 乘1.5的误差 还不在范围内剔除
if(ratioDist*ratioFactor<ratioOctave || ratioDist>ratioOctave*ratioFactor)
continue;
// Triangulation is succesfull 如果三角化是成功的,那么建立新的地图点,并设置地图点的相关属性,然后将地图点加入当前关键帧和全局地图中
// 地图点属性:
// a.观测到该MapPoint的关键帧
// b.该MapPoint的描述子
// c.该MapPoint的平均观测方向和深度范围
MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
//地图点的两个索引
pMP->AddObservation(mpCurrentKeyFrame,idx1);
pMP->AddObservation(pKF2,idx2);
mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
pKF2->AddMapPoint(pMP,idx2);
pMP->ComputeDistinctiveDescriptors();
pMP->UpdateNormalAndDepth();
mpMap->AddMapPoint(pMP);
// 步骤6.8：将新产生的点放入检测队列
// 这些MapPoints都会经过MapPointCulling函数的检验
mlpRecentAddedMapPoints.push_back(pMP);
nnew++;
}
}
}
1）找到与当前帧共视关系最好的前nn个共视帧vpNeighKFs
2）ComputeF12 基础矩阵
SearchForTriangulation
// 找出 pKF1中 特征点在pKF2中的匹配点
// 根据BOW向量匹配在同一节点下的特征点 根据匹配点描述子距离最小的点并且满足对极几何的约束
int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo)
{
//取图片1的特征向量
const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
//取图片2的特征向量
const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;
//Compute epipole in second image 根据相机1的相机中心,以及相机2的旋转矩阵和平移矩阵计算相机二的相机中心
cv::Mat Cw = pKF1->GetCameraCenter();
cv::Mat R2w = pKF2->GetRotation();
cv::Mat t2w = pKF2->GetTranslation();
cv::Mat C2 = R2w*Cw+t2w;
//步骤0：得到KF1的相机光心在KF2中的坐标（极点坐标）
const float invz = 1.0f/C2.at<float>(2);
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 通过orb词典进行快速匹配
// Compare only ORB that share the same node 仅仅比较在同一节点上的orb特征点
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;
//图像一中的特征点迭代器
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();
// 将左图像的每个特征点与右图像同一node节点的所有特征点
// 依次检测，判断是否满足对极几何约束，满足约束就是匹配的特征点
while(f1it!=f1end && f2it!=f2end) //循环每幅图像的每一个特征点
{
if(f1it->first == f2it->first)//如果两个特征点在同一个BOW词典节点上 则寻找匹配
{
for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)//循环图片1 中所有特征点的特征索引
{
//特征索引
const size_t idx1 = f1it->second[i1];
//得到该特征点在关键帧中对应的地图点
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;
// 该关键帧1中该特征向量对应的关键点
const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];
//该关键帧1中对应于该特征向量的描述子向量
const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);
int bestDist = TH_LOW;
int bestIdx2 = -1;
//循环图片2中所有特征点的特征索引 找到最小描述子距离的情况下满足对极几何约束条件的匹配点
for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++)
{
size_t idx2 = f2it->second[i2];
MapPoint* pMP2 = pKF2->GetMapPoint(idx2);
// If we have already matched or there is a MapPoint skip
if(vbMatched2[idx2] || pMP2)
continue;
const bool bStereo2 = pKF2->mvuRight[idx2]>=0; //该特征点对应的右眼坐标
if(bOnlyStereo) //如果是双目 右眼坐标不能为空
if(!bStereo2)
continue;
//关键帧2中特征点2的描述子
const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);
// 两个关键帧中两描述子的距离
const int dist = DescriptorDistance(d1,d2);
if(dist>TH_LOW || dist>bestDist) //如果两特征点之间的描述子距离大于阈值或者大于当前最优距离 ,则跳过该特征点
continue;
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;
}
// 检测匹配点是否满足对极几何的约束
if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
{
bestIdx2 = idx2;
bestDist = dist;
}
}
//图2 遍历结束
if(bestIdx2>=0)
{
//最佳匹配
const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
vMatches12[idx1]=bestIdx2;
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 && bin<HISTO_LENGTH);
//在某个角度范围内加上 匹配点的索引
rotHist[bin].push_back(idx1);
}
}
}
f1it++; //迭代器指针后移
f2it++; //迭代器指针后移
}
else if(f1it->first < f2it->first) //如果图像1中的特征点节点id小于图像2中特征点节点id 则将图像1特征点的节点id跳转到图像2特征点节点id
{
f1it = vFeatVec1.lower_bound(f2it->first);
}
else //如果图像2中的特征点节点id小于图像1中特征点节点id 则将图像2特征点的节点id跳转到图像1特征点节点id
{
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<HISTO_LENGTH; i++)
{
if(i==ind1 || i==ind2 || i==ind3)
continue;
//匹配点 不在最高的三个角度范围内 删除匹配
for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
{
vMatches12[rotHist[i][j]]=-1;
nmatches--;
}
}
}
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;
}
1）计算出对极点
2）词节点迭代，只要同一个词节点，在两帧索引出的特征点描述子接近，且远离対极线。
3）满足对极约束CheckDistEpipolarLine(kp1,kp2,F12,pKF2)
4）找到帧1关键点的最佳匹配点
5）返回匹配数量。
3）用匹配成功的 匹配点反映出出 3d点
1> 两帧中的深度都是正的 2> 地图点在两帧的重投影误差 3> 检测地图点的尺度连续性
4）检验成功的新地图点 mlpRecentAddedMapPoints
推荐阅读