AppearanceModel.h

#ifndef MTF_APPEARANCE_MODEL_H
#define MTF_APPEARANCE_MODEL_H

#define am_func_not_implemeted(func_name) \
    throw mtf::utils::FunctonNotImplemented(cv::format("%s :: %s :: Not implemented Yet", name.c_str(), #func_name))

#include "ImageBase.h"
#include "AMParams.h"
#include "mtf/Utilities/excpUtils.h"
#include <memory>

_MTF_BEGIN_NAMESPACE

struct AMStatus : ImgStatus{
    bool similarity, grad, hess;
    bool sampler;

    AMStatus() : ImgStatus(){ clear(); }

    void set(){
        setPixState();
        similarity = grad = hess = true;
        sampler = true;
    }
    void clear(){
        clearPixState();
        similarity = grad = hess = false;
        sampler = false;
    }
};

struct AMDist{
    typedef double ElementType;
    typedef double ResultType;
    const string name;
    AMDist(const string &_name) : name(_name){}
    virtual ~AMDist() {}
    virtual double operator()(const double* a, const double* b,
        size_t dist_size, double worst_dist = -1) const {
        am_func_not_implemeted(distance_functor);
    }
};

class AppearanceModel : public ImageBase{
public:
    string name;

    explicit AppearanceModel(const AMParams *params = nullptr, 
        const int _n_channels = 1) :
        ImageBase(params, _n_channels), f(0), state_size(0),
        first_iter(false), spi_mask(nullptr){}
    virtual ~AppearanceModel(){}

    virtual int getStateSize() const{ return 0; }
    virtual double getSimilarity() const{ return f; }
    virtual double getLikelihood() const{ am_func_not_implemeted(getLikelihood); }
    virtual const VectorXd& getState(){ return p_am; }
    virtual const RowVectorXd& getInitGrad() const{ return df_dI0; }
    virtual const RowVectorXd& getCurrGrad() const{ return df_dIt; }

    virtual void setSimilarity(double _similarity){ f = _similarity; }
    virtual void setInitGrad(const RowVectorXd &df_dI){ df_dI0 = df_dI; }
    virtual void setCurrGrad(const RowVectorXd &df_dI){ df_dIt = df_dI; }


    virtual void initializeSimilarity(){ am_func_not_implemeted(initializeSimilarity); }
    virtual void initializeGrad() { am_func_not_implemeted(initializeGrad); }
    virtual void initializeHess(){ am_func_not_implemeted(initializeHess); }

    virtual void reinitialize(){
        if(is_initialized.similarity){ initializeSimilarity(); }
        if(is_initialized.grad){ initializeGrad(); }
        if(is_initialized.hess){ initializeHess(); }
    }

    virtual void updateSimilarity(bool prereq_only = true){ am_func_not_implemeted(updateSimilarity); }
    virtual void updateState(const VectorXd& state_update){}
    virtual void invertState(VectorXd& inv_p, const VectorXd& p){}
    virtual void updateInitGrad(){ am_func_not_implemeted(updateInitGrad); }
    virtual void updateCurrGrad() { am_func_not_implemeted(updateCurrGrad); }
    virtual void updateParamGrad() { am_func_not_implemeted(updateParamGrad); }

    //virtual void updateInitHess() { am_func_not_implemeted(updateInitHess); }
    //virtual void updateCurrHess() { am_func_not_implemeted(updateCurrHess); }

    virtual void cmptInitJacobian(RowVectorXd &df_dp, const MatrixXd &dI0_dpssm){
        assert(dI0_dpssm.rows() == n_pix && dI0_dpssm.cols() == df_dp.cols());
        df_dp.noalias() = df_dI0 * dI0_dpssm;
    }
    virtual void cmptCurrJacobian(RowVectorXd &df_dp, const MatrixXd &dIt_dpssm){
        assert(dIt_dpssm.rows() == n_pix && dIt_dpssm.cols() == df_dp.cols());
        df_dp.noalias() = df_dIt * dIt_dpssm;
    }
    virtual void cmptDifferenceOfJacobians(RowVectorXd &df_dp_diff,
        const MatrixXd &dI0_dpssm, const MatrixXd &dIt_dpssm){
        df_dp_diff.noalias() = (df_dIt * dIt_dpssm) - (df_dI0 * dI0_dpssm);
    }

    virtual void cmptInitHessian(MatrixXd &d2f_dp2, const MatrixXd &dI0_dpssm){
        am_func_not_implemeted(cmptInitHessian(first order));
    }
    virtual void cmptCurrHessian(MatrixXd &d2f_dp2, const MatrixXd &dIt_dpssm){
        am_func_not_implemeted(cmptCurrHessian(first order));
    }
    virtual void cmptSelfHessian(MatrixXd &d2f_dp2, const MatrixXd &dIt_dpssm) {
        am_func_not_implemeted(cmptSelfHessian(first order));
    }

    virtual void cmptInitHessian(MatrixXd &d2f_dp2, const MatrixXd &dI0_dpssm, const MatrixXd &d2I0_dpssm2){
        am_func_not_implemeted(cmptInitHessian(second order));
    }
    virtual void cmptCurrHessian(MatrixXd &d2f_dp2, const MatrixXd &dIt_dpssm,
        const MatrixXd &d2It_dpssm2){
        am_func_not_implemeted(cmptCurrHessian(second order));
    }
    virtual void cmptSelfHessian(MatrixXd &d2f_dp2, const MatrixXd &dIt_dpssm,
        const MatrixXd &d2I_dpssm2) {
        am_func_not_implemeted(cmptSelfHessian(second order));
    }

    virtual void cmptSumOfHessians(MatrixXd &d2f_dp2_sum,
        const MatrixXd &dI0_dpssm, const MatrixXd &dIt_dpssm){
        int pssm_size = dIt_dpssm.cols();
        assert(d2f_dp2_sum.rows() == pssm_size && d2f_dp2_sum.cols() == pssm_size);
        MatrixXd d2f_dp2_0(pssm_size, pssm_size);
        cmptInitHessian(d2f_dp2_0, dI0_dpssm);

        MatrixXd d2f_dp2_t(pssm_size, pssm_size);
        cmptCurrHessian(d2f_dp2_t, dIt_dpssm);

        d2f_dp2_sum = d2f_dp2_0 + d2f_dp2_t;
    }
    virtual void cmptSumOfHessians(MatrixXd &d2f_dp2_sum,
        const MatrixXd &dI0_dpssm, const MatrixXd &dIt_dpssm,
        const MatrixXd &d2I0_dpssm2, const MatrixXd &d2It_dpssm2){
        int pssm_size = dIt_dpssm.cols();
        assert(d2f_dp2_sum.rows() == pssm_size && d2f_dp2_sum.cols() == pssm_size);
        MatrixXd d2f_dp2_0(pssm_size, pssm_size);
        cmptInitHessian(d2f_dp2_0, dI0_dpssm, d2I0_dpssm2);

        MatrixXd d2f_dp2_t(pssm_size, pssm_size);
        cmptCurrHessian(d2f_dp2_t, dIt_dpssm, d2It_dpssm2);

        d2f_dp2_sum = d2f_dp2_0 + d2f_dp2_t;
    }

    virtual void estimateOpticalFlow(std::vector<cv::Point2f> &curr_pts,
        const cv::Mat &prev_img, const std::vector<cv::Point2f> &prev_pts,
        const cv::Size &win_size, unsigned int n_pts, int max_iters,
        double term_eps, bool const_grad = true) const{
        am_func_not_implemeted(estimateOpticalFlow);
    }

    virtual void setSPIMask(const bool *_spi_mask){ spi_mask = _spi_mask; }
    virtual const bool* getSPIMask() const{ return spi_mask; }
    virtual void clearSPIMask(){ spi_mask = nullptr; }

    virtual bool supportsSPI() const { return false; }

    virtual void setFirstIter(){ first_iter = true; }
    virtual void clearFirstIter(){ first_iter = false; }

    ImgStatus* isInitialized() override { return &is_initialized; }
    virtual void setInitStatus(){ is_initialized.set(); }
    virtual void clearInitStatus(){ is_initialized.clear(); }

    virtual bool isSymmetrical() const{ return true; }

    virtual void updateModel(const PtsT& curr_pts){ am_func_not_implemeted(updateModel); }

    // ------------------------ Distance Feature ------------------------ //

    virtual void initializeDistFeat() {
        am_func_not_implemeted(initializeDistFeat);
    }
    virtual void updateDistFeat() {
        am_func_not_implemeted(updateDistFeat);
    }
    virtual const double* getDistFeat(){
        am_func_not_implemeted(getDistFeat);
    }
    virtual void updateDistFeat(double* feat_addr) {
        am_func_not_implemeted(updateDistFeat);
    }
    virtual unsigned int getDistFeatSize() {
        am_func_not_implemeted(getDistFeatSize);
    }
    virtual const AMDist* getDistFunc() {
        am_func_not_implemeted(getDistFunc);
    }

    // -------------------------------------------------------------------------- //
    // --------------------------- Stochastic Sampler --------------------------- //
    // -------------------------------------------------------------------------- //

    virtual void initializeSampler(const VectorXd &state_sigma,
        const VectorXd &state_mean){
        am_func_not_implemeted(initializeSampler(VectorXd, VectorXd));
    }
    virtual void setSampler(const VectorXd &state_sigma,
        const VectorXd &state_mean){
        am_func_not_implemeted(setSampler);
    }
    virtual void setSamplerMean(const VectorXd &mean){
        am_func_not_implemeted(setSamplerMean(VectorXd));
    }
    virtual void setSamplerSigma(const VectorXd &std){
        am_func_not_implemeted(setSamplerSigma(VectorXd));
    }
    virtual void getSamplerMean(VectorXd &mean){
        am_func_not_implemeted(getSamplerMean);
    }
    virtual void getSamplerSigma(VectorXd &std){
        am_func_not_implemeted(getSamplerMean);
    }
    virtual void generatePerturbation(VectorXd &perturbation){
        am_func_not_implemeted(generatePerturbation);
    }

    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

protected:
    double f;

    RowVectorXd df_dI0, df_dIt;

    VectorXd p_am;

    int state_size;

    RowVectorXd df_dpam;

    MatrixXd d2f_dpam2;

    RowVectorXd df_dg0, df_dgt;

    MatrixXd d2f_dpam_dIt;

    MatrixXd d2f_dI02, d2f_dIt2;

    bool first_iter;

    const bool *spi_mask;

    AMStatus is_initialized;
};

_MTF_END_NAMESPACE

#endif