20-参考关键帧跟踪当前普通帧，仅位姿优化

ORB-SLAM2

---

---

代码

Tracking::track()

//在前面判断是否初始化，初始化完成后再进来的就到这里来
else
    {
        // System is initialized. Track Frame.
        bool bOK;//临时变量，表示每个函数是否运行成功

        // Initial camera pose estimation using motion model or relocalization (if tracking is lost)
        if(!mbOnlyTracking)//fasle为正常SLAM模式（定位+地图更新）true表示仅定位，默认为false
        {
            // Local Mapping is activated. This is the normal behaviour, unless
            // you explicitly activate the "only tracking" mode.

            if(mState==OK)//是否初始化成功
            {
                // Local Mapping might have changed some MapPoints tracked in last frame
                CheckReplacedInLastFrame();//局部建图线程可能对地图点进行了替换

                if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)//如果跟踪模型是空的，表示可能刚刚初始化，或者是跟踪失败了
                {
                    bOK = TrackReferenceKeyFrame();//用最近关键帧来跟踪普通帧，进入
                }
                else
                {
                    bOK = TrackWithMotionModel();//恒速模型来跟踪
                    if(!bOK)
                        bOK = TrackReferenceKeyFrame();
                }
            }

TrackReferenceKeyFrame()

bool Tracking::TrackReferenceKeyFrame()
{
    // Compute Bag of Words vector
    mCurrentFrame.ComputeBoW();//首先，对关键帧进行词袋模型计算

    // We perform first an ORB matching with the reference keyframe
    // If enough matches are found we setup a PnP solver
    ORBmatcher matcher(0.7,true);//开始匹配
    vector<MapPoint*> vpMapPointMatches;//三维地图点匹配对
//通过特征点的BoW加快当前帧与参考帧的匹配，进入
    int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);

    if(nmatches<15)
        return false;

   mCurrentFrame.mvpMapPoints = vpMapPointMatches;
    mCurrentFrame.SetPose(mLastFrame.mTcw);//将上一帧的位姿作为当前位姿的初始值

    Optimizer::PoseOptimization(&mCurrentFrame);//重投影误差优化3D-2d,仅对位姿优化，进入

    // 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++;
        }
    }

    return nmatchesMap>=10;
}

进入SearchByBoW（）这个函数

int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)
{
	const vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches();//获取关键帧的地图点

    vpMapPointMatches = vector<MapPoint*>(F.N,static_cast<MapPoint*>(NULL));//匹配上的地图点，就是普通帧的索引与关键帧的指针

    const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec;//取出关键帧的词袋向量

    int nmatches=0;//匹配点的个数

    vector<int> rotHist[HISTO_LENGTH];//旋转直方图
    for(int i=0;i<HISTO_LENGTH;i++)
        rotHist[i].reserve(500);
    const float factor = 1.0f/HISTO_LENGTH;

    // We perform the matching over ORB that belong to the same vocabulary node (at a certain level)
    DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin();//定义迭代器，关键帧词袋向量
    DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin();//普通帧的词袋向量
    DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
    DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();

    while(KFit != KFend && Fit != Fend)//开始遍历循环关键帧与普通帧的词袋向量
    {
        if(KFit->first == Fit->first)//如果关键帧的节点与当前普通帧的节点在同一层
        {
            const vector<unsigned int> vIndicesKF = KFit->second;//取出这个描述子下面子节点的id
            const vector<unsigned int> vIndicesF = Fit->second;//取出普通帧的

            for(size_t iKF=0; iKF<vIndicesKF.size(); iKF++)//遍历该节点下的所有子节点的index
            {
                const unsigned int realIdxKF = vIndicesKF[iKF];//取出该节点对应的叶子节点的index所对应的描述子的index，也就是第几个描述子

                MapPoint* pMP = vpMapPointsKF[realIdxKF];//取出地图点中该特征点对应的mappoint

                if(!pMP)//判断3D点是否合法
                    continue;

                if(pMP->isBad())
                    continue;                

                const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF);//将这个点对应的描述子取出

                int bestDist1=256;//最小距离
                int bestIdxF =-1 ;
                int bestDist2=256;//次小距离

                for(size_t iF=0; iF<vIndicesF.size(); iF++)//找F下的特征点所对应的最优匹配
                {
                    const unsigned int realIdxF = vIndicesF[iF];//下面和上面的额同理

                    if(vpMapPointMatches[realIdxF])//判断如果匹配上了，就continue
                        continue;

                    const cv::Mat &dF = F.mDescriptors.row(realIdxF);//取出F中的描述子

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

                    if(dist<bestDist1)//记录最佳距离与次佳距离
                    {
                        bestDist2=bestDist1;
                        bestDist1=dist;
                        bestIdxF=realIdxF;
                    }
                    else if(dist<bestDist2)
                    {
                        bestDist2=dist;
                    }
                }

                if(bestDist1<=TH_LOW)//如过最佳距离小于阈值
                {
                    if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))//最佳距离明显比次佳距离好
                    {
                        vpMapPointMatches[bestIdxF]=pMP;//将匹配关系存进，将匹配好的地图点存入

                        const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];//记录关键帧中的特征点的Id

                        if(mbCheckOrientation)//计算角度直方图
                        {
                            float rot = kp.angle-F.mvKeys[bestIdxF].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(bestIdxF);
                        }
                        nmatches++;
                    }
                }

            }

            KFit++;
            Fit++;
        }
        else if(KFit->first < Fit->first)
        {
            KFit = vFeatVecKF.lower_bound(Fit->first);
        }
        else
        {
            Fit = F.mFeatVec.lower_bound(KFit->first);
        }
    }

	//删除误匹配点
    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)
                continue;
            for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
            {
                vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
                nmatches--;
            }
        }
    }

    return nmatches;//得到匹配关系
}

g2o仅对位姿进行优化
优化这部分还是有点害怕的，但是没办法，还是得看

int Optimizer::PoseOptimization(Frame *pFrame)//传入参数，是图像
{
    g2o::SparseOptimizer optimizer;//定义一个稀疏优化器
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;//首先定义一个线性求解器

    linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();

    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);//定义BlockSolver

    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);//定义优化方法
    optimizer.setAlgorithm(solver);

    int nInitialCorrespondences=0;

    // Set Frame vertex添加顶点
    g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
    vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));//输入的是当前帧的位姿
    vSE3->setId(0);//设置ID
    vSE3->setFixed(false);
    optimizer.addVertex(vSE3);//添加顶点

    // Set MapPoint vertices设置
    const int N = pFrame->N;
	//单目
    vector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
    vector<size_t> vnIndexEdgeMono;
    vpEdgesMono.reserve(N);
    vnIndexEdgeMono.reserve(N);
	//双目
    vector<g2o::EdgeStereoSE3ProjectXYZOnlyPose*> vpEdgesStereo;
    vector<size_t> vnIndexEdgeStereo;
    vpEdgesStereo.reserve(N);
    vnIndexEdgeStereo.reserve(N);
	//定义自由度为2的卡方分布
    const float deltaMono = sqrt(5.991);
    const float deltaStereo = sqrt(7.815);


    {
    unique_lock<mutex> lock(MapPoint::mGlobalMutex);//先锁定地图点

    for(int i=0; i<N; i++)
    {
        MapPoint* pMP = pFrame->mvpMapPoints[i];//取出地图点
        if(pMP)//如果地图点中有的话
        {
            // Monocular observation单目情况，没有右相机
            if(pFrame->mvuRight[i]<0)
            {
                nInitialCorrespondences++;
                pFrame->mvbOutlier[i] = false;

                Eigen::Matrix<double,2,1> obs;//定义一个观测量
                const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];//取出普通帧中去畸变后的特征点
                obs << kpUn.pt.x, kpUn.pt.y;//将特征点的坐标传给obs

                g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();//定义一元边，仅位姿

                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));//设置顶点连接的边
                e->setMeasurement(obs);
                const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];//这个点的可信度与金字塔层级相关
                e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);//设置信息阵

                g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;//使用鲁棒核函数
                e->setRobustKernel(rk);
                rk->setDelta(deltaMono);

                e->fx = pFrame->fx;//设置相机内参
                e->fy = pFrame->fy;
                e->cx = pFrame->cx;
                e->cy = pFrame->cy;
                cv::Mat Xw = pMP->GetWorldPos();//地图点中的空间位置，作为迭代初始值
                e->Xw[0] = Xw.at<float>(0);
                e->Xw[1] = Xw.at<float>(1);
                e->Xw[2] = Xw.at<float>(2);

                optimizer.addEdge(e);//添加边

                vpEdgesMono.push_back(e);
                vnIndexEdgeMono.push_back(i);
            }
            else  // Stereo observation
            {
                nInitialCorrespondences++;
                pFrame->mvbOutlier[i] = false;

                //SET EDGE
                Eigen::Matrix<double,3,1> obs;
                const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
                const float &kp_ur = pFrame->mvuRight[i];
                obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

                g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();

                e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
                e->setMeasurement(obs);
                const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
                Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
                e->setInformation(Info);

                g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
                e->setRobustKernel(rk);
                rk->setDelta(deltaStereo);

                e->fx = pFrame->fx;
                e->fy = pFrame->fy;
                e->cx = pFrame->cx;
                e->cy = pFrame->cy;
                e->bf = pFrame->mbf;
                cv::Mat Xw = pMP->GetWorldPos();
                e->Xw[0] = Xw.at<float>(0);
                e->Xw[1] = Xw.at<float>(1);
                e->Xw[2] = Xw.at<float>(2);

                optimizer.addEdge(e);

                vpEdgesStereo.push_back(e);
                vnIndexEdgeStereo.push_back(i);
            }
        }

    }
    }


    if(nInitialCorrespondences<3)
        return 0;

    // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier
    // At the next optimization, outliers are not included, but at the end they can be classified as inliers again.
	//开始优化，总共优化四次，每次迭代10次，优化后，将点分为内点与外点，外点不参与优化
    const float chi2Mono[4]={5.991,5.991,5.991,5.991};//单目卡方
    const float chi2Stereo[4]={7.815,7.815,7.815, 7.815};//双目卡方
    const int its[4]={10,10,10,10};    

    int nBad=0;
    for(size_t it=0; it<4; it++)
    {

        vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));//设位估计值
        optimizer.initializeOptimization(0);
        optimizer.optimize(its[it]);//优化次数

        nBad=0;
        for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
        {
            g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];//提取优化完成后的边

            const size_t idx = vnIndexEdgeMono[i];//获取优化边的索引

            if(pFrame->mvbOutlier[idx])//如果这个边来自于离群点
            {
                e->computeError();//计算误差
            }

            const float chi2 = e->chi2();//计算e2

            if(chi2>chi2Mono[it])//判断是否大于卡方？判断离群点
            {                
                pFrame->mvbOutlier[idx]=true;
                e->setLevel(1);
                nBad++;
            }
            else
            {
                pFrame->mvbOutlier[idx]=false;
                e->setLevel(0);
            }

            if(it==2)//优化两次之后就不要鲁棒核函数了
                e->setRobustKernel(0);
        }
		//双目
        for(size_t i=0, iend=vpEdgesStereo.size(); i<iend; i++)
        {
            g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = vpEdgesStereo[i];

            const size_t idx = vnIndexEdgeStereo[i];

            if(pFrame->mvbOutlier[idx])
            {
                e->computeError();
            }

            const float chi2 = e->chi2();

            if(chi2>chi2Stereo[it])
            {
                pFrame->mvbOutlier[idx]=true;
                e->setLevel(1);
                nBad++;
            }
            else
            {                
                e->setLevel(0);
                pFrame->mvbOutlier[idx]=false;
            }

            if(it==2)
                e->setRobustKernel(0);
        }

        if(optimizer.edges().size()<10)
            break;
    }    

    // Recover optimized pose and return number of inliers优化后的位姿
    g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));
    g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();
    cv::Mat pose = Converter::toCvMat(SE3quat_recov);
    pFrame->SetPose(pose);

    return nInitialCorrespondences-nBad;//内点数目
}

对于顶点的自定义设置

class  VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  VertexSE3Expmap();//默认构造函数

  bool read(std::istream& is);

  bool write(std::ostream& os) const;

  virtual void setToOriginImpl() {//设置优化初值，这里面设置的是相机的位姿作为初始值
    _estimate = SE3Quat();
  }

  virtual void oplusImpl(const double* update_)  {//更新优化的值
    Eigen::Map<const Vector6d> update(update_);
    setEstimate(SE3Quat::exp(update)*estimate());
  }
};

边

class  EdgeSE3ProjectXYZOnlyPose: public  BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>{//测量维两维，类型是Vector2D，优化变量的类型是 VertexSE3Expmap，一元边
public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  EdgeSE3ProjectXYZOnlyPose(){}//构造函数

  bool read(std::istream& is);

  bool write(std::ostream& os) const;

  void computeError()  {//重载，计算误差
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
    Vector2d obs(_measurement);
    _error = obs-cam_project(v1->estimate().map(Xw));//这个就相当于将Xw的世界坐标系下的点转换到相机坐标系下的点，并且将其投影，也就是重投影误差
  }

  bool isDepthPositive() {
    const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
    return (v1->estimate().map(Xw))(2)>0.0;
  }


  virtual void linearizeOplus();

  Vector2d cam_project(const Vector3d & trans_xyz) const;

  Vector3d Xw;
  double fx, fy, cx, cy;
};