单目初始化构建地图、计算地图点具有代表性的描述子、计算平均观测方向

ORB-SLAM2


ORB-SLAM2-单目初始化构建地图、计算地图点具有代表性的描述子、计算平均观测方向


代码

//在单目初始化计算位姿完成后,开始初始化构建地图
            CreateInitialMapMonocular();

进入这个函数中

void Tracking::CreateInitialMapMonocular()
{
    // Create KeyFrames创建关键帧,在初始化的时候会认为当前帧与参考帧都是关键帧
    KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);//第一帧
    KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);//第二帧

对于关键帧的构造函数

long unsigned int KeyFrame::nNextId=0;

KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB):
    mnFrameId(F.mnId),  mTimeStamp(F.mTimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),
    mfGridElementWidthInv(F.mfGridElementWidthInv), mfGridElementHeightInv(F.mfGridElementHeightInv),
    mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0),
    mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0),
    fx(F.fx), fy(F.fy), cx(F.cx), cy(F.cy), invfx(F.invfx), invfy(F.invfy),
    mbf(F.mbf), mb(F.mb), mThDepth(F.mThDepth), N(F.N), mvKeys(F.mvKeys), mvKeysUn(F.mvKeysUn),
    mvuRight(F.mvuRight), mvDepth(F.mvDepth), mDescriptors(F.mDescriptors.clone()),
    mBowVec(F.mBowVec), mFeatVec(F.mFeatVec), mnScaleLevels(F.mnScaleLevels), mfScaleFactor(F.mfScaleFactor),
    mfLogScaleFactor(F.mfLogScaleFactor), mvScaleFactors(F.mvScaleFactors), mvLevelSigma2(F.mvLevelSigma2),
    mvInvLevelSigma2(F.mvInvLevelSigma2), mnMinX(F.mnMinX), mnMinY(F.mnMinY), mnMaxX(F.mnMaxX),
    mnMaxY(F.mnMaxY), mK(F.mK), mvpMapPoints(F.mvpMapPoints), mpKeyFrameDB(pKFDB),
    mpORBvocabulary(F.mpORBvocabulary), mbFirstConnection(true), mpParent(NULL), mbNotErase(false),
    mbToBeErased(false), mbBad(false), mHalfBaseline(F.mb/2), mpMap(pMap)//一大串参数,看的头疼,暂时没看
{
    mnId=nNextId++;//获取ID,自增

    mGrid.resize(mnGridCols);//依据指定的普通帧,初始化用于加速匹配的网格对象;将每个网格中有的特征点索引复制过来
    for(int i=0; i<mnGridCols;i++)
    {
        mGrid[i].resize(mnGridRows);
        for(int j=0; j<mnGridRows; j++)
            mGrid[i][j] = F.mGrid[i][j];
    }

    SetPose(F.mTcw);    //设置关键帧的位姿
}

创建完关键帧后,开始将关键帧的描述子转化为BoW

    pKFini->ComputeBoW();
    pKFcur->ComputeBoW();
    //将关键帧插入地图
    mpMap->AddKeyFrame(pKFini);
    mpMap->AddKeyFrame(pKFcur);

	
	//用初始化3D点来生成地图点
	for(size_t i=0; i<mvIniMatches.size();i++)//mvIniMatches存储的是初始化两帧的匹配关系
    {
        if(mvIniMatches[i]<0)//表示没有匹配,就continue
            continue;
        
        cv::Mat worldPos(mvIniP3D[i]);//用三角化点初始化为空间点的世界坐标,就是世界坐标系下的三维点

        MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);//地图点的世界坐标,生成地图点的关键帧,地图点所在的地图

		//下面为地图点增加属性
		//1.观测到该地图点的关键帧
		//2.该points的描述子
		//3.该mappoints的平均观测方向和深度范围
		pKFini->AddMapPoint(pMP,i);//表示该keyFrame的2D特征点与对应的3D地图点
        pKFcur->AddMapPoint(pMP,mvIniMatches[i]);

        pMP->AddObservation(pKFini,i);//1.观测到该地图点的关键帧
        pMP->AddObservation(pKFcur,mvIniMatches[i]);

        pMP->ComputeDistinctiveDescriptors();//从众多的特征点中挑选出代表这个3D点的描述子
        pMP->UpdateNormalAndDepth();//跟新平均观测方向与距离

       

进入ComputeDistinctiveDescriptors()

//一个3D点能投影多个2D点,选择具有代表性的描述子来描述它,获取所有描述子,计算描述子两两距离,最好的描述子应该与其他描述子具有最小的中值距离
void MapPoint::ComputeDistinctiveDescriptors()
{
    // Retrieve all observed descriptors
    vector<cv::Mat> vDescriptors;//观测描述子

    map<KeyFrame*,size_t> observations;//观测
    //获取所有观测,跳过坏点

    {
        unique_lock<mutex> lock1(mMutexFeatures);
        if(mbBad)
            return;
        observations=mObservations;
    }
    if(observations.empty())//观测是空的话,跳过
        return;

    vDescriptors.reserve(observations.size());

	for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)//遍历观测3D点的所有关键帧,获取ORB描述子,插入vDescriptor
    {
        KeyFrame* pKF = mit->first;

        if(!pKF->isBad())
            vDescriptors.push_back(pKF->mDescriptors.row(mit->second));
    }
    if(vDescriptors.empty())
        return;


	const size_t N = vDescriptors.size();//获得描述子的两两距离

    float Distances[N][N];//将distance定义为一个对称阵
    for(size_t i=0;i<N;i++)
    {
        Distances[i][i]=0;//自己与自己为0
        for(size_t j=i+1;j<N;j++)
        {
            int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
            Distances[i][j]=distij;
            Distances[j][i]=distij;
        }
    }

	int BestMedian = INT_MAX;//选择最具有代表性的描述子,与其他的有最小的距离中值
    int BestIdx = 0;
    for(size_t i=0;i<N;i++)
    {
        vector<int> vDists(Distances[i],Distances[i]+N);
        sort(vDists.begin(),vDists.end());//从小到达排列
        int median = vDists[0.5*(N-1)];//取中值

        if(median<BestMedian)//找最小中值
        {
            BestMedian = median;
            BestIdx = i;
        }
    }

    {
        unique_lock<mutex> lock(mMutexFeatures);
        mDescriptor = vDescriptors[BestIdx].clone();
    }
}

进入UpdateNormalAndDepth()

void MapPoint::UpdateNormalAndDepth()
{
    map<KeyFrame*,size_t> observations;//定义观测
    KeyFrame* pRefKF;
    cv::Mat Pos;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        if(mbBad)
            return;
        observations=mObservations;
        pRefKF=mpRefKF;//观测到该点的参考帧
        Pos = mWorldPos.clone();//3D点z在世界坐标系的位置

    if(observations.empty())
        return;

    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);//计算该地图点法线方向
    int n=0;
    for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)//迭代所有观测
    {
        KeyFrame* pKF = mit->first;
        cv::Mat Owi = pKF->GetCameraCenter();//获取相机光心在世界坐标系下所在的位置
        cv::Mat normali = mWorldPos - Owi;
        normal = normal + normali/cv::norm(normali);//归一化
        n++;
    }

    cv::Mat PC = Pos - pRefKF->GetCameraCenter();//同理
    const float dist = cv::norm(PC);
    const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;//金字塔的层级数
    const float levelScaleFactor =  pRefKF->mvScaleFactors[level];//金字塔的缩放倍数
    const int nLevels = pRefKF->mnScaleLevels;

    {
        unique_lock<mutex> lock3(mMutexPos);
        mfMaxDistance = dist*levelScaleFactor;//观测到该点距离上下限
        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];
        mNormalVector = normal/n;//获取平均观测方向
    }
}

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

        //Add to Map
        mpMap->AddMapPoint(pMP);

在这里插入图片描述

  • 0
    点赞
  • 0
    收藏
  • 打赏
    打赏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
©️2022 CSDN 皮肤主题:大白 设计师:CSDN官方博客 返回首页
评论

打赏作者

ningxiner2016

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

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

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

打赏作者

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

抵扣说明:

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

余额充值