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)
SparseOptimizerOnline *	optimizer ()

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

SolveResult

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

G2oSlamInterface()

g2o::G2oSlamInterface::G2oSlamInterface

(

SparseOptimizerOnline *

optimizer

)

Definition at line 107 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

addEdge()

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.

                                                                     {
  (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
      Vector7 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
      Vector4::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
      Vector6 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;
}

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::OnlineEdgeSE2::initialEstimate(), g2o::OnlineEdgeSE3::initialEstimate(), g2o::EdgeSE2::initialEstimatePossible(), g2o::EdgeSE3::initialEstimatePossible(), g2o::jac_quat3_euler3(), g2o::OptimizableGraph::jacobianWorkspace(), g2o::BaseEdge< D, E >::setInformation(), g2o::EdgeSE3::setMeasurement(), g2o::EdgeSE2::setMeasurement(), g2o::OptimizableGraph::vertex(), and g2o::HyperGraph::Edge::vertices().

Referenced by main().

addNode()

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.

                                                                {
  // 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;
}

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

Referenced by main().

addVertex()

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

protected

Definition at line 369 of file g2o_slam_interface.cpp.

                                                                         {
  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;
  }
}

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

Referenced by addEdge().

batchSolveEachN()

int g2o::G2oSlamInterface::batchSolveEachN ( ) const

inline

Definition at line 67 of file g2o_slam_interface.h.

{ return _batchEveryN; }

fixNode()

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 332 of file g2o_slam_interface.cpp.

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

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

optimizer()

SparseOptimizerOnline * g2o::G2oSlamInterface::optimizer ( )

inline

Definition at line 70 of file g2o_slam_interface.h.

{ return _optimizer; }

Referenced by solveAndPrint().

printVertex()

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

protected

Definition at line 385 of file g2o_slam_interface.cpp.

                                                            {
  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;
}

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

Referenced by queryState().

queryState()

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.

                                                             {
  // 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;
}

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

setBatchSolveEachN()

void g2o::G2oSlamInterface::setBatchSolveEachN

(

int

n

)

Definition at line 469 of file g2o_slam_interface.cpp.

{ _batchEveryN = n; }

References _batchEveryN.

Referenced by main().

setUpdateGraphEachN()

void g2o::G2oSlamInterface::setUpdateGraphEachN

(

int

n

)

Definition at line 433 of file g2o_slam_interface.cpp.

{ _updateGraphEachN = n; }

References _updateGraphEachN.

Referenced by main().

solve()

G2oSlamInterface::SolveResult g2o::G2oSlamInterface::solve

(

)

Definition at line 435 of file g2o_slam_interface.cpp.

                                                    {
  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;
}

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 solveAndPrint(), and solveState().

solveState()

bool g2o::G2oSlamInterface::solveState ( )

virtual

ask the engine to solve

Returns

true, if successful

Implements SlamParser::AbstractSlamInterface.

Definition at line 364 of file g2o_slam_interface.cpp.

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

References ERROR, and solve().

updatedGraphEachN()

int g2o::G2oSlamInterface::updatedGraphEachN ( ) const

inline

Definition at line 64 of file g2o_slam_interface.h.

{ return _updateGraphEachN; }

Member Data Documentation

_batchEveryN

int g2o::G2oSlamInterface::_batchEveryN

protected

Definition at line 78 of file g2o_slam_interface.h.

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

_edgesAdded

HyperGraph::EdgeSet g2o::G2oSlamInterface::_edgesAdded

protected

Definition at line 83 of file g2o_slam_interface.h.

Referenced by addEdge(), and solve().

_firstOptimization

bool g2o::G2oSlamInterface::_firstOptimization

protected

Definition at line 74 of file g2o_slam_interface.h.

Referenced by solve().

_incIterations

int g2o::G2oSlamInterface::_incIterations

protected

Definition at line 76 of file g2o_slam_interface.h.

Referenced by solve().

_initSolverDone

bool g2o::G2oSlamInterface::_initSolverDone

protected

Definition at line 80 of file g2o_slam_interface.h.

Referenced by addNode().

_lastBatchStep

int g2o::G2oSlamInterface::_lastBatchStep

protected

Definition at line 79 of file g2o_slam_interface.h.

Referenced by solve().

_nodesAdded

int g2o::G2oSlamInterface::_nodesAdded

protected

Definition at line 75 of file g2o_slam_interface.h.

Referenced by addEdge(), and solve().

_optimizer

SparseOptimizerOnline* g2o::G2oSlamInterface::_optimizer

protected

Definition at line 73 of file g2o_slam_interface.h.

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

_updateGraphEachN

int g2o::G2oSlamInterface::_updateGraphEachN

protected

Definition at line 77 of file g2o_slam_interface.h.

Referenced by setUpdateGraphEachN(), and solve().

_verticesAdded

HyperGraph::VertexSet g2o::G2oSlamInterface::_verticesAdded

protected

Definition at line 82 of file g2o_slam_interface.h.

Referenced by addEdge(), and solve().

---

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

• /build/reproducible-path/g2o-0~20230806/g2o/examples/interactive_slam/g2o_interactive/g2o_slam_interface.h
• /build/reproducible-path/g2o-0~20230806/g2o/examples/interactive_slam/g2o_interactive/g2o_slam_interface.cpp