src/Tracker.cpp

   1 #include "Tracker.h"
   2
   3 using namespace suanzi;
   4 using namespace cv;
   5 using namespace std;
   6
   7 static const int MaxLost = 5;
   8
   9 Tracker::Tracker(const cv::Mat& image, const Detection& detection, int id)
  10 : id(id)
  11 {
  12     status = TrackerStatus::Fire;
  13     preStatus = TrackerStatus::Fire;
  14
  15     // TODO: Kalman filter
  16     KF.transitionMatrix = (Mat_<double>(4, 4) <<
  17                                                 1, 0, 1, 0,
  18                                                 0, 1, 0, 1,
  19                                                 0, 0, 1, 0,
  20                                                 0, 0, 0, 1);
  21
  22     KF.measurementMatrix = (Mat_<double>(2, 2) <<
  23                                                 1, 0, 0, 0,
  24                                                 0, 1, 0, 0);
  25
  26     KF.processNoiseCov = 1e-5 * Mat_<double>::eye(4, 4);
  27     KF.measurementNoiseCov = 1e-1 * Mat_<double>::ones(2, 2);
  28     KF.errorCovPost = 1. * Mat_<double>::ones(4, 4);
  29     //this->kf.statePre = 0.1 * Matx_<int, 4, 1>::randn(4, 1);
  30     //??? TODO
  31     randn(KF.statePre, Scalar::all(0), Scalar::all(0.1));
  32     KF.statePost = (Mat_<double>(4, 1) << detection.center_x, detection.center_y, 0, 0);
  33 }
  34
  35 Tracker::~Tracker()
  36 {
  37     patches.clear();
  38 }
  39
  40 void Tracker::updateState(const Mat& image)
  41 {
  42     preStatus = this->status;
  43     int lost_age = this->age - this->last_active;
  44     int active_age = this->last_active;
  45
  46     if (lost_age >= MaxLost){
  47         status = TrackerStatus::Delete;
  48     } else if (lost_age >= 1 && active_age == 1){
  49         status = TrackerStatus::Delete;
  50     } else if (lost_age >= 1) {
  51         status = TrackerStatus::Lost;
  52     }
  53 }
  54
  55 void Tracker::addPatch(PatchPtr p)
  56 {
  57     patches.insert(patches.begin(), p);
  58     if (patches.size() > MaxPatch){
  59         patches.erase(patches.end());
  60     }
  61 }
  62
  63 void Tracker::correct(const cv::Mat& image, const Detection& detection)
  64 {
  65     // detection.center_x,   detection.center_y,
  66     // KF.correct(detect.center_x, detect.center_y);
  67     preStatus = status;
  68     status = TrackerStatus::Active;
  69     last_active = age;
  70 }
  71
  72 void Tracker::predict()
  73 {
  74     age++;
  75     //detection = KF.predict();
  76 }