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