G2oTypes.h

#pragma once
#include "GraphOptimization/Headers/base_vertex.h"
#include "GraphOptimization/Headers/base_binary_edge.h"
#include "GraphOptimization/Headers/types_sba.h"
#include "GraphOptimization/Headers/base_multi_edge.h"
#include "GraphOptimization/Headers/base_unary_edge.h"
 
#include <opencv2/core/core.hpp>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
 
#include "OrbSLAM/Headers/Frame.h"
#include "OrbSLAM/Headers/KeyFrame.h"
 
namespace NDEVR
{
 
    class KeyFrame;
    class Frame;
    class GeometricCamera;
 
    typedef Eigen::Matrix<g_type, 6, 1> vector6d;       
    typedef Eigen::Matrix<g_type, 9, 1> vector9d;       
    typedef Eigen::Matrix<g_type, 12, 1> vector12d;     
    typedef Eigen::Matrix<g_type, 15, 1> vector15d;     
    typedef Eigen::Matrix<g_type, 12, 12> Matrix12d;    
    typedef Eigen::Matrix<g_type, 15, 15> Matrix15d;    
    typedef Eigen::Matrix<g_type, 9, 9> Matrix9d;       

    Eigen::Matrix3<g_type> ExpSO3(const g_type x, const g_type y, const g_type z);
    Eigen::Matrix3<g_type> ExpSO3(const Eigen::Vector3<g_type>& w);

    Eigen::Vector3<g_type> LogSO3(const Eigen::Matrix3<g_type>& R);

    Eigen::Matrix3<g_type> InverseRightJacobianSO3(const Eigen::Vector3<g_type>& v);
    Eigen::Matrix3<g_type> RightJacobianSO3(const Eigen::Vector3<g_type>& v);
    Eigen::Matrix3<g_type> RightJacobianSO3(g_type x, g_type y, g_type z);

    Eigen::Matrix3<g_type> Skew(const Eigen::Vector3<g_type>& w);
    Eigen::Matrix3<g_type> InverseRightJacobianSO3(g_type x, g_type y, g_type z);

    template<typename T = g_type>
    Eigen::Matrix<T, 3, 3> NormalizeRotation(const Eigen::Matrix<T, 3, 3>& R) {
        Eigen::JacobiSVD<Eigen::Matrix<T, 3, 3>> svd(R, Eigen::ComputeFullU | Eigen::ComputeFullV);
        return svd.matrixU() * svd.matrixV().transpose();
    }
 

    class ImuCamPose
    {
    public:
        ImuCamPose() {}
        ImuCamPose(Frame& pF);
        ImuCamPose(Eigen::Matrix3<g_type>& _Rwc, Eigen::Vector3<g_type>& _twc, KeyFrame* pKF);

        void SetParam(const Buffer<Eigen::Matrix3<g_type>>& _Rcw, const Buffer<Eigen::Vector3<g_type>>& _tcw, const Buffer<Eigen::Matrix3<g_type>>& _Rbc,
            const Buffer<Eigen::Vector3<g_type>>& _tbc, const g_type& _bf);

        void Update(const g_type* pu);
        void UpdateW(const g_type* pu);
        Eigen::Vector2<g_type> Project(const Eigen::Vector3<g_type>& Xw, int cam_idx = 0) const;
        Eigen::Vector3<g_type> ProjectStereo(const Eigen::Vector3<g_type>& Xw, int cam_idx = 0) const;
        bool isDepthPositive(const Eigen::Vector3<g_type>& Xw, int cam_idx = 0) const;
 
    public:
        Eigen::Matrix3<g_type> Rwb;     
        Eigen::Vector3<g_type> twb;     
 
        Buffer<Eigen::Matrix3<g_type>> Rcw;     
        Buffer<Eigen::Vector3<g_type>> tcw;     
        Buffer<Eigen::Matrix3<g_type>> Rcb, Rbc;    
        Buffer<Eigen::Vector3<g_type>> tcb, tbc;    
        g_type bf;                              
        Buffer<GeometricCamera*> pCamera;       
 
        Eigen::Matrix3<g_type> Rwb0;    
        Eigen::Matrix3<g_type> DR;      
 
        int its;    
    };

    class InvDepthPoint
    {
    public:
        InvDepthPoint() {}
        InvDepthPoint(g_type _rho, g_type _u, g_type _v, KeyFrame* pHostKF);

        void Update(const g_type* pu);
 
        g_type rho;                 
        g_type u, v;                
        g_type fx, fy, cx, cy, bf;  
        int its;                    
    };

    class VertexPose : public BaseVertex<6, ImuCamPose>
    {
    public:
        VertexPose() {}
        VertexPose(Frame& pF) {
            setEstimate(ImuCamPose(pF));
        }
        virtual void oplusImpl(const g_type* update_) final override 
        {
            _estimate.Update(update_);
        }
    };

    class VertexPose4DoF : public BaseVertex<4, ImuCamPose>
    {
    public:
        VertexPose4DoF() {}
        VertexPose4DoF(Frame& pF) {
            setEstimate(ImuCamPose(pF));
        }
        VertexPose4DoF(Eigen::Matrix3<g_type>& _Rwc, Eigen::Vector3<g_type>& _twc, KeyFrame* pKF) {
 
            setEstimate(ImuCamPose(_Rwc, _twc, pKF));
        }
        virtual void oplusImpl(const g_type* update_) override {
            g_type update6DoF[6];
            update6DoF[0] = 0;
            update6DoF[1] = 0;
            update6DoF[2] = update_[0];
            update6DoF[3] = update_[1];
            update6DoF[4] = update_[2];
            update6DoF[5] = update_[3];
            _estimate.UpdateW(update6DoF);
        }
    };

    class VertexVelocity : public BaseVertex<3, Eigen::Vector3<g_type>>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        VertexVelocity() {}
        VertexVelocity(Frame& pF);
 
        virtual void oplusImpl(const g_type* update_) final override
        {
            Eigen::Vector3<g_type> uv;
            uv << update_[0], update_[1], update_[2];
            setEstimate(estimate() + uv);
            lib_assert(abs(_estimate.x()) < 10.0, "Bad speed");
            lib_assert(abs(_estimate.y()) < 10.0, "Bad speed");
            lib_assert(abs(_estimate.z()) < 10.0, "Bad speed");
        }
    };

    class VertexGyroBias : public BaseVertex<3, Eigen::Vector3<g_type>>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        VertexGyroBias() {}
        VertexGyroBias(Frame& pF);
 
        virtual void oplusImpl(const g_type* update_) final override
        {
            Eigen::Vector3<g_type> ubg;
            ubg << update_[0], update_[1], update_[2];
            setEstimate(estimate() + ubg);
        }
    };
 

    class VertexAccBias : public BaseVertex<3, Eigen::Vector3<g_type>>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        VertexAccBias() {}
        VertexAccBias(Frame& pF);
 
        virtual void oplusImpl(const g_type* update_) final override
        {
            Eigen::Vector3<g_type> uba;
            uba << update_[0], update_[1], update_[2];
            setEstimate(estimate() + uba);
        }
    };
 

    class GDirection
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
            GDirection() {}
        GDirection(Eigen::Matrix3<g_type> pRwg) : Rwg(pRwg) {}
 
        void Update(const g_type* pu)
        {
            Rwg = Rwg * ExpSO3(pu[0], pu[1], 0.0);
        }
 
        Eigen::Matrix3<g_type> Rwg, Rgw;    
 
        int its;    
    };

    class VertexGDir : public BaseVertex<2, GDirection>
    {
    public:
        VertexGDir() {}
        VertexGDir(Eigen::Matrix3<g_type> pRwg) {
            setEstimate(GDirection(pRwg));
        }
        virtual void oplusImpl(const g_type* update_) final override {
            _estimate.Update(update_);
        }
    };

    class VertexScale : public BaseVertex<1, g_type>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
            VertexScale() {
            setEstimate(1.0);
        }
        VertexScale(g_type ps) {
            setEstimate(ps);
        }
 
        virtual void oplusImpl(const g_type* update_) final override {
            setEstimate(estimate() * exp(*update_));
        }
    };
 

    class VertexInvDepth : public BaseVertex<1, InvDepthPoint>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        VertexInvDepth() {}
        VertexInvDepth(g_type invDepth, g_type u, g_type v, KeyFrame* pHostKF) {
            setEstimate(InvDepthPoint(invDepth, u, v, pHostKF));
        }
        virtual void oplusImpl(const g_type* update_) final override {
            _estimate.Update(update_);
        }
    };

    class EdgeMono : public BaseBinaryEdge<2, Eigen::Vector2<g_type>, VertexSBAPointXYZ, VertexPose>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        EdgeMono(int cam_idx_ = 0) 
            : cam_idx(cam_idx_) 
        {}
        void computeError() {
            _error = _measurement - m_b->estimate().Project(m_a->estimate(), cam_idx);
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
 
 
        virtual void linearizeOplus();
 
        bool isDepthPositive()
        {
            return m_b->estimate().isDepthPositive(m_a->estimate(), cam_idx);
        }
 
        Eigen::Matrix<g_type, 2, 9> GetJacobian() {
            linearizeOplus();
            Eigen::Matrix<g_type, 2, 9> J;
            J.block<2, 3>(0, 0) = _jacobianOplusXi;
            J.block<2, 6>(0, 3) = _jacobianOplusXj;
            return J;
        }
 
        Eigen::Matrix<g_type, 9, 9> GetHessian()
        {
            linearizeOplus();
            Eigen::Matrix<g_type, 2, 9> J;
            J.block<2, 3>(0, 0) = _jacobianOplusXi;
            J.block<2, 6>(0, 3) = _jacobianOplusXj;
            return J.transpose() * information() * J;
 
        }
 
    public:
        const int cam_idx;
    };

    class EdgeMonoOnlyPose : public BaseUnaryEdge<2, Eigen::Vector2<g_type>, VertexPose>
    {
    public:
        EdgeMonoOnlyPose(const Eigen::Vector3<g_type>& Xw_, int cam_idx_ = 0)
            : Xw(Xw_)
            , cam_idx(cam_idx_)
        {}
        void computeError()
        {
            _error = _measurement - m_vertex->estimate().Project(Xw, cam_idx);
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
 
        virtual void linearizeOplus();
 
        bool isDepthPositive()
        {
            return m_vertex->estimate().isDepthPositive(Xw, cam_idx);
        }
 
        Eigen::Matrix<g_type, 6, 6> GetHessian() {
            linearizeOplus();
            return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi;
        }
 
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        const Eigen::Vector3<g_type> Xw;
        const int cam_idx;
    };

    class EdgeStereo : public BaseBinaryEdge<3, Eigen::Vector3<g_type>, VertexSBAPointXYZ, VertexPose>
    {
    public:
        EdgeStereo(int cam_idx_ = 0)
            : cam_idx(cam_idx_)
        {}
        bool isDepthPositive()
        {
            return true;
        }
        void computeError()
        {
            _error = _measurement - m_b->estimate().ProjectStereo(m_a->estimate(), cam_idx);
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
 
 
        virtual void linearizeOplus();
 
        Eigen::Matrix<g_type, 3, 9> GetJacobian()
        {
            linearizeOplus();
            Eigen::Matrix<g_type, 3, 9> J;
            J.block<3, 3>(0, 0) = _jacobianOplusXi;
            J.block<3, 6>(0, 3) = _jacobianOplusXj;
            return J;
        }
 
        Eigen::Matrix<g_type, 9, 9> GetHessian()
        {
            linearizeOplus();
            Eigen::Matrix<g_type, 3, 9> J;
            J.block<3, 3>(0, 0) = _jacobianOplusXi;
            J.block<3, 6>(0, 3) = _jacobianOplusXj;
            return J.transpose() * information() * J;
        }
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        const int cam_idx;
    };
 

    class EdgeStereoOnlyPose : public BaseUnaryEdge<3, Eigen::Vector3<g_type>, VertexPose>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
            EdgeStereoOnlyPose(const Eigen::Vector3f& Xw_, int cam_idx_ = 0) :
            Xw(Xw_.cast<g_type>()), cam_idx(cam_idx_) {}
        void computeError() {
            _error = _measurement - m_vertex->estimate().ProjectStereo(Xw, cam_idx);
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
 
        virtual void linearizeOplus();
 
        Eigen::Matrix<g_type, 6, 6> GetHessian() {
            linearizeOplus();
            return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi;
        }
 
    public:
        const Eigen::Vector3<g_type> Xw; // 3D point coordinates
        const int cam_idx;
    };

    class EdgeInertial : public BaseMultiEdge<9, vector9d>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
            EdgeInertial(IMU::Preintegrated* pInt);
        void computeError();
        virtual void linearizeOplus();
 
        Eigen::Matrix<g_type, 24, 24> GetHessian() {
            linearizeOplus();
            Eigen::Matrix<g_type, 9, 24> J;
            J.block<9, 6>(0, 0) = _jacobianOplus[0];
            J.block<9, 3>(0, 6) = _jacobianOplus[1];
            J.block<9, 3>(0, 9) = _jacobianOplus[2];
            J.block<9, 3>(0, 12) = _jacobianOplus[3];
            J.block<9, 6>(0, 15) = _jacobianOplus[4];
            J.block<9, 3>(0, 21) = _jacobianOplus[5];
            return J.transpose() * information() * J;
        }
 
        Eigen::Matrix<g_type, 18, 18> GetHessianNoPose1() {
            linearizeOplus();
            Eigen::Matrix<g_type, 9, 18> J;
            J.block<9, 3>(0, 0) = _jacobianOplus[1];
            J.block<9, 3>(0, 3) = _jacobianOplus[2];
            J.block<9, 3>(0, 6) = _jacobianOplus[3];
            J.block<9, 6>(0, 9) = _jacobianOplus[4];
            J.block<9, 3>(0, 15) = _jacobianOplus[5];
            return J.transpose() * information() * J;
        }
 
        Eigen::Matrix<g_type, 9, 9> GetHessian2() {
            linearizeOplus();
            Eigen::Matrix<g_type, 9, 9> J;
            J.block<9, 6>(0, 0) = _jacobianOplus[4];
            J.block<9, 3>(0, 6) = _jacobianOplus[5];
            return J.transpose() * information() * J;
        }
 
        const Eigen::Matrix3<g_type> JRg, JVg, JPg;
        const Eigen::Matrix3<g_type> JVa, JPa;
        IMU::Preintegrated* mpInt;
        const g_type dt;
        Eigen::Vector3<g_type> g;
    };
 

    class EdgeInertialGS : public BaseMultiEdge<9, vector9d>
    {
    public:
        EdgeInertialGS(IMU::Preintegrated* pInt);
        void computeError() final override;
        virtual void linearizeOplus() final override;
 
        const Eigen::Matrix3<g_type> JRg, JVg, JPg;
        const Eigen::Matrix3<g_type> JVa, JPa;
        IMU::Preintegrated* mpInt;
        const g_type dt;
        Eigen::Vector3<g_type> g, gI;
        Eigen::Matrix<g_type, 27, 27> GetHessian() 
        {
            linearizeOplus();
            Eigen::Matrix<g_type, 9, 27> J;
            J.block<9, 6>(0, 0) = _jacobianOplus[0];
            J.block<9, 3>(0, 6) = _jacobianOplus[1];
            J.block<9, 3>(0, 9) = _jacobianOplus[2];
            J.block<9, 3>(0, 12) = _jacobianOplus[3];
            J.block<9, 6>(0, 15) = _jacobianOplus[4];
            J.block<9, 3>(0, 21) = _jacobianOplus[5];
            J.block<9, 2>(0, 24) = _jacobianOplus[6];
            J.block<9, 1>(0, 26) = _jacobianOplus[7];
            return J.transpose() * information() * J;
        }
 
        Eigen::Matrix<g_type, 27, 27> GetHessian2() {
            linearizeOplus();
            Eigen::Matrix<g_type, 9, 27> J;
            J.block<9, 3>(0, 0) = _jacobianOplus[2];
            J.block<9, 3>(0, 3) = _jacobianOplus[3];
            J.block<9, 2>(0, 6) = _jacobianOplus[6];
            J.block<9, 1>(0, 8) = _jacobianOplus[7];
            J.block<9, 3>(0, 9) = _jacobianOplus[1];
            J.block<9, 3>(0, 12) = _jacobianOplus[5];
            J.block<9, 6>(0, 15) = _jacobianOplus[0];
            J.block<9, 6>(0, 21) = _jacobianOplus[4];
            return J.transpose() * information() * J;
        }
 
        Eigen::Matrix<g_type, 9, 9> GetHessian3() {
            linearizeOplus();
            Eigen::Matrix<g_type, 9, 9> J;
            J.block<9, 3>(0, 0) = _jacobianOplus[2];
            J.block<9, 3>(0, 3) = _jacobianOplus[3];
            J.block<9, 2>(0, 6) = _jacobianOplus[6];
            J.block<9, 1>(0, 8) = _jacobianOplus[7];
            return J.transpose() * information() * J;
        }
 
 
 
        Eigen::Matrix<g_type, 1, 1> GetHessianScale() {
            linearizeOplus();
            Eigen::Matrix<g_type, 9, 1> J = _jacobianOplus[7];
            return J.transpose() * information() * J;
        }
 
        Eigen::Matrix<g_type, 3, 3> GetHessianBiasGyro() {
            linearizeOplus();
            Eigen::Matrix<g_type, 9, 3> J = _jacobianOplus[2];
            return J.transpose() * information() * J;
        }
 
        Eigen::Matrix<g_type, 3, 3> GetHessianBiasAcc() {
            linearizeOplus();
            Eigen::Matrix<g_type, 9, 3> J = _jacobianOplus[3];
            return J.transpose() * information() * J;
        }
 
        Eigen::Matrix<g_type, 2, 2> GetHessianGDir() {
            linearizeOplus();
            Eigen::Matrix<g_type, 9, 2> J = _jacobianOplus[6];
            return J.transpose() * information() * J;
        }
    };
 
 

    class EdgeGyroRW : public BaseBinaryEdge<3, Eigen::Vector3<g_type>, VertexGyroBias, VertexGyroBias>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        EdgeGyroRW() {}
        void computeError() final override {
            _error = m_b->estimate() - m_a->estimate();
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
 
        virtual void linearizeOplus() final override
        {
            _jacobianOplusXi = -Eigen::Matrix3<g_type>::Identity();
            _jacobianOplusXj.setIdentity();
        }
 
        Eigen::Matrix<g_type, 6, 6> GetHessian() {
            linearizeOplus();
            Eigen::Matrix<g_type, 3, 6> J;
            J.block<3, 3>(0, 0) = _jacobianOplusXi;
            J.block<3, 3>(0, 3) = _jacobianOplusXj;
            return J.transpose() * information() * J;
        }
 
        Eigen::Matrix3<g_type> GetHessian2() {
            linearizeOplus();
            return _jacobianOplusXj.transpose() * information() * _jacobianOplusXj;
        }
    };
 

    class EdgeAccRW : public BaseBinaryEdge<3, Eigen::Vector3<g_type>, VertexAccBias, VertexAccBias>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        EdgeAccRW() {}
        void computeError() final override
        {
            _error = m_b->estimate() - m_a->estimate();
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
 
        virtual void linearizeOplus() final override
        {
            _jacobianOplusXi = -Eigen::Matrix3<g_type>::Identity();
            _jacobianOplusXj.setIdentity();
        }
 
        Eigen::Matrix<g_type, 6, 6> GetHessian() {
            linearizeOplus();
            Eigen::Matrix<g_type, 3, 6> J;
            J.block<3, 3>(0, 0) = _jacobianOplusXi;
            J.block<3, 3>(0, 3) = _jacobianOplusXj;
            return J.transpose() * information() * J;
        }
 
        Eigen::Matrix3<g_type> GetHessian2() {
            linearizeOplus();
            return _jacobianOplusXj.transpose() * information() * _jacobianOplusXj;
        }
    };

    class ConstraintPoseImu
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        ConstraintPoseImu(const Eigen::Matrix3<g_type>& Rwb_
            , const Eigen::Vector3<g_type>& twb_
            , const Eigen::Vector3<g_type>& vwb_
            , const Eigen::Vector3<g_type>& bg_
            , const Eigen::Vector3<g_type>& ba_
            , const Matrix15d& H_)
        : Rwb(Rwb_)
        , twb(twb_)
        , vwb(vwb_)
        , bg(bg_)
        , ba(ba_)
        , H(H_)
        {
            H = (H + H) / 2;
            Eigen::SelfAdjointEigenSolver<Eigen::Matrix<g_type, 15, 15> > es(H);
            Eigen::Matrix<g_type, 15, 1> eigs = es.eigenvalues();
            for (int i = 0; i < 15; i++)
                if (eigs[i] < 1e-12)
                    eigs[i] = 0;
            H = es.eigenvectors() * eigs.asDiagonal() * es.eigenvectors().transpose();
        }
 
        Eigen::Matrix3<g_type> Rwb; 
        Eigen::Vector3<g_type> twb; 
        Eigen::Vector3<g_type> vwb; 
        Eigen::Vector3<g_type> bg;  
        Eigen::Vector3<g_type> ba;  
        Matrix15d H;                
    };

    class EdgePriorPoseImu : public BaseMultiEdge<15, vector15d>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        EdgePriorPoseImu(ConstraintPoseImu* c);
        void computeError() override;
        virtual void linearizeOplus() override;
 
        Eigen::Matrix<g_type, 15, 15> GetHessian()
        {
            linearizeOplus();
            Eigen::Matrix<g_type, 15, 15> J;
            J.block<15, 6>(0, 0) = _jacobianOplus[0];
            J.block<15, 3>(0, 6) = _jacobianOplus[1];
            J.block<15, 3>(0, 9) = _jacobianOplus[2];
            J.block<15, 3>(0, 12) = _jacobianOplus[3];
            return J.transpose() * information() * J;
        }
 
        Eigen::Matrix<g_type, 9, 9> GetHessianNoPose()
        {
            linearizeOplus();
            Eigen::Matrix<g_type, 15, 9> J;
            J.block<15, 3>(0, 0) = _jacobianOplus[1];
            J.block<15, 3>(0, 3) = _jacobianOplus[2];
            J.block<15, 3>(0, 6) = _jacobianOplus[3];
            return J.transpose() * information() * J;
        }
        Eigen::Matrix3<g_type> Rwb;
        Eigen::Vector3<g_type> twb, vwb;
        Eigen::Vector3<g_type> bg, ba;
    };

    class EdgePriorAcc : public BaseUnaryEdge<3, Eigen::Vector3<g_type>, VertexAccBias>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        EdgePriorAcc(const Eigen::Vector3f& bprior_)
            : bprior(bprior_.cast<g_type>())
        {}
 
        void computeError() final override
        {
            _error = bprior - m_vertex->estimate();
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
        virtual void linearizeOplus() final override;
 
        Eigen::Matrix<g_type, 3, 3> GetHessian()
        {
            linearizeOplus();
            return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi;
        }
 
        const Eigen::Vector3<g_type> bprior;
    };

    class EdgePriorGyro : public BaseUnaryEdge<3, Eigen::Vector3<g_type>, VertexGyroBias>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        EdgePriorGyro(const Eigen::Vector3f& bprior_) :bprior(bprior_.cast<g_type>()) {}
 
        void computeError() final override
        {
            _error = bprior - m_vertex->estimate();
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
        virtual void linearizeOplus() final override;
 
        Eigen::Matrix<g_type, 3, 3> GetHessian()
        {
            linearizeOplus();
            return _jacobianOplusXi.transpose() * information() * _jacobianOplusXi;
        }
 
        const Eigen::Vector3<g_type> bprior;
    };
 

    class Edge4DoF : public BaseBinaryEdge<6, vector6d, VertexPose4DoF, VertexPose4DoF>
    {
    public:
        Edge4DoF(const Eigen::Matrix4<g_type>& deltaT) {
            dTij = deltaT;
            dRij = deltaT.block<3, 3>(0, 0);
            dtij = deltaT.block<3, 1>(0, 3);
        }
 
        void computeError() 
        {
            _error << LogSO3(m_a->estimate().Rcw[0] * m_a->estimate().Rcw[0].transpose() * dRij.transpose()),
                m_a->estimate().Rcw[0] * (-m_a->estimate().Rcw[0].transpose() * m_a->estimate().tcw[0])
                + m_a->estimate().tcw[0] - dtij;
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
 
        // virtual void linearizeOplus(); // numerical implementation
 
        Eigen::Matrix4<g_type> dTij;
        Eigen::Matrix3<g_type> dRij;
        Eigen::Vector3<g_type> dtij;
    };
 
}