ORB-SLAM2单目初始化过程

Posted SKINNNNN

tags:

篇首语:本文由小常识网(cha138.com)小编为大家整理,主要介绍了ORB-SLAM2单目初始化过程相关的知识,希望对你有一定的参考价值。

在ORB-SLAM2进行跟踪前,需要先进行初始化,初始化包括估计相机初始帧位姿,新建地图,新建关键帧等。初始化代码如下:

  // Tracking.cc line 279~290
  if(mState==NOT_INITIALIZED)
    
        if(mSensor==System::STEREO || mSensor==System::RGBD)
            StereoInitialization();
        else
            MonocularInitialization();

        mpFrameDrawer->Update(this);

        if(mState!=OK)
            return;
    

我们对单目初始化进行分析,单目初始化函数为void Tracking::MonocularInitialization()

单目初始化是试图从初始的两帧单目图像中,对SLAM系统进行初始化,以便之后进行跟踪。

寻找匹配点

对前两帧图像帧提取完ORB特征后,对特征点进行匹配:

// Tracking.cc line:600 function: void Tracking::MonocularInitialization()
int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

条件 :检测特征匹配对个数是否大于等于100,若小于100,则初始化失败。

从匹配点中恢复初始相机位姿

在得到超过100个匹配点对后,运用RANSAC 8点法同时计算单应矩阵和基础矩阵,并选择最合适的结果作为相机的初始位姿。

(1) 计算单应矩阵cv::Mat Hn = ComputeH21(vPn1i,vPn2i);,并进行评分currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);,分别通过单应矩阵将第一帧的特征点投影到第二帧,以及将第二帧的特征点投影到第一帧,计算两次重投影误差,叠加作为评分SH。

(2) 计算基础矩阵cv::Mat Fn = ComputeF21(vPn1i,vPn2i);,并进行评分currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);,两次重投影误差叠加作为评分SF。

(3) 计算评分比:float RH = SH/(SH+SF);,如果RH>0.4,选择单应矩阵来恢复相机位姿,否则选择基础矩阵来恢复相机位姿。

(4) 恢复相机位姿

从基础矩阵和相机内参可以得到本质矩阵:

// Initializer.cc line:479 function: bool Initializer::ReconstructF()    
// Compute Essential Matrix from Fundamental Matrix
    cv::Mat E21 = K.t()*F21*K;

从本质矩阵E21分解会得到4组可能的R,t,需要对这些解进行检查调用CheckRT().

检查1:根据R,t对特征点进行三角测量得到对应的三维地图点,检查地图点各个坐标值是否是有限,如果是无限的,标记该特征点为无效;

检查2:计算视差,即三维地图点与两帧相机中心连线的夹角的cos值需要小于0.99998,且地图点的深度必须大于等于0;

检查3:根据R,t计算双向重投影误差,需要小于预先设定的阈值

统计满足以上三步检查的特征点个数得到nGood,对应4组R,t得到4个nGood.

条件 :某个nGood大于阈值int nMinGood = max(static_cast<int>(0.9*N),minTriangulated);且与其他nGood有较大区别时,同时有较好视差时,选择该nGood对应的R,t作为最终结果,否则初始化失败。

创建初始地图

如果恢复相机初始位姿成功,那么我们能得到前两帧图像的位姿,以及三角测量后的3维地图点。之后进行如下操作:

// Tracking.cc line:614 function:Tracking::MonocularInitialization()   
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
  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();

我们主要关注CreateInitialMapMonocular()函数,其通过前两帧图像的位姿以及3维地图点,创建初始地图,并进行相应的数据关联操作:

(1)创建关键帧

因为只有前两帧,所以将这两帧都设置为关键帧,并计算对应的BoW,并在地图中插入这两个关键帧。

(2)数据关联

根据3维地图点,创建对应的地图点,将地图点与关键帧,地图进行数据关联:

  • 地图点与关键帧关联

    一个地图点可被多个关键帧观测到,将观测到这个地图点的关键帧与这个地图点进行关联,同时记录关键帧上哪一个特征点与这个地图点有关联。对于单目初始化来说,地图点需要关联第一步创建的两个关键帧。

    // Tracking.cc line:665 function: void Tracking::CreateInitialMapMonocular() 
    pMP->AddObservation(pKFini,i);
    pMP->AddObservation(pKFcur,mvIniMatches[i]);

    ​地图点与关键帧上的特征点关联后,计算最合适的描述子来描述该地图点,用于之后跟踪的匹配。

  • 关键帧与地图点关联

    一个关键帧上的特征点由多个地图点投影而成,将关键帧与地图点关联。

    // Tracking.cc line:671 function: void Tracking::CreateInitialMapMonocular() 
    //Fill Current Frame structure
    mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
    mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;
  • 关键帧与关键帧关联

    关键帧之间会共视一个地图点,如果共视的地图点个数越多,说明这两个关键帧之间的联系越紧密。对于某个关键帧,统计其与其他关键帧共视的特征点个数,如果大于某个阈值,那么将这两个关键帧进行关联。

    // Update Connections
    pKFini->UpdateConnections();
    pKFcur->UpdateConnections();
  • 将关键帧和地图点加入到地图中

(3)Global BA

​ 对上述只有两个关键帧和地图点的地图进行全局BA。

(4) 地图尺寸初始化

​ 由于单目SLAM的的地图点坐标尺寸不是真实的,比如x=1可能是1m也可能是1cm。选择地图点深度的中位数作为单位尺寸1当作基准来进行地图的尺寸初始化。

以上是关于ORB-SLAM2单目初始化过程的主要内容,如果未能解决你的问题,请参考以下文章

ORB-SLAM2初步(跟踪模块)

光流算法:关于根据光流(单目)或视差(双目)进行图像变换的原理

怎么在ubuntu下装orb-slam2,并用单目摄像头实时跑起来

怎么在ubuntu下装ORB-SLAM2,并用单目摄像头实时跑起来

ORB-SLAM优化

如何评价ORB-SLAM3?