estimate_propagator.h

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.

#ifndef G2O_ESTIMATE_PROPAGATOR_H
#define G2O_ESTIMATE_PROPAGATOR_H

#include "optimizable_graph.h"
#include "sparse_optimizer.h"
#include "g2o_core_api.h"

#include <map>
#include <set>
#include <limits>

#include <unordered_map>

namespace g2o {

  class G2O_CORE_API EstimatePropagatorCost {
    public:
      EstimatePropagatorCost (SparseOptimizer* graph);
      virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const;
      virtual const char* name() const { return "spanning tree";}
    protected:
      SparseOptimizer* _graph;
  };

  class G2O_CORE_API EstimatePropagatorCostOdometry : public EstimatePropagatorCost {
    public:
      EstimatePropagatorCostOdometry(SparseOptimizer* graph);
      virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const;
      virtual const char* name() const { return "odometry";}
  };

  class G2O_CORE_API EstimatePropagator {
    public:

      struct PropagateAction {
        virtual void operator()(OptimizableGraph::Edge* e, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) const
        {
          if (! to->fixed())
            e->initialEstimate(from, to);
        }
      };

      typedef EstimatePropagatorCost PropagateCost;

      class AdjacencyMapEntry;

      class PriorityQueue : public std::multimap<double, AdjacencyMapEntry*> {
        public:
          void push(AdjacencyMapEntry* entry);
          AdjacencyMapEntry* pop();
      };

      class AdjacencyMapEntry {
        public:
          friend class EstimatePropagator;
          friend class PriorityQueue;
          AdjacencyMapEntry();
          void reset();
          OptimizableGraph::Vertex* child() const {return _child;}
          const OptimizableGraph::VertexSet& parent() const {return _parent;}
          OptimizableGraph::Edge* edge() const {return _edge;}
          double distance() const {return _distance;}
          int frontierLevel() const { return _frontierLevel;}

        protected:
          OptimizableGraph::Vertex* _child;
          OptimizableGraph::VertexSet _parent;
          OptimizableGraph::Edge* _edge;
          double _distance;
          int _frontierLevel;
        private: // for PriorityQueue
          bool inQueue;
          PriorityQueue::iterator queueIt;
      };

      class VertexIDHashFunction {
        public:
          size_t operator ()(const OptimizableGraph::Vertex* v) const { return v->id();}
      };

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

    public:
      EstimatePropagator(OptimizableGraph* g);
      OptimizableGraph::VertexSet& visited() {return _visited; }
      AdjacencyMap& adjacencyMap() {return _adjacencyMap; }
      OptimizableGraph* graph() {return _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:
      void reset();

      AdjacencyMap _adjacencyMap;
      OptimizableGraph::VertexSet _visited;
      OptimizableGraph* _graph;
  };

}
#endif