30局部建图线程中的localBA

ORBSLAM2

---

---

代码

void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)//传入参数：关键帧、是否停止BA、地图点
{    
    // Local KeyFrames: First Breath Search from Current Keyframe
    list<KeyFrame*> lLocalKeyFrames;

    lLocalKeyFrames.push_back(pKF);//将传入的关键帧存放在lLocalKeyFrame这个list中
    pKF->mnBALocalForKF = pKF->mnId;//将当前帧的ID赋给当前帧的mnBALocalForKF

    const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();//获取关键帧的共视帧
    for(int i=0, iend=vNeighKFs.size(); i<iend; i++)//开始遍历共视帧
    {
        KeyFrame* pKFi = vNeighKFs[i];//取出共视帧
        pKFi->mnBALocalForKF = pKF->mnId;//将当前帧的ID传给共视帧的mnLocalBAForKF
        if(!pKFi->isBad())//如果共视帧没怀
            lLocalKeyFrames.push_back(pKFi);//那么就将共视帧传入lLocalKeyFrame
    }

    // Local MapPoints seen in Local KeyFrames
    list<MapPoint*> lLocalMapPoints;//地图点
    for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++)//开始遍历所有关键帧
    {
        vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();//取出他们的地图点
        for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++)//开始遍历地图点
        {
            MapPoint* pMP = *vit;//取出地图点
            if(pMP)
                if(!pMP->isBad())//地图点存在且不坏
                    if(pMP->mnBALocalForKF!=pKF->mnId)//防止重复添加地图点
                    {
                        lLocalMapPoints.push_back(pMP);
                        pMP->mnBALocalForKF=pKF->mnId;
                    }
        }
    }

    // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes能观测到局部mappoints观测到，但是不属于局部关键帧的关键帧，这些关键帧在局部BA时不优化
    list<KeyFrame*> lFixedCameras;
    for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)//开始迭代所有地图点
    {
        map<KeyFrame*,size_t> observations = (*lit)->GetObservations();//获取每个图点的观测
        for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)//遍历观测
        {
            KeyFrame* pKFi = mit->first;//取出每个观测帧

            if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)//如果这个关键帧不是局部关键帧且mnBAFixedForKF不等于mnid
            {                
                pKFi->mnBAFixedForKF=pKF->mnId;//就将他设置成为相同ID
                if(!pKFi->isBad())
                    lFixedCameras.push_back(pKFi);
            }
        }
    }
	//下面开始优化
    // Setup optimizer
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;//定义线性求解器

	//typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,3>> BlockSolver_6_3
	//g2o::BlockSolver_6_3::LinearSolver* linearSolver=new LinearSolverDense<>(g2o::BlockSolver_6_3::PoseMatrixType);
	//g2o::BlockSolver_6_3 *slover_ptr= new g2o::BlockSolver_6_3(linearSolver);
	//g2o::OptimizationAlgorithmLevenberg *solver=new g2o::OptimizationAlgorithmLenvenberg(slover_ptr);
	//g2o::SparaseOptimizer optimizer;
	//optimizer.setAlgorithm(solver);

    linearSolver = new g2o::LinearSolverEigen<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);

    if(pbStopFlag)
        optimizer.setForceStopFlag(pbStopFlag);//如果要求停止，就停止localBA

    unsigned long maxKFid = 0;//记录参与localBA的最大关键帧ID

    // Set Local KeyFrame vertices
    for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)//开始遍历关键帧以及相邻帧，也就是局部关键帧
    {
        KeyFrame* pKFi = *lit;//取出每一帧
        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));//设置初值，位姿
        vSE3->setId(pKFi->mnId);//设置id
        vSE3->setFixed(pKFi->mnId==0);//如果ID=0，就不优化，也就是初始化关键帧时不优化的
        optimizer.addVertex(vSE3);//添顶点
        if(pKFi->mnId>maxKFid)//记录最大的关键帧的ID
            maxKFid=pKFi->mnId;
    }

    // Set Fixed KeyFrame vertices//添加不优化的顶点

    for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++)
    {
        KeyFrame* pKFi = *lit;//取出这些帧
        g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
        vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));//设置初值
        vSE3->setId(pKFi->mnId);//id
        vSE3->setFixed(true);//确保他们时固定的不优化
        optimizer.addVertex(vSE3);//添加顶点
        if(pKFi->mnId>maxKFid)
            maxKFid=pKFi->mnId;
    }

    // Set MapPoint vertices//添加地图点
    const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();//设置期待的边的数目，也就是局部关键帧的数目+固定关键帧的数目+局部地图点的数目

    vector<g2o::EdgeSE3ProjectXYZ*> vpEdgesMono;
    vpEdgesMono.reserve(nExpectedSize);

    vector<KeyFrame*> vpEdgeKFMono;
    vpEdgeKFMono.reserve(nExpectedSize);

    vector<MapPoint*> vpMapPointEdgeMono;
    vpMapPointEdgeMono.reserve(nExpectedSize);

    vector<g2o::EdgeStereoSE3ProjectXYZ*> vpEdgesStereo;
    vpEdgesStereo.reserve(nExpectedSize);

    vector<KeyFrame*> vpEdgeKFStereo;
    vpEdgeKFStereo.reserve(nExpectedSize);

    vector<MapPoint*> vpMapPointEdgeStereo;
    vpMapPointEdgeStereo.reserve(nExpectedSize);

    const float thHuberMono = sqrt(5.991);//卡方分布。核函数
    const float thHuberStereo = sqrt(7.815);

    for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)//开始遍历局部地图点
    {
        MapPoint* pMP = *lit;//取出局部地图点
        g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
        vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));//设置初值
        int id = pMP->mnId+maxKFid+1;//前面添加多少帧ID+1就是地图点初始id
        vPoint->setId(id);
        vPoint->setMarginalized(true);//设置边缘化
        optimizer.addVertex(vPoint);

        const map<KeyFrame*,size_t> observations = pMP->GetObservations();//获取该地图点的观测帧

        //Set edges
        for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)//开始遍历观测帧
        {
            KeyFrame* pKFi = mit->first;//取出观测帧

            if(!pKFi->isBad())//不是坏的话
            {                
                const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];//取出该帧的去畸变后的特征点

                // Monocular observation
                if(pKFi->mvuRight[mit->second]<0)
                {
                    Eigen::Matrix<double,2,1> obs;//设置观测点
                    obs << kpUn.pt.x, kpUn.pt.y;//将特征点坐标传入

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

                    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                    e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
                    e->setMeasurement(obs);
                    const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];//设置信息阵
                    e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);

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

                    e->fx = pKFi->fx;
                    e->fy = pKFi->fy;
                    e->cx = pKFi->cx;
                    e->cy = pKFi->cy;

                    optimizer.addEdge(e);//加入边
                    vpEdgesMono.push_back(e);
                    vpEdgeKFMono.push_back(pKFi);
                    vpMapPointEdgeMono.push_back(pMP);
                }
                else // Stereo observation
                {
                    Eigen::Matrix<double,3,1> obs;
                    const float kp_ur = pKFi->mvuRight[mit->second];
                    obs << kpUn.pt.x, kpUn.pt.y, kp_ur;

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

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

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

                    e->fx = pKFi->fx;
                    e->fy = pKFi->fy;
                    e->cx = pKFi->cx;
                    e->cy = pKFi->cy;
                    e->bf = pKFi->mbf;

                    optimizer.addEdge(e);
                    vpEdgesStereo.push_back(e);
                    vpEdgeKFStereo.push_back(pKFi);
                    vpMapPointEdgeStereo.push_back(pMP);
                }
            }
        }
    }

    if(pbStopFlag)//是否停止？
        if(*pbStopFlag)
            return;

    optimizer.initializeOptimization();//开始优化
    optimizer.optimize(5);//是否优化五次

    bool bDoMore= true;

    if(pbStopFlag)//检查
        if(*pbStopFlag)
            bDoMore = false;

    if(bDoMore)
    {

    // Check inlier observations
    for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)//开始遍历上述存储的边
    {
        g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
        MapPoint* pMP = vpMapPointEdgeMono[i];

        if(pMP->isBad())
            continue;

        if(e->chi2()>5.991 || !e->isDepthPositive())//筛选，如果边的卡方分布值大于5.991或者边对应的深度值为负的话，就将他剔除
        {
            e->setLevel(1);
        }

        e->setRobustKernel(0);//剔除后就不设置鲁棒核函数了
    }

    for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)//双目
    {
        g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
        MapPoint* pMP = vpMapPointEdgeStereo[i];

        if(pMP->isBad())
            continue;

        if(e->chi2()>7.815 || !e->isDepthPositive())
        {
            e->setLevel(1);
        }

        e->setRobustKernel(0);
    }

    // Optimize again without the outliers

    optimizer.initializeOptimization(0);//排除了离群点后，再次进行优化
    optimizer.optimize(10);

    }

    vector<pair<KeyFrame*,MapPoint*> > vToErase;
    vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());

    // Check inlier observations       
    for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)//优化完后重新进行误差计算，剔除误差较大点。和前面差不多
    {
        g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
        MapPoint* pMP = vpMapPointEdgeMono[i];

        if(pMP->isBad())
            continue;

        if(e->chi2()>5.991 || !e->isDepthPositive())
        {
            KeyFrame* pKFi = vpEdgeKFMono[i];
            vToErase.push_back(make_pair(pKFi,pMP));
        }
    }

    for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
    {
        g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
        MapPoint* pMP = vpMapPointEdgeStereo[i];

        if(pMP->isBad())
            continue;

        if(e->chi2()>7.815 || !e->isDepthPositive())
        {
            KeyFrame* pKFi = vpEdgeKFStereo[i];
            vToErase.push_back(make_pair(pKFi,pMP));
        }
    }

    // Get Map Mutex
    unique_lock<mutex> lock(pMap->mMutexMapUpdate);//锁定地图点

    if(!vToErase.empty())
    {
        for(size_t i=0;i<vToErase.size();i++)
        {
            KeyFrame* pKFi = vToErase[i].first;
            MapPoint* pMPi = vToErase[i].second;
            pKFi->EraseMapPointMatch(pMPi);
            pMPi->EraseObservation(pKFi);
        }
    }

    // Recover optimized data

    //Keyframes//更新属性
    for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
    {
        KeyFrame* pKF = *lit;
        g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
        g2o::SE3Quat SE3quat = vSE3->estimate();
        pKF->SetPose(Converter::toCvMat(SE3quat));
    }

    //Points
    for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
    {
        MapPoint* pMP = *lit;
        g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
        pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
        pMP->UpdateNormalAndDepth();
    }
}