sparse_optimizer.h

#pragma once
#include "macros.h"
 
#include "optimizable_graph.h"
 
namespace NDEVR {
 
    // forward declaration
    class ActivePathCostFunction;
    class OptimizationAlgorithm;

    class  SparseOptimizer : public OptimizableGraph
    {
    public:
        enum
        {
            AT_COMPUTEACTIVERROR = OptimizableGraph::AT_NUM_ELEMENTS,
            AT_NUM_ELEMENTS, // keep as last element
        };
 
        friend class ActivePathCostFunction;

        bool isReadyToUpdate() const
        {
            return _ivMap.size() != 0;
        }
 
 

        bool* forceStopFlag() const { return _forceStopFlag; };

        bool terminate() { return _forceStopFlag ? (*_forceStopFlag) : false; }

        const VertexContainer& indexMapping() const { return _ivMap; }
        const VertexContainer& activeVertices() const { return _activeVertices; }
        const EdgeContainer& activeEdges() const { return _activeEdges; }
 

        G2O_ATTRIBUTE_DEPRECATED(void linearizeSystem())
        {
            // nothing needed, linearization is now done inside the solver
        }

        void update(const g_type* update)
        {
            // update the graph by calling oplus on the vertices
            for (uint04 i = 0; i < _ivMap.size(); ++i)
            {
                OGVertex* v = _ivMap[i];
                v->oplus(update);
                update += v->dimension();
            }
        }
        SparseOptimizer()
            : _forceStopFlag(0)
        {
            //_graphActions.resize(AT_NUM_ELEMENTS);
        }

        void computeActiveErrors()
        {
            // call the callbacks in case there is something registered
            for (OptimizableGraph::OGEdge* e : _activeEdges)
                e->computeError();
        }

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

        g_type activeRobustChi2() const
        {
            Eigen::Vector3<g_type> rho;
            g_type chi = 0.0;
            for (auto& e : _activeEdges)
            {
                if (e->robustKernel())
                {
                    e->robustKernel()->robustify(e->chi2(), rho);
                    chi += rho[0];
                }
                else
                {
                    chi += e->chi2();
                }
            }
            return chi;
        }

        OGVertex* findGauge() {
            if (vertices().empty())
                return 0;
 
            sint04 maxDim = 0;
            for (auto it = vertices().begin(); it != vertices().end(); ++it) {
                OGVertex* v = static_cast<OGVertex*>(it->second);
                maxDim = getMax(maxDim, v->dimension());
            }
 
            OGVertex* rut = 0;
            for (auto it = vertices().begin(); it != vertices().end(); ++it)
            {
                OGVertex* v = static_cast<OGVertex*>(it->second);
                if (v->dimension() == maxDim)
                {
                    rut = v;
                    break;
                }
            }
            return rut;
        }

        bool gaugeFreedom()
        {
            if (vertices().empty())
                return false;
 
            sint04 maxDim = 0;
            for (auto it = vertices().begin(); it != vertices().end(); ++it) {
                OGVertex* v = static_cast<OGVertex*>(it->second);
                maxDim = getMax(maxDim, v->dimension());
            }
 
            for (auto it = vertices().begin(); it != vertices().end(); ++it) {
                OGVertex* v = static_cast<OGVertex*>(it->second);
                if (v->dimension() == maxDim) {
                    // test for fixed vertex
                    if (v->fixed()) {
                        return false;
                    }
                    // test for full dimension prior
                    for (auto eit = v->edges().begin(); eit != v->edges().end(); ++eit) 
                    {
                        OptimizableGraph::OGEdge* e = static_cast<OptimizableGraph::OGEdge*>(*eit);
                        if (e->vertexCount() == 1 && e->dimension() == maxDim)
                            return false;
                    }
                }
            }
            return true;
        }

        bool buildIndexMapping(SparseOptimizer::VertexContainer& vlist) {
            if (!vlist.size()) {
                _ivMap.clear();
                return false;
            }
 
            _ivMap.resize(vlist.size());
            uint04 i = 0;
            for (int k = 0; k < 2; k++)
                for (auto it = vlist.begin(); it != vlist.end(); ++it) {
                    OGVertex* v = *it;
                    if (!v->fixed()) {
                        if (static_cast<int>(v->marginalized()) == k) {
                            v->setHessianIndex(i);
                            _ivMap[i] = v;
                            i++;
                        }
                    }
                    else {
                        v->setHessianIndex(-1);
                    }
                }
            _ivMap.resize(i);
            return true;
        }

        void clearIndexMapping() {
            for (uint04 i = 0; i < _ivMap.size(); ++i) {
                _ivMap[i]->setHessianIndex(-1);
                _ivMap[i] = 0;
            }
        }


        bool initializeOptimization(int level = 0)
        {
            if (edges().size() == 0) 
            {
                lib_assert(false, ": Attempt to initialize an empty graph");
                return false;
            }
            auto& vset = vertices();
            _jacobianWorkspace.allocate();
            clearIndexMapping();
            _activeVertices.clear();
            _activeVertices.ensureCapacity(cast<uint04>(vset.size()));
            _activeEdges.clear();
            Set<OGEdge*> auxEdgeSet; // temporary structure to avoid duplicates
            for (const auto& vset_it : vset) 
            {
                OGVertex* v = (OGVertex*)vset_it.second;
                const Buffer<HyperGraph::HGEdge*>& vEdges = v->edges();
                // count if there are edges in that level. If not remove from the pool
                int levelEdges = 0;
                for (auto it = vEdges.begin(); it != vEdges.end(); ++it)
                {
                    OptimizableGraph::OGEdge* e = cast<OptimizableGraph::OGEdge*>(*it);
                    if (level < 0 || e->level() == level) 
                    {
                        bool allVerticesOK = e->vertexCount() > 0;
                        for (uint04 i = 0; i < e->vertexCount(); i++)
                        {
                            if (!vset.hasKey(e->vertex(i)->id()))
                            {
                                allVerticesOK = false;
                                break;
                            }
                        }
                        if (allVerticesOK && !e->allVerticesFixed()) 
                        {
                            auxEdgeSet.insert(e);
                            levelEdges++;
                        }
 
                    }
                }
                if (levelEdges) 
                {
                    _activeVertices.add(v);
                }
            }
 
            _activeEdges.ensureCapacity(auxEdgeSet.size());
            for (auto it = auxEdgeSet.begin(); it != auxEdgeSet.end(); ++it)
                _activeEdges.add(*it);
 
            sortVectorContainers();
            return buildIndexMapping(_activeVertices);
        }

        bool initializeOptimization(Set<HyperGraph::HGVertex*>& eset)
        {
            _jacobianWorkspace.allocate();
            clearIndexMapping();
            _activeVertices.clear();
            _activeEdges.clear();
            _activeEdges.ensureCapacity(cast<uint04>(eset.size()));
            Set<OGVertex*> auxVertexSet; // temporary structure to avoid duplicates
            for (auto it = eset.begin(); it != eset.end(); ++it)
            {
                OptimizableGraph::OGEdge* e = (OptimizableGraph::OGEdge*)(*it);
                for (uint04 i = 0; i < e->vertexCount(); i++)
                    auxVertexSet.insert(cast<OGVertex*>(e->vertex(i)));
                _activeEdges.add(reinterpret_cast<OptimizableGraph::OGEdge*>(*it));
            }
 
            _activeVertices.ensureCapacity(cast<uint04>(auxVertexSet.size()));
            for (auto it = auxVertexSet.begin(); it != auxVertexSet.end(); ++it)
                _activeVertices.add(*it);
 
            sortVectorContainers();
            return buildIndexMapping(_activeVertices);
        }
 
        /*void setToOrigin()
        {
            for (VertexIDMap::iterator it = vertices().begin(); it != vertices().end(); ++it) {
                OGVertex* v = static_cast<OGVertex*>(it->second);
                v->setToOrigin();
            }
        }*/
        void clear() final override
        {
            _ivMap.clear();
            _activeVertices.clear();
            _activeEdges.clear();
            OptimizableGraph::clear();
        }

        OGVertex* const* findActiveVertex(const OGVertex* v) const
        {
            auto lower = std::lower_bound(_activeVertices.begin(), _activeVertices.end(), v, VertexIDCompare());
            if (lower == _activeVertices.end())
                return _activeVertices.end();
            if ((*lower) == v)
                return lower;
            return _activeVertices.end();
        }

        OptimizableGraph::OGEdge* const* findActiveEdge(const OptimizableGraph::OGEdge* e) const
        {
            auto lower = std::lower_bound(_activeEdges.begin(), _activeEdges.end(), e, EdgeIDCompare());
            if (lower == _activeEdges.end())
                return _activeEdges.end();
            if ((*lower) == e)
                return lower;
            return _activeEdges.end();
        }
 
        void push(SparseOptimizer::VertexContainer& vlist)
        {
            for (auto it = vlist.begin(); it != vlist.end(); ++it)
                (*it)->push();
        }
 
        void pop(SparseOptimizer::VertexContainer& vlist)
        {
            for (auto it = vlist.begin(); it != vlist.end(); ++it)
                (*it)->pop();
        }
 
        void push(Set<HyperGraph::HGVertex*>& vlist) final override
        {
            for (auto it = vlist.begin(); it != vlist.end(); ++it) 
            {
                OGVertex* v = dynamic_cast<OGVertex*>(*it);
                if (v)
                    v->push();
            }
        }
 
        void pop(Set<HyperGraph::HGVertex*>& vlist) final override
        {
            for (auto it = vlist.begin(); it != vlist.end(); ++it) 
            {
                OGVertex* v = dynamic_cast<OGVertex*> (*it);
                if (v)
                    v->pop();
                //else 
                    //std::cerr << __FUNCTION__ << ": FATAL POP SET" << std::endl;
            }
        }
 
        void discardTop(SparseOptimizer::VertexContainer& vlist)
        {
            for (auto it = vlist.begin(); it != vlist.end(); ++it)
                (*it)->discardTop();
        }
 

        void setForceStopFlag(bool* flag)
        {
            _forceStopFlag = flag;
        }

        /*bool removeVertex(HyperGraph::HGVertex* v) final override
        {
            OGVertex* vv = static_cast<OGVertex*>(v);
            if (vv->hessianIndex() >= 0) {
                clearIndexMapping();
                _ivMap.clear();
            }
            return HyperGraph::removeVertex(v);
        }*/
 
 
 
        void push() final override
        {
            push(_activeVertices);
        }
 
        void pop() final override
        {
            pop(_activeVertices);
        }
 
        void discardTop() final override
        {
            discardTop(_activeVertices);
        }
 
    protected:
        bool* _forceStopFlag;       
 
        VertexContainer _ivMap;                             
        VertexContainer _activeVertices;                    
        EdgeContainer _activeEdges;                         

        void sortVectorContainers()
        {
            // sort vector structures to get deterministic ordering based on IDs
            std::sort(_activeVertices.begin(), _activeVertices.end(), VertexIDCompare());
            std::sort(_activeEdges.begin(), _activeEdges.end(), EdgeIDCompare());
        }

    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
}