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 (Solver *solver)
virtual	~SolverSLAM2DLinear ()
virtual OptimizationAlgorithm::SolverResult	solve (int iteration, bool online=false)
Public Member Functions inherited from g2o::OptimizationAlgorithmGaussNewton
	OptimizationAlgorithmGaussNewton (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< MatrixXD > &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 More...
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 More...
SparseOptimizer *	optimizer ()
void	setOptimizer (SparseOptimizer *optimizer)
const PropertyMap &	properties () const
	return the properties of the solver More...
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 More...
PropertyMap	_properties
	the properties of your solver, use this to store the parameters of your solver More...

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 excactly 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 52 of file solver_slam2d_linear.h.

Constructor & Destructor Documentation

g2o::SolverSLAM2DLinear::SolverSLAM2DLinear ( 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 73 of file solver_slam2d_linear.cpp.

                                                       :
    OptimizationAlgorithmGaussNewton(solver)
  {
  }

g2o::SolverSLAM2DLinear::~SolverSLAM2DLinear ( )

virtual

Definition at line 78 of file solver_slam2d_linear.cpp.

  {
  }

Member Function Documentation

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 82 of file solver_slam2d_linear.cpp.

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

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

    return OptimizationAlgorithmGaussNewton::solve(iteration, online);
  }

bool g2o::SolverSLAM2DLinear::solveOrientation ( )

protected

Definition at line 93 of file solver_slam2d_linear.cpp.

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

Referenced by solve().

  {
    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");
    VectorXD 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;

    ScopedArray<int> blockIndeces(new int[_optimizer->indexMapping().size()]);
    for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i)
      blockIndeces[i] = i+1;

    SparseBlockMatrix<ScalarMatrix> H(blockIndeces.get(), blockIndeces.get(), _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());
    VectorXD thetaGuess;
    thetaGuess.setZero(_optimizer->indexMapping().size());
    UniformCostFunction uniformCost;
    HyperDijkstra hyperDijkstra(_optimizer);
    hyperDijkstra.shortestPaths(root, &uniformCost);

    HyperDijkstra::computeTree(hyperDijkstra.adjacencyMap());
    ThetaTreeAction thetaTreeAction(thetaGuess.data());
    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 LinearSolverCSparse<ScalarMatrix> SystemSolver;
    SystemSolver linearSystemSolver;
    linearSystemSolver.init();
    bool ok = linearSystemSolver.solve(H, x.data(), b.data());
    if (!ok) {
      cerr << __PRETTY_FUNCTION__ << "Failure while solving linear system" << endl;
      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;
  }

---

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

• /home/xuezhisd/CLionProjects/g2o/g2o/solvers/slam2d_linear/solver_slam2d_linear.h
• /home/xuezhisd/CLionProjects/g2o/g2o/solvers/slam2d_linear/solver_slam2d_linear.cpp