ORB-SLAM2从理论到代码实现（七）：Tracking.cc程序详解（中）

本人邮箱jinbo666888@qq.com,欢迎交流！

这篇博客接上一篇的内容，继续看tracking线程。

• void Tracking::MonocularInitialization()（StereoInitialization()由于类似，不再赘述）

[code]void Tracking::MonocularInitialization()//单目初始化
{
// 如果单目初始器还没有被创建，则创建单目初始器
if(!mpInitializer)
{
// Set Reference Frame
// 单目初始帧的特征点数必须大于100
if(mCurrentFrame.mvKeys.size()>100)
{
// 步骤1：得到用于初始化的第一帧，初始化需要两帧
mInitialFrame = Frame(mCurrentFrame);
// 记录最后的一帧
mLastFrame = Frame(mCurrentFrame);
// mvbPrevMatched最大的情况就是所有特征点都被跟踪上
mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;

// 这两句是多余的，因为先前有if（!mpInitializer）
if(mpInitializer)
delete mpInitializer;

// 由当前帧构造初始器 sigma:1.0 iterations:200
mpInitializer =  new Initializer(mCurrentFrame,1.0,200);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
}
else // 如果是第二次进入，已经创建了初始器
{
// Try to initialize
// 步骤2：如果当前帧特征点数大于100，则得到用于单目初始化的第二帧
// 如果当前帧特征点太少，重新构造初始器
// 因此只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程
if((int)mCurrentFrame.mvKeys.size()<=100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
return;
}
// Find correspondences
// 步骤3：在mInitialFrame与mCurrentFrame中找匹配的特征点对
// mvbPrevMatched为前一帧的特征点，存储了mInitialFrame中哪些点将进行接下来的匹配
// mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点
ORBmatcher matcher(0.9,true);
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
// Check if there are enough correspondences
// 步骤4：如果初始化的两帧之间的匹配点太少，重新初始
if(nmatches<100)
{
delete mpInitializer;
mpInitializer = static_cast<Initializer*>(NULL);
return;
}
cv::Mat Rcw; // Current Camera Rotation
cv::Mat tcw; // Current Camera Translation
vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)

// 步骤5：通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
{
// 步骤6：删除那些无法进行三角化的匹配点
for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
{
if(mvIniMatches[i]>=0 && !vbTriangulated[i])
{
mvIniMatches[i]=-1;
nmatches--;
}
}

// Set Frame Poses
// 将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵
mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
// 由Rcw和tcw构造Tcw,并赋值给mTcw，mTcw为世界坐标系到该帧的变换矩阵
cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
tcw.copyTo(Tcw.rowRange(0,3).col(3));
mCurrentFrame.SetPose(Tcw);

// 步骤6：将三角化得到的3D点包装成MapPoints
// Initialize函数会得到mvIniP3D，
// mvIniP3D是cv::Point3f类型的一个容器，是个存放3D点的临时变量，
// CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
CreateInitialMapMonocular();
}

}

}

• void Tracking::CreateInitialMapMonocular() 
• 其中用到了Bundle Adjustment,我会对此专门写一篇博客

[code]void Tracking::CreateInitialMapMonocular()
{
// 创建两帧，一个为初始帧，一个为当前帧
KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
// 步骤1：将初始关键帧的描述子转为BoW
pKFini->ComputeBoW();
// 步骤2：将当前关键帧的描述子转为BoW
pKFcur->ComputeBoW();
// Insert KFs in the map
// 步骤3：将关键帧插入到地图
// 凡是关键帧，都要插入地图
mpMap->AddKeyFrame(pKFini);
mpMap->AddKeyFrame(pKFcur);
// Create MapPoints and asscoiate to keyframes
// 步骤4：将3D点包装成MapPoints
for(size_t i=0; i<mvIniMatches.size();i++)//遍历所有匹配
{
if(mvIniMatches[i]<0)
continue;
//Create MapPoint.
cv::Mat worldPos(mvIniP3D[i]);
// 步骤4.1：用3D点构造MapPoint
MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);
// 步骤4.2：为该MapPoint添加属性：
// a.观测到该MapPoint的关键帧
// b.该MapPoint的描述子
// c.该MapPoint的平均观测方向和深度范围
// 步骤4.3：表示该KeyFrame的哪个特征点可以观测到哪个3D点
pKFini->AddMapPoint(pMP,i);
pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
// a.表示该MapPoint可以被哪个KeyFrame的哪个特征点观测到
pMP->AddObservation(pKFini,i);
pMP->AddObservation(pKFcur,mvIniMatches[i]);
// b.从众多观测到该MapPoint的特征点中挑选区分读最高的描述子
pMP->ComputeDistinctiveDescriptors();
// c.更新该MapPoint平均观测方向以及观测距离的范围
pMP->UpdateNormalAndDepth();
//Fill Current Frame structure
mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;
//Add to Map
// 步骤4.4：在地图中添加该MapPoint
mpMap->AddMapPoint(pMP);
}

// Update Connections
// 步骤5：更新关键帧间的连接关系
// 在3D点和关键帧之间建立边，每个边有一个权重，边的权重是该关键帧与当前帧公共3D点的个数
pKFini->UpdateConnections();
pKFcur->UpdateConnections();

// Bundle Adjustment

cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl；
// 步骤5：BA优化
Optimizer::GlobalBundleAdjustemnt(mpMap,20);

// Set median depth to 1
// 步骤6：!!!将MapPoints的中值深度归一化到1，并归一化两帧之间变换
// 评估关键帧场景深度，q=2表示中值
float medianDepth = pKFini->ComputeSceneMedianDepth(2);
float invMedianDepth = 1.0f/medianDepth;

if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
{
cout << "Wrong initialization, reseting..." << endl;
Reset();
return;
}

// Scale initial baseline
cv::Mat Tc2w = pKFcur->GetPose();
// x/z y/z 将z归一化到1
Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
pKFcur->SetPose(Tc2w);

// Scale point
// 把3D点的尺度也归一化到
vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();
for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++)
{
if(vpAllMapPoints[iMP])
{

223f0
MapPoint* pMP = vpAllMapPoints[iMP];
pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);
}
}

// 这部分和SteroInitialization()相似
mpLocalMapper->InsertKeyFrame(pKFini);
mpLocalMapper->InsertKeyFrame(pKFcur);

mCurrentFrame.SetPose(pKFcur->GetPose());
mnLastKeyFrameId=mCurrentFrame.mnId;
mpLastKeyFrame = pKFcur;

mvpLocalKeyFrames.push_back(pKFcur)
mvpLocalKeyFrames.push_back(pKFini);
mvpLocalMapPoints=mpMap->GetAllMapPoints();
mpReferenceKF = pKFcur;
mCurrentFrame.mpReferenceKF = pKFcur;

mLastFrame = Frame(mCurrentFrame);
mpMap->SetReferenceMapPoints(mvpLocalMapPoints)
mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());
mpMap->mvpKeyFrameOrigins.push_back(pKFini);
mState=OK;// 初始化成功，至此，初始化过程完成

}

•  void Tracking::CheckReplacedInLastFrame()//Local Mapping线程可能会将关键帧中某些MapPoints进行替换，由于tracking中需要用到mLastFrame，这里检查并更新上一帧中被替换的MapPoints
• bool Tracking::TrackReferenceKeyFrame()

[code]bool Tracking::TrackReferenceKeyFrame()
{
// Compute Bag of Words vector
// 步骤1：将当前帧的描述子转化为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;

// 步骤2：通过特征点的BoW加快当前帧与参考帧之间的特征点匹配
// 特征点的匹配关系由MapPoints进行维护
int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
if(nmatches<15)//匹配少于15个
return false;

// 步骤3:将上一帧的位姿态作为当前帧位姿的初始值

mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw设置初值，在PoseOptimization可以收敛快一些

// 步骤4:通过优化3D-2D的重投影误差来获得位姿
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers

// 步骤5：剔除优化后的outlier匹配点（MapPoints）
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++;
}
}
return nmatchesMap>=10;
}

•  void Tracking::UpdateLastFrame()

[code]void Tracking::UpdateLastFrame()
{
// Update pose according to reference keyframe
// 步骤1：更新最近一帧的位姿
KeyFrame* pRef = mLastFrame.mpReferenceKF;
cv::Mat Tlr = mlRelativeFramePoses.back();

mLastFrame.SetPose(Tlr*pRef->GetPose()); // Tlr*Trw = Tlw 1:last r:reference w:world

// 如果上一帧为关键帧，或者单目的情况，则退出
if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR)

return;

// 步骤2：对于双目或rgbd摄像头，为上一帧临时生成新的MapPoints
// 注意这些MapPoints不加入到Map中，在tracking的最后会删除
// 跟踪过程中需要将将上一帧的MapPoints投影到当前帧可以缩小匹配范围，加快当前帧与上一帧进行特征点匹配
// Create "visual odometry" MapPoints
// We sort points according to their measured depth by the stereo/RGB-D sensor
// 步骤2.1：得到上一帧有深度值的特征点

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)//如果深度大于0
{
vDepthIdx.push_back(make_pair(z,i));
}

}

if(vDepthIdx.empty())//如果没深度值则退出

return;
// 步骤2.2：按照深度从小到大排序
sort(vDepthIdx.begin(),vDepthIdx.end());

// We insert all close points (depth<mThDepth)
// If less than 100 close points, we insert the 100 closest ones.
// 步骤2.3：将距离比较近的点包装成MapPoints

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)
{

// 这些生成MapPoints后并没有通过：
// a.AddMapPoint、
// b.AddObservation、
// c.ComputeDistinctiveDescriptors、
// d.UpdateNormalAndDepth添加属性，
// 这些MapPoint仅仅为了提高双目和RGBD的跟踪成功率
cv::Mat x3D = mLastFrame.UnprojectStereo(i);
MapPoint* pNewMP = new MapPoint(x3D,mpMap,&mLastFrame,i);
mLastFrame.mvpMapPoints[i]=pNewMP; // 添加新的MapPoint

// 标记为临时添加的MapPoint，之后在CreateNewKeyFrame之前会全部删除
mlpTemporalPoints.push_back(pNewMP);
nPoints++;
}
else
{
nPoints++;
}
if(vDepthIdx[j].first>mThDepth && nPoints>100)
break;
}
}

•  bool Tracking::TrackWithMotionModel()

[code]bool Tracking::TrackWithMotionModel()
{
ORBmatcher matcher(0.9,true);
// Update last frame pose according to its reference keyframe
// Create "visual odometry" points
// 步骤1：对于双目或rgbd摄像头，根据深度值为上一关键帧生成新的MapPoints
// （跟踪过程中需要将当前帧与上一帧进行特征点匹配，将上一帧的MapPoints投影到当前帧可以缩小匹配范围）
// 在跟踪过程中，去除outlier的MapPoint，如果不及时增加MapPoint会逐渐减少
// 这个函数的功能就是补充增加RGBD和双目相机上一帧的MapPoints数
UpdateLastFrame();
// 根据Const Velocity Model(认为这两帧之间的相对运动和之前两帧间相对运动相同)估计当前帧的位姿
mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);//当前帧位置等于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)//非双目搜索范围系数设为15
th=15;
else
th=7;

// 步骤2：根据匀速度模型进行对上一帧的MapPoints进行跟踪
// 根据上一帧特征点对应的3D点投影的位置缩小特征点匹配范围
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); // 2*th
}

if(nmatches<20)//如果匹配点少于20，返回
return false;
// Optimize frame pose with all matches
// 步骤3：优化位姿
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers
// 步骤4：优化位姿后剔除outlier的mvpMapPoints
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)//如果仅跟踪
{
mbVO = nmatchesMap<10;
return nmatches>20;
}
return nmatchesMap>=10;
}

• bool Tracking::TrackLocalMap()

  步骤	1. 更新Covisibility Graph， 更新局部关键帧 2. 根据局部关键帧，更新局部地图点，接下来运行过滤函数 isInFrustum 3. 将地图点投影到当前帧上，超出图像范围的舍弃 4. 当前视线方向v和地图点云平均视线方向n, 舍弃n*v<cos(60)的点云 5. 舍弃地图点到相机中心距离不在一定阈值内的点 6. 计算图像的尺度因子 isInFrustum 函数结束 7. 进行非线性最小二乘优化 8. 更新地图点的统计量

[code]bool Tracking::TrackLocalMap()
{
// We have an estimation of the camera pose and some map points tracked in the frame.
// We retrieve the local map and try to find matches to points in the local map.
// Update Local KeyFrames and Local Points
// 步骤1：更新局部关键帧mvpLocalKeyFrames和局部地图点mvpLocalMapPoints

UpdateLocalMap();
// 步骤2：在局部地图中查找与当前帧匹配的MapPoints
SearchLocalPoints();
// Optimize Pos
// 在这个函数之前，在Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel中都有位姿优化，
// 步骤3：更新局部所有MapPoints后对位姿再次优化
Optimizer::PoseOptimization(&mCurrentFrame);
mnMatchesInliers = 0;

// Update MapPoints Statistics
// 步骤3：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
for(int i=0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
// 由于当前帧的MapPoints可以被当前帧观测到，其被观测统计量加1
if(!mCurrentFrame.mvbOutlier[i])
{
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
if(!mbOnlyTracking)
{
// 该MapPoint被其它关键帧观测到过
if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
mnMatchesInliers++;
}
else
// 记录当前帧跟踪到的MapPoints，用于统计跟踪效果
mnMatchesInliers++;
}
else if(mSensor==System::STEREO)
mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);

}
}

// Decide if the tracking was succesful
//More restrictive if there was a relocalization recently
// 步骤4：决定是否跟踪成功
if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50
return false;

if(mnMatchesInliers<30)
return false;
else
return true;

}