warpUtils.h

#ifndef MTF_WARP_UTILS_H
#define MTF_WARP_UTILS_H

#include "mtf/Macros/common.h"

_MTF_BEGIN_NAMESPACE

namespace utils{
    template<typename T1, typename T2>
    inline void homogenize(const T1 &dehom_mat, T2 &hom_mat){
        assert(hom_mat.cols() == dehom_mat.cols());
        hom_mat.topRows(2) = dehom_mat;
        hom_mat.row(2).fill(1);
    }

    template<typename T1, typename T2>
    inline void dehomogenize(const T1 &hom_mat, T2 &dehom_mat){
        assert(hom_mat.cols() == dehom_mat.cols());
        dehom_mat = hom_mat.topRows(2);
        dehom_mat = dehom_mat.array().rowwise() / hom_mat.array().row(2);
    }

    // returning versions
    inline HomPtsT homogenize(const PtsT &dehom_mat){
        HomPtsT hom_mat;
        hom_mat.resize(NoChange, dehom_mat.cols());
        homogenize(dehom_mat, hom_mat);
        return hom_mat;
    }
    inline PtsT dehomogenize(const HomPtsT &hom_mat){
        PtsT dehom_mat;
        dehom_mat.resize(NoChange, hom_mat.cols());
        dehomogenize(hom_mat, dehom_mat);
        return dehom_mat;
    }
    inline ProjWarpT getTranslationMatrix(double tx, double ty){
        ProjWarpT trans_mat = ProjWarpT::Identity();
        trans_mat(0, 2) = tx;
        trans_mat(1, 2) = ty;
        return trans_mat;
    }
    inline ProjWarpT getRotationMatrix(double theta){
        ProjWarpT rot_mat = ProjWarpT::Identity();
        double cos_theta = cos(theta);
        double sin_theta = sin(theta);
        rot_mat(0, 0) = cos_theta;
        rot_mat(0, 1) = -sin_theta;
        rot_mat(1, 0) = sin_theta;
        rot_mat(1, 1) = cos_theta;
        return rot_mat;
    }
    inline ProjWarpT getScalingMatrix(double s){
        ProjWarpT scale_mat = ProjWarpT::Identity();
        scale_mat(0, 0) += s;
        scale_mat(1, 1) += s;
        return scale_mat;
    }
    inline ProjWarpT getShearingMatrix(double a, double b){
        ProjWarpT shear_mat = ProjWarpT::Identity();
        shear_mat(0, 0) += a;
        shear_mat(0, 1) = b;
        return shear_mat;
    }

    ProjWarpT computeHomographyDLT(const PtsT &in_pts, const PtsT &out_pts, int n_pix);
    ProjWarpT computeHomographyDLT(const CornersT &in_corners, const CornersT &out_corners);
    ProjWarpT computeHomographyNDLT(const CornersT &in_corners, const CornersT &out_corners);
    ProjWarpT computeAffineDLT(const CornersT &in_corners, const CornersT &out_corners);
    ProjWarpT computeAffineDLT(const PtsT &in_pts, const PtsT &out_pts);
    ProjWarpT computeAffineDLT(const Matrix23d &in_pts, const Matrix23d &out_pts);
    ProjWarpT computeAffineNDLT(const CornersT &in_corners, const CornersT &out_corners);
    ProjWarpT computeASRTDLT(const CornersT &in_corners, const CornersT &out_corners);
    ProjWarpT computeASRTDLT(const PtsT &in_pts, const PtsT &out_pts);
    ProjWarpT computeSimilitudeDLT(const CornersT &in_corners, const CornersT &out_corners);
    ProjWarpT computeSimilitudeNDLT(const CornersT &in_corners, const CornersT &out_corners);
    ProjWarpT computeSimilitudeDLT(const PtsT &in_pts, const PtsT &out_pts);
    ProjWarpT computeSimilitudeNDLT(const PtsT &in_pts, const PtsT &out_pts);
    ProjWarpT computeASTDLT(const CornersT &in_corners, const CornersT &out_corners);
    ProjWarpT computeASTDLT(const PtsT &in_corners, const PtsT &out_corners);
    Vector3d computeIsometryDLT(const PtsT &in_pts, const PtsT &out_pts);
    Vector3d computeIsometryDLT(const CornersT &in_pts, const CornersT &out_pts);
    ProjWarpT computeISTDLT(const CornersT &in_corners, const CornersT &out_corners);
    ProjWarpT computeISTDLT(const PtsT &in_corners, const PtsT &out_corners);

    void decomposeHomographyForward(ProjWarpT &affine_mat, ProjWarpT &proj_mat, const ProjWarpT &hom_mat);
    void decomposeHomographyInverse(ProjWarpT &affine_mat, ProjWarpT &proj_mat, const ProjWarpT &hom_mat);
    void decomposeAffineForward(Vector6d &affine_params, const ProjWarpT &affine_mat);
    void decomposeAffineInverse(Vector6d &affine_params, const ProjWarpT &affine_mat);
    void decomposeAffineInverse(ProjWarpT &trans_mat, ProjWarpT &rot_mat,
        ProjWarpT &scale_mat, ProjWarpT &shear_mat, const ProjWarpT &affine_mat);
    void decomposeAffineForward(ProjWarpT &trans_mat, ProjWarpT &rot_mat,
        ProjWarpT &scale_mat, ProjWarpT &shear_mat, const ProjWarpT &affine_mat);


    void normalizePts(CornersT &norm_pts, ProjWarpT &inv_norm_mat, const CornersT &pts);
    void normalizePts(PtsT &norm_pts, ProjWarpT &inv_norm_mat, const PtsT &pts);
    void getNormUnitSquarePts(PtsT &std_grid, CornersT &basis_corners,
        int resx, int resy, double min_x = -0.5, double min_y = -0.5,
        double max_x = 0.5, double max_y = 0.5);
    void getPtsFromCorners(PtsT &pts, const CornersT &corners,
        int resx, int resy);
    PtsT getPtsFromCorners(const CornersT &corners,
        int resx, int resy);
    PtsT getPtsFromCorners(const cv::Mat &corners_cv,int resx, int resy);
    void getPtsFromCorners(PtsT &pts, const CornersT &corners,
        const PtsT basis_pts, const CornersT &basis_corners);

    void getBoundingPts(cv::Mat &bounding_pts, const PtsT &grid_pts, int res_x, int res_y);
    bool isInsideRegion(const cv::Mat &verices, double testx, double testy);
    void getBilinearPts(cv::Vec4i &neigh_pts_id, std::vector<cv::Vec2d> &neigh_pts_dist,
        double x, double y, const mtf::PtsT &grid_pts, int n_pts);
    int getNearestPt(double x, double y, const mtf::PtsT &grid_pts, int n_pts);

    MatrixX2d computeTPS(const CornersT &in_corners, const CornersT &out_corners);
    void applyTPS(PtsT &out_pts, const PtsT &in_pts,
        const PtsT &control_pts, const MatrixX2d &tps_params);
    inline double tps(double r){ return r*r*log(r); }

    //void getSupportPoints(ProjWarpT &warp, PtsT &pts, HomPtsT &pts_hm, 
    //  const CornersT &corners,    const CornersT &basis_corners, 
    //  const HomPtsT &basis_pts_hm);   
}
_MTF_END_NAMESPACE
#endif