LieIsometry.h

#ifndef MTF_LIE_ISOMETRY_H
#define MTF_LIE_ISOMETRY_H

#define VALIDATE_LIE_ISO_WARP(warp) \
    assert(warp.determinant() == 1.0);

#define LISO_DEBUG_MODE 0
#define LISO_INIT_AS_BASIS 0

#include "ProjectiveBase.h"

_MTF_BEGIN_NAMESPACE

struct LieIsometryParams{
    bool debug_mode;
    bool init_as_basis;

    LieIsometryParams( bool _debug_mode, bool _init_as_basis) : c(_c),
        debug_mode(_debug_mode), init_as_basis(_init_as_basis){}
    LieIsometryParams(LieIsometryParams *params = nullptr) : 
        debug_mode(LISO_DEBUG_MODE),
        init_as_basis(LISO_INIT_AS_BASIS){
        if(params){
            c = params->c;
            debug_mode = params->debug_mode;
            init_as_basis = params->init_as_basis;
        }
    }
};

class LieIsometry : public ProjectiveBase{
public:
    typedef LieIsometryParams ParamType;

    LieIsometry( LieIsometryParams *params_in = nullptr);

    void compositionalUpdate(const VectorXd& state_update) override;

    void cmptInitPixJacobian(MatrixXd &jacobian_prod, const PixGradT &pix_jacobian) override;
    void cmptPixJacobian(MatrixXd &jacobian_prod, const PixGradT &pix_jacobian) override;
    void cmptApproxPixJacobian(MatrixXd &jacobian_prod,
        const PixGradT &pix_jacobian) override;

    void estimateWarpFromCorners(VectorXd &state_update, const Matrix24d &in_corners,
        const Matrix24d &out_corners) override;

    void estimateWarpFromPts(VectorXd &state_update, vector<uchar> &mask,
        const vector<cv::Point2f> &in_pts, const vector<cv::Point2f> &out_pts,
        int estimation_method, double ransac_reproj_thresh) override;

    void invertState(VectorXd& inv_state, const VectorXd& state) override;

    void updateGradPts(double grad_eps) override;
    void updateHessPts(double hess_eps) override;

    void applyWarpToCorners(Matrix24d &warped_corners, const Matrix24d &orig_corners,
        const VectorXd &state_update) override;
    void applyWarpToPts(Matrix2Xd &warped_pts, const Matrix2Xd &orig_pts,
        const VectorXd &state_update) override;

    void getWarpFromState(Matrix3d &warp_mat, const VectorXd& ssm_state) override;
    void getStateFromWarp(VectorXd &state_vec, const Matrix3d& warp_mat) override;

protected:
    ParamType params;

    Matrix3d lieAlgBasis[3];
    RowVector3d zero_vec;
    Matrix3d lie_alg_mat;
};

_MTF_END_NAMESPACE

#endif