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) : id(id)
  10 {
  11     status = TrackerStatus::Fire;
  12     preStatus = TrackerStatus::Fire;
  13
  14     // TODO: Kalman filter
  15     this->kf.transitionMatrix = (Mat_<double>(4, 4) <<
  16                                                 1, 0, 1, 0,
  17                                                 0, 1, 0, 1,
  18                                                 0, 0, 1, 0,
  19                                                 0, 0, 0, 1);
  20
  21     this->kf.measurementMatrix = (Mat_<double>(2, 2) <<
  22                                                 1, 0, 0, 0,
  23                                                 0, 1, 0, 0);
  24
  25     this->kf.processNoiseCov = 1e-5 * Mat_<double>::eye(4, 4);
  26     this->kf.measurementNoiseCov = 1e-1 * Mat_<double>::ones(2, 2);
  27     this->kf.errorCovPost = 1. * Mat_<double>::ones(4, 4);
  28     //this->kf.statePre = 0.1 * Matx_<int, 4, 1>::randn(4, 1);
  29     //??? TODO
  30     randn(this->kf.statePre, Scalar::all(0), Scalar::all(0.1));
  31     this->kf.statePost = (Mat_<double>(4, 1) << detection.center_x, detection.center_y, 0, 0);
  32 }
  33
  34 Tracker::~Tracker()
  35 {
  36     patches.clear();
  37 }
  38
  39 void Tracker::updateState(const Mat& image)
  40 {
  41     preStatus = this->status;
  42     int lost_age = this->age - this->last_active;
  43     int active_age = this->last_active;
  44
  45     if (lost_age >= MaxLost){
  46         status = TrackerStatus::Delete;
  47     } else if (lost_age >= 1 && active_age == 1){
  48         status = TrackerStatus::Delete;
  49     } else if (lost_age >= 1) {
  50         status = TrackerStatus::Lost;
  51     }
  52 }
  53
  54 void Tracker::addPatch(PatchPtr p)
  55 {
  56     this->patches.push_back(p);
  57 }
  58
  59 void Tracker::correct(const cv::Mat& image, const Detection& detection)
  60 {
  61     //kf.correct();
  62     preStatus = status;
  63     status = TrackerStatus::Active;
  64     last_active = age;
  65 }
  66