g2o::G2oSlamInterface Class Reference

#include <g2o_slam_interface.h>

Inheritance diagram for g2o::G2oSlamInterface:

Collaboration diagram for g2o::G2oSlamInterface:

Public Types
enum	SolveResult { SOLVED, SOLVED_BATCH, NOOP, ERROR }

Public Member Functions
	G2oSlamInterface (SparseOptimizerOnline *optimizer)
bool	addNode (const std::string &tag, int id, int dimension, const std::vector< double > &values)
bool	addEdge (const std::string &tag, int id, int dimension, int v1, int v2, const std::vector< double > &measurement, const std::vector< double > &information)
bool	fixNode (const std::vector< int > &nodes)
bool	queryState (const std::vector< int > &nodes)
bool	solveState ()
SolveResult	solve ()
int	updatedGraphEachN () const
void	setUpdateGraphEachN (int n)
int	batchSolveEachN () const
void	setBatchSolveEachN (int n)

Protected Member Functions
OptimizableGraph::Vertex *	addVertex (int dimension, int id)
bool	printVertex (OptimizableGraph::Vertex *v)

Protected Attributes
SparseOptimizerOnline *	_optimizer
bool	_firstOptimization
int	_nodesAdded
int	_incIterations
int	_updateGraphEachN
int	_batchEveryN
int	_lastBatchStep
bool	_initSolverDone
HyperGraph::VertexSet	_verticesAdded
HyperGraph::EdgeSet	_edgesAdded

Detailed Description

Definition at line 41 of file g2o_slam_interface.h.

Member Enumeration Documentation

enum g2o::G2oSlamInterface::SolveResult

Enumerator
SOLVED
SOLVED_BATCH
NOOP
ERROR

Definition at line 44 of file g2o_slam_interface.h.

{ SOLVED, SOLVED_BATCH, NOOP, ERROR };

Constructor & Destructor Documentation

g2o::G2oSlamInterface::G2oSlamInterface

(

SparseOptimizerOnline *

optimizer

)

Definition at line 110 of file g2o_slam_interface.cpp.

                                                                   :
  _optimizer(optimizer), _firstOptimization(true), _nodesAdded(0),
  _incIterations(1), _updateGraphEachN(10), _batchEveryN(100),
  _lastBatchStep(0), _initSolverDone(false)
{
}

Member Function Documentation

bool g2o::G2oSlamInterface::addEdge ( const std::string &  tag, int  id, int  dimension, int  v1, int  v2, const std::vector< double > &  measurement, const std::vector< double > &  information  )

virtual

adding an edge to the SLAM engine.

Parameters

tag	the tag specifying the type of the vertex
id	the unique id of the edge.
dimension	the dimension of the edge.
v1	the unique id of the edge of the first vertex
v2	the unique id of the edge of the second vertex
measurement	the measurement of the constraint
information	the information matrix (inverse of the covariance) representing the uncertainty of the measurement (row-major upper triangular and diagonal)

Returns

true, if adding was successful

Implements SlamParser::AbstractSlamInterface.

Definition at line 133 of file g2o_slam_interface.cpp.

References __PRETTY_FUNCTION__, _edgesAdded, _nodesAdded, _optimizer, _verticesAdded, g2o::OptimizableGraph::addEdge(), addVertex(), g2o::JacobianWorkspace::allocate(), g2o::HyperGraph::edges(), g2o::internal::fromVectorET(), g2o::internal::fromVectorQT(), g2o::HyperGraph::Vertex::id(), g2o::OnlineEdgeSE3::initialEstimate(), g2o::OnlineEdgeSE2::initialEstimate(), g2o::EdgeSE3::initialEstimatePossible(), g2o::EdgeSE2::initialEstimatePossible(), g2o::jac_quat3_euler3(), g2o::OptimizableGraph::jacobianWorkspace(), g2o::internal::normalize(), g2o::BaseEdge< D, E >::setInformation(), g2o::EdgeSE3::setMeasurement(), g2o::EdgeSE2::setMeasurement(), g2o::OptimizableGraph::vertex(), and g2o::HyperGraph::Edge::vertices().

Referenced by main().

{
  (void) tag;
  (void) id;
  size_t oldEdgesSize = _optimizer->edges().size();

  if (dimension == 3) {

    SE2 transf(measurement[0], measurement[1], measurement[2]);
    Eigen::Matrix3d infMat;
    int idx = 0;
    for (int r = 0; r < 3; ++r)
      for (int c = r; c < 3; ++c, ++idx) {
        assert(idx < (int)information.size());
        infMat(r,c) = infMat(c,r) = information[idx];
      }
    //cerr << PVAR(infMat) << endl;

    int doInit = 0;
    SparseOptimizer::Vertex* v1 = _optimizer->vertex(v1Id);
    SparseOptimizer::Vertex* v2 = _optimizer->vertex(v2Id);
    if (! v1) {
      OptimizableGraph::Vertex* v = v1 = addVertex(dimension, v1Id);
      _verticesAdded.insert(v);
      doInit = 1;
      ++_nodesAdded;
    }

    if (! v2) {
      OptimizableGraph::Vertex* v = v2 = addVertex(dimension, v2Id);
      _verticesAdded.insert(v);
      doInit = 2;
      ++_nodesAdded;
    }

    if (_optimizer->edges().size() == 0) {
      cerr << "FIRST EDGE ";
      if (v1->id() < v2->id()) {
        cerr << "fixing " << v1->id() << endl;
        v1->setFixed(true);
      }
      else {
        cerr << "fixing " << v2->id() << endl;
        v2->setFixed(true);
      }
    }

    OnlineEdgeSE2* e = new OnlineEdgeSE2;
    e->vertices()[0] = v1;
    e->vertices()[1] = v2;
    e->setMeasurement(transf);
    e->setInformation(infMat);
    _optimizer->addEdge(e);
    _edgesAdded.insert(e);

    if (doInit) {
      OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
      OptimizableGraph::Vertex* to   = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
      switch (doInit){
        case 1: // initialize v1 from v2
          {
            HyperGraph::VertexSet toSet;
            toSet.insert(to);
            if (e->initialEstimatePossible(toSet, from) > 0.) {
              e->initialEstimate(toSet, from);
            }
            break;
          }
        case 2: 
          {
            HyperGraph::VertexSet fromSet;
            fromSet.insert(from);
            if (e->initialEstimatePossible(fromSet, to) > 0.) {
              e->initialEstimate(fromSet, to);  
            }
            break;
          }
        default: cerr << "doInit wrong value\n"; 
      }
    }

  }
  else if (dimension == 6) {

    Eigen::Isometry3d transf;
    Matrix<double, 6, 6> infMat;

    if (measurement.size() == 7) { // measurement is a Quaternion
      Vector7d meas;
      for (int i=0; i<7; ++i) 
        meas(i) = measurement[i];
      // normalize the quaternion to recover numerical precision lost by storing as human readable text
      Vector4d::MapType(meas.data()+3).normalize();
      transf = internal::fromVectorQT(meas);

      for (int i = 0, idx = 0; i < infMat.rows(); ++i)
        for (int j = i; j < infMat.cols(); ++j){
          infMat(i,j) = information[idx++];
          if (i != j)
            infMat(j,i)=infMat(i,j);
        }
    } else { // measurement consists of Euler angles
      Vector6d aux;
      aux << measurement[0], measurement[1], measurement[2],measurement[3], measurement[4], measurement[5];
      transf = internal::fromVectorET(aux);
      Matrix<double, 6, 6> infMatEuler;
      int idx = 0;
      for (int r = 0; r < 6; ++r)
        for (int c = r; c < 6; ++c, ++idx) {
          assert(idx < (int)information.size());
          infMatEuler(r,c) = infMatEuler(c,r) = information[idx];
        }
      // convert information matrix to our internal representation
      Matrix<double, 6, 6> J;
      SE3Quat transfAsSe3(transf.matrix().topLeftCorner<3,3>(), transf.translation());
      jac_quat3_euler3(J, transfAsSe3);
      infMat.noalias() = J.transpose() * infMatEuler * J;
      //cerr << PVAR(transf.matrix()) << endl;
      //cerr << PVAR(infMat) << endl;
    }

    int doInit = 0;
    SparseOptimizer::Vertex* v1 = _optimizer->vertex(v1Id);
    SparseOptimizer::Vertex* v2 = _optimizer->vertex(v2Id);
    if (! v1) {
      OptimizableGraph::Vertex* v = v1 = addVertex(dimension, v1Id);
      _verticesAdded.insert(v);
      doInit = 1;
      ++_nodesAdded;
    }

    if (! v2) {
      OptimizableGraph::Vertex* v = v2 = addVertex(dimension, v2Id);
      _verticesAdded.insert(v);
      doInit = 2;
      ++_nodesAdded;
    }

    if (_optimizer->edges().size() == 0) {
      cerr << "FIRST EDGE ";
      if (v1->id() < v2->id()) {
        cerr << "fixing " << v1->id() << endl;
        v1->setFixed(true);
      }
      else {
        cerr << "fixing " << v2->id() << endl;
        v2->setFixed(true);
      }
    }

    OnlineEdgeSE3* e = new OnlineEdgeSE3;
    e->vertices()[0] = v1;
    e->vertices()[1] = v2;
    e->setMeasurement(transf);
    e->setInformation(infMat);
    _optimizer->addEdge(e);
    _edgesAdded.insert(e);

    if (doInit) {
      OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
      OptimizableGraph::Vertex* to   = static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
      switch (doInit){
        case 1: // initialize v1 from v2
          {
            HyperGraph::VertexSet toSet;
            toSet.insert(to);
            if (e->initialEstimatePossible(toSet, from) > 0.) {
              e->initialEstimate(toSet, from);
            }
            break;
          }
        case 2: 
          {
            HyperGraph::VertexSet fromSet;
            fromSet.insert(from);
            if (e->initialEstimatePossible(fromSet, to) > 0.) {
              e->initialEstimate(fromSet, to);  
            }
            break;
          }
        default: cerr << "doInit wrong value\n"; 
      }
    }

  }
  else {
    cerr << __PRETTY_FUNCTION__ << " not implemented for this dimension" << endl;
    return false;
  }

  if (oldEdgesSize == 0) {
    _optimizer->jacobianWorkspace().allocate();
  }

  return true;
}

bool g2o::G2oSlamInterface::addNode ( const std::string &  tag, int  id, int  dimension, const std::vector< double > &  values  )

virtual

adding a node to the SLAM engine.

Parameters

tag	the tag specifying the type of the vertex
id	the unique id of the node.
dimension	the dimension of the node.
values	the pose of the node, may be empty (i.e., the engine should initialize the node itself)

Returns

true, if adding was successful

Implements SlamParser::AbstractSlamInterface.

Definition at line 117 of file g2o_slam_interface.cpp.

References _batchEveryN, _initSolverDone, _optimizer, and g2o::SparseOptimizerOnline::initSolver().

Referenced by main().

{
  // allocating the desired solver + testing whether the solver is okay
  if (! _initSolverDone) {
    _initSolverDone = true;
    _optimizer->initSolver(dimension, _batchEveryN);
  }

  // we add the node when we are asked to add the according edge
  (void) tag;
  (void) id;
  (void) dimension;
  (void) values;
  return true;
}

OptimizableGraph::Vertex * g2o::G2oSlamInterface::addVertex ( int  dimension, int  id  )

protected

Definition at line 369 of file g2o_slam_interface.cpp.

References _optimizer, g2o::OptimizableGraph::addVertex(), and g2o::OptimizableGraph::Vertex::setId().

Referenced by addEdge().

{
  if (dimension == 3) {
    OnlineVertexSE2* v =  new OnlineVertexSE2;
    v->setId(id); // estimate will be set later when the edge is added
    _optimizer->addVertex(v);
    return v;
  }
  else if (dimension == 6) {
    OnlineVertexSE3* v =  new OnlineVertexSE3;
    v->setId(id); // estimate will be set later when the edge is added
    _optimizer->addVertex(v);
    return v;
  }
  else {
    return 0;
  }
}

int g2o::G2oSlamInterface::batchSolveEachN ( ) const

inline

Definition at line 64 of file g2o_slam_interface.h.

{ return _batchEveryN;}

bool g2o::G2oSlamInterface::fixNode ( const std::vector< int > &  nodes )

virtual

set some nodes to a fixed position

Parameters

nodes

the list of vertex IDs to fix

Returns

true, if successful

Implements SlamParser::AbstractSlamInterface.

Definition at line 330 of file g2o_slam_interface.cpp.

References _optimizer, g2o::OptimizableGraph::Vertex::setFixed(), and g2o::OptimizableGraph::vertex().

{
  for (size_t i = 0; i < nodes.size(); ++i) {
    OptimizableGraph::Vertex* v = _optimizer->vertex(nodes[i]);
    if (v)
      v->setFixed(true);
  }
  return true;
}

bool g2o::G2oSlamInterface::printVertex ( OptimizableGraph::Vertex *  v )

protected

Definition at line 388 of file g2o_slam_interface.cpp.

References g2o::OptimizableGraph::Vertex::dimension(), g2o::HyperGraph::Vertex::id(), modp_dtoa(), modp_itoa10(), g2o::SE2::rotation(), g2o::internal::toEuler(), g2o::SE2::translation(), g2o::OnlineVertexSE3::updatedEstimate, and g2o::OnlineVertexSE2::updatedEstimate.

Referenced by queryState().

{
  static char buffer[10000]; // that should be more than enough
  int vdim = v->dimension();
  if (vdim == 3) {
    char* s = buffer;
    OnlineVertexSE2* v2 = static_cast<OnlineVertexSE2*>(v);
    memcpy(s, "VERTEX_XYT ", 11);
    s += 11;
    s += modp_itoa10(v->id(), s);
    *s++ = ' ';
    s += modp_dtoa(v2->updatedEstimate.translation().x(), s, 6);
    *s++ = ' ';
    s += modp_dtoa(v2->updatedEstimate.translation().y(), s, 6);
    *s++ = ' ';
    s += modp_dtoa(v2->updatedEstimate.rotation().angle(), s, 6);
    *s++ = '\n';
    cout.write(buffer, s - buffer);
    return true;
  }
  else if (vdim == 6) {
    char* s = buffer;
    OnlineVertexSE3* v3 = static_cast<OnlineVertexSE3*>(v);
    Vector3d eulerAngles = internal::toEuler(v3->updatedEstimate.matrix().topLeftCorner<3,3>());
    const double& roll = eulerAngles(0);
    const double& pitch = eulerAngles(1);
    const double& yaw = eulerAngles(2);
    memcpy(s, "VERTEX_XYZRPY ", 14);
    s += 14;
    s += modp_itoa10(v->id(), s);
    *s++ = ' ';
    s += modp_dtoa(v3->updatedEstimate.translation().x(), s, 6);
    *s++ = ' ';
    s += modp_dtoa(v3->updatedEstimate.translation().y(), s, 6);
    *s++ = ' ';
    s += modp_dtoa(v3->updatedEstimate.translation().z(), s, 6);
    *s++ = ' ';
    s += modp_dtoa(roll, s, 6);
    *s++ = ' ';
    s += modp_dtoa(pitch, s, 6);
    *s++ = ' ';
    s += modp_dtoa(yaw, s, 6);
    *s++ = '\n';
    cout.write(buffer, s - buffer);
    return true;
  }
  return false;
}

bool g2o::G2oSlamInterface::queryState ( const std::vector< int > &  nodes )

virtual

Ask the SLAM engine to print the current estimate of the variables

Parameters

nodes

the list of vertex IDs to print, if empty print all variables

Returns

true, if successful

Implements SlamParser::AbstractSlamInterface.

Definition at line 340 of file g2o_slam_interface.cpp.

References _optimizer, printVertex(), g2o::OptimizableGraph::vertex(), and g2o::HyperGraph::vertices().

{
  //return true;
  cout << "BEGIN" << endl;
#if 1
  if (nodes.size() == 0) {
    for (OptimizableGraph::VertexIDMap::const_iterator it = _optimizer->vertices().begin(); it != _optimizer->vertices().end(); ++it) {
      OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
      printVertex(v);
    }
  } else {
    for (size_t i = 0; i < nodes.size(); ++i) {
      OptimizableGraph::Vertex* v = _optimizer->vertex(nodes[i]);
      if (v)
        printVertex(v);
    }
  }
#endif
  cout << "END" << endl << flush;

  return true;
}

void g2o::G2oSlamInterface::setBatchSolveEachN

(

int

n

)

Definition at line 478 of file g2o_slam_interface.cpp.

References _batchEveryN.

Referenced by main().

{
  _batchEveryN = n;
}

void g2o::G2oSlamInterface::setUpdateGraphEachN

(

int

n

)

Definition at line 437 of file g2o_slam_interface.cpp.

References _updateGraphEachN.

Referenced by main().

{
  _updateGraphEachN = n;
}

G2oSlamInterface::SolveResult g2o::G2oSlamInterface::solve

(

)

Definition at line 442 of file g2o_slam_interface.cpp.

References _batchEveryN, _edgesAdded, _firstOptimization, _incIterations, _lastBatchStep, _nodesAdded, _optimizer, _updateGraphEachN, _verticesAdded, g2o::SparseOptimizerOnline::batchStep, ERROR, g2o::SparseOptimizer::initializeOptimization(), NOOP, g2o::SparseOptimizerOnline::optimize(), SOLVED, SOLVED_BATCH, g2o::SparseOptimizerOnline::updateInitialization(), and g2o::HyperGraph::vertices().

Referenced by main(), and solveState().

{
  if (_nodesAdded >= _updateGraphEachN) {

    // decide on batch step or normal step
    _optimizer->batchStep = false;
    if ((int)_optimizer->vertices().size() - _lastBatchStep >= _batchEveryN) {
      _lastBatchStep = _optimizer->vertices().size();
      _optimizer->batchStep = true;
    }

    if (_firstOptimization) {
      if (!_optimizer->initializeOptimization()){
        cerr << "initialization failed" << endl;
        return ERROR;
      }
    } else {
      if (! _optimizer->updateInitialization(_verticesAdded, _edgesAdded)) {
        cerr << "updating initialization failed" << endl;
        return ERROR;
      }
    }

    int currentIt = _optimizer->optimize(_incIterations, !_firstOptimization); (void) currentIt;
    _firstOptimization = false;
    _nodesAdded = 0;
    _verticesAdded.clear();
    _edgesAdded.clear();
    if (_optimizer->batchStep)
      return SOLVED_BATCH;
    return SOLVED;
  }

  return NOOP;
}

bool g2o::G2oSlamInterface::solveState ( )

virtual

ask the engine to solve

Returns

true, if successful

Implements SlamParser::AbstractSlamInterface.

Definition at line 363 of file g2o_slam_interface.cpp.

References ERROR, and solve().

{
  SolveResult state = solve();
  return state != ERROR;
}

int g2o::G2oSlamInterface::updatedGraphEachN ( ) const

inline

Definition at line 61 of file g2o_slam_interface.h.

{ return _updateGraphEachN;}

Member Data Documentation

int g2o::G2oSlamInterface::_batchEveryN

protected

Definition at line 73 of file g2o_slam_interface.h.

Referenced by addNode(), setBatchSolveEachN(), and solve().

HyperGraph::EdgeSet g2o::G2oSlamInterface::_edgesAdded

protected

Definition at line 78 of file g2o_slam_interface.h.

Referenced by addEdge(), and solve().

bool g2o::G2oSlamInterface::_firstOptimization

protected

Definition at line 69 of file g2o_slam_interface.h.

Referenced by solve().

int g2o::G2oSlamInterface::_incIterations

protected

Definition at line 71 of file g2o_slam_interface.h.

Referenced by solve().

bool g2o::G2oSlamInterface::_initSolverDone

protected

Definition at line 75 of file g2o_slam_interface.h.

Referenced by addNode().

int g2o::G2oSlamInterface::_lastBatchStep

protected

Definition at line 74 of file g2o_slam_interface.h.

Referenced by solve().

int g2o::G2oSlamInterface::_nodesAdded

protected

Definition at line 70 of file g2o_slam_interface.h.

Referenced by addEdge(), and solve().

SparseOptimizerOnline* g2o::G2oSlamInterface::_optimizer

protected

Definition at line 68 of file g2o_slam_interface.h.

Referenced by addEdge(), addNode(), addVertex(), fixNode(), queryState(), and solve().

int g2o::G2oSlamInterface::_updateGraphEachN

protected

Definition at line 72 of file g2o_slam_interface.h.

Referenced by setUpdateGraphEachN(), and solve().

HyperGraph::VertexSet g2o::G2oSlamInterface::_verticesAdded

protected

Definition at line 77 of file g2o_slam_interface.h.

Referenced by addEdge(), and solve().

---

The documentation for this class was generated from the following files:

• /home/xuezhisd/CLionProjects/g2o/g2o/examples/interactive_slam/g2o_interactive/g2o_slam_interface.h
• /home/xuezhisd/CLionProjects/g2o/g2o/examples/interactive_slam/g2o_interactive/g2o_slam_interface.cpp