src/MultiTracker.cpp

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