OptimizableTypes.h

#pragma once
#include "base_unary_edge.h"
#include "types_six_dof_expmap.h"
#include "sim3.h"
#include "GraphOptimization/Headers/GeometricCamera.h"
#include <Eigen/Geometry>
 
 
namespace NDEVR
{
    class GeometricCamera;
    class EdgeSE3ProjectXYZOnlyPose : public BaseUnaryEdge<2, Eigen::Vector2<g_type>, VertexSE3Expmap>
    {
    public:
        EdgeSE3ProjectXYZOnlyPose()
        {}

        void computeError() final override
        {
            _error = _measurement - pCamera->project(m_vertex->estimate().map(Xw));
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
 
        bool isDepthPositive() 
        {
            return (m_vertex->estimate().map(Xw))(2) > g_type(0.0);
        }
        void linearizeOplus() final override 
        {
            Eigen::Vector3<g_type> xyz_trans = m_vertex->estimate().map(Xw);
 
            g_type x = xyz_trans[0];
            g_type y = xyz_trans[1];
            g_type z = xyz_trans[2];
 
            Eigen::Matrix<g_type, 3, 6> SE3deriv;
            SE3deriv << 0.f, z, -y, 1.f, 0.f, 0.f,
                -z, 0.f, x, 0.f, 1.f, 0.f,
                y, -x, 0.f, 0.f, 0.f, 1.f;
 
            _jacobianOplusXi = -pCamera->projectJac(xyz_trans) * SE3deriv;
        }
        
 
        Eigen::Vector3<g_type> Xw;
        GeometricCamera* pCamera = nullptr;
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

    class EdgeSE3ProjectXYZOnlyPoseToBody : public  BaseUnaryEdge<2, Eigen::Vector2<g_type>, VertexSE3Expmap>
    {
    public:
        EdgeSE3ProjectXYZOnlyPoseToBody()
        {}
 
        bool isDepthPositive() 
        {
            return ((mTrl * m_vertex->estimate()).map(Xw))(2) > 0.0;
        }
        void computeError() final override
        {
            _error = _measurement - pCamera->project((mTrl * m_vertex->estimate()).map(Xw));
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
 
        void linearizeOplus() final override
        {
            SE3Quat T_lw(m_vertex->estimate());
            Eigen::Vector3<g_type> X_l = T_lw.map(Xw);
            Eigen::Vector3<g_type> X_r = mTrl.map(T_lw.map(Xw));
 
            g_type x_w = X_l[0];
            g_type y_w = X_l[1];
            g_type z_w = X_l[2];
 
            Eigen::Matrix<g_type, 3, 6> SE3deriv;
            SE3deriv << 0.f, z_w, -y_w, 1.f, 0.f, 0.f,
                -z_w, 0.f, x_w, 0.f, 1.f, 0.f,
                y_w, -x_w, 0.f, 0.f, 0.f, 1.f;
 
            _jacobianOplusXi = -pCamera->projectJac(X_r) * mTrl.rotation().toRotationMatrix() * SE3deriv;
        }
        Eigen::Vector3<g_type> Xw;              
        GeometricCamera* pCamera = nullptr;     
        SE3Quat mTrl;                           
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

    class EdgeSE3ProjectXYZ : public BaseBinaryEdge<2, Eigen::Vector2<g_type>, VertexSBAPointXYZ, VertexSE3Expmap>
    {
    public:
        EdgeSE3ProjectXYZ()
            : BaseBinaryEdge<2, Eigen::Vector2<g_type>, VertexSBAPointXYZ, VertexSE3Expmap>()
        {}
        bool isDepthPositive() 
        {
            return (m_b->estimate().map(m_a->estimate()))(2) > 0.0;
        }
        void computeError() final override
        {
            Eigen::Vector2<g_type> obs(_measurement);
            _error = obs - pCamera->project(m_b->estimate().map(m_a->estimate()));
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
        void linearizeOplus() final override
        {
            SE3Quat T(m_b->estimate());
            Eigen::Vector3<g_type> xyz = m_a->estimate();
            Eigen::Vector3<g_type> xyz_trans = T.map(xyz);
 
            g_type x = xyz_trans[0];
            g_type y = xyz_trans[1];
            g_type z = xyz_trans[2];
 
            auto projectJac = -pCamera->projectJac(xyz_trans);
 
            _jacobianOplusXi = projectJac * T.rotation().toRotationMatrix();
 
            Eigen::Matrix<g_type, 3, 6> SE3deriv;
            SE3deriv << 0.f, z, -y, 1.f, 0.f, 0.f,
                -z, 0.f, x, 0.f, 1.f, 0.f,
                y, -x, 0.f, 0.f, 0.f, 1.f;
 
            _jacobianOplusXj = projectJac * SE3deriv;
        }
        GeometricCamera* pCamera;   
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    };

    class  EdgeSE3ProjectXYZToBody : public  BaseBinaryEdge<2, Eigen::Vector2<g_type>, VertexSBAPointXYZ, VertexSE3Expmap> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        EdgeSE3ProjectXYZToBody()
            : BaseBinaryEdge<2, Eigen::Vector2<g_type>, VertexSBAPointXYZ, VertexSE3Expmap>()
        {}
 
        void computeError() final override
        {
            _error = _measurement - pCamera->project((mTrl * m_b->estimate()).map(m_a->estimate()));
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
        void linearizeOplus() final override
        {
            SE3Quat T_lw(m_b->estimate());
            SE3Quat T_rw = mTrl * T_lw;
            Eigen::Vector3<g_type> X_w = m_a->estimate();
            Eigen::Vector3<g_type> X_l = T_lw.map(X_w);
            Eigen::Vector3<g_type> X_r = mTrl.map(T_lw.map(X_w));
 
            _jacobianOplusXi = -pCamera->projectJac(X_r) * T_rw.rotation().toRotationMatrix();
 
            g_type x = X_l[0];
            g_type y = X_l[1];
            g_type z = X_l[2];
 
            Eigen::Matrix<g_type, 3, 6> SE3deriv;
            SE3deriv << 0.f, z, -y, 1.f, 0.f, 0.f,
                -z, 0.f, x, 0.f, 1.f, 0.f,
                y, -x, 0.f, 0.f, 0.f, 1.f;
 
            _jacobianOplusXj = -pCamera->projectJac(X_r) * mTrl.rotation().toRotationMatrix() * SE3deriv;
        }
        bool isDepthPositive() {
            return ((mTrl * m_b->estimate()).map(m_a->estimate()))(2) > 0.0;
        }
        GeometricCamera* pCamera = nullptr; 
        SE3Quat mTrl;                       
    };

    class VertexSim3Expmap : public BaseVertex<7, Sim3>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        VertexSim3Expmap()
            : BaseVertex<7, Sim3>()
            , pCamera1(nullptr)
            , pCamera2(nullptr)
        {
            _marginalized = false;
            _fix_scale = false;
        }
 
        
        virtual void setToOriginImpl() 
        {
            _estimate = Sim3();
        }
 
        virtual void oplusImpl(const g_type* update_)
        {
            Vector7d update(update_);
            Sim3 s(update, _fix_scale);
            setEstimate(s * estimate());
        }
 
        
 
        GeometricCamera* pCamera1, * pCamera2;  
        bool _fix_scale;                        
    };
 

    class EdgeSim3ProjectXYZ : public  BaseBinaryEdge<2, Eigen::Vector2<g_type>, VertexSBAPointXYZ, VertexSim3Expmap>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        EdgeSim3ProjectXYZ() 
            : BaseBinaryEdge<2, Eigen::Vector2<g_type>, VertexSBAPointXYZ, VertexSim3Expmap>()
        {
        }
        void computeError() final override
        {
            _error = _measurement - m_b->pCamera1->project(m_b->estimate().map(m_a->estimate()));
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
 
        // virtual void linearizeOplus();
 
    };

    class EdgeInverseSim3ProjectXYZ : public BaseBinaryEdge<2, Eigen::Vector2<g_type>, VertexSBAPointXYZ, VertexSim3Expmap>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        EdgeInverseSim3ProjectXYZ()
            : BaseBinaryEdge<2, Eigen::Vector2<g_type>, VertexSBAPointXYZ, VertexSim3Expmap>()
        {
        }
        void computeError() final override
        {
            _error = _measurement - m_b->pCamera2->project((m_b->estimate().inverse().map(m_a->estimate())));
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
        // virtual void linearizeOplus();
 
    };
 
}