예제 #1
0
파일: slam.cpp 프로젝트: longchao9527/slam
void GraphicEnd::checkLoopClosure()
{
    cout<<"**************************************************"<<endl;
    cout<<"checking loop closure!!!"<<endl;
    cout<<"**************************************************"<<endl;
    
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
    //first extract currKF desp and keys
    vector<cv::KeyPoint> keys;
    vector<vector<float> > descrip;
    loadFeatures(currKF.frame.rgb,descrip,keys);

    //add currKF into database
    db.add(descrip);

    //query, first is the image itself
    QueryResults ret;
    db.query(descrip,ret,10);//only get 3 highest score frame;

    //ransac check
    for(int j=1;j<ret.size();++j)
    {
        //extract chose image feature
        vector<cv::KeyPoint> keypoint;
        vector<vector<float> > descriptor;
        loadFeatures(keyframes[ret[ j ].Id].frame.rgb,descriptor,keypoint);
        
        bool ransacResult=checkFundumental(keys,descrip,keypoint,descriptor);

        //if pass the ransac check
        if(ransacResult)
        {
            Eigen::Matrix4f H;
            bool validTrans=calcTrans2(keyframes[ ret[ j ].Id ],currKF,H);

            loop_fout<<currKF.frame_index<<" and "<<keyframes[ret[ j ].Id].frame_index<<" : "<<validTrans<<endl;
            if(!validTrans || H==Eigen::Matrix4f::Identity())
                continue;
           
            Eigen::Isometry3d T(H.cast<double>());
            T = T.inverse();

            EdgeSE3* edge = new EdgeSE3();
            edge->vertices() [0] = opt.vertex( keyframes[ret[ j ].Id].id );
            edge->vertices() [1] = opt.vertex( currKF.id );
            Matrix<double, 6,6> information = Matrix<double, 6, 6>::Identity();
            information(0, 0) = information(2,2) = 100; 
            information(1,1) = 100;
            information(3,3) = information(4,4) = information(5,5) = 100; 
            edge->setInformation( information );
            edge->setMeasurement( T );
            edge->setRobustKernel( _pSLAMEnd->robustKernel );
            opt.addEdge( edge );
        }
    }
}
예제 #2
0
파일: slam.cpp 프로젝트: longchao9527/slam
//bow loopclosure
void GraphicEnd::Bowloopclosure()
{
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
    vector<cv::KeyPoint> keys;
    vector<FSurf64::TDescriptor> descrip;
    DetectionResult result;
    bow_extractor(currKF.frame.rgb,keys,descrip);
    bow_detector.detectLoop(keys,descrip,result);

    if(result.detection())
    {
        cout<<"Loop found with image "<<result.match<<"!"<<endl;
        /******write loop into file*******/
        //ofstream loop_fout("/home/lc/workspace/paper_related/Appolo/test/result/loop.txt");
        
        /********************************/
        Eigen::Matrix4f H;
        bool validTrans=calcTrans(keyframes[ result.match ],currKF,H,true);

        loop_fout<<currKF.frame_index<<" and "<<keyframes[result.match].frame_index<<" : "<<validTrans<<endl;
        if(!validTrans)
           return;
           
        Eigen::Isometry3d T(H.cast<double>());
        //T = T.inverse();

        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex( keyframes[result.match].id );
        edge->vertices() [1] = opt.vertex( currKF.id );
        Matrix<double, 6,6> information = Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(2,2) = 100; 
        information(1,1) = 100;
        information(3,3) = information(4,4) = information(5,5) = 100; 
        edge->setInformation( information );
        edge->setMeasurement( T );
        edge->setRobustKernel( _pSLAMEnd->robustKernel );
        opt.addEdge( edge );
         
    }
}
예제 #3
0
void GraphicEnd::lostRecovery()
{
    //以present作为新的关键帧
    cout<<BOLDYELLOW<<"Lost Recovery..."<<RESET<<endl;
    //把present中的数据存储到current中
    _currKF.id ++;
    _currKF.planes = _present.planes;
    _currKF.frame_index = _index;
    _lastRGB = _currRGB.clone();
    _kf_pos = _robot;

    //waitKey(0);
    _keyframes.push_back( _currKF );
    
    //将current关键帧存储至全局优化器
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
    //顶点
    VertexSE3* v = new VertexSE3();
    v->setId( _currKF.id );
    if (_use_odometry)
        v->setEstimate( _odo_this );
    else
        v->setEstimate( Eigen::Isometry3d::Identity() );
    opt.addVertex( v );

    //由于当前位置不知道,所以不添加与上一帧相关的边
    if (_use_odometry)
    {
        Eigen::Isometry3d To = _odo_last.inverse()*_odo_this;
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices()[0] = opt.vertex( _currKF.id - 1 );
        edge->vertices()[1] = opt.vertex( _currKF.id );
        Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(2,2) = information(1, 1) = information(3,3) = information(4,4) = information(5,5) = 1/(_error_odometry*_error_odometry);
        edge->setInformation( information );
        edge->setMeasurement( To );
        opt.addEdge( edge );
        _odo_last = _odo_this;
        _lost = 0;
        return;
    }
    //check loop closure
    for (int i=0; i<_keyframes.size() - 1; i++)
    {
        vector<PLANE>& p1 = _keyframes[ i ].planes;
        Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
        
        if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
            continue;
        T = T.inverse();
        //若匹配上,则在两个帧之间加一条边
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex( _keyframes[i].id );
        edge->vertices() [1] = opt.vertex( _currKF.id );
        Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(1,1) = information(2,2) = 100; 
        information(3,3) = information(4,4) = information(5,5) = 100; 
        edge->setInformation( information );
        edge->setMeasurement( T );
        edge->setRobustKernel( _pSLAMEnd->_robustKernel );
        opt.addEdge( edge );
    }
    
}
예제 #4
0
//回环检测:在过去的帧中随机取_loopclosure_frames那么多帧进行两两比较
void GraphicEnd::loopClosure()
{
    if (_keyframes.size() <= 3 )  //小于3时,回环没有意义
        return;
    cout<<"Checking loop closure."<<endl;
    waitKey(10);
    vector<int> checked;
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;

    //相邻帧
    for (int i=-3; i>-5; i--)
    {
        int n = _keyframes.size() + i;
        if (n>=0)
        {
            vector<PLANE>& p1 = _keyframes[n].planes;
            Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
            if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
                continue;
            T = T.inverse();
            //若匹配上,则在两个帧之间加一条边
            EdgeSE3* edge = new EdgeSE3();
            edge->vertices() [0] = opt.vertex( _keyframes[n].id );
            edge->vertices() [1] = opt.vertex( _currKF.id );
            Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
            information(0, 0) = information(2,2) = 100; 
            information(1,1) = 100;
            information(3,3) = information(4,4) = information(5,5) = 100; 
            edge->setInformation( information );
            edge->setMeasurement( T );
            edge->setRobustKernel( _pSLAMEnd->_robustKernel );
            opt.addEdge( edge );
        }
        else
            break;
    }
    //搜索种子帧
    cout<<"checking seeds, seed.size()"<<_seed.size()<<endl;
    vector<int> newseed;
    for (size_t i=0; i<_seed.size(); i++)
    {
        vector<PLANE>& p1 = _keyframes[_seed[i]].planes;
        Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
        if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
            continue;
        T = T.inverse();
        //若匹配上,则在两个帧之间加一条边
        checked.push_back( _seed[i] );
        newseed.push_back( _seed[i] );
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex( _keyframes[_seed[i]].id );
        edge->vertices() [1] = opt.vertex( _currKF.id );
        Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(2,2) = 100; 
        information(1,1) = 100;
        information(3,3) = information(4,4) = information(5,5) = 100; 
        edge->setInformation( information );
        edge->setMeasurement( T );
        edge->setRobustKernel( _pSLAMEnd->_robustKernel );
        opt.addEdge( edge );
    }

    //随机搜索
    cout<<"checking random frames"<<endl;
    for (int i=0; i<_loopclosure_frames; i++)
    {
        int frame = rand() % (_keyframes.size() -3 ); //随机在过去的帧中取一帧
        if ( find(checked.begin(), checked.end(), frame) != checked.end() ) //之前已检查过
            continue;
        checked.push_back( frame );
        vector<PLANE>& p1 = _keyframes[frame].planes;
        RESULT_OF_MULTIPNP result = multiPnP( p1, _currKF.planes, true, _keyframes[frame].frame_index, 20 );
        Eigen::Isometry3d T = result.T;
        
        if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
            continue;
        T = T.inverse();
        displayLC( _keyframes[frame].frame_index, _currKF.frame_index, result.norm);
        newseed.push_back( frame );
        //若匹配上,则在两个帧之间加一条边
        cout<<BOLDBLUE<<"find a loop closure between kf "<<_currKF.id<<" and kf "<<frame<<RESET<<endl;
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex( _keyframes[frame].id );
        edge->vertices() [1] = opt.vertex( _currKF.id );
        Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(2,2) = 100; 
        information(1,1) = 100;
        information(3,3) = information(4,4) = information(5,5) = 100; 
        edge->setInformation( information );
        edge->setMeasurement( T );
        edge->setRobustKernel( _pSLAMEnd->_robustKernel );
        opt.addEdge( edge );
    }

    waitKey(10);
    _seed = newseed;
}
예제 #5
0
파일: slam.cpp 프로젝트: longchao9527/slam
void GraphicEnd::lostRecovery()
{
    cout<<"Lost Recovery..."<<endl;
    //
    currKF.id++;
    currKF.frame.rgb=present.frame.rgb.clone();
    currKF.frame.dep=present.frame.dep.clone();
    //currKF.frame.cloud=present.frame.cloud;
    pcl::copyPointCloud(*(present.frame.cloud),*(currKF.frame.cloud));
    currKF.frame_index = index;
    
    //waitKey(0);
    keyframes.push_back( currKF );
    
    //
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
    //
    VertexSE3* v = new VertexSE3();
    v->setId( currKF.id );
    v->setEstimate( Eigen::Isometry3d::Identity() );
    opt.addVertex( v );

   /****************************************
    //check bow loop
    //first extract currKF desp and keys
    vector<cv::KeyPoint> keys;
    vector<vector<float> > descrip;
    loadFeatures(currKF.frame.rgb,descrip,keys);

    //add currKF into database
    db.add(descrip);

    //query, first is the image itself
    QueryResults ret;
    db.query(descrip,ret,10);//only get 3 highest score frame;

    //ransac check
    for(int j=1;j<ret.size();++j)
    {
        //extract chose image feature
        vector<cv::KeyPoint> keypoint;
        vector<vector<float> > descriptor;
        loadFeatures(keyframes[ret[ j ].Id].frame.rgb,descriptor,keypoint);
        
        bool ransacResult=checkFundumental(keys,descrip,keypoint,descriptor);

        //if pass the ransac check
        if(ransacResult)
        {
            Eigen::Matrix4f H;
            bool validTrans=calcTrans2(keyframes[ ret[ j ].Id ],currKF,H);

            loop_fout<<"lost frame: "<<currKF.frame_index<<" and "<<keyframes[ret[ j ].Id].frame_index<<endl;
            
            cout<<"find loop between "<<currKF.frame_index<<" and "<<keyframes[ret[ j ].Id].frame_index<<endl;
            if(!validTrans || H==Eigen::Matrix4f::Identity())
                continue;
           
            Eigen::Isometry3d T(H.cast<double>());
            T = T.inverse();

            EdgeSE3* edge = new EdgeSE3();
            edge->vertices() [0] = opt.vertex( keyframes[ret[ j ].Id].id );
            edge->vertices() [1] = opt.vertex( currKF.id );
            Matrix<double, 6,6> information = Matrix<double, 6, 6>::Identity();
            information(0, 0) = information(2,2) = 100; 
            information(1,1) = 100;
            information(3,3) = information(4,4) = information(5,5) = 100; 
            edge->setInformation( information );
            edge->setMeasurement( T );
            edge->setRobustKernel( _pSLAMEnd->robustKernel );
            opt.addEdge( edge );
        }
    }
    
    
    ****************************************/
    //check loop closure
    for (int i=0; i<keyframes.size()-1; i++)
    {
        KEYFRAME& p1 = keyframes[ i ];
        //Eigen::Isometry3d T = calcTrans( p1, currKF.frame ).T;
        Eigen::Matrix4f H;
        //bool validTrans=calcTrans(p1,currKF,H);
        bool validTrans=calcTrans2(p1,currKF,H);  
            
        if(!validTrans)
            continue;
            
        Eigen::Isometry3d T(H.cast<double>());
        
        T = T.inverse();
        //
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex( keyframes[i].id );
        edge->vertices() [1] = opt.vertex( currKF.id );
        Matrix<double, 6,6> information = Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(1,1) = information(2,2) = 100; 
        information(3,3) = information(4,4) = information(5,5) = 100; 
        edge->setInformation( information );
        edge->setMeasurement( T );
        edge->setRobustKernel( _pSLAMEnd->robustKernel );
        opt.addEdge( edge );
    }
    
    lost = 0;
}
예제 #6
0
파일: slam.cpp 프로젝트: longchao9527/slam
void GraphicEnd::loopClosure()
{
    if (keyframes.size() >3 ) 
   {
    cout<<"Checking loop closure."<<endl;
    waitKey(10);
    vector<int> checked;
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;

    for (int i=-2; i>-5; i--)
    {
        int n = keyframes.size() + i;
        if (n>=0)
        {
            KEYFRAME& p1 = keyframes[n];
            Eigen::Matrix4f H;
            //bool validTrans=calcTrans(p1,currKF,H);
            bool validTrans=calcTrans2(p1,currKF,H);
            if(!validTrans)
               continue;
            
            Eigen::Isometry3d T(H.cast<double>());
            T = T.inverse();
            //
            EdgeSE3* edge = new EdgeSE3();
            edge->vertices() [0] = opt.vertex( keyframes[n].id );
            edge->vertices() [1] = opt.vertex( currKF.id );
            Matrix<double, 6,6> information = Matrix<double, 6, 6>::Identity();
            information(0, 0) = information(2,2) = 100; 
            information(1,1) = 100;
            information(3,3) = information(4,4) = information(5,5) = 100; 
            edge->setInformation( information );
            edge->setMeasurement( T );
            edge->setRobustKernel( _pSLAMEnd->robustKernel );
            opt.addEdge( edge );
        }
        else
            break;
    }
    //
    cout<<"checking seeds, seed.size()"<<seed.size()<<endl;
    vector<int> newseed;
    for (size_t i=0; i<seed.size(); i++)
    {
        KEYFRAME& p1 = keyframes[seed[i]];
        //Eigen::Isometry3d T = calcTrans( p1, currKF.frame ).T;
        Eigen::Matrix4f H;
        //bool validTrans=calcTrans(p1,currKF,H);
         bool validTrans=calcTrans2(p1,currKF,H);  
        if(!validTrans)
            continue;
            
        Eigen::Isometry3d T(H.cast<double>());  
         
        T = T.inverse();
        //
        checked.push_back( seed[i] );
        newseed.push_back( seed[i] );
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex( keyframes[seed[i]].id );
        edge->vertices() [1] = opt.vertex( currKF.id );
        Matrix<double, 6,6> information = Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(2,2) = 100; 
        information(1,1) = 100;
        information(3,3) = information(4,4) = information(5,5) = 100; 
        edge->setInformation( information );
        edge->setMeasurement( T );
        edge->setRobustKernel( _pSLAMEnd->robustKernel );
        opt.addEdge( edge );
    }

    //
    cout<<"checking random frames"<<endl;
    for (int i=0; i<10; i++)
    {
        int frame = rand() % (keyframes.size() -3 ); //
        if ( find(checked.begin(), checked.end(), frame) != checked.end() ) //
            continue;
        checked.push_back( frame );
        KEYFRAME& p1 = keyframes[frame];
        //Eigen::Isometry3d T = calcTrans( p1, currKF.frame ).T;
        Eigen::Matrix4f H;
        //bool validTrans=calcTrans(p1,currKF,H);
        bool validTrans=calcTrans2(p1,currKF,H);  
          
        if(!validTrans)
             continue;
            
        Eigen::Isometry3d T(H.cast<double>());
            
        T = T.inverse();

        newseed.push_back( frame );
        //
        cout<<"find a loop closure between kf "<<currKF.id<<" and kf "<<frame<<endl;
        EdgeSE3* edge = new EdgeSE3();
        edge->vertices() [0] = opt.vertex(keyframes[frame].id );
        edge->vertices() [1] = opt.vertex( currKF.id );
        Matrix<double, 6,6> information = Matrix<double, 6, 6>::Identity();
        information(0, 0) = information(2,2) = 100; 
        information(1,1) = 100;
        information(3,3) = information(4,4) = information(5,5) = 100; 
        edge->setInformation( information );
        edge->setMeasurement( T );
        edge->setRobustKernel( _pSLAMEnd->robustKernel );
        opt.addEdge( edge );
    }

    waitKey(10);
    seed = newseed;
   }
}