ORB SLAM2学习笔记之mono_kitti(四)

ORB SLAM2 专栏收录该内容
4 篇文章 1 订阅

探索Track函数之MonocularInitialization(单目初始化)

Tracking.cpp里面的Track()函数可以说是 Tracking 线程最主体部分了,来看看调用Track()函数的过程:
在这里插入图片描述
Tracking线程中初始化函数用的是MonocularInitialization(),在第一次进入这个函数之前,初始化了mpInitializer(单目初始器)赋值为null,所以一开始会进入下面的if语句:

void Tracking::MonocularInitialization()
{

    //(第一次进入函数时)初始化tracking时赋了null
    //(第二次进入函数时)第一次新建了mpInitializer,所以执行else
    if(!mpInitializer)
    {
        // Set Reference Frame
        // 单目初始帧的特征点数必须大于100
        if(mCurrentFrame.mvKeys.size()>100)
        {

            //初始化mono的时候,初始帧和上一帧都是当前帧赋值
            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);  //先都填上-1 现在还没匹配呢

            return;
        }
    }

第二次进入这个函数的时候已经创建了一个mpInitializer,所以会进入else语句:

    else
    {
        // Try to initialize
        if((int)mCurrentFrame.mvKeys.size()<=100)
        {
            delete mpInitializer;
            mpInitializer = static_cast<Initializer*>(NULL);
            fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
            return;
        }

        // Find correspondences
        ORBmatcher matcher(0.9,true);

        // mvbPrevMatched为前一帧的特征点,存储了mInitialFrame中哪些点将进行接下来的匹配
        // mvIniMatches存储mInitialFrame,mCurrentFrame之间匹配的特征点,键值对是两帧匹配特征点的索引
        // mvbPrevMatched,mvIniMatches获得更新
        int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

        // Check if there are enough correspondences
        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)

        //如果初始化成功,intializer得到Rcw, tcw
        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);

            //Tcw赋值给mCurrentFrame
            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();
        }
    }
}

第一次进函数

先看if里面的,利用当前帧 mCurrentFrame创建了两个Frame对象,分别是 mInitialFrame(初始帧)和 mLastFrame(上一帧),课可见一开始的时候,上一帧就是当前帧。

mInitialFrame = Frame(mCurrentFrame);
mLastFrame = Frame(mCurrentFrame);

mvIniMatches用于存储 mInitialFrame, mCurrentFrame之间匹配的特征点,键值对是两帧匹配特征点的索引,所以初始化 mpInitializer后还要给 mvIniMatches填上 -1,以记录现在还未初始化。

第二次进函数

第二次进函数就已经有了两帧图像了,分别是 mInitialFrame(初始帧第一帧), mCurrentFrame(当前帧第二帧),有些细节比如判特征点数量的之类的就不说了,接下来初始化一个matcher,用于匹配,参数分别为mfNNratio(匹配分数设置)和mbCheckOrientation(要不要检查方向)。接下来开始匹配,函数最后得到的nmatches记录了匹配数:

int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);

参数:mvbPrevMatched为前一帧的特征点,存储了mInitialFrame中哪些点将进行接下来的匹配。mvIniMatches存储 mInitialFrame, mCurrentFrame之间匹配的特征点,键值对是两帧匹配特征点的索引
好,接下来mpInitializer要开始初始化了:

mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated

mpintializer初始化后,如果成功,会得到Rcw(旋转阵), tcw(平移向量), mvIniP3D(地图点的三维信息),成功后会进入一个if语句中,这个先不谈,先看这个Initialize函数:

Initialize函数

Initialize函数属于Initializer类,这是一个跟初始化相关的类,函数大致是讲计算 基础矩阵(F)单应性矩阵(H),然后选取其中一个模型,恢复出最开始两帧之间的相对姿态以及3d点。主要的步骤是下面这些:

① 组织特征点对:

mvMatches12.reserve(mvKeys2.size()); //mvKeys2:当前帧特征点
mvbMatched1.resize(mvKeys1.size()); //mvKeys1:参考帧特征点

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
    // 新建一个容器vAllIndices,生成0到N-1的数作为特征点的索引
    vector<size_t> vAllIndices;
    vAllIndices.reserve(N);
    vector<size_t> vAvailableIndices;

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

vMatches12就是前面说的 mvIniMatches,而 mvMatches12则是装了一个个 初始、当前帧的match 的 vector。vAllIndices 是装了所有match的索引(一个索引对应一个pair)

② 在所有匹配特征点对中随机选择8对匹配特征点:

    mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));

    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();
        }
    }

迭代赋值,采取RANSAC的方法每组随机取八个点,取mMaxIterations组。mvSets是一个装有mMaxIterations这么多个vector的vector,它装载的vector中是随机的八个点的索引。
③ 调用多线程分别用于计算fundamental matrixhomography

vector<bool> vbMatchesInliersH, vbMatchesInliersF;
    float SH, SF;
    cv::Mat H, F;

    thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
    thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(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)
        return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50); //得到R21,t21
    else //if(pF_HF>0.6)
        return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50); //得到R21,t21

利用FindHomographyFindFundamental多线程计算H和F,RH是两个矩阵的评分,高于一个阈值就选用其中的一个。ReconstructHReconstructF利用之前得到的H和F解算位姿R21,t21,并三角化计算3d点vP3D,看了一下代码结构,vP3D这个量应该是第一帧初始帧下的坐标,但这时相机光心坐标设置为0,所以vP3D应该是世界坐标。
FindHomography中运用了个技巧——归一化直接线性计算(Normalized DLT),下面是MVG的PPT上讲的算法流程:
在这里插入图片描述
之前对这个归一化疑惑了很久参考了一下PPT,好像是蒙特卡洛仿真效果比较好见下图。路过的大神能稍微解释一下吗~~
在这里插入图片描述
FindFundamental就没仔细看了。。。反正就是算F矩阵呗。。太菜了看不懂不看了。。
至此,Initialize 函数任务完成,得到了两帧之间的相对姿态以及3d点坐标和每个点三角化的成功与否的vector。

剔除没有三角化的点

Initialize 函数执行完并且return true后,进入if语句,首先进行的是根据返回的 vbTriangulated 剔除没有三角化成功的点。

  for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
  {
       if(mvIniMatches[i]>=0 && !vbTriangulated[i])
      {
            mvIniMatches[i]=-1;
            nmatches--;
      }
  }

设置帧的Pose

详情见注释

 // 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);

CreateInitialMapMonocular()

经过上面不痛不痒的一些操作,接下来进入大头部分…根据三角化后的3d点进行MapPoints的包装。CreateInitialMapMonocular函数适合分段讲,层次比较清晰。

第一步:将初始帧和当前帧转化成关键帧,KeyFrame类是关键帧类:

KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

包括各种参数的传递,传递格子grid的信息,设置关键帧的Pose。mpMapmpKeyFrameDB在System初始化的时候已经new过了。

第二步:将初始和当前关键帧的描述子转为BoW

pKFini->ComputeBoW();
pKFcur->ComputeBoW();

第三步:将关键帧插入到地图

mpMap->AddKeyFrame(pKFini);
mpMap->AddKeyFrame(pKFcur); //mspKeyFrames获得更新

mspKeyFrames是一个set类,详情请见set类参考网站

第四步:将3D点包装成MapPoints

for(size_t i=0; i<mvIniMatches.size();i++)
    {
        if(mvIniMatches[i]<0)
            continue;

        //Create MapPoint.
        //mvIniP3D是第一张初始帧的相机坐标系下的点,由于第一个是世界坐标系所以就是世界坐标系下的点
        cv::Mat worldPos(mvIniP3D[i]);

        //用3D点构造MapPoint
        MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);

        // 初始帧一个 i 对应当前帧一个 mvIniMatches[i],都是索引
        // 把用 mvIniP3D 初始化好的 MapPoint 分别送入初始帧和当前帧的 mvpMapPoints 中
        pKFini->AddMapPoint(pMP,i);
        pKFcur->AddMapPoint(pMP,mvIniMatches[i]);

        // 对该MapPoint添加观测
        pMP->AddObservation(pKFini,i);
        pMP->AddObservation(pKFcur,mvIniMatches[i]);

        // 从观测到该MapPoint的关键帧中挑选区分度最高的描述子
        //更新该MapPoint平均观测方向以及观测距离的范围
        pMP->ComputeDistinctiveDescriptors();
        pMP->UpdateNormalAndDepth();

        //Fill Current Frame structure
        mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
        mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;

        //Add to Map
        //在地图中添加该MapPoint
        mpMap->AddMapPoint(pMP);
    }
    

worldPos是之前Initialize函数得到的mvIniP3D,pMP是根据当前帧和 3d点worldPos 和地图mpMap初始化的MapPoint对象。

接下来的操作AddMapPointAddObservation都是pKFinipKFcur一对一对进行操作的,即在关键帧中添加地图点,地图点选择能被哪个帧观测到AddMapPoint中的重要参数mvpMapPoints根据索引记录了每个地图点。AddObservation中一个重要的参数是mObservations,是个map类,键值对为关键帧和mappoint的索引,如下表所示。

keyframeid
pKFinii
pKFcurmvIniMatches[i]

然后是 ComputeDistinctiveDescriptors,函数计算最有代表性的描述子,先获得当前点的所有描述子,然后计算描述子之间的两两距离,最好的描述子与其他描述子应该具有最小的距离中值。UpdateNormalAndDepth 函数没细看,是更新平均观测方向和观测距离范围的。最后在map里添加地图点。

第五步:更新连接关系

pKFini->UpdateConnections();
pKFcur->UpdateConnections();

下图所示为权重连接示意图:
在这里插入图片描述
这个部分主要是用于利用各关键帧的共视程度(covisibility)建立帧与帧之间的连接关系。在关键帧和关键帧之间建立边,每个边有一个权重,边的权重是该关键帧与当前帧公共3D点的个数。首先,从调用这个函数的关键帧得到所有特征点,然后根据这些特征点mappoints得到观测值也就是之前提到的mObservations,函数中的 KFcounter 记录当前帧和与其他关键帧之间的权重的。vPairs 这个参数用于记录权重大于阈值的帧,一旦发现大于阈值就把帧连接上,该帧的mConnectedKeyFrameWeights获得更新,其值描述了该帧和添加的帧的共视权重,详解见注释:

void KeyFrame::UpdateConnections()
{
    //===============1==================================
    map<KeyFrame*,int> KFcounter; // 关键帧-权重,权重为其它关键帧与当前关键帧共视3d点的个数

    vector<MapPoint*> vpMP;

    {
        // 获得该关键帧的所有3D点
        unique_lock<mutex> lockMPs(mMutexFeatures);
        vpMP = mvpMapPoints;
    }

    // 通过3D点间接统计可以观测到这些3D点的所有关键帧之间的共视程度
    // 即统计每一个关键帧都有多少关键帧与它存在共视关系,统计结果放在KFcounter
    for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
    {
        MapPoint* pMP = *vit;

        if(!pMP)
            continue;

        if(pMP->isBad())
            continue;

        // 对于每一个MapPoint点,observations记录了可以观测到该MapPoint的所有关键帧
        map<KeyFrame*,size_t> observations = pMP->GetObservations();

        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
            // 除去自身,自己与自己不算共视
            if(mit->first->mnId==mnId)
                continue;
            //权重 + 1
            KFcounter[mit->first]++;
        }
    }
    // This should not happen
    if(KFcounter.empty())
        return;

    //===============2==================================
    // 超过阈值就添加连接
    // 如果没有超过该阈值的权重,那么就只保留权重最大的边
    int nmax=0;
    KeyFrame* pKFmax=NULL;
    int th = 15;

    // vPairs记录与其它关键帧共视帧数大于th的关键帧
    // pair<int,KeyFrame*>将关键帧的权重写在前面,关键帧写在后面方便后面排序
    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(KFcounter.size());
    for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
    {
        if(mit->second > nmax)
        {
            nmax = mit->second;
            // 找到对应权重最大的关键帧(共视程度最高的关键帧)
            pKFmax = mit->first;
        }
        if(mit->second >= th)
        {
            // 对应权重需要大于阈值,对这些关键帧建立连接
            vPairs.push_back(make_pair(mit->second,mit->first));
            // 更新KFcounter中该关键帧的mConnectedKeyFrameWeights
            // 更新其它KeyFrame的mConnectedKeyFrameWeights,更新其它关键帧与当前帧的连接权重
            (mit->first)->AddConnection(this,mit->second);
        }
    }

    // 如果没有超过阈值的权重,则对权重最大的关键帧建立连接
    if(vPairs.empty())
    {
        // 如果每个关键帧与它共视的关键帧的个数都少于th,
        // 那就只更新与其它关键帧共视程度最高的关键帧的mConnectedKeyFrameWeights
        // 这是对之前th这个阈值可能过高的一个补丁
        vPairs.push_back(make_pair(nmax,pKFmax));
        pKFmax->AddConnection(this,nmax);
    }

    // vPairs里存的都是相互共视程度比较高的关键帧和共视权重,由小到大
    sort(vPairs.begin(),vPairs.end());
    
    //lKFs和lWs由大到小
    list<KeyFrame*> lKFs;
    list<int> lWs;
    for(size_t i=0; i<vPairs.size();i++)
    {
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }

    //===============3==================================
    {
        unique_lock<mutex> lockCon(mMutexConnections);

        // mspConnectedKeyFrames = spConnectedKeyFrames;
        // 更新图的连接(权重)
        mConnectedKeyFrameWeights = KFcounter;//更新该KeyFrame的mConnectedKeyFrameWeights,更新当前帧与其它关键帧的连接权重
        mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

        // 更新生成树的连接
        if(mbFirstConnection && mnId!=0)
        {
            // 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
            mpParent = mvpOrderedConnectedKeyFrames.front();
            // 建立双向连接关系
            mpParent->AddChild(this);
            mbFirstConnection = false;
        }
    }
}

第六步:全局优化

    // Bundle Adjustment
    cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;

    // BA优化
    Optimizer::GlobalBundleAdjustemnt(mpMap,20);

    

第七步:中值深度归一化

这步用的函数ComputeSceneMedianDepth里用到了dot运算,详情见 dot用法

    // Set median depth to 1
    // !!!将MapPoints的中值深度归一化到1,并归一化两帧之间变换
    // 评估关键帧场景深度,q=2表示中值
    float medianDepth = pKFini->ComputeSceneMedianDepth(2);
    float invMedianDepth = 1.0f/medianDepth;

    if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
    {
        cout << "Wrong initialization, reseting..." << endl;
        Reset();
        return;
    }

    // Scale initial baseline
    cv::Mat Tc2w = pKFcur->GetPose();
    // x/z y/z 将z归一化到1 
    Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
    pKFcur->SetPose(Tc2w);

    // Scale points
    // 把3D点的尺度也归一化到1
    vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();
    for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++)
    {
        if(vpAllMapPoints[iMP])
        {
            MapPoint* pMP = vpAllMapPoints[iMP];
            pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);
        }
    }


第八步:收尾工作,参数交接

包括比如localmapping线程插入关键帧,当前帧pose的设置,“上一关键帧”(当前帧)的传递,参考帧的传递,MapDrawer画图等,最后状态置为OK。

    mpLocalMapper->InsertKeyFrame(pKFini);
    mpLocalMapper->InsertKeyFrame(pKFcur);

    mCurrentFrame.SetPose(pKFcur->GetPose());
    mnLastKeyFrameId=mCurrentFrame.mnId;
    mpLastKeyFrame = pKFcur;

    mvpLocalKeyFrames.push_back(pKFcur);
    mvpLocalKeyFrames.push_back(pKFini);
    mvpLocalMapPoints=mpMap->GetAllMapPoints();
    mpReferenceKF = pKFcur;
    mCurrentFrame.mpReferenceKF = pKFcur;

    mLastFrame = Frame(mCurrentFrame);

    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);

    mpMapDrawer->SetCurrentCameraPose(pKFcur->GetPose());

    mpMap->mvpKeyFrameOrigins.push_back(pKFini);

    mState=OK;// 初始化成功,至此,初始化过程完成
}

至此,MonocularInitialization工作完成。。。。正式进入Track!

  • 0
    点赞
  • 2
    评论
  • 5
    收藏
  • 一键三连
    一键三连
  • 扫一扫,分享海报

打赏
文章很值,打赏犒劳作者一下
相关推荐
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页

打赏

幽冥小龙

你的鼓励将是我创作的最大动力

¥2 ¥4 ¥6 ¥10 ¥20
输入1-500的整数
余额支付 (余额:-- )
扫码支付
扫码支付:¥2
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值