orb-slam系列 Tracking线程 剩余(七)

TrackLocalMap()

/*****************************************************************************
 * 		追踪局部地图
 * 		1. 更新局部地图(更新局部关键帧和局部地图点)
 * 		2. 在局部地图点中寻找在当前帧的视野范围内的点,匹配视野范围内的点与当前帧,匹配点存储到当前帧的地图点容器中
 * 		3. 根据新得到的匹配点重新优化相机位姿并得到匹配内点, 通过匹配内点的数量来判断当前局部地图是否追踪成功
 ****************************************************************************/
bool Tracking::TrackLocalMap()
{
    // We have an estimation of the camera pose and some map points tracked in the frame.
    // We retrieve the local map and try to find matches to points in the local map.
    // 更新局部地图
    UpdateLocalMap();
    // 匹配局部地图点与当前帧的地图点
    SearchLocalPoints();

    // Optimize Pose  根据新更新的匹配局部地图点进行当前帧的位姿优化
    Optimizer::PoseOptimization(&mCurrentFrame);
    mnMatchesInliers = 0;

    // Update MapPoints Statistics  根据优化后的位姿重新更新匹配点的内点和外点
    for(int i=0; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            if(!mCurrentFrame.mvbOutlier[i])
            {
                mCurrentFrame.mvpMapPoints[i]->IncreaseFound();// 当前帧的被查找次数+1
                if(!mbOnlyTracking)
                {
                    if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
                        mnMatchesInliers++;
                }
                else
                    mnMatchesInliers++;
            }
            else if(mSensor==System::STEREO)
                mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);

        }
    }

    // Decide if the tracking was succesful
    // More restrictive if there was a relocalization recently
    if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50)
        return false;

    if(mnMatchesInliers<30)  // 内点数量太少 则局部地图追踪失败
        return false;
    else
        return true;
}

其中

UpdateLocalMap

/*  
 * 更新局部地图
  1  为全局地图设置参考地图点
  2  更新局部关键帧和局部地图点*/
void Tracking::UpdateLocalMap()
{
    // This is for visualization   为全局地图设置参考地图点  ,参考地图点用于DrawMapPoints函数画图
    mpMap->SetReferenceMapPoints(mvpLocalMapPoints);

    // Update  更新局部关键帧和局部地图点
    UpdateLocalKeyFrames();
    UpdateLocalPoints();
}

UpdateLocalKeyFrames

/************
 *    更新局部关键帧
 *    添加的关键帧为:     1  当前帧的所有地图点还在哪些关键帧中看到   将这些关键帧加入局部关键帧
 *                                         2  将所有看到地图点的所有关键帧的共视关键帧添加到局部地图中
 *                                         3  将所有看到地图点的所有关键帧的父关键帧和子关键帧添加到局部地图中
 *    更新参考关键帧为局部关键帧中看到最多当前帧地图点的关键帧
 ********/
void Tracking::UpdateLocalKeyFrames()
{
    // Each map point vote for the keyframes in which it has been observed
  // 每一个地图点为看到该地图点的关键帧投票 并将看到地图点的关键帧和该关键帧看到当前帧地图点的数量放到keyframeCounter容器中
    map<KeyFrame*,int> keyframeCounter;
    for(int i=0; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
            if(!pMP->isBad())
            {
                const map<KeyFrame*,size_t> observations = pMP->GetObservations();
                for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++)
                    keyframeCounter[it->first]++;
            }
            else
            {
                mCurrentFrame.mvpMapPoints[i]=NULL;
            }
        }
    }
    
    if(keyframeCounter.empty())
        return;

    int max=0;
    KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL);

    mvpLocalKeyFrames.clear();
    mvpLocalKeyFrames.reserve(3*keyframeCounter.size());

    // All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points
    // 将所有看到地图点的所有关键帧添加到局部地图中   并且检查哪个关键帧能看到最多的地图点
    for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++)
    {
        KeyFrame* pKF = it->first;

        if(pKF->isBad())
            continue;

        if(it->second>max)
        {
            max=it->second;
            pKFmax=pKF;
        }

        mvpLocalKeyFrames.push_back(it->first);
        pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
    }


    // Include also some not-already-included keyframes that are neighbors to already-included keyframes
    // 如果局部关键帧不足够的话将这些关键帧的共视关键帧也加入进来
    for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
    {
        // Limit the number of keyframes   限制关键帧的数量
        if(mvpLocalKeyFrames.size()>80)
            break;

        KeyFrame* pKF = *itKF;

        const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
        //最好的公视帧
        for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++)
        {
            KeyFrame* pNeighKF = *itNeighKF;
            if(!pNeighKF->isBad())
            {
                if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
                {
                    mvpLocalKeyFrames.push_back(pNeighKF);
                    pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                    break;
                }
            }
        }
	//  将这些关键帧的孩子加入到局部关键帧中   自关键帧为本关键帧为哪些关键帧的最大关联关键帧
        const set<KeyFrame*> spChilds = pKF->GetChilds();
        for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)
        {
            KeyFrame* pChildKF = *sit;
            if(!pChildKF->isBad())
            {
                if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
                {
                    mvpLocalKeyFrames.push_back(pChildKF);
                    pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                    break;
                }
            }
        }
      //  将这些关键帧的父辈加入到局部关键帧中  父关键帧为本关键帧的最大关联关键帧
        KeyFrame* pParent = pKF->GetParent();
        if(pParent)
        {
            if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
            {
                mvpLocalKeyFrames.push_back(pParent);
                pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
                break;
            }
        }

    }

    if(pKFmax)   // 将能看到当前帧最多地图点的关键帧设为参考关键帧,并更新当前帧的参考关键帧为该帧
    {
        mpReferenceKF = pKFmax;
        mCurrentFrame.mpReferenceKF = mpReferenceKF;
    }
}

1）当前帧的所有地图点，通过地图点索引出 能够看到当前帧地图的所有关键帧 看到加一keyframeCounter(frame,int) frame看到地图点的帧 看到当前帧的地图点数目
2）将所有看到地图点的所有关键帧添加到局部地图中 并且检查哪个关键帧能看到最多的地图点
3）pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId; 将该帧的track参数更改为参考当前帧
4）如果局部关键帧不足够80的话将这些关键帧的共视关键帧也加入进来 mvpLocalKeyFrames
5）子帧 父帧加入mvpLocalKeyFrames
6）看到地图点最多的关键帧 设为参考帧

UpdateLocalPoints

//  更新局部地图点
//  将局部关键帧中的所有关键帧的地图点加入局部地图点中(没有局部关键帧就没有局部地图点)
void Tracking::UpdateLocalPoints()
{
    mvpLocalMapPoints.clear();
    // 将局部关键帧中的所有关键帧的地图点加入局部地图点中
    for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
    {
        KeyFrame* pKF = *itKF;
        const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();

        for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++)
        {
            MapPoint* pMP = *itMP;
            if(!pMP)
                continue;
            if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId)
                continue;
            if(!pMP->isBad())
            {
                mvpLocalMapPoints.push_back(pMP);
                pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId;
            }
        }
    }
}

位姿更新逻辑

if(!mLastFrame.mTcw.empty())
            {
	        //上一帧的位姿变换矩阵
                cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
                mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
                mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
                mVelocity = mCurrentFrame.mTcw*LastTwc;  //运动速度  上一帧到这一帧的位姿变换矩阵
            }
            else  //第一帧
                mVelocity = cv::Mat();

判断是否需要关键帧

// 判断是否添加关键帧   筛选关键帧的操作
//判断条件:   1 仅线程追踪模式不需要添加新的关键帧
//                      2 局部地图模式被暂停或者被发起暂停请求(回环检测冻结局部地图)    则不添加关键帧
//                      3 上次重定位后没有足够的帧通过   则不添加关键帧
//                      4  追踪线程是比较弱的
//                      5   此帧距离上次插入关键帧已经超过了最大的帧数(很久没插入关键帧了)
//                      6  局部地图处于空闲状态并且没有超过最大关键帧数
//                      7  局内点数量比较少  跟踪地图中地图点比较少
bool Tracking::NeedNewKeyFrame()
{
    if(mbOnlyTracking)   // 如果是仅线程追踪则则不需要添加新的关键帧
        return false;

    // If Local Mapping is freezed by a Loop Closure do not insert keyframes  如果局部地图给暂停或者是已经被请求暂停(局部地图被一个回环检测冻结)  则不会插入关键帧
    if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
        return false;
    //得到整个地图中关键帧的数量
    const int nKFs = mpMap->KeyFramesInMap();  

    // Do not insert keyframes if not enough frames have passed from last relocalisation  如果在上次重定位后还没有足够的帧通过  则不插入关键帧
    if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)
        return false;

    // Tracked MapPoints in the reference keyframe   在参考关键帧中追踪地图点
    int nMinObs = 3;
    if(nKFs<=2)
        nMinObs=2;
    // 参考关键帧中所有地图点被观察到的次数大于2或3次的地图点数量
    int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);

    // Local Mapping accept keyframes?  局部地图是否接收关键帧
    bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();

    // Check how many "close" points are being tracked and how many could be potentially created.   检测有多少近点被追踪
    // 未被追踪的近点数量
    int nNonTrackedClose = 0;
    // 被追踪的近点的数量
    int nTrackedClose= 0;
    if(mSensor!=System::MONOCULAR)     //非单目相机   检测每一特征点是否是近点   有多少近点中局内点数量,有近点中局外点数量
    {
        for(int i =0; i<mCurrentFrame.N; i++)
        {
            if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)
            {
                if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
                    nTrackedClose++;
                else
                    nNonTrackedClose++;
            }
        }
    }
    //近点中被追踪数量小于100   并且近点未被追踪点数量大于70
    bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);     

    // Thresholds  设置阈值   参考比率(当前帧内点数量/参考关键帧中所有地图点被观察到的次数大于2或3次的地图点数量)
    float thRefRatio = 0.75f;
    if(nKFs<2)//如果整个地图中关键帧的数量小于2  则阈值设为0.4
        thRefRatio = 0.4f;

    if(mSensor==System::MONOCULAR)//  如果是单目相机   比例设为0.9
        thRefRatio = 0.9f;

    // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion    此帧距离上次插入关键帧已经超过了最大的帧数
    const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
    // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle     此帧距离上次插入关键帧已经超过了最小帧数并且此时局部地图线程处于空闲状态
    const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
    //Condition 1c: tracking is weak        如果传感器不是单目   并且(匹配到的内点小于好地图点的0.25倍或者 近点中局内点数量小于100   并且局外点大于70)   追踪线程是比较弱的
    const bool c1c =  mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;
    // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
    // 内点数量大于15  并且  内点匹配小于好的地图点的thRefRatio倍或者近点中局内点数量小于100   并且局外点大于70
    const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);

    if((c1a||c1b||c1c)&&c2)
    {
        // If the mapping accepts keyframes, insert keyframe.
        // Otherwise send a signal to interrupt BA     
        if(bLocalMappingIdle)   //如果局部建图线程处于空闲状态则返回真
        {
            return true;
        }
        else   //否则中断BA优化
        {
            mpLocalMapper->InterruptBA();
            if(mSensor!=System::MONOCULAR)
            {
                if(mpLocalMapper->KeyframesInQueue()<3)// 如果关键帧队列中关键帧数量小于3   则添加关键帧
                    return true;
                else
                    return false;
            }
            else
                return false;
        }
    }
    else
        return false;
}

创建关键帧

// 添加新的关键帧
// 建立关键帧中的地图点加入全局map中(非单目情况下),并且将关键帧传递给localmapping线程
void Tracking::CreateNewKeyFrame()
{
    if(!mpLocalMapper->SetNotStop(true))   // 如果局部建图线程被暂停
        return;

    KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);

    mpReferenceKF = pKF;  //更新参考关键帧为当前新添加的关键帧
    mCurrentFrame.mpReferenceKF = pKF;   //当前帧的参考关键帧设为该帧

    if(mSensor!=System::MONOCULAR)   // 双目或rgbd  添加地图点到地图中
    {
        mCurrentFrame.UpdatePoseMatrices();

        // We sort points by the measured depth by the stereo/RGBD sensor.
        // We create all those MapPoints whose depth < mThDepth.
        // If there are less than 100 close points we create the 100 closest.
	//  通过深度对点进行排序   将100个近点插入到地图中
        vector<pair<float,int> > vDepthIdx;
        vDepthIdx.reserve(mCurrentFrame.N);
        for(int i=0; i<mCurrentFrame.N; i++)
        {
            float z = mCurrentFrame.mvDepth[i];
            if(z>0)
            {
                vDepthIdx.push_back(make_pair(z,i));
            }
        }
	
        if(!vDepthIdx.empty())
        {
            sort(vDepthIdx.begin(),vDepthIdx.end());

            int nPoints = 0;
            for(size_t j=0; j<vDepthIdx.size();j++)
            {
                int i = vDepthIdx[j].second;

                bool bCreateNew = false;

                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
                if(!pMP)
                    bCreateNew = true;
                else if(pMP->Observations()<1)
                {
                    bCreateNew = true;
                    mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
                }

                if(bCreateNew)
                {
                    cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
                    MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);
                    pNewMP->AddObservation(pKF,i);
                    pKF->AddMapPoint(pNewMP,i);
                    pNewMP->ComputeDistinctiveDescriptors();
                    pNewMP->UpdateNormalAndDepth();
                    mpMap->AddMapPoint(pNewMP);   // 将地图点添加进全局地图mpMap中

                    mCurrentFrame.mvpMapPoints[i]=pNewMP;
                    nPoints++;
                }
                else
                {
                    nPoints++;
                }
		// 如果地图点的深度大于阈值(远点)并且当前添加的地图点数量已经大于100 则不再继续添加地图点了
                if(vDepthIdx[j].first>mThDepth && nPoints>100)
                    break;
            }
        }
    }
    // 局部地图加入关键帧
    mpLocalMapper->InsertKeyFrame(pKF);
    
    mpLocalMapper->SetNotStop(false);

    mnLastKeyFrameId = mCurrentFrame.mnId;
    mpLastKeyFrame = pKF;
}

其中

    mpReferenceKF = pKF;  //更新参考关键帧为当前新添加的关键帧
    mCurrentFrame.mpReferenceKF = pKF;   //当前帧的参考关键帧设为该帧

最后位姿逻辑

当前帧与参考关键帧的关系

// Store frame pose information to retrieve the complete camera trajectory afterward存储完整的帧姿态信息以获取完整的相机轨迹
    //每个关键帧都有三个坐标系下的变换矩阵，世界坐标系相对于相机坐标系的变换矩阵，相机坐标系相对于参考帧相机坐标系的变换矩阵，参考帧相机坐标系相对于世界坐标系的变换矩阵
    if(!mCurrentFrame.mTcw.empty())   //如果当前帧的世界坐标系下的变换矩阵不为空    即未丢失，跟踪成功
    {
        cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();    //计算当前帧相对于参考帧的变换矩阵
        mlRelativeFramePoses.push_back(Tcr);                               //与参考帧的相对位姿
        mlpReferences.push_back(mpReferenceKF);                             //将当前参考关键帧存入到参考帧列表中
        mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);     //将当前帧的时间戳存入mlFrameTimes
        mlbLost.push_back(mState==LOST);                                           //存储当前帧的状态，是否丢失
    }
    else
    {
        // This can happen if tracking is lost    如果追踪丢失，将保持原参考数据不变，并将该帧定义为LOST帧
        mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
        mlpReferences.push_back(mlpReferences.back());
        mlFrameTimes.push_back(mlFrameTimes.back());
        mlbLost.push_back(mState==LOST);
    }