当前位置: 首页 > 编程日记 > 正文

ORB_SLAM2中Tracking线程

Tracking线程是ORB_SLAM2的主线程。在System.cc中,使用构造函数进行了初始化,开启了三个线程。
可执行程序—>System构造函数(初始化三个线程)—>处理输入的帧(TrackMonocular)—>调用Tracking线程中的GrabImageMonocular函数(追踪线程的主要函数)。

可执行程序---->system构造函数初始化三个线程---->system中的单目初始化函数(TrackMonocular)----->
Tracking线程(GrabImageMonocular)
(1)灰度化处理
(2)构建当前帧(在这里会提取特征点,并将特征点分配到网格里)
(3)进入Track函数---->
Track函数(Track())
(1)单目初始化(MonocularInitialization):
1)特征点匹配,如果匹配数目不够的话,等待后两帧。(SearchForInitialization)
2)并行计算H、F矩阵。H矩阵采用(RANSAC+DLC),F矩阵采用(RANSAC+八点法)。
3)根据评分(重投影误差)选择F/H。
4)分解F/H,选择最合适的R,t(通过统计合理的路标点的数量,如:视差、重投影误差、Z>0)
5)三角化估计路标点深度信息。(2,3,4,5都在(Initialize)函数中实现)
6)全局BA优化,进行尺度归一化。(CreateInitialMapMonocular)

(2)使用追踪方式获得优化后的相机位姿:
1)恒速运动模型:TrackWithMotionModel()。通过前两帧的位姿变换估计当前位姿。使用领域半径搜索的方法获得匹配点对(SearchByProjection),随后优化位姿(PoseOptimization)。
2)参考关键帧模型:TrackReferenceKeyFrame()。以上一帧的位姿为初始位姿。找到当前帧的参考关键帧,使用词袋加速算法获得特征点对(SearchByBoW),随后进行位姿优化(PoseOptimization)。
3)重定位模型:Relocalization():使用词袋加速算法获得候选关键帧(DetectRelocalizationCandidates、SearchByBoW),使用EPNP估计相机位姿(SetRansacParameters)。如果内点数不够的话,将候选关键帧的路标点投影至当前帧中(SearchByProjection),获得更多的匹配点,再次进行优化。

(3)追踪局部地图(TrackLocalMap):
1)获得局部关键帧(有共视关系的帧及其邻居)和局部路标点。
2)优化位姿。如果跟踪失败的话,Tracking线程返回false。

(4)判断关键帧(NeedNewKeyFrame):
必要条件:1)内点数目不能小于15。2)内点数目小于参考关键帧的一定的比例。
以下条件必须满足一个:1)很长时间没有插入关键帧。2)满足插入关键帧的最小间隔并且localMapper处于空闲状态。

// 创建了System类的一个对象。
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true);
.........
// 在System构造函数内部对Tracking进行了初始化。
mpTracker = new Tracking(this,						//现在还不是很明白为什么这里还需要一个this指针  TODO  mpVocabulary,				//字典mpFrameDrawer, 			//帧绘制器mpMapDrawer,				//地图绘制器mpMap, 					//地图mpKeyFrameDatabase, 		//关键帧地图strSettingsFile, 			//设置文件路径mSensor);					//传感器类型iomanip

并且调用Tracking线程中的函数去获得相机位姿。

// 使用单目追踪函数处理每一帧的图像
SLAM.TrackMonocular(im,tframe);


构造函数

Tracking::Tracking(System *pSys,                       //系统实例ORBVocabulary* pVoc,                //BOW字典FrameDrawer *pFrameDrawer,          //帧绘制器MapDrawer *pMapDrawer,              //地图点绘制器Map *pMap,                          //地图句柄KeyFrameDatabase* pKFDB,            //关键帧产生的词袋数据库const string &strSettingPath,       //配置文件路径const int sensor):                  //传感器类型mState(NO_IMAGES_YET),                              //当前系统还没有准备好mSensor(sensor),                                mbOnlyTracking(false),                              //处于SLAM模式mbVO(false),                                        //当处于纯跟踪模式的时候,这个变量表示了当前跟踪状态的好坏mpORBVocabulary(pVoc),          mpKeyFrameDB(pKFDB), mpInitializer(static_cast<Initializer*>(NULL)),     //暂时给地图初始化器设置为空指针mpSystem(pSys), mpViewer(NULL),                                     //注意可视化的查看器是可选的,因为ORB-SLAM2最后是被编译成为一个库,所以对方人拿过来用的时候也应该有权力说我不要可视化界面(何况可视化界面也要占用不少的CPU资源)mpFrameDrawer(pFrameDrawer),mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0)                               //恢复为0,没有进行这个过程的时候的默认值

其中mState表示追踪线程的状态。在Tracking.h中可知追踪线程的状态包括:系统还没准备好,当前无图像、未完成初始化、正常工作、跟丢。

    enum eTrackingState{SYSTEM_NOT_READY=-1,        ///<系统没有准备好的状态,一般就是在启动后加载配置文件和词典文件时候的状态NO_IMAGES_YET=0,            ///<当前无图像NOT_INITIALIZED=1,          ///<有图像但是没有完成初始化OK=2,                       ///<正常时候的工作状态LOST=3                      ///<系统已经跟丢了的状态};

mbVO:在纯定位模式中使用,追踪的地图点小于10个,表明定位快失败了,置为true。
mnLastRelocFrameId:上一次进行重定位的帧的ID。

读取相机参数

读取配置文件中的相机参数,配置文件由使用的数据集和相机的类型决定。对于单目镜头、使用KITTI00数据集时,所使用的配置文件为:

Examples/Monocular/EuRoC_TimeStamps/KITTI00-02.yaml

对于每一帧提取的特征点数:nFeatures,指的是所有层级的图像金字塔中提取出的特征点数目之和。在高层提取出的特征点要投影回最底层。
  对于双目、RGB-D相机(会计算RGB-D虚拟的基线长度),定义大于32倍基线长度的路标点为远点(只提供旋转信息),小于32倍基线长度的点为近点(提供旋转、平移信息)。

设置局部建图器、回环检测器

在追踪线程中,追踪线程、局部建图线程、回环检测线程相互影响。因此他们间彼此包含,以方便上锁,防止数据混乱(因为线程和进程不同,多线程公用同一片内存空间)。

//设置局部建图器
void Tracking::SetLocalMapper(LocalMapping *pLocalMapper)
{mpLocalMapper=pLocalMapper;
}//设置回环检测器
void Tracking::SetLoopClosing(LoopClosing *pLoopClosing)
{mpLoopClosing=pLoopClosing;
}

获得相机相对于世界坐标的位姿

该函数是追踪线程的核心,在可执行程序中,调用system.cc中的TrackMonocular函数,执行tracking线程中的GrabImageMonocular函数。函数的目的是获得当前相机相对于世界坐标的变换矩阵Tcw。

cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im,const double &timestamp)

转为灰度图

需要注意ORB_SLAM2会将输入的图片转为灰度图然后进行处理。

构造当前帧

单目初始化时所需的特征点是追踪所需的特征点的两倍,因此在构建当前帧时,需要分为两部分:
(1)还没有图片输入,或有图片输入,但没有成功初始化的时候,构建当前帧,所需的特征点提取器是mpIniORBextractor。在这个过程中会提取特征点。
(2)成功追踪时,特征点提取器为mpORBextractorLeft。

追踪函数Track()

主要功能为单目初始化—>估计当前位姿—>跟踪局部地图—>决定新的关键帧—>当前帧信息保存。

追踪函数Track()

void Tracking::Track()

在图像reset或开始运行时,Tracking线程的状态为NO_IMAGES_YET(可执行程序调用System的构造函数,初始化三个线程。在这是会调用Tracking的构造函数,将mstate置为NO_IMAGES_YET)。此时将Tracking的状态改为NOT_INITIALIZED。(初始化之后会改为OK)
  在地图更新过程中上锁,保证地图的信息不会发生变化。

没初始化时候时初始化

初始化的目的是获得初始的位姿、路标点,方便之后的追踪。单目初始化的话可以获得尺度信息。
  在还没有初始化的时候,调用初始化函数。并行计算基础矩阵(F)和单应矩阵(H),选择一个评分最高的。分解获得相机的位姿,进行三角化,获得路标点坐标。(初始化成功后,将Tracking的状态改为OK)
  如果未初始化成功,则直接返回。

        if(mState!=OK)return;

已经初始化时通过跟踪估计相机位姿

追踪方式分为三种:运动模型(不使用对极约束求解相机位姿,而是根据恒速运动模型估计当前帧的位姿)、追踪参考关键帧、重定位。
  先检查上一帧的路标点是否被替换。因为在局部建图线程中,可能会删除一些路标点。(使用恒速运动模型时候,会将上一帧的地图点投影到当前帧)

定位且建图模式

局部建图线程则可能会对原有的地图点进行替换.在这里进行检查。
(1)运动模型为空||刚重定位完成:使用跟踪关键帧模型
1、使用词袋加速算法在当前帧、参考关键帧中进行特征点匹配。
2、使用重投影误差优化位姿。
(2)在上一帧位姿已知的情况下,使用恒速运动模型
在这里插入图片描述(3)追踪失败了,使用重定位模型(最后的补救)
在这里插入图片描述

定位模式

mbVO表示此帧匹配的路标点的个数。大于10则为false,小于10则为true。
mbVO为false:运动模型||参考关键帧模型
mbVO为true:表明追踪的路标点已经很少了,要死了。此时就要综合考虑运动模型和重定位模型。同时运行运动模型和重定位模型,优先考虑重定位模型。重定位模型成功的话使用重定位模式,否则使用运动模型。

通过运动模型、参考关键帧模型、重定位模型进行追踪,估计相机的位姿。
这三种操作都会返回一个bool值表示其结果是否可接受,记为bOK。

进行局部地图追踪

前面只是跟踪一帧得到初始位姿,这里搜索局部关键帧、局部地图点,和当前帧进行投影匹配,得到更多匹配的MapPoints后进行Pose优化。
  根据跟踪局部地图成功与否,设置Tracking线程的状态为OK还是Lost。

判断是否需要创建新的关键帧

(1)获得当前帧相对于上一帧的运动模型(即上一帧转换为当前帧的转换矩阵)。上一帧的运动为空的话,当前帧的运动也为空。

// 更新恒速运动模型 TrackWithMotionModel 中的mVelocity
cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
// mVelocity = Tcl = Tcw * Twl,表示上一帧到当前帧的变换, 其中 Twl = LastTwc
mVelocity = mCurrentFrame.mTcw*LastTwc; 

(2)清除观察不到的路标点。
(3)检测并插入关键帧(这一步条件比较宽松,因为局部建图线程还会剔除一些关键帧)。
(4)删除外点。

当前帧相关信息保存在Tracking的成员中

最后保存一下当前帧的位姿等信息(前面分析过的保存相机轨迹要用)。如果当前帧的mTcw已经得到了,则进行以下操作:(1)计算当前帧相对于参考关键帧的变换Tcr(Tcr = Tcw × Twr),Tcr被推入Tracking的成员 mlRelativeFramePoses 中保存;(2)当前帧的时间戳推入Tracking的成员 mlFrameTimes 中保存;(3)当前帧的参考关键帧推入Tracking的成员 mlpReferences 中保存;(4)当前帧是否跟踪失败的bool信息(mState==LOST)推入Tracking的成员 mlbLost 中保存。当然跟踪失败的时候当前帧的mTcw未知,则前面(1)(2)(3)步都只在尾部重复保存对应三个列表的尾部的元素,然后mlbLost尾部保存true。

单目初始化

认为单目初始化时候的参考帧和当前帧都是关键帧。单目初始化是很重要的一步,因此打算单独写一篇。

void Tracking::StereoInitialization()

约束包括:
(1)第一帧、第二帧的特征点的数量。
(2)第一帧、第二帧的特征点匹配对数。

未创建单目初始器时

(1)以第一帧为参考帧,构建初始器,并且初始化相关系数。
(2)直接返回,然后传入下一帧,计算基础矩阵。

创建单目初始器时

(1)如果第二帧的特征点数目小于100,则直接退出。
(2)对第一帧、第二帧进行特征点匹配,匹配数量小于100的话,直接退出,重新开始。
(3)进行初始化(计算F、H,使用最佳的进行分解,随后进行三角化),获得相机位姿变换以及路标点坐标。
(4)删除未三角化成功的路标点。
(5)设置第一帧为世界坐标原点。
(6)创建初始化时的路标点集合。

创建初始化地图时的路标点

初始化成功后进行归一化处理。
  将路标点从Point3f变为MapPoint。

void Tracking::CreateInitialMapMonocular()

设定关键帧

初始化时的第一帧、第二帧都为关键帧。

将路标点加入到地图中

全局优化,同时优化位姿与路标点

Optimizer::GlobalBundleAdjustemnt(mpMap,20);

进行尺度归一化处理

常用的尺度归一化处理为:(1)使用初始时计算的平移向量为单位1。(2)取场景的平均深度为单位1。ORB_SLAM2采用第二种方案。
  将位姿变换矩阵、路标点坐标进行归一化处理。

    // Step 6 将两帧之间的变换归一化到平均深度1的尺度下// Scale initial baselinecv::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 points// Step 7 把3D点的尺度也归一化到1// 为什么是pKFini? 是不是就算是使用 pKFcur 得到的结果也是相同的? 答:是的,因为是同样的三维点vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++){if(vpAllMapPoints[iMP]){MapPoint* pMP = vpAllMapPoints[iMP];pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);}}

将该两帧插入到局部地图中


判断上一帧的路标点是否被替换

在局部建图线程中,可能会对一些路标点进行替换(地图点的剔除)。因此在追踪线程开始的时候,要先判断一下路标点是否被替换。

void Tracking::CheckReplacedInLastFrame()


参考关键帧追踪模式

Tracking线程的三种追踪模式打算另写一篇,感觉很重要。

bool Tracking::TrackReferenceKeyFrame()


更新上一帧位姿

更新上一帧位姿,在上一帧深度值中生成临时地图点。
单目情况:只计算了上一帧的世界坐标系位姿。在恒速模型中被使用
双目和rgbd情况:选取有深度值的并且没有被选为地图点的点生成新的临时地图点,提高跟踪鲁棒性。(优化时有更多的约束)
  但需要注意临时地图点后面会被删除。因为毕竟不是真正的地图点。

void Tracking::UpdateLastFrame()

在追踪模式中被使用,根据上一帧的参考关键帧更新上一帧。(如果上一帧没有参考关键帧,则创建一些临时地图点)(单目相机不会调用,个人感觉是因为双目、RGB-D相机本身就可以获得深度信息,对于那些无法通过特征点匹配加入的地图点,双目相机仍可获得深度,而单目不行。)。

获得上一帧的位姿

创建临时地图点(单目不用)(感觉到ORB_SLAM2真的是将信息利用到了极致)

在这里插入图片描述

对于双目、RGB-D相机,将那些未通过特征点匹配获得的地图点也做为地图点加入后序的约束中。
(1)如果双目相机中的已知深度的点没有作为路标点的话,则作为临时地图点加入到地图中。

        MapPoint* pMP = mLastFrame.mvpMapPoints[i];if(!pMP)bCreateNew = true;else if(pMP->Observations()<1)      {// 地图点被创建后就没有被观测,也需要创建bCreateNew = true;}

(2)停止添加临时地图点
条件1:当前点的深度已经超过了阈值(35倍基线长度)
条件2:已近添加够了100个。

更新局部地图

每来一帧,都会进行跟踪获得位姿,然后进行优化,然后跟踪局部地图。在跟踪局部地图的时候会更新局部地图,并获得当前帧的参考关键帧(共视程度最高的关键帧)。
  局部地图包括局部关键帧、局部路标点。

void Tracking::UpdateLocalMap()

将局部地图点放入地图中

将局部关键帧、局部地图点放入地图中,即ORN_SLAM运行时的红色点。

更新局部关键帧和局部地图点

更新局部关键帧:获得局部关键帧。
更新局部地图点:在这里将局部地图路标点加入到局部地图中


更新局部关键帧

void Tracking::UpdateLocalKeyFrames()

局部关键帧的选择策略:
(1)将与当前关键帧有共视点的关键帧设为局部关键帧。
(2)遍历(1)中的局部关键帧,选择共视程度最高的10个关键帧作为局部关键帧。
(3)将局部关键帧的子关键帧作为局部关键帧。
(4)将局部关键帧的夫关键帧作为局部关键帧。

获得与当前关键帧有共视点的关键帧及其代价

将与当前关键帧有共视关系的关键帧保存在keyframeCounter(有共视关系的关键帧,共视次数)中。遍历当前帧的路标点,通过函数GetObservations()获得能观察到该路标点的所有关键帧。遍历所有关键帧,并将公式关系+1。

更新局部关键帧

首先将局部关键帧容器清空。

策略1:有共视点的关键帧为局部关键帧

在这个过程中同时获得共视程度最高的关键帧,及其共视程度。

    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){max=it->second;pKFmax=pKF;}// 添加到局部关键帧的列表里mvpLocalKeyFrames.push_back(it->first);// 用该关键帧的成员变量mnTrackReferenceForFrame 记录当前帧的id// 表示它已经是当前帧的局部关键帧了,可以防止重复添加局部关键帧pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;}

策略二:取出共视程度最高的10个关键帧

const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);

策略三:将局部关键帧的子关键帧作为局部关键帧

        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;//? 找到一个就直接跳出for循环?break;}}}

策略四:将局部关键帧的父关键帧作为局部关键帧

        KeyFrame* pParent = pKF->GetParent();if(pParent){// mnTrackReferenceForFrame防止重复添加局部关键帧if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId){mvpLocalKeyFrames.push_back(pParent);pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;//! 感觉是个bug!如果找到父关键帧会直接跳出整个循环break;}}

将与当前帧共视程度最高的关键帧作为参考关键帧

    if(pKFmax){mpReferenceKF = pKFmax;mCurrentFrame.mpReferenceKF = mpReferenceKF;}


更新局部地图点

清空之前的局部地图点

mvpLocalMapPoints.clear();

遍历所有局部关键帧,将局部关键帧的路标点放入局部地图点中

        for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++){MapPoint* pMP = *itMP;if(!pMP)continue;// 用该地图点的成员变量mnTrackReferenceForFrame 记录当前帧的id// 表示它已经是当前帧的局部地图点了,可以防止重复添加局部地图点if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId)continue;if(!pMP->isBad()){mvpLocalMapPoints.push_back(pMP);pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId;}}


查找当前帧视野范围内的局部地图点,建立与特征点的联系

在局部地图中查找在当前帧视野范围内的点,将视野范围内的点和当前帧的特征点进行投影匹配。

void Tracking::SearchLocalPoints()

遍历当前帧的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;}}}//遍历当前帧中所有的地图点

判断哪些局部地图点在视野范围内

如果视野范围内有点的话,建立特征点与局部地图点的联系

	if(nToMatch>0){ORBmatcher matcher(0.8);int th = 1;if(mSensor==System::RGBD)   //RGBD相机输入的时候,搜索的阈值会变得稍微大一些th=3;// If the camera has been relocalised recently, perform a coarser search// 如果不久前进行过重定位,那么进行一个更加宽泛的搜索,阈值需要增大if(mCurrentFrame.mnId<mnLastRelocFrameId+2)th=5;// 对视野范围内的MapPoints通过投影进行特征点匹配matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);}


追踪局部地图

bool Tracking::TrackLocalMap()

更新局部地图(局部关键帧与局部路标点)–>跟踪局部地图
  其主要目的就是找到与当前帧存在约束的局部关键帧,随后进行优化。

更新局部地图与局部路标点,并建立与当前关键帧的特征点的联系

	// Step 1:更新局部关键帧 mvpLocalKeyFrames 和局部地图点 mvpLocalMapPointsUpdateLocalMap();// Step 2:在局部地图中查找与当前帧匹配的MapPoints, 其实也就是用局部地图点进行跟踪SearchLocalPoints();

再次位姿优化

在这个函数之前,在 Relocalization、TrackReferenceKeyFrame、TrackWithMotionModel 中都有位姿优化,这里再次对当前帧的位姿进行优化。
  因为此时当前帧中路标点与特征点间的联系变多,约束增加,所以再次进行优化。

统计跟踪效果

记录当前帧追踪的局部地图点中为内点的数量

判断追踪局部地图是否成功

如果是刚刚重定位之后,对追踪是否成功的要求较为严格。要求内点数大于50,否则大于30就行。
局部地图追踪失败的话,会将Tracking线程的状态置为LOST

判断是否为关键帧

bool Tracking::NeedNewKeyFrame()

(1)只定位模式下,不需判断。
(2)如果局部建图线程被回环检测线程使用。
(3)如果当前帧距离上一次重定位较近,或关键帧的数目超过了限制,不插入关键帧。

判断关键帧

nRefMatches:参考关键帧中地图点的数量
thRefRatio:当前帧与参考关键帧追踪到的点的比例。
c1a:很长时间没有插入关键帧,可以插入。
c1b:满足插入关键帧的最小间隔并且localMapper处于空闲状态,可以插入。
c2a:当前帧的内点数小于阈值*参考关键帧内点数。
c2b:当前帧的内点数不能小于15。
c2a&&c2b是必须满足的条件。

是否可以插入关键帧

局部建图线程空闲:可以插入关键帧
局部建图线程繁忙:有一个缓存队列,先将关键帧插入到缓存队列里。等局部建图线程空闲下来,再插入关键帧。

创建关键帧

void Tracking::CreateNewKeyFrame()

停止局部建图线程

    if(!mpLocalMapper->SetNotStop(true))return;

创建关键帧中新的路标点

单目相机:由于无法获得深度,直接跳过。
双目、RGB-D相机:遍历特征点,将未被观测的路标点进行排序,随后从大到小选出路标点加入关键帧中。该过程的终止条件:(1)路标点的深度大于35倍基线长度。(2)最多加100个。

插入局部建图线程

将关键帧插入到局部建图线程中,然后启动局部建图线程。

    mpLocalMapper->InsertKeyFrame(pKF);// 插入好了,允许局部建图停止mpLocalMapper->SetNotStop(false);

将该关键帧设为新的关键帧

    mnLastKeyFrameId = mCurrentFrame.mnId;mpLastKeyFrame = pKF;




总结

进入追踪线程的流程为:
mono_kitti.cc—>
ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::MONOCULAR,true)---->
SLAM.TrackMonocular(im,tframe)—>
GrabImageMonocular(im,timestamp)。
在这里插入图片描述

进入Tracking线程之后:
在这里插入图片描述(1)初始化:第一帧为参考关键帧,第二帧和第一帧进行匹配。特征点匹配上的数量不满足阈值的话,会舍弃这两帧,然后再进入两帧进行判断。满足阈值后,这两帧都会作为局部关键帧插入局部地图中。然后对位姿和路标点进行优化。
(2)相机位姿跟踪:估计相机位姿并进行优化(每一帧都会优化位姿)。跟踪方式分为恒速运动模型,参考关键帧模型、重定位模型。正常情况下使用恒速运动模型;出现意外了,无法获得速度,使用参考关键帧模型;跟踪丢了(Tracking的状态不是OK),使用重定位模型。
(3)局部地图跟踪:更新局部关键帧和局部地图后,获得局部地图点,并与当前关键帧的特征点进行匹配。然后根据重投影误差进行优化(每一帧都会优化位姿)。目的是局部地图点会提供更多的重投影误差约束。对于双目、RGB-D相机,会创建临时地图点以提供更多的约束。
(4)判断是否生成关键帧。如果生成关键帧的话,先停止局部建图线程,然后将关键帧插入局部建图线程中,然后启动局部建图线程。

在这里插入图片描述
在这里插入图片描述在这里插入图片描述在这里插入图片描述
在这里插入图片描述

相关文章:

selenium的基础知识点

from selenium import webdriver from scrapy.selector import Selector#模拟登陆 browser webdriver.Chrome(executable_pathChromedriver.exe) #路径是Chromedriver.exe的存放位置&#xff0c;windows下只要配置好这个环境就不需要了browser.get(http://w) #需要登陆的那个网…

iOS 直播专题2-音视频采集

从设备(手机)的摄像头、MIC中采集音频、视频的原始数据ios的音视频采集可以从AVFoundation框架里采集 视频采集 这里我们选取GPUImage来采集视频,因为这个框架集成了很多视频滤镜,例如美颜 采集流程: 摄像头采集视频代码 GPUImageVideoCamera.m // 从前摄像头或后摄像头…

bzoj 4871: [Shoi2017]摧毁“树状图”

4871: [Shoi2017]摧毁“树状图” Time Limit: 25 Sec Memory Limit: 512 MBSubmit: 53 Solved: 9[Submit][Status][Discuss]Description 自从上次神刀手帮助蚯蚓国增添了上千万人口&#xff08;蚯口&#xff1f;&#xff09;&#xff0c;蚯蚓国发展得越来越繁荣了&#xff01…

Linux03-本地账户和组

目录 一、本地账户/etc/passwd 二、本地组/etc/group 三、切换账户su - 四、增删改本地账户useradd、userdel、usermod 五、账户默认配置文件/etc/login.defs 六、设置密码passwd(5)命令 七、增删改组groupadd、groupdel和groupmod 八、通过sudo以root身份运行命令 九…

ORB_SLAM2单目初始化策略

基本流程 单目初始化程序存储在Initializer.cc中   需要注意&#xff0c;对于双目/RGB-D相机&#xff0c;初始化时&#xff0c;由于可以直接获得相机的深度信息&#xff0c;因此无需求H/F&#xff0c;直接作为关键帧插入就行。   使用RANSACDLT求解H&#xff0c;RANSAC八点…

Powerdesigner逆向工程64位Oracle数据库

Powerdesigner老版本不支持64位Client&#xff0c;新版本弄不到破解码 解决方法&#xff0c;用Powerdesigner32位Oracle Clent访问64位Oracle Server 遇到的坑分享下 安装完64位的Oracle Server ,32位的 Oracle Clent默认的listener.ora文件有PROGRAM和ENVS这两个节点 Plsql(3…

运行jsp时,报错404

The origin server did not find a current reprsentation for the target resource or is not willing to disclose that one exists. 解决&#xff1a; 1. web.xml文件位置是否放错&#xff0c;应该放在WebContent/WEB-INF文件夹中 2. web.xml文件中是否有拼写错误&#xff0…

iOS 直播专题3-前置处理

前置处理 对视频添加美颜、水印、滤镜等对音频进行混音、消除环境音、声音特效等上一篇iOS 直播专题2-音视频采集提到视频采集采用的是GPUImage框架,这个框架集成了很多滤镜效果 这里主要介绍美颜、水印处理 处理流程: 美颜 这里的美颜效果用的是GPUImageBeautyFilter 功…

ORA-10873解决办法

今天&#xff0c;发现SAP系统的oracle数据库宕掉了。报错ORA-10873&#xff0c;经过查证解决该问题。记录一下&#xff0c;备忘。 一、问题 Oracle版本为12.1.0.2.0&#xff0c;在启动服务器后启动数据库startup&#xff0c;报错ORA-10873。 二、查证 到SAP Support Portal上…

ORB_SLAM2局部建图线程

局部建图线程入口&#xff1a;可执行程序在初始化三个线程的时候&#xff0c;在System.cc的构造函数中进入局部建图线程 mpLocalMapper new LocalMapping(mpMap, //指定使iomanipmSensorMONOCULAR); // TODO 为什么这个要设置成为MONOCULAR&#xff1f;&#xff1f;&#…

十一连测day1

这次测试&#xff0c;是福建第三中学的某同学出的&#xff0c;感觉难度还行吧&#xff0c;今天我就浅谈一下这场比赛的时间分配与心得 打开题目&#xff0c;看到了T1&#xff0c;这题是一道计数题吧&#xff0c;感觉心态一下子就崩了&#xff0c;100%的数据点应该是组合数学容斥…

iOS 直播专题5-推流

常用的推流协议有: 协议内容RTP实时流传输协议,但不保证服务质量RTCPRTP数据流协议的一个姐妹协议,为RTP提供服务质量反馈SRTP & SRTCPRTP和RTCP的安全版本,提供数据加密、消息认证功能RTSP控制声音或影像的多媒体数据串流协议RTMPADOBE公司播放器与服务器之间多媒体数…

centos6.5-vsftp搭建

我的机子是默认是没有的vsftp。 yum install -y vsftp 创建账户专为ftp而生。useradd ftp01 更改账户不可登录系统。usermod -s /sbin/nologin ftp01 vsftp默认是可以匿名登录的&#xff0c;也是默认的端口&#xff0c;这些不安全选项都要修改&#xff01; anonymous_enableYES…

Linux04-文件系统权限与ACL权限

目录 一、文件系统权限 1.1、认识文件系统权限 1.2、管理文件系统权限 1.3、特殊权限 1.4、默认权限 二、ACL权限 2.1、ACL本质是文件系统的一个挂载选项 2.2、更改文件的ACL权限 2.3、设置文件和目录的默认ACL权限 Linux中的权限管理分为两种类型 用户自主访问控制&…

ORB_SLAM2帧Frame

在追踪线程的一开始就会创建一个帧 cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im,const double &timestamp)构造函数 在构造函数中&#xff0c;会对特征点进行提取。 ExtractORB(0,imGray);特征点分配至网格 将图像划分为48*64的网格&#xff0c;然后将…

Servlet的基本架构

Servlet的基本架构&#xff1a; package test;import java.io.IOException;import javax.servlet.ServletException;import javax.servlet.http.HttpServlet;import javax.servlet.http.HttpServletRequest; import javax.servlet.http.HttpServletResponse; public class Serv…

ORACLE 用户权限管理

Oracle创建用户的语法&#xff1a; CREATE USER username IDENTIFIED BY password OR IDENTIFIED EXETERNALLY OR IDENTIFIED GLOBALLY AS CNuser [DEFAULT TABLESPACE tablespace] [TEMPORARY TABLESPACE temptablespace] [QUOTA [integer K[M] ] [UNLIMITED] ] ON tables…

iOS 直播专题6-流媒体服务器

常用的流媒体服务器有: nginx、SRS、BMS 这里主要介绍nginx、SRS 这里都用docker来运行流媒体服务器 docker 安装 下载Mac版docker stable 直接安装 注册一个docer账号直接登录SRS 安装 SRS guthub地址:https://github.com/ossrs/srs/ 启动上面安装的docker软件后,打开终端…

Linux05-进程管理

目录 一、进程 1.1、进程ID 1.2、列出进程 1.3、进程前后台 二、使用信号控制进程 三、以管理员身份注销用户&#xff08;踢掉在线用户&#xff09; 四、监控进程活动 4.1、负载平均值 4.2、实时进程监控 进程是已启动的可执行程序的运行中的实力。它由以下部分组成&a…

Mat常用赋值方式

参考https://blog.csdn.net/wanggao_1990/article/details/53264753 #include <iostream> #include <opencv2/opencv.hpp> #include <unordered_map> using namespace std; using namespace cv; int main(int argc,char** argv) {// 1Mat mat (Mat_<flo…

java modbus协议

概念 Modbus是一种串行通信协议&#xff0c;Modbus协议目前存在用于串口、以太网以及其他支持互联网协议的网络的版本。 大多数Modbus设备通信通过串口EIA-485物理层进行。 通讯格式 地址域功能码数据CRC校验(低字节在前)1字节1字节N字节2字节 在单片机硬件通讯串口行业&…

layui栅格布局问题

在使用layer.open弹出到窗口中&#xff0c;使用布局一直不起作用。 开始到写法如下, 目的是一行分成左右两块&#xff0c;比例为8:4等分。 <div class"layui-fluid"><div class"layui-row layui-col-space10"><div class"layui-col-md…

Unity3d载入外部图片文件

unity里的图片在生成时会压缩成资源文件&#xff0c;有时客户想自己放一些图片用unity显示&#xff0c;就必须载入外部图片。 大体思路&#xff1a;用Application.streamingAssetsPath或Application.dataPath来指定存放图片的相对路径。用DirectoryInfo获得目录。遍历后FileInf…

Linux06-服务、守护进程和systemd

目录 一、简介systemd 二、使用systemd 2.1、systemctl命令与systemd单元 2.2、控制系统服务 一、简介systemd RHEL6及以前&#xff0c;系统启动和服务器进程是由第一个进程 init 管理&#xff0c;init按顺序启动、启动慢。 RHEL7以后系统启动和服务器进程由 systemd系统和…

ORB_SLAM2回环检测

词典是特征点的描述子的集合&#xff0c;属于同一类特征的特征点的描述子组成单词。 在局部建图线程中&#xff0c;处理完一个关键帧后&#xff0c;会将其放入回环检测线程     在使用关键帧数据库搜索候选关键帧组&#xff08;DetectLoopCandidates&#xff09;的时候&…

nginx 启动 + uwsgi + django

https://www.cnblogs.com/chenice/p/6921727.html https://blog.csdn.net/Aaroun/article/details/78218131转载于:https://www.cnblogs.com/pythonClub/p/9746866.html

poj1741(树的点分治)

题目连接&#xff1a;POJ - 1741 看了好长时间才明白了点...... 网上讲解很多但感觉都不够详细。。。大概是太弱了吧-_-|| 学通了再回来写详解。。。 1 #include<iostream>2 #include<cstring>3 #include<cstdio>4 #include<algorithm>5 #define LL lo…

Android 串口通讯

概念 串行接口简称串口&#xff0c;也称串行通信接口或串行通讯接口&#xff08;通常指COM接口&#xff09;&#xff0c;是采用串行通信方式的扩展接口。串行接口&#xff08;Serial Interface&#xff09;是指数据一位一位地顺序传送。其特点是通信线路简单&#xff0c;只要一…

Linux07-OpenSSH

目录 一、使用SSH访问远程主机 1.1、什么是OpenSSH Secure Shell&#xff08;SSH&#xff09; 1.2、SSH主机密钥 二、配置基于SSH密钥的身份验证 2.1、基于SSH密钥的身份验证 2.2、自定义SSH服务配置 2.3、sftp传输文件 一、使用SSH访问远程主机 1.1、什么是OpenSSH Se…

ORB_SLAM2中的Sim3变换

对于双目、RGB-D相机&#xff0c;可获得深度&#xff0c;因此不存在尺度问题&#xff0c;因此Sim3中的尺度s1。 &#xff08;1&#xff09;通过词袋加速算法实现当前帧、闭环帧的特征点的匹配&#xff0c;建立闭环帧的路标点和当前帧的特征点间的联系。 &#xff08;2&#xff…