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 <set>
#include <iostream>
#include <list>
#include <limits>
#include <cmath>
#include <typeinfo>

#include "openmp_mutex.h"
#include "hyper_graph.h"
#include "parameter.h"
#include "parameter_container.h"
#include "jacobian_workspace.h"

#include "../stuff/macros.h"

namespace g2o {

  class HyperGraphAction;
  struct OptimizationAlgorithmProperty;
  class Cache;
  class CacheContainer;
  class RobustKernel;

  struct  OptimizableGraph : public HyperGraph {

    enum ActionType {
      AT_PREITERATION, AT_POSTITERATION,
      AT_NUM_ELEMENTS, // keep as last element
    };

    typedef std::set<HyperGraphAction*>    HyperGraphActionSet;

    // forward declarations
    class  Vertex;
    class  Edge;

    class  Data : public HyperGraph::HyperGraphElement
    {
      friend struct OptimizableGraph;
      public:
        virtual ~Data();
        Data();
        virtual bool read(std::istream& is) = 0;
        virtual bool write(std::ostream& os) const = 0;
        virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_DATA;}
        const Data* next() const {return _next;}
        Data* next() {return _next;}
        void setNext(Data* next_) { _next = next_; }
      protected:
        Data* _next; // linked list of multiple data;
    };

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

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

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

    class  Vertex : public HyperGraph::Vertex {
      private:
        friend struct OptimizableGraph;
      public:
        Vertex();

        virtual Vertex* clone() const ;

        const Data* userData() const { return _userData; }
        Data* userData() { return _userData; }

        void setUserData(Data* obs) { _userData = obs;}
        void addUserData(Data* obs) { 
          if (obs) {
            obs->setNext(_userData);
            _userData=obs;
          }
        }
        
        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) { 
#ifndef NDEBUG
          int dim = estimateDimension();
          assert((dim == -1) || (estimate.size() == std::size_t(dim)));
#endif
          return setEstimateData(&estimate[0]);
        };

        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]);
        };

        virtual int estimateDimension() const;

        bool setMinimalEstimateData(const double* estimate);

        bool setMinimalEstimateData(const std::vector<double>& estimate) {
#ifndef NDEBUG
          int dim = minimalEstimateDimension();
          assert((dim == -1) || (estimate.size() == std::size_t(dim)));
#endif
          return setMinimalEstimateData(&estimate[0]);
        };

        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]);
        };

        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  Edge: public HyperGraph::Edge {
      private:
        friend struct OptimizableGraph;
      public:
        Edge();
        virtual ~Edge();
        virtual Edge* clone() const;

        // 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;}

        virtual Vertex* createFrom() {return 0;}
        virtual Vertex* createTo()   {return 0;}

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

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

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

    double chi2() const;

    int maxDimension() const;

    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 createEdges=true);
    bool load(const char* filename, bool createEdges=true);
    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 functions to save an individual edge
    bool saveEdge(std::ostream& os, Edge* e) const;
    JacobianWorkspace& jacobianWorkspace() { return _jacobianWorkspace;}
    const JacobianWorkspace& jacobianWorkspace() const { return _jacobianWorkspace;}

    static bool initMultiThreading();

  protected:
    std::map<std::string, std::string> _renamedTypesLookup;
    long long _nextEdgeId;
    std::vector<HyperGraphActionSet> _graphActions;

    // do not watch this. To be removed soon, or integrated in a nice way
    bool _edge_has_id;

    ParameterContainer _parameters;
    JacobianWorkspace _jacobianWorkspace;
  };
  
} // end namespace

#endif