g2o::HyperDijkstra Struct Reference

#include <hyper_dijkstra.h>

Collaboration diagram for g2o::HyperDijkstra:

Classes
struct	AdjacencyMapEntry
struct	CostFunction
struct	TreeAction

Public Types
typedef std::map< HyperGraph::Vertex *, AdjacencyMapEntry >	AdjacencyMap

Public Member Functions
	HyperDijkstra (HyperGraph *g)
HyperGraph::VertexSet &	visited ()
AdjacencyMap &	adjacencyMap ()
HyperGraph *	graph ()
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())
void	shortestPaths (HyperGraph::VertexSet &vset, HyperDijkstra::CostFunction *cost, double maxDistance=std::numeric_limits< double >::max(), double comparisonConditioner=1e-3, bool directed=false, double maxEdgeCost=std::numeric_limits< double >::max())

Static Public Member Functions
static void	computeTree (AdjacencyMap &amap)
static void	visitAdjacencyMap (AdjacencyMap &amap, TreeAction *action, bool useDistance=false)
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())

Protected Member Functions
void	reset ()

Protected Attributes
AdjacencyMap	_adjacencyMap
HyperGraph::VertexSet	_visited
HyperGraph *	_graph

Detailed Description

Definition at line 38 of file hyper_dijkstra.h.

Member Typedef Documentation

typedef std::map<HyperGraph::Vertex*, AdjacencyMapEntry> g2o::HyperDijkstra::AdjacencyMap

Definition at line 70 of file hyper_dijkstra.h.

Constructor & Destructor Documentation

g2o::HyperDijkstra::HyperDijkstra

(

HyperGraph *

g

)

Definition at line 61 of file hyper_dijkstra.cpp.

References _adjacencyMap, _graph, g2o::HyperDijkstra::AdjacencyMapEntry::child(), and g2o::HyperGraph::vertices().

                                           : _graph(g)
  {
    for (HyperGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); it++){
      AdjacencyMapEntry entry(it->second, 0,0,std::numeric_limits< double >::max());
      _adjacencyMap.insert(make_pair(entry.child(), entry));
    }
  }

Member Function Documentation

AdjacencyMap& g2o::HyperDijkstra::adjacencyMap ( )

inline

Definition at line 73 of file hyper_dijkstra.h.

Referenced by g2o::computeSimpleStars(), and g2o::SolverSLAM2DLinear::solveOrientation().

{return _adjacencyMap; }

void g2o::HyperDijkstra::computeTree ( AdjacencyMap &  amap )

static

Definition at line 157 of file hyper_dijkstra.cpp.

References g2o::HyperDijkstra::AdjacencyMapEntry::_children, g2o::HyperDijkstra::AdjacencyMapEntry::child(), and g2o::HyperDijkstra::AdjacencyMapEntry::parent().

Referenced by g2o::computeSimpleStars(), and g2o::SolverSLAM2DLinear::solveOrientation().

  {
    for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
      AdjacencyMapEntry& entry(it->second);
      entry._children.clear();
    }
    for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
      AdjacencyMapEntry& entry(it->second);
      HyperGraph::Vertex* parent=entry.parent();
      if (!parent){
        continue;
      }
      HyperGraph::Vertex* v=entry.child();
      assert (v==it->first);

      AdjacencyMap::iterator pt=amap.find(parent);
      assert(pt!=amap.end());
      pt->second._children.insert(v);
    }
  }

void g2o::HyperDijkstra::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

Definition at line 227 of file hyper_dijkstra.cpp.

References shortestPaths(), and visited().

  {
    typedef std::queue<HyperGraph::Vertex*> VertexDeque;
    visited.clear();
    connected.clear();
    VertexDeque frontier;
    HyperDijkstra dv(g);
    connected.insert(v);
    frontier.push(v);
    while (! frontier.empty()){
      HyperGraph::Vertex* v0=frontier.front();
      frontier.pop();
      dv.shortestPaths(v0, cost, distance, comparisonConditioner, false, maxEdgeCost);
      for (HyperGraph::VertexSet::iterator it=dv.visited().begin(); it!=dv.visited().end(); ++it){
        visited.insert(*it);
        if (startingSet.find(*it)==startingSet.end())
          continue;
        std::pair<HyperGraph::VertexSet::iterator, bool> insertOutcome=connected.insert(*it);
        if (insertOutcome.second){ // the node was not in the connectedSet;
          frontier.push(dynamic_cast<HyperGraph::Vertex*>(*it));
        }
      }
    }
  }

HyperGraph* g2o::HyperDijkstra::graph ( )

inline

Definition at line 74 of file hyper_dijkstra.h.

{return _graph;} 

void g2o::HyperDijkstra::reset ( )

protected

Definition at line 69 of file hyper_dijkstra.cpp.

References _adjacencyMap, and _visited.

Referenced by shortestPaths().

  {
    for (HyperGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); it++){
      AdjacencyMap::iterator at=_adjacencyMap.find(*it);
      assert(at!=_adjacencyMap.end());
      at->second=AdjacencyMapEntry(at->first,0,0,std::numeric_limits< double >::max());
    }
    _visited.clear();
  }

void g2o::HyperDijkstra::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()
	)

Definition at line 149 of file hyper_dijkstra.cpp.

Referenced by g2o::computeSimpleStars(), connectedSubset(), main(), and g2o::SolverSLAM2DLinear::solveOrientation().

  {
    HyperGraph::VertexSet vset;
    vset.insert(v);
    shortestPaths(vset, cost, maxDistance, comparisonConditioner, directed, maxEdgeCost);
  }

void g2o::HyperDijkstra::shortestPaths	(	HyperGraph::VertexSet &	vset,
		HyperDijkstra::CostFunction *	cost,
		double	maxDistance = std::numeric_limits< double >::max(),
		double	comparisonConditioner = 1e-3,
		bool	directed = false,
		double	maxEdgeCost = std::numeric_limits< double >::max()
	)

Definition at line 86 of file hyper_dijkstra.cpp.

References __PRETTY_FUNCTION__, _adjacencyMap, _visited, g2o::HyperDijkstra::AdjacencyMapEntry::child(), g2o::HyperGraph::Vertex::edges(), g2o::HyperGraph::Vertex::id(), reset(), g2o::HyperGraph::Edge::vertex(), and g2o::HyperGraph::Edge::vertices().

  {
    reset();
    std::priority_queue< AdjacencyMapEntry > frontier;
    for (HyperGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
      HyperGraph::Vertex* v=*vit;
      assert(v!=0);
      AdjacencyMap::iterator it=_adjacencyMap.find(v);
      if (it == _adjacencyMap.end()) {
        cerr << __PRETTY_FUNCTION__ << "Vertex " << v->id() << " is not in the adjacency map" << endl;
      }
      assert(it!=_adjacencyMap.end());
      it->second._distance=0.;
      it->second._parent=0;
      frontier.push(it->second);
    }

    while(! frontier.empty()){
      AdjacencyMapEntry entry=frontier.top();
      frontier.pop();
      HyperGraph::Vertex* u=entry.child();
      AdjacencyMap::iterator ut=_adjacencyMap.find(u);
      if (ut == _adjacencyMap.end()) {
        cerr << __PRETTY_FUNCTION__ << "Vertex " << u->id() << " is not in the adjacency map" << endl;
      }
      assert(ut!=_adjacencyMap.end());
      double uDistance=ut->second.distance();

      std::pair< HyperGraph::VertexSet::iterator, bool> insertResult=_visited.insert(u); (void) insertResult;
      HyperGraph::EdgeSet::iterator et=u->edges().begin();
      while (et != u->edges().end()){
        HyperGraph::Edge* edge=*et;
        ++et;

        if (directed && edge->vertex(0) != u)
          continue;

        for (size_t i = 0; i < edge->vertices().size(); ++i) {
          HyperGraph::Vertex* z = edge->vertex(i);
          if (z == u)
            continue;

          double edgeDistance=(*cost)(edge, u, z);
          if (edgeDistance==std::numeric_limits< double >::max() || edgeDistance > maxEdgeCost)
            continue;
          double zDistance=uDistance+edgeDistance;
          //cerr << z->id() << " " << zDistance << endl;

          AdjacencyMap::iterator ot=_adjacencyMap.find(z);
          assert(ot!=_adjacencyMap.end());

          if (zDistance+comparisonConditioner<ot->second.distance() && zDistance<maxDistance){
            ot->second._distance=zDistance;
            ot->second._parent=u;
            ot->second._edge=edge;
            frontier.push(ot->second);
          }
        }
      }
    }
  }

void g2o::HyperDijkstra::visitAdjacencyMap ( AdjacencyMap &  amap, TreeAction *  action, bool  useDistance = false  )

static

Definition at line 179 of file hyper_dijkstra.cpp.

References g2o::HyperDijkstra::AdjacencyMapEntry::parent(), and g2o::HyperDijkstra::TreeAction::perform().

Referenced by g2o::computeSimpleStars(), and g2o::SolverSLAM2DLinear::solveOrientation().

  {
    
    typedef std::deque<HyperGraph::Vertex*> Deque;
    Deque q;
    // scans for the vertices without the parent (whcih are the roots of the trees) and applies the action to them.
    for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
      AdjacencyMapEntry& entry(it->second);
      if (! entry.parent()) {
        action->perform(it->first,0,0);
        q.push_back(it->first);
      }
    }

    //std::cerr << "q.size()" << q.size() << endl;
    int count=0;
    while (! q.empty()){
      HyperGraph::Vertex* parent=q.front();
      q.pop_front();
      ++count;
      AdjacencyMap::iterator parentIt=amap.find(parent);
      if (parentIt==amap.end()) {
        continue;
      }
      //cerr << "parent= " << parent << " parent id= " << parent->id() << "\t children id =";
      HyperGraph::VertexSet& childs(parentIt->second.children());
      for (HyperGraph::VertexSet::iterator childsIt=childs.begin(); childsIt!=childs.end(); ++childsIt){
        HyperGraph::Vertex* child=*childsIt;
        //cerr << child->id();
        AdjacencyMap::iterator adjacencyIt=amap.find(child);
        assert (adjacencyIt!=amap.end());
        HyperGraph::Edge* edge=adjacencyIt->second.edge();  

        assert(adjacencyIt->first==child);
        assert(adjacencyIt->second.child()==child);
        assert(adjacencyIt->second.parent()==parent);
        if (! useDistance) {
          action->perform(child, parent, edge);
        } else {
          action->perform(child, parent, edge, adjacencyIt->second.distance());
        }
        q.push_back(child);
      }
      //cerr << endl;
    }

  }

HyperGraph::VertexSet& g2o::HyperDijkstra::visited ( )

inline

Definition at line 72 of file hyper_dijkstra.h.

Referenced by connectedSubset(), and main().

{return _visited; }

Member Data Documentation

AdjacencyMap g2o::HyperDijkstra::_adjacencyMap

protected

Definition at line 102 of file hyper_dijkstra.h.

Referenced by HyperDijkstra(), reset(), and shortestPaths().

HyperGraph* g2o::HyperDijkstra::_graph

protected

Definition at line 104 of file hyper_dijkstra.h.

Referenced by HyperDijkstra().

HyperGraph::VertexSet g2o::HyperDijkstra::_visited

protected

Definition at line 103 of file hyper_dijkstra.h.

Referenced by reset(), and shortestPaths().

---

The documentation for this struct was generated from the following files:

• /home/xuezhisd/CLionProjects/g2o/g2o/core/hyper_dijkstra.h
• /home/xuezhisd/CLionProjects/g2o/g2o/core/hyper_dijkstra.cpp