propagation of an initial guess More...

#include <estimate_propagator.h>

Collaboration diagram for g2o::EstimatePropagator:

[legend]

Classes
class	AdjacencyMapEntry
	data structure for loopuk during Dijkstra More...

class	PriorityQueue
	priority queue for AdjacencyMapEntry More...

struct	PropagateAction
	Applying the action for propagating. More...

class	VertexIDHashFunction
	hash function for a vertex More...

Public Types
typedef EstimatePropagatorCost	PropagateCost

typedef std::unordered_map< OptimizableGraph::Vertex *, AdjacencyMapEntry, VertexIDHashFunction >	AdjacencyMap

Public Member Functions
	EstimatePropagator (OptimizableGraph *g)

OptimizableGraph::VertexSet &	visited ()

AdjacencyMap &	adjacencyMap ()

OptimizableGraph *	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())

void	propagate (OptimizableGraph::VertexSet &vset, const EstimatePropagator::PropagateCost &cost, const EstimatePropagator::PropagateAction &action=PropagateAction(), double maxDistance=std::numeric_limits< double >::max(), double maxEdgeCost=std::numeric_limits< double >::max())

Protected Member Functions
void	reset ()

Protected Attributes
AdjacencyMap	_adjacencyMap

OptimizableGraph::VertexSet	_visited

OptimizableGraph *	_graph

Detailed Description

propagation of an initial guess

Definition at line 78 of file estimate_propagator.h.

Member Typedef Documentation

◆ AdjacencyMap

typedef std::unordered_map<OptimizableGraph::Vertex*, AdjacencyMapEntry, VertexIDHashFunction> g2o::EstimatePropagator::AdjacencyMap

Definition at line 146 of file estimate_propagator.h.

◆ PropagateCost

typedef EstimatePropagatorCost g2o::EstimatePropagator::PropagateCost

Definition at line 94 of file estimate_propagator.h.

Constructor & Destructor Documentation

◆ EstimatePropagator()

g2o::EstimatePropagator::EstimatePropagator ( OptimizableGraph * g )

Definition at line 65 of file estimate_propagator.cpp.

                                                          : _graph(g) {
  for (OptimizableGraph::VertexIDMap::const_iterator it =
           _graph->vertices().begin();
       it != _graph->vertices().end(); ++it) {
    AdjacencyMapEntry entry;
    entry._child = static_cast<OptimizableGraph::Vertex*>(it->second);
    _adjacencyMap.insert(make_pair(entry.child(), entry));
  }
}

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

Member Function Documentation

◆ adjacencyMap()

AdjacencyMap & g2o::EstimatePropagator::adjacencyMap ( )

inline

Definition at line 151 of file estimate_propagator.h.

151{ return _adjacencyMap; }

◆ graph()

OptimizableGraph * g2o::EstimatePropagator::graph ( )

inline

Definition at line 152 of file estimate_propagator.h.

152{ return _graph; }

◆ propagate() [1/2]

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

propagate an initial guess starting from v. The function computes a spanning tree whereas the cost for each edge is determined by calling cost() and the action applied to each vertex is action().

Definition at line 86 of file estimate_propagator.cpp.

                        {
  OptimizableGraph::VertexSet vset;
  vset.insert(v);
  propagate(vset, cost, action, maxDistance, maxEdgeCost);
}

References propagate().

Referenced by g2o::SparseOptimizer::computeInitialGuess(), and propagate().

◆ propagate() [2/2]

void g2o::EstimatePropagator::propagate	(	OptimizableGraph::VertexSet &	vset,
		const EstimatePropagator::PropagateCost &	cost,
		const EstimatePropagator::PropagateAction &	action = `PropagateAction()`,
		double	maxDistance = `std::numeric_limits<double>::max()`,
		double	maxEdgeCost = `std::numeric_limits<double>::max()`
	)

same as above but starting to propagate from a set of vertices instead of just a single one.

Definition at line 95 of file estimate_propagator.cpp.

                        {
  reset();
 
  PriorityQueue frontier;
  for (OptimizableGraph::VertexSet::iterator vit = vset.begin();
       vit != vset.end(); ++vit) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
    AdjacencyMap::iterator it = _adjacencyMap.find(v);
    assert(it != _adjacencyMap.end());
    it->second._distance = 0.;
    it->second._parent.clear();
    it->second._frontierLevel = 0;
    frontier.push(&it->second);
  }
 
  while (!frontier.empty()) {
    AdjacencyMapEntry* entry = frontier.pop();
    OptimizableGraph::Vertex* u = entry->child();
    double uDistance = entry->distance();
 
    // initialize the vertex
    if (entry->_frontierLevel > 0) {
      action(entry->edge(), entry->parent(), u);
    }
 
    /* std::pair< OptimizableGraph::VertexSet::iterator, bool> insertResult = */
    _visited.insert(u);
    OptimizableGraph::EdgeSet::iterator et = u->edges().begin();
    while (et != u->edges().end()) {
      OptimizableGraph::Edge* edge = static_cast<OptimizableGraph::Edge*>(*et);
      ++et;
 
      int maxFrontier = -1;
      OptimizableGraph::VertexSet initializedVertices;
      for (size_t i = 0; i < edge->vertices().size(); ++i) {
        OptimizableGraph::Vertex* z =
            static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
        if (!z) continue;
        AdjacencyMap::iterator ot = _adjacencyMap.find(z);
        if (ot->second._distance != numeric_limits<double>::max()) {
          initializedVertices.insert(z);
          maxFrontier = (max)(maxFrontier, ot->second._frontierLevel);
        }
      }
      assert(maxFrontier >= 0);
 
      for (size_t i = 0; i < edge->vertices().size(); ++i) {
        OptimizableGraph::Vertex* z =
            static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
        if (!z) continue;
        if (z == u) continue;
        size_t wasInitialized = initializedVertices.erase(z);
 
        double edgeDistance = cost(edge, initializedVertices, z);
        if (edgeDistance > 0. &&
            edgeDistance != std::numeric_limits<double>::max() &&
            edgeDistance < maxEdgeCost) {
          double zDistance = uDistance + edgeDistance;
 
          AdjacencyMap::iterator ot = _adjacencyMap.find(z);
          assert(ot != _adjacencyMap.end());
 
          if (zDistance < ot->second.distance() && zDistance < maxDistance) {
            ot->second._distance = zDistance;
            ot->second._parent = initializedVertices;
            ot->second._edge = edge;
            ot->second._frontierLevel = maxFrontier + 1;
            frontier.push(&ot->second);
          }
        }
 
        if (wasInitialized > 0) initializedVertices.insert(z);
      }
    }
  }
 
  // writing debug information like cost for reaching each vertex and the parent
  // used to initialize
#ifdef DEBUG_ESTIMATE_PROPAGATOR
  G2O_DEBUG("Writing cost.dat");
  ofstream costStream("cost.dat");
  for (AdjacencyMap::const_iterator it = _adjacencyMap.begin();
       it != _adjacencyMap.end(); ++it) {
    HyperGraph::Vertex* u = it->second.child();
    costStream << "vertex " << u->id() << "  cost " << it->second._distance
               << endl;
  }
  G2O_DEBUG("Writing init.dat");
  ofstream initStream("init.dat");
  vector<AdjacencyMapEntry*> frontierLevels;
  for (AdjacencyMap::iterator it = _adjacencyMap.begin();
       it != _adjacencyMap.end(); ++it) {
    if (it->second._frontierLevel > 0) frontierLevels.push_back(&it->second);
  }
  sort(frontierLevels.begin(), frontierLevels.end(), FrontierLevelCmp());
  for (vector<AdjacencyMapEntry*>::const_iterator it = frontierLevels.begin();
       it != frontierLevels.end(); ++it) {
    AdjacencyMapEntry* entry = *it;
    OptimizableGraph::Vertex* to = entry->child();
 
    initStream << "calling init level = " << entry->_frontierLevel << "\t (";
    for (OptimizableGraph::VertexSet::iterator pit = entry->parent().begin();
         pit != entry->parent().end(); ++pit) {
      initStream << " " << (*pit)->id();
    }
    initStream << " ) -> " << to->id() << endl;
  }
#endif
}

References _adjacencyMap, g2o::EstimatePropagator::AdjacencyMapEntry::_frontierLevel, _visited, g2o::EstimatePropagator::AdjacencyMapEntry::child(), g2o::EstimatePropagator::AdjacencyMapEntry::distance(), g2o::EstimatePropagator::AdjacencyMapEntry::edge(), g2o::HyperGraph::Vertex::edges(), G2O_DEBUG, g2o::HyperGraph::Vertex::id(), g2o::EstimatePropagator::AdjacencyMapEntry::parent(), g2o::EstimatePropagator::PriorityQueue::pop(), g2o::EstimatePropagator::PriorityQueue::push(), reset(), g2o::HyperGraph::Edge::vertex(), and g2o::HyperGraph::Edge::vertices().

◆ reset()

void g2o::EstimatePropagator::reset ( )

protected

Definition at line 75 of file estimate_propagator.cpp.

                               {
  for (OptimizableGraph::VertexSet::iterator it = _visited.begin();
       it != _visited.end(); ++it) {
    OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
    AdjacencyMap::iterator at = _adjacencyMap.find(v);
    assert(at != _adjacencyMap.end());
    at->second.reset();
  }
  _visited.clear();
}

References _adjacencyMap, and _visited.

Referenced by propagate().

◆ visited()

OptimizableGraph::VertexSet & g2o::EstimatePropagator::visited ( )

inline

Definition at line 150 of file estimate_propagator.h.

150{ return _visited; }

Member Data Documentation

◆ _adjacencyMap

AdjacencyMap g2o::EstimatePropagator::_adjacencyMap

protected

Definition at line 180 of file estimate_propagator.h.

Referenced by EstimatePropagator(), propagate(), and reset().

◆ _graph

OptimizableGraph* g2o::EstimatePropagator::_graph

protected

Definition at line 182 of file estimate_propagator.h.

Referenced by EstimatePropagator().

◆ _visited

OptimizableGraph::VertexSet g2o::EstimatePropagator::_visited

protected

Definition at line 181 of file estimate_propagator.h.

Referenced by propagate(), and reset().

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

/build/reproducible-path/g2o-0~20230806/g2o/core/estimate_propagator.h
/build/reproducible-path/g2o-0~20230806/g2o/core/estimate_propagator.cpp

Classes

Public Types

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Member Typedef Documentation

◆ AdjacencyMap

◆ PropagateCost

Constructor & Destructor Documentation

◆ EstimatePropagator()

Member Function Documentation

◆ adjacencyMap()

◆ graph()

◆ propagate() [1/2]

◆ propagate() [2/2]

◆ reset()

◆ visited()

Member Data Documentation

◆ _adjacencyMap

◆ _graph

◆ _visited