g2o::StructureOnlySolver< PointDoF > Class Template Reference

This is a solver for "structure-only" optimization". More...

#include <structure_only_solver.h>

Inheritance diagram for g2o::StructureOnlySolver< PointDoF >:

Collaboration diagram for g2o::StructureOnlySolver< PointDoF >:

Public Member Functions
	StructureOnlySolver ()
virtual OptimizationAlgorithm::SolverResult	solve (int iteration, bool online=false)
OptimizationAlgorithm::SolverResult	calc (OptimizableGraph::VertexContainer &vertices, int num_iters, int num_max_trials=10)
virtual bool	init (bool)
virtual bool	computeMarginals (SparseBlockMatrix< MatrixXD > &, const std::vector< std::pair< int, int > > &)
virtual bool	updateStructure (const std::vector< HyperGraph::Vertex * > &, const HyperGraph::EdgeSet &)
OptimizableGraph::VertexContainer &	points ()
	return the points of the optimization problem More...
const OptimizableGraph::VertexContainer &	points () const
Public Member Functions inherited from g2o::OptimizationAlgorithm
	OptimizationAlgorithm ()
virtual	~OptimizationAlgorithm ()
virtual void	printVerbose (std::ostream &os) const
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 Attributes
bool	_verbose
OptimizableGraph::VertexContainer	_points
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

template<int PointDoF>
class g2o::StructureOnlySolver< PointDoF >

This is a solver for "structure-only" optimization".

Given the problem of landmark-based SLAM or bundle adjustment, this class performs optimization on the landmarks while the poses are kept fixed. This can be done very efficiently, since the position on the landmarks are indepdented given the poses are known.

This class slightly misuses the API of g2o. It is designed in a way, it can work on the very same graph which reflects the problem of landmark-based SLAM, bundle adjustment and which is meant to be solved using the Schur complement. Thus, it can be called just before or after joint-optimization without the need of additional setup. Call calc() with the point features you want to optimize.

This class is still considered as being experimentally!

Definition at line 57 of file structure_only_solver.h.

Constructor & Destructor Documentation

template<int PointDoF>

g2o::StructureOnlySolver< PointDoF >::StructureOnlySolver ( )

inline

Definition at line 60 of file structure_only_solver.h.

References g2o::StructureOnlySolver< PointDoF >::_verbose.

    {
      _verbose = true;
    }

Member Function Documentation

template<int PointDoF>

OptimizationAlgorithm::SolverResult g2o::StructureOnlySolver< PointDoF >::calc ( OptimizableGraph::VertexContainer &  vertices, int  num_iters, int  num_max_trials = 10  )

inline

Definition at line 72 of file structure_only_solver.h.

References g2o::JacobianWorkspace::allocate(), g2o::OptimizableGraph::Vertex::bData(), g2o::OptimizableGraph::Edge::chi2(), g2o::OptimizableGraph::Vertex::clearQuadraticForm(), g2o::OptimizableGraph::Edge::computeError(), g2o::OptimizableGraph::Edge::constructQuadraticForm(), g2o::OptimizableGraph::Vertex::dimension(), g2o::OptimizableGraph::Vertex::discardTop(), g2o::HyperGraph::Vertex::edges(), g2o::OptimizableGraph::Vertex::fixed(), g2o_isfinite, g2o_isnan, g2o::OptimizableGraph::Edge::linearizeOplus(), g2o::OptimizableGraph::Vertex::mapHessianMemory(), OK, g2o::OptimizableGraph::Vertex::oplus(), g2o::OptimizableGraph::Vertex::pop(), g2o::OptimizableGraph::Vertex::push(), g2o::OptimizableGraph::Vertex::setFixed(), g2o::JacobianWorkspace::updateSize(), g2o::HyperGraph::Edge::vertex(), and g2o::HyperGraph::Edge::vertices().

Referenced by main(), and g2o::StructureOnlySolver< PointDoF >::solve().

    {
      JacobianWorkspace auxWorkspace;
      auxWorkspace.updateSize(2, 50);
      auxWorkspace.allocate();

      for (OptimizableGraph::VertexContainer::iterator it_v=vertices.begin(); it_v!=vertices.end(); ++it_v) {
        bool stop = false;
        g2o::OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it_v);
        assert(v->dimension() == PointDoF);
        g2o::HyperGraph::EdgeSet& track = v->edges();
        assert(track.size()>=2);
        double chi2 = 0;
        // TODO make these parameters
        double mu = 0.01;
        double nu = 2;

        for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin(); it_t!=track.end(); ++it_t) {
          g2o::OptimizableGraph::Edge* e = dynamic_cast<g2o::OptimizableGraph::Edge *>(*it_t);
          e->computeError();
          chi2 += e->chi2();
        }

        if (v->fixed() == false) {
          Eigen::Matrix<double, PointDoF, PointDoF, Eigen::ColMajor> H_pp;
          H_pp.resize(v->dimension(), v->dimension());
          v->mapHessianMemory(H_pp.data());
          for (int i_g = 0; i_g < num_iters; ++i_g) {
            H_pp.setZero();
            v->clearQuadraticForm();

            g2o::HyperGraph::EdgeSet& track = v->edges();
            assert(track.size()>=1);

            for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin(); it_t!=track.end(); ++it_t) {
              g2o::OptimizableGraph::Edge* e = dynamic_cast<g2o::OptimizableGraph::Edge *>(*it_t);

              // fix all the other vertices and remember their fix value
#ifdef WINDOWS
              std::vector<bool> remember_fix_status(e->vertices().size());
#else
              bool remember_fix_status[e->vertices().size()];
#endif
              for (size_t k = 0; k < e->vertices().size(); ++k) {
                OptimizableGraph::Vertex* otherV = static_cast<OptimizableGraph::Vertex*>(e->vertex(k));
                if (otherV != v) {
                  remember_fix_status[k] = otherV->fixed();
                  otherV->setFixed(true);
                }
              }

              // build the matrix
              e->computeError();
              e->linearizeOplus(auxWorkspace);
              e->constructQuadraticForm();

              // Restore frame's initial fixed() values
              for (size_t k = 0; k < e->vertices().size(); ++k) {
                OptimizableGraph::Vertex* otherV = static_cast<g2o::OptimizableGraph::Vertex*>(e->vertex(k));
                if (otherV != v) {
                  otherV->setFixed(remember_fix_status[k]);
                }
              }
            }

            Eigen::Map<Eigen::Matrix<double,PointDoF,1,Eigen::ColMajor> > b(v->bData(), v->dimension());

            if (b.norm()<0.001) {
              stop = true;
              break;
            }

            int trial=0;
            do {
              Eigen::Matrix<double,PointDoF,PointDoF,Eigen::ColMajor> H_pp_mu = H_pp;
              H_pp_mu.diagonal().array() += mu;
              Eigen::LDLT<Eigen::Matrix<double,PointDoF,PointDoF,Eigen::ColMajor> > chol_H_pp(H_pp_mu);
              bool goodStep = false;
              if (chol_H_pp.isPositive()) {
                Eigen::Matrix<double,PointDoF,1,Eigen::ColMajor> delta_p = chol_H_pp.solve(b);
                v->push();
                v->oplus(delta_p.data());
                double new_chi2 = 0.;
                for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin(); it_t!=track.end(); ++it_t) {
                  g2o::OptimizableGraph::Edge* e = dynamic_cast<g2o::OptimizableGraph::Edge *>(*it_t);
                  e->computeError();
                  new_chi2 += e->chi2();
                }
                assert(g2o_isnan(new_chi2)==false && "Chi is NaN");
                double rho = (chi2 - new_chi2);
                if (rho > 0 && g2o_isfinite(new_chi2)) {
                  goodStep = true;
                  chi2 = new_chi2;
                  v->discardTop();
                } else {
                  goodStep = false;
                  v->pop();
                }
              }

              // update the damping factor based on the result of the last increment
              if (goodStep) {
                mu *= 1./3.;
                nu = 2.;
                trial=0;
                break;
              } else {
                mu *= nu;
                nu *= 2.;
                ++trial;
                if (trial >= num_max_trials) {
                  stop=true;
                  break;
                }
              }
            } while(!stop);
            if (stop)
              break;
          }
        }
      }
      return OK;
    }

template<int PointDoF>

virtual bool g2o::StructureOnlySolver< PointDoF >::computeMarginals ( SparseBlockMatrix< MatrixXD > &  spinv, const std::vector< std::pair< int, int > > &  blockIndices  )

inlinevirtual

computes the block diagonal elements of the pattern specified in the input and stores them in given SparseBlockMatrix. If your solver does not support computing the marginals, return false.

Implements g2o::OptimizationAlgorithm.

Definition at line 209 of file structure_only_solver.h.

{ return false;}

template<int PointDoF>

virtual bool g2o::StructureOnlySolver< PointDoF >::init ( bool  online )

inlinevirtual

initialize the solver, called once before the first call to solve()

Implements g2o::OptimizationAlgorithm.

Definition at line 196 of file structure_only_solver.h.

References g2o::StructureOnlySolver< PointDoF >::_points, g2o::SparseOptimizer::activeVertices(), g2o::OptimizableGraph::Vertex::marginalized(), and g2o::OptimizationAlgorithm::optimizer().

    { 
      // collect the vertices
      _points.clear();
      for (OptimizableGraph::VertexContainer::const_iterator it =  optimizer()->activeVertices().begin(); it != optimizer()->activeVertices().end(); ++it) {
        OptimizableGraph::Vertex* v = *it;
        if (v->marginalized()) {
          _points.push_back(v);
        }
      }
      return true;
    }

template<int PointDoF>

OptimizableGraph::VertexContainer& g2o::StructureOnlySolver< PointDoF >::points ( )

inline

return the points of the optimization problem

Definition at line 214 of file structure_only_solver.h.

References g2o::StructureOnlySolver< PointDoF >::_points.

{ return _points;}

template<int PointDoF>

const OptimizableGraph::VertexContainer& g2o::StructureOnlySolver< PointDoF >::points ( ) const

inline

Definition at line 215 of file structure_only_solver.h.

References g2o::StructureOnlySolver< PointDoF >::_points.

{ return _points;}

template<int PointDoF>

virtual OptimizationAlgorithm::SolverResult g2o::StructureOnlySolver< PointDoF >::solve ( int  iteration, bool  online = false  )

inlinevirtual

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

Parameters

iteration

indicates the current iteration

Implements g2o::OptimizationAlgorithm.

Definition at line 65 of file structure_only_solver.h.

References g2o::StructureOnlySolver< PointDoF >::_points, and g2o::StructureOnlySolver< PointDoF >::calc().

    {
      (void) iteration;
      (void) online;
      return calc(_points, 1);
    }

template<int PointDoF>

virtual bool g2o::StructureOnlySolver< PointDoF >::updateStructure ( const std::vector< HyperGraph::Vertex * > &  vset, const HyperGraph::EdgeSet &  edges  )

inlinevirtual

update the structures for online processing

Implements g2o::OptimizationAlgorithm.

Definition at line 211 of file structure_only_solver.h.

{ return true;}

Member Data Documentation

template<int PointDoF>

OptimizableGraph::VertexContainer g2o::StructureOnlySolver< PointDoF >::_points

protected

Definition at line 219 of file structure_only_solver.h.

Referenced by g2o::StructureOnlySolver< PointDoF >::init(), g2o::StructureOnlySolver< PointDoF >::points(), and g2o::StructureOnlySolver< PointDoF >::solve().

template<int PointDoF>

bool g2o::StructureOnlySolver< PointDoF >::_verbose

protected

Definition at line 218 of file structure_only_solver.h.

Referenced by g2o::StructureOnlySolver< PointDoF >::StructureOnlySolver().

---

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

• /home/xuezhisd/CLionProjects/g2o/g2o/solvers/structure_only/structure_only_solver.h