ORB_SLAM2学习笔记（3）——单目初始化（1）

ORB_SLAM2学习笔记（3）——单目初始化

代码如下（文件Tracking.cc中）：

//单目初始化
void Tracking::MonocularInitialization()
{

    if(!mpInitializer)//如果为空
    {
        // Set Reference Frame 特征点数目>100才能初始化
        if(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;
            //Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations)
            //当前帧 方差 容量
            mpInitializer =  new Initializer(mCurrentFrame,1.0,200);
            //mvIniMatches中存储原始帧与当前帧匹配的索引值 -1 就是让其他位置空着
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);

            return;
        }
    }
    else
    {
        // Try to initialize 尝试初始化
        //如果当前帧的特征点的数量<100
        if((int)mCurrentFrame.mvKeys.size()<=100)
        {
            //清除初始化内容
            delete mpInitializer;
            //重新初始化
            mpInitializer = static_cast<Initializer*>(NULL);
            //把新的匹配点的索引加入初始化中 更新参考帧
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }

        // Find correspondences
        //第一个参数：最佳匹配与次佳匹配的比值，默认为 0.6；第二个参数：是否检查特征点的方向.
        ORBmatcher matcher(0.9,true);
        //输入参数:mInitialFrame,mCurrentFrame这是待匹配的两帧图片
        //mvbPrevMatched为预匹配点坐标,函数运算后将会更新为真正的匹配点坐标
        //mvIniMatches为mInitialFrame帧中的特征点在mCurrentFrame帧中的匹配点的索引
        //100为搜索的区域边长,源码中是正方形的边长
        int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

        // Check if there are enough correspondences
        //如果匹配到的点<100 再次刷新重新匹配 换掉参考点
        if(nmatches<100)
        {
            delete mpInitializer;
            mpInitializer = static_cast<Initializer*>(NULL);
            return;
        }
        //世界到相机的选装矩阵
        cv::Mat Rcw; // Current Camera Rotation
        //世界到相机的位移
        cv::Mat tcw; // Current Camera Translation
        vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
        //mvIniMatches:帧1特征点在帧2中的匹配
        //Rcw,tcw:待求相机位姿
        //mvIniP3D:成功匹配点的空间3D坐标
        //vbTriangulated:成功三角测量--ture,三角测量失败--false
        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//初始pose设为单位矩阵 因为没动
            mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
            //初始T 单位矩阵 没动
            cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
            //提取出T中的R
            Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
            //提取出T中的t
            tcw.copyTo(Tcw.rowRange(0,3).col(3));
            //更新pose
            mCurrentFrame.SetPose(Tcw);
            //创建初始化 空间点
            CreateInitialMapMonocular();
        }
    }
}

接下来是转到Initialize函数,大致思路就是先随机选点，然后进行评分选择用E计算还是F计算
Initialize函数代码详解