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 72 of file estimate_propagator.h.

Member Typedef Documentation

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

Definition at line 135 of file estimate_propagator.h.

typedef EstimatePropagatorCost g2o::EstimatePropagator::PropagateCost

Definition at line 88 of file estimate_propagator.h.

Constructor & Destructor Documentation

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

Definition at line 66 of file estimate_propagator.cpp.

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

                                                            : _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));
     }
   }

Member Function Documentation

AdjacencyMap& g2o::EstimatePropagator::adjacencyMap ( )

inline

Definition at line 140 of file estimate_propagator.h.

140 {return _adjacencyMap; }

g2o::EstimatePropagator::_adjacencyMap

AdjacencyMap _adjacencyMap

Definition: estimate_propagator.h:166

OptimizableGraph* g2o::EstimatePropagator::graph ( )

inline

Definition at line 141 of file estimate_propagator.h.

141 {return _graph;}

g2o::EstimatePropagator::_graph

OptimizableGraph * _graph

Definition: estimate_propagator.h:168

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.

Referenced by g2o::SparseOptimizer::computeInitialGuess().

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

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 97 of file estimate_propagator.cpp.

References _adjacencyMap, g2o::EstimatePropagator::AdjacencyMapEntry::_frontierLevel, _visited, g2o::EstimatePropagator::AdjacencyMapEntry::child(), g2o::EstimatePropagator::AdjacencyMapEntry::distance(), g2o::EstimatePropagator::AdjacencyMapEntry::edge(), g2o::HyperGraph::Vertex::edges(), 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();
 
     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();
       //cerr << "uDistance " << uDistance << endl;
 
       // 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;
             //cerr << z->id() << " " << zDistance << endl;
 
             AdjacencyMap::iterator ot = _adjacencyMap.find(z);
             assert(ot!=_adjacencyMap.end());
 
             if (zDistance < ot->second.distance() && zDistance < maxDistance){
               //if (ot->second.inQueue)
                 //cerr << "Updating" << endl;
               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
     cerr << "Writing cost.dat" << endl;
     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;
     }
     cerr << "Writing init.dat" << endl;
     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
 
   }

void g2o::EstimatePropagator::reset ( )

protected

Definition at line 75 of file estimate_propagator.cpp.

References _adjacencyMap, and _visited.

Referenced by propagate().

   {
     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();
   }