LineTracker.h

#ifndef MTF_LINE_TRACKER_H
#define MTF_LINE_TRACKER_H

#include "CompositeBase.h"
#include "mtf/Macros/common.h"

#define LINE_GRID_SIZE_X 5
#define LINE_GRID_SIZE_Y 5
#define LINE_PATCH_SIZE 25
#define LINE_USE_CONSTANT_SLOPE false
#define LINE_USE_LS false
#define LINE_INTER_ALPHA_THRESH 0.1
#define LINE_INTRA_ALPHA_THRESH 0.05
#define LINE_RESET_POS false
#define LINE_RESET_TEMPLATE false
#define LINE_DEBUG_MODE false

#define INF_VAL 1e5
#define TINY_VAL 1e-5
#define SMALL_VAL 2

#define PI_RAD 3.14159265358979323846

_MTF_BEGIN_NAMESPACE

struct lineParams {
    double m;
    double c;
    double c_diff;
    double theta;
    int is_vert;
    lineParams(double _m = 0, double _c = 0, double _c_diff=0,
        int _is_vert = 0) {
        m = _m;
        c = _c;
        c_diff = _c_diff;
        is_vert = _is_vert;
    }
};

struct gridPoint {
    double x;
    double y;
    double tracker_x;
    double tracker_y;
    double xx;
    double xy;

    double wt;
    double wt_y;
    double wt_x;
    double alpha_wt[2];
    double alpha[2];

    double inter_alpha_diff[2];
    double intra_alpha_diff[2];

    int is_successful[2];

};

struct gridLine {
    int no_of_pts;
    lineParams *params;
    gridPoint **pts;

    gridPoint *start_pt;
    gridPoint *end_pt;

    int type;

    int inited;
    int is_successful;

    gridLine();
    ~gridLine() {
        if(inited) {
            delete(params);
            delete(pts);
        }
    }

    void init(int no_of_pts, int type);
    void assignAddrs(gridPoint *start_addr = nullptr, int offset_diff = 1);
    void updateParams();
};
struct LineTrackerParams {
    int grid_size_x, grid_size_y;
    int patch_size;
    int use_constant_slope;
    int use_ls;
    double inter_alpha_thresh;
    double intra_alpha_thresh;
    int reset_pos;
    int reset_template;
    int debug_mode;
    LineTrackerParams(int grid_size_x, int grid_size_y,
        int patch_size, bool use_constant_slope, bool use_ls,
        double inter_alpha_thresh, double intra_alpha_thresh,
        bool reset_pos, bool reset_template, bool debug_mode);
    LineTrackerParams(const LineTrackerParams *params = nullptr);
};

class LineTracker : public CompositeBase {

public:

    typedef  LineTrackerParams ParamType;

    LineTracker(const vector<TrackerBase*> trackers,
     const ParamType *line_params = nullptr);
    ~LineTracker(){}
    void initialize(const cv::Mat& cv_corners) override;
    void update() override;

private:
    ParamType params;

    int corner_tracker_ids[4];
    int frame_id;

    cv::Point2d *tracker_pos;
    cv::Point2d wt_mean_pt;

    gridPoint *curr_grid_pts;
    gridPoint *prev_grid_pts;

    gridLine *curr_vert_lines;
    gridLine *curr_horz_lines;
    gridLine *prev_vert_lines;
    gridLine *prev_horz_lines;

    lineParams *mean_horz_params;
    lineParams *mean_vert_params;

    int is_initialized;

    void initGridPositions(const cv::Mat& cv_corners);
    void updateAlpha(gridLine* curr_lines, gridLine* prev_lines, 
        int no_of_lines, int line_type);
     int updateLineParamsLS(gridLine* curr_lines, gridLine* prev_lines, lineParams *mean_params,
         int no_of_lines, int line_type);
     int updateLineParamsWeightedLS(gridLine* curr_lines, gridLine* prev_lines, lineParams *mean_params,
         int no_of_lines, int line_type);
    void resetLineParamsToPrev(gridLine* curr_lines,
        gridLine* prev_lines, int no_of_lines);
     void resetLineParamsToMean(gridLine* curr_lines, gridLine* prev_lines, lineParams* mean_params,
         int no_of_lines, int reset_c = 0);
     void updateGridWithLineIntersections();

     void updateDistanceWeights();
     void resetTrackerStates();
     void updateCVCorners();
};
_MTF_END_NAMESPACE
#endif