types_six_dof_expmap.h

#pragma once
#include "base_vertex.h"
#include "base_binary_edge.h"
#include "base_unary_edge.h"
#include "vertex_se3_expmap.h"
#include "se3_ops.h"
#include "se3quat.h"
#include "types_sba.h"
#include <Eigen/Geometry>
 
namespace NDEVR
{
    using namespace Eigen;
 
    typedef Eigen::Matrix<g_type, 6, 6> Matrix6d;   
 
 
 

    class EdgeSE3 : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        EdgeSE3()
            : BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>()
        {}
        void computeError() final override
        {
            SE3Quat C(_measurement);
            SE3Quat error_ = C * m_a->estimate() * m_b->estimate().inverse();
            _error = error_.log();
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
    };
 

    class  EdgeStereoSE3ProjectXYZ : public BaseBinaryEdge<3, Vector3<g_type>, VertexSBAPointXYZ, VertexSE3Expmap> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        void computeError() final override 
        {
            _error = _measurement - cam_project(m_b->estimate().map(m_a->estimate()), bf);
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
 
        bool isDepthPositive() 
        {
            return (m_b->estimate().map(m_a->estimate()))(2) > 0.0;
        }
        
 
 
        Vector3<g_type> cam_project(const Vector3<g_type>& trans_xyz, g_type bf_a) const
        {
            const g_type invz = g_type(1.0) / trans_xyz[2];
            Vector3<g_type> res;
            res[0] = trans_xyz[0] * invz * fx + cx;
            res[1] = trans_xyz[1] * invz * fy + cy;
            res[2] = res[0] - bf_a * invz;
            return res;
        }
 
        EdgeStereoSE3ProjectXYZ()
            : BaseBinaryEdge<3, Vector3<g_type>, VertexSBAPointXYZ, VertexSE3Expmap>()
            , fx(0), fy(0), cx(0), cy(0), bf(0)
        {}
 
 
        void linearizeOplus() final override
        {
            SE3Quat T(m_b->estimate());
            Vector3<g_type> xyz = m_a->estimate();
            Vector3<g_type> xyz_trans = T.map(xyz);
 
            const Matrix3<g_type> R = T.rotation().toRotationMatrix();
 
            g_type x = xyz_trans[0];
            g_type y = xyz_trans[1];
            g_type z = xyz_trans[2];
            g_type z_2 = z * z;
 
            _jacobianOplusXi(0, 0) = -fx * R(0, 0) / z + fx * x * R(2, 0) / z_2;
            _jacobianOplusXi(0, 1) = -fx * R(0, 1) / z + fx * x * R(2, 1) / z_2;
            _jacobianOplusXi(0, 2) = -fx * R(0, 2) / z + fx * x * R(2, 2) / z_2;
 
            _jacobianOplusXi(1, 0) = -fy * R(1, 0) / z + fy * y * R(2, 0) / z_2;
            _jacobianOplusXi(1, 1) = -fy * R(1, 1) / z + fy * y * R(2, 1) / z_2;
            _jacobianOplusXi(1, 2) = -fy * R(1, 2) / z + fy * y * R(2, 2) / z_2;
 
            _jacobianOplusXi(2, 0) = _jacobianOplusXi(0, 0) - bf * R(2, 0) / z_2;
            _jacobianOplusXi(2, 1) = _jacobianOplusXi(0, 1) - bf * R(2, 1) / z_2;
            _jacobianOplusXi(2, 2) = _jacobianOplusXi(0, 2) - bf * R(2, 2) / z_2;
 
            _jacobianOplusXj(0, 0) = x * y / z_2 * fx;
            _jacobianOplusXj(0, 1) = -(1 + (x * x / z_2)) * fx;
            _jacobianOplusXj(0, 2) = y / z * fx;
            _jacobianOplusXj(0, 3) = g_type(- 1.) / z * fx;
            _jacobianOplusXj(0, 4) = 0;
            _jacobianOplusXj(0, 5) = x / z_2 * fx;
 
            _jacobianOplusXj(1, 0) = (1 + y * y / z_2) * fy;
            _jacobianOplusXj(1, 1) = -x * y / z_2 * fy;
            _jacobianOplusXj(1, 2) = -x / z * fy;
            _jacobianOplusXj(1, 3) = 0;
            _jacobianOplusXj(1, 4) = g_type(-1.) / z * fy;
            _jacobianOplusXj(1, 5) = y / z_2 * fy;
 
            _jacobianOplusXj(2, 0) = _jacobianOplusXj(0, 0) - bf * y / z_2;
            _jacobianOplusXj(2, 1) = _jacobianOplusXj(0, 1) + bf * x / z_2;
            _jacobianOplusXj(2, 2) = _jacobianOplusXj(0, 2);
            _jacobianOplusXj(2, 3) = _jacobianOplusXj(0, 3);
            _jacobianOplusXj(2, 4) = 0;
            _jacobianOplusXj(2, 5) = _jacobianOplusXj(0, 5) - bf / z_2;
        }
 
 
        g_type fx, fy, cx, cy, bf;  
    };

    class  EdgeStereoSE3ProjectXYZOnlyPose : public  BaseUnaryEdge<3, Vector3<g_type>, VertexSE3Expmap> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 
        EdgeStereoSE3ProjectXYZOnlyPose() : fx(0), fy(0), cx(0), cy(0), bf(0) {}
        void computeError() final override 
        {
            Vector3<g_type> obs(_measurement);
            _error = obs - cam_project(m_vertex->estimate().map(Xw));
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
 
        bool isDepthPositive()  const
        {
            return (m_vertex->estimate().map(Xw))(2) > 0.0;
        }
 
 
        Vector2<g_type> project2d(const Vector3<g_type>& v)
        {
            Vector2<g_type> res;
            res(0) = v(0) / v(2);
            res(1) = v(1) / v(2);
            return res;
        }
 
        Vector3<g_type> unproject2d(const Vector2<g_type>& v)
        {
            Vector3<g_type> res;
            res(0) = v(0);
            res(1) = v(1);
            res(2) = 1;
            return res;
        }
 
        
 
 
        //Only Pose
 
        Vector3<g_type> cam_project(const Vector3<g_type>& trans_xyz) const
        {
            const g_type invz = g_type(1.0) / trans_xyz[2];
            Vector3<g_type> res;
            res[0] = trans_xyz[0] * invz * fx + cx;
            res[1] = trans_xyz[1] * invz * fy + cy;
            res[2] = res[0] - bf * invz;
            return res;
        }
 
        void linearizeOplus() final override
        {
            Vector3<g_type> xyz_trans = m_vertex->estimate().map(Xw);
 
            g_type x = xyz_trans[0];
            g_type y = xyz_trans[1];
            //if (abs(xyz_trans[2]) < 1e-12)
                //throw "sd";
            g_type invz = g_type(1.0) / xyz_trans[2];
            g_type invz_2 = invz * invz;
 
            _jacobianOplusXi(0, 0) = x * y * invz_2 * fx;
            _jacobianOplusXi(0, 1) = -(1 + (x * x * invz_2)) * fx;
            _jacobianOplusXi(0, 2) = y * invz * fx;
            _jacobianOplusXi(0, 3) = -invz * fx;
            _jacobianOplusXi(0, 4) = 0;
            _jacobianOplusXi(0, 5) = x * invz_2 * fx;
 
            _jacobianOplusXi(1, 0) = (1 + y * y * invz_2) * fy;
            _jacobianOplusXi(1, 1) = -x * y * invz_2 * fy;
            _jacobianOplusXi(1, 2) = -x * invz * fy;
            _jacobianOplusXi(1, 3) = 0;
            _jacobianOplusXi(1, 4) = -invz * fy;
            _jacobianOplusXi(1, 5) = y * invz_2 * fy;
 
            _jacobianOplusXi(2, 0) = _jacobianOplusXi(0, 0) - bf * y * invz_2;
            _jacobianOplusXi(2, 1) = _jacobianOplusXi(0, 1) + bf * x * invz_2;
            _jacobianOplusXi(2, 2) = _jacobianOplusXi(0, 2);
            _jacobianOplusXi(2, 3) = _jacobianOplusXi(0, 3);
            _jacobianOplusXi(2, 4) = 0;
            _jacobianOplusXi(2, 5) = _jacobianOplusXi(0, 5) - bf * invz_2;
        }
        Vector3<g_type> Xw;         
        g_type fx, fy, cx, cy, bf;  
    };
 
}