本次主要讲解ORBSLAM2中单目初始化函数Tracking::MonocularInitialization
的框架
单目初始化函数中主要进行以下工作:
- 设置参考帧,由于单目初始化需要两帧,所以需要将第一帧设置为参考帧,并初始化一个初始化器
其中的初始化器构造函数如下// Step 1 如果单目初始器还没有被创建,则创建。后面如果重新初始化时会清掉这个 if(!mpInitializer) { // 设置参考帧 // 单目初始帧的特征点数必须大于100 if(mCurrentFrame.mvKeys.size()>100) { // 初始化需要两帧,分别是mInitialFrame,mCurrentFrame 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) delete mpInitializer; // 由当前帧构造初始器 sigma:1.0 iterations:200 mpInitializer = new Initializer(mCurrentFrame,1.0,200); // 初始化为-1 表示没有任何匹配。这里面存储的是匹配的点的id fill(mvIniMatches.begin(),mvIniMatches.end(),-1); return; } }
Initializer::Initializer(const Frame &ReferenceFrame, // 参考帧 float sigma, // 测量误差 int iterations) // RANSAC迭代次数 { //从参考帧中获取相机的内参数矩阵 mK = ReferenceFrame.mK.clone(); // 从参考帧中获取去畸变后的特征点 mvKeys1 = ReferenceFrame.mvKeysUn; //获取估计误差 mSigma = sigma; mSigma2 = sigma*sigma; //最大迭代次数 mMaxIterations = iterations; }
- 判断当前帧的特征点数目,如果特征点数目太少,则删除初始化器
// Step 2 如果当前帧特征点数太少(不超过100),则重新构造初始器 // NOTICE 只有连续两帧的特征点个数都大于100时,才能继续进行初始化过程 if((int)mCurrentFrame.mvKeys.size()<=100) { delete mpInitializer; mpInitializer = static_cast<Initializer*>(NULL); fill(mvIniMatches.begin(),mvIniMatches.end(),-1); return; }
- 构造一个特征点匹配器
// Step 3 在mInitialFrame与mCurrentFrame中找匹配的特征点对 ORBmatcher matcher( 0.9, //最佳的和次佳特征点评分的比值阈值,这里是比较宽松的,跟踪时一般是0.7 true); //检查特征点的方向
- 进行特征匹配
// Step 4 进行特征匹配 // 对 mInitialFrame,mCurrentFrame 进行特征点匹配 // mvbPrevMatched为参考帧的特征点坐标,初始化存储的是mInitialFrame中特征点坐标,匹配后存储的是匹配好的当前帧的特征点坐标 // mvIniMatches 保存参考帧F1中特征点是否匹配上,index保存是F1对应特征点索引,值保存的是匹配好的F2特征点索引 int nmatches = matcher.SearchForInitialization( mInitialFrame,mCurrentFrame, //初始化时的参考帧和当前帧 mvbPrevMatched, //在初始化参考帧中提取得到的特征点 mvIniMatches, //保存匹配关系 100); //搜索窗口大小
- 验证匹配结果,如果找到的匹配特征点对太少,则进行重新初始化
// Step 5 验证匹配结果,如果初始化的两帧之间的匹配点太少,重新初始化 if(nmatches<100) { delete mpInitializer; mpInitializer = static_cast<Initializer*>(NULL); return; }
- 通过H矩阵或者F矩阵进行初始化
// Step 6 通过H模型或F模型进行单目初始化,得到两帧间相对运动、初始MapPoints if(mpInitializer->Initialize( mCurrentFrame, //当前帧 mvIniMatches, //当前帧和参考帧的特征点的匹配关系 Rcw, tcw, //初始化得到的相机的位姿 mvIniP3D, //进行三角化得到的空间点集合 vbTriangulated)) //以及对应于mvIniMatches来讲,其中哪些点被三角化了
- 初始化成功后,删除无法进行三角化的特征点
// Step 7 初始化成功后,删除那些无法进行三角化的匹配点 for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++) { if(mvIniMatches[i]>=0 && !vbTriangulated[i]) { mvIniMatches[i]=-1; nmatches--; } }
- 设置初始化的参考帧和当前帧的位姿,参考帧作为世界坐标系
// Step 7 将初始化的第一帧作为世界坐标系,因此第一帧变换矩阵为单位矩阵 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);
- 创建初始化的地图点
// Step 8 创建初始化地图点MapPoints // Initialize函数会得到mvIniP3D, // mvIniP3D是cv::Point3f类型的一个容器,是个存放3D点的临时变量, // CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中 CreateInitialMapMonocular();