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 #define MaxCost  100000
  16
  17 MultiTracker::MultiTracker(EngineWPtr e)
  18 : engine(e)
  19 {
  20     LOG_DEBUG(TAG, "init - loading model.pkl");
  21     predictor = PredictorWrapper::create("./python/model.pkl");
  22     predictor->dump();
  23     this->descriptor = {Size(64, 128), Size(16, 16), Size(8, 8), Size(8, 8), 9};
  24
  25     std::vector<double> ff (40, 1);
  26     double prob = predictor->predict(4, ff);
  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     cv::Mat result;
  43     cv::matchTemplate(im1, im2, result, CV_TM_CCOEFF_NORMED);
  44     feature.push_back(result.at<double>(0, 0));
  45     cv::matchTemplate(im1, im2, result, CV_TM_CCORR_NORMED);
  46     feature.push_back(result.at<double>(0, 0));
  47
  48
  49     vector<double>& f1_hog = p1->features.first; Mat f1_hue = p1->features.second;
  50     vector<double>& f2_hog = p1->features.first; Mat f2_hue = p1->features.second;
  51     feature.push_back(distance_cosine(Eigen::Map<Eigen::VectorXd>(f1_hog.data(), f1_hog.size()),
  52                                     Eigen::Map<Eigen::VectorXd>(f2_hog.data(), f2_hog.size())));
  53     feature.push_back(distance_euclidean(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(compareHist(f1_hue, f2_hue, HISTCMP_CORREL));
  56     feature.push_back(compareHist(f1_hue, f2_hue, HISTCMP_HELLINGER));
  57
  58     Detection& d1 = p1->detection;
  59     Detection& d2 = p2->detection;
  60
  61     double center_distance = sqrt(pow((d1.center_x - d2.center_x), 2) + pow((d1.center_y - d2.center_y), 2));
  62     feature.push_back(center_distance / (d1.width + d1.height + d2.width + d2.height) * 4);
  63
  64     feature.push_back(calc_iou_ratio(getRectInDetection(d1), getRectInDetection(d2)));
  65
  66     return feature;
  67 }
  68
  69
  70 double MultiTracker::distance(TrackerPtr tracker, const cv::Mat& image, const Detection& d)
  71 {
  72     PatchPtr patch = createPatch(image, d);
  73     std::vector<double> features;
  74
  75     std::vector<double> ss;
  76     for (auto i : tracker->patches){
  77         ss = similarity(i, patch);
  78         features.insert(features.end(),  ss.begin(), ss.end());
  79     }
  80     double prob = predictor->predict(4, features);
  81     return prob;
  82 }
  83
  84 static long cc = 0;
  85
  86 void MultiTracker::update(unsigned int total, const Detection* detections, const Mat& image)
  87 {
  88     //////
  89     if ((cc % 50) == 0){
  90         if (EnginePtr e = engine.lock()){
  91             e->onStatusChanged();
  92         }
  93     }
  94     cc++;
  95
  96     //////
  97     int row = trackers.size();
  98     int col = total;
  99     Eigen::MatrixXi cost_matrix = Eigen::MatrixXi::Zero(row, col);
 100     for (int i = 0; i < row; i++){
 101         for (int j = 0; j < col; j++){
 102             //if (calc_iou_ratio(trackers[i], detections[j]) < -0.1)
 103             //    cost_matrix(i, j) = MaxCost;
 104             //else
 105                 cost_matrix(i, j) = distance(trackers[i], image, detections[j]);
 106         }
 107     }
 108
 109     Eigen::VectorXi tracker_inds, bb_inds;
 110     linear_sum_assignment(cost_matrix, tracker_inds, bb_inds);
 111
 112     // handle unmatched trackers
 113     //vector<TrackerPtr> unmatched_trackers;
 114     for (int i = 0; i < row; i++){
 115         if (!(tracker_inds.array() == i).any()){
 116             trackers[i]->updateState(image);
 117         }
 118     }
 119
 120     // handle unmatched detections
 121     vector<int> unmatched_detection;
 122     for(int j = 0; j < col; j++){
 123         if (!(bb_inds.array() == j).any()){
 124             unmatched_detection.push_back(j);
 125         }
 126     }
 127     // create new trackers for new detections
 128     for (auto i : unmatched_detection){
 129         TrackerPtr t (new Tracker(image));
 130         this->trackers.push_back(t);
 131     }
 132
 133     Detection dd;
 134
 135     PatchPtr pp = createPatch(image, dd);
 136 }
 137
 138 static cv::Mat image_crop(const cv::Mat& image, const Detection& bb)
 139 {
 140     return image(getRectInDetection(bb));
 141 }
 142
 143 PatchPtr MultiTracker::createPatch(const Mat& image, const Detection& detect)
 144 {
 145     PatchPtr patch(new Patch());
 146
 147     // calculate hog descriptors, size is 3780
 148     Mat im, im2;
 149     im = image_crop(image, detect);
 150     resize(im, im2, PREFERRED_SIZE);
 151     vector<float> feature_hog;
 152     this->descriptor.compute(im2, feature_hog);
 153
 154     // calculate histogram, size is (64 x 45)
 155     Mat hsv, hist;
 156     cvtColor(im, hsv, COLOR_BGR2HSV);
 157     int channels[] = {0, 1};
 158     int histSize[] = {45, 64};
 159     float hranges[] = {0, 180};
 160     float sranges[] = {0, 256};
 161     const float* ranges[] = {hranges, sranges};
 162     calcHist(&hsv, 1, channels, Mat(), hist, 2, histSize, ranges, true, false);
 163
 164     patch->image_crop = im.clone();
 165     patch->detection = detect;
 166     std::vector<double> feature_hog_double (feature_hog.begin(), feature_hog.end()); // convert to double
 167     patch->features = std::make_pair(feature_hog_double, hist);
 168     return patch;
 169 }