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< MatrixX > &, 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
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
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 Attributes
bool	_verbose
OptimizableGraph::VertexContainer	_points
Protected Attributes inherited from g2o::OptimizationAlgorithm
SparseOptimizer *	_optimizer
	the optimizer the solver is working on
PropertyMap	_properties

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 independent 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 58 of file structure_only_solver.h.

Constructor & Destructor Documentation

StructureOnlySolver()

template<int PointDoF>

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

inline

Definition at line 60 of file structure_only_solver.h.

{ _verbose = true; }

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

Member Function Documentation

calc()

template<int PointDoF>

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

inline

Definition at line 69 of file structure_only_solver.h.

                               {
    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 = cst(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();
        if (e->robustKernel()) {
          Vector3 rho;
          e->robustKernel()->robustify(e->chi2(), rho);
          chi2 += rho[0];
        } else {
          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();
                if (e->robustKernel()) {
                  Vector3 rho;
                  e->robustKernel()->robustify(e->chi2(), rho);
                  new_chi2 += rho[0];
                } else {
                  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 *= cst(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;
  }

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::cst(), 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::RobustKernel::robustify(), g2o::OptimizableGraph::Edge::robustKernel(), g2o::OptimizableGraph::Vertex::setFixed(), g2o::JacobianWorkspace::updateSize(), g2o::HyperGraph::Edge::vertex(), and g2o::HyperGraph::Edge::vertices().

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

computeMarginals()

template<int PointDoF>

virtual bool g2o::StructureOnlySolver< PointDoF >::computeMarginals ( SparseBlockMatrix< MatrixX > &  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 235 of file structure_only_solver.h.

                                                                      {
    return false;
  }

init()

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 221 of file structure_only_solver.h.

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

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

points() [1/2]

template<int PointDoF>

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

inline

return the points of the optimization problem

Definition at line 246 of file structure_only_solver.h.

{ return _points; }

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

points() [2/2]

template<int PointDoF>

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

inline

Definition at line 247 of file structure_only_solver.h.

{ return _points; }

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

solve()

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 62 of file structure_only_solver.h.

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

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

updateStructure()

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 240 of file structure_only_solver.h.

                                                         {
    return true;
  }

Member Data Documentation

_points

template<int PointDoF>

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

protected

Definition at line 251 of file structure_only_solver.h.

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

_verbose

template<int PointDoF>

bool g2o::StructureOnlySolver< PointDoF >::_verbose

protected

Definition at line 250 of file structure_only_solver.h.

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

---

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

• /build/reproducible-path/g2o-0~20230806/g2o/solvers/structure_only/structure_only_solver.h