hyper_dijkstra.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 <queue>
28 #include <deque>
29 #include <vector>
30 #include <assert.h>
31 #include <iostream>
32 #include "hyper_dijkstra.h"
33 #include "../stuff/macros.h"
34 
35 namespace g2o{
36 
37  using namespace std;
38 
40  (void) v;
41  (void) vParent;
42  (void) e;
43  return std::numeric_limits<double>::max();
44  }
45 
47  if (distance==-1)
48  return perform (v,vParent,e);
49  return std::numeric_limits<double>::max();
50  }
51 
53  HyperGraph::Edge* edge_, double distance_)
54  {
55  _child=child_;
56  _parent=parent_;
57  _edge=edge_;
58  _distance=distance_;
59  }
60 
62  {
63  for (HyperGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); it++){
64  AdjacencyMapEntry entry(it->second, 0,0,std::numeric_limits< double >::max());
65  _adjacencyMap.insert(make_pair(entry.child(), entry));
66  }
67  }
68 
70  {
71  for (HyperGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); it++){
72  AdjacencyMap::iterator at=_adjacencyMap.find(*it);
73  assert(at!=_adjacencyMap.end());
74  at->second=AdjacencyMapEntry(at->first,0,0,std::numeric_limits< double >::max());
75  }
76  _visited.clear();
77  }
78 
79 
81  {
82  return a.distance()>b.distance();
83  }
84 
85 
87  double maxDistance, double comparisonConditioner, bool directed, double maxEdgeCost)
88  {
89  reset();
90  std::priority_queue< AdjacencyMapEntry > frontier;
91  for (HyperGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
92  HyperGraph::Vertex* v=*vit;
93  assert(v!=0);
94  AdjacencyMap::iterator it=_adjacencyMap.find(v);
95  if (it == _adjacencyMap.end()) {
96  cerr << __PRETTY_FUNCTION__ << "Vertex " << v->id() << " is not in the adjacency map" << endl;
97  }
98  assert(it!=_adjacencyMap.end());
99  it->second._distance=0.;
100  it->second._parent=0;
101  frontier.push(it->second);
102  }
103 
104  while(! frontier.empty()){
105  AdjacencyMapEntry entry=frontier.top();
106  frontier.pop();
107  HyperGraph::Vertex* u=entry.child();
108  AdjacencyMap::iterator ut=_adjacencyMap.find(u);
109  if (ut == _adjacencyMap.end()) {
110  cerr << __PRETTY_FUNCTION__ << "Vertex " << u->id() << " is not in the adjacency map" << endl;
111  }
112  assert(ut!=_adjacencyMap.end());
113  double uDistance=ut->second.distance();
114 
115  std::pair< HyperGraph::VertexSet::iterator, bool> insertResult=_visited.insert(u); (void) insertResult;
116  HyperGraph::EdgeSet::iterator et=u->edges().begin();
117  while (et != u->edges().end()){
118  HyperGraph::Edge* edge=*et;
119  ++et;
120 
121  if (directed && edge->vertex(0) != u)
122  continue;
123 
124  for (size_t i = 0; i < edge->vertices().size(); ++i) {
125  HyperGraph::Vertex* z = edge->vertex(i);
126  if (z == u)
127  continue;
128 
129  double edgeDistance=(*cost)(edge, u, z);
130  if (edgeDistance==std::numeric_limits< double >::max() || edgeDistance > maxEdgeCost)
131  continue;
132  double zDistance=uDistance+edgeDistance;
133  //cerr << z->id() << " " << zDistance << endl;
134 
135  AdjacencyMap::iterator ot=_adjacencyMap.find(z);
136  assert(ot!=_adjacencyMap.end());
137 
138  if (zDistance+comparisonConditioner<ot->second.distance() && zDistance<maxDistance){
139  ot->second._distance=zDistance;
140  ot->second._parent=u;
141  ot->second._edge=edge;
142  frontier.push(ot->second);
143  }
144  }
145  }
146  }
147  }
148 
150  double comparisonConditioner, bool directed, double maxEdgeCost)
151  {
153  vset.insert(v);
154  shortestPaths(vset, cost, maxDistance, comparisonConditioner, directed, maxEdgeCost);
155  }
156 
158  {
159  for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
160  AdjacencyMapEntry& entry(it->second);
161  entry._children.clear();
162  }
163  for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
164  AdjacencyMapEntry& entry(it->second);
165  HyperGraph::Vertex* parent=entry.parent();
166  if (!parent){
167  continue;
168  }
169  HyperGraph::Vertex* v=entry.child();
170  assert (v==it->first);
171 
172  AdjacencyMap::iterator pt=amap.find(parent);
173  assert(pt!=amap.end());
174  pt->second._children.insert(v);
175  }
176  }
177 
178 
179  void HyperDijkstra::visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance)
180  {
181 
182  typedef std::deque<HyperGraph::Vertex*> Deque;
183  Deque q;
184  // scans for the vertices without the parent (whcih are the roots of the trees) and applies the action to them.
185  for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
186  AdjacencyMapEntry& entry(it->second);
187  if (! entry.parent()) {
188  action->perform(it->first,0,0);
189  q.push_back(it->first);
190  }
191  }
192 
193  //std::cerr << "q.size()" << q.size() << endl;
194  int count=0;
195  while (! q.empty()){
196  HyperGraph::Vertex* parent=q.front();
197  q.pop_front();
198  ++count;
199  AdjacencyMap::iterator parentIt=amap.find(parent);
200  if (parentIt==amap.end()) {
201  continue;
202  }
203  //cerr << "parent= " << parent << " parent id= " << parent->id() << "\t children id =";
204  HyperGraph::VertexSet& childs(parentIt->second.children());
205  for (HyperGraph::VertexSet::iterator childsIt=childs.begin(); childsIt!=childs.end(); ++childsIt){
206  HyperGraph::Vertex* child=*childsIt;
207  //cerr << child->id();
208  AdjacencyMap::iterator adjacencyIt=amap.find(child);
209  assert (adjacencyIt!=amap.end());
210  HyperGraph::Edge* edge=adjacencyIt->second.edge();
211 
212  assert(adjacencyIt->first==child);
213  assert(adjacencyIt->second.child()==child);
214  assert(adjacencyIt->second.parent()==parent);
215  if (! useDistance) {
216  action->perform(child, parent, edge);
217  } else {
218  action->perform(child, parent, edge, adjacencyIt->second.distance());
219  }
220  q.push_back(child);
221  }
222  //cerr << endl;
223  }
224 
225  }
226 
228  HyperGraph::VertexSet& startingSet,
230  HyperDijkstra::CostFunction* cost, double distance,
231  double comparisonConditioner, double maxEdgeCost)
232  {
233  typedef std::queue<HyperGraph::Vertex*> VertexDeque;
234  visited.clear();
235  connected.clear();
236  VertexDeque frontier;
237  HyperDijkstra dv(g);
238  connected.insert(v);
239  frontier.push(v);
240  while (! frontier.empty()){
241  HyperGraph::Vertex* v0=frontier.front();
242  frontier.pop();
243  dv.shortestPaths(v0, cost, distance, comparisonConditioner, false, maxEdgeCost);
244  for (HyperGraph::VertexSet::iterator it=dv.visited().begin(); it!=dv.visited().end(); ++it){
245  visited.insert(*it);
246  if (startingSet.find(*it)==startingSet.end())
247  continue;
248  std::pair<HyperGraph::VertexSet::iterator, bool> insertOutcome=connected.insert(*it);
249  if (insertOutcome.second){ // the node was not in the connectedSet;
250  frontier.push(dynamic_cast<HyperGraph::Vertex*>(*it));
251  }
252  }
253  }
254  }
255 
257  {
258  return 1.;
259  }
260 
261 };
AdjacencyMapEntry(HyperGraph::Vertex *_child=0, HyperGraph::Vertex *_parent=0, HyperGraph::Edge *_edge=0, double _distance=std::numeric_limits< double >::max())
int id() const
returns the id
Definition: hyper_graph.h:103
const Vertex * vertex(size_t i) const
Definition: hyper_graph.h:140
#define __PRETTY_FUNCTION__
Definition: macros.h:95
HyperGraph::VertexSet & visited()
static void computeTree(AdjacencyMap &amap)
HyperGraph::Vertex * parent() const
std::set< Vertex * > VertexSet
Definition: hyper_graph.h:91
HyperDijkstra(HyperGraph *g)
const VertexIDMap & vertices() const
Definition: hyper_graph.h:177
HyperGraph::VertexSet _visited
static void connectedSubset(HyperGraph::VertexSet &connected, HyperGraph::VertexSet &visited, HyperGraph::VertexSet &startingSet, HyperGraph *g, HyperGraph::Vertex *v, HyperDijkstra::CostFunction *cost, double distance, double comparisonConditioner, double maxEdgeCost=std::numeric_limits< double >::max())
static void visitAdjacencyMap(AdjacencyMap &amap, TreeAction *action, bool useDistance=false)
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
void shortestPaths(HyperGraph::Vertex *v, HyperDijkstra::CostFunction *cost, double maxDistance=std::numeric_limits< double >::max(), double comparisonConditioner=1e-3, bool directed=false, double maxEdgeCost=std::numeric_limits< double >::max())
HyperGraph * _graph
std::map< HyperGraph::Vertex *, AdjacencyMapEntry > AdjacencyMap
TFSIMD_FORCE_INLINE const tfScalar & z() const
abstract Vertex, your types must derive from that one
Definition: hyper_graph.h:97
bool operator<(const HyperDijkstra::AdjacencyMapEntry &a, const HyperDijkstra::AdjacencyMapEntry &b)
virtual double operator()(HyperGraph::Edge *edge, HyperGraph::Vertex *from, HyperGraph::Vertex *to)
virtual double perform(HyperGraph::Vertex *v, HyperGraph::Vertex *vParent, HyperGraph::Edge *e)
AdjacencyMap _adjacencyMap
HyperGraph::Vertex * child() const


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