当前位置：首页 > news >正文

ORB_SLAM2原理及代码解析：Tracking::MonocularInitialization() 函数

news 2025/10/4 7:41:17

目录

1 作用

2 参数及含义说明

3 代码逻辑

4 代码

5 代码解析

5.1 检查是否有初始化器

5.1.1 随机采样一致性算法：RANdom SAmple Consensus（RANSAC）

（1）作用

（2）原理

5.2 初始化

5.3 当前帧和参考帧中找匹配点

5.3.1 最近邻比值阈值（nnratio）

5.3.2 参数说明

5.4 调用初始化器估计R、t，三角化

5.5 设置位姿、建立初始化地图

1 作用

单目相机初始化：

（1）找到合适的两帧（参考帧 + 当前帧）、估计相机的初始运动（R, t）；

（2）三角化生成第一批 3D 地图点；

（3）构建初始地图。

2 参数及含义说明

3 代码逻辑

（1）第一次检测到足够特征点（>100） → 保存为 mInitialFrame，并构造初始化器Initializer。

（2）后续帧进入初始化阶段

用 ORBmatcher 在 mInitialFrame 和 mCurrentFrame 之间找匹配点。

如果匹配数不足 100 → 初始化失败，清空。

如果匹配足够 → 用 Initializer->Initialize() 估计 Rcw 和 tcw，并三角化得到初始 3D 点。

设置初始帧和当前帧的位姿，调用 CreateInitialMapMonocular() 建图。

4 代码

void Tracking::MonocularInitialization()
{if(!mpInitializer){// Set Reference Frameif(mCurrentFrame.mvKeys.size()>100){mInitialFrame = Frame(mCurrentFrame);mLastFrame = Frame(mCurrentFrame);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;mpInitializer =  new Initializer(mCurrentFrame,1.0,200);fill(mvIniMatches.begin(),mvIniMatches.end(),-1);return;}}else{// Try to initializeif((int)mCurrentFrame.mvKeys.size()<=100){delete mpInitializer;mpInitializer = static_cast<Initializer*>(NULL);fill(mvIniMatches.begin(),mvIniMatches.end(),-1);return;}// Find correspondencesORBmatcher matcher(0.9,true);int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);// Check if there are enough correspondencesif(nmatches<100){delete mpInitializer;mpInitializer = static_cast<Initializer*>(NULL);return;}cv::Mat Rcw; // Current Camera Rotationcv::Mat tcw; // Current Camera Translationvector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)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 PosesmInitialFrame.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();}}
}

5 代码解析

5.1 检查是否有初始化器

    if(!mpInitializer){// Set Reference Frameif(mCurrentFrame.mvKeys.size()>100){mInitialFrame = Frame(mCurrentFrame);mLastFrame = Frame(mCurrentFrame);//分配存储空间，另mvbPrevMatched空间大小等于当前帧去畸变后的特征点数量mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)//.pt二维像素坐标，将当前帧归一化去畸变后的像素坐标存储在mvbPrevMatchedmvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;if(mpInitializer)delete mpInitializer;//RANSAC 的噪声阈值 (1.0 像素) ，迭代次数 (200)mpInitializer =  new Initializer(mCurrentFrame,1.0,200);//将mvIniMatches中所有元素全部置为-1fill(mvIniMatches.begin(),mvIniMatches.end(),-1);return;}}

5.1.1 随机采样一致性算法：RANdom SAmple Consensus（RANSAC）

（1）作用

从数据里“挖出”一组可靠的内点（inliers），用它们来拟合正确的模型，并自动剔除外点。

（2）原理

求解对极几何E矩阵为例：

随机采样，

从匹配点集中随机选取最小数量的点（比如 8 点法 → 取 8 对点）；

拟合模型，

用这组点计算一个候选的E矩阵；

验证一致性，

用这个E矩阵检查所有匹配点，看有多少点满足约束（误差 < 阈值）。符合约束的点称为内点；

记录最佳结果，

如果内点数比之前的最好结果还多 → 更新最佳模型；

迭代多次，

重复 1~4（比如 200 次），最终选出包含最多内点的模型。

5.2 初始化

    else{// Try to initializeif((int)mCurrentFrame.mvKeys.size()<=100){delete mpInitializer;mpInitializer = static_cast<Initializer*>(NULL);fill(mvIniMatches.begin(),mvIniMatches.end(),-1);return;}
5.1、5.2综合起来看，如果没有初始化器则找到一帧特征点大于100的作为初始化器的建立，如果有初始化器但当前帧特征点数小于100，则删除重新找。

5.3 当前帧和参考帧中找匹配点

        // Find correspondences//根据ORB描述子在两帧图像之间寻找对应点，最近邻比值阈值0.9，检查特征点方向是否一致ORBmatcher matcher(0.9,true);int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);// Check if there are enough correspondencesif(nmatches<100){delete mpInitializer;mpInitializer = static_cast<Initializer*>(NULL);return;}

5.3.1 最近邻比值阈值（nnratio）

（1）对一个特征点找出在另一帧中的最近邻（距离 d1）和次近邻（距离 d2），该距离即汉明距离(Hamming Distance)；

（2）如果 d1 < nnratio * d2，就认为匹配有效。

5.3.2 参数说明

（1）mInitialFrame

参考帧，保存了参考帧的关键点、ORB描述子

（2）mCurrentFrame

当前帧，要和参考帧匹配

（3）mvbPrevMatched

参考帧中特征点在上一帧的位置，初始化时将当前帧的关键点位置给mvbPrevMatched，连续跟踪mvbPrevMatched是参考帧的关键点在当前帧中的预测位置，由运动模型计算。

（4）mvIniMatches

参考帧在当前帧中的匹配情况

（5）100

匹配的范围，在附近 100 像素范围内搜索可能的匹配点

PS.SearchForInitialization() 函数详见：

https://blog.csdn.net/weixin_45728280/article/details/152455350?sharetype=blogdetail&sharerId=152455350&sharerefer=PC&sharesource=weixin_45728280&spm=1011.2480.3001.8118

5.4 调用初始化器估计R、t，三角化

        cv::Mat Rcw; // Current Camera Rotationcv::Mat tcw; // Current Camera Translationvector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)//用参考帧 + 当前帧 → 估计相机运动（R,t）并恢复初始 3D 点云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--;}}

PS.Initialize() 函数详见：

https://blog.csdn.net/weixin_45728280/article/details/152455699?sharetype=blogdetail&sharerId=152455699&sharerefer=PC&sharesource=weixin_45728280&spm=1011.2480.3001.8118

5.5 设置位姿、建立初始化地图

            // Set Frame PosesmInitialFrame.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();}}

查看全文

http://www.dtcms.com/a/438721.html