Exemplo n.º 1
0
void GraphicEnd::generateKeyFrame( Eigen::Isometry3d T )
{
    cout<<BOLDGREEN<<"GraphicEnd::generateKeyFrame"<<RESET<<endl;
    //把present中的数据存储到current中
    _currKF.id ++;
    _currKF.planes = _present.planes;
    _currKF.frame_index = _index;
    _lastRGB = _currRGB.clone();
    _kf_pos = _robot;

    cout<<"add key frame: "<<_currKF.id<<endl;
    //waitKey(0);
    _keyframes.push_back( _currKF );
    
    //将current关键帧存储至全局优化器
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
    //顶点
    VertexSE3* v = new VertexSE3();
    v->setId( _currKF.id );
    //v->setEstimate( _robot );
    if (_use_odometry)
        v->setEstimate( _odo_this );
    //v->setEstimate( Eigen::Isometry3d::Identity() );
    else
        v->setEstimate( Eigen::Isometry3d::Identity() );
    opt.addVertex( v );
    //边
    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) = 100;
    information(1, 1) = 100;
    information(3,3) = information(4,4) = information(5,5) = 100; 
    edge->setInformation( information );
    edge->setMeasurement( T );
    opt.addEdge( edge );

    if (_use_odometry)
    {
        Eigen::Isometry3d To = _odo_last.inverse()*_odo_this;
        cout<<"odo last = "<<endl<<_odo_last.matrix()<<endl<<"odo this = "<<endl<<_odo_this.matrix()<<endl;
        cout<<"To = "<<endl<<To.matrix()<<endl;
        cout<<"Measurement = "<<endl<<T.matrix()<<endl;
        //waitKey( 0 );
        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(1,1) = information(2,2) = 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;
    }
    
}
Exemplo n.º 2
0
void GraphicEnd::generateKeyframe(Eigen::Isometry3d& T)
{
    cout<<"generating keyframes..."<<endl;
    currKF.id++;
    currKF.frame_index=index;
    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.fullpose=present.fullpose;
    //keyframes.reserve(100);

    keyframes.push_back(currKF);
    //cout<<"keyframes capacity is: "<<keyframes.capacity()<<endl;
    
    SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
    VertexSE3* v = new VertexSE3();
    v->setId( currKF.id );
    v->setEstimate( Eigen::Isometry3d::Identity() );
    opt.addVertex( v );
    cout<<"add vertex success!!!"<<endl;
    //edge
    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) = 100;
    information(1, 1) = 100;
    information(3,3) = information(4,4) = information(5,5) = 100; 
    edge->setInformation( information );
    edge->setMeasurement( T );
    opt.addEdge( edge );
    cout<<"add edge success!!!"<<endl;
}
Exemplo n.º 3
0
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 );
        }
    }
}
Exemplo n.º 4
0
//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 );
         
    }
}
Exemplo n.º 5
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 );
    }
    
}
Exemplo n.º 6
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;
}
Exemplo n.º 7
0
void Optimizer::optimizeUseG2O()
{


    // create the linear solver
    BlockSolverX::LinearSolverType * linearSolver = new LinearSolverCSparse<BlockSolverX::PoseMatrixType>();

    // create the block solver on top of the linear solver
    BlockSolverX* blockSolver = new BlockSolverX(linearSolver);

    // create the algorithm to carry out the optimization
    //OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(blockSolver);
    OptimizationAlgorithmLevenberg* optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver);

    // NOTE: We skip to fix a variable here, either this is stored in the file
    // itself or Levenberg will handle it.

    // create the optimizer to load the data and carry out the optimization
    SparseOptimizer optimizer;
    SparseOptimizer::initMultiThreading();
    optimizer.setVerbose(true);
    optimizer.setAlgorithm(optimizationAlgorithm);

    {
        pcl::ScopeTime time("G2O setup Graph vertices");
        for (size_t cloud_count = 0; cloud_count < m_pointClouds.size(); ++cloud_count)
        {
            VertexSE3 *vertex = new VertexSE3;
            vertex->setId(cloud_count);
            Isometry3D affine = Isometry3D::Identity();
            affine.linear() = m_pointClouds[cloud_count]->sensor_orientation_.toRotationMatrix().cast<Isometry3D::Scalar>();
            affine.translation() = m_pointClouds[cloud_count]->sensor_origin_.block<3, 1>(0, 0).cast<Isometry3D::Scalar>();
            vertex->setEstimate(affine);
            optimizer.addVertex(vertex);
        }
        optimizer.vertex(0)->setFixed(true);
    }

    {
        pcl::ScopeTime time("G2O setup Graph edges");
        double trans_noise = 0.5, rot_noise = 0.5235;
        EdgeSE3::InformationType infomation = EdgeSE3::InformationType::Zero();
        infomation.block<3, 3>(0, 0) << trans_noise * trans_noise, 0, 0,
                                        0, trans_noise * trans_noise, 0,
                                        0, 0, trans_noise * trans_noise;
        infomation.block<3, 3>(3, 3) << rot_noise * rot_noise, 0, 0,
                                        0, rot_noise * rot_noise, 0,
                                        0, 0, rot_noise * rot_noise;
        for (size_t pair_count = 0; pair_count < m_cloudPairs.size(); ++pair_count)
        {
            CloudPair pair = m_cloudPairs[pair_count];
		    int from = pair.corresIdx.first;
		    int to = pair.corresIdx.second;
            EdgeSE3 *edge = new EdgeSE3;
		    edge->vertices()[0] = optimizer.vertex(from);
		    edge->vertices()[1] = optimizer.vertex(to);

            Eigen::Matrix<double, 6, 6> ATA = Eigen::Matrix<double, 6, 6>::Zero();
            Eigen::Matrix<double, 6, 1> ATb = Eigen::Matrix<double, 6, 1>::Zero();
#pragma unroll 8
            for (size_t point_count = 0; point_count < pair.corresPointIdx.size(); ++point_count) {
                int point_p = pair.corresPointIdx[point_count].first;
                int point_q = pair.corresPointIdx[point_count].second;
                PointType P = m_pointClouds[from]->points[point_p];
                PointType Q = m_pointClouds[to]->points[point_q];

                Eigen::Vector3d p = P.getVector3fMap().cast<double>();
                Eigen::Vector3d q = Q.getVector3fMap().cast<double>();
                Eigen::Vector3d Np = P.getNormalVector3fMap().cast<double>();

                double b = (p - q).dot(Np);

                Eigen::Matrix<double, 6, 1> A_p;
                A_p.block<3, 1>(0, 0) = p.cross(Np);
                A_p.block<3, 1>(3, 0) = Np;

                ATA += A_p * A_p.transpose();
                ATb += A_p * b;
            }

            Eigen::Matrix<double, 6, 1> X = ATA.ldlt().solve(ATb);
            Isometry3D measure = Isometry3D::Identity();
            float beta = X[0];
            float gammar = X[1];
            float alpha = X[2];
            measure.linear() = (Eigen::Matrix3d)Eigen::AngleAxisd(alpha, Eigen::Vector3d::UnitZ()) *
                Eigen::AngleAxisd(gammar, Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxisd(beta, Eigen::Vector3d::UnitX());
            measure.translation() = X.block<3, 1>(3, 0);

            edge->setMeasurement(measure);

		    edge->setInformation(infomation);
            
            optimizer.addEdge(edge);
        }
    }

    optimizer.save("debug_preOpt.g2o");
    {
        pcl::ScopeTime time("g2o optimizing");
        optimizer.initializeOptimization();
        optimizer.optimize(30);
    }
    optimizer.save("debug_postOpt.g2o");

    for (size_t cloud_count = 0; cloud_count < m_pointClouds.size(); ++cloud_count)
    {
        CloudTypePtr tmp(new CloudType);
        Isometry3D trans = ((VertexSE3 *)optimizer.vertices()[cloud_count])->estimate();
        Eigen::Affine3d affine;
        affine.linear() = trans.rotation();
        affine.translation() = trans.translation();
        pcl::transformPointCloudWithNormals(*m_pointClouds[cloud_count], *tmp, affine.cast<float>());
        pcl::copyPointCloud(*tmp, *m_pointClouds[cloud_count]);
    }

    PCL_WARN("Opitimization DONE!!!!\n");

    if (m_params.saveDirectory.length()) {
        if (boost::filesystem::exists(m_params.saveDirectory) && !boost::filesystem::is_directory(m_params.saveDirectory)) {
            boost::filesystem::remove(m_params.saveDirectory);
        }
        boost::filesystem::create_directories(m_params.saveDirectory);

        char filename[1024] = { 0 };
        for (size_t i = 0; i < m_pointClouds.size(); ++i) {
            sprintf(filename, "%s/cloud_%d.ply", m_params.saveDirectory.c_str(), i);
            pcl::io::savePLYFileBinary(filename, *m_pointClouds[i]);
        }
    }
}
Exemplo n.º 8
0
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;
}
Exemplo n.º 9
0
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;
   }
}