src/MultiTracker.cpp

   1 #include "MultiTracker.h"
   2 #include "Metrics.h"
   3 #include <algorithm>
   4 #include "hungarian.h"
   5 #include "Logger.h"
   6
   7 using namespace suanzi;
   8 using namespace cv;
   9 using namespace Eigen;
  10
  11 static const std::string TAG = "MultiTracker";
  12 MultiTracker::MultiTracker(MetricsPtr m) : metrics(m)
  13 {
  14     LOG_DEBUG(TAG, "init - load model.pkl");
  15     predictor = PredictorWrapper::create("./python/model.pkl");
  16     predictor->dump();
  17 }
  18
  19
  20 MultiTracker::~MultiTracker()
  21 {
  22     trackers.clear();
  23 }
  24 //
  25 //TrackerPtr MultiTracker::createTracker(int id)
  26 //{
  27 //    TrackerPtr t (new Tracker(id));
  28 //    addTracker(t);
  29 //    return t;
  30 //}
  31 //
  32 //void MultiTracker::addTracker(TrackerPtr t)
  33 //{
  34 //    trackers.push_back(t);
  35 //}
  36 //
  37 void MultiTracker::removeTracker(TrackerPtr t)
  38 {
  39 //    trackers.erase(t);
  40 }
  41
  42 void MultiTracker::initNewTrackers(cv::Mat& iamge)
  43 {
  44 }
  45
  46
  47 void MultiTracker::correctTrackers(MetricsPtr m, Mat& image)
  48 {
  49 }
  50
  51 void calculate_edistance()
  52 {
  53 }
  54
  55 #define MaxCost  100000
  56
  57 static double distance(TrackerPtr t, const cv::Mat& image, const Detection& d)
  58 {
  59     return 0.1;
  60 }
  61
  62 void MultiTracker::update(unsigned int total, const Detection* detections, const Mat& image)
  63 {
  64     // correct_trackers
  65     // Generate cost matrix
  66     int row = trackers.size();
  67     int col = total;
  68     MatrixXi cost_matrix = MatrixXi::Zero(row, col);
  69     for (int i = 0; i < row; i++){
  70         for (int j = 0; j < col; j++){
  71             // TODO
  72             // int cost = MaxCost;
  73             cost_matrix(i, j) = distance(trackers[i], image, detections[j]);
  74         }
  75     }
  76
  77     // assignment
  78     VectorXi tracker_inds, bb_inds;
  79     linear_sum_assignment(cost_matrix, tracker_inds, bb_inds);
  80
  81     // handle unmatched trackers
  82     vector<TrackerPtr> unmatched_trackers;
  83     for (int i = 0; i < row; i++){
  84         if (!(tracker_inds.array() == i).any()){
  85             unmatched_trackers.push_back(trackers[i]);
  86         }
  87     }
  88     for (auto t : unmatched_trackers){
  89         t->updateState(image);
  90     }
  91
  92
  93     // handle unmatched detections
  94     vector<int> unmatched_detection;
  95     for(int j = 0; j < col; j++){
  96         if (!(bb_inds.array() == j).any()){
  97             unmatched_detection.push_back(j);
  98         }
  99     }
 100     // create new trackers for new detections
 101     for (auto i : unmatched_detection){
 102         TrackerPtr t (new Tracker(image));
 103         this->trackers.push_back(t);
 104     }
 105 }