base_unary_edge.h

#pragma once
#include <cassert>
#include <limits>
 
#include "base_edge.h"
#include "robust_kernel.h"
 
namespace NDEVR
{
 
    using namespace Eigen;

    template <sint04 t_dims, typename E, typename VertexXi>
    class BaseUnaryEdge : public BaseEdge<t_dims, E>
    {
    public:
        typedef typename BaseEdge<t_dims, E>::Measurement Measurement;      
        typedef VertexXi VertexXiType;                                      
        typedef typename Eigen::Matrix<g_type, t_dims, VertexXiType::Dimension>::AlignedMapType JacobianXiOplusType;    
        typedef typename BaseEdge<t_dims, E>::ErrorVector ErrorVector;      
        typedef typename BaseEdge<t_dims, E>::InformationType InformationType;  

        BaseUnaryEdge()
            : BaseEdge<t_dims, E>()
            , _jacobianOplusXi(0, t_dims, VertexXiType::Dimension)
        {}

        virtual uint04 vertexCount() const final override { return 1; };
        virtual const HyperGraph::HGVertex* vertex(uint04) const final override { return m_vertex; };
        virtual HyperGraph::HGVertex* vertex(uint04) final override { return m_vertex; };
        virtual void setVertex(uint04, HyperGraph::HGVertex* v) final override
        { 
            lib_assert(dynamic_cast<VertexXiType*>(v), "wrong type"); 
            m_vertex = cast<VertexXiType*>(v); 
        };

        virtual bool allVerticesFixed() const final override
        {
            return m_vertex->fixed();
        }

        virtual void linearizeOplusAndConstructQuadraticForm(JacobianWorkspace& jacobianWorkspace) final override
        {
            lib_assert(jacobianWorkspace.maxNumVertices() >= 1, "Bad vertex size");
            lib_assert(jacobianWorkspace.maxDimension() >= t_dims * VertexXiType::Dimension, "Bad vertex dimension");
 
            new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), t_dims, VertexXiType::Dimension);
            linearizeOplus();
            //VertexXiType* from = static_cast<VertexXiType*>(_vertices[0]);
            /*auto b = from->b();
            auto H = from->A();
            constructQuadraticForm2();
            auto ba = from->b();
            auto Ha = from->A();
            from->b() = b;
            from->A() = H;*/
            constructQuadraticForm();
            //lib_assert(ba.isApprox(from->b(), 1e-6), "Bad b match");
            //lib_assert(Ha.isApprox(from->A(), 1e-6), "Bad A match");
        }
 

        virtual void linearizeOplus()
        {
            //Xi - estimate the jacobian numerically
            VertexXiType* vi = m_vertex;
 
            if (vi->fixed())
                return;
            constexpr g_type delta = g_type(1e-9);
            constexpr g_type scalar = g_type(1.0) / (2 * delta);
            ErrorVector error1;
            ErrorVector errorBeforeNumeric = _error;
 
            g_type add_vi[VertexXiType::Dimension];
            std::fill(add_vi, add_vi + VertexXiType::Dimension, g_type(0.0));
            // add small step along the unit vector in each dimension
            for (int d = 0; d < VertexXiType::Dimension; ++d)
            {
                vi->push();
                add_vi[d] = delta;
                vi->oplus(add_vi);
                computeError();
                error1 = _error;
                vi->pop();
                vi->push();
                add_vi[d] = -delta;
                vi->oplus(add_vi);
                computeError();
                vi->pop();
                add_vi[d] = 0.0;
                //if (scalar * (error1 - _error).array().isNaN().any())
                    //throw "waht";
                _jacobianOplusXi.col(d) = scalar * (error1 - _error);
            } // end dimension
 
            _error = errorBeforeNumeric;
            //if (!_error.array().isFinite().all())
                //throw "error";
        }

        const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi; }
        void constructQuadraticForm()
        {
            if (m_vertex->fixed())
                return;
            const JacobianXiOplusType& A = jacobianOplusXi(); // Fixed-size matrix
            const InformationType& omega = BaseEdge<t_dims, E>::information(); // Symmetric 3×3
            auto& b = m_vertex->b();
            auto& H = m_vertex->A();
 
            // Cache transpose and error product
            const auto A_trans = A.transpose();
            const auto omega_e = omega.selfadjointView<Eigen::Upper>() * _error;
 
            if (auto* kernel = BaseEdge<t_dims, E>::robustKernel())
            {
                Eigen::Vector3<g_type> rho;
                kernel->robustify(BaseEdge<t_dims, E>::chi2(), rho);
 
                const g_type w = rho[1];
                EIGEN_ALIGN32 InformationType weighted_omega = BaseEdge<t_dims, E>::robustInformation(rho);
 
                //if ((w * A_trans * omega_e).array().isNaN().any())
                    //throw "waht";
                // Fast right-hand side update
                b.noalias() -= w * A_trans * omega_e;
 
                // Hessian update: exploit symmetry
                H.noalias() += A_trans * weighted_omega.selfadjointView<Eigen::Upper>() * A;
                //if (H.array().isNaN().any())
                    //throw "waht";
            }
            else
            {
                b.noalias() -= A_trans * omega_e;
                //if (H.array().isNaN().any())
                    //throw "waht";
                H.noalias() += A_trans * omega.selfadjointView<Eigen::Upper>() * A;
                //if (H.array().isNaN().any())
                    //throw "waht";
            }
        }
        /*void constructQuadraticForm2()
        {
            VertexXiType* from = static_cast<VertexXiType*>(_vertices[0]);
 
            // chain rule to get the Jacobian of the nodes in the manifold domain
            const JacobianXiOplusType& A = jacobianOplusXi();
            const InformationType& omega = BaseEdge<t_dims, E>::information();
            if (!from->fixed())
            {
                if (BaseEdge<t_dims, E>::robustKernel())
                {
                    g_type error = BaseEdge<t_dims, E>::chi2();
                    Eigen::Vector3<g_type> rho;
                    BaseEdge<t_dims, E>::robustKernel()->robustify(error, rho);
                    EIGEN_ALIGN32 InformationType weighted_omega = BaseEdge<t_dims, E>::robustInformation(rho);
                    from->b().noalias() -= rho[1] * A.transpose() * omega * _error;
                    from->A().noalias() += A.transpose() * weighted_omega * A;
                }
                else
                {
                    from->b().noalias() -= A.transpose() * omega * _error;
                    from->A().noalias() += A.transpose() * omega * A;
                }
            }
        }*/
        virtual void mapHessianMemory(g_type*, int, int, bool) final override { lib_assert(false, "BaseUnaryEdge does not map memory of the Hessian"); }
 
        using BaseEdge<t_dims, E>::computeError;
 
    protected:
        VertexXiType* m_vertex = nullptr;   
        using BaseEdge<t_dims, E>::_measurement;
        using BaseEdge<t_dims, E>::_error;
 
        JacobianXiOplusType _jacobianOplusXi;   
 
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
}