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

目录

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 Frame
        if(mCurrentFrame.mvKeys.size()>100)
        {
            mInitialFrame = Frame(mCurrentFrame);
            mLastFrame = Frame(mCurrentFrame);
            mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
            for(size_t i=0; i(NULL);
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }
        // Find correspondences
        ORBmatcher matcher(0.9,true);
        int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
        // Check if there are enough correspondences
        if(nmatches<100)
        {
            delete mpInitializer;
            mpInitializer = static_cast(NULL);
            return;
        }
        cv::Mat Rcw; // Current Camera Rotation
        cv::Mat tcw; // Current Camera Translation
        vector vbTriangulated; // Triangulated Correspondences (mvIniMatches)
        if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
        {
            for(size_t i=0, iend=mvIniMatches.size(); 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();
        }
    }
}

5 代码解析

5.1 检查是否有初始化器

    if(!mpInitializer)
    {
        // Set Reference Frame
        if(mCurrentFrame.mvKeys.size()>100)
        {
            mInitialFrame = Frame(mCurrentFrame);
            mLastFrame = Frame(mCurrentFrame);
            //分配存储空间，另mvbPrevMatched空间大小等于当前帧去畸变后的特征点数量
            mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
            for(size_t i=0; i

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

（1）作用

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

（2）原理

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

随机采样，

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

拟合模型，

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

验证一致性，

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

记录最佳结果，

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

迭代多次，

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

5.2 初始化

    else
    {
        // Try to initialize
        if((int)mCurrentFrame.mvKeys.size()<=100)
        {
            delete mpInitializer;
            mpInitializer = static_cast(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 correspondences
        if(nmatches<100)
        {
            delete mpInitializer;
            mpInitializer = static_cast(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 Rotation
        cv::Mat tcw; // Current Camera Translation
        vector 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=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 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();
        }
    }