功能
/*******************************************************************************
* 函数属性：类Tracking的成员函数Track() 跟踪线程有四个状态分别是 没有图片,未初始化(第一张图片),线程完好, 追踪线程失败
* 函数功能：检测当前系统状态是处于NO_IMAGES_YET，NOT_INITIALIZED，还是OK or LOST
* 1. 根据当前的跟踪状态来进行相机位姿的计算,处于NO_IMAGES_YET则将位姿状态转化为NOT_INITIALIZED
* 处在NOT_INITIALIZED则进行初始化程序,StereoInitialization() or MonocularInitialization()
* 处在LOST则进行重定位
* 处在OK状态,则通过TrackReferenceKeyFrame()和TrackWithMotionModel()进行相机位姿的计算
* TrackReferenceKeyFrame()是根据上一帧的位姿为初始位姿进行优化,匹配的是关键帧和当前帧得到匹配地图点
* TrackWithMotionModel()是根据上一帧的速度和上一帧的积来估算相机初始位姿,匹配的是上一帧和当前帧得到匹配地图点
* 2. 局部地图的跟踪,更新局部地图(局部地图点和局部关键帧)
* 3. 检测是否需要加入关键帧,将关键帧交给局部建图线程
* 4. 如果上述都顺利进行,则更新当前相机的最终位姿,存储完整的帧姿态信息以获取完整的相机轨迹
*
* 另: 仅线程追踪模式下只存在以前未开启仅线程追踪模式下的关键帧,不会添加新的关键帧,因此不会有新的地图点,仅仅追踪的是现有地图中的地图点
*
* 函数参数介绍：NULL
* 备注：追踪线程的主要成员函数，实现视觉里程计
* 仅线程追踪模式下只存在以前未开启仅线程追踪模式下的关键帧,不会添加新的关键帧,因此不会有新的地图点,仅仅追踪的是现有地图中的地图点
******************************************************************************/
1）双目初始化
双目初始化很简单
/*******************************************************************************
* 函数属性：类Tracking的成员函数StereoInitialization()
* 函数功能：（1）设定初始化位姿
* （2）将当前帧构造为初始关键帧
* （3）在地图中添加该初始关键帧
* （4）为每个特征点构造mappoint
* （5）在局部地图中添加该初始关键帧
* 函数参数介绍：NULL
* 备注：双目或者RGBDSLAM的第一帧处理函数，地图初始化
*
******************************************************************************/
void Tracking::StereoInitialization()
{
if(mCurrentFrame.N>500) //当前帧关键点的数量大于500，才将此帧作为初始帧并认为其为关键帧
{
// Set Frame pose to the origin 设置初始帧的位姿
mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
// Create KeyFrame 将当前帧（第一帧）作为初始关键帧（调用关键帧的构造函数）
KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
// Insert KeyFrame in the map 将关键帧插入地图中. KeyFrame中包含了地图、反过来地图中也包含了KeyFrame，相互包含
mpMap->AddKeyFrame(pKFini);
// Create MapPoints and asscoiate to KeyFrame 创建地图点并将其与关键帧建立联系
for(int i=0; i<mCurrentFrame.N;i++)
{
float z = mCurrentFrame.mvDepth[i]; //获取当前帧第i个关键点的深度值
if(z>0)
{
cv::Mat x3D = mCurrentFrame.UnprojectStereo(i); //将当前帧的第i个特征点反投影到3D世界坐标系下
MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap); //用该特征点构造新的地图点
pNewMP->AddObservation(pKFini,i); //地图点添加关键帧 说明该地图点属于哪一关键帧
pKFini->AddMapPoint(pNewMP,i); //关键帧添加地图点 表明在该关键帧下可以看到该地图点
pNewMP->ComputeDistinctiveDescriptors(); //从众多观测到该MapPoint的特征点中挑选区分读最高的描述子
pNewMP->UpdateNormalAndDepth(); //更新该MapPoint平均观测方向以及观测距离的范围
mpMap->AddMapPoint(pNewMP); //将新的地图点加入到地图中
mCurrentFrame.mvpMapPoints[i]=pNewMP; // 将地图点加入到当前针的mvpMapPoints中，为当前Frame的特征点与MapPoint之间建立联系
}
}
cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl;
//在局部地图中添加该初始关键帧
mpLocalMapper->InsertKeyFrame(pKFini); //将该帧插入局部地图
//更新上一帧为当前帧
mLastFrame = Frame(mCurrentFrame);
mnLastKeyFrameId=mCurrentFrame.mnId;
// 更新上一关键帧为当前关键帧
mpLastKeyFrame = pKFini;
mvpLocalKeyFrames.push_back(pKFini); //将初始关键帧加入到局部地图的关键帧
mvpLocalMapPoints=mpMap->GetAllMapPoints(); //将全部地图点加入到当前局部地图点
mpReferenceKF = pKFini; //将当前关键帧作为参考关键帧
mCurrentFrame.mpReferenceKF = pKFini; //将该关键帧作为当前帧的参考关键帧
mpMap->SetReferenceMapPoints(mvpLocalMapPoints); //将当前局部地图点作为整个地图参考地图点,用于画图
mpMap->mvpKeyFrameOrigins.push_back(pKFini); //将关键帧加入地图的原始的关键帧
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw); //将当前帧加入到地图观测器
mState=OK;
}
}
CheckReplacedInLastFrame
检查一下是不是有的地图点被替代了 完成替代
更新Fuse函数和SearchAndFuse函数替换的MapPoints
void Tracking::CheckReplacedInLastFrame()
{
for(int i =0; i<mLastFrame.N; i++) //遍历上一帧所有关键点
{
MapPoint* pMP = mLastFrame.mvpMapPoints[i]; //该帧中所有关键点对应的地图点
if(pMP)
{
MapPoint* pRep = pMP->GetReplaced(); //找到该地图点的可代替其他地图点
if(pRep) //检测上一帧中的地图点是否可以找到被代替的其他地图点
{
mLastFrame.mvpMapPoints[i] = pRep;
}
}
}
}
分类 运动模型是空的或刚完成重定位
TrackReferenceKeyFrame()
/*******************************************************************************
* 函数属性：类Tracking的成员函数TrackReferenceKeyFrame()
* 函数功能：
* 1. 计算当前帧的词包，将当前帧的特征点分到特定层的nodes上
* 2. 对属于同一node的描述子进行匹配
* 3. 根据匹配对估计当前帧的姿态
* 4. 根据位姿估计下的模型判断是否为内点来剔除误匹配
* 5. 返回当前优化位姿模型下内点数量是否大于10 大于10证明当前位姿估计模型是好的,否则是差的
* 函数参数介绍：NULL
* 备注：
* 根据上一帧的位姿作为初始位姿进行位姿优化得到当前帧的位姿
******************************************************************************/
bool Tracking::TrackReferenceKeyFrame()
{
// Compute Bag of Words vector
// 将当前帧描述子转化为BoW向量
mCurrentFrame.ComputeBoW();
// We perform first an ORB matching with the reference keyframe
// If enough matches are found we setup a PnP solver
ORBmatcher matcher(0.7,true);
vector<MapPoint*> vpMapPointMatches;
//根据BoW进行将参考帧和当前帧进行特征匹配 将匹配点存储到vpMapPointMatches中 返回的是匹配点的数量
int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
//匹配点数量小于15无法进行优化，证明不是一个有效的匹配
if(nmatches<15)
return false;
//设置初始位姿为上一帧的位姿，优化节点为vpMapPointMatches中的匹配点
mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw);
//进行位姿优化，从而获得准确的相机位姿
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers 剔除优化后的局外匹配点
int nmatchesMap = 0;
for(int i =0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
if(mCurrentFrame.mvbOutlier[i]) //如果是局外点 则剔除
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
mCurrentFrame.mvbOutlier[i]=false;
pMP->mbTrackInView = false;
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
nmatches--;
}
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) //否则说明是当前帧和参考帧的好匹配点
nmatchesMap++;//匹配点数量加1
}
}
return nmatchesMap>=10; //如果匹配点数量足够,则证明此次位姿估计是好的
}
其中
matcher.SearchByBoW
//根据BoW进行将参考帧和当前帧进行特征匹配 将匹配点存储到vpMapPointMatches中 返回的是匹配点的数量
int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
// 关键帧和当前帧通过词袋进行快速匹配 在同一词典节点上搜索匹配
// 筛选方法:
// 在投影区域内遍历所有的特征点,计算描述子距离第一小和描述子距离第二小 当第一小明显小于第二小时证明该特征点具有最优性
// 从而确定匹配
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));
//关键帧的特征向量
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;
// We perform the matching over ORB that belong to the same vocabulary node (at a certain level) 执行匹配在属于同一词典节点（字）
//关键帧特征向量的迭代器 first代表该特征向量的节点id second代表该特征点在图像所有特征点集合中的索引
DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin();
//当前帧特征向量的迭代器 first代表该特征向量的节点id second代表该特征点在图像所有特征点集合中的索引
DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin();
DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();
while(KFit != KFend && Fit != Fend)
{
if(KFit->first == Fit->first) //如果当前帧和关键帧的当前特征向量属于同一节点
{
const vector<unsigned int> vIndicesKF = KFit->second;
const vector<unsigned int> vIndicesF = Fit->second;
for(size_t iKF=0; iKF<vIndicesKF.size(); iKF++) //遍历关键帧所有特征点在图像所有特征点集合中的索引
{
const unsigned int realIdxKF = vIndicesKF[iKF];
//关键帧特征点对应的地图点
MapPoint* pMP = vpMapPointsKF[realIdxKF];
if(!pMP)
continue;
if(pMP->isBad())
continue;
//特征点在关键帧中的描述子
const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF);
//第一最优距离
int bestDist1=256;
int bestIdxF =-1 ;
//第二最优距离
int bestDist2=256;
for(size_t iF=0; iF<vIndicesF.size(); iF++) // 遍历当前帧所有特征点在图像所有特征点集合中的索引
{
const unsigned int realIdxF = vIndicesF[iF];
if(vpMapPointMatches[realIdxF])
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;
}
}
if(bestDist1<=TH_LOW) //第一最优距离小于阈值
{
// 第一最优距离/第二最优距离 < 距离比阈值 此条件证明该特征点只与第一距离的描述子距离近,而与其他描述子距离远
// 提高匹配的正确率,如果不满足该条件,直接视为此次匹配失败
if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
{
//将匹配到的地图点添加到vpMapPointMatches容器中
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 && bin<HISTO_LENGTH);
rotHist[bin].push_back(bestIdxF); // 将最优匹配点在当前帧的索引添加进rotHist[]容器
}
nmatches++; //匹配点数量增加1
}
}
}
KFit++;//关键帧特征点迭代指针后移
Fit++; // 当前帧特征点指针后移
}
else if(KFit->first < Fit->first)//如果关键帧的当前特征向量节点id小于当前帧的特征点节点id
{
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;
//选出图像特征点的三个主方向
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;
}
然后 位姿优化
// 优化位姿和地图点,并将地图点分为局外点和局内点
/*********************************************************
* 建立优化器
* 设置优化器的优化算法
* 添加位姿节点
* 添加地图点边
* 开始优化 执行四次迭代优化,每次优化都将所有点分为内点和外点,在下次优化时只根据上次结果的内点进行优化,计算出位姿后,对所有点重新分类内点/外点
* 将优化后的相机位姿添加到帧中 ,并将帧中的地图点分为内点和外点,返回内点数量
***************************************************************/
int Optimizer::PoseOptimization(Frame *pFrame)
{
// 建立优化器
g2o::SparseOptimizer optimizer;
//为优化器添加优化算法: 莱温伯格梯度下降法
g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
// 初始的地图点数量
int nInitialCorrespondences=0;
// Set Frame vertex 添加位姿节点
g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
vSE3->setId(0);
vSE3->setFixed(false);
optimizer.addVertex(vSE3);
// Set MapPoint vertices 添加地图点节点
const int N = pFrame->N;
// 单目 定义边
vector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
// 边的索引
vector<size_t> vnIndexEdgeMono;
vpEdgesMono.reserve(N);
vnIndexEdgeMono.reserve(N);
// 双目 定义边
vector<g2o::EdgeStereoSE3ProjectXYZOnlyPose*> vpEdgesStereo;
vector<size_t> vnIndexEdgeStereo;
vpEdgesStereo.reserve(N);
vnIndexEdgeStereo.reserve(N);
const float deltaMono = sqrt(5.991);
const float deltaStereo = sqrt(7.815);
{
unique_lock<mutex> lock(MapPoint::mGlobalMutex);
for(int i=0; i<N; i++) // 循环所有地图点
{
MapPoint* pMP = pFrame->mvpMapPoints[i];
if(pMP)
{
// Monocular observation 单目相机
if(pFrame->mvuRight[i]<0)
{
nInitialCorrespondences++;
pFrame->mvbOutlier[i] = false;
Eigen::Matrix<double,2,1> obs;
const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
obs << kpUn.pt.x, kpUn.pt.y;
g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();
// 添加节点
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
// 添加测量值
e->setMeasurement(obs);
// 当前关键点的尺度的逆
const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
// 设置信息矩阵
e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(deltaMono);
// 设置边的相机内参数矩阵
e->fx = pFrame->fx;
e->fy = pFrame->fy;
e->cx = pFrame->cx;
e->cy = pFrame->cy;
// 设置边的地图点
cv::Mat Xw = pMP->GetWorldPos();
e->Xw[0] = Xw.at<float>(0);
e->Xw[1] = Xw.at<float>(1);
e->Xw[2] = Xw.at<float>(2);
optimizer.addEdge(e);
vpEdgesMono.push_back(e);
vnIndexEdgeMono.push_back(i);
}
else // Stereo observation 双目相机
{
nInitialCorrespondences++;
pFrame->mvbOutlier[i] = false;
//SET EDGE
Eigen::Matrix<double,3,1> obs;
const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i]; //矫正后的左眼坐标
const float &kp_ur = pFrame->mvuRight[i];
obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();
// 添加节点
e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));//???
// 添加测量值
e->setMeasurement(obs);
// 当前关键点的尺度的逆
const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
// 设置信息矩阵
Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
e->setInformation(Info);
g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
e->setRobustKernel(rk);
rk->setDelta(deltaStereo);
e->fx = pFrame->fx;
e->fy = pFrame->fy;
e->cx = pFrame->cx;
e->cy = pFrame->cy;
e->bf = pFrame->mbf;
// 设置边的地图点
cv::Mat Xw = pMP->GetWorldPos();
e->Xw[0] = Xw.at<float>(0);
e->Xw[1] = Xw.at<float>(1);
e->Xw[2] = Xw.at<float>(2);
optimizer.addEdge(e);
vpEdgesStereo.push_back(e);
vnIndexEdgeStereo.push_back(i);
}
}
}
}
if(nInitialCorrespondences<3) // 如果初始的地图点边数量小于3
return 0;
// We perform 4 optimizations, after each optimization we classify observation as inlier/outlier
// At the next optimization, outliers are not included, but at the end they can be classified as inliers again.
// 执行四次迭代优化,每次优化都将所有点分为内点和外点,在下次优化时只根据上次结果的内点进行优化,计算出位姿后,对所有点重新分类内点/外点
// 单目相机下允许的最大误差
const float chi2Mono[4]={5.991,5.991,5.991,5.991};
// 双目相机下允许的最大误差
const float chi2Stereo[4]={7.815,7.815,7.815, 7.815};
// 迭代下降次数
const int its[4]={10,10,10,10};
// 外点数量
int nBad=0;
for(size_t it=0; it<4; it++)
{
vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
optimizer.initializeOptimization(0);
optimizer.optimize(its[it]); //每次优化都迭代下降10次
nBad=0;
for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++) // 如果是单目相机 双目相机时vpEdgesMono.size() = 0
{
g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];
const size_t idx = vnIndexEdgeMono[i];
if(pFrame->mvbOutlier[idx])
{
e->computeError();
}
// 当前地图点的对于当前位姿模型下的误差
const float chi2 = e->chi2();
// 如果误差大于阈值 则证明是外点,该边的层数置为1
if(chi2>chi2Mono[it])
{
pFrame->mvbOutlier[idx]=true;
e->setLevel(1);
nBad++;
}
else //否则误差小于阈值,则不是外点,边的层数置为0
{
pFrame->mvbOutlier[idx]=false;
e->setLevel(0);
}
if(it==2)// 除了前两次优化需要RobustKernel以外, 其余的优化都不需要
e->setRobustKernel(0);
}
for(size_t i=0, iend=vpEdgesStereo.size(); i<iend; i++) //如果是双目相机 单目相机时vpEdgesStereo.size() = 0
{
g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = vpEdgesStereo[i];
const size_t idx = vnIndexEdgeStereo[i];
if(pFrame->mvbOutlier[idx])
{
e->computeError(); //y-fx
}
const float chi2 = e->chi2();
if(chi2>chi2Stereo[it])
{
// 当前帧的局外点设置为1
pFrame->mvbOutlier[idx]=true;
e->setLevel(1);
nBad++;
}
else
{
e->setLevel(0);
pFrame->mvbOutlier[idx]=false;
}
if(it==2)
e->setRobustKernel(0);
}
if(optimizer.edges().size()<10) // 如果优化边数量小于10 也就是说内点数量太少 则退出优化
break;
}
// Recover optimized pose and return number of inliers 设置优化后的位姿和内点数量
g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));
g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();
cv::Mat pose = Converter::toCvMat(SE3quat_recov);
pFrame->SetPose(pose);
return nInitialCorrespondences-nBad;
}
根据恒速模型设定当前帧的初始位姿
通过投影的方式在参考帧中找当前帧特征点的匹配点
优化每个特征点所对应3D点的投影误差即可得到位姿
TrackWithMotionModel()
/******************************************************************************
* 根据运动模型来进行位姿计算 并完成追踪
* 1 更新上一帧的相机位姿 根据上一帧相机位姿以及上一帧相机位姿变化的速度来估算当前位姿
* 2 通过映射的方式匹配当前帧和上一帧,并追踪上一帧的地图点
* 3 优化当前帧的相机位姿
* 4 去除局外点,如果当前模型下的局内点数量大于10 则返回成功追踪
* 5 仅追踪模式下地图内的匹配点数量小于10将mbVO置1 表明地图内匹配内点数量不足 返回局内点数量是否大于20
***********************************************************************/
bool Tracking::TrackWithMotionModel()
{
ORBmatcher matcher(0.9,true);
// Update last frame pose according to its reference keyframe
// Create "visual odometry" points if in Localization Mode
UpdateLastFrame();
//将当前帧的初始位姿设为上一帧位姿乘上一帧位姿的变化速度
mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
//将当前帧的地图点清空
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
// Project points seen in previous frame 通过映射来计算匹配
int th;
if(mSensor!=System::STEREO)
th=15;
else
th=7;
// 匹配局内点的数量
int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
// If few matches, uses a wider window search 如果匹配点不足够多,则增大筛选阈值
if(nmatches<20)
{
fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR);
}
if(nmatches<20)
return false;
// Optimize frame pose with all matches 优化位姿并将地图点分为局外点和局内点
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers 匹配点在地图点中的数量
int nmatchesMap = 0;
for(int i =0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
if(mCurrentFrame.mvbOutlier[i])
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
mCurrentFrame.mvbOutlier[i]=false;
pMP->mbTrackInView = false;
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
nmatches--;
}
else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0) // 在其他关键帧中也能看到该地图点
nmatchesMap++;
}
}
if(mbOnlyTracking)//仅追踪模式下地图内的匹配点数量小于10将mbVO置1 表明地图内匹配内点数量不足 局内点数量是否大于20
{
mbVO = nmatchesMap<10;
return nmatches>20;
}
return nmatchesMap>=10; //返回局内点是否大于10
}
其中 UpdateLastFrame
/***********************************************************
* 更新上一帧
* 1 根据参考关键帧相对于初始帧的相机位姿和上一帧相对于参考关键帧的相机位姿来计算上一帧相对于初始帧的相机位姿
* 2 如果上一帧不为关键帧并且当前不是单目相机并且是在仅追踪模式下,则
* 1.创建视觉里程计的地图点 根据测量他们的地图点深度排序这些点
* 2.创建视觉里程计的地图点 根据测量他们的地图点深度排序这些点
****************************************************************/
void Tracking::UpdateLastFrame()
{
// Update pose according to reference keyframe
KeyFrame* pRef = mLastFrame.mpReferenceKF;
cv::Mat Tlr = mlRelativeFramePoses.back();
mLastFrame.SetPose(Tlr*pRef->GetPose());
//如果上一帧为关键帧或者当前是单目相机或者是不在仅追踪模式下 则返回
if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR || !mbOnlyTracking)
return;
// Create "visual odometry" MapPoints
// We sort points according to their measured depth by the stereo/RGB-D sensor
// 创建视觉里程计的地图点 根据测量他们的地图点深度排序这些点
vector<pair<float,int> > vDepthIdx;
vDepthIdx.reserve(mLastFrame.N);
for(int i=0; i<mLastFrame.N;i++)
{
float z = mLastFrame.mvDepth[i];
if(z>0)
{
vDepthIdx.push_back(make_pair(z,i));
}
}
if(vDepthIdx.empty())
return;
sort(vDepthIdx.begin(),vDepthIdx.end());
// We insert all close points (depth<mThDepth)
// If less than 100 close points, we insert the 100 closest ones.
// 插入所有的近点,如果近点数量大于100则取最近的100
int nPoints = 0;
for(size_t j=0; j<vDepthIdx.size();j++)
{
int i = vDepthIdx[j].second;
bool bCreateNew = false;
// 取地图点,如果没有创建该地图点,则创建一下然后判断近点
MapPoint* pMP = mLastFrame.mvpMapPoints[i];
if(!pMP)
bCreateNew = true;
else if(pMP->Observations()<1)
{
bCreateNew = true;
}
if(bCreateNew)
{
cv::Mat x3D = mLastFrame.UnprojectStereo(i);
MapPoint* pNewMP = new MapPoint(x3D,mpMap,&mLastFrame,i);
mLastFrame.mvpMapPoints[i]=pNewMP;
mlpTemporalPoints.push_back(pNewMP);
nPoints++;
}
else
{
nPoints++;
}
if(vDepthIdx[j].first>mThDepth && nPoints>100)
break;
}
}
1）上一帧设置位姿
2）选择深度最近的100个地图点
SearchByProjection
int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)
{
int nmatches = 0;
// Rotation Histogram (to check rotation consistency) 旋转直方图 为了检测旋转方向的一致性
vector<int> rotHist[HISTO_LENGTH];
for(int i=0;i<HISTO_LENGTH;i++)
rotHist[i].reserve(500);
const float factor = 1.0f/HISTO_LENGTH;
//当前帧的相机位姿
const 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);
// 上一帧相机坐标系到当前帧相机坐标系的位移向量
const cv::Mat tlc = Rlw*twc+tlw;
const bool bForward = tlc.at<float>(2)>CurrentFrame.mb && !bMono;
const bool bBackward = -tlc.at<float>(2)>CurrentFrame.mb && !bMono;
//追踪上一帧的特征点
for(int i=0; i<LastFrame.N; i++) //遍历上一帧所有的特征点
{
MapPoint* pMP = LastFrame.mvpMapPoints[i];
if(pMP)
{
if(!LastFrame.mvbOutlier[i])
{
// 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);
//判断地图点深度是否是正值
if(invzc<0)
continue;
// 计算映射到当前帧的像素坐标 k*
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 半径大小 特征点所在高斯金字塔的层数决定了半径大小
float radius = th*CurrentFrame.mvScaleFactors[nLastOctave];
//投影区域内的特征点索引
vector<size_t> 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<size_t>::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++)
{
const size_t i2 = *vit;
if(CurrentFrame.mvpMapPoints[i2])
if(CurrentFrame.mvpMapPoints[i2]->Observations()>0)
continue;
if(CurrentFrame.mvuRight[i2]>0)
{
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++;
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 判断旋转方向的一致性
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]]=static_cast<MapPoint*>(NULL);
nmatches--;
}
}
}
}
return nmatches;
}
从上一帧映射地图点与当前帧进行匹配，使用追踪线程的追踪上一帧的位姿
当前帧和上一帧之间进行匹配
根据上一帧到当前帧的位姿,将上一帧的地图点投影到当前帧,然后将地图点反投影到像素坐标,在像素坐标一定范围内寻找最佳匹配点
注意这里用到的当前帧的位姿是根据上一帧的位姿和上一帧的位姿变化速度来推算的相机位姿
1）lastframe 的关键点对应的地图点
2）投影在currentframe上 看是否在特征区域范围内
3）在符合区域内的所有点，找最小描述子距离。
4）旋转直方图判断
5）只保留三个最大旋转方向的特征点
跳出来
在TrackWithMotionModel 继续进行位姿优化
剔除外点
2747
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
