base_vertex.h

#pragma once
#include "optimizable_graph.h"
 
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Cholesky>
#include <Eigen/StdVector>
namespace NDEVR
{
    template <sint04 t_dims, typename T>
    class BaseVertex : public OptimizableGraph::OGVertex
    {
    public:
        static constexpr sint04 Dimension = t_dims;
        typedef T EstimateType;
        typedef Buffer<EstimateType> BackupStackType;
 
        typedef Eigen::Map<Eigen::Matrix<g_type, t_dims, t_dims>, Eigen::Matrix<g_type, t_dims, t_dims>::Flags& Eigen::PacketAccessBit
            ? Eigen::Aligned
            : Eigen::Unaligned> HessianBlockType;
 
    public:
        static_assert(t_dims > 0, "Bad vertex");
        BaseVertex()
            : OptimizableGraph::OGVertex()
            , _hessian(0, t_dims, t_dims)
        {
            _b.setZero();
        }

        sint04 dimension() const final override { return t_dims; }
        g_type solveDirect(g_type lambda = 0.0) final override
        {
            Eigen::Matrix<g_type, t_dims, t_dims> tempA = _hessian 
                + Eigen::Matrix<g_type, t_dims, t_dims>::Identity()* lambda;
            g_type det = tempA.determinant();
            if (IsInvalid(det) || det < std::numeric_limits<g_type>::epsilon())
                return det;
            Eigen::Matrix <g_type, t_dims, 1> dx = tempA.llt().solve(_b);
            oplus(&dx[0]);
            return det;
        }

        void mapHessianMemory(g_type* d) final override
        {
            new (&_hessian) HessianBlockType(d, t_dims, t_dims);
        }
 

        const g_type& hessian(int i, int j) const final override { assert(i < t_dims && j < t_dims); return _hessian(i, j); }
        g_type& hessian(int i, int j)  final override { assert(i < t_dims && j < t_dims); return _hessian(i, j); }
        g_type hessianDeterminant() const final override { return _hessian.determinant(); }
        g_type* hessianData() final override { return const_cast<g_type*>(_hessian.data()); }

        sint04 copyB(g_type* b_) const final override
        {
            memcpy(b_, _b.data(), t_dims * sizeof(g_type));
            return t_dims;
        }

        const g_type& b(int i) const final override { assert(i < t_dims); return _b(i); }
        g_type& b(int i) final override { assert(i < t_dims); return _b(i); }
        g_type* bData() final override { return _b.data(); }

        void clearQuadraticForm() final override {
            _b.setZero();
        }
 

        Eigen::Matrix<g_type, t_dims, 1>& b() { return _b; }
        const Eigen::Matrix<g_type, t_dims, 1>& b() const { return _b; }
        HessianBlockType& A() { return _hessian; }
        const HessianBlockType& A() const { return _hessian; }

        virtual void push() final override { _backup.add(_estimate); }
        virtual void pop() final override { _estimate = _backup.last(); _backup.removeLast(); }
        virtual void discardTop() final override { _backup.removeLast(); }
        virtual int stackSize() const final override { return _backup.size(); }

        const EstimateType& estimate() const { return _estimate; }
        void setEstimate(const EstimateType& et) { _estimate = et; }
 
    protected:
        HessianBlockType _hessian;              
        Eigen::Matrix<g_type, t_dims, 1> _b;    
        EstimateType _estimate;                 
        BackupStackType _backup;                
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
}