base_binary_edge.h

#pragma once
#include "base_edge.h"
namespace NDEVR
{
 
    using namespace Eigen;

    template <sint04 D, typename E, typename VertexXi, typename VertexXj>
    class BaseBinaryEdge : public BaseEdge<D, E>
    {
    public:
        typedef VertexXi VertexXiType;                  
        typedef VertexXj VertexXjType;                  
 
        static constexpr int Di = VertexXiType::Dimension;  
        static constexpr int Dj = VertexXjType::Dimension;  
 
        typedef typename BaseEdge<D, E>::Measurement Measurement;                   
        typedef typename Eigen::Matrix<g_type, D, Di>::AlignedMapType JacobianXiOplusType;  
        typedef typename Eigen::Matrix<g_type, D, Dj>::AlignedMapType JacobianXjOplusType;  
        typedef typename BaseEdge<D, E>::ErrorVector ErrorVector;               
        typedef typename BaseEdge<D, E>::InformationType InformationType;           
 
        typedef Eigen::Map<Eigen::Matrix<g_type, Di, Dj>
            , Eigen::Matrix<g_type, Di, Dj>::Flags & Eigen::PacketAccessBit
            ? Eigen::Aligned
            : Eigen::Unaligned> HessianBlockType;               
        typedef Eigen::Map<Eigen::Matrix<g_type, Dj, Di>
            , Eigen::Matrix<g_type, Di, Dj>::Flags& Eigen::PacketAccessBit
            ? Eigen::Aligned
            : Eigen::Unaligned> HessianBlockTransposedType;     

        BaseBinaryEdge()
            : BaseEdge<D, E>()
            // HACK we map to the null pointer for initializing the Maps
            , _hessian(0, VertexXiType::Dimension, VertexXjType::Dimension)
            , _hessianTransposed(0, VertexXjType::Dimension, VertexXiType::Dimension)
            , _jacobianOplusXi(0, D, Di)
            , _jacobianOplusXj(0, D, Dj)
            , _hessianRowMajor(false)
        {}

        virtual uint04 vertexCount() const final override { return 2; };
        virtual const HyperGraph::HGVertex* vertex(uint04 i) const final override
        { 
            if (i == 0)
                return m_a;
            else
                return m_b;
        };

        virtual HyperGraph::HGVertex* vertex(uint04 i) final override
        {
            if (i == 0)
                return m_a;
            else
                return m_b;
        };

        virtual void setVertex(uint04 i, HyperGraph::HGVertex* v) final override
        { 
            lib_assert(i < 2, "index out of bounds");
            if (i == 0)
            {
                lib_assert(dynamic_cast<VertexXi*>(v) != nullptr, "Invalid vertex type");
                m_a = cast<VertexXi*>(v);
            }
            else
            {
                lib_assert(dynamic_cast<VertexXj*>(v) != nullptr, "Invalid vertex type");
                m_b = cast<VertexXj*>(v);
            }
        };

        const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi; }
        const JacobianXjOplusType& jacobianOplusXj() const { return _jacobianOplusXj; }

        void constructQuadraticForm2()
        {
            VertexXiType* from = m_a;
            VertexXjType* to = m_b;
 
            const bool fromNotFixed = !from->fixed();
            const bool toNotFixed = !to->fixed();
            if (!(fromNotFixed || toNotFixed)) 
                return;
 
            const InformationType& omega = BaseEdge<D, E>::information();
            const auto& A = jacobianOplusXi();
            const auto& B = jacobianOplusXj();
            const auto At = A.transpose().eval();
            const auto Bt = B.transpose().eval();
            const auto omega_error = (omega * _error).eval();
 
            g_type w_b = 1;
            InformationType omega_eff;
            if (this->robustKernel() == nullptr)
            {
                omega_eff = omega;
            }
            else
            {
                const g_type err = this->chi2();
                EIGEN_ALIGN32 Eigen::Vector3<g_type> rho;
                this->robustKernel()->robustify(err, rho);
                w_b = rho[1];
                omega_eff = this->robustInformation(rho);
            }
 
            const auto AtOmega = (At * omega_eff).eval();
            const auto BtOmega = (Bt * omega_eff).eval();
 
            if (fromNotFixed)
            {
                from->b().noalias() -= At * (omega_error * w_b);
                from->A().noalias() += AtOmega * A;
            }
 
            if (toNotFixed)
            {
                to->b().noalias() -= Bt * (omega_error * w_b);
                to->A().noalias() += BtOmega * B;
            }
 
            if (fromNotFixed && toNotFixed)
            {
                const auto Hij = (AtOmega * B).eval();
                if (_hessianRowMajor)
                    _hessianTransposed.noalias() += Hij.transpose();
                else
                    _hessian.noalias() += Hij;
            }
        }

        void constructQuadraticForm()
        {
 
            // get the Jacobian of the nodes in the manifold domain
            const JacobianXiOplusType& A = jacobianOplusXi();
            //if (!A.array().isFinite().any())
                //throw "bad a";
            const JacobianXjOplusType& B = jacobianOplusXj();
            //if (!B.array().isFinite().any())
                //throw "bad a";
            bool fromNotFixed = !(m_a->fixed());
            bool toNotFixed = !(m_b->fixed());
 
            if (fromNotFixed || toNotFixed)
            {
                const InformationType& omega = BaseEdge<D, E>::information();
                if (this->robustKernel() == nullptr)
                {
                    if (fromNotFixed)
                    {
                        m_a->b().noalias() += A.transpose() * (-omega * _error);
                        m_a->A().noalias() += (A.transpose() * omega) * A;
                        if (toNotFixed)
                        {
                            if (_hessianRowMajor) // we have to write to the block as transposed
                                _hessianTransposed.noalias() += B.transpose() * (A.transpose() * omega).transpose();
                            else
                                _hessian.noalias() += (A.transpose() * omega) * B;
                        }
                        //if (from->b().array().isNaN().any())
                        //  throw "bad a";
                        //if (from->A().array().isNaN().any())
                        //  throw "bad a";
                    }
                    if (toNotFixed)
                    {
                        m_b->b().noalias() += B.transpose() * (-omega * _error);
                        m_b->A().noalias() += B.transpose() * omega * B;
                        //if (!to->b().array().isFinite().any())
                        //  throw "bad a";
                        //if (!to->A().array().isFinite().any())
                        //  throw "bad a";
                    }
 
                }
                else
                { // robust (weighted) error according to some kernel
                    g_type error = this->chi2();
                    EIGEN_ALIGN32 Eigen::Vector3<g_type> rho;
                    this->robustKernel()->robustify(error, rho);
                    EIGEN_ALIGN32 InformationType weightedOmega = this->robustInformation(rho);
                    //std::cout << PVAR(rho.transpose()) << std::endl;
                    //std::cout << PVAR(weightedOmega) << std::endl;
                    auto a_trans = A.transpose();
                    auto b_trans = B.transpose();
                    auto b_weight_omega = weightedOmega * B;
                    if (fromNotFixed)
                    {
                        auto a_weight_omega = weightedOmega * A;
                        m_a->b().noalias() += a_trans * -omega * _error * rho[1];
                        m_a->A().noalias() += a_trans * a_weight_omega;
                        if (toNotFixed)
                        {
                            if (_hessianRowMajor) // we have to write to the block as transposed
                                _hessianTransposed.noalias() += b_trans * a_weight_omega;
                            else
                                _hessian.noalias() += a_trans * b_weight_omega;
                        }
                        //if (from->b().array().isNaN().any())
                            //throw "bad a";
                        //if (from->A().array().isNaN().any())
                            //throw "bad a";
                    }
                    if (toNotFixed)
                    {
                        m_b->b().noalias() += b_trans * -omega * _error * rho[1];
                        //if (to->b().array().isNaN().any())
                            //throw "bad a";
                        m_b->A().noalias() += b_trans * b_weight_omega;
                        //if (to->A().array().isNaN().any())
                            //throw "bad a";
                    }
 
                }
            }
        }

        bool allVerticesFixed() const final override
        {
            return m_a->fixed() && m_b->fixed();
        }
 

        void linearizeOplusAndConstructQuadraticForm(JacobianWorkspace& jacobianWorkspace) final override
        {
            lib_assert(jacobianWorkspace.maxNumVertices() >= 2, "Bad vertex size");
            lib_assert(jacobianWorkspace.maxDimension() >= D * Dj, "Bad vertex dimension");
            static_assert(sizeof(JacobianXiOplusType) <= D * (1 + Dj) * sizeof(g_type));
            static_assert(sizeof(JacobianXjOplusType) <= D * (1 + Dj) * sizeof(g_type));
            new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, Di);
            new (&_jacobianOplusXj) JacobianXjOplusType(jacobianWorkspace.workspaceForVertex(1), D, Dj);
            linearizeOplus();
            constructQuadraticForm();
        }

        virtual void linearizeOplus()
        {
            VertexXiType* vi = m_a;
            VertexXjType* vj = m_b;
 
            bool iNotFixed = !(vi->fixed());
            bool jNotFixed = !(vj->fixed());
 
            if (!iNotFixed && !jNotFixed)
                return;
            const g_type delta = g_type(1e-9);
            const g_type scalar = g_type(1.0) / (2 * delta);
            ErrorVector errorBak;
            ErrorVector errorBeforeNumeric = _error;
 
            if (iNotFixed)
            {
                //Xi - estimate the jacobian numerically
                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();
                    errorBak = _error;
                    vi->pop();
                    vi->push();
                    add_vi[d] = -delta;
                    vi->oplus(add_vi);
                    computeError();
                    errorBak -= _error;
                    vi->pop();
                    add_vi[d] = 0.0;
 
                    _jacobianOplusXi.col(d) = scalar * errorBak;
                } // end dimension
            }
 
            if (jNotFixed)
            {
                //Xj - estimate the jacobian numerically
                g_type add_vj[VertexXjType::Dimension];
                std::fill(add_vj, add_vj + VertexXjType::Dimension, g_type(0.0));
                // add small step along the unit vector in each dimension
                for (int d = 0; d < VertexXjType::Dimension; ++d)
                {
                    vj->push();
                    add_vj[d] = delta;
                    vj->oplus(add_vj);
                    computeError();
                    errorBak = _error;
                    vj->pop();
                    vj->push();
                    add_vj[d] = -delta;
                    vj->oplus(add_vj);
                    computeError();
                    errorBak -= _error;
                    vj->pop();
                    add_vj[d] = 0.0;
 
                    _jacobianOplusXj.col(d) = scalar * errorBak;
                }
            } // end dimension
 
            _error = errorBeforeNumeric;
        }

        void mapHessianMemory(g_type* d, int i, int j, bool rowMajor) final override
        {
            (void)i; (void)j;
            //assert(i == 0 && j == 1);
            if (rowMajor) {
                new (&_hessianTransposed) HessianBlockTransposedType(d, VertexXjType::Dimension, VertexXiType::Dimension);
            }
            else {
                new (&_hessian) HessianBlockType(d, VertexXiType::Dimension, VertexXjType::Dimension);
            }
            _hessianRowMajor = rowMajor;
        }
        using BaseEdge<D, E>::computeError;
 
    protected:
        VertexXi* m_a = nullptr;    
        VertexXj* m_b = nullptr;    
        using BaseEdge<D, E>::_measurement;
        using BaseEdge<D, E>::_information;
        using BaseEdge<D, E>::_error;
 
        HessianBlockType _hessian;                  
        HessianBlockTransposedType _hessianTransposed;      
        JacobianXiOplusType _jacobianOplusXi;           
        JacobianXjOplusType _jacobianOplusXj;           
        bool _hessianRowMajor;                      
 
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };
 
}