histUtils.h

#ifndef MTF_HIST_UTILS_H
#define MTF_HIST_UTILS_H

//------ Functions for computing histograms, joint histograms and their derivatives------//

#include "mtf/Macros/common.h"
#include "mtf/Utilities/excpUtils.h"

// precomputed constants for BSpl function
#define _1_BY_3 0.33333333333
#define _2_BY_3 0.66666666666
#define _1_BY_6 0.16666666666
#define _eig_set_zero(eig_mat, scalar_type) eig_mat.setZero()
//#define _eig_set_zero(eig_mat, scalar_type) \
//  memset(eig_mat.data(), 0, eig_mat.size()*sizeof(scalar_type))
    

_MTF_BEGIN_NAMESPACE

namespace utils{

    inline void getLog(MatrixXd &eig_log,
        const MatrixXd &eig_mat, int n_rows, int n_cols){
            for(int i = 0; i < n_rows; i++) {
                for(int j = 0; j < n_cols; j++) {
                    if(eig_mat(i, j)<=0){
                        eig_log(i, j) = 0;
                    }else{
                        eig_log(i, j) = log(eig_mat(i, j));
                    }
                }
            }
    }

    inline void getLog(VectorXd &eig_log,
        const VectorXd &eig_vec, int vec_size){
            for(int i = 0; i < vec_size; i++) {
                if(eig_vec(i)<=0){
                    eig_log(i) = 0;
                }else{
                    eig_log(i) = log(eig_vec(i));
                }
            }
    }

    inline double cumBSpl3(double x){
        //fprintf(stdout, "x=%f\t", x);
        if(x<=-2){
            return 1;
        } else if((x>-2) && (x <= -1)){
            double temp = 2 + x;
            double temp2 = temp*temp;
            return 1.0 - (temp2*temp2) / 24;
        } else if((x>-1) && (x <= 0)){
            return 0.5 + x*(x*x*(1.0/3.0 + x / 8) - 2.0 / 3.0);
        } else if((x>0) && (x <= 1)){
            return 0.5 + x*(x*x*(1.0 / 3.0 - x / 8) - 2.0 / 3.0);
        } else if((x>1) && (x<2)){
            double temp = 2-x;
            double temp2 = temp*temp;
            return (temp2*temp2) / 24;
        }
        return 0;
    }

    template<int bspl_id>
    inline void cumBSpl3WithGradFast(double &val, double &diff, double x){
        printf("bspl_id: %d\n", bspl_id);
        throw utils::InvalidArgument("cumBSpl3WithGradFast :: Invalid bspl_id specified");
    }
    template<>
    inline void cumBSpl3WithGradFast<0>(double &val, double &diff, double x){
        double temp = 2 + x;
        diff = -(temp*temp*temp) / 6;
        val = 1 + (diff * temp) / 4;
    }
    template<>
    inline void cumBSpl3WithGradFast<1>(double &val, double &diff, double x){
        double x2 = x*x;
        val = 0.5 + x*(x2*(_1_BY_3 + x / 8) - _2_BY_3);
        diff = x2*(1 + x / 2) - _2_BY_3;
    }
    template<>
    inline void cumBSpl3WithGradFast<2>(double &val, double &diff, double x){
        double x2 = x*x;
        val = 0.5 + x*(x2*(_1_BY_3 - x / 8) - _2_BY_3);
        diff = x2*(1 - x / 2) - _2_BY_3;
    }
    template<>
    inline void cumBSpl3WithGradFast<3>(double &val, double &diff, double x){
        double temp = x - 2;
        diff = (temp*temp*temp) / 6;
        val = (diff * temp) / 4;
    }

    inline void cumBSpl3WithGrad(double &val, double &diff, double x){
        if(x <= -2){
            diff = 0;
            val = 1;
        } else if((x>-2) && (x <= -1)){
            double temp = 2 + x;
            diff = -(temp*temp*temp) / 6;
            val = 1 + (diff * temp) / 4;
        } else if((x>-1) && (x <= 0)){
            double x2 = x*x;
            val = 0.5 + x*(x2*(_1_BY_3 + x / 8) - _2_BY_3);
            diff = x2*(1 + x / 2) - _2_BY_3;
        } else if((x>0) && (x <= 1)){
            double x2 = x*x;
            val = 0.5 + x*(x2*(_1_BY_3 - x / 8) - _2_BY_3);
            diff = x2*(1 - x/2) - _2_BY_3;
        } else if((x>1) && (x<2)){
            double temp = x-2;
            diff = (temp*temp*temp) / 6;
            val = (diff * temp) / 4;
        } else{
            val = diff = 0;
        }
    }

    template<int bspl_id>
    inline double cumBSpl3HessFast(double x){
        printf("bspl_id: %d\n", bspl_id);
        throw utils::InvalidArgument("cumBSpl3HessFast :: Invalid bspl_id specified");
    }
    template<>
    inline double cumBSpl3HessFast<0>(double x){
        double temp = 2 + x;
        return -(temp*temp) / 2;
    }
    template<>
    inline double cumBSpl3HessFast<1>(double x){
        return x*(3 * x + 4) / 2;
    }
    template<>
    inline double cumBSpl3HessFast<2>(double x){
        return -x*(3 * x - 4) / 2;
    }
    template<>
    inline double cumBSpl3HessFast<3>(double x){
        double temp = 2 - x;
        return (temp * temp) / 2;
    }

    inline double cumBSpl3Hess(double x){
        //fprintf(stdout, "x=%f\t", x);
        if((x>-2) && (x <= -1)){
            double temp = 2 + x;
            return -(temp*temp) / 2;
        } else if((x>-1) && (x <= 0)){
            return x*(3 * x + 4) / 2;
        } else if((x>0) && (x <= 1)){
            return -x*(3 * x - 4) / 2;
        } else if((x>1) && (x<2)){
            double temp = 2 - x;
            return (temp * temp) / 2;
        }
        return 0;
    }

    inline double bSpl3(double x){
        //fprintf(stdout, "x=%f\t", x);
        if((x>-2) && (x<=-1)){
            double temp = 2 + x;
            return (temp*temp*temp) / 6;
        } else if((x>-1) && (x<=0)){
            return (4 - 3*x*x*(2+x)) / 6;
        } else if((x>0) && (x<=1)){
            return (4 - 3*x*x*(2-x)) / 6;
        } else if((x>1) && (x<2)){
            double temp = 2-x;
            return (temp*temp*temp) / 6;
        }
        return 0;
    }
    template<int bspl_id>
    void bSpl3WithGradFast(double &val, double &diff, double x){    
        printf("bspl_id: %d\n", bspl_id);
        throw utils::InvalidArgument("bSpl3WithGradFast :: Invalid bspl_id specified");
    }
    template<>
    inline void bSpl3WithGradFast<0>(double &val, double &diff, double x){
        double temp = 2 + x;
        diff = (temp * temp) / 2;
        val = (diff * temp) / 3;
    }
    template<>
    inline void bSpl3WithGradFast<1>(double &val, double &diff, double x){
        double temp = x / 2;
        val = _2_BY_3 - x*x*(1 + temp);
        diff = -x * (temp + x + 2);
    }
    template<>
    inline void bSpl3WithGradFast<2>(double &val, double &diff, double x){
        double temp = x / 2;
        val = _2_BY_3 - x*x*(1 - temp);
        diff = x * (temp + x - 2);
    }
    template<>
    inline void bSpl3WithGradFast<3>(double &val, double &diff, double x){
        double temp = 2 - x;
        diff = -(temp * temp) / 2;
        val = -(diff * temp) / 3;
    }

    inline void bSpl3WithGrad(double &val, double &diff, double x){
        if((x>-2) && (x<=-1)){
            double temp = 2 + x;            
            diff = (temp * temp) / 2;
            val = (diff * temp) / 3;
        } else if((x>-1) && (x<=0)){
            double temp = x / 2;
            val =_2_BY_3 - x*x*(1 + temp);
            diff = -x * (temp + x + 2);
        } else if((x>0) && (x<=1)){
            double temp = x / 2;
            val =_2_BY_3 - x*x*(1 - temp);
            diff = x * (temp + x -2);
        } else if((x>1) && (x<2)){
            double temp = 2 - x;
            diff = - (temp * temp) / 2;
            val = - (diff * temp) / 3 ;
        }
    }
    inline double bSpl3Grad(double x){
        //fprintf(stdout, "x=%f\t", x);
        if((x>-2) && (x<=-1)){
            double temp = 2 + x;
            return (temp*temp) / 2;
        } else if((x>-1) && (x<=0)){
            return -x*(3*x + 4) / 2;
        } else if((x>0) && (x<=1)){
            return x*(3*x - 4) / 2;
        } else if((x>1) && (x<2)){
            double temp = 2 - x;
            return -(temp * temp) / 2;
        }
        return 0;
    }

    template<int bspl_id>
    inline double bSpl3HessFast(double x){
        printf("bspl_id: %d\n", bspl_id);
        throw utils::InvalidArgument("bSpl3HessFast :: Invalid bspl_id specified");
    }
    template<>
    inline double bSpl3HessFast<0>(double x){
        return 2 + x;
    }
    template<>
    inline double bSpl3HessFast<1>(double x){
        return -(3 * x + 2);
    }
    template<>
    inline double bSpl3HessFast<2>(double x){
        return 3 * x - 2;
    }
    template<>
    inline double bSpl3HessFast<3>(double x){
        return 2 - x;
    }

    inline double bSpl3Hess(double x){
        //fprintf(stdout, "x=%f\t", x);
        if((x>-2) && (x <= -1)){
            return 2 + x;
        } else if((x > -1) && (x <= 0)){
            return -(3 * x + 2);
        } else if((x > 0) && (x <= 1)){
            return 3 * x - 2;
        } else if((x > 1) && (x < 2)){
            return 2 - x;
        }
        return 0;
    }


    typedef void(*BSpl3WithGradPtr)(double &, double &, double);
    inline void bSpl3WithGrad0(double &val, double &diff, double x){
        double temp = 2 + x;
        diff = (temp * temp) / 2;
        val = (diff * temp) / 3;
    }
    inline void bSpl3WithGrad1(double &val, double &diff, double x){
        double temp = 2 + x;
        diff = (temp * temp) / 2;
        val = (diff * temp) / 3;
    }
    inline void bSpl3WithGrad2(double &val, double &diff, double x){
        double temp = x / 2;
        val = _2_BY_3 - x*x*(1 + temp);
        diff = -x * (temp + x + 2);
    }
    inline void bSpl3WithGrad3(double &val, double &diff, double x){
        double temp = 2 - x;
        diff = -(temp * temp) / 2;
        val = -(diff * temp) / 3;
    }
    void getDiracHist(VectorXd &hist, VectorXi &pix_vals_int,
        const VectorXd &pix_vals, double hist_pre_seed, int n_pix);
    void getDiracHist(VectorXi &hist, const VectorXi &vals, int n_vals);
    void getDiracJointHist(MatrixXd &joint_hist, VectorXd &hist1,
        const VectorXd &pix_vals1, const VectorXi &pix_vals2_int,
        double hist_pre_seed, double joint_hist_pre_seed, int n_pix, int n_bins);
    void getDiracJointHist(MatrixXd &joint_hist, VectorXd &hist1, VectorXd &hist2,
        const VectorXd &pix_vals1, const VectorXd &pix_vals2,
        double hist_pre_seed, double joint_hist_pre_seed, int n_pix, int n_bins);
    void getDiracJointHist(MatrixXd &joint_hist, VectorXd &hist1, VectorXd &hist2,
        const MatrixXdMr &patch1, const MatrixXdMr &patch2,
        int start_x, int end_x, int start_y, int end_y,
        double hist_pre_seed, double joint_hist_pre_seed, int n_pix, int n_bins);
    void getBilinearHist(
        // output arguments
        VectorXd &hist, VectorXi &pix_vals_int,
        // input arguments
        const VectorXd &pix_vals, double hist_pre_seed, int n_pix
        );
    void getBilinearJointHist(
        // output arguments
        MatrixXd &joint_hist, VectorXd &hist1, VectorXd &hist2,
        // input arguments
        const VectorXd &pix_vals1, const VectorXd &pix_vals2,
        double hist_pre_seed, double joint_hist_pre_seed, int n_pix, int n_bins
        );
    void getBSplHist(VectorXd &hist, MatrixXd &hist_mat, MatrixX2i &bspl_ids, 
        const VectorXd &pix_vals, const MatrixX2i &std_bspl_ids, 
        double pre_seed, int n_pix);

    void getBSplJointHist(MatrixXd &joint_hist, VectorXd &hist1, VectorXd &hist2,
        const VectorXd &pix_vals1, const VectorXd &pix_vals2,
        double hist_pre_seed, double joint_hist_pre_seed, int n_pix, int n_bins);
    void getBSplJointHist(MatrixXd &joint_hist, VectorXd &hist1, VectorXd &hist2,
        MatrixXd &hist1_mat, MatrixXd &hist2_mat,
        MatrixX2i &bspl_ids1, MatrixX2i &bspl_ids2,
        const VectorXd &pix_vals1, const VectorXd &pix_vals2,
        const MatrixX2i &std_bspl_ids, double hist_pre_seed,
        double joint_hist_pre_seed, int n_pix);

    void getBSplJointHist(MatrixXd &joint_hist, VectorXd &hist1, 
        MatrixXd &hist1_mat, MatrixX2i &bspl_ids1, 
        const VectorXd &pix_vals1, const MatrixX2i &bspl_ids2, 
        const MatrixXd &hist2_mat, const MatrixX2i &std_bspl_ids,
        double hist_pre_seed, double joint_hist_pre_seed, int n_pix);

    // computes the joint histogram of an image with itself
    void  getBSplJointHist(MatrixXd &joint_hist, 
        const MatrixXd &hist_mat, const MatrixX2i &bspl_ids, 
        double pre_seed, int n_pix);
    
    // which are therefore required to be between 0 and n_bins-1
    void getBSplHistGrad(
        MatrixXd &hist_grad,
        // input arguments
        const VectorXd &pix_vals, const MatrixX2i &bspl_ids, int n_pix
        );
    void getBSplHistHess(
        MatrixXd &hist_hess,
        // input arguments
        const VectorXd &pix_vals, const MatrixX2i &bspline_ids,
        int n_pix, double hist_norm_mult
        );

    void getBSplJointHistGrad(MatrixXd &joint_hist_grad, MatrixXd &hist1_grad,
        const VectorXd &pix_vals1, const MatrixXd &hist2_mat, 
        const MatrixX2i &bspl_ids1, const MatrixX2i &bspl_ids2, 
        const MatrixXi &linear_idx, int n_pix);
    void getBSplJointHistGrad(MatrixXd &joint_hist_grad, 
        const MatrixXd &hist1_grad, const MatrixXd &hist2_mat, 
        const MatrixX2i &bspl_ids1, const MatrixX2i &bspl_ids2, 
        const MatrixXi &linear_idx, int n_pix);

    void getBSplHistWithGrad(

        VectorXd &hist, MatrixXd &hist_mat, 
        MatrixXd &hist_grad, MatrixX2i &bspl_ids, 

        const VectorXd &pix_vals, const MatrixX2i &std_bspl_ids,
        double pre_seed, int n_pix, double hist_norm_mult
        );
    void getBSplJointHistWithGrad(
        
        MatrixXd &joint_hist, VectorXd &hist1, 
        MatrixXd &hist1_mat, MatrixXd &hist1_grad, 
        MatrixXd &joint_hist_grad, MatrixX2i &bspl_ids1, 

        const VectorXd &pix_vals1, const MatrixX2i &bspl_ids2, 
        const MatrixXd &hist2_mat, const MatrixX2i &std_bspl_ids, const MatrixXi &linear_idx, 
        double hist_pre_seed, double joint_hist_pre_seed, int n_pix, 
        double hist_norm_mult=1
        );

    void getBSplJointHistWithGradFast(
        // output arguments
        MatrixXd &joint_hist, VectorXd &hist1,
        MatrixXd &hist1_mat, MatrixXd &hist1_grad,
        MatrixXd &joint_hist_grad, MatrixX2i &bspl_ids1,
        // input arguments
        const VectorXd &pix_vals1, const MatrixX2i &bspl_ids2,
        const MatrixXd &hist2_mat, const MatrixX2i &std_bspl_ids,
        const MatrixXi &linear_idx, double hist_pre_seed,
        double joint_hist_pre_seed, int n_pix, double hist_norm_mult
        );


    void getEntropyVec(VectorXd &entropy_vec, const VectorXd &norm_hist, int n_bins);

    void getEntropyVec( VectorXd &entropy_vec, 
        const VectorXd &norm_hist, const VectorXd &norm_hist_log, int n_bins
        );

    void getMIVec(VectorXd &mi_vec, VectorXd &log_hist_ratio,
        const MatrixXd &joint_hist, const VectorXd &norm_hist1, const VectorXd &norm_hist2,
        const MatrixXi &linear_idx, int n_bins);

    void getMIVec(VectorXd &mi_vec, VectorXd &log_hist_ratio, 
        const MatrixXd &joint_hist, const MatrixXd &joint_hist_log, 
        const VectorXd &hist1_log, const VectorXd &hist2_log, const MatrixXi &linear_idx, int n_bins);

    void getMIVecGrad(MatrixXd &mi_vec_grad,
        const MatrixXd &joint_hist_grad, const VectorXd &log_hist_ratio, 
        const MatrixXd &hist1_grad_ratio, const MatrixXd &joint_hist, const MatrixX2i &bspl_ids, 
        const MatrixXi &linear_idx, int n_bins, int n_pix);


    //---------------------------------------------------------------------------------------------------//
    //------------------------ Functions for Cumulative Cubic BSpline Histograms ------------------------//
    //---------------------------------------------------------------------------------------------------//

    void getCumBSplHistWithGrad(
        // output arguments
        VectorXd &cum_hist, MatrixXd &cum_hist_mat, 
        MatrixXd &cum_hist_grad, MatrixX2i &bspline_ids, 
        // input arguments
        const VectorXd &pix_vals, const MatrixX2i &std_bspline_ids,
        double pre_seed, int n_bins, 
        int n_pix, double hist_norm_mult
        );
    // cumulative joint histogram from precomputed cumulative and marginal histogram matrices 
    // which may be either from the same image or from two different images
    void getCumBSplJointHistWithGrad(
        // output arguments
        MatrixXd &cum_joint_hist, MatrixXd &cum_joint_hist_grad,
        // input arguments
        const MatrixXd &cum_hist_mat, const MatrixXd &hist_mat,
        const MatrixXd &cum_hist_grad, const MatrixX2i &cum_bspl_ids,
        const MatrixX2i &bspl_ids, const MatrixXi &linear_idx,
        double pre_seed, int n_bins, int n_pix
        );
    // computes the marginal histogram and its matrix too
    void getCumBSplJointHistWithGrad(
        // output arguments
        MatrixXd &cum_joint_hist, MatrixXd &cum_joint_hist_grad,
        VectorXd &hist, MatrixXd &hist_mat,
        // input arguments
        const VectorXd &pix_vals, const MatrixXd &cum_hist_mat,
        const MatrixXd &cum_hist_grad, const MatrixX2i &cum_bspl_ids, 
        const MatrixX2i &bspline_ids, const MatrixXi &linear_idx, 
        double pre_seed, double joint_pre_seed, int n_bins, int n_pix
        );
    void getCumBSplJointHistWithGrad(
        MatrixXd &cum_joint_hist, VectorXd &cum_hist,
        MatrixXd &cum_hist_mat, MatrixXd &cum_hist_grad,
        MatrixXd &cum_joint_hist_grad, MatrixX2i &bspline_ids1,
        // input arguments
        const VectorXd &pix_vals1, const MatrixX2i &bspline_ids2,
        const MatrixXd &hist2_mat, const MatrixX2i &std_bspline_ids, const MatrixXi &linear_idx,
        double hist_pre_seed, double joint_hist_pre_seed,
        int n_bins, int n_pix, double hist_norm_mult
        );
    // derivative of the cumulative joint histogram w.r.t. 
    // pixels used in the non cumulative component
    void getInvCumBSplJointHistGrad(
        // output arguments
        MatrixXd &cum_joint_hist_grad,
        // input arguments
        const MatrixXd &hist_grad, const MatrixXd &cum_hist_mat,
        const MatrixX2i &cum_bspl_ids, const MatrixX2i &bspl_ids, 
        const MatrixXi &linear_idx, int n_bins, int n_pix
        );
    void getCumBSplHistHess(
        // output arguments
        MatrixXd &cum_hist_hess,
        // input arguments
        const VectorXd &pix_vals,
        const MatrixX2i &bspline_ids,
        int n_pix, double hist_norm_mult
        );

    // histogram validation functions for debugging

    void validateJointHist(const MatrixXd &joint_hist, 
        const VectorXd &hist1, const VectorXd &hist2);

    void validateJointHistGrad(const MatrixXd &joint_hist_grad, 
        const MatrixXd &hist1_grad, const MatrixXd &hist2_grad, 
        const MatrixXi &linear_idx, int n_bins, int n_pix);
}
_MTF_END_NAMESPACE
#endif