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
// 
// g2o is free software: you can redistribute it and/or modify
// it under the terms of the GNU Lesser General Public License as published
// by the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
// 
// g2o is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU Lesser General Public License for more details.
// 
// You should have received a copy of the GNU Lesser General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.

#ifndef _AIS_OPTIMIZABLE_GRAPH_HH_
#define _AIS_OPTIMIZABLE_GRAPH_HH_

#include <deque>
#include <set>
#include <iostream>
#include <list>
#include <limits>
#include <cmath>

#include "openmp_mutex.h"
#include "hyper_graph.h"

namespace g2o {

  class HyperGraphAction;
  struct SolverProperty;

  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;

    struct Data : public HyperGraph::HyperGraphElement
    {
      virtual ~Data() {};
      virtual bool read(std::istream& is) = 0;
      virtual bool write(std::ostream& os) const = 0;
    };

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

        virtual ~Vertex();

        virtual void setToOrigin() = 0;

        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;

        virtual bool setEstimateData(const double* estimate);

        virtual bool getEstimateData(double* estimate) const ;

        virtual int estimateDimension() const;

        
        virtual bool setMinimalEstimateData(const double* estimate);

        virtual bool getMinimalEstimateData(double* estimate) const ;

        virtual int minimalEstimateDimension() const;

        virtual void push() = 0;

        virtual void pop() = 0;

        virtual void discardTop() = 0;

        virtual int stackSize() const = 0;

        virtual void oplus(double* v) = 0;

        int tempIndex() const { return _tempIndex;}
        void setTempIndex(int ti) { _tempIndex = 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;}

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

        void setColInHessian(int c) { _colInHessian = c;}
        int colInHessian() const {return _colInHessian;}

        const OptimizableGraph* graph() const {return _graph;}

        virtual void setUncertainty(double* c) = 0;
        virtual double* uncertaintyData() = 0;

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

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

      protected:
        OptimizableGraph* _graph;
        Data* _userData;
        int _tempIndex;
        bool _fixed;
        bool _marginalized;
        int _dimension;
        int _colInHessian;
        OpenMPMutex _quadraticFormMutex;

    };
    
    class Edge: public HyperGraph::Edge {
      private:
        friend struct OptimizableGraph;
      public:
        Edge();
        virtual Edge* clone() const;

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

        virtual void robustifyError() = 0;

        bool robustKernel() const { return _robustKernel;}
        void setRobustKernel(bool rk) { _robustKernel = rk;}

        double huberWidth() const { return _huberWidth;}
        void setHuberWidth(double hw) { _huberWidth = hw;}

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

      protected:
        int _dimension;
        int _level;
        bool _robustKernel;
        double _huberWidth;
        long long _internalId;

        inline double sqrtOfHuberByNrm(double delta, double b) const
        {
          if (delta<b)
            return 1;
          return sqrt(2*b*fabs(delta) - b*b)/delta;
        }

    };

    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(OptimizableGraph::Vertex* v, Data* userData=0);

    virtual bool addEdge(OptimizableGraph::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 SolverProperty& solverProperty, const std::set<int>& vertDims = std::set<int>()) const;

  protected:
      std::list<Vertex*> 
        _taintedList, 
        _partialList; 
      OptimizableGraph* _upperGraph, *_lowerGraph;
      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;
  };
  
} // end namespace

#endif