ORB-SLAM2学习之Tracking线程（一）

最新推荐文章于 2023-12-03 16:19:14 发布

需要努力的阿蒙

最新推荐文章于 2023-12-03 16:19:14 发布

阅读量1.3k

点赞数 1

分类专栏： SLAM学习文章标签： SLAM学习

本文链接：https://blog.csdn.net/shenghuaijing3314/article/details/80791093

版权

SLAM学习专栏收录该内容

1 篇文章 0 订阅

订阅专栏

最近在研究ORB SLAM，ORB分为4个线程，Tracking、Local Mapping、Loop closing和Viewer。Tracking模块负责相机的初始化、跟踪、重定位以及确定关键帧；Local Mapping则负责了关键帧的添加、验证最近添加的Map point、生成新的Map point、局部的BA以及验证关键帧；Loop Closing负责了词袋法的相似帧的选取、闭环检测、跟新图以及全局的BA；最后的Viewer则负责了Map point的显示以及相机位姿的可视化。

Tracking线程

下面先详细的介绍一下Tracking线程。Tracking可以细分为4大块。初始化、跟踪、重定位和确定关键帧。博客参考了吴博的ORB-SLAM2源码详解以及泡泡slam在哔哩哔哩上的冯兵的ORB-SLAM讲解（冯兵讲解）等，在此表示感谢。

一、初始化

由于单目相机有尺度问题，所以需要初始化步骤，恢复出深度信息。初始化又可以细分。

1、选取满足要求的相邻两帧作为初始帧；

2、通过初始帧计算两帧之间的位姿；

3、通过三角测量恢复初始帧特征点的深度信息，转化为点云信息，也就是初步的Map point；

4、BA优化。

ORB首先在System类的构造函数中初始化了4个线程。

    //Initialize the Tracking thread
    //(it will live in the main thread of execution, the one that called this constructor)
    mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
                             mpMap, mpKeyFrameDatabase, strSettingsFile, mSensor); //初始化Tracking线程

    //Initialize the Local Mapping thread and launch
    mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
    mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);//初始化Local Mapping线程

    //Initialize the Loop Closing thread and launch
    mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
    mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);//初始化Loop Closing线程

    //Initialize the Viewer thread and launch
    mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile);
    if(bUseViewer)
        mptViewer = new thread(&Viewer::Run, mpViewer);//初始化Viewer线程

下面进入Tracking类里，通过GrabImageMonocular()函数将图像转化为灰度并同时初始化Frame类，下一步便是通过Track()函数进行跟踪，Track中首先判断相机有没有初始化，如果没有，那么调用MonocularIntialization函数来初始化相机。

void Tracking::MonocularInitialization()
{
    //这里通过改进的ORB类实例化一个对象，通过matcher对初始帧和第二帧进行匹配
    if(!mpInitializer)
    {
        // Set Reference Frame
        // 单目初始帧的特征点数必须大于100
        if(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++)
                mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;

            if(mpInitializer)
                delete mpInitializer;

            // 这里实例化Initialize类的一个对象，初始化的值为选取的当前帧和测量误差1.0，RANSAC迭代次数200
            mpInitializer =  new Initializer(mCurrentFrame,1.0,200);

            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);

            return;
        }
    }
    else
    {
        // Try to initialize
         //如果当前帧特征点数大于100，则得到用于单目初始化的第二帧
        // 如果当前帧特征点太少，重新构造初始器
        // 因此只有连续两帧的特征点个数都大于100时，才能继续进行初始化过程
        if((int)mCurrentFrame.mvKeys.size()<=100)
        {
            delete mpInitializer;
            mpInitializer = static_cast<Initializer*>(NULL);
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }

        // Find correspondences
        //这里通过改进的ORB类实例化一个对象，通过matcher对初始帧和第二帧进行匹配
        ORBmatcher matcher(0.9,true);
        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)

        // 调用Initializer类的Initialize函数对相机进行初始化，通过评分来选择基本矩阵F或者单应矩阵H初始化，之后分解得到前两帧之间的R,t以及Map point，同时判断是否可以三角话，vbTrangulated是一个bool，用于判断是否可以三角话。 
        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));
            // 由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);

            // 将三角化得到的3D点包装成MapPoints
            // Initialize函数会得到mvIniP3D，
            // mvIniP3D是cv::Point3f类型的一个容器，是个存放3D点的临时变量，
            // CreateInitialMapMonocular将3D点包装成MapPoint类型存入KeyFrame和Map中
            CreateInitialMapMonocular();
        }
    }
}

可以总结一下，通过MonocularIntialization函数，选取了符合条件的相邻两帧作为初始帧，通过改进的orb进行匹配，通过Initializer类来实现选择基本矩阵F还是单应矩阵H来初始化相机，再将初始化的第一帧作为坐标系初始位置，再获取最初的Map point。下面详细看一下上面主要调用的Initialize函数的实现。

bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,
                             vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
{  //首先我们看函数的参数，输入为当前帧CurrentFrame，前两帧的匹配Matches12，输出了旋转矩阵R21，平移矩阵t21，初始的Map point以及一个bool，用来判断是否可以三角话
    // Fill structures with current keypoints and matches with reference frame
    // Reference Frame: 1, Current Frame: 2
    // 
    mvKeys2 = CurrentFrame.mvKeysUn;

    mvMatches12.clear();
    mvMatches12.reserve(mvKeys2.size());

    mvbMatched1.resize(mvKeys1.size());

    for(size_t i=0, iend=vMatches12.size();i<iend; i++)
    {
        if(vMatches12[i]>=0)
        {
            mvMatches12.push_back(make_pair(i,vMatches12[i]));
            mvbMatched1[i]=true;
        }
        else
            mvbMatched1[i]=false;
    }

    const int N = mvMatches12.size();

    // Indices for minimum set selection
    vector<size_t> vAllIndices;
    vAllIndices.reserve(N);
    vector<size_t> vAvailableIndices;

    for(int i=0; i<N; i++)
    {
        vAllIndices.push_back(i);
    }

    // Generate sets of 8 points for each RANSAC iteration
    // mMaxIterations:200
    mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));//从匹配对中选取8个点对

    DUtils::Random::SeedRandOnce(0);

    for(int it=0; it<mMaxIterations; it++)
    {
        vAvailableIndices = vAllIndices;

        // Select a minimum set
        for(size_t j=0; j<8; j++)
        {
            int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
            int idx = vAvailableIndices[randi];

            mvSets[it][j] = idx;
            vAvailableIndices[randi] = vAvailableIndices.back();
            vAvailableIndices.pop_back();
        }
    }

    // Launch threads to compute in parallel a fundamental matrix and a homography
    vector<bool> vbMatchesInliersH, vbMatchesInliersF;
    float SH, SF; // score for H and F
    cv::Mat H, F; // H and F
    thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));//开启计算H矩阵的线程
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));//开启计算F矩阵的线程

    // Wait until both threads have finished
    threadH.join();
    threadF.join(); //等待线程完成

    // Compute ratio of scores
    float RH = SH/(SH+SF); //计算得分

    // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
    if(RH>0.40) //若大于0.4，则使用H
        return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
    else //if(pF_HF>0.6)//使用F
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);

    return false;
}

下面便是如何计算F和H的评分了，分别为FindHomography和FindFundamental函数。ORB在两个线程中并行计算H和F模型，

/**
 * @brief 计算单应矩阵
 *
 * 假设场景为平面情况下通过前两帧求取Homography矩阵(current frame 2 到 reference frame 1),并得到该模型的评分并得到该模型的评分
 */
void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
{
    // Number of putative matches
    const int N = mvMatches12.size();

    // Normalize coordinates
    // 将mvKeys1和mvKey2归一化到均值为0，一阶绝对矩为1，归一化矩阵分别为T1、T2
    vector<cv::Point2f> vPn1, vPn2;
    cv::Mat T1, T2;
    Normalize(mvKeys1,vPn1, T1);
    Normalize(mvKeys2,vPn2, T2);
    cv::Mat T2inv = T2.inv();

    // Best Results variables
    // 最终最佳的MatchesInliers与得分
    score = 0.0;
    vbMatchesInliers = vector<bool>(N,false);

    // Iteration variables
    vector<cv::Point2f> vPn1i(8);
    vector<cv::Point2f> vPn2i(8);
    cv::Mat H21i, H12i;
    // 每次RANSAC的MatchesInliers与得分
    vector<bool> vbCurrentInliers(N,false);
    float currentScore;

    // Perform all RANSAC iterations and save the solution with highest score
    for(int it=0; it<mMaxIterations; it++)
    {
        // Select a minimum set
        for(size_t j=0; j<8; j++)
        {
            int idx = mvSets[it][j];

            // vPn1i和vPn2i为匹配的特征点对的坐标
            vPn1i[j] = vPn1[mvMatches12[idx].first];
            vPn2i[j] = vPn2[mvMatches12[idx].second];
        }

        cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
        // 恢复原始的均值和尺度
        H21i = T2inv*Hn*T1;
        H12i = H21i.inv();

        // 利用重投影误差为当次RANSAC的结果评分
        currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);

        // 得到最优的vbMatchesInliers与score
        if(currentScore>score)
        {
            H21 = H21i.clone();
            vbMatchesInliers = vbCurrentInliers;
            score = currentScore;
        }
    }
}

/**
 * @brief 计算基础矩阵
 *
 * 假设场景为非平面情况下通过前两帧求取Fundamental矩阵(current frame 2 到 reference frame 1),并得到该模型的评分
 */
void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
{
    // Number of putative matches
    const int N = vbMatchesInliers.size();

    // Normalize coordinates
    vector<cv::Point2f> vPn1, vPn2;
    cv::Mat T1, T2;
    Normalize(mvKeys1,vPn1, T1);
    Normalize(mvKeys2,vPn2, T2);
    cv::Mat T2t = T2.t();

    // Best Results variables
    score = 0.0;
    vbMatchesInliers = vector<bool>(N,false);

    // Iteration variables
    vector<cv::Point2f> vPn1i(8);
    vector<cv::Point2f> vPn2i(8);
    cv::Mat F21i;
    vector<bool> vbCurrentInliers(N,false);
    float currentScore;

    // Perform all RANSAC iterations and save the solution with highest score
    for(int it=0; it<mMaxIterations; it++)
    {
        // Select a minimum set
        for(int j=0; j<8; j++)
        {
            int idx = mvSets[it][j];

            vPn1i[j] = vPn1[mvMatches12[idx].first];
            vPn2i[j] = vPn2[mvMatches12[idx].second];
        }

        cv::Mat Fn = ComputeF21(vPn1i,vPn2i);

        F21i = T2t*Fn*T1;

        // 利用重投影误差为当次RANSAC的结果评分
        currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);

        if(currentScore>score)
        {
            F21 = F21i.clone();
            vbMatchesInliers = vbCurrentInliers;
            score = currentScore;
        }
    }
}

未完待续。。。