estimate_propagator.cpp

Go to the documentation of this file.

// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are
// met:
//
// * Redistributions of source code must retain the above copyright notice,
//   this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
//   notice, this list of conditions and the following disclaimer in the
//   documentation and/or other materials provided with the distribution.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
#include "estimate_propagator.h"
 
#include <algorithm>
#include <cassert>
#include <fstream>
#include <iostream>
#include <queue>
#include <vector>
 
// #define DEBUG_ESTIMATE_PROPAGATOR
#ifdef DEBUG_ESTIMATE_PROPAGATOR
#include "g2o/stuff/logger.h"
#endif
 
using namespace std;
 
namespace g2o {
 
#ifdef DEBUG_ESTIMATE_PROPAGATOR
struct FrontierLevelCmp {
  bool operator()(EstimatePropagator::AdjacencyMapEntry* e1,
                  EstimatePropagator::AdjacencyMapEntry* e2) const {
    return e1->frontierLevel() < e2->frontierLevel();
  }
};
#endif
 
EstimatePropagator::AdjacencyMapEntry::AdjacencyMapEntry() { reset(); }
 
void EstimatePropagator::AdjacencyMapEntry::reset() {
  _child = 0;
  _parent.clear();
  _edge = 0;
  _distance = numeric_limits<double>::max();
  _frontierLevel = -1;
  inQueue = false;
}
 
EstimatePropagator::EstimatePropagator(OptimizableGraph* g) : _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));
  }
}
 
void EstimatePropagator::reset() {
  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();
}
 
void EstimatePropagator::propagate(
    OptimizableGraph::Vertex* v, const EstimatePropagator::PropagateCost& cost,
    const EstimatePropagator::PropagateAction& action, double maxDistance,
    double maxEdgeCost) {
  OptimizableGraph::VertexSet vset;
  vset.insert(v);
  propagate(vset, cost, action, maxDistance, maxEdgeCost);
}
 
void EstimatePropagator::propagate(
    OptimizableGraph::VertexSet& vset,
    const EstimatePropagator::PropagateCost& cost,
    const EstimatePropagator::PropagateAction& action, double maxDistance,
    double maxEdgeCost) {
  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
}
 
void EstimatePropagator::PriorityQueue::push(AdjacencyMapEntry* entry) {
  assert(entry != NULL);
  if (entry->inQueue) {
    assert(entry->queueIt->second == entry);
    erase(entry->queueIt);
  }
 
  entry->queueIt = insert(std::make_pair(entry->distance(), entry));
  assert(entry->queueIt != end());
  entry->inQueue = true;
}
 
EstimatePropagator::AdjacencyMapEntry*
EstimatePropagator::PriorityQueue::pop() {
  assert(!empty());
  iterator it = begin();
  AdjacencyMapEntry* entry = it->second;
  erase(it);
 
  assert(entry != NULL);
  entry->queueIt = end();
  entry->inQueue = false;
  return entry;
}
 
EstimatePropagatorCost::EstimatePropagatorCost(SparseOptimizer* graph)
    : _graph(graph) {}
 
double EstimatePropagatorCost::operator()(
    OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from,
    OptimizableGraph::Vertex* to_) const {
  OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
  OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
  SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
  if (it == _graph->activeEdges().end())  // it has to be an active edge
    return std::numeric_limits<double>::max();
  return e->initialEstimatePossible(from, to);
}
 
EstimatePropagatorCostOdometry::EstimatePropagatorCostOdometry(
    SparseOptimizer* graph)
    : EstimatePropagatorCost(graph) {}
 
double EstimatePropagatorCostOdometry::operator()(
    OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_,
    OptimizableGraph::Vertex* to_) const {
  OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
  OptimizableGraph::Vertex* from =
      dynamic_cast<OptimizableGraph::Vertex*>(*from_.begin());
  OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
  if (std::abs(from->id() - to->id()) !=
      1)  // simple method to identify odometry edges in a pose graph
    return std::numeric_limits<double>::max();
  SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
  if (it == _graph->activeEdges().end())  // it has to be an active edge
    return std::numeric_limits<double>::max();
  return e->initialEstimatePossible(from_, to);
}
 
}  // namespace g2o