ORB-SLAM2单目初始化过程
来源:互联网 发布:什么是网络直销模式 编辑:程序博客网 时间:2024/06/06 03:15
在ORB-SLAM2进行跟踪前,需要先进行初始化,初始化包括估计相机初始帧位姿,新建地图,新建关键帧等。初始化代码如下:
// Tracking.cc line 279~290 if(mState==NOT_INITIALIZED) { if(mSensor==System::STEREO || mSensor==System::RGBD) StereoInitialization(); else MonocularInitialization(); mpFrameDrawer->Update(this); if(mState!=OK) return; }
我们对单目初始化进行分析,单目初始化函数为void Tracking::MonocularInitialization()
。
单目初始化是试图从初始的两帧单目图像中,对SLAM系统进行初始化,以便之后进行跟踪。
寻找匹配点
对前两帧图像帧提取完ORB特征后,对特征点进行匹配:
// Tracking.cc line:600 function: void Tracking::MonocularInitialization()int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
条件 :检测特征匹配对个数是否大于等于100,若小于100,则初始化失败。
从匹配点中恢复初始相机位姿
在得到超过100个匹配点对后,运用RANSAC 8点法同时计算单应矩阵和基础矩阵,并选择最合适的结果作为相机的初始位姿。
(1) 计算单应矩阵cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
,并进行评分currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
,分别通过单应矩阵将第一帧的特征点投影到第二帧,以及将第二帧的特征点投影到第一帧,计算两次重投影误差,叠加作为评分SH。
(2) 计算基础矩阵cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
,并进行评分currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
,两次重投影误差叠加作为评分SF。
(3) 计算评分比:float RH = SH/(SH+SF);
,如果RH>0.4,选择单应矩阵来恢复相机位姿,否则选择基础矩阵来恢复相机位姿。
(4) 恢复相机位姿
从基础矩阵和相机内参可以得到本质矩阵:
// Initializer.cc line:479 function: bool Initializer::ReconstructF() // Compute Essential Matrix from Fundamental Matrix cv::Mat E21 = K.t()*F21*K;
从本质矩阵E21分解会得到4组可能的R,t,需要对这些解进行检查调用CheckRT()
.
检查1:根据R,t对特征点进行三角测量得到对应的三维地图点,检查地图点各个坐标值是否是有限,如果是无限的,标记该特征点为无效;
检查2:计算视差,即三维地图点与两帧相机中心连线的夹角的cos值需要小于0.99998,且地图点的深度必须大于等于0;
检查3:根据R,t计算双向重投影误差,需要小于预先设定的阈值
统计满足以上三步检查的特征点个数得到nGood,对应4组R,t得到4个nGood.
条件 :某个nGood大于阈值int nMinGood = max(static_cast<int>(0.9*N),minTriangulated);
且与其他nGood有较大区别时,同时有较好视差时,选择该nGood对应的R,t作为最终结果,否则初始化失败。
创建初始地图
如果恢复相机初始位姿成功,那么我们能得到前两帧图像的位姿,以及三角测量后的3维地图点。之后进行如下操作:
// Tracking.cc line:614 function:Tracking::MonocularInitialization() if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated)){ 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)); 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); CreateInitialMapMonocular();}
我们主要关注CreateInitialMapMonocular()
函数,其通过前两帧图像的位姿以及3维地图点,创建初始地图,并进行相应的数据关联操作:
(1)创建关键帧
因为只有前两帧,所以将这两帧都设置为关键帧,并计算对应的BoW,并在地图中插入这两个关键帧。
(2)数据关联
根据3维地图点,创建对应的地图点,将地图点与关键帧,地图进行数据关联:
地图点与关键帧关联
一个地图点可被多个关键帧观测到,将观测到这个地图点的关键帧与这个地图点进行关联,同时记录关键帧上哪一个特征点与这个地图点有关联。对于单目初始化来说,地图点需要关联第一步创建的两个关键帧。
// Tracking.cc line:665 function: void Tracking::CreateInitialMapMonocular() pMP->AddObservation(pKFini,i);pMP->AddObservation(pKFcur,mvIniMatches[i]);
地图点与关键帧上的特征点关联后,计算最合适的描述子来描述该地图点,用于之后跟踪的匹配。
关键帧与地图点关联
一个关键帧上的特征点由多个地图点投影而成,将关键帧与地图点关联。
// Tracking.cc line:671 function: void Tracking::CreateInitialMapMonocular() //Fill Current Frame structuremCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;
关键帧与关键帧关联
关键帧之间会共视一个地图点,如果共视的地图点个数越多,说明这两个关键帧之间的联系越紧密。对于某个关键帧,统计其与其他关键帧共视的特征点个数,如果大于某个阈值,那么将这两个关键帧进行关联。
// Update ConnectionspKFini->UpdateConnections();pKFcur->UpdateConnections();
将关键帧和地图点加入到地图中
(3)Global BA
对上述只有两个关键帧和地图点的地图进行全局BA。
(4) 地图尺寸初始化
由于单目SLAM的的地图点坐标尺寸不是真实的,比如x=1可能是1m也可能是1cm。选择地图点深度的中位数作为单位尺寸1当作基准来进行地图的尺寸初始化。
- ORB-SLAM2单目初始化过程
- ORB SLAM2 ROS实例 单目摄像头
- ORB-SLAM2搭建详细过程
- ORB-SLAM2搭建详细过程
- ORB-SLAM2搭建详细过程
- ORB-SLAM2(单目slam、ROS环境)安装总结
- Ubuntu下使用单目相机运行ORB SLAM2
- ROS下orb-slam2 单目 (二)
- ORB SLAM2 ROS单目摄像头环境搭建
- ORB-Slam2详解3 地图初始化
- ORB-SLAM2
- ORB-SLAM2
- 怎么在ubuntu下装ORB-SLAM2,并用单目摄像头实时跑起来
- 怎么在ubuntu下装ORB-SLAM2,并用单目摄像头实时跑起来
- ORB-SLAM2详解(三)自动地图初始化
- ORB-SLAM2编译过程中遇到libavcodec.a问题解决
- ORB-SLAM2实现
- ORB-SLAM2源码详解
- Makefile文件编写及make命令使用简介
- 1013. 数素数 (20)
- 时频分析要点整理
- cocos2dx lua 触摸事件+计时器
- Ftrl算法和FFM算法 广告点击率预测
- ORB-SLAM2单目初始化过程
- Maven详解(八)------ 继承和聚合
- PAT乙级1054. 求平均值 (20)
- Hbase基础原理
- SELinux的策略与规则管理相关命令
- python3多进程实现
- .h头文件、 .lib库文件、 .dll动态链接库文件之间的关系
- Hbase过滤器(Filter)汇总
- 什么是webhook