base_edge.h

#pragma once
#include <limits>
#include <Eigen/Core>
#include "optimizable_graph.h"
 
namespace NDEVR
{
    using namespace Eigen;
    template <sint04 t_dims, typename E>
    class BaseEdge : public OptimizableGraph::OGEdge
    {
    public:
        static constexpr sint04 Dimension = t_dims; 
        typedef E Measurement;                  
        typedef Eigen::Matrix<g_type, t_dims, 1> ErrorVector;           
        typedef Eigen::Matrix<g_type, t_dims, t_dims> InformationType;  

        BaseEdge()
            : OptimizableGraph::OGEdge()
        {}

        virtual sint04 dimension() const final override
        {
            return t_dims;
        }

        g_type chi2() const final override
        {
            return _error.dot(information() * _error);
        }

        const ErrorVector& error() const { return _error; }
        //ErrorVector& error() { return _error; }


        g_type scalerInformation() const { return _information.data()[0]; }
        const InformationType& information() const { return _information; }
        void setInformation(const InformationType& information) { _information = information; }
        void setInformation(g_type information) { _information = InformationType::Identity() * information; }

        const g_type* informationData() const final override { return _information.data(); }
        g_type* informationData() final override { return _information.data(); }

        const Measurement& measurement() const { return _measurement; }
        void setMeasurement(const Measurement& m) { _measurement = m; }

        int rank() const { return t_dims; }
 
    protected:
        InformationType _information;   
        Measurement _measurement;       
        ErrorVector _error;             
        //bool m_scaler_info = false;
        InformationType robustInformation(const Eigen::Vector3<g_type>& rho) const
        {
            InformationType result = rho[1] * _information;
            //ErrorVector weightedErrror = _information * _error;
            //result.noalias() += 2 * rho[2] * (weightedErrror * weightedErrror.transpose());
            return result;
        }
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
}