21基于恒速模型跟踪当前帧

ORBSLAM2

---

---

代码

bool Tracking::TrackWithMotionModel()
{
    ORBmatcher matcher(0.9,true);//最优距离小于0.9*次优距离，并检查旋转

    // Update last frame pose according to its reference keyframe
    // Create "visual odometry" points if in Localization Mode
    UpdateLastFrame();//更新上一帧的位姿

    mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);//依据恒速模型来估计当前帧位姿

    fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));//清空当前帧地图点

    // Project points seen in previous frame
    int th;//设置搜索半径
    if(mSensor!=System::STEREO)
        th=15;
    else
        th=7;
    int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);//依据上一帧对应的地图点来进行投影，进入

    // If few matches, uses a wider window search
    if(nmatches<20)//如果匹配的点数少于20
    {
        fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));//清空当前帧地图点
        nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR);//扩大搜索半径
    }

    if(nmatches<20)//还小于20，直接退出
        return false;

    // Optimize frame pose with all matches
    Optimizer::PoseOptimization(&mCurrentFrame);//对它的位姿进行优化

    // Discard outliers
    int nmatchesMap = 0;//去除外点
    for(int i =0; i<mCurrentFrame.N; i++)
    {
        if(mCurrentFrame.mvpMapPoints[i])
        {
            if(mCurrentFrame.mvbOutlier[i])
            {
                MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];

                mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
                mCurrentFrame.mvbOutlier[i]=false;
                pMP->mbTrackInView = false;
                pMP->mnLastFrameSeen = mCurrentFrame.mnId;
                nmatches--;
            }
            else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
                nmatchesMap++;
        }
    }    

    if(mbOnlyTracking)//仅定位
    {
        mbVO = nmatchesMap<10;
        return nmatches>20;
    }

    return nmatchesMap>=10;
}

int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)//传参：当前帧图像、上一帧图像、阈值、是否为单目
{
    int nmatches = 0;

    // Rotation Histogram (to check rotation consistency)//定义旋转直方图
    vector<int> rotHist[HISTO_LENGTH];
    for(int i=0;i<HISTO_LENGTH;i++)
        rotHist[i].reserve(500);
    const float factor = 1.0f/HISTO_LENGTH;

    const cv::Mat Rcw = CurrentFrame.mTcw.rowRange(0,3).colRange(0,3);//当前帧从世界坐标系到该相机坐标系的旋转阵
    const cv::Mat tcw = CurrentFrame.mTcw.rowRange(0,3).col(3);//平移向量

    const cv::Mat twc = -Rcw.t()*tcw;//反平移向量从当前帧坐标系转到世界坐标系的平移向量

    const cv::Mat Rlw = LastFrame.mTcw.rowRange(0,3).colRange(0,3);//上一帧的世界坐标系到上一帧相机坐标系的旋转阵
    const cv::Mat tlw = LastFrame.mTcw.rowRange(0,3).col(3);//平移向量

    const cv::Mat tlc = Rlw*twc+tlw;//当前帧对于上一帧的频移向量

    const bool bForward = tlc.at<float>(2)>CurrentFrame.mb && !bMono;//判断相机是前进还是后退，并且不是单目相机
    const bool bBackward = -tlc.at<float>(2)>CurrentFrame.mb && !bMono;

    for(int i=0; i<LastFrame.N; i++)//遍历上一帧的每一个地图点
    {
        MapPoint* pMP = LastFrame.mvpMapPoints[i];//取出来

        if(pMP)//如果是地图点
        {
            if(!LastFrame.mvbOutlier[i])//如果不是外点
            {
                // Project
                cv::Mat x3Dw = pMP->GetWorldPos();//获取上一帧地图点的世界坐标系下的坐标
                cv::Mat x3Dc = Rcw*x3Dw+tcw;//转达当前帧相机坐标系下
				//归一化平面
                const float xc = x3Dc.at<float>(0);
                const float yc = x3Dc.at<float>(1);
                const float invzc = 1.0/x3Dc.at<float>(2);

                if(invzc<0)//如果深度小于0就跳过
                    continue;

                float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx;//将上一帧的地图点投影到当前帧的图像上
                float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;

                if(u<CurrentFrame.mnMinX || u>CurrentFrame.mnMaxX)//判断是不是超过图像的边界
                    continue;
                if(v<CurrentFrame.mnMinY || v>CurrentFrame.mnMaxY)
                    continue;

                int nLastOctave = LastFrame.mvKeys[i].octave;//上一帧图像的keypoints在图像金字塔的那一层

                // Search in a window. Size depends on scale
                float radius = th*CurrentFrame.mvScaleFactors[nLastOctave];//15*尺度，也就是尺度越大图像的搜索半径也就越大

                vector<size_t> vIndices2;

                if(bForward)//依据图像前进后退判断搜索尺寸范围。开始画圆
                    vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);
                else if(bBackward)
                    vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, 0, nLastOctave);
                else
                    vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave-1, nLastOctave+1);

                if(vIndices2.empty())//候选匹配点为空
                    continue;

                const cv::Mat dMP = pMP->GetDescriptor();//获取上一帧的描述子

                int bestDist = 256;//
                int bestIdx2 = -1;

                for(vector<size_t>::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++)//开始遍历候选匹配点
                {
                    const size_t i2 = *vit;
                    if(CurrentFrame.mvpMapPoints[i2])
                        if(CurrentFrame.mvpMapPoints[i2]->Observations()>0)
                            continue;

                    if(CurrentFrame.mvuRight[i2]>0)//双目情况下
                    {
                        const float ur = u - CurrentFrame.mbf*invzc;
                        const float er = fabs(ur - CurrentFrame.mvuRight[i2]);
                        if(er>radius)
                            continue;
                    }

                    const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);

                    const int dist = DescriptorDistance(dMP,d);//计算描述子的距离

                    if(dist<bestDist)
                    {
                        bestDist=dist;
                        bestIdx2=i2;
                    }
                }

                if(bestDist<=TH_HIGH)//最佳距离小于阈值
                {
                    CurrentFrame.mvpMapPoints[bestIdx2]=pMP;
                    nmatches++;

                    if(mbCheckOrientation)
                    {
                        float rot = LastFrame.mvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].angle;
                        if(rot<0.0)
                            rot+=360.0f;
                        int bin = round(rot*factor);
                        if(bin==HISTO_LENGTH)
                            bin=0;
                        assert(bin>=0 && bin<HISTO_LENGTH);
                        rotHist[bin].push_back(bestIdx2);
                    }
                }
            }
        }
    }

    //Apply rotation consistency
    if(mbCheckOrientation)//计算旋转直方图
    {
        int ind1=-1;
        int ind2=-1;
        int ind3=-1;

        ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);

        for(int i=0; i<HISTO_LENGTH; i++)
        {
            if(i!=ind1 && i!=ind2 && i!=ind3)
            {
                for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
                {
                    CurrentFrame.mvpMapPoints[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
                    nmatches--;
                }
            }
        }
    }

    return nmatches;
}