g2o
Loading...
Searching...
No Matches
Public Member Functions | Protected Attributes | List of all members
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 >:
Inheritance graph
[legend]
Collaboration diagram for g2o::StructureOnlySolver< PointDoF >:
Collaboration graph
[legend]

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.

60{ _verbose = true; }

References _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.

71 {
72 JacobianWorkspace auxWorkspace;
73 auxWorkspace.updateSize(2, 50);
74 auxWorkspace.allocate();
75
76 for (OptimizableGraph::VertexContainer::iterator it_v = vertices.begin();
77 it_v != vertices.end(); ++it_v) {
78 bool stop = false;
79 g2o::OptimizableGraph::Vertex* v =
80 dynamic_cast<OptimizableGraph::Vertex*>(*it_v);
81 assert(v->dimension() == PointDoF);
82 g2o::HyperGraph::EdgeSet& track = v->edges();
83 assert(track.size() >= 2);
84 double chi2 = 0;
85 // TODO make these parameters
86 double mu = cst(0.01);
87 double nu = 2;
88
89 for (g2o::HyperGraph::EdgeSet::iterator it_t = track.begin();
90 it_t != track.end(); ++it_t) {
91 g2o::OptimizableGraph::Edge* e =
92 dynamic_cast<g2o::OptimizableGraph::Edge*>(*it_t);
93 e->computeError();
94 if (e->robustKernel()) {
95 Vector3 rho;
96 e->robustKernel()->robustify(e->chi2(), rho);
97 chi2 += rho[0];
98 } else {
99 chi2 += e->chi2();
100 }
101 }
102
103 if (v->fixed() == false) {
104 Eigen::Matrix<double, PointDoF, PointDoF, Eigen::ColMajor> H_pp;
105 H_pp.resize(v->dimension(), v->dimension());
106 v->mapHessianMemory(H_pp.data());
107 for (int i_g = 0; i_g < num_iters; ++i_g) {
108 H_pp.setZero();
109 v->clearQuadraticForm();
110
111 g2o::HyperGraph::EdgeSet& track = v->edges();
112 assert(track.size() >= 1);
113
114 for (g2o::HyperGraph::EdgeSet::iterator it_t = track.begin();
115 it_t != track.end(); ++it_t) {
116 g2o::OptimizableGraph::Edge* e =
117 dynamic_cast<g2o::OptimizableGraph::Edge*>(*it_t);
118
119 // fix all the other vertices and remember their fix value
120#ifdef WINDOWS
121 std::vector<bool> remember_fix_status(e->vertices().size());
122#else
123 bool remember_fix_status[e->vertices().size()];
124#endif
125 for (size_t k = 0; k < e->vertices().size(); ++k) {
126 OptimizableGraph::Vertex* otherV =
127 static_cast<OptimizableGraph::Vertex*>(e->vertex(k));
128 if (otherV != v) {
129 remember_fix_status[k] = otherV->fixed();
130 otherV->setFixed(true);
131 }
132 }
133
134 // build the matrix
135 e->computeError();
136 e->linearizeOplus(auxWorkspace);
137 e->constructQuadraticForm();
138
139 // Restore frame's initial fixed() values
140 for (size_t k = 0; k < e->vertices().size(); ++k) {
141 OptimizableGraph::Vertex* otherV =
142 static_cast<g2o::OptimizableGraph::Vertex*>(e->vertex(k));
143 if (otherV != v) {
144 otherV->setFixed(remember_fix_status[k]);
145 }
146 }
147 }
148
149 Eigen::Map<Eigen::Matrix<double, PointDoF, 1, Eigen::ColMajor> > b(
150 v->bData(), v->dimension());
151
152 if (b.norm() < 0.001) {
153 stop = true;
154 break;
155 }
156
157 int trial = 0;
158 do {
159 Eigen::Matrix<double, PointDoF, PointDoF, Eigen::ColMajor> H_pp_mu =
160 H_pp;
161 H_pp_mu.diagonal().array() += mu;
162 Eigen::LDLT<
163 Eigen::Matrix<double, PointDoF, PointDoF, Eigen::ColMajor> >
164 chol_H_pp(H_pp_mu);
165 bool goodStep = false;
166 if (chol_H_pp.isPositive()) {
167 Eigen::Matrix<double, PointDoF, 1, Eigen::ColMajor> delta_p =
168 chol_H_pp.solve(b);
169 v->push();
170 v->oplus(delta_p.data());
171 double new_chi2 = 0;
172 for (g2o::HyperGraph::EdgeSet::iterator it_t = track.begin();
173 it_t != track.end(); ++it_t) {
174 g2o::OptimizableGraph::Edge* e =
175 dynamic_cast<g2o::OptimizableGraph::Edge*>(*it_t);
176 e->computeError();
177 if (e->robustKernel()) {
178 Vector3 rho;
179 e->robustKernel()->robustify(e->chi2(), rho);
180 new_chi2 += rho[0];
181 } else {
182 new_chi2 += e->chi2();
183 }
184 }
185 assert(g2o_isnan(new_chi2) == false && "Chi is NaN");
186 double rho = (chi2 - new_chi2);
187 if (rho > 0 && g2o_isfinite(new_chi2)) {
188 goodStep = true;
189 chi2 = new_chi2;
190 v->discardTop();
191 } else {
192 goodStep = false;
193 v->pop();
194 }
195 }
196
197 // update the damping factor based on the result of the last
198 // increment
199 if (goodStep) {
200 mu *= cst(1. / 3.);
201 nu = 2.;
202 trial = 0;
203 break;
204 } else {
205 mu *= nu;
206 nu *= 2.;
207 ++trial;
208 if (trial >= num_max_trials) {
209 stop = true;
210 break;
211 }
212 }
213 } while (!stop);
214 if (stop) break;
215 }
216 }
217 }
218 return OK;
219 }
g2o::StructureOnlySolver
This is a solver for "structure-only" optimization".
Definition structure_only_solver.h:58
g2o::StructureOnlySolver::solve
virtual OptimizationAlgorithm::SolverResult solve(int iteration, bool online=false)
Definition structure_only_solver.h:62
g2o_isfinite
#define g2o_isfinite(x)
Definition macros.h:102
g2o_isnan
#define g2o_isnan(x)
Definition macros.h:100
g2o::cst
constexpr double cst(long double v)
Definition misc.h:60

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 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.

236 {
237 return false;
238 }

◆ 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.

221 {
222 // collect the vertices
223 _points.clear();
224 for (OptimizableGraph::VertexContainer::const_iterator it =
225 optimizer()->activeVertices().begin();
226 it != optimizer()->activeVertices().end(); ++it) {
227 OptimizableGraph::Vertex* v = *it;
228 if (v->marginalized()) {
229 _points.push_back(v);
230 }
231 }
232 return true;
233 }
g2o::OptimizationAlgorithm::optimizer
const SparseOptimizer * optimizer() const
return the optimizer operating on
Definition optimization_algorithm.h:87
g2o::SparseOptimizer::activeVertices
const VertexContainer & activeVertices() const
the vertices active in the current optimization
Definition sparse_optimizer.h:188
g2o::StructureOnlySolver::_points
OptimizableGraph::VertexContainer _points
Definition structure_only_solver.h:251

References _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.

246{ return _points; }

References _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.

247{ return _points; }

References _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
iterationindicates the current iteration

Implements g2o::OptimizationAlgorithm.

Definition at line 62 of file structure_only_solver.h.

63 {
64 (void)iteration;
65 (void)online;
66 return calc(_points, 1);
67 }
g2o::StructureOnlySolver::calc
OptimizationAlgorithm::SolverResult calc(OptimizableGraph::VertexContainer &vertices, int num_iters, int num_max_trials=10)
Definition structure_only_solver.h:69

References _points, and 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.

241 {
242 return true;
243 }

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 init(), points(), points(), and solve().

◆ _verbose

template<int PointDoF>
bool g2o::StructureOnlySolver< PointDoF >::_verbose
protected

Definition at line 250 of file structure_only_solver.h.

Referenced by StructureOnlySolver().

The documentation for this class was generated from the following file:
Generated on for g2o by doxygen 1.15.0