g2o::SolverSLAM2DLinear Class Reference

Implementation of a linear approximation for 2D pose graph SLAM. More...

#include <solver_slam2d_linear.h>

Inheritance diagram for g2o::SolverSLAM2DLinear:

Collaboration diagram for g2o::SolverSLAM2DLinear:

Public Member Functions
	SolverSLAM2DLinear (std::unique_ptr< Solver > solver)
virtual	~SolverSLAM2DLinear ()
virtual OptimizationAlgorithm::SolverResult	solve (int iteration, bool online=false)
Public Member Functions inherited from g2o::OptimizationAlgorithmGaussNewton
	OptimizationAlgorithmGaussNewton (std::unique_ptr< Solver > solver)
virtual	~OptimizationAlgorithmGaussNewton ()
virtual void	printVerbose (std::ostream &os) const
Public Member Functions inherited from g2o::OptimizationAlgorithmWithHessian
	OptimizationAlgorithmWithHessian (Solver &solver)
virtual	~OptimizationAlgorithmWithHessian ()
virtual bool	init (bool online=false)
virtual bool	computeMarginals (SparseBlockMatrix< MatrixX > &spinv, const std::vector< std::pair< int, int > > &blockIndices)
virtual bool	buildLinearStructure ()
virtual void	updateLinearSystem ()
virtual bool	updateStructure (const std::vector< HyperGraph::Vertex * > &vset, const HyperGraph::EdgeSet &edges)
Solver &	solver ()
	return the underlying solver used to solve the linear system
virtual void	setWriteDebug (bool writeDebug)
virtual bool	writeDebug () const
Public Member Functions inherited from g2o::OptimizationAlgorithm
	OptimizationAlgorithm ()
virtual	~OptimizationAlgorithm ()
const SparseOptimizer *	optimizer () const
	return the optimizer operating on
SparseOptimizer *	optimizer ()
void	setOptimizer (SparseOptimizer *optimizer)
const PropertyMap &	properties () const
	return the properties of the solver
bool	updatePropertiesFromString (const std::string &propString)
void	printProperties (std::ostream &os) const

Protected Member Functions
bool	solveOrientation ()

Additional Inherited Members
Protected Attributes inherited from g2o::OptimizationAlgorithmWithHessian
Solver &	_solver
Property< bool > *	_writeDebug
Protected Attributes inherited from g2o::OptimizationAlgorithm
SparseOptimizer *	_optimizer
	the optimizer the solver is working on
PropertyMap	_properties

Detailed Description

Implementation of a linear approximation for 2D pose graph SLAM.

Needs to operate on the full graph, whereas the nodes connected by odometry are 0 -> 1 -> 2 -> ... Furthermore exactly one node should be the fixed vertex. Within the first iteration the orientation of the nodes is computed. In the subsequent iterations full non-linear GN is carried out. The linear approximation is correct, if the covariance of the constraints is a diagonal matrix.

More or less the solver is an implementation of the approach described by Carlone et al, RSS'11.

Definition at line 49 of file solver_slam2d_linear.h.

Constructor & Destructor Documentation

SolverSLAM2DLinear()

g2o::SolverSLAM2DLinear::SolverSLAM2DLinear ( std::unique_ptr< Solver >  solver )

explicit

Construct a Solver for solving 2D pose graphs. Within the first iteration the rotations are solved and afterwards standard non-linear Gauss Newton is carried out.

Definition at line 75 of file solver_slam2d_linear.cpp.

    : OptimizationAlgorithmGaussNewton(std::move(solver)) {}

~SolverSLAM2DLinear()

g2o::SolverSLAM2DLinear::~SolverSLAM2DLinear ( )

virtual

Definition at line 78 of file solver_slam2d_linear.cpp.

{}

Member Function Documentation

solve()

OptimizationAlgorithm::SolverResult g2o::SolverSLAM2DLinear::solve ( int  iteration, bool  online = false  )

virtual

Solve one iteration. The SparseOptimizer running on-top will call this for the given number of iterations.

Parameters

iteration

indicates the current iteration

Reimplemented from g2o::OptimizationAlgorithmGaussNewton.

Definition at line 80 of file solver_slam2d_linear.cpp.

                                                                           {
  if (iteration == 0) {
    bool status = solveOrientation();
    if (!status) return OptimizationAlgorithm::Fail;
  }
 
  return OptimizationAlgorithmGaussNewton::solve(iteration, online);
}

References g2o::OptimizationAlgorithmGaussNewton::solve(), and solveOrientation().

solveOrientation()

bool g2o::SolverSLAM2DLinear::solveOrientation ( )

protected

Definition at line 90 of file solver_slam2d_linear.cpp.

                                          {
  assert(_optimizer->indexMapping().size() + 1 ==
             _optimizer->vertices().size() &&
         "Needs to operate on full graph");
  assert(_optimizer->vertex(0)->fixed() && "Graph is not fixed by vertex 0");
  VectorX b, x;  // will be used for theta and x/y update
  b.setZero(_optimizer->indexMapping().size());
  x.setZero(_optimizer->indexMapping().size());
 
  typedef Eigen::Matrix<double, 1, 1, Eigen::ColMajor> ScalarMatrix;
 
  std::vector<int> blockIndices(_optimizer->indexMapping().size());
  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i)
    blockIndices[i] = i + 1;
 
  SparseBlockMatrix<ScalarMatrix> H(blockIndices.data(), blockIndices.data(),
                                    _optimizer->indexMapping().size(),
                                    _optimizer->indexMapping().size());
 
  // building the structure, diagonal for each active vertex
  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
    OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
    int poseIdx = v->hessianIndex();
    ScalarMatrix* m = H.block(poseIdx, poseIdx, true);
    m->setZero();
  }
 
  HyperGraph::VertexSet fixedSet;
 
  // off diagonal for each edge
  for (SparseOptimizer::EdgeContainer::const_iterator it =
           _optimizer->activeEdges().begin();
       it != _optimizer->activeEdges().end(); ++it) {
#ifndef NDEBUG
    EdgeSE2* e = dynamic_cast<EdgeSE2*>(*it);
    assert(e && "Active edges contain non-odometry edge");  //
#else
    EdgeSE2* e = static_cast<EdgeSE2*>(*it);
#endif
    OptimizableGraph::Vertex* from =
        static_cast<OptimizableGraph::Vertex*>(e->vertices()[0]);
    OptimizableGraph::Vertex* to =
        static_cast<OptimizableGraph::Vertex*>(e->vertices()[1]);
 
    int ind1 = from->hessianIndex();
    int ind2 = to->hessianIndex();
    if (ind1 == -1 || ind2 == -1) {
      if (ind1 == -1) fixedSet.insert(from);  // collect the fixed vertices
      if (ind2 == -1) fixedSet.insert(to);
      continue;
    }
 
    bool transposedBlock = ind1 > ind2;
    if (transposedBlock) {  // make sure, we allocate the upper triangle block
      std::swap(ind1, ind2);
    }
 
    ScalarMatrix* m = H.block(ind1, ind2, true);
    m->setZero();
  }
 
  // walk along the Minimal Spanning Tree to compute the guess for the robot
  // orientation
  assert(fixedSet.size() == 1);
  VertexSE2* root = static_cast<VertexSE2*>(*fixedSet.begin());
  VectorX thetaGuess;
  thetaGuess.setZero(_optimizer->indexMapping().size());
  UniformCostFunction uniformCost;
  HyperDijkstra hyperDijkstra(_optimizer);
  hyperDijkstra.shortestPaths(root, &uniformCost);
 
  HyperDijkstra::computeTree(hyperDijkstra.adjacencyMap());
  ThetaTreeAction thetaTreeAction(thetaGuess);
  HyperDijkstra::visitAdjacencyMap(hyperDijkstra.adjacencyMap(),
                                   &thetaTreeAction);
 
  // construct for the orientation
  for (SparseOptimizer::EdgeContainer::const_iterator it =
           _optimizer->activeEdges().begin();
       it != _optimizer->activeEdges().end(); ++it) {
    EdgeSE2* e = static_cast<EdgeSE2*>(*it);
    VertexSE2* from = static_cast<VertexSE2*>(e->vertices()[0]);
    VertexSE2* to = static_cast<VertexSE2*>(e->vertices()[1]);
 
    double omega = e->information()(2, 2);
 
    double fromThetaGuess =
        from->hessianIndex() < 0 ? 0. : thetaGuess[from->hessianIndex()];
    double toThetaGuess =
        to->hessianIndex() < 0 ? 0. : thetaGuess[to->hessianIndex()];
    double error = normalize_theta(-e->measurement().rotation().angle() +
                                   toThetaGuess - fromThetaGuess);
 
    bool fromNotFixed = !(from->fixed());
    bool toNotFixed = !(to->fixed());
 
    if (fromNotFixed || toNotFixed) {
      double omega_r = -omega * error;
      if (fromNotFixed) {
        b(from->hessianIndex()) -= omega_r;
        (*H.block(from->hessianIndex(), from->hessianIndex()))(0, 0) += omega;
        if (toNotFixed) {
          if (from->hessianIndex() > to->hessianIndex())
            (*H.block(to->hessianIndex(), from->hessianIndex()))(0, 0) -= omega;
          else
            (*H.block(from->hessianIndex(), to->hessianIndex()))(0, 0) -= omega;
        }
      }
      if (toNotFixed) {
        b(to->hessianIndex()) += omega_r;
        (*H.block(to->hessianIndex(), to->hessianIndex()))(0, 0) += omega;
      }
    }
  }
 
  // solve orientation
  typedef LinearSolverEigen<ScalarMatrix> SystemSolver;
  SystemSolver linearSystemSolver;
  linearSystemSolver.init();
  bool ok = linearSystemSolver.solve(H, x.data(), b.data());
  if (!ok) {
    G2O_ERROR("{} Failure while solving linear system", __PRETTY_FUNCTION__);
    return false;
  }
 
  // update the orientation of the 2D poses and set translation to 0, GN shall
  // solve that
  root->setToOrigin();
  for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
    VertexSE2* v = static_cast<VertexSE2*>(_optimizer->indexMapping()[i]);
    int poseIdx = v->hessianIndex();
    SE2 poseUpdate(0, 0, normalize_theta(thetaGuess(poseIdx) + x(poseIdx)));
    v->setEstimate(poseUpdate);
  }
 
  return true;
}

References __PRETTY_FUNCTION__, g2o::OptimizationAlgorithm::_optimizer, g2o::SparseOptimizer::activeEdges(), g2o::HyperDijkstra::adjacencyMap(), g2o::SparseBlockMatrix< MatrixType >::block(), g2o::HyperDijkstra::computeTree(), g2o::OptimizableGraph::Vertex::fixed(), G2O_ERROR, g2o::OptimizableGraph::Vertex::hessianIndex(), g2o::SparseOptimizer::indexMapping(), g2o::BaseEdge< D, E >::information(), g2o::LinearSolverEigen< MatrixType >::init(), g2o::BaseEdge< D, E >::measurement(), g2o::normalize_theta(), g2o::BaseVertex< D, T >::setEstimate(), g2o::OptimizableGraph::Vertex::setToOrigin(), g2o::HyperDijkstra::shortestPaths(), g2o::OptimizableGraph::vertex(), g2o::HyperGraph::Edge::vertices(), g2o::HyperGraph::vertices(), and g2o::HyperDijkstra::visitAdjacencyMap().

Referenced by solve().

---

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

• /build/reproducible-path/g2o-0~20230806/g2o/solvers/slam2d_linear/solver_slam2d_linear.h
• /build/reproducible-path/g2o-0~20230806/g2o/solvers/slam2d_linear/solver_slam2d_linear.cpp