g2o
structure_only_solver.h
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 H. Strasdat
3 // Copyright (C) 2012 R. Kuemmerle
4 // All rights reserved.
5 //
6 // Redistribution and use in source and binary forms, with or without
7 // modification, are permitted provided that the following conditions are
8 // met:
9 //
10 // * Redistributions of source code must retain the above copyright notice,
11 // this list of conditions and the following disclaimer.
12 // * Redistributions in binary form must reproduce the above copyright
13 // notice, this list of conditions and the following disclaimer in the
14 // documentation and/or other materials provided with the distribution.
15 //
16 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
17 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
18 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
19 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
22 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
23 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
24 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
25 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 
28 #ifndef G2O_STRUCTURE_ONLY_SOLVER_H
29 #define G2O_STRUCTURE_ONLY_SOLVER_H
30 
31 #include "g2o/core/base_vertex.h"
35 
36 namespace g2o
37 {
38 
56 template <int PointDoF>
58 {
59  public:
61  {
62  _verbose = true;
63  }
64 
65  virtual OptimizationAlgorithm::SolverResult solve(int iteration, bool online = false)
66  {
67  (void) iteration;
68  (void) online;
69  return calc(_points, 1);
70  }
71 
72  OptimizationAlgorithm::SolverResult calc(OptimizableGraph::VertexContainer& vertices, int num_iters, int num_max_trials=10)
73  {
74  JacobianWorkspace auxWorkspace;
75  auxWorkspace.updateSize(2, 50);
76  auxWorkspace.allocate();
77 
78  for (OptimizableGraph::VertexContainer::iterator it_v=vertices.begin(); it_v!=vertices.end(); ++it_v) {
79  bool stop = false;
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 = 0.01;
87  double nu = 2;
88 
89  for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin(); it_t!=track.end(); ++it_t) {
90  g2o::OptimizableGraph::Edge* e = dynamic_cast<g2o::OptimizableGraph::Edge *>(*it_t);
91  e->computeError();
92  chi2 += e->chi2();
93  }
94 
95  if (v->fixed() == false) {
96  Eigen::Matrix<double, PointDoF, PointDoF, Eigen::ColMajor> H_pp;
97  H_pp.resize(v->dimension(), v->dimension());
98  v->mapHessianMemory(H_pp.data());
99  for (int i_g = 0; i_g < num_iters; ++i_g) {
100  H_pp.setZero();
101  v->clearQuadraticForm();
102 
103  g2o::HyperGraph::EdgeSet& track = v->edges();
104  assert(track.size()>=1);
105 
106  for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin(); it_t!=track.end(); ++it_t) {
107  g2o::OptimizableGraph::Edge* e = dynamic_cast<g2o::OptimizableGraph::Edge *>(*it_t);
108 
109  // fix all the other vertices and remember their fix value
110 #ifdef WINDOWS
111  std::vector<bool> remember_fix_status(e->vertices().size());
112 #else
113  bool remember_fix_status[e->vertices().size()];
114 #endif
115  for (size_t k = 0; k < e->vertices().size(); ++k) {
116  OptimizableGraph::Vertex* otherV = static_cast<OptimizableGraph::Vertex*>(e->vertex(k));
117  if (otherV != v) {
118  remember_fix_status[k] = otherV->fixed();
119  otherV->setFixed(true);
120  }
121  }
122 
123  // build the matrix
124  e->computeError();
125  e->linearizeOplus(auxWorkspace);
127 
128  // Restore frame's initial fixed() values
129  for (size_t k = 0; k < e->vertices().size(); ++k) {
130  OptimizableGraph::Vertex* otherV = static_cast<g2o::OptimizableGraph::Vertex*>(e->vertex(k));
131  if (otherV != v) {
132  otherV->setFixed(remember_fix_status[k]);
133  }
134  }
135  }
136 
137  Eigen::Map<Eigen::Matrix<double,PointDoF,1,Eigen::ColMajor> > b(v->bData(), v->dimension());
138 
139  if (b.norm()<0.001) {
140  stop = true;
141  break;
142  }
143 
144  int trial=0;
145  do {
146  Eigen::Matrix<double,PointDoF,PointDoF,Eigen::ColMajor> H_pp_mu = H_pp;
147  H_pp_mu.diagonal().array() += mu;
148  Eigen::LDLT<Eigen::Matrix<double,PointDoF,PointDoF,Eigen::ColMajor> > chol_H_pp(H_pp_mu);
149  bool goodStep = false;
150  if (chol_H_pp.isPositive()) {
151  Eigen::Matrix<double,PointDoF,1,Eigen::ColMajor> delta_p = chol_H_pp.solve(b);
152  v->push();
153  v->oplus(delta_p.data());
154  double new_chi2 = 0.;
155  for (g2o::HyperGraph::EdgeSet::iterator it_t=track.begin(); it_t!=track.end(); ++it_t) {
156  g2o::OptimizableGraph::Edge* e = dynamic_cast<g2o::OptimizableGraph::Edge *>(*it_t);
157  e->computeError();
158  new_chi2 += e->chi2();
159  }
160  assert(g2o_isnan(new_chi2)==false && "Chi is NaN");
161  double rho = (chi2 - new_chi2);
162  if (rho > 0 && g2o_isfinite(new_chi2)) {
163  goodStep = true;
164  chi2 = new_chi2;
165  v->discardTop();
166  } else {
167  goodStep = false;
168  v->pop();
169  }
170  }
171 
172  // update the damping factor based on the result of the last increment
173  if (goodStep) {
174  mu *= 1./3.;
175  nu = 2.;
176  trial=0;
177  break;
178  } else {
179  mu *= nu;
180  nu *= 2.;
181  ++trial;
182  if (trial >= num_max_trials) {
183  stop=true;
184  break;
185  }
186  }
187  } while(!stop);
188  if (stop)
189  break;
190  }
191  }
192  }
193  return OK;
194  }
195 
196  virtual bool init(bool )
197  {
198  // collect the vertices
199  _points.clear();
200  for (OptimizableGraph::VertexContainer::const_iterator it = optimizer()->activeVertices().begin(); it != optimizer()->activeVertices().end(); ++it) {
201  OptimizableGraph::Vertex* v = *it;
202  if (v->marginalized()) {
203  _points.push_back(v);
204  }
205  }
206  return true;
207  }
208 
209  virtual bool computeMarginals(SparseBlockMatrix<MatrixXD>&, const std::vector<std::pair<int, int> >&) { return false;}
210 
211  virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& , const HyperGraph::EdgeSet& ) { return true;}
212 
216 
217  protected:
218  bool _verbose;
220 };
221 
222 }
223 
224 #endif
std::vector< OptimizableGraph::Vertex * > VertexContainer
vector container for vertices
const Vertex * vertex(size_t i) const
Definition: hyper_graph.h:186
const OptimizableGraph::VertexContainer & points() const
virtual bool updateStructure(const std::vector< HyperGraph::Vertex * > &, const HyperGraph::EdgeSet &)
virtual void discardTop()=0
pop the last element from the stack, without restoring the current estimate
virtual double chi2() const =0
computes the chi2 based on the cached error value, only valid after computeError has been called...
This is a solver for "structure-only" optimization".
const SparseOptimizer * optimizer() const
return the optimizer operating on
virtual OptimizationAlgorithm::SolverResult solve(int iteration, bool online=false)
virtual void linearizeOplus(JacobianWorkspace &jacobianWorkspace)=0
OptimizableGraph::VertexContainer & points()
return the points of the optimization problem
std::set< Edge * > EdgeSet
Definition: hyper_graph.h:135
int dimension() const
dimension of the estimated state belonging to this node
Protocol The SLAM executable accepts such as solving the and retrieving or vertices in the explicitly state the reprensentation poses are represented by poses by VERTEX_XYZRPY In the Quaternions and other representations could be but note that it is up to the SLAM algorithm to choose the internal representation of the angles The keyword is followed by a unique vertex ID and an optional initialization of the or edges in the explicitly state the type of the constraint pose constraints are given by pose constraints by EDGE_XYZRPY The keyword is followed by a unique edge the IDs of the referenced vertices
Definition: protocol.txt:7
const VertexContainer & vertices() const
Definition: hyper_graph.h:178
const EdgeSet & edges() const
returns the set of hyper-edges that are leaving/entering in this vertex
Definition: hyper_graph.h:151
virtual bool computeMarginals(SparseBlockMatrix< MatrixXD > &, const std::vector< std::pair< int, int > > &)
virtual void constructQuadraticForm()=0
virtual double * bData()=0
return a pointer to the b vector associated with this vertex
void updateSize(const HyperGraph::Edge *e)
virtual void computeError()=0
virtual void push()=0
backup the position of the vertex to a stack
virtual void pop()=0
restore the position of the vertex by retrieving the position from the stack
virtual void mapHessianMemory(double *d)=0
virtual void clearQuadraticForm()=0
bool marginalized() const
true => this node is marginalized out during the optimization
A general case Vertex for optimization.
#define g2o_isfinite(x)
Definition: macros.h:101
#define g2o_isnan(x)
Definition: macros.h:99
const VertexContainer & activeVertices() const
the vertices active in the current optimization
bool fixed() const
true => this node is fixed during the optimization
void setFixed(bool fixed)
true => this node should be considered fixed during the optimization
Generic interface for a non-linear solver operating on a graph.
provide memory workspace for computing the Jacobians
OptimizableGraph::VertexContainer _points
Sparse matrix which uses blocks.
OptimizationAlgorithm::SolverResult calc(OptimizableGraph::VertexContainer &vertices, int num_iters, int num_max_trials=10)