sparse_optimizer.h
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 #ifndef G2O_GRAPH_OPTIMIZER_CHOL_H_
28 #define G2O_GRAPH_OPTIMIZER_CHOL_H_
29 
30 #include "../stuff/macros.h"
31 
32 #include "optimizable_graph.h"
33 #include "sparse_block_matrix.h"
34 #include "batch_stats.h"
35 
36 #include <map>
37 
38 namespace g2o {
39 
40  // forward declaration
41  class ActivePathCostFunction;
42  class OptimizationAlgorithm;
43  class EstimatePropagatorCost;
44 
46 
47  public:
48  enum {
50  AT_NUM_ELEMENTS, // keep as last element
51  };
52 
53  friend class ActivePathCostFunction;
54 
55  // Attention: _solver & _statistics is own by SparseOptimizer and will be
56  // deleted in its destructor.
58  virtual ~SparseOptimizer();
59 
60  // new interface for the optimizer
61  // the old functions will be dropped
69  virtual bool initializeOptimization(HyperGraph::EdgeSet& eset);
70 
79  virtual bool initializeOptimization(HyperGraph::VertexSet& vset, int level=0);
80 
88  virtual bool initializeOptimization(int level=0);
89 
94 
103  virtual void computeInitialGuess();
104 
108  virtual void computeInitialGuess(EstimatePropagatorCost& propagator);
109 
113  virtual void setToOrigin();
114 
115 
121  int optimize(int iterations, bool online = false);
122 
130  bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
131 
139  if (vertex->hessianIndex() < 0) {
140  return false;
141  }
142  std::vector<std::pair<int, int> > index;
143  index.push_back(std::pair<int, int>(vertex->hessianIndex(), vertex->hessianIndex()));
144  return computeMarginals(spinv, index);
145  }
146 
154  std::vector<std::pair<int, int> > indices;
155  for (VertexContainer::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
156  indices.push_back(std::pair<int, int>((*it)->hessianIndex(),(*it)->hessianIndex()));
157  }
158  return computeMarginals(spinv, indices);
159  }
160 
162  // The gauge should be fixed() and then the optimization can work (if no additional dof are in
163  // the system. The default implementation returns a node with maximum dimension.
164  virtual Vertex* findGauge();
165 
166  bool gaugeFreedom();
167 
169  double activeChi2() const;
175  double activeRobustChi2() const;
176 
178  bool verbose() const {return _verbose;}
179  void setVerbose(bool verbose);
180 
184  void setForceStopFlag(bool* flag);
185  bool* forceStopFlag() const { return _forceStopFlag;};
186 
188  bool terminate() {return _forceStopFlag ? (*_forceStopFlag) : false; }
189 
191  const VertexContainer& indexMapping() const {return _ivMap;}
195  const EdgeContainer& activeEdges() const { return _activeEdges;}
196 
203  virtual bool removeVertex(HyperGraph::Vertex* v);
204 
209  VertexContainer::const_iterator findActiveVertex(const OptimizableGraph::Vertex* v) const;
214  EdgeContainer::const_iterator findActiveEdge(const OptimizableGraph::Edge* e) const;
215 
217  const OptimizationAlgorithm* algorithm() const { return _algorithm;}
220 
224  void push(HyperGraph::VertexSet& vlist);
226  void push();
230  void pop(HyperGraph::VertexSet& vlist);
232  void pop();
233 
237  void discardTop();
239 
245  virtual void clear();
246 
250  void computeActiveErrors();
251 
256  G2O_ATTRIBUTE_DEPRECATED(void linearizeSystem())
257  {
258  // nothing needed, linearization is now done inside the solver
259  }
260 
266  void update(const double* update);
267 
276 
278 
280 
281  /**** callbacks ****/
286 
287 
288 
289  protected:
291  bool _verbose;
292 
296 
297  void sortVectorContainers();
298 
300 
305  void clearIndexMapping();
306 
309  };
310 } // end namespace
311 
312 #endif
virtual bool updateInitialization(HyperGraph::VertexSet &vset, HyperGraph::EdgeSet &eset)
virtual void computeInitialGuess()
const BatchStatisticsContainer & batchStatistics() const
VertexContainer _ivMap
std::vector< OptimizableGraph::Vertex * > VertexContainer
vector container for vertices
bool terminate()
if external stop flag is given, return its state. False otherwise
VertexContainer _activeVertices
sorted according to VertexIDCompare
std::vector< Vertex * > VertexContainer
Definition: hyper_graph.h:94
void setComputeBatchStatistics(bool computeBatchStatistics)
bool removeComputeErrorAction(HyperGraphAction *action)
remove an action that should no longer be execured before computing the error vectors ...
std::set< Vertex * > VertexSet
Definition: hyper_graph.h:91
int optimize(int iterations, bool online=false)
OptimizationAlgorithm * solver()
bool computeMarginals(SparseBlockMatrix< MatrixXd > &spinv, const VertexContainer &vertices)
const EdgeContainer & activeEdges() const
the edges active in the current optimization
virtual void setToOrigin()
const VertexIDMap & vertices() const
Definition: hyper_graph.h:177
Vertex * vertex(int id)
returns the vertex number id appropriately casted
void update(const std::string &key, const XmlRpc::XmlRpcValue &v)
std::vector< OptimizableGraph::Edge * > EdgeContainer
vector container for edges
std::set< Edge * > EdgeSet
Definition: hyper_graph.h:90
virtual Vertex * findGauge()
finds a gauge in the graph to remove the undefined dof.
virtual bool removeVertex(HyperGraph::Vertex *v)
void setVerbose(bool verbose)
bool computeMarginals(SparseBlockMatrix< MatrixXd > &spinv, const std::vector< std::pair< int, int > > &blockIndices)
VertexContainer::const_iterator findActiveVertex(const OptimizableGraph::Vertex *v) const
void setForceStopFlag(bool *flag)
void setAlgorithm(OptimizationAlgorithm *algorithm)
BatchStatisticsContainer _batchStatistics
global statistics of the optimizer, e.g., timing, num-non-zeros
void push()
push all the active vertices onto a stack
const VertexContainer & indexMapping() const
the index mapping of the vertices
EdgeContainer _activeEdges
sorted according to EdgeIDCompare
cost for traversing along active edges in the optimizer
friend class ActivePathCostFunction
BatchStatisticsContainer & batchStatistics()
bool buildIndexMapping(SparseOptimizer::VertexContainer &vlist)
void pop()
pop (restore) the estimate of the active vertices from the stack
A general case Vertex for optimization.
action
bool addComputeErrorAction(HyperGraphAction *action)
add an action to be executed before the error vectors are computed
abstract Vertex, your types must derive from that one
Definition: hyper_graph.h:97
bool computeBatchStatistics() const
bool * forceStopFlag() const
EdgeContainer::const_iterator findActiveEdge(const OptimizableGraph::Edge *e) const
#define G2O_ATTRIBUTE_DEPRECATED(func)
Definition: macros.h:102
const VertexContainer & activeVertices() const
the vertices active in the current optimization
double activeRobustChi2() const
void discardTop()
same as above, but for the active vertices
virtual void discardTop()
discard the last backup of the estimate for all variables by removing it from the stack ...
virtual bool initializeOptimization(HyperGraph::EdgeSet &eset)
Generic interface for a non-linear solver operating on a graph.
bool verbose() const
verbose information during optimization
const OptimizationAlgorithm * algorithm() const
the solver used by the optimizer
std::vector< G2OBatchStatistics > BatchStatisticsContainer
Definition: batch_stats.h:80
Sparse matrix which uses blocks.
OptimizationAlgorithm * _algorithm
Abstract action that operates on an entire graph.
bool computeMarginals(SparseBlockMatrix< MatrixXd > &spinv, const Vertex *vertex)
double activeChi2() const


orb_slam2_with_maps_odom
Author(s): teng zhang
autogenerated on Fri Sep 25 2020 03:24:47