src/MultiTracker.cpp

   1 #include "MultiTracker.h"
   2 #include "Metrics.h"
   3 #include <algorithm>
   4 #include "hungarian.h"
   5 #include "Logger.h"
   6 #include "Utils.h"
   7
   8 using namespace suanzi;
   9 using namespace cv;
  10 using namespace std;
  11
  12 static const std::string TAG = "MultiTracker";
  13 static const cv::Size PREFERRED_SIZE = Size(64, 128);
  14
  15 static const double MaxCost  = 100000;
  16 static const int MaxPatch  = 5;
  17 static const double ProbThreshold = 0.05;
  18
  19 MultiTracker::MultiTracker(EngineWPtr e)
  20 : engine(e)
  21 {
  22     LOG_DEBUG(TAG, "init - loading model.pkl");
  23     predictor = PredictorWrapper::create("./python", "./python/model.pkl");
  24     predictor->dump();
  25     this->descriptor = {Size(64, 128), Size(16, 16), Size(8, 8), Size(8, 8), 9};
  26 }
  27
  28 MultiTracker::~MultiTracker()
  29 {
  30     predictor.reset();
  31     trackers.clear();
  32 }
  33
  34 static std::vector<double> similarity(const PatchPtr p1, const PatchPtr p2)
  35 {
  36     std::vector<double> feature;
  37     cv::Mat im1(PREFERRED_SIZE, p1->image_crop.type());
  38     cv::Mat im2(PREFERRED_SIZE, p2->image_crop.type());
  39     cv::resize(p1->image_crop, im1, im1.size());
  40     cv::resize(p2->image_crop, im2, im2.size());
  41     cv::Mat result;
  42     cv::matchTemplate(im1, im2, result, CV_TM_CCOEFF_NORMED);
  43     feature.push_back(result.at<double>(0, 0));
  44     cv::matchTemplate(im1, im2, result, CV_TM_CCORR_NORMED);
  45     feature.push_back(result.at<double>(0, 0));
  46
  47
  48     vector<double>& f1_hog = p1->features.first; Mat f1_hue = p1->features.second;
  49     vector<double>& f2_hog = p1->features.first; Mat f2_hue = p1->features.second;
  50     feature.push_back(distance_cosine(Eigen::Map<Eigen::VectorXd>(f1_hog.data(), f1_hog.size()),
  51                                     Eigen::Map<Eigen::VectorXd>(f2_hog.data(), f2_hog.size())));
  52     feature.push_back(distance_euclidean(Eigen::Map<Eigen::VectorXd>(f1_hog.data(), f1_hog.size()),
  53                                     Eigen::Map<Eigen::VectorXd>(f2_hog.data(), f2_hog.size())));
  54     feature.push_back(compareHist(f1_hue, f2_hue, HISTCMP_CORREL));
  55     feature.push_back(compareHist(f1_hue, f2_hue, HISTCMP_HELLINGER));
  56
  57     Detection& d1 = p1->detection;
  58     Detection& d2 = p2->detection;
  59
  60     double center_distance = sqrt(pow((d1.center_x - d2.center_x), 2) + pow((d1.center_y - d2.center_y), 2));
  61     feature.push_back(center_distance / (d1.width + d1.height + d2.width + d2.height) * 4);
  62
  63     feature.push_back(calc_iou_ratio(getRectInDetection(d1), getRectInDetection(d2)));
  64
  65     return feature;
  66 }
  67
  68 double MultiTracker::distance(TrackerPtr tracker, const cv::Mat& image, const Detection& d)
  69 {
  70     PatchPtr patch = createPatch(image, d);
  71     std::vector<double> features;
  72
  73     std::vector<double> ss;
  74     for (auto i : tracker->patches){
  75         ss = similarity(i, patch);
  76         features.insert(features.end(),  ss.begin(), ss.end());
  77     }
  78     double prob = predictor->predict(MaxPatch - 1, features); // TODO ???
  79     if (prob > ProbThreshold)
  80         return -log(prob);
  81     else
  82         return MaxCost;
  83 }
  84
  85 static float calc_iou_ratio(const Detection& d1, const Detection& d2)
  86 {
  87     return calc_iou_ratio(getRectInDetection(d1), getRectInDetection(d2));
  88 }
  89
  90 void MultiTracker::update(unsigned int total, const Detection* detections, const Mat& image)
  91 {
  92     // predict trackers, update trackers using kalman filter
  93     for (auto t : trackers){
  94         t->predict();
  95     }
  96
  97     // match the trackers with the detections
  98     int row = trackers.size();
  99     int col = total;
 100     Eigen::MatrixXi cost_matrix = Eigen::MatrixXi::Zero(row, col);
 101     for (int i = 0; i < row; i++){
 102         for (int j = 0; j < col; j++){
 103             if (calc_iou_ratio(trackers[i]->detection, detections[j]) < -0.1)
 104                 cost_matrix(i, j) = MaxCost;
 105             else
 106                 cost_matrix(i, j) = distance(trackers[i], image, detections[j]);
 107         }
 108     }
 109
 110     Eigen::VectorXi tracker_inds, bb_inds;
 111     linear_sum_assignment(cost_matrix, tracker_inds, bb_inds);
 112
 113     set<TrackerPtr> unmatched_trackers;
 114     set<int> unmatch_bbs_indices;
 115
 116     for(unsigned int i = 0; i < trackers.size(); i++){
 117         if (!(tracker_inds.array() == i).any()){
 118             unmatched_trackers.insert(trackers[i]);
 119         }
 120     }
 121     for (unsigned int j = 0; j < total; j++){
 122         if (!(bb_inds.array() == j).any()){
 123             unmatch_bbs_indices.insert(j);
 124         }
 125     }
 126
 127     // handle matched trackers
 128     for (unsigned int i = 0; i < tracker_inds.size(); i++){
 129         for (int j = 0; j < bb_inds.size(); j++){
 130             int rr = tracker_inds(i);
 131             int cc = bb_inds(j);
 132             TrackerPtr tracker = trackers[rr];
 133             const Detection& detect = detections[cc];
 134             if (cost_matrix(rr, cc) < MaxCost){
 135                 tracker->correct(image, detect);
 136                 tracker->addPatch(createPatch(image, detect));
 137             } else {
 138                 unmatched_trackers.insert(tracker);  // failed trackers
 139                 unmatch_bbs_indices.insert(cc);     // filed detection
 140             }
 141         }
 142     }
 143
 144     // handle unmatched trackers
 145     for (auto t : unmatched_trackers){
 146         t->updateState(image);
 147     }
 148
 149     // handle unmatched detections - Create new trackers
 150     vector<Person> inPersons;
 151     for (auto i : unmatch_bbs_indices){
 152         TrackerPtr new_tracker (new Tracker(image, detections[i]));
 153         new_tracker->addPatch(createPatch(image, detections[i]));
 154         this->trackers.push_back(new_tracker);
 155         Person test; // TODO
 156         inPersons.push_back(test);
 157     }
 158
 159     // callback and notify engine - persons in
 160     if (inPersons.size() > 0){
 161         if (auto e = engine.lock()){
 162             e->onPersonsIn(inPersons);
 163         }
 164     }
 165
 166     // Delete lost trackers
 167     vector<Person> outPersons;
 168     for (auto it = trackers.begin(); it < trackers.end(); it++){
 169         if ((*it)->status == TrackerStatus::Delete){
 170             Person test; // TODO
 171             outPersons.push_back(test);
 172             trackers.erase(it);
 173         }
 174     }
 175
 176     // callback and notify engine - persons out
 177     if (outPersons.size() > 0){
 178         if (auto e = engine.lock()){
 179             e->onPersonsOut(outPersons);
 180         }
 181     }
 182 }
 183
 184 static cv::Mat image_crop(const cv::Mat& image, const Detection& bb)
 185 {
 186     return image(getRectInDetection(bb));
 187 }
 188
 189 PatchPtr MultiTracker::createPatch(const Mat& image, const Detection& detect)
 190 {
 191     PatchPtr patch(new Patch());
 192
 193     // calculate hog descriptors, size is 3780
 194     Mat im, im2;
 195     im = image_crop(image, detect);
 196     resize(im, im2, PREFERRED_SIZE);
 197     vector<float> feature_hog;
 198     this->descriptor.compute(im2, feature_hog);
 199
 200     // calculate histogram, size is (64 x 45)
 201     Mat hsv, hist;
 202     cvtColor(im, hsv, COLOR_BGR2HSV);
 203     int channels[] = {0, 1};
 204     int histSize[] = {45, 64};
 205     float hranges[] = {0, 180};
 206     float sranges[] = {0, 256};
 207     const float* ranges[] = {hranges, sranges};
 208     calcHist(&hsv, 1, channels, Mat(), hist, 2, histSize, ranges, true, false);
 209
 210     patch->image_crop = im.clone();
 211     patch->detection = detect;
 212     std::vector<double> feature_hog_double (feature_hog.begin(), feature_hog.end()); // convert to double
 213     patch->features = std::make_pair(feature_hog_double, hist);
 214     return patch;
 215 }