estimate_propagator.cpp
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 #include "estimate_propagator.h"
28 
29 #include <queue>
30 #include <vector>
31 #include <cassert>
32 #include <iostream>
33 #include <algorithm>
34 #include <fstream>
35 
36 //#define DEBUG_ESTIMATE_PROPAGATOR
37 
38 using namespace std;
39 
40 namespace g2o {
41 
42 # ifdef DEBUG_ESTIMATE_PROPAGATOR
43  struct FrontierLevelCmp {
44  bool operator()(EstimatePropagator::AdjacencyMapEntry* e1, EstimatePropagator::AdjacencyMapEntry* e2) const
45  {
46  return e1->frontierLevel() < e2->frontierLevel();
47  }
48  };
49 # endif
50 
51  EstimatePropagator::AdjacencyMapEntry::AdjacencyMapEntry()
52  {
53  reset();
54  }
55 
56  void EstimatePropagator::AdjacencyMapEntry::reset()
57  {
58  _child = 0;
59  _parent.clear();
60  _edge = 0;
61  _distance = numeric_limits<double>::max();
62  _frontierLevel = -1;
63  inQueue = false;
64  }
65 
66  EstimatePropagator::EstimatePropagator(OptimizableGraph* g): _graph(g)
67  {
68  for (OptimizableGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it){
69  AdjacencyMapEntry entry;
70  entry._child = static_cast<OptimizableGraph::Vertex*>(it->second);
71  _adjacencyMap.insert(make_pair(entry.child(), entry));
72  }
73  }
74 
76  {
77  for (OptimizableGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); ++it){
78  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
79  AdjacencyMap::iterator at = _adjacencyMap.find(v);
80  assert(at != _adjacencyMap.end());
81  at->second.reset();
82  }
83  _visited.clear();
84  }
85 
89  double maxDistance,
90  double maxEdgeCost)
91  {
93  vset.insert(v);
94  propagate(vset, cost, action, maxDistance, maxEdgeCost);
95  }
96 
100  double maxDistance,
101  double maxEdgeCost)
102  {
103  reset();
104 
105  PriorityQueue frontier;
106  for (OptimizableGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
107  OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
108  AdjacencyMap::iterator it = _adjacencyMap.find(v);
109  assert(it != _adjacencyMap.end());
110  it->second._distance = 0.;
111  it->second._parent.clear();
112  it->second._frontierLevel = 0;
113  frontier.push(&it->second);
114  }
115 
116  while(! frontier.empty()){
117  AdjacencyMapEntry* entry = frontier.pop();
118  OptimizableGraph::Vertex* u = entry->child();
119  double uDistance = entry->distance();
120  //cerr << "uDistance " << uDistance << endl;
121 
122  // initialize the vertex
123  if (entry->_frontierLevel > 0) {
124  action(entry->edge(), entry->parent(), u);
125  }
126 
127  /* std::pair< OptimizableGraph::VertexSet::iterator, bool> insertResult = */ _visited.insert(u);
128  OptimizableGraph::EdgeSet::iterator et = u->edges().begin();
129  while (et != u->edges().end()){
130  OptimizableGraph::Edge* edge = static_cast<OptimizableGraph::Edge*>(*et);
131  ++et;
132 
133  int maxFrontier = -1;
134  OptimizableGraph::VertexSet initializedVertices;
135  for (size_t i = 0; i < edge->vertices().size(); ++i) {
136  OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
137  AdjacencyMap::iterator ot = _adjacencyMap.find(z);
138  if (ot->second._distance != numeric_limits<double>::max()) {
139  initializedVertices.insert(z);
140  maxFrontier = (max)(maxFrontier, ot->second._frontierLevel);
141  }
142  }
143  assert(maxFrontier >= 0);
144 
145  for (size_t i = 0; i < edge->vertices().size(); ++i) {
146  OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
147  if (z == u)
148  continue;
149 
150  size_t wasInitialized = initializedVertices.erase(z);
151 
152  double edgeDistance = cost(edge, initializedVertices, z);
153  if (edgeDistance > 0. && edgeDistance != std::numeric_limits<double>::max() && edgeDistance < maxEdgeCost) {
154  double zDistance = uDistance + edgeDistance;
155  //cerr << z->id() << " " << zDistance << endl;
156 
157  AdjacencyMap::iterator ot = _adjacencyMap.find(z);
158  assert(ot!=_adjacencyMap.end());
159 
160  if (zDistance < ot->second.distance() && zDistance < maxDistance){
161  //if (ot->second.inQueue)
162  //cerr << "Updating" << endl;
163  ot->second._distance = zDistance;
164  ot->second._parent = initializedVertices;
165  ot->second._edge = edge;
166  ot->second._frontierLevel = maxFrontier + 1;
167  frontier.push(&ot->second);
168  }
169  }
170 
171  if (wasInitialized > 0)
172  initializedVertices.insert(z);
173 
174  }
175  }
176  }
177 
178  // writing debug information like cost for reaching each vertex and the parent used to initialize
179 #ifdef DEBUG_ESTIMATE_PROPAGATOR
180  cerr << "Writing cost.dat" << endl;
181  ofstream costStream("cost.dat");
182  for (AdjacencyMap::const_iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
183  HyperGraph::Vertex* u = it->second.child();
184  costStream << "vertex " << u->id() << " cost " << it->second._distance << endl;
185  }
186  cerr << "Writing init.dat" << endl;
187  ofstream initStream("init.dat");
188  vector<AdjacencyMapEntry*> frontierLevels;
189  for (AdjacencyMap::iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
190  if (it->second._frontierLevel > 0)
191  frontierLevels.push_back(&it->second);
192  }
193  sort(frontierLevels.begin(), frontierLevels.end(), FrontierLevelCmp());
194  for (vector<AdjacencyMapEntry*>::const_iterator it = frontierLevels.begin(); it != frontierLevels.end(); ++it) {
195  AdjacencyMapEntry* entry = *it;
196  OptimizableGraph::Vertex* to = entry->child();
197 
198  initStream << "calling init level = " << entry->_frontierLevel << "\t (";
199  for (OptimizableGraph::VertexSet::iterator pit = entry->parent().begin(); pit != entry->parent().end(); ++pit) {
200  initStream << " " << (*pit)->id();
201  }
202  initStream << " ) -> " << to->id() << endl;
203  }
204 #endif
205 
206  }
207 
209  {
210  assert(entry != NULL);
211  if (entry->inQueue) {
212  assert(entry->queueIt->second == entry);
213  erase(entry->queueIt);
214  }
215 
216  entry->queueIt = insert(std::make_pair(entry->distance(), entry));
217  assert(entry->queueIt != end());
218  entry->inQueue = true;
219  }
220 
222  {
223  assert(!empty());
224  iterator it = begin();
225  AdjacencyMapEntry* entry = it->second;
226  erase(it);
227 
228  assert(entry != NULL);
229  entry->queueIt = end();
230  entry->inQueue = false;
231  return entry;
232  }
233 
235  _graph(graph)
236  {
237  }
238 
240  {
241  OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
242  OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
243  SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
244  if (it == _graph->activeEdges().end()) // it has to be an active edge
245  return std::numeric_limits<double>::max();
246  return e->initialEstimatePossible(from, to);
247  }
248 
251  {
252  }
253 
255  {
256  OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
257  OptimizableGraph::Vertex* from = dynamic_cast<OptimizableGraph::Vertex*>(*from_.begin());
258  OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
259  if (std::abs(from->id() - to->id()) != 1) // simple method to identify odometry edges in a pose graph
260  return std::numeric_limits<double>::max();
261  SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
262  if (it == _graph->activeEdges().end()) // it has to be an active edge
263  return std::numeric_limits<double>::max();
264  return e->initialEstimatePossible(from_, to);
265  }
266 
267 } // end namespace
int id() const
returns the id
Definition: hyper_graph.h:103
const Vertex * vertex(size_t i) const
Definition: hyper_graph.h:140
EstimatePropagatorCost(SparseOptimizer *graph)
std::set< Vertex * > VertexSet
Definition: hyper_graph.h:91
OptimizableGraph::VertexSet _visited
virtual double initialEstimatePossible(const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to)
Applying the action for propagating.
virtual double operator()(OptimizableGraph::Edge *edge, const OptimizableGraph::VertexSet &from, OptimizableGraph::Vertex *to_) const
const EdgeContainer & activeEdges() const
the edges active in the current optimization
const VertexIDMap & vertices() const
Definition: hyper_graph.h:177
OptimizableGraph * graph()
data structure for loopuk during Dijkstra
const VertexContainer & vertices() const
Definition: hyper_graph.h:132
const EdgeSet & edges() const
returns the set of hyper-edges that are leaving/entering in this vertex
Definition: hyper_graph.h:106
OptimizableGraph::Vertex * child() const
const OptimizableGraph::VertexSet & parent() const
EstimatePropagatorCostOdometry(SparseOptimizer *graph)
void propagate(OptimizableGraph::Vertex *v, const EstimatePropagator::PropagateCost &cost, const EstimatePropagator::PropagateAction &action=PropagateAction(), double maxDistance=std::numeric_limits< double >::max(), double maxEdgeCost=std::numeric_limits< double >::max())
cost for traversing along active edges in the optimizer
OptimizableGraph::Edge * edge() const
A general case Vertex for optimization.
TFSIMD_FORCE_INLINE const tfScalar & z() const
abstract Vertex, your types must derive from that one
Definition: hyper_graph.h:97
EdgeContainer::const_iterator findActiveEdge(const OptimizableGraph::Edge *e) const
virtual double operator()(OptimizableGraph::Edge *edge, const OptimizableGraph::VertexSet &from_, OptimizableGraph::Vertex *to_) const
priority queue for AdjacencyMapEntry


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