Пример #1
0
//Second cam is fixed, therefore the transformation from the second to the first cam will be computed
std::pair<g2o::VertexSE3*, g2o::VertexSE3*>  sensorVerticesSetup(g2o::SparseOptimizer& optimizer, Eigen::Matrix4f& tf_estimate){
    g2o::VertexSE3 *vc2 = new VertexSE3();
    {// set up rotation and translation for the newer node as identity
      Eigen::Quaterniond q(1,0,0,0);
      Eigen::Vector3d t(0,0,0);
      q.setIdentity();
      g2o::SE3Quat cam2(q,t);

      vc2->setEstimate(cam2);
      vc2->setId(1); 
      vc2->setFixed(true);
      optimizer.addVertex(vc2);
    }

    g2o::VertexSE3 *vc1 = new VertexSE3();
    {
      Eigen::Quaterniond q(tf_estimate.topLeftCorner<3,3>().cast<double>());//initialize rotation from estimate 
      Eigen::Vector3d t(tf_estimate.topRightCorner<3,1>().cast<double>());  //initialize translation from estimate
      g2o::SE3Quat cam1(q,t);
      vc1->setEstimate(cam1);
      vc1->setId(0);  
      optimizer.addVertex(vc1);
    }

    // add to optimizer
    return std::make_pair(vc1, vc2);
}
Пример #2
0
//!Set parameters for icp optimizer
void optimizerSetup(g2o::SparseOptimizer& optimizer){
  //optimizer.setMethod(g2o::SparseOptimizer::LevenbergMarquardt);
  //g2o::Optimizato
  optimizer.setVerbose(false);

  // variable-size block solver
  g2o::BlockSolverX::LinearSolverType * linearSolver
      = new g2o::LinearSolverCholmod<g2o ::BlockSolverX::PoseMatrixType>();


  g2o::BlockSolverX * solver_ptr
      = new g2o::BlockSolverX(linearSolver);

  g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton(solver_ptr);
  optimizer.setAlgorithm(solver);
  //optimizer.setSolver(solver_ptr);

  g2o::ParameterCamera* cameraParams = new g2o::ParameterCamera();
  //FIXME From Parameter server or cam calibration
  cameraParams->setKcam(521,521,319.5,239.5);
  g2o::SE3Quat offset; // identity
  cameraParams->setOffset(offset);
  cameraParams->setId(0);
  optimizer.addParameter(cameraParams);
}
Пример #3
0
  g2o::VertexSE3* addFrameVertex(const ros::Time& timestamp)
  {
    g2o::VertexSE3* frame_vertex = new g2o::VertexSE3();
    frame_vertex->setId(max_vertex_id_++);
    frame_vertex->setUserData(new Timestamped(timestamp));

    if(!graph_.addVertex(frame_vertex))
    {
      throw std::runtime_error("failed to add vertex to g2o graph!");
    }

    return frame_vertex;
  }
Пример #4
0
  LocalMapImpl(const dvo::core::RgbdImagePyramid::Ptr& keyframe, const dvo::core::AffineTransformd& keyframe_pose) :
    keyframe_(keyframe),
    keyframe_vertex_(0),
    previous_vertex_(0),
    current_vertex_(0),
    max_vertex_id_(1),
    max_edge_id_(1)
  {
    // g2o setup
    graph_.setAlgorithm(
        new g2o::OptimizationAlgorithmLevenberg(
            new BlockSolver(
                new LinearSolver()
            )
        )
    );
    graph_.setVerbose(false);

    keyframe_vertex_ = addFrameVertex(ros::Time(keyframe->timestamp()));
    keyframe_vertex_->setFixed(true);
    keyframe_vertex_->setEstimate(toIsometry(keyframe_pose));
  }
Пример #5
0
  g2o::EdgeSE3* addTransformationEdge(g2o::VertexSE3 *from, g2o::VertexSE3 *to, const dvo::core::AffineTransformd& transform, const dvo::core::Matrix6d& information)
  {
    assert(from != 0 && to != 0);

    g2o::EdgeSE3* edge = new g2o::EdgeSE3();
    edge->setId(max_edge_id_++);
    edge->resize(2);
    edge->setVertex(0, from);
    edge->setVertex(1, to);
    edge->setMeasurement(toIsometry(transform));
    edge->setInformation(information);

    graph_.addEdge(edge);

    return edge;
  }
Пример #6
0
void test()
{
	Isometry3d T = Eigen::Isometry3d::Identity();
	T(0,0)=1;
	T(0,1)=0;
	T(0,2)=0;
	T(1,0)=0;
	T(1,1)=0.707;
	T(1,2)=-0.707;
	T(2,0)=0;
	T(2,1)=0.707;
	T(2,2)=0.707;
	T(0,3)=0;
	T(1,3)=0;
	T(2,3)=0.5;

	cout<<T.matrix()<<endl;
	
	for (int i = 2; i <=8; i++)
	{
		g2o::EdgeSE3* edge = new g2o::EdgeSE3();
		g2o::VertexSE3 *v = new g2o::VertexSE3();
		
		v->setId(i);
		v->setEstimate( Eigen::Isometry3d::Identity() );
	        m_globalOptimizer.addVertex(v);
	        
	        edge->vertices() [0] = m_globalOptimizer.vertex( i-1 );
	        edge->vertices() [1] = m_globalOptimizer.vertex( i );
	        
	        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 );
	        m_globalOptimizer.addEdge(edge);

		
		
	}
	
#if 0
	T = Eigen::Isometry3d::Identity();
	//T(2,3)=1;
	//cout<<T.matrix()<<endl;
	
	g2o::EdgeSE3* edge = new g2o::EdgeSE3();
		g2o::VertexSE3 *v = new g2o::VertexSE3();
		
		v->setId(8);
		v->setEstimate( Eigen::Isometry3d::Identity() );
	        m_globalOptimizer.addVertex(v);
	        
	        edge->vertices() [0] = m_globalOptimizer.vertex( 1 );
	        edge->vertices() [1] = m_globalOptimizer.vertex( 8 );
	        
	        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 );
	        m_globalOptimizer.addEdge(edge);
	

#endif
	
	m_globalOptimizer.save("pa.g2o");
	m_globalOptimizer.initializeOptimization();
	//m_globalOptimizer.optimize(50);
	m_globalOptimizer.save("pa2.g2o");
	cout<<"f**k"<<endl;
	VertexSE3* vertex = dynamic_cast<VertexSE3*>(m_globalOptimizer.vertex(2));
	Isometry3d pose = vertex -> estimate();
	//cout << "matrix:" << endl;
	cout << pose.matrix() << endl;
	
}