这里写目录标题
- tracking线程
- 1. 基本流程
- 2. 各部分详解
- 其他相关知识
阅读本篇文章前建议先阅读
orb-slam2代码详解之mono kitti
orb-slam2代码详解之system
tracking线程
Tracking线程的主要工作是从图像中提取ORB特征，根据上一帧进行姿态估计或者进行通过全局重定位初始化位姿，然后跟踪已经重建的局部地图，优化位姿，再根据一些规则确定新的关键帧，将这些关键帧送给localmapping线程
1. 基本流程
首先从主线程system中的GrabImageMonocular()函数开始，进行图像的预处理和Frame的构造，Frame构造完成后真正进入Tracking。
↓↓↓主要看下面这张图↓↓↓
↑↑↑主要看上面这张图↑↑↑
2. 各部分详解
Tracking构造函数
Tracking线程在构造时主要是读取了一些参数，包括：
- 相机参数：
相机内参矩阵、
畸变校正矩阵、
双目摄像头基线、
相机的帧数、
颜色通道、
深度相机深度与距离的转化因子 - ORB特征提取相关参数：
每一帧提取的特征点数、
图像建立金字塔时的变化尺度、
尺度金字塔的层数、
提取fast特征点的默认阈值、
提取fast特征点的最小阈值(如果默认阈值提取不出足够fast特征点，则使用最小阈值)
具体代码如下
Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor):
mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc),
mpKeyFrameDB(pKFDB), mpInitializer(static_cast<Initializer*>(NULL)), mpSystem(pSys), mpViewer(NULL),
mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0)
{
//读取相机内参数据
// Load camera parameters from settings file
cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);//opencv的FileStorage类，用于实现数据的存取等操作
float fx = fSettings["Camera.fx"];
float fy = fSettings["Camera.fy"];
float cx = fSettings["Camera.cx"];
float cy = fSettings["Camera.cy"];
//生成相机内参矩阵
// |fx 0 cx|
// K = |0 fy cy|
// |0 0 1 |
cv::Mat K = cv::Mat::eye(3,3,CV_32F);
K.at<float>(0,0) = fx;
K.at<float>(1,1) = fy;
K.at<float>(0,2) = cx;
K.at<float>(1,2) = cy;
K.copyTo(mK);//把临时变量K中的数据copy到tracking类的成员变量中
// 图像畸变矫正系数（畸变部分可以参考十四讲的5.1.2）
// 通常只有鱼眼相机才进行校正，mono_kitti中并没有进行校正
// [k1 k2 p1 p2 k3]
cv::Mat DistCoef(4,1,CV_32F);
DistCoef.at<float>(0) = fSettings["Camera.k1"];
DistCoef.at<float>(1) = fSettings["Camera.k2"];
DistCoef.at<float>(2) = fSettings["Camera.p1"];
DistCoef.at<float>(3) = fSettings["Camera.p2"];
const float k3 = fSettings["Camera.k3"];
if(k3!=0)
{
DistCoef.resize(5);
DistCoef.at<float>(4) = k3;
}
DistCoef.copyTo(mDistCoef);//把临时变量DistCoef中的数据copy到tracking类的成员变量中
// 双目摄像头baseline * fx（双目相机能观测的最远距离）
mbf = fSettings["Camera.bf"];
//相机的帧数
float fps = fSettings["Camera.fps"];
if(fps==0)
fps=30;
// Max/Min Frames to insert keyframes and to check relocalisation
//(用在关键帧插入和重定位时的阈值)
mMinFrames = 0;
mMaxFrames = fps;
cout << endl << "Camera Parameters: " << endl;
cout << "- fx: " << fx << endl;
cout << "- fy: " << fy << endl;
cout << "- cx: " << cx << endl;
cout << "- cy: " << cy << endl;
cout << "- k1: " << DistCoef.at<float>(0) << endl;
cout << "- k2: " << DistCoef.at<float>(1) << endl;
if(DistCoef.rows==5)
cout << "- k3: " << DistCoef.at<float>(4) << endl;
cout << "- p1: " << DistCoef.at<float>(2) << endl;
cout << "- p2: " << DistCoef.at<float>(3) << endl;
cout << "- fps: " << fps << endl;
// 1:RGB 0:BGR
int nRGB = fSettings["Camera.RGB"];
mbRGB = nRGB;
if(mbRGB)
cout << "- color order: RGB (ignored if grayscale)" << endl;
else
cout << "- color order: BGR (ignored if grayscale)" << endl;
// Load ORB parameters
// 每一帧提取的特征点数 1000
int nFeatures = fSettings["ORBextractor.nFeatures"];
// 图像建立金字塔时的变化尺度 1.2
float fScaleFactor = fSettings["ORBextractor.scaleFactor"];
// 尺度金字塔的层数 8
int nLevels = fSettings["ORBextractor.nLevels"];
// 提取fast特征点的默认阈值 20
int fIniThFAST = fSettings["ORBextractor.iniThFAST"];
// 如果默认阈值提取不出足够fast特征点，则使用最小阈值 8
int fMinThFAST = fSettings["ORBextractor.minThFAST"];
// tracking过程都会用到mpORBextractorLeft作为特征点提取器
mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
// 如果是双目，tracking过程中还会用用到mpORBextractorRight作为右目特征点提取器
if(sensor==System::STEREO)
mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
// 在单目初始化的时候，会用mpIniORBextractor来作为特征点提取器
//单目初始化时要求的特征点数是平时的2倍
if(sensor==System::MONOCULAR)
mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
cout << endl << "ORB Extractor Parameters: " << endl;
cout << "- Number of Features: " << nFeatures << endl;
cout << "- Scale Levels: " << nLevels << endl;
cout << "- Scale Factor: " << fScaleFactor << endl;
cout << "- Initial Fast Threshold: " << fIniThFAST << endl;
cout << "- Minimum Fast Threshold: " << fMinThFAST << endl;
if(sensor==System::STEREO || sensor==System::RGBD)
{
// 判断一个3D点远/近的阈值
//双目和深度相机在建图时会剔除比较远的点(这里不是很确定)
mThDepth = mbf*(float)fSettings["ThDepth"]/fx;
cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl;
}
if(sensor==System::RGBD)
{
// 深度相机disparity转化为depth时的因子，是相机本身的参数
mDepthMapFactor = fSettings["DepthMapFactor"];
if(fabs(mDepthMapFactor)<1e-5)
mDepthMapFactor=1;
else
mDepthMapFactor = 1.0f/mDepthMapFactor;
}
}
入口——GrabImageMonocular()
这个函数主要完成以下几个功能
1、接受RGB、BGR或RGBA、BGRA图像（A可以理解为透明度）
2、将图像转为灰度图(mImGray)
3、把输入的图像构造成当前帧mCurrentFrame
4、进入Track()函数进行tracking过程
5、输出世界坐标系到该帧相机坐标系的变换矩阵Tcw
具体代码如下
cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp)
{
mImGray = im;
// 步骤1：将RGB或RGBA图像转为灰度图像
if(mImGray.channels()==3)
{
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGB2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGR2GRAY);
}
else if(mImGray.channels()==4)//在三通道的基础上增加了深度
{
if(mbRGB)
cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
else
cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
}
// 步骤2：使用生成的灰度图构造Frame
if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)// 没有成功初始化的前一个状态就是NO_IMAGES_YET，这个状态只会在刚开机时才存在
mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
else
mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
// 步骤3：跟踪
Track();
//输出世界坐标系到该帧相机坐标系的变换矩阵Tcw
return mCurrentFrame.mTcw.clone();
}
其中Frame的构造函数如下
Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
:mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)
{
// Frame ID
//记录这是第几帧
mnId=nNextId++;
// Scale Level Info
//1.获取特征提取器相关参数
mnScaleLevels = mpORBextractorLeft->GetLevels();
mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
mfLogScaleFactor = log(mfScaleFactor);
mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
// ORB extraction
// 2.ORB特征提取，得到特征点mvKeys和描述子mDescriptors
ExtractORB(0,imGray);
//提取出来的特征点的数目
N = mvKeys.size();
if(mvKeys.empty())
return;
// 调用OpenCV的矫正函数矫正orb提取的特征点（去畸变）
UndistortKeyPoints();
// Set no stereo information
//单目情况下没有深度信息，mvuRight、mvDepth值都设置为-1
mvuRight = vector<float>(N,-1);
mvDepth = vector<float>(N,-1);
//保存mappoint的vector向量
mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));//static_cast<type>(B)把变量B转换为type类型的变量
//标识异常关联的标志，值都设置为false
mvbOutlier = vector<bool>(N,false);
// This is done only for the first Frame (or after a change in the calibration)
//4.对第一帧图像的灰度图计算图像边界
//mbInitialComputations初始化的值为true,进行转换后值变为false,则后边进来的帧都不会进行图像边界计算的操作
//转换后图像的边界信息会发生变化，这边先对帧计算出边界信息
if(mbInitialComputations)
{
//计算图像边界
ComputeImageBounds(imGray);
/**
* 此处需要重点理解：FRAME_GRID_COLS=64，FRAME_GRID_ROWS=48
* static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX)相当于把64平均分配到图像宽度中的每一个像素上，
* 计算出每一个像素占用64的百分之多少。
* static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY)相当于把48平均分配到图像高度中每一个像素上，
* 计算出每一个像素占用48的百分之多少。
* 后边使用关键点所在的像素坐标和这个值相乘，也就得到了在64*48的网格中关键点所在的坐标
*/
mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
//从内参矩阵中获取相机内参各个参数
fx = K.at<float>(0,0);
fy = K.at<float>(1,1);
cx = K.at<float>(0,2);
cy = K.at<float>(1,2);
invfx = 1.0f/fx;
invfy = 1.0f/fy;
//只有第一帧进行这些计算，后面所有的帧都不计算
mbInitialComputations=false;
}
mb = mbf/fx;
// 把每一帧分割成48x64个网格
// 根据关键点的畸变矫正后的位置分在不同的网格里面.
//5.根据关键点的位置将其分布在不同网格中
AssignFeaturesToGrid();
}
Tracking的“主函数”——Track()
这是tracking线程的主流程，包含了tracking线程中的各个部分及它们的状态判断，并在最后将轨迹记录下来。
注：其中的仅跟踪模式是用户在图形界面手动选择的，在这个模式下只进行跟踪和定位，不进行关键帧和地图点的更新
{
// track包含两部分：估计运动、跟踪局部地图
//mState 共有四个状态
//NO_IMAGES_YET（没有成功初始化的前一个状态就是NO_IMAGES_YET，这个状态只会在刚开机或复位时才存在）
//NOT_INITIALIZED（在第一个状态之后就进入这个状态）
//OK（正常状态）
//LOST（跟丢了）
if(mState==NO_IMAGES_YET)
{
mState = NOT_INITIALIZED;
}
// mLastProcessedState存储了Tracking最新的状态，用于FrameDrawer中的绘制
mLastProcessedState=mState;
// Get Map Mutex -> Map cannot be changed
unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
// 步骤1：如果未初始化进行初始化
if(mState==NOT_INITIALIZED)
{
if(mSensor==System::STEREO || mSensor==System::RGBD)
StereoInitialization();
else
MonocularInitialization();
mpFrameDrawer->Update(this);
if(mState!=OK)
return;
}
else// 步骤2：跟踪(正常运行状态，已经初始化好了)
{
// System is initialized. Track Frame.
// bOK为临时变量，用于表示每个函数是否执行成功
bool bOK;
// Initial camera pose estimation using motion model or relocalization (if tracking is lost)
// 在viewer中有个开关menuLocalizationMode，由它控制是否ActivateLocalizationMode，并最终管控mbOnlyTracking
// mbOnlyTracking等于false表示正常VO模式（有地图更新），mbOnlyTracking等于true表示用户手动选择定位模式
if(!mbOnlyTracking)
{
// Local Mapping is activated. This is the normal behaviour, unless
// you explicitly activate the "only tracking" mode.
// 正常初始化成功
if(mState==OK)
{
// Local Mapping might have changed some MapPoints tracked in last frame
// 检查并更新上一帧被替换的MapPoints
// 更新Fuse函数和SearchAndFuse函数替换的MapPoints
CheckReplacedInLastFrame();
//TODO:当前帧和上一帧匹配并优化pose
// 步骤2.1：选择跟踪参考帧还是上一帧（恒速模型）
// 若运动速度为0或刚完成重定位，就用跟踪参考帧的模式
// mCurrentFrame.mnId<mnLastRelocFrameId+2这个判断不应该有（Dada持怀疑态度）
// 应该只要mVelocity不为空，就优先选择TrackWithMotionModel
// mnLastRelocFrameId上一次重定位的那一帧
if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
{
//TrackReferenceKeyFrame是跟踪参考帧，不能根据固定运动速度模型预测当前帧的位姿态，通过bow加速匹配（SearchByBow）
// 将上一帧的位姿作为当前帧的初始位姿
// 通过BoW的方式在参考帧中找当前帧特征点的匹配点
// 优化每个特征点都对应3D点重投影误差即可得到当前位姿
bOK = TrackReferenceKeyFrame();
}
//正常情况下采用恒速模型，也就是跟踪上一帧的模式
else
{
// 根据恒速模型设定当前帧的初始位姿
// 通过投影的方式在参考帧中找当前帧特征点的匹配点
// 优化每个特征点所对应3D点的投影误差即可得到位姿
bOK = TrackWithMotionModel();
//如果恒速模型跟踪失败了就采用跟踪参考帧的方式
if(!bOK)
bOK = TrackReferenceKeyFrame();
}
}
//重定位
else
{
// BOW搜索，PnP求解位姿
bOK = Relocalization();
}
}
else//用户选择只定位模式，不插入关键帧，局部地图不工作
{
// Localization Mode: Local Mapping is deactivated
// 只进行跟踪tracking，局部地图不工作
// 步骤2.1：跟踪上一帧或者参考帧或者重定位
// tracking跟丢了
if(mState==LOST)
{
//重定位
bOK = Relocalization();
}
else
{
// mbVO是mbOnlyTracking为true时的才有的一个变量
// mbVO为false表示此帧匹配了很多的MapPoints，跟踪很正常，
// mbVO为true表明此帧匹配了很少的MapPoints，少于10个，要跪的节奏
if(!mbVO)//跟踪正常
{
// In last frame we tracked enough MapPoints in the map
// mbVO为false则表明此帧匹配了很多的mappoints点，非常好
if(!mVelocity.empty())
{
bOK = TrackWithMotionModel();
// 这个地方是不是应该加上：
// if(!bOK)
// bOK = TrackReferenceKeyFrame();
}
else
{
bOK = TrackReferenceKeyFrame();
}
}
//mbVO为true, 跟踪不正常，则表明此帧匹配了很少的map point点，少于10个，要跪的节奏，既做跟踪又做定位
//下面这一整段就是在跟踪不正常的时候采用恒速模型和重定位两种方式进行跟踪，但更相信重定位的
else
{
// In last frame we tracked mainly "visual odometry" points.
// We compute two camera poses, one from motion model and one doing relocalization.
// If relocalization is sucessfull we choose that solution, otherwise we retain
// the "visual odometry" solution.
//在最后一帧，我们跟踪了主要的视觉里程计点
//我们计算两个相机位姿，一个来自运动模型一个做重定位。如果重定位成功我们就选择这个思路，否则我们继续使用视觉里程计思路
bool bOKMM = false;
bool bOKReloc = false;
vector<MapPoint*> vpMPsMM;
vector<bool> vbOutMM;
cv::Mat TcwMM;
//有速度，采用恒速模型跟踪
if(!mVelocity.empty())
{
bOKMM = TrackWithMotionModel();
// 这三行没啥用？
vpMPsMM = mCurrentFrame.mvpMapPoints;
vbOutMM = mCurrentFrame.mvbOutlier;
TcwMM = mCurrentFrame.mTcw.clone();
}
//重定位
bOKReloc = Relocalization();
// 重定位没有成功，但是如果跟踪成功
if(bOKMM && !bOKReloc)
{
// 这三行没啥用？
mCurrentFrame.SetPose(TcwMM);
mCurrentFrame.mvpMapPoints = vpMPsMM;
mCurrentFrame.mvbOutlier = vbOutMM;
if(mbVO)
{
// 这段代码是不是有点多余？应该放到TrackLocalMap函数中统一做
// 更新当前帧的MapPoints被观测程度
for(int i =0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
{
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
}
}
}
}
else if(bOKReloc)// 只要重定位成功整个跟踪过程正常进行（定位与跟踪，更相信重定位）
{
mbVO = false;
}
bOK = bOKReloc || bOKMM;
}
}
}
// 将最新的关键帧作为当前帧的参考帧reference frame
mCurrentFrame.mpReferenceKF = mpReferenceKF;
// If we have an initial estimation of the camera pose and matching. Track the local map.
// 步骤2.2：在帧间匹配得到初始的姿态后，现在对local map进行跟踪得到更多的匹配，并优化当前位姿
// local map:当前帧、当前帧的MapPoints、与当前帧有共视关系的所有关键帧及它们的mappoints
// 在步骤2.1中主要是两两跟踪（恒速模型跟踪上一帧、跟踪参考帧），这里搜索局部关键帧后搜集所有局部MapPoints，
// 然后将局部地图中的MapPoints和当前帧进行投影匹配，得到更多匹配的MapPoints后进行Pose优化
//TODO:当前帧和局部mappoint匹配优化pose
if(!mbOnlyTracking)
{
if(bOK)
bOK = TrackLocalMap();
}
else//用户选择了仅定位模式
{
// mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
// a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
// the camera we will use the local map again.
// 重定位成功
if(bOK && !mbVO)//正常状态，没有跟丢
bOK = TrackLocalMap();
}
if(bOK)
mState = OK;
else
mState=LOST;
// Update drawer
mpFrameDrawer->Update(this);
// If tracking were good, check if we insert a keyframe
if(bOK)
{
// Update motion model
if(!mLastFrame.mTcw.empty())
{
// 步骤2.3：更新恒速运动模型TrackWithMotionModel中的mVelocity
cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));//注意这里colRange(0,3)指的是从第0列开始，往右数3列，也就是0,1,2列
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
mVelocity = mCurrentFrame.mTcw*LastTwc; // 为什么这个是速度呢，因为这个乘以上一帧的位姿Tcw就是当前帧的位姿Tcw
}
else
mVelocity = cv::Mat();
//在用户显示窗口中画当前帧的相机位姿
mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.mTcw);
// Clean VO matches
// 步骤2.4：在当前帧中清除UpdateLastFrame中为当前帧临时添加的MapPoints（只有深度和双目相机有）
for(int i=0; i<mCurrentFrame.N; i++)
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(pMP)
//如果该MapPoint没有被其他帧观察到(就是前面说的临时mappoints)，则将该MapPoint去掉
if(pMP->Observations()<1)
{
mCurrentFrame.mvbOutlier[i] = false;
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Delete temporal MapPoints
// 步骤2.5：清除临时的MapPoints，这些MapPoints在TrackWithMotionModel的UpdateLastFrame函数里生成（仅双目和rgbd）
// 步骤2.4中只是在当前帧中将这些MapPoints剔除，这里从MapPoints数据库中删除
// 这里生成的仅仅是为了提高双目或rgbd摄像头的帧间跟踪效果，用完以后就扔了，没有添加到地图中
for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
{
MapPoint* pMP = *lit;
delete pMP;
}
// 这里不仅仅是清除mlpTemporalPoints，通过上面delete pMP还删除了指针指向的MapPoint
mlpTemporalPoints.clear();
// Check if we need to insert a new keyframe
// 步骤2.6：检测并插入关键帧，对于双目会产生新的MapPoints
if(NeedNewKeyFrame())
CreateNewKeyFrame();
// We allow points with high innovation (considererd outliers by the Huber Function)
// pass to the new keyframe, so that bundle adjustment will finally decide
// if they are outliers or not. We don't want next frame to estimate its position
// with those points so we discard them in the frame.
// 删除那些在bundle adjustment中检测为outlier的3D mappoint点
for(int i=0; i<mCurrentFrame.N;i++)
{
if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
}
}
// Reset if the camera get lost soon after initialization
// 跟踪失败，并且Relocalization也没有搞定，只能重新Reset
if(mState==LOST)
{
if(mpMap->KeyFramesInMap()<=5)//地图中的关键帧少于5个，说明初始化不久
{
cout << "Track lost soon after initialisation, reseting..." << endl;
mpSystem->Reset();
return;
}
}
if(!mCurrentFrame.mpReferenceKF)
mCurrentFrame.mpReferenceKF = mpReferenceKF;
//Tracking完成的时候，记录当前帧为上一帧
mLastFrame = Frame(mCurrentFrame);
}
//TODO: 至此，tracking结束
单目相机初始化——MonocularInitialization()
由于单目相机没有深度信息，所以要通过初始化确定一个初始的地图信息和初始位姿。
初始化一般使用两帧即可，但是需要有足够多的特征点以及足够多的匹配对。
找到了合适的两帧图像后，就可以利用RANSAC迭代来求解基础矩阵F和单应矩阵H。它们对应两种不同的通过几组2d点对求解位姿的方法。其中单应矩阵求解的方法要求地图点分布在3维空间中的一个平面上。（这部分是对极几何知识，参考十四讲的7.3）
那么我们选择哪一种方法来求解呢：对于两种方法求解出的位姿，分别计算重投影误差，得到两个模型的分数，通过比较分数来让ORB-SLAM系统自动选择使用哪个模型。
如果初始化成功，就将第一帧的位姿设为原点，第二帧的位姿就是根据F或H计算出的位姿变换矩阵。同时，利用全局BA对位姿和地图点进行优化。并将第一帧作为关键帧送给localmapping。
具体代码如下
/**
* @brief 单目的地图初始化
*
* 并行地计算基础矩阵和单应性矩阵，选取其中一个模型，恢复出最开始两帧之间的相对姿态以及点云
* 得到初始两帧的匹配、相对运动、初始MapPoints
*/
/**
* 单目相机初始化函数
* 功能：创建初始化器，并对前两个关键点数大于100的帧进行特征点匹配，根据匹配结果计算出当前帧的变换矩阵并在窗口中显示
* 1. 第一次进入该方法,如果当前帧关键点数>100,将当前帧保存为初始帧和最近的一帧，并创建一个初始化器；
* 2. 第二次进入该方法的时候，已经有初始化器了，如果当前帧中的关键点数>100，利用ORB匹配器，对当前帧和初始帧进行匹配，匹配关键点数小于100时失败；
* 3. 利用匹配的关键点信息进行单应矩阵和基础矩阵的计算，进而计算出相机位姿的旋转矩阵和平移矩阵；
* 4. 进行三角化判断，删除不能三角化的无用特征关键点；
* 5. 由旋转矩阵和平移矩阵构造变换矩阵；
* 6. 将三角化得到的3D点包装成MapPoints，在地图中显示；
*/
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());
//把特征点的坐标存到mvbPrevMatched中，用于下一帧到来时匹配
for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;
// 这两句是多余的，因为在上上面这个if条件下肯定不会有这种情况
if(mpInitializer)
delete mpInitializer;
// 由当前帧构造初始器 1.0是参考误差，200是RANSAC迭代次数
mpInitializer = new Initializer(mCurrentFrame,1.0,200);
//mvIniMatches中所有的元素值设置为-1
// mvIniMatches用在第二帧来的时候，存储了初始帧和第二帧之间匹配的特征点
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：特征点数大于100，则在mInitialFrame与mCurrentFrame中找匹配的特征点对
//matcher的第一个参数0.9是最优匹配和次优匹配距离之间的阈值，最优匹配的距离要小于次优匹配的0.9倍才能被认为是合格的匹配
//matcher的第二个参数表示是否开启方向匹配
ORBmatcher matcher(0.9,true);
/**
* 计算当前帧和初始化帧之间的匹配关系
* 在mInitialFrame与mCurrentFrame中找匹配的特征点对
* mvbPrevMatched为前一帧的特征点的坐标，存储了mInitialFrame中哪些点将进行接下来的匹配
* mvIniMatches用于存储mInitialFrame,mCurrentFrame之间匹配的特征点
* mvIniMatches[i]中i为前一帧匹配的关键点的索引下标()，值为当前帧的匹配的关键点的索引下标。对于没匹配上的i，mvIniMatches[i]的值是-1
**/
/*
最后一个参数是匹配的范围
我们认为初始化时帧A和帧B变化不大，对于帧A中坐标为(x,y)的特征点，只在帧B中(x,y)的一定范围内寻找匹配特征点
*/
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; // 当前旋转矩阵
cv::Mat tcw; // 当前平移矩阵
vector<bool> vbTriangulated; // 三角化对应关系(三角化是指通过在两处观察同一个点的夹角,确定该点的距离。参考十四讲7.5)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;
//关键点匹配个数-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));//colRange(0,3)指的是从第0列开始，往右数3列，也就是0,1,2列，而不是0,1,2,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();
}
}
}
2.1 位姿跟踪
恒速运动模型——TrackWithMotionModel()
先假设这次的帧间运动和上次的帧间运动相同，根据上上帧和上一帧的位姿粗略地估计一个当前位姿，再用最小化重投影误差(BA)的方法对位姿进行优化（注意这里的优化只优化位姿，不动地图点）
步骤：
- 1.首先通过上一帧的位姿和速度来设置当前帧相机的位姿
- 2.通过PnP方法估计相机位姿，再将上一帧的地图点投影到当前固定大小范围的帧平面上，如果匹配点少，那么扩大两倍的采点范围
- 3.然后进行一次BA算法，优化相机的位姿
- 4.优化位姿之后，对当前帧的关键点和地图点，抛弃无用的杂点，剩下的点供下一次操作使用
代码如下
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数（但这个mappoint是临时的，仅在这两帧之间用，不参与局部地图构建）
UpdateLastFrame();
// 根据恒速运动模型估计当前帧的位姿
// mVelocity为最近一次前后帧位姿之差，它乘以上上帧等于上一帧
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;//双目相机搜索范围系数比较小，Dada觉得是因为双目的本身两只眼，给它一个小的范围系数，最后的实际范围是一样的(这不是很清楚)
// 步骤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)
return false;
// Optimize frame pose with all matches
// 步骤3：优化位姿，only-pose BA优化，只优化位姿不优化地图点
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers
// 步骤4：优化位姿后解除outlier的mvpMapPoints与当前帧特征点的对应关系
int nmatchesMap = 0;//表示优化位姿后当前帧的特征点中还能与局部地图的mappoint匹配的特征点的数目
for(int i =0; i<mCurrentFrame.N; i++)//这一帧图片*有N个特征点
{
if(mCurrentFrame.mvpMapPoints[i])//如果这个特征点有对应的mappoint
{
if(mCurrentFrame.mvbOutlier[i])//优化位姿后这个mappoint在这个位姿下是不应该观测到的，就把这个特征点和这个mappoint的匹配解除
{
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;//当前帧与局部地图匹配的特征点数
}
跟踪参考关键帧——TrackReferenceKeyFrame()
当恒速运动模型的跟踪效果不好时，就根据参考关键帧来计算位姿。
这种方法先假设当前帧的位姿为参考关键帧的位姿，后面与恒速模型相似，进行BA优化求解位姿。
注：参考关键帧是指与当前帧共视程度最高的关键帧
步骤：
- 1.按照关键帧进行Track的方法和运动模式恢复相机运动位姿的方法接近。首先求解当前帧的BOW向量。
- 2.再搜索当前帧和关键帧之间的关键点匹配关系，如果这个匹配关系小于15对的话，就Track失败了。
- 3.接着将当前帧的位置假定到上一帧的位置那里
- 4.并通过最小二乘法优化相机的位姿。
- 5.最后依然是抛弃无用的杂点，当match数大于等于10的时候，返回true成功。
具体代码如下：
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);//这种跟踪上一帧的模式比恒速运动模型的跟踪模式对ORB特征匹配的要求更严格
/**
* vpMapPointMatches中存储的是当前帧特征点的下标，值为mpReferenceKF参考帧中对应的MapPoint，没匹配上的下标对应的值为NULL
*/
vector<MapPoint*> vpMapPointMatches;
// 步骤2：通过特征点的BoW加快当前帧与参考帧之间的特征点匹配，对上一个关键帧进行BOW搜索匹配点
// 特征点的匹配关系由MapPoints进行维护
int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
if(nmatches<15)
return false;
// 步骤3:将上一帧的位姿态作为当前帧位姿的初始值
mCurrentFrame.mvpMapPoints = vpMapPointMatches;
mCurrentFrame.SetPose(mLastFrame.mTcw); // 用上一次的Tcw设置初值，在PoseOptimization可以收敛快一些
// 步骤4:通过优化3D-2D的重投影误差来获得位姿(PnP，详见十四讲7.7)
Optimizer::PoseOptimization(&mCurrentFrame);
// Discard outliers
// 步骤5：剔除优化后的outlier匹配点（MapPoints）（这一部分详细注释见TrackWithMotionModel函数）
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;
}
Relocalization——重定位
当上面两种方法都跟踪失败时，对相机的位姿进行重定位。
重定位的主要思想是在以前的关键帧数据库中找到与当前帧相似的一些候选关键帧，根据这些候选关键帧的数据，与当前帧进行匹配，利用PnP算法通过RANSAC迭代的方式求解当前位姿。
步骤：
* 1. 先计算当前帧的BOW值，并从关键帧数据库中查找候选的匹配关键帧
* 2. 构建PnP求解器，标记杂点，准备好每个关键帧和当前帧的匹配点集
* 3. 用PnP算法求解位姿，进行若干次P4P Ransac迭代，并使用非线性最小二乘优化，直到发现一个有充足inliers支持的相机位置
* 4. 返回成功或失败
具体代码如下：
bool Tracking::Relocalization()
{
// Compute Bag of Words Vector
//步骤1：计算当前帧特征点的Bow映射
mCurrentFrame.ComputeBoW();
// Relocalization is performed when tracking is lost
// Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
//步骤2：找到与当前帧相似的候选关键帧
vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
//如果未找到候选帧，返回false
if(vpCandidateKFs.empty())
return false;
const int nKFs = vpCandidateKFs.size();
// We perform first an ORB matching with each candidate
// If enough matches are found we setup a PnP solver
//我们首先执行与每个候选匹配的ORB匹配
//如果找到足够的匹配，我们设置一个PNP解算器
ORBmatcher matcher(0.75,true);
vector<PnPsolver*> vpPnPsolvers;
vpPnPsolvers.resize(nKFs);
vector<vector<MapPoint*> > vvpMapPointMatches;
vvpMapPointMatches.resize(nKFs);
vector<bool> vbDiscarded;
vbDiscarded.resize(nKFs);
int nCandidates=0;
//遍历候选关键帧
for(int i=0; i<nKFs; i++)
{
KeyFrame* pKF = vpCandidateKFs[i];
if(pKF->isBad())
vbDiscarded[i] = true;//去除不好的候选关键帧
else
{
//步骤3：通过BoW进行匹配，计算出pKF中和mCurrentFrame匹配的关键点的MapPoint存入vvpMapPointMatches中。
int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
if(nmatches<15)
{
//候选关键帧中匹配点数小于15的丢弃
vbDiscarded[i] = true;
continue;
}
else//用pnp求解
{
//候选关键帧中匹配点数大于15的构建PnP求解器。这个PnP求解器中的3D point为vvpMapPointMatches中的MapPoint，2D点为mCurrentFrame中的关键点
//因为是重定位，所以就是求重定位的候选帧对应的MapPoint到当前帧的关键点之间的投影关系，通过投影关系确定当前帧的位姿，也就进行了重定位
PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
//候选帧中匹配点数大于15的进行Ransac迭代
pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);
vpPnPsolvers[i] = pSolver;
nCandidates++;
}
}
}
// Alternatively perform some iterations of P4P RANSAC
// Until we found a camera pose supported by enough inliers
// 执行一些P4P RANSAC迭代，直到我们找到一个由足够的inliers支持的相机位姿
bool bMatch = false;
ORBmatcher matcher2(0.9,true);
while(nCandidates>0 && !bMatch)
{
for(int i=0; i<nKFs; i++)
{
if(vbDiscarded[i])
continue;
// Perform 5 Ransac Iterations
vector<bool> vbInliers;
int nInliers;
bool bNoMore;
//步骤4：通过EPnP算法估计姿态
PnPsolver* pSolver = vpPnPsolvers[i];
cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
// If Ransac reachs max. iterations discard keyframe
if(bNoMore)
{
vbDiscarded[i]=true;
nCandidates--;
}
// If a Camera Pose is computed, optimize
if(!Tcw.empty())
{
Tcw.copyTo(mCurrentFrame.mTcw);
set<MapPoint*> sFound;
const int np = vbInliers.size();
for(int j=0; j<np; j++)
{
if(vbInliers[j])
{
mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];
sFound.insert(vvpMapPointMatches[i][j]);
}
else
mCurrentFrame.mvpMapPoints[j]=NULL;
}
//步骤5：通过PoseOptimization对姿态进行优化求解
int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
if(nGood<10)
continue;
for(int io =0; io<mCurrentFrame.N; io++)
if(mCurrentFrame.mvbOutlier[io])
mCurrentFrame.mvpMapPoints[io]=static_cast<MapPoint*>(NULL);
// If few inliers, search by projection in a coarse window and optimize again
//步骤6：如果内点比较少，在一个大概的窗口中投影并再次进行位姿优化
if(nGood<50)
{
int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);
if(nadditional+nGood>=50)
{
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
// If many inliers but still not enough, search by projection again in a narrower window
// the camera has been already optimized with many points
if(nGood>30 && nGood<50)
{
sFound.clear();
for(int ip =0; ip<mCurrentFrame.N; ip++)
if(mCurrentFrame.mvpMapPoints[ip])
sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,3,64);
// Final optimization
if(nGood+nadditional>=50)
{
nGood = Optimizer::PoseOptimization(&mCurrentFrame);
for(int io =0; io<mCurrentFrame.N; io++)
if(mCurrentFrame.mvbOutlier[io])
mCurrentFrame.mvpMapPoints[io]=NULL;
}
}
}
}
// If the pose is supported by enough inliers stop ransacs and continue
if(nGood>=50)
{
bMatch = true;
break;
}
}
}
}
if(!bMatch)
{
return false;
}
else
{
mnLastRelocFrameId = mCurrentFrame.mnId;
return true;
}
}
2.2 TrackLocalMap()——局部地图跟踪
局部地图由局部关键帧(LocalKeyFrames)和它们的地图点(LocalMapPoints)组成。
局部关键帧包含：
- 与当前帧有共视关系的关键帧
- 与上一组得到的关键帧共视关系较好的关键帧
这一部分主要是对它们进行更新，更新后再进行位姿的BA优化（这是第二次优化，第一次在位姿跟踪的部分。这次同样也是只优化位姿不优化地图点）
步骤：
- 更新局部地图：UpdateLocalMap()，这里面包含UpdateLocalKeyFrames()和UpdateLocalPoints()
- 在局部地图中查找与当前帧匹配的地图点：SearchLocalPoints()
- 进行位姿优化：Optimizer::PoseOptimization()
- 判断局部地图是否跟踪成功
跟踪失败的条件为：
-
第一种情况：在刚进行重定位后，对跟踪的要求比较严格，当前帧的MapPoints被其他关键帧观测到50个以下就认为跟踪失败
-
第二种情况：正常状态，当前帧的MapPoints被其他关键帧观测到30个以下。
具体代码如下：
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 Pose，位姿优化(还是只优化位姿不优化地图点)
// 在这个函数之前，在Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel中都有位姿优化，
// 步骤3：更新局部所有MapPoints后对位姿再次优化
Optimizer::PoseOptimization(&mCurrentFrame);
// Update MapPoints Statistics
// 步骤3：更新当前帧的MapPoints被观测程度，并统计跟踪局部地图的效果
mnMatchesInliers = 0;//表示当前帧的mappoint中能被其他关键帧观测到的个数
for(int i=0; i<mCurrentFrame.N; i++)//遍历当前帧所有特征点
{
if(mCurrentFrame.mvpMapPoints[i])//这个特征点有相应的mappoint匹配
{
if(!mCurrentFrame.mvbOutlier[i])//mvbOutlier是指优化位姿后被排除在视野外的匹配对
{
// 由于当前帧的MapPoints可以被当前帧观测到，其被观测统计量加1
mCurrentFrame.mvpMapPoints[i]->IncreaseFound();//这个加的是被普通帧观测到的次数
if(!mbOnlyTracking)//用户选择正常模式
{
// 该MapPoint被其它关键帧观测到过
if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)//这个Observations表示的是这个mappoint被关键帧观测到的次数
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：决定是否跟踪成功
//在刚进行重定位后，对跟踪的要求比较严格，要求当前帧的MapPoints被其他关键帧观测到50个以上
if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50)
return false;
//正常状态要求当前帧的MapPoints被其他关键帧观测到30个以上
if(mnMatchesInliers<30)
return false;
else
return true;
}
/**
* @brief 判断当前帧是否为关键帧
* @return true if needed
*/
/**
* 函数功能：判断是否需要生成新的关键帧
* 确定关键帧的标准（必须要同时满足）：
* 1.在上一全局重定位后，过了20帧；
* 2.很久没有插入关键帧或localmapper空闲或跟踪要跟丢了（避免关键帧过密）
* 3.当前帧跟踪到大于50个点；（当前帧跟踪状态良好）
* 4.当前帧观测到的地图点中，参考关键帧也观测到的地图点的比例小于90%(单目)（确保新关键帧观测到了足够多的新环境）
*/
bool Tracking::NeedNewKeyFrame()
{
// 步骤1：如果用户在界面上选择仅跟踪定位，那么将不插入关键帧
// 由于插入关键帧过程中会生成MapPoint，因此用户选择仅跟踪定位后地图上的点云和关键帧都不会再增加
if(mbOnlyTracking)
return false;
// If Local Mapping is freezed by a Loop Closure do not insert keyframes
// 如果局部地图被回环检测线程使用，则不插入关键帧
if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
return false;
const int nKFs = mpMap->KeyFramesInMap();//关键帧数
// Do not insert keyframes if not enough frames have passed from last relocalisation
// 步骤2：判断是否距离上一次重定位的时间太短
// mCurrentFrame.mnId是当前帧的ID
// mnLastRelocFrameId是最近一次重定位帧的ID
// mMaxFrames等于图像输入的帧率
// 如果距离上一次重定位超过1s（mMaxFrames个图像就是1s），则考虑插入关键帧
// 或关键帧比较少，则考虑插入关键帧
if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)
return false;
// Tracked MapPoints in the reference keyframe
// 步骤3：得到参考关键帧跟踪到的MapPoints数量
// 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧
int nMinObs = 3;
if(nKFs<=2)
nMinObs=2;
//获取参考关键帧跟踪到的MapPoints数量
int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);//关键帧中，大于等于minObs的MapPoints的数量，一个高质量的MapPoint会被多个KeyFrame观测到
// Local Mapping accept keyframes?
// 步骤4：查询局部地图管理器是否繁忙
bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
// Stereo & RGB-D: Ratio of close "matches to map"/"total matches"
// "total matches = matches to map + visual odometry matches"
// Visual odometry matches will become MapPoints if we insert a keyframe.
// This ratio measures how many MapPoints we could create if we insert a keyframe.
// 步骤5：对于双目或RGBD摄像头，统计总的可以添加的MapPoints数量和跟踪到地图中的MapPoints数量
int nMap = 0;
int nTotal= 0;
if(mSensor!=System::MONOCULAR)// 双目或rgbd
{
for(int i =0; i<mCurrentFrame.N; i++)//遍历当前帧所有特征点
{
if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)//剔除掉一些比较远的点
{
nTotal++;// 总的可以添加mappoints数
if(mCurrentFrame.mvpMapPoints[i])
if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
nMap++;// 被关键帧观测到的mappoints数，即观测到地图中的MapPoints数量
}
}
}
else
{
// There are no visual odometry matches in the monocular case
nMap=1;
nTotal=1;
}
const float ratioMap = (float)nMap/(float)(std::max(1,nTotal));//被关键帧观测到的mappoints数占当前帧中mappoint总数的比例
// 步骤6：决策是否需要插入关键帧
// Thresholds
// 设定inlier阈值，和之前帧特征点匹配的inlier比例
float thRefRatio = 0.75f;
if(nKFs<2)
thRefRatio = 0.4f;// 如果关键帧只有一帧，那么插入关键帧的阈值设置很低
if(mSensor==System::MONOCULAR)
thRefRatio = 0.9f;
// MapPoints中和地图关联的比例阈值
float thMapRatio = 0.35f;
if(mnMatchesInliers>300)//mnMatchesInliers表示当前帧和参考帧共视点的数量
thMapRatio = 0.20f;
// Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
// 很长时间没有插入关键帧
const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
// Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
// localMapper处于空闲状态
const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
// Condition 1c: tracking is weak
// 跟踪要跪的节奏，0.25和0.3是一个比较低的阈值
//第一个条件表示当前帧和参考帧重复度太低(因为参考关键帧是和当前帧共视点最多的了)
//第二个条件表示当前帧的地图点大部分都没有被关键帧观测到
const bool c1c = mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || ratioMap<0.3f) ;
// Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
// 阈值比c1c要高，与之前参考帧（最近的一个关键帧）重复度不是太高
const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio || ratioMap<thMapRatio) && mnMatchesInliers>15);
if((c1a||c1b||c1c)&&c2)
{
// If the mapping accepts keyframes, insert keyframe.
// Otherwise send a signal to interrupt BA
//如果mapping接受关键帧，则插入关键帧，否则发送信号中断BA
if(bLocalMappingIdle)
{
return true;
}
else
{
mpLocalMapper->InterruptBA();
if(mSensor!=System::MONOCULAR)//这后面不是很清楚
{
// 队列里不能阻塞太多关键帧
// tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，
// 然后localmapper再逐个pop出来插入到mspKeyFrames
if(mpLocalMapper->KeyframesInQueue()<3)
return true;
else
return false;
}
else
return false;
}
}
else
return false;
}
更新局部关键帧——UpdateLocalKeyFrames()
局部关键帧的选取策略如下：
- 策略1：能观测到当前帧MapPoints的关键帧作为局部关键帧
- 策略2：与策略1得到的局部关键帧共视程度很高的关键帧作为局部关键帧
策略2.1:最佳共视的10帧
策略2.2:自己的子关键帧
策略2.3:自己的父关键帧
代码如下:
void Tracking::UpdateLocalKeyFrames()
{
// Each map point vote for the keyframes in which it has been observed
// 步骤1：遍历当前帧的MapPoints，记录所有能观测到当前帧MapPoints的关键帧
map<KeyFrame*,int> keyframeCounter;//keyframeCounter中最后存放的就是对应关键帧可以看到当前帧mCurrentFrame里多少个MapPoint
for(int i=0; i<mCurrentFrame.N; i++)
{
if(mCurrentFrame.mvpMapPoints[i])
{
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(!pMP->isBad())//这个Bad是在local BA里面判别的，大致是这个地图点被观测的帧数小于2（具体不是很清楚，暂时留在这里）
{
// 能观测到当前帧MapPoints的关键帧
//这两步操作不是很清楚（原理懂，代码不是很清楚）
const map<KeyFrame*,size_t> observations = pMP->GetObservations();
for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++)
keyframeCounter[it->first]++;
}
else
{
mCurrentFrame.mvpMapPoints[i]=NULL;
}
}
}
if(keyframeCounter.empty())
return;
int max=0;
KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL);//pKFmax表示能看到MapPoint点数最多的关键帧
// 步骤2：更新局部关键帧（mvpLocalKeyFrames），添加局部关键帧有三个策略
// 先清空局部关键帧
mvpLocalKeyFrames.clear();
mvpLocalKeyFrames.reserve(3*keyframeCounter.size());
// All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points
// V-D K1: shares the map points with current frame
// 策略1：能观测到当前帧MapPoints的关键帧作为局部关键帧
for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++)
{
KeyFrame* pKF = it->first;
if(pKF->isBad())
continue;
if(it->second>max)//it->second表示pKF这个关键帧可以看到多少个MapPoint
{
max=it->second;
pKFmax=pKF;//pKFmax表示能看到MapPoint点数最多的关键帧
}
//mvpLocalKeyFrames里边存放的是能看到当前帧对应的MapPoint的关键帧列表
mvpLocalKeyFrames.push_back(it->first);
// mnTrackReferenceForFrame防止重复添加局部关键帧
pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
}
// Include also some not-already-included keyframes that are neighbors to already-included keyframes
// V-D K2: neighbors to K1 in the covisibility graph
// 策略2：与策略1得到的局部关键帧共视程度很高的关键帧作为局部关键帧
for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
{
// Limit the number of keyframes
if(mvpLocalKeyFrames.size()>80)
break;
KeyFrame* pKF = *itKF;
// 策略2.1:最佳共视的10帧
const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++)
{
KeyFrame* pNeighKF = *itNeighKF;
if(!pNeighKF->isBad())
{
// mnTrackReferenceForFrame防止重复添加局部关键帧
if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
{
mvpLocalKeyFrames.push_back(pNeighKF);
pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
break;
}
}
}
// 策略2.2:自己的子关键帧
const set<KeyFrame*> spChilds = pKF->GetChilds();
for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)
{
KeyFrame* pChildKF = *sit;
if(!pChildKF->isBad())
{
if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
{
mvpLocalKeyFrames.push_back(pChildKF);
pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
break;
}
}
}
// 策略2.3:自己的父关键帧
KeyFrame* pParent = pKF->GetParent();
if(pParent)
{
// mnTrackReferenceForFrame防止重复添加局部关键帧
if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
{
mvpLocalKeyFrames.push_back(pParent);
pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
break;
}
}
}
更新局部地图点——UpdateLocalPoints()
局部地图点：局部关键帧对应的地图点
代码如下:
void Tracking::UpdateLocalPoints()
{
// 步骤1：清空局部MapPoints
mvpLocalMapPoints.clear();
// 步骤2：遍历局部关键帧mvpLocalKeyFrames
for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
{
KeyFrame* pKF = *itKF;
const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();
// 步骤2：将局部关键帧的每个MapPoints添加到mvpLocalMapPoints
for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++)
{
MapPoint* pMP = *itMP;
if(!pMP)
continue;
// mnTrackReferenceForFrame防止重复添加局部MapPoint
if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId)
continue;
if(!pMP->isBad())
{
mvpLocalMapPoints.push_back(pMP);
pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId;
}
}
}
}
获取局部地图和当前帧的匹配关系SearchLocalPoints()
在局部地图中查找在当前帧视野范围内的点，将视野范围内的点和当前帧的特征点进行投影匹配
代码如下：
void Tracking::SearchLocalPoints()
{
// Do not search map points already matched
// 步骤1：遍历当前帧的mvpMapPoints，标记这些MapPoints不参与之后的搜索
// 因为当前的mvpMapPoints一定在当前帧的视野中
for(vector<MapPoint*>::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
if(pMP)
{
if(pMP->isBad())
{
*vit = static_cast<MapPoint*>(NULL);
}
else
{
// 更新能观测到该点的帧数加1
pMP->IncreaseVisible();
// 标记该点被当前帧观测到
pMP->mnLastFrameSeen = mCurrentFrame.mnId;
// 标记该点将来不被投影，因为已经匹配过
pMP->mbTrackInView = false;
}
}
}
int nToMatch=0;//局部地图中除了当前帧的mvpMapPoints之外的点，在当前视野内的数目
// Project points in frame and check its visibility
// 步骤2：将所有局部MapPoints投影到当前帧，判断是否在视野范围内，然后进行投影匹配
for(vector<MapPoint*>::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++)
{
MapPoint* pMP = *vit;
// 已经被当前帧观测到MapPoint不再判断是否能被当前帧观测到
if(pMP->mnLastFrameSeen == mCurrentFrame.mnId)
continue;
if(pMP->isBad())
continue;
// Project (this fills MapPoint variables for matching)
// 步骤2.1：判断LocalMapPoints中的点是否在在视野内
if(mCurrentFrame.isInFrustum(pMP,0.5))//0.5表示当前帧对该mappoint的观测视角与该mappoint被观测的平均视角差的余弦值不能超过0.5，也就是60度
{
// 观测到该点的帧数加1，该MapPoint在某些帧的视野范围内
pMP->IncreaseVisible();
// 只有在视野范围内的MapPoints才参与之后的投影匹配
nToMatch++;
}
}
if(nToMatch>0)
{
ORBmatcher matcher(0.8);
int th = 1;
if(mSensor==System::RGBD)
th=3;
// If the camera has been relocalised recently, perform a coarser search
// 如果不久前进行过重定位，那么进行一个更加宽泛的搜索，阈值需要增大
if(mCurrentFrame.mnId<mnLastRelocFrameId+2)
th=5;
// 步骤2.2：对视野范围内的MapPoints通过投影进行特征点匹配
matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);
}
}
2.3 新关键帧的生成判断和生成
判断能否生成新的关键帧
确定关键帧的标准（必须要同时满足）：
- 1.局部地图没有被回环检测线程使用；
- 2.距离上一次重定位时间比较长，或者比较短但是关键帧总数比较少
- 3.很久没有插入关键帧或localmapper空闲或跟踪要跟丢了
- 4.当前帧观测到的地图点中，参考关键帧也观测到的地图点的比例小于90%(单目)（确保新关键帧观测到了足够多的新环境）
代码如下：
bool Tracking::NeedNewKeyFrame()
{
// 步骤1：如果用户在界面上选择仅跟踪定位，那么将不插入关键帧
// 由于插入关键帧过程中会生成MapPoint，因此用户选择仅跟踪定位后地图上的点云和关键帧都不会再增加
if(mbOnlyTracking)
return false;
// If Local Mapping is freezed by a Loop Closure do not insert keyframes
// 如果局部地图被回环检测线程使用，则不插入关键帧
if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
return false;
const int nKFs = mpMap->KeyFramesInMap();//关键帧数
// Do not insert keyframes if not enough frames have passed from last relocalisation
// 步骤2：判断是否距离上一次重定位的时间太短
// mCurrentFrame.mnId是当前帧的ID
// mnLastRelocFrameId是最近一次重定位帧的ID
// mMaxFrames等于图像输入的帧率
// 如果距离上一次重定位超过1s（mMaxFrames个图像就是1s），则考虑插入关键帧
// 或关键帧比较少，则考虑插入关键帧
if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)
return false;
// Tracked MapPoints in the reference keyframe
// 步骤3：得到参考关键帧跟踪到的MapPoints数量
// 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧
int nMinObs = 3;
if(nKFs<=2)
nMinObs=2;
//获取参考关键帧跟踪到的MapPoints数量
int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);//关键帧中，大于等于minObs的MapPoints的数量，一个高质量的MapPoint会被多个KeyFrame观测到
// Local Mapping accept keyframes?
// 步骤4：查询局部地图管理器是否繁忙
bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
// Stereo & RGB-D: Ratio of close "matches to map"/"total matches"
// "total matches = matches to map + visual odometry matches"
// Visual odometry matches will become MapPoints if we insert a keyframe.
// This ratio measures how many MapPoints we could create if we insert a keyframe.
// 步骤5：对于双目或RGBD摄像头，统计总的可以添加的MapPoints数量和跟踪到地图中的MapPoints数量
int nMap = 0;
int nTotal= 0;
if(mSensor!=System::MONOCULAR)// 双目或rgbd
{
for(int i =0; i<mCurrentFrame.N; i++)//遍历当前帧所有特征点
{
if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)//剔除掉一些比较远的点
{
nTotal++;// 总的可以添加mappoints数
if(mCurrentFrame.mvpMapPoints[i])
if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
nMap++;// 被关键帧观测到的mappoints数，即观测到地图中的MapPoints数量
}
}
}
else
{
// There are no visual odometry matches in the monocular case
nMap=1;
nTotal=1;
}
const float ratioMap = (float)nMap/(float)(std::max(1,nTotal));//被关键帧观测到的mappoints数占当前帧中mappoint总数的比例
// 步骤6：决策是否需要插入关键帧
// Thresholds
// 设定inlier阈值，和之前帧特征点匹配的inlier比例
float thRefRatio = 0.75f;
if(nKFs<2)
thRefRatio = 0.4f;// 如果关键帧只有一帧，那么插入关键帧的阈值设置很低
if(mSensor==System::MONOCULAR)
thRefRatio = 0.9f;
// MapPoints中和地图关联的比例阈值
float thMapRatio = 0.35f;
if(mnMatchesInliers>300)//mnMatchesInliers表示当前帧和参考帧共视点的数量
thMapRatio = 0.20f;
// Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
// 很长时间没有插入关键帧
const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
// Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
// localMapper处于空闲状态
const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
// Condition 1c: tracking is weak
// 跟踪要跪的节奏，0.25和0.3是一个比较低的阈值
//第一个条件表示当前帧和参考帧重复度太低(因为参考关键帧是和当前帧共视点最多的了)
//第二个条件表示当前帧的地图点大部分都没有被关键帧观测到
const bool c1c = mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || ratioMap<0.3f) ;
// Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
// 阈值比c1c要高，与之前参考帧（最近的一个关键帧）重复度不是太高
const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio || ratioMap<thMapRatio) && mnMatchesInliers>15);
if((c1a||c1b||c1c)&&c2)
{
// If the mapping accepts keyframes, insert keyframe.
// Otherwise send a signal to interrupt BA
//如果mapping接受关键帧，则插入关键帧，否则发送信号中断BA
if(bLocalMappingIdle)
{
return true;
}
else
{
mpLocalMapper->InterruptBA();
if(mSensor!=System::MONOCULAR)//这后面不是很清楚
{
// 队列里不能阻塞太多关键帧
// tracking插入关键帧不是直接插入，而且先插入到mlNewKeyFrames中，
// 然后localmapper再逐个pop出来插入到mspKeyFrames
if(mpLocalMapper->KeyframesInQueue()<3)
return true;
else
return false;
}
else
return false;
}
}
else
return false;
}
关键帧的生成
步骤:
- 1：用当前帧构造成关键帧
- 2：将当前关键帧设置为当前帧的参考关键帧
- 3：对于双目或rgbd摄像头，为当前帧生成新的MapPoints
void Tracking::CreateNewKeyFrame()
{
if(!mpLocalMapper->SetNotStop(true))
return;
// 步骤1：将当前帧构造成关键帧
KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
// 步骤2：将当前关键帧设置为当前帧的参考关键帧
// 在UpdateLocalKeyFrames函数中会将与当前关键帧共视程度最高的关键帧设定为当前帧的参考关键帧
mpReferenceKF = pKF;
mCurrentFrame.mpReferenceKF = pKF;
// 这段代码和UpdateLastFrame中的那一部分代码功能相同
// 步骤3：对于双目或rgbd摄像头，为当前帧生成新的MapPoints
if(mSensor!=System::MONOCULAR)
{
// 根据Tcw计算mRcw、mtcw和mRwc、mOw
mCurrentFrame.UpdatePoseMatrices();
// We sort points by the measured depth by the stereo/RGBD sensor.
// We create all those MapPoints whose depth < mThDepth.
// If there are less than 100 close points we create the 100 closest.
// 步骤3.1：得到当前帧深度小于阈值的特征点
// 创建新的MapPoint, depth < mThDepth
vector<pair<float,int> > vDepthIdx;
vDepthIdx.reserve(mCurrentFrame.N);
for(int i=0; i<mCurrentFrame.N; i++)
{
float z = mCurrentFrame.mvDepth[i];
if(z>0)
{
vDepthIdx.push_back(make_pair(z,i));
}
}
if(!vDepthIdx.empty())
{
// 步骤3.2：按照深度从小到大排序
sort(vDepthIdx.begin(),vDepthIdx.end());
// 步骤3.3：将距离比较近的点包装成MapPoints
int nPoints = 0;
for(size_t j=0; j<vDepthIdx.size();j++)
{
int i = vDepthIdx[j].second;
bool bCreateNew = false;
MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
if(!pMP)
bCreateNew = true;
else if(pMP->Observations()<1)
{
bCreateNew = true;
mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
}
if(bCreateNew)
{
cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);
// 这些添加属性的操作是每次创建MapPoint后都要做的
pNewMP->AddObservation(pKF,i);
pKF->AddMapPoint(pNewMP,i);
pNewMP->ComputeDistinctiveDescriptors();
pNewMP->UpdateNormalAndDepth();
mpMap->AddMapPoint(pNewMP);
mCurrentFrame.mvpMapPoints[i]=pNewMP;
nPoints++;
}
else
{
nPoints++;
}
// 这里决定了双目和rgbd摄像头时地图点云的稠密程度
// 但是仅仅为了让地图稠密直接改这些不太好，
// 因为这些MapPoints会参与之后整个slam过程
if(vDepthIdx[j].first>mThDepth && nPoints>100)
break;
}
}
}
/**
* 往LocalMapping线程的mlNewKeyFrames队列中插入新生成的keyframe
* LocalMapping线程中检测到有队列中有keyframe插入后线程会run起来
*/
mpLocalMapper->InsertKeyFrame(pKF);
mpLocalMapper->SetNotStop(false);
mnLastKeyFrameId = mCurrentFrame.mnId;
mpLastKeyFrame = pKF;
}
其他相关知识
三角化
三角化是指通过在两处观察同一个点的夹角,确定该点的距离。（详见十四讲7.5）
Mat类的rowRange和colRange
colRange(0,3)指的是从第0列开始，往右数3列，也就是0,1,2列，而不是0,1,2,3列
这个成员函数出现在T矩阵和R,t矩阵之间转化的时候，比如单目相机初始化的这段。所以取R矩阵的这行和下面取t矩阵的那行并不冲突。
// 由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);
map类
这里只做简单介绍，详见这篇文章
- map的定义
map是STL的一个关联容器，它提供一对一的hash（可以理解为映射）。 - map的组成
map对象是模板类，需要关键字key和存储对象value两个模板参数。key 和 value可以是任意你需要的类型，包括自定义类型。每个key只能在这个map中出现一次(就像数组的下标，只不过key可以是任意类型任意值)。 - map的构造
map的构造函数有6种，我们最常用下面这种方式构造一个map
map<int, string> mapStudent;
-
map的基本操作
begin() 返回指向map头部的迭代器clear(） 删除所有元素
count() 返回指定元素出现的次数
empty() 如果map为空则返回true
end() 返回指向map末尾的迭代器
equal_range() 返回特殊条目的迭代器对
erase() 删除一个元素
find() 查找一个元素
get_allocator() 返回map的配置器
insert() 插入元素
key_comp() 返回比较元素key的函数
lower_bound() 返回键值>=给定元素的第一个位置
max_size() 返回可以容纳的最大元素个数
rbegin() 返回一个指向map尾部的逆向迭代器
rend() 返回一个指向map头部的逆向迭代器
size() 返回map中元素的个数
swap() 交换两个map
upper_bound() 返回键值>给定元素的第一个位置
value_comp() 返回比较元素value的函数