Tracking线程
公有函数:
(1)
//构造函数。
Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,KeyFrameDatabase* pKFDB, const string &strSettingPath, const int sensor);
strSettingPath?其实就是十四讲中的config文件
//相机参数及ORBextractor()初始化
(2)
//预处理输入并调用Track()。 提取特征并执行立体匹配。
cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp);
cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp);
cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp);
(3)
//设置localmapping,localclosing和viewer
void SetLocalMapper(LocalMapping* pLocalMapper);
void SetLoopClosing(LoopClosing* pLoopClosing);
void SetViewer(Viewer* pViewer);
(4)
//加载新设置
//焦距应该相似,否则比例预测在投射点时会失败
// 修改MapPoint :: PredictScale以考虑焦点长度
void ChangeCalibration(const string &strSettingPath);
(5
//TrackMonocular()函数中使用
//如果您已取消激活localmapping并且只想定位摄像机,请使用此功能。
void InformOnlyTracking(const bool &flag);
公有数据:
(1)
//跟踪状态
enum eTrackingState
eTrackingState mState;
eTrackingState mLastProcessedState;
(2)
//输入传感器:MONOCULAR,STEREO,RGBD
int mSensor;
(3)
//当前帧
Frame mCurrentFrame; //这是所有图像帧的起点,由Frame()创建
cv::Mat mImGray;
(4)
// 初始化时前两帧相关变量(单目)
//MonocularInitialization()中使用下列数据
std::vector mvIniLastMatches;
std::vector mvIniMatches;// 跟踪初始化时前两帧之间的匹配
std::vectorcv::Point2f mvbPrevMatched;
std::vectorcv::Point3f mvIniP3D;
Frame mInitialFrame;
(5)
//列表用于在执行结束时恢复完整的摄像机轨迹。
//基本上我们存储每个帧的参考关键帧及其相对变换
listcv::Mat mlRelativeFramePoses;
list<KeyFrame*> mlpReferences;
list mlFrameTimes;
list mlbLost;
//Reset()中使用
//Track()的步骤三,这里是记录数据,进而在Reset()中使用
(6)
//如果取消激活local mapping并且我们仅执行本地化,则为True
bool mbOnlyTracking;
(7)
//重置系统。
void Reset();
//这里执行了具体的系统重置工作,与System中不同哦。
私有函数:
(1) 这里很关键哦
//主跟踪函数,取决于传感器类型。
void Track();
初始化:
//单目初始化MonocularInitialization();
//mpFrameDrawer->Update(this);
//注意其中的mState辨析
跟踪:
// bOK为临时变量,用于表示每个函数是否执行成功(bool)
//mbOnlyTracking决定是否实现仅定位功能。mbVO,是在没有后端建图时使用的bool量。
A:同时定位与地图构建
//CheckReplacedInLastFrame();注释:更新Fuse函数和SearchAndFuse函数替换的MapPoints
//最初运动模型是空的或刚完成重定位TrackReferenceKeyFrame();
//在有上一步速度的情况下,优先执行TrackWithMotionModel()
//丢失。则执行重定位Relocalization()
B:仅定位
//若丢失,执行重定位Relocalization()
//执行TrackWithMotionModel()或TrackReferenceKeyFrame();
//在上一帧匹配少的情况下,同时执行TrackWithMotionModel()和Relocalization()
在A或B结束后:
//更新当前帧的参考关键帧
//条件允许则执行TrackLocalMap();这里跟踪的是局部地图。不是后端的完整大地图。
执行帧绘制器更新:
//mpFrameDrawer->Update(this);
若Tracking的bOK为OK,则判断是否增加新的关键帧:
//更新速度模型。
补充:通过源码可知Tracking::MonocularInitialization()仅执行一次,并且会在执行中通过只执行一次的Tracking::CreateInitialMapMonocular(),设置mLastFrame,但从下一帧(第三帧图像)到当前帧(第二帧为关键帧)通过TrackReferenceKeyFrame()运行。另外,CreateInitialMapMonocular()还更新当前帧的参考关键帧。
//接着,调用mpMapDrawer->SetCurrentCameraPose(mCurrentFrame.Mtc
w);
//然后,清除UpdateLastFrame中为当前帧临时添加的MapPoints(单目不需要这个操作
//再然后,关键帧的c1a||c1b||c1c)&&c2判断。如果判断通过,但是后端在工作,则调用mpLocalMapper->InterruptBA();
//我猜测后端会有用mbAbortBA = true的时候,什么时候会设定mbAbortBA ???后端优化前会设置mbAbortBA =false然后才能优化,即猜测错误。
//再再然后,Tracking::CreateNewKeyFrame(),首先判断后端是否停止了,没停止再插入关键帧。
保存上一帧数据:
//记录位姿信息,用于轨迹复现。
(2)
//双目,深度相机地图初始化
void StereoInitialization();
(3)
//单目初始化
void MonocularInitialization();
void CreateInitialMapMonocular();
(4)
//Track()内要使用的一些函数
void CheckReplacedInLastFrame(); //检查并替代上一帧
bool TrackReferenceKeyFrame(); //跟踪参考关键帧,<10
void UpdateLastFrame(); //更新上一关键帧
//TrackWithMotionModel()中使用,对于单目仅更新最近一帧的位姿。双目还会产生一些跟踪的临时地图点(即不加入地图MAP)。
bool TrackWithMotionModel();//跟踪运动模型,<20
//速度模型累加成为当前速度,ORBmatcher::SearchByProjection(),上一帧3d点(记录在帧中)投影过来找到匹配,经匹配的点参与位姿计算。
bool Relocalization(); //追踪失效后的重定位
void UpdateLocalMap(); //更新局部地图
// TrackLocalMap()中使用
void UpdateLocalPoints();//更新局部点
void UpdateLocalKeyFrames(); //更新局部关键帧
//map<KeyFrame,int> keyframeCounter中记录看到当前帧地图点的各个关键帧及关键帧看到的地图点总数。
//mnTrackReferenceForFrame有两个作用。防止重复添加局部关键帧,防止重复添加局部地图点。
//补充:更新当前帧的参考关键帧,与自己共视程度最高(相同地图点多)的关键帧作为参考关键帧
bool TrackLocalMap(); //跟踪局部地图,<30,在跟踪中要求最高
//我们估计了相机姿态和帧中跟踪的一些地图点。
//我们检索本地地图并尝试查找与本地地图中的点匹配。*
void SearchLocalPoints();//搜索局部点,获得局部地图与当前帧的匹配
//调用mCurrentFrame.isInFrustum(pMP,0.5),判断LocalMapPoints中的点是否在在视野内
bool NeedNewKeyFrame();//需要新的关键帧
void CreateNewKeyFrame();//创建新的关键帧
(5)
//如果仅执行定位,则当与地图中的点没有匹配时,此标志为true。 如果有足够的临时点匹配,仍将继续跟踪。在那种情况下,我们会执行VO。
// 系统将尝试重新定位以恢复到地图的“零漂移”位置。
bool mbVO;
(6)
//其他线程指针
LocalMapping* mpLocalMapper;
LoopClosing* mpLoopClosing;
(7)
//ORB
// orb特征提取器,不管单目还是双目,mpORBextractorLeft都要用到
// 如果是双目,则要用到mpORBextractorRight
// 如果是单目,在初始化的时候使用mpIniORBextractor而不是mpORBextractorLeft,
// mpIniORBextractor属性中提取的特征点个数是mpORBextractorLeft的两倍
ORBextractor* mpORBextractorLeft, *mpORBextractorRight;
ORBextractor* mpIniORBextractor;
//在Frame()初始化时使用
//由Tracking()构造函数初始化
(8)
//BoW****加粗样式
ORBVocabulary* mpORBVocabulary;
KeyFrameDatabase* mpKeyFrameDB;
(9)
// Initalization (only for monocular)
// 单目初始器
Initializer* mpInitializer;
(10)
//Local Map
KeyFrame* mpReferenceKF;// 当前帧就是参考帧
std::vector<KeyFrame*> mvpLocalKeyFrames;
std::vector<MapPoint*> mvpLocalMapPoints;
(11)
//系统
System* mpSystem;
(12)
//画图
Viewer* mpViewer;
FrameDrawer* mpFrameDrawer;
MapDrawer* mpMapDrawer;
(13)
//地图
Map* mpMap;
(14)
//标定矩阵
cv::Mat mK;
cv::Mat mDistCoef;
float mbf; //双目相机参数
(15)
//新的KeyFrame规则(根据fps)
int mMinFrames;//0
int mMaxFrames;//30
(16)//双目、深度相机参数
//阈值近/远点
//被立体声/ RGBD传感器看得近的点被认为是可靠的并且仅从一帧插入。 远点需要两个关键帧匹配。
float mThDepth;
注:单目有没有远近点之分?会用这个参数么?
(17)//深度相机参数
//仅适用于RGB-D输入。 对于一些数据集(例如TUM),缩放深度图值。
float mDepthMapFactor;
(18)
//图像帧的当前匹配内点
int mnMatchesInliers;
(19)
//上一图像帧、关键帧和重定位信息
KeyFrame* mpLastKeyFrame;
Frame mLastFrame;
unsigned int mnLastKeyFrameId;
unsigned int mnLastRelocFrameId;
(20)
//运动模型
cv::Mat mVelocity;
(21)
//颜色顺序(真RGB,假BGR,如果灰度则忽略)
bool mbRGB;
(22)
//是双目才会用到
list<MapPoint*> mlpTemporalPoints;
localmapping线程
共有函数:
(1)
//构造函数。
LocalMapping(Map* pMap, const float bMonocular);
(2)
//设置localclosing和Tracking
void SetLoopCloser(LoopClosing* pLoopCloser);
void SetTracker(Tracking* pTracker);
(3)这里很关键哦
//主函数
void Run();
调用:LocalMapping::SetAcceptKeyFrames(bool flag)
//改为不接受新的关键帧,因为当前繁忙mbAcceptKeyFrames=flag;
调用:LocalMapping::CheckNewKeyFrames()
//判断待处理关键帧列表为非空
调用:ProcessNewKeyFrame();
//计算关键帧关键点BOW
//并且这里会调用KeyFrame::UpdateConnections()更新共视图关系,很关键好不啦~
//最后,关键帧会插入地图中。
调用:MapPointCulling();
//对地图点进行判断,对于单目,连续三个关键帧均被看到的点保留。
调用:CreateNewMapPoints()
// pKF2->ComputeSceneMedianDepth(2);计算共视帧的景深
//SearchForTriangulation(),在已知位姿和基础矩阵的条件下,进行匹配
//三角化后,进行是否在相机前以及重投影后的基于卡方检验计算出的阈值判断(假设测量有一个像素的偏差)
//尺度连续性判断,会与金字塔尺度进行判断,不太懂内涵,为毛判断这个。。
//构造地图点
调用:LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
//暂时不补充
调用: KeyFrameCulling();
//判断90%重复和尺度性条件,决定是否删除
//这并不是解决动态场景问题的措施。
(4)
//插入关键帧
void InsertKeyFrame(KeyFrame* pKF);
(5)
//线程同步
void RequestStop();
void RequestReset();
bool Stop();
void Release();
bool isStopped();
bool stopRequested();
bool AcceptKeyFrames();
void SetAcceptKeyFrames(bool flag);
bool SetNotStop(bool flag);
void InterruptBA();
void RequestFinish();
bool isFinished();
私有函数:
(1)
//检查、处理新的关键帧
bool CheckNewKeyFrames();
void ProcessNewKeyFrame();
(2)
//创建新的地图点
void CreateNewMapPoints();
(3)
//地图点裁剪、搜索邻居
void MapPointCulling();
void SearchInNeighbors();
(4)
//关键帧裁剪
void KeyFrameCulling();
(5)
cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2);//计算基础矩阵
cv::Mat SkewSymmetricMatrix(const cv::Mat &v); //反对称矩阵转换
(6)
//其他线程指针
LocalMapping* mpLocalMapper;
LoopClosing* mpLoopClosing;
(7)
//是否是单目
bool mbMonocular;
(8)
//重置
void ResetIfRequested();
bool mbResetRequested;
std::mutex mMutexReset;
(9)
//在ros_mono.cpp中负责localmapping线程的彻底结束,后面的是localmapping功能停止,但是Tracking功能还在
//检查完成,设置完成,完成请求
bool CheckFinish();
void SetFinish();
bool mbFinishRequested;//RequestFinish()中调用
bool mbFinished;//isFinished()中调用
std::mutex mMutexFinish;
(10)
//指针
Map* mpMap;
LoopClosing* mpLoopCloser;
Tracking* mpTracker;
(11)
// Tracking线程向LocalMapping中插入关键帧是先插入到该队列中
std::list<KeyFrame*> mlNewKeyFrames; ///< 等待处理的关键帧列表
(12)
//当前关键帧
KeyFrame* mpCurrentKeyFrame;
(13)
//最近添加的地图点,有可能从地图中删除的还不稳定的点
std::list<MapPoint*> mlpRecentAddedMapPoints;
std::mutex mMutexNewKFs;
(14)
bool mbAbortBA;//在RequestStop()中调用
//具体是,TrackMonocular()的Check mode change时可能进行调用
//Optimizer::LocalBundleAdjustment()的参数。
(15)
//停止
bool mbStopped;//isStopped()中调用
bool mbStopRequested;//在RequestStop()中调用
bool mbNotStop;
std::mutex mMutexStop;
(16)
//是否接受为关键帧
bool mbAcceptKeyFrames;
std::mutex mMutexAccept;
localclosing线程
略