ORB-SLAM2学习笔记——局部BA优化

1、理论部分（待更新）

2、代码详解

void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)
{
    
    // 该优化函数用于LocalMapping线程的局部BA优化    
    //step 1 : 得到所有局部关键帧
    //step 2 : 得到所有地图点
    //step 3 : 把关键帧和能看到的地图点对应到一起
    //step 4 : 进行优化 

    // Local KeyFrames: First Breath Search from Current Keyframe
    //局部 关键帧: 当前帧的第一次轨迹搜索
    //定义局部关键帧的列表
    list<KeyFrame*> lLocalKeyFrames;
    //关键帧放入列表
    lLocalKeyFrames.push_back(pKF);
    //索引导入到匹配好的局部优化关键帧
    pKF->mnBALocalForKF = pKF->mnId;
    //把匹配上的关键帧的顺序排好进行保存
    const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
    //遍历关键帧保存到pKFi中,然后把相应的索引也赋值过来
    for(int i=0, iend=vNeighKFs.size(); i<iend; i++)
    {
    
        KeyFrame* pKFi = vNeighKFs[i];
        pKFi->mnBALocalForKF = pKF->mnId;
        if(!pKFi->isBad())
            lLocalKeyFrames.push_back(pKFi);
    }

    // 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)
            {
                    
                pKFi->mnBAFixedForKF=pKF->mnId;
                if(!pKFi->isBad())
                    lFixedCameras.push_back(pKFi);
            }
        }
    }

    // Setup optimizer
    //创建优化器
    g2o::SparseOptimizer optimizer;
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;

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

    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
    //选用LM算法进行求解线性方程
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    optimizer.setAlgorithm(solver);
    //停止优化标志
    if(pbStopFlag)
        optimizer.setForceStopFlag(pbStopFlag);

    unsigned long maxKFid = 0;

    // 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);
        //固定第一帧防止整体乱动
        vSE3->setFixed(pKFi->mnId==0);
        //添加顶点到优化器
        optimizer.addVertex(vSE3);
        
        if(pKFi->mnId>maxKFid)
            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);
        //不进行优化
        vSE3->setFixed(true);
        optimizer.addVertex(vSE3);
        if(pKFi->mnId>maxKFid)
            maxKFid=pKFi->mnId;
    }

    // Set MapPoint vertices
    ///添加3D顶点
    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;
        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();
                    //优化的顶点 0 地图点
                    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
                    //优化的顶点 1 位姿
                    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
    //检测outlier，并设置下次不优化
    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())
        {
    
            //设置为标志 1 不进行优化
            e->setLevel(1);
        }
        //鲁棒核函数设置为 0 不使用核函数
        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();
    }
}