optimizable_graph.h

Go to the documentation of this file.

// g2o - General Graph Optimization
// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, 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_AIS_OPTIMIZABLE_GRAPH_HH_
#define G2O_AIS_OPTIMIZABLE_GRAPH_HH_
 
#include <functional>
#include <iostream>
#include <set>
#include <typeinfo>
 
#include "g2o/stuff/macros.h"
#include "g2o_core_api.h"
#include "hyper_graph.h"
#include "io_helper.h"
#include "jacobian_workspace.h"
#include "openmp_mutex.h"
#include "parameter.h"
#include "parameter_container.h"
 
namespace g2o {
 
class HyperGraphAction;
struct OptimizationAlgorithmProperty;
class CacheContainer;
class RobustKernel;

struct G2O_CORE_API OptimizableGraph : public HyperGraph {
  enum ActionType {
    AT_PREITERATION,
    AT_POSTITERATION,
    AT_NUM_ELEMENTS,  // keep as last element
  };
 
  typedef std::set<HyperGraphAction*> HyperGraphActionSet;
 
  // forward declarations
  class G2O_CORE_API Vertex;
  class G2O_CORE_API Edge;

  struct G2O_CORE_API VertexIDCompare {
    bool operator()(const Vertex* v1, const Vertex* v2) const {
      return v1->id() < v2->id();
    }
  };

  struct G2O_CORE_API EdgeIDCompare {
    bool operator()(const Edge* e1, const Edge* e2) const {
      return e1->internalId() < e2->internalId();
    }
    bool operator()(const HyperGraph::Edge* e1,
                    const HyperGraph::Edge* e2) const {
      return operator()(static_cast<const Edge*>(e1),
                        static_cast<const Edge*>(e2));
    }
  };

  typedef std::vector<OptimizableGraph::Vertex*> VertexContainer;
  typedef std::vector<OptimizableGraph::Edge*> EdgeContainer;

  class G2O_CORE_API Vertex : public HyperGraph::Vertex,
                              public HyperGraph::DataContainer {
   private:
    friend struct OptimizableGraph;
 
   public:
    Vertex();
    virtual ~Vertex();

    void setToOrigin() {
      setToOriginImpl();
      updateCache();
    }

    virtual const double& hessian(int i, int j) const = 0;
    virtual double& hessian(int i, int j) = 0;
    virtual double hessianDeterminant() const = 0;
    virtual double* hessianData() = 0;

    virtual void mapHessianMemory(double* d) = 0;

    virtual int copyB(double* b_) const = 0;

    virtual const double& b(int i) const = 0;
    virtual double& b(int i) = 0;
    virtual double* bData() = 0;

    virtual void clearQuadraticForm() = 0;

    virtual double solveDirect(double lambda = 0) = 0;

    bool setEstimateData(const double* estimate);

    bool setEstimateData(const std::vector<double>& estimate) {
      int dim = estimateDimension();
      if ((dim == -1) || (estimate.size() != std::size_t(dim))) return false;
      return setEstimateData(&estimate[0]);
    };

    template <typename Derived>
    bool setEstimateData(const Eigen::MatrixBase<Derived>& estimate) {
      int dim = estimateDimension();
      if ((dim == -1) || (estimate.size() != dim)) return false;
      return setEstimateData(estimate.derived().data());
    };

    virtual bool getEstimateData(double* estimate) const;

    virtual bool getEstimateData(std::vector<double>& estimate) const {
      int dim = estimateDimension();
      if (dim < 0) return false;
      estimate.resize(dim);
      return getEstimateData(&estimate[0]);
    };

    template <typename Derived>
    bool getEstimateData(Eigen::MatrixBase<Derived>& estimate) const {
      int dim = estimateDimension();
      // If dim is -ve, getEstimateData is not implemented and fails
      if (dim < 0) return false;
 
      // If the vector isn't the right size to store the estimate, try to resize
      // it. This only works if the vector is dynamic. If it is static, fail.
      if (estimate.size() != dim) {
        if ((estimate.RowsAtCompileTime == Eigen::Dynamic) ||
            (estimate.ColsAtCompileTime == Eigen::Dynamic))
          estimate.derived().resize(dim);
        else
          return false;
      }
      return getEstimateData(estimate.derived().data());
    };

    virtual int estimateDimension() const;

    bool setMinimalEstimateData(const double* estimate);

    bool setMinimalEstimateData(const std::vector<double>& estimate) {
      int dim = minimalEstimateDimension();
      if ((dim == -1) || (estimate.size() != std::size_t(dim))) return false;
      return setMinimalEstimateData(&estimate[0]);
    };

    template <typename Derived>
    bool setMinimalEstimateData(const Eigen::MatrixBase<Derived>& estimate) {
      int dim = minimalEstimateDimension();
      if ((dim == -1) || (estimate.size() != dim)) return false;
      return setMinimalEstimateData(estimate.derived().data());
    };

    virtual bool getMinimalEstimateData(double* estimate) const;

    virtual bool getMinimalEstimateData(std::vector<double>& estimate) const {
      int dim = minimalEstimateDimension();
      if (dim < 0) return false;
      estimate.resize(dim);
      return getMinimalEstimateData(&estimate[0]);
    };

    template <typename Derived>
    bool getMinimalEstimateData(Eigen::MatrixBase<Derived>& estimate) const {
      int dim = minimalEstimateDimension();
      // If dim is -ve, getMinimalEstimateData is not implemented and fails
      if (dim < 0) return false;
 
      // If the vector isn't the right size to store the estimate, try to resize
      // it. This only works if the vector is dynamic. If it is static, fail.
      if (estimate.size() != dim) {
        if ((estimate.RowsAtCompileTime == Eigen::Dynamic) ||
            (estimate.ColsAtCompileTime == Eigen::Dynamic))
          estimate.derived().resize(dim);
        else
          return false;
      }
      return getMinimalEstimateData(estimate.derived().data());
    };

    virtual int minimalEstimateDimension() const;

    virtual void push() = 0;

    virtual void pop() = 0;

    virtual void discardTop() = 0;

    virtual int stackSize() const = 0;

    void oplus(const double* v) {
      oplusImpl(v);
      updateCache();
    }

    int hessianIndex() const { return _hessianIndex; }
    int G2O_ATTRIBUTE_DEPRECATED(tempIndex() const) { return hessianIndex(); }
    void setHessianIndex(int ti) { _hessianIndex = ti; }
    void G2O_ATTRIBUTE_DEPRECATED(setTempIndex(int ti)) { setHessianIndex(ti); }

    bool fixed() const { return _fixed; }
    void setFixed(bool fixed) { _fixed = fixed; }

    bool marginalized() const { return _marginalized; }
    void setMarginalized(bool marginalized) { _marginalized = marginalized; }

    int dimension() const { return _dimension; }

    virtual void setId(int id) { _id = id; }

    void setColInHessian(int c) { _colInHessian = c; }
    int colInHessian() const { return _colInHessian; }
 
    const OptimizableGraph* graph() const { return _graph; }
    OptimizableGraph* graph() { return _graph; }

    void lockQuadraticForm() { _quadraticFormMutex.lock(); }
    void unlockQuadraticForm() { _quadraticFormMutex.unlock(); }

    virtual bool read(std::istream& is) = 0;
    virtual bool write(std::ostream& os) const = 0;
 
    virtual void updateCache();
 
    CacheContainer* cacheContainer();
 
   protected:
    OptimizableGraph* _graph;
    Data* _userData;
    int _hessianIndex;
    bool _fixed;
    bool _marginalized;
    int _dimension;
    int _colInHessian;
    OpenMPMutex _quadraticFormMutex;
 
    CacheContainer* _cacheContainer;

    virtual void oplusImpl(const double* v) = 0;

    virtual void setToOriginImpl() = 0;

    virtual bool setEstimateDataImpl(const double*) { return false; }

    virtual bool setMinimalEstimateDataImpl(const double*) { return false; }
  };
 
  class G2O_CORE_API Edge : public HyperGraph::Edge,
                            public HyperGraph::DataContainer {
   private:
    friend struct OptimizableGraph;
 
   public:
    Edge();
    virtual ~Edge();
 
    // indicates if all vertices are fixed
    virtual bool allVerticesFixed() const = 0;
 
    // computes the error of the edge and stores it in an internal structure
    virtual void computeError() = 0;

    virtual bool setMeasurementData(const double* m);

    virtual bool getMeasurementData(double* m) const;

    virtual int measurementDimension() const;

    virtual bool setMeasurementFromState();

    RobustKernel* robustKernel() const { return _robustKernel; }
    void setRobustKernel(RobustKernel* ptr);

    virtual const double* errorData() const = 0;
    virtual double* errorData() = 0;

    virtual const double* informationData() const = 0;
    virtual double* informationData() = 0;

    virtual double chi2() const = 0;

    virtual void constructQuadraticForm() = 0;

    virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor) = 0;

    virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace) = 0;

    virtual void initialEstimate(const OptimizableGraph::VertexSet& from,
                                 OptimizableGraph::Vertex* to) = 0;

    virtual double initialEstimatePossible(
        const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) {
      (void)from;
      (void)to;
      return -1.;
    }

    int level() const { return _level; }
    void setLevel(int l) { _level = l; }

    int dimension() const { return _dimension; }
 
    G2O_ATTRIBUTE_DEPRECATED(virtual Vertex* createFrom()) { return nullptr; }
    G2O_ATTRIBUTE_DEPRECATED(virtual Vertex* createTo()) { return nullptr; }
    virtual Vertex* createVertex(int) { return nullptr; }

    virtual bool read(std::istream& is) = 0;
    virtual bool write(std::ostream& os) const = 0;

    long long internalId() const { return _internalId; }
 
    OptimizableGraph* graph();
    const OptimizableGraph* graph() const;
 
    bool setParameterId(int argNum, int paramId);
    inline const Parameter* parameter(int argNo) const {
      return *_parameters.at(argNo);
    }
    inline size_t numParameters() const { return _parameters.size(); }
    inline void resizeParameters(size_t newSize) {
      _parameters.resize(newSize, 0);
      _parameterIds.resize(newSize, -1);
      _parameterTypes.resize(newSize, typeid(void*).name());
    }
 
   protected:
    int _dimension;
    int _level;
    RobustKernel* _robustKernel;
    long long _internalId;
    std::vector<int> _cacheIds;
 
    template <typename ParameterType>
    bool installParameter(ParameterType*& p, size_t argNo, int paramId = -1) {
      if (argNo >= _parameters.size()) return false;
      _parameterIds[argNo] = paramId;
      _parameters[argNo] = (Parameter**)&p;
      _parameterTypes[argNo] = typeid(ParameterType).name();
      return true;
    }
 
    template <typename CacheType>
    void resolveCache(CacheType*& cache, OptimizableGraph::Vertex*,
                      const std::string& _type,
                      const ParameterVector& parameters);
 
    bool resolveParameters();
    virtual bool resolveCaches();
 
    std::vector<std::string> _parameterTypes;
    std::vector<Parameter**> _parameters;
    std::vector<int> _parameterIds;
  };

  inline Vertex* vertex(int id) {
    return reinterpret_cast<Vertex*>(HyperGraph::vertex(id));
  }

  inline const Vertex* vertex(int id) const {
    return reinterpret_cast<const Vertex*>(HyperGraph::vertex(id));
  }

  OptimizableGraph();
  virtual ~OptimizableGraph();

  virtual bool addVertex(HyperGraph::Vertex* v, Data* userData);
  virtual bool addVertex(HyperGraph::Vertex* v) { return addVertex(v, 0); }
  bool addVertex(OptimizableGraph::Vertex* v, Data* userData);
  bool addVertex(OptimizableGraph::Vertex* v) { return addVertex(v, 0); }

  virtual bool addEdge(HyperGraph::Edge* e);
  bool addEdge(OptimizableGraph::Edge* e);

  virtual bool setEdgeVertex(HyperGraph::Edge* e, int pos,
                             HyperGraph::Vertex* v);

  double chi2() const;

  int maxDimension() const;

  void recomputeJacobianWorkspaceSize() {
    _jacobianWorkspace.updateSize(*this, true);
  }

  std::set<int> dimensions() const;

  virtual int optimize(int iterations, bool online = false);

  virtual void preIteration(int);
  virtual void postIteration(int);

  bool addPreIterationAction(HyperGraphAction* action);
  bool addPostIterationAction(HyperGraphAction* action);

  bool removePreIterationAction(HyperGraphAction* action);
  bool removePostIterationAction(HyperGraphAction* action);

  virtual void push();
  virtual void pop();
  virtual void discardTop();

  virtual bool load(std::istream& is);
  bool load(const char* filename);
  virtual bool save(std::ostream& os, int level = 0) const;
  bool save(const char* filename, int level = 0) const;

  bool saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, int level = 0);

  bool saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset);

  virtual void push(HyperGraph::VertexSet& vset);
  virtual void pop(HyperGraph::VertexSet& vset);
  virtual void discardTop(HyperGraph::VertexSet& vset);

  virtual void setFixed(HyperGraph::VertexSet& vset, bool fixed);

  void setRenamedTypesFromString(const std::string& types);

  bool isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty,
                        const std::set<int>& vertDims = std::set<int>()) const;

  virtual void clearParameters();
 
  bool addParameter(Parameter* p) { return _parameters.addParameter(p); }
 
  Parameter* parameter(int id) { return _parameters.getParameter(id); }

  bool verifyInformationMatrices(bool verbose = false) const;
 
  // helper functions to save an individual vertex
  bool saveVertex(std::ostream& os, Vertex* v) const;
 
  // helper function to save an individual parameter
  bool saveParameter(std::ostream& os, Parameter* v) const;
 
  // helper functions to save an individual edge
  bool saveEdge(std::ostream& os, Edge* e) const;
 
  // helper functions to save the data packets
  bool saveUserData(std::ostream& os, HyperGraph::Data* v) const;

  JacobianWorkspace& jacobianWorkspace() { return _jacobianWorkspace; }
  const JacobianWorkspace& jacobianWorkspace() const {
    return _jacobianWorkspace;
  }

  static bool initMultiThreading();
 
  inline ParameterContainer& parameters() { return _parameters; }
  inline const ParameterContainer& parameters() const { return _parameters; }

  void forEachVertex(std::function<void(OptimizableGraph::Vertex*)> fn);
  void forEachVertex(HyperGraph::VertexSet& vset,
                     std::function<void(OptimizableGraph::Vertex*)> fn);
 
 protected:
  std::map<std::string, std::string> _renamedTypesLookup;
  long long _nextEdgeId;
  std::vector<HyperGraphActionSet> _graphActions;
 
  ParameterContainer _parameters;
  JacobianWorkspace _jacobianWorkspace;
 
  void performActions(int iter, HyperGraphActionSet& actions);
};

 
}  // namespace g2o
 
#endif