ORB-SLAM2 ---- Tracking::MonocularInitialization函数

目录

1.为什么需要地图初始化

2. 单目初始化的详细流程

1.为什么需要地图初始化

        在ORB-SLAM2中初始化和使用的传感器类型有关，其中单目相机模式初始化相对复杂，需要运行一段时 间才能成功初始化。

        而双目相机、RGB-D相机模式下比较简单，一般从第一帧开始就可以完成初始化。

        为什么不同传感器类型初始化差别这么大呢？ 我们从最简单的RGB-D相机初始化来说，因为该相机可以直接输出RGB图像和对应的深度图像，所以每个像素点对应的深度值是确定的，也就是说，我在第一帧提取了特征点后，特征点对应的三维点在空间的绝对坐标是可以计算出来的（需要用到内参）。

        对于双目相机来说，也可以通过第一帧左右目图像立体匹配来得到特征点对应的三维点在空间的绝对坐标。因为第一帧的三维点是作为地图来实现跟踪的，所以这些三维点我们也称为地图点。所以理论来说，双目相机、RGB-D相机在第一帧就可以完成初始化。

        而对于单目相机来说，仅仅有第一帧无法得到三维点，想要初始化，需要像双目相机那样去进行立体匹配。

        但单目只能得到相对尺度，双目和RGB-D相机可以得到绝对尺度。也就是说在双目里面想要测量两地图点的举例或者进行重建都是可以的，但单目得到相对尺度无法进行重建（尺度归一化的地图）。

2. 单目初始化的详细流程

void Tracking::MonocularInitialization()
{
    // Step 1 如果单目初始器还没有被创建，则创建。后面如果重新初始化时会清掉这个
    if(!mpInitializer)
    {
        // Set Reference Frame
        // 追踪线程的当前帧mCurrentFrame mvKeys是特征点数量 
        if(mCurrentFrame.mvKeys.size()>100)
        {
            // 初始化需要两帧，分别是mInitialFrame，mCurrentFrame
            mInitialFrame = Frame(mCurrentFrame);
            // 用当前帧更新上一帧
            mLastFrame = Frame(mCurrentFrame);
            // mvbPrevMatched  记录"上一帧"所有特征点
            mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
            for(size_t i=0; isize(); 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;
        }
    }
    else    //如果单目初始化器已经被创建
    {
        // Try to initialize
        // Step 2 如果当前帧特征点数太少（不超过100），则重新构造初始器
        // NOTICE 只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程
        if((int)mCurrentFrame.mvKeys.size()<=100)
        {
            delete mpInitializer;
            mpInitializer = static_cast(NULL);
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }
 
        // Find correspondences
        // Step 3 在mInitialFrame与mCurrentFrame中找匹配的特征点对
        ORBmatcher matcher(
            0.9,        //最佳的和次佳特征点评分的比值阈值，这里是比较宽松的，跟踪时一般是0.7，值越大匹配越宽松
            true);      //检查特征点的方向
 
        // 对 mInitialFrame,mCurrentFrame 进行特征点匹配
        // mvbPrevMatched为参考帧的特征SearchForInitialization点坐标，初始化存储的是mInitialFrame中特征点坐标，匹配后存储的是匹配好的当前帧的特征点坐标
        // mvIniMatches 保存参考帧F1中特征点是否匹配上，index保存是F1对应特征点索引，值保存的是匹配好的F2特征点索引
        int nmatches = matcher.SearchForInitialization(
            mInitialFrame,mCurrentFrame,    //初始化时的参考帧和当前帧
            mvbPrevMatched,                 //在初始化参考帧中提取得到的特征点
            mvIniMatches,                   //保存匹配关系
            100);                           //搜索窗口大小
 
        // Check if there are enough correspondences
        // Step 4 验证匹配结果，如果初始化的两帧之间的匹配点太少，重新初始化
        if(nmatches<100)
        {
            delete mpInitializer;
            mpInitializer = static_cast(NULL);
            return;
        }
 
        cv::Mat Rcw; // Current Camera Rotation
        cv::Mat tcw; // Current Camera Translation
        vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
 
        // Step 5 通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoints
        if(mpInitializer->Initialize(
            mCurrentFrame,      //当前帧
            mvIniMatches,       //当前帧和参考帧的特征点的匹配关系
            Rcw, tcw,           //初始化得到的相机的位姿
            mvIniP3D,           //进行三角化得到的空间点集合
            vbTriangulated))    //以及对应于mvIniMatches来讲,其中哪些点被三角化了
        {
            // Step 6 初始化成功后，删除那些无法进行三角化的匹配点
            for(size_t i=0, iend=mvIniMatches.size(); i
            {
                if(mvIniMatches[i]>=0 && !vbTriangulated[i])
                {
                    mvIniMatches[i]=-1;
                    nmatches--;
                }
            }
 
            // Set Frame Poses
            // 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();
        }//当初始化成功的时候进行
    }//如果单目初始化器已经被创建
}

        首先我们构造一个单目初始化器用于单目初始化，如果跟踪线程的当前帧的特征点（我们传入slam系统的图片是一帧一帧的，当输入第一帧图片时，我们对其进行单目初始化）

        我们判断第一帧图片的特征点数量mCurrentFrame.mvKeys.size是否满足初始化要求（大于100特征点），如果满足，由于初始化要求两帧我们将初始化的第一帧第二帧都初始化为第一帧图像。用mvbPrevMatched保存参考帧（第一帧）中的特征点。

std::vector<int> mvIniMatches;// 跟踪初始化时前两帧之间的匹配

        用第一帧初始化单目初始化器，并将初始化时前两帧之间的匹配设为-1表示无匹配，mvIniMatches容器的意思是如果第一帧的第一个特征点和第二帧的第五个特征点有匹配，则记mvIniMatches[0] = 4，然后退出初始化，等待下一帧的到来。

        当第二帧到来，检测到了单目初始化器的存在，检查第二帧的特征点数量是否满足slam系统的要求，若不满足则重新构造初始器（将初始化器的内存释放掉，用第三第四帧....去尝试初始化），若满足要求初始化ORB匹配器，对这两帧的特征点进行匹配，由于单目初始化要求的帧质量比较高（特征点要求多，特征匹配要求高，因为约束越多位姿越准确），因此如果特征点匹配的数量不能够达到验证匹配结果，如果初始化的两帧之间的匹配点太少，重新初始化。

        如果数量达到验证匹配结果，通过H模型或F模型进行单目初始化，得到两帧间相对运动、初始MapPoint，初始化成功后，删除那些无法进行三角化的匹配点。

        将初始化的第一帧作为世界坐标系，因此第一帧变换矩阵为单位矩阵，随后创建初始化地图点MapPoints。

        这些函数我都会在后文解释清楚的！