optimizable_graph.h

#pragma once
#include <NDEVR/Set.h>
#include <limits>
#include <cmath>
#include <typeinfo>
 
#include "openmp_mutex.h"
#include "hyper_graph.h"
#include "jacobian_workspace.h"
 
 
#include "optimization_algorithm_property.h"
#include "robust_kernel.h"
 
#include <Eigen/Dense>
 
 
#include "macros.h"
#ifdef GRAPH_PARALLEL
#include <mutex>
#endif
namespace NDEVR 
{
 
    class HyperGraphAction;
    struct OptimizationAlgorithmProperty;
    class RobustKernel;
    struct OptimizableGraph : public HyperGraph
    {

        enum ActionType {
            AT_PREITERATION,    
            AT_POSTITERATION,   
            AT_NUM_ELEMENTS,    
        };
 
        typedef Set<HyperGraphAction*>  HyperGraphActionSet;
 
        // forward declarations
        class OGVertex;
        class  OGEdge;

        class Data
        {
            friend struct OptimizableGraph;
        public:
            ~Data() { delete _next; };
            Data() {};
            const Data* next() const { return _next; }
            Data* next() { return _next; }
            void setNext(Data* next_) { _next = next_; }
        protected:
            Data* _next = nullptr; // linked list of multiple data;
        };

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

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

        typedef Buffer<OptimizableGraph::OGVertex*>   VertexContainer;
        typedef Buffer<OptimizableGraph::OGEdge*>       EdgeContainer;

        class OGVertex : public HyperGraph::HGVertex {
        private:
            friend struct OptimizableGraph;
        public:
            OGVertex()
                : HyperGraph::HGVertex()
                , _hessianIndex(-1)
                , _fixed(false)
                , _marginalized(false)
                , _colInHessian(-1)
            {}
 
 
            virtual ~OGVertex()
            {}

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

            virtual void mapHessianMemory(g_type* d) = 0;

            virtual sint04 copyB(g_type* b_) const = 0;

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

            virtual void clearQuadraticForm() = 0;

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

            bool setEstimateData(const g_type* estimate)
            {
                return setEstimateDataImpl(estimate);
            }

            bool setEstimateData(const Buffer<g_type>& estimate) {
#ifndef NDEBUG
                int dim = estimateDimension();
                assert((dim == -1) || (estimate.size() == std::size_t(dim)));
#endif
                return setEstimateData(&estimate[0]);
            };

            virtual bool getEstimateData(g_type*) const
            {
                return false;
            }

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

            virtual sint04 estimateDimension() const { return -1; };

            bool setMinimalEstimateData(const g_type* estimate)
            {
                return setMinimalEstimateDataImpl(estimate);
            }

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

            virtual bool getMinimalEstimateData(g_type*) const
            {
                return false;
            }

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

            virtual sint04 minimalEstimateDimension() const { return -1; }

            virtual void push() = 0;

            virtual void pop() = 0;

            virtual void discardTop() = 0;

            virtual int stackSize() const = 0;

            void oplus(const g_type* v)
            {
                oplusImpl(v);
            }

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

            virtual sint04 dimension() const = 0;

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

            void setColInHessian(int c) { _colInHessian = c; }
            int colInHessian() const { return _colInHessian; }
 
 
        protected:
            int _hessianIndex = 0;      
            bool _fixed = false;        
            bool _marginalized = false; 
            int _colInHessian = 0;      
            virtual void oplusImpl(const g_type* v) = 0;

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

            virtual bool setMinimalEstimateDataImpl(const g_type*) { return false; }
 
        };

        class OGEdge : public HyperGraph::HGEdge
        {
        private:
            friend struct OptimizableGraph;
        public:
            OGEdge()
            {}

            virtual bool allVerticesFixed() const = 0;

            virtual void computeError() = 0;
            
            RobustKernel* robustKernel() const { return _robustKernel; }
            void setRobustKernel(RobustKernel& ptr)
            {
                _robustKernel = &ptr;
            }

            void clearRobustKernel()
            {
                _robustKernel = nullptr;
            }

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

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

            virtual void linearizeOplusAndConstructQuadraticForm(JacobianWorkspace& jacobianWorkspace) = 0;

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

            virtual sint04 dimension() const = 0;

            long long internalId() const { return _internalId; }
 
 
 
        protected:
            RobustKernel* _robustKernel = nullptr;  
            int _internalId = 0;                    
            int _level = 0;                         
            
 
        };

        OptimizableGraph()
        {
            _nextEdgeId = 0;
        }
        virtual ~OptimizableGraph()
        {
            clear();
        }

        virtual bool addVertex(HyperGraph::HGVertex& v) final override
        {
            lib_assert(!vertex(v.id()), "Vertex with this ID already contained in the graph");
            lib_assert(dynamic_cast<OptimizableGraph::OGVertex*>(&v), "Vertex does not inherit from OGVertex");
            return HyperGraph::addVertex(v);
        }

        virtual bool addEdge(HyperGraph::HGEdge* e_) final override
        {
            OptimizableGraph::OGEdge* e = static_cast<OptimizableGraph::OGEdge*>(e_);
            lib_assert(e, "Edge does not inherit from OptimizableGraph::OGEdge");
            bool eresult = HyperGraph::addEdge(e);
            if (!eresult)
                return false;
            e->_internalId = _nextEdgeId++;
            _jacobianWorkspace.updateSize(e);
 
            return true;
        }

        g_type chi2() const
        {
            g_type chi = 0.0;
            for (auto it = this->edges().begin(); it != this->edges().end(); ++it) 
            {
                const OptimizableGraph::OGEdge* e = static_cast<const OptimizableGraph::OGEdge*>(*it);
                chi += e->chi2();
            }
            return chi;
        }

        sint04 maxDimension() const
        {
            sint04 maxDim = 0;
            for (auto it = vertices().begin(); it != vertices().end(); ++it) 
            {
                const OptimizableGraph::OGVertex* v = static_cast<const OptimizableGraph::OGVertex*>(it->second);
                maxDim = getMax(maxDim, v->dimension());
            }
            return maxDim;
        }

        Set<sint04> dimensions() const
        {
            Set<int> auxDims;
            for (auto it = vertices().begin(); it != vertices().end(); ++it)
            {
                OGVertex* v = static_cast<OGVertex*>(it->second);
                auxDims.insert(v->dimension());
            }
            return auxDims;
        }
 

        virtual void push()
        {
            for (auto it = _vertices.begin(); it != _vertices.end(); ++it) 
            {
                OGVertex* v = static_cast<OGVertex*>(it->second);
                v->push();
            }
        }

        virtual void pop()
        {
            for (auto it = _vertices.begin(); it != _vertices.end(); ++it)
            {
                OGVertex* v = static_cast<OGVertex*>(it->second);
                v->pop();
            }
        }

        virtual void discardTop()
        {
            for (auto it = _vertices.begin(); it != _vertices.end(); ++it) {
                OGVertex* v = static_cast<OGVertex*>(it->second);
                v->discardTop();
            }
        }

        virtual void push(Set<HyperGraph::HGVertex*>& vset)
        {
            for (auto it = vset.begin(); it != vset.end(); ++it) {
                OGVertex* v = static_cast<OGVertex*>(*it);
                v->push();
            }
        }

        virtual void pop(Set<HyperGraph::HGVertex*>& vset)
        {
            for (auto it = vset.begin(); it != vset.end(); ++it) {
                OGVertex* v = static_cast<OGVertex*>(*it);
                v->pop();
            }
        }
        

        virtual void setFixed(Set<HyperGraph::HGVertex*>& vset, bool fixed)
        {
            for (auto it = vset.begin(); it != vset.end(); ++it) 
            {
                OGVertex* v = static_cast<OGVertex*>(*it);
                v->setFixed(fixed);
            }
        }

        bool isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const Set<sint04>& vertDims_ = Set<int>()) const
        {
            Set<sint04> auxDims;
            if (vertDims_.size() == 0)
                auxDims = dimensions();
            const Set<sint04>& vertDims = vertDims_.size() == 0 ? auxDims : vertDims_;
            bool suitableSolver = true;
            if (vertDims.size() == 2) 
            {
                if (solverProperty.requiresMarginalize)
                    suitableSolver = vertDims.count(solverProperty.poseDim) == 1 && vertDims.count(solverProperty.landmarkDim) == 1;
                else
                    suitableSolver = solverProperty.poseDim == -1;
            }
            else if (vertDims.size() == 1) 
            {
                suitableSolver = vertDims.count(solverProperty.poseDim) == 1 || solverProperty.poseDim == -1;
            }
            else
            {
                suitableSolver = solverProperty.poseDim == -1 && !solverProperty.requiresMarginalize;
            }
            return suitableSolver;
        }

         /*bool verifyInformationMatrices(bool verbose = false) const
         {
             bool allEdgeOk = true;
             SelfAdjointEigenSolver<MatrixXd> eigenSolver;
             for (OptimizableGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
                 OptimizableGraph::OGEdge* e = static_cast<OptimizableGraph::OGEdge*>(*it);
                 Eigen::MatrixXd::MapType information(e->informationData(), e->dimension(), e->dimension());
                 // test on symmetry
                 bool isSymmetric = information.transpose() == information;
                 bool okay = isSymmetric;
                 if (isSymmetric) {
                     // compute the eigenvalues
                     eigenSolver.compute(information, Eigen::EigenvaluesOnly);
                     bool isSPD = eigenSolver.eigenvalues()(0) >= 0.;
                     okay = okay && isSPD;
                 }
                 allEdgeOk = allEdgeOk && okay;
                 if (!okay) {
                     if (verbose) {
                         if (!isSymmetric)
                             std::cerr << "Information Matrix for an edge is not symmetric:";
                         else
                             std::cerr << "Information Matrix for an edge is not SPD:";
                         for (uint04 i = 0; i < e->vertices().size(); ++i)
                             std::cerr << " " << e->vertex(i)->id();
                         if (isSymmetric)
                             std::cerr << "\teigenvalues: " << eigenSolver.eigenvalues().transpose();
                         std::cerr << std::endl;
                     }
                 }
             }
             return allEdgeOk;
         }*/

        JacobianWorkspace& jacobianWorkspace() { return _jacobianWorkspace; }
        const JacobianWorkspace& jacobianWorkspace() const { return _jacobianWorkspace; }
 
    protected:
        JacobianWorkspace _jacobianWorkspace;   
        int _nextEdgeId = 0;                    
 
        
    };
 
}