edge_se3_expmap.h

#pragma once
 
#include "base_binary_edge.h"
#include "vertex_se3_expmap.h"
 
namespace NDEVR 
{

    class EdgeSE3Expmap
        : public BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap> {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
        EdgeSE3Expmap()
            : BaseBinaryEdge<6, SE3Quat, VertexSE3Expmap, VertexSE3Expmap>()
        {}

        void computeError() override
        {
            SE3Quat error_ = m_b->estimate().inverse() * _measurement * m_a->estimate();
            _error = error_.log();
            //if (!_error.array().isFinite().all())
              //  throw "error";
        }

        void linearizeOplus() override
        {
            SE3Quat Ti(m_a->estimate());
            SE3Quat Tj(m_b->estimate());
 
            const SE3Quat& Tij = _measurement;
            SE3Quat invTij = Tij.inverse();
 
            SE3Quat invTj_Tij = Tj.inverse() * Tij;
            SE3Quat infTi_invTij = Ti.inverse() * invTij;
 
            _jacobianOplusXi = invTj_Tij.adj();
            _jacobianOplusXj = -infTi_invTij.adj();
        }
    };
 
}