tracker.h

#ifndef TRACKER_H
#define TRACKER_H
#include "sensor.h"
 
struct TrackingResult{
    b2Transform displacement=b2Transform_zero; //estimated displacement
    Disturbance observed_disturbance; //the disturbance as observed at the current time step
 
    TrackingResult()=default;
 
    TrackingResult(const Disturbance& d, const b2Transform & tr=b2Transform_zero): displacement(tr), observed_disturbance(d){}
};
 
class Tracker{
    protected:
    friend Configurator;
    ThresholdLearner *learner=NULL;
    b2Transform deltaTransform=b2Transform_zero;
    Threshold threshold=Threshold();
 
    public:
 
    Tracker(){}
 
    virtual Threshold get_threshold(const State &s){
        return threshold;
    }
 
    void register_learner(ThresholdLearner * l){
        learner=l;
    }
 
    b2Transform getDeltaTransform(){return deltaTransform;}
    virtual TrackingResult get_transform(const Task &t, const CoordinateContainer &pts, const std::vector <BodyFeatures> & objects);
    virtual TrackingResult track(const Task &t, const CoordinateContainer &pts, const std::vector <BodyFeatures> & objects)=0;
    //void adjust_task(const vertexDescriptor&, TransitionSystem &, Task*, const b2Transform &);                
 
    virtual void on_new_task(const Task &task, const Task & goal)=0;
 
    virtual void on_new_reading(const Task & goal,const Task &task ){}
 
    virtual void init(const Task & goal)=0;
 
    virtual bool hasTaskEnded(Task & t);
 
 
    void make_log();
    
    protected:
 
 
    void log_thresholds(){
        FILE *f= fopen("/tmp/thresholds.txt", "a");
        fprintf(f, "%f\t%f\t%f\t%f\t%f\n", threshold.for_Di().get_x(),
                                            threshold.for_Di().get_y(),
                                            threshold.for_Di().get_angle(),
                                            threshold.for_Di().get_width(),
                                            threshold.for_Di().get_length());
        fclose(f);
    }
 
    
};
 
class DeadReckoner: public Tracker{
    public:
    DeadReckoner(){}   
    
    TrackingResult track(const Task &t, const CoordinateContainer &pts, const std::vector <BodyFeatures> & objects);
 
    void on_new_task(const Task &task, const Task & goal)override{} //does nothing
 
    void init(const Task & goal){}
 
 
};
 
class ClosedLoop_Tracker:public Tracker{
    protected:
    Disturbance tracked_disturbance; //disturbance to be tracked as at task start, kept in memory when task is changed
    b2PolygonShape attention_window; //a box drawn at the beginning of task which bounds the robot and the goal
    bool hasReading=false; //has the disturbance's actual position been found with a scan
    float window_area();
 
    public:
 
    ClosedLoop_Tracker(){}
 
    // ClosedLoop_Tracker(Task * goal){
    //     attention_window=sensor_box(Robot::get_vertices(),b2Transform_zero, goal->get_disturbance());
    // }
 
    TrackingResult get_transform(const Task &t, const CoordinateContainer &pts, const std::vector <BodyFeatures> & objects)override;    
    
    TrackingResult track(const Task &t, const CoordinateContainer &pts, const std::vector <BodyFeatures> & objects);
 
    cv::Rect2f real_world_focus(const Task * );
 
    virtual std::vector <BodyFeatures>::const_iterator find_disturbance(std::vector <BodyFeatures>::const_iterator objects_begin, std::vector <BodyFeatures>::const_iterator objects_end, const BodyFeatures & dist, b2Transform t, float * _least_square=NULL);
 
    void correctAngle(BodyFeatures & found, const BodyFeatures & dist);
    void on_new_task(const Task &task, const Task & goal)override;
 
 
    void set_attention(b2PolygonShape ps){
        attention_window=ps;
    }
 
    void init(const Task & goal){
        attention_window=sensor_box(Robot::get_vertices(),b2Transform_zero, goal.get_disturbance());
 
    }
 
    virtual bool hasTaskEnded(Task & t);
 
    void makeAttentionWindow(const Task &goal, const Task & currentTask);
    
};
 
 
 
//same as CL Tracker but creates custom threshold based on state
class CLAdaptiveTracker:public ClosedLoop_Tracker{
    protected:
    Threshold get_threshold(const State &s){
        float distance=std::max(Threshold::FIXED_ENDPOSE, s.distance()/2);
        return Threshold(distance, Threshold::FIXED_ANGLE, Threshold::FIXED_DISTPOS, Threshold::FIXED_AFFORDANCE, Threshold::FIXED_DIMENSIONS);
    }
 
};
#endif