estimate_propagator.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_ESTIMATE_PROPAGATOR_H
28 #define G2O_ESTIMATE_PROPAGATOR_H
29 
30 #include "optimizable_graph.h"
31 #include "sparse_optimizer.h"
32 
33 #include <map>
34 #include <set>
35 #include <limits>
36 
37 #ifdef _MSC_VER
38 #include <unordered_map>
39 #else
40 #include <tr1/unordered_map>
41 #endif
42 
43 namespace g2o {
44 
51  public:
53  virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const;
54  virtual const char* name() const { return "spanning tree";}
55  protected:
57  };
58 
66  public:
68  virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const;
69  virtual const char* name() const { return "odometry";}
70  };
71 
76  public:
77 
83  struct PropagateAction {
85  {
86  if (! to->fixed())
87  e->initialEstimate(from, to);
88  }
89  };
90 
92 
93  class AdjacencyMapEntry;
94 
98  class PriorityQueue : public std::multimap<double, AdjacencyMapEntry*> {
99  public:
100  void push(AdjacencyMapEntry* entry);
101  AdjacencyMapEntry* pop();
102  };
103 
108  public:
109  friend class EstimatePropagator;
110  friend class PriorityQueue;
112  void reset();
113  OptimizableGraph::Vertex* child() const {return _child;}
114  const OptimizableGraph::VertexSet& parent() const {return _parent;}
115  OptimizableGraph::Edge* edge() const {return _edge;}
116  double distance() const {return _distance;}
117  int frontierLevel() const { return _frontierLevel;}
118 
119  protected:
123  double _distance;
125  private: // for PriorityQueue
126  bool inQueue;
127  PriorityQueue::iterator queueIt;
128  };
129 
134  public:
135  size_t operator ()(const OptimizableGraph::Vertex* v) const { return v->id();}
136  };
137 
138  typedef std::tr1::unordered_map<OptimizableGraph::Vertex*, AdjacencyMapEntry, VertexIDHashFunction> AdjacencyMap;
139 
140  public:
142  OptimizableGraph::VertexSet& visited() {return _visited; }
143  AdjacencyMap& adjacencyMap() {return _adjacencyMap; }
145 
151  void propagate(OptimizableGraph::Vertex* v,
154  double maxDistance=std::numeric_limits<double>::max(),
155  double maxEdgeCost=std::numeric_limits<double>::max());
156 
160  void propagate(OptimizableGraph::VertexSet& vset,
163  double maxDistance=std::numeric_limits<double>::max(),
164  double maxEdgeCost=std::numeric_limits<double>::max());
165 
166  protected:
167  void reset();
168 
169  AdjacencyMap _adjacencyMap;
172  };
173 
174 }
175 #endif
int id() const
returns the id
Definition: hyper_graph.h:103
EstimatePropagatorCost(SparseOptimizer *graph)
std::set< Vertex * > VertexSet
Definition: hyper_graph.h:91
OptimizableGraph::VertexSet _visited
propagation of an initial guess
OptimizableGraph::VertexSet & visited()
Applying the action for propagating.
virtual double operator()(OptimizableGraph::Edge *edge, const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to_) const
OptimizableGraph * graph()
data structure for loopuk during Dijkstra
std::tr1::unordered_map< OptimizableGraph::Vertex *, AdjacencyMapEntry, VertexIDHashFunction > AdjacencyMap
virtual void initialEstimate(const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to)=0
OptimizableGraph::Vertex * child() const
const OptimizableGraph::VertexSet & parent() const
virtual const char * name() const
cost for traversing along active edges in the optimizer
OptimizableGraph::Edge * edge() const
A general case Vertex for optimization.
action
virtual const char * name() const
bool fixed() const
true => this node is fixed during the optimization
virtual void operator()(OptimizableGraph::Edge *e, const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to) const
cost for traversing only odometry edges.
EstimatePropagatorCost PropagateCost
priority queue for AdjacencyMapEntry


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