コード例 #1
0
ファイル: slam.cpp プロジェクト: longchao9527/slam
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;
}
コード例 #2
0
ファイル: edge_se3.cpp プロジェクト: cvfish/g2o
 HyperGraphElementAction* EdgeSE3DrawAction::operator()(HyperGraph::HyperGraphElement* element, 
              HyperGraphElementAction::Parameters* params_){
   if (typeid(*element).name()!=_typeName)
     return 0;
   refreshPropertyPtrs(params_);
   if (! _previousParams)
     return this;
   
   if (_show && !_show->value())
     return this;
   
   EdgeSE3* e =  static_cast<EdgeSE3*>(element);
   VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
   VertexSE3* toEdge   = static_cast<VertexSE3*>(e->vertices()[1]);
   if (! fromEdge || ! toEdge)
     return this;
   glColor3f(POSE_EDGE_COLOR);
   glPushAttrib(GL_ENABLE_BIT);
   glDisable(GL_LIGHTING);
   glBegin(GL_LINES);
   glVertex3f((float)fromEdge->estimate().translation().x(),(float)fromEdge->estimate().translation().y(),(float)fromEdge->estimate().translation().z());
   glVertex3f((float)toEdge->estimate().translation().x(),(float)toEdge->estimate().translation().y(),(float)toEdge->estimate().translation().z());
   glEnd();
   glPopAttrib();
   return this;
 }
コード例 #3
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 );
        }
    }
}
コード例 #4
0
ファイル: GraphicEnd.cpp プロジェクト: tyuownu/ubuntu_opencv
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;
    }
    
}
コード例 #5
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 );
         
    }
}
コード例 #6
0
ファイル: edge_se3.cpp プロジェクト: rmihalyi/g2o
HyperGraphElementAction* EdgeSE3WriteGnuplotAction::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params_) {
    if (typeid(*element).name()!=_typeName)
        return 0;
    WriteGnuplotAction::Parameters* params=static_cast<WriteGnuplotAction::Parameters*>(params_);
    if (!params->os) {
        std::cerr << __PRETTY_FUNCTION__ << ": warning, on valid os specified" << std::endl;
        return 0;
    }

    EdgeSE3* e =  static_cast<EdgeSE3*>(element);
    VertexSE3* fromEdge = static_cast<VertexSE3*>(e->vertices()[0]);
    VertexSE3* toEdge   = static_cast<VertexSE3*>(e->vertices()[1]);
    Vector6d fromV, toV;
    fromV=toVectorMQT(fromEdge->estimate());
    toV=toVectorMQT(toEdge->estimate());
    for (int i=0; i<6; i++) {
        *(params->os) << fromV[i] << " ";
    }
    for (int i=0; i<6; i++) {
        *(params->os) << toV[i] << " ";
    }
    *(params->os) << std::endl;
    return this;
}
コード例 #7
0
ファイル: GraphicEnd.cpp プロジェクト: tyuownu/ubuntu_opencv
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 );
    }
    
}
コード例 #8
0
ファイル: GraphicEnd.cpp プロジェクト: tyuownu/ubuntu_opencv
//回环检测:在过去的帧中随机取_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;
}
コード例 #9
0
ファイル: Logger.cpp プロジェクト: 9578577/g2o_frontend
void writeQueue() {
  SensorData* data;
  std::ofstream ofG2O(&filename[0]);
  geometry_msgs::TransformStamped msg;
  int num = 0;

  
  // this is the vertex where we are packing the data
  g2o::VertexSE3* activeVertex = 0;
  // this is the timestamp of the first measurement added to the vertex
  double activeVertexTime=0;

  // this is the previous vertex
  g2o::VertexSE3* previousVertex = 0;
  // this is the timestamp of the first measurement added to the previous vertex
  double previousVertexTime=0;

  // the last position of the robot (not of the vertex, needed to measure the distances)
  Eigen::Isometry3d lastRobotPose;

  // set of sensors we packed in the current data. 
  // We do not want to put 10 camera images of the same camera in the same vertex.
  std::set<Sensor*> addedSensors;

  Eigen::Vector2d distances(0.,0);
  while (true)
  {
    if(! _queue.empty())
    {
      data = (SensorData*)_queue.front();
      double timeNow = _queue.lastElementTime();
      conditionalPrint(annoyingLevel) <<  "size=" << _queue.size() << " lastTime=" << FIXED(timeNow) << endl;
      if (timeNow - data->timeStamp()> initialDelay)
      { // we have enough stuff in the queue
	_queue.pop_front();
	if (! nptr->ok())
	  continue;
	
	tf::StampedTransform transform;
	bool we_got_transf = false;
	try
	{
	  ros::Time timeStamp;
	  // Get transformation
	  (*tfListener).lookupTransform("/odom", "/base_link", timeStamp.fromSec(data->timeStamp()), transform);
	  we_got_transf = true;
	}
	catch (tf::TransformException & ex)
	{
	  ROS_ERROR("%s", ex.what());
	}

	if (! we_got_transf)
	  continue;
				
	Eigen::Isometry3d currentRobotPose = fromStampedTransform(transform);
	double currentDataTime = data->timeStamp();
	distances += isometry2distance(lastRobotPose.inverse()*currentRobotPose);
	double passedTime = currentDataTime-previousVertexTime;
	lastRobotPose = currentRobotPose;

	conditionalPrint(annoyingLevel) << "distances: " << distances[0] << " " << distances[1] <<  " " << passedTime << endl;
	if (distances[0] < minDistances[0] && 
	    distances[1] < minDistances[1] && 
	    passedTime   < minTime){
	  conditionalPrint(annoyingLevel) << "reject: (time/distance)" << endl;
	  // SKIP THE FRAME
	  delete data;
	  data = 0;
	  continue;
	} 

	if (!activeVertex) {
	  activeVertex = new g2o::VertexSE3();
	  activeVertex->setId(num);
	  activeVertex->setEstimate(fromStampedTransform(transform));
	  activeVertexTime = currentDataTime;
	}
						
	Sensor* sensor = data->sensor();
	assert (sensor && "!");
						
	// check if we already packed the data for this kind of sensor
	if (addedSensors.count(sensor)){
	  conditionalPrint(annoyingLevel) << "reject: (sensor) "<< endl;
	  delete data;
	} else {
	  addedSensors.insert(sensor);
	  Parameter* parameter = sensor->parameter();
	  assert (parameter && "!@#");
	  //data->writeOut(filename);
	  if (! graph->parameters().getParameter(parameter->id())){
	    graph->parameters().addParameter(parameter);
	    graph->saveParameter(ofG2O, parameter);
	  }
					
	  activeVertex->addUserData(data);
	  data->setDataContainer(activeVertex);
	}
	// detach the data from the thing
	data = 0;

	if (currentDataTime - activeVertexTime > vertexTimeWindow) {
	  conditionalPrint(annoyingLevel) << "flush" << endl;
	  graph->addVertex(activeVertex);
	  graph->saveVertex(ofG2O, activeVertex);
	  				
	  if (previousVertex) {
	    EdgeSE3* e = new EdgeSE3();
	    e->setVertex(0, previousVertex);
	    e->setVertex(1, activeVertex);
	    e->setMeasurementFromState();
	    Eigen::Matrix<double, 6,6> m;
	    m.setIdentity();
	    e->setInformation(m);
	    graph->addEdge(e);
	    graph->saveEdge(ofG2O, e);

	    // JACP: do not do the remove, scan the data list and do a release() of the images which are big. The rest can stay in memory
	    g2o::HyperGraph::Data* d = previousVertex->userData();
	    while (d) {
	      RGBDData* rgbd = dynamic_cast<RGBDData*> (d);
	      if (rgbd)
		rgbd->release();
	      d = d->next();
	    }
	    vertecesQueue.push_back(previousVertex);
	  }
					
	  previousVertex = activeVertex;
	  previousVertexTime = activeVertexTime;
	  addedSensors.clear();
	  activeVertex = 0;
	  distances.setZero();
	  num++;
	  conditionalPrint(defaultLevel) << ".";
	}
      }
    }
    usleep(20e3); // queue is emp-ty
  }
}
コード例 #10
0
ファイル: Optimizer.cpp プロジェクト: rickytan/KALOFution
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]);
        }
    }
}
コード例 #11
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;
}
コード例 #12
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;
   }
}