types_seven_dof_expmap.h

#pragma once
#include "base_vertex.h"
#include "base_binary_edge.h"
#include "OptimizableTypes.h"
#include "sim3.h"
namespace NDEVR
{
    class EdgeSim3 : public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>
    {
    public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        EdgeSim3()
            : BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>()
        {}
        void computeError()
        {
            Sim3 error_ = _measurement * m_a->estimate() * m_b->estimate().inverse();
            _error = error_.log();
            //if (!_error.array().isFinite().all())
                //throw "error";
        }
    };
}