测试

 

 

在localMapping 中,我们需要利用局部地图进行BA()

void Optimizer::LocalBundleAdjustmentWithLine(KeyFrame *pKF, bool *pbStopFlag, Map *pMap)
    {
        double invSigma = 0.01;
        // Local KeyFrames: First Breath Search from Current KeyFrame
        list<KeyFrame*> lLocalKeyFrames;

        // step1: 将当前关键帧加入到lLocalKeyFrames
        lLocalKeyFrames.push_back(pKF);
        pKF->mnBALocalForKF = pKF->mnId;

        // step2:找到关键帧连接的关键帧(一级相连),加入到lLocalKeyFrames中

        const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
        cout<<"Optimizer: 关键帧相连的帧:"<<vNeighKFs.size()<<endl;
        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);
        }

        // step3:将lLocalKeyFrames的MapPoints加入到lLocalMapPoints
        list<MapPoint*> lLocalMapPoints;
        for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
        {
            vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();
            cout<<"Optimizer: get mappointMatches():"<<vpMPs.size()<<endl;
            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;
                        }
                    }
                }
            }
        }
        cout<<"Optimizer: finish adding points"<<endl;
        // step4: 遍历lLocalKeyFrames,将每个关键帧所能观测到的MapLine提取出来,放到lLocalMapLines
        list<MapLine*> lLocalMapLines;
        vector<long unsigned int> mlID;
        for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
        {
            vector<MapLine*> vpMLs = (*lit)->GetMapLineMatches();
            cout<<"Optimizer: get mapLineMatches():"<<vpMLs.size()<<endl;
            for(vector<MapLine*>::iterator vit=vpMLs.begin(), vend=vpMLs.end(); vit!=vend; vit++)
            {

                MapLine* pML = *vit;
                if(pML)
                {
                    cout<<"Optimizer: mapline exist"<<endl;
                    if(!pML->isBad())
                    {
                        cout<<"Optimizer: mapline is not bad"<<endl;
                        if(pML->mnBALocalForKF!=pKF->mnId)
                        {
                            lLocalMapLines.push_back(pML);
                            pML->mnBALocalForKF = pKF->mnId;
                            mlID.push_back(pML->mnId);
                            cout<<"Optimizer:maplinesmatches id: "<<pKF->mnId<<endl;
                        }
                    }
                }
            }
        }

        cout<<"Optimizer: finish adding lines: size:"<< lLocalMapLines.size()<<endl;
#if 0
        for(list<MapLine*>::iterator lit=lLocalMapLines.begin(), lend=lLocalMapLines.end(); lit!=lend; lit++)
    {
        int i=0;
        if(count(mlID.begin(), mlID.end(), mlID[i])>1)
        {
            cout << "exist ===============" << endl;
            lit = lLocalMapLines.erase(lit);
        }
        i++;
    }
#endif

        // step5: 得到能被局部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);
                }
            }
        }

        // step6:构造g2o优化器
        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);

        g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
        optimizer.setAlgorithm(solver);

        if(pbStopFlag)
            optimizer.setForceStopFlag(pbStopFlag);

        unsigned long maxKFid = 0;

        cout<<"Optimizer: adding pose od local Keyframe"<<endl;
        // step7:添加顶点,Pose of Local KeyFrame
        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);
//        cout << "KeyFrame Id = " << pKFi->mnId << endl;
            vSE3->setFixed(pKFi->mnId==0);  //第一个关键帧需要Fix
            optimizer.addVertex(vSE3);
            if(pKFi->mnId>maxKFid)
                maxKFid=pKFi->mnId;
        }
        cout<<"Optimizer:  adding fixed keyframe"<<endl;
        // step8:添加固定帧的顶点,Pose of Fixed KeyFrame
        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);
//        cout << "Fixed KeyFrame Id = " << pKFi->mnId << endl;
            vSE3->setFixed(true);
            optimizer.addVertex(vSE3);
            if(pKFi->mnId > maxKFid)
                maxKFid = pKFi->mnId;
        }

        vector<int> MapPointID;
        //***********************Set MapPoint Vertices******************************
        // step9:添加MapPoint的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);
//        cout << "MapPoint Id = " << id << endl;
            vPoint->setMarginalized(true);
            optimizer.addVertex(vPoint);
            MapPointID.push_back(id);

            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];

                    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);
                    }

                }
            }
        }

        sort(MapPointID.begin(), MapPointID.end());
        int maxMapPointID = MapPointID[MapPointID.size()-1];
        cout << "Optimizer:************ maxMapPointID = " << maxMapPointID << " ************" << endl;

       

            // =========设置线段终止点和相机位姿之间的边=========
            for(map<KeyFrame*, size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
            {
                KeyFrame* pKFi = mit->first;

                if(!pKFi->isBad())
                {
                    Eigen::Vector3d line_obs;
                    line_obs = pKFi->mvKeyLineFunctions[mit->second];

                    EdgeLineProjectXYZ* e = new EdgeLineProjectXYZ();

                    e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(ide)));
                    e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
                    e->setMeasurement(line_obs);
                    e->setInformation(Eigen::Matrix3d::Identity()*invSigma);

                    g2o::RobustKernelHuber* rk_line_e = new g2o::RobustKernelHuber;
                    e->setRobustKernel(rk_line_e);
                    rk_line_e->setDelta(thHuberMono);

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

                    e->Xw = pML->mWorldPos.head(3);

                    optimizer.addEdge(e);
                    vpLineEdgesEP.push_back(e);
                    vpLineEdgeKF.push_back(pKFi);
                    vpMapLineEdge.push_back(pML);
                }
            }
        }

        cout<<"Optimizer: finish 终点of lines"<<endl;

        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())
                {
                    e->setLevel(1);
                }
                e->setRobustKernel(0);
            }
            cout<<"Optimizer: finish checking inlier observations"<<endl;

            for(size_t i=0, iend=vpLineEdgesSP.size(); i<iend; i++)
            {
                EdgeLineProjectXYZ* e = vpLineEdgesSP[i];
                MapLine* pML = vpMapLineEdge[i];

                if(pML->isBad())
                    continue;

                if(e->chi2()>7.815)
                {
                    e->setLevel(1);
                }
                e->setRobustKernel(0);
            }
            cout<<"Optimizer: finish start points of lines"<<endl;
            for(size_t i=0, iend=vpLineEdgesEP.size(); i<iend; i++)
            {
                EdgeLineProjectXYZ* e = vpLineEdgesEP[i];
                MapLine* pML = vpMapLineEdge[i];

                if(pML->isBad())
                    continue;

                if(e->chi2()>7.815)
                {
                    e->setLevel(1);
                }
                e->setRobustKernel(0);
            }
            cout<<"Optimizer: finish end points of lines"<<endl;
            // Optimize again without the outliers
            optimizer.initializeOptimization(0);
            cout<<"Optimizer: finish initialization optimization"<<endl;
            optimizer.optimize(10);
        }
        cout<<"Optimizer: finish bDoMore"<<endl;

        vector<pair<KeyFrame*, MapPoint*>> vToErase;
        vToErase.reserve(vpEdgesMono.size());
        cout<<"Optimizer: check inlier observations"<<endl;
        // 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));
            }
        }

        vector<pair<KeyFrame*,MapLine*>> vLineToErase;
        vLineToErase.reserve(vpLineEdgesSP.size());
        for(size_t i=0, iend=vpLineEdgesSP.size(); i<iend; i++)
        {
            EdgeLineProjectXYZ* e = vpLineEdgesSP[i];
            MapLine* pML = vpMapLineEdge[i];

            if(pML->isBad())
                continue;

            if(e->chi2()>7.815)
            {
                KeyFrame* pKFi = vpLineEdgeKF[i];
                vLineToErase.push_back(make_pair(pKFi, pML));
            }
        }

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

        if(!vLineToErase.empty())
        {
            for(size_t i=0; i<vLineToErase.size(); i++)
            {
                KeyFrame* pKFi = vLineToErase[i].first;
                MapLine* pMLi = vLineToErase[i].second;
                pKFi->EraseMapLineMatch(pMLi);
                pMLi->EraseObservation(pKFi);
            }
        }
        cout<<"Optimizer: recover optimized data"<<endl;
        // 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();
        }

        // Lines
        for(list<MapLine*>::iterator lit=lLocalMapLines.begin(), lend=lLocalMapLines.end(); lit!=lend; lit++)
        {
            MapLine* pML = *lit;
            g2o::VertexSBAPointXYZ* vStartP = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pML->mnId + maxKFid + maxMapPointID + 1));
            g2o::VertexSBAPointXYZ* vEndP = static_cast<g2o::VertexSBAPointXYZ *>(optimizer.vertex(pML->mnId + maxKFid + maxMapPointID + maxMapLineSPID + 1));

            Vector6d LinePos;
            LinePos << Converter::toVector3d(Converter::toCvMat(vStartP->estimate())), Converter::toVector3d(Converter::toCvMat(vEndP->estimate()));
            pML->SetWorldPos(LinePos);
            pML->UpdateAverageDir();
        }
        cout<<"Optimizer: finish local BA "<<endl;
    }
在localkeyframe中,装的是当前关键帧 相关联的关键帧(一级关键帧)。我们需要从这些关键帧中获得mvpMapPoint。使用GetMapPointMatches.

 

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值