#include "MultiTracker.h"
-#include "Metrics.h"
+#include <algorithm>
+#include "hungarian.h"
+#include "Logger.h"
+#include "Utils.h"
+#include <cstdlib>
+#include <stdexcept>
using namespace suanzi;
+using namespace cv;
+using namespace std;
-MultiTracker::MultiTracker(MetricsPtr m) : metrics(m)
+static const std::string TAG = "MultiTracker";
+static const cv::Size PREFERRED_SIZE = Size(64, 128);
+static const double MaxCost = 100000;
+static const double ProbThreshold = 0.05;
+static const int MaxTrackers = 100;
+
+MultiTracker::MultiTracker(EngineWPtr e)
+: engine(e)
{
+ LOG_DEBUG(TAG, "init - loading model.pkl");
+ predictor = PredictorWrapperPtr(new PredictorWrapper());
+ predictor->load("./resources/model.pkl");
+ predictor->dump();
+ this->descriptor = {Size(64, 128), Size(16, 16), Size(8, 8), Size(8, 8), 9};
}
-
MultiTracker::~MultiTracker()
{
+ predictor.reset();
trackers.clear();
}
-TrackerPtr MultiTracker::createTracker(int id)
+static std::vector<double> similarity(const PatchPtr p1, const PatchPtr p2)
+{
+ std::vector<double> feature;
+ cv::Mat im1(PREFERRED_SIZE, p1->image_crop.type());
+ cv::Mat im2(PREFERRED_SIZE, p2->image_crop.type());
+ cv::resize(p1->image_crop, im1, im1.size());
+ cv::resize(p2->image_crop, im2, im2.size());
+
+ cv::Mat result = Mat::zeros(1,1, CV_64F);
+
+ cv::matchTemplate(im1, im2, result, CV_TM_CCOEFF_NORMED);
+ feature.push_back(result.at<double>(0, 0));
+ cv::matchTemplate(im1, im2, result, CV_TM_CCORR_NORMED);
+ feature.push_back(result.at<double>(0, 0));
+
+
+ vector<double>& f1_hog = p1->features.first; Mat f1_hue = p1->features.second;
+ vector<double>& f2_hog = p1->features.first; Mat f2_hue = p1->features.second;
+ feature.push_back(distance_cosine(Eigen::Map<Eigen::VectorXd>(f1_hog.data(), f1_hog.size()),
+ Eigen::Map<Eigen::VectorXd>(f2_hog.data(), f2_hog.size())));
+ feature.push_back(distance_euclidean(Eigen::Map<Eigen::VectorXd>(f1_hog.data(), f1_hog.size()),
+ Eigen::Map<Eigen::VectorXd>(f2_hog.data(), f2_hog.size())));
+ feature.push_back(compareHist(f1_hue, f2_hue, HISTCMP_CORREL));
+ feature.push_back(compareHist(f1_hue, f2_hue, HISTCMP_HELLINGER));
+
+ Detection& d1 = p1->detection;
+ Detection& d2 = p2->detection;
+
+ double center_distance = sqrt(pow((d1.center_x - d2.center_x), 2) + pow((d1.center_y - d2.center_y), 2));
+ feature.push_back(center_distance / (d1.width + d1.height + d2.width + d2.height) * 4);
+
+ feature.push_back(calc_iou_ratio(getRectInDetection(d1), getRectInDetection(d2)));
+
+// for (auto i : feature){
+// cout << i << "\t";
+// if (isnan(i)){
+// throw overflow_error("Nan in feature");
+// }
+// }
+// cout << endl;
+//
+ return feature;
+}
+
+double MultiTracker::distance(TrackerPtr tracker, const cv::Mat& image, const Detection& d)
{
- TrackerPtr t (new Tracker(id));
- addTracker(t);
- return t;
+ PatchPtr patch = createPatch(image, d);
+ std::vector<double> features;
+
+ std::vector<double> ss;
+ for (auto i : tracker->patches){
+ ss = similarity(i, patch);
+ features.insert(features.end(), ss.begin(), ss.end());
+ }
+ double prob = predictor->predict(tracker->patches.size() - 1, features); // TODO why is MaxPatch-1
+ if (prob > ProbThreshold)
+ return -log(prob);
+ else
+ return MaxCost;
}
-void MultiTracker::addTracker(TrackerPtr t)
+static float calc_iou_ratio(const Detection& d1, const Detection& d2)
{
- trackers.insert(t);
+ return calc_iou_ratio(getRectInDetection(d1), getRectInDetection(d2));
}
-void MultiTracker::removeTracker(TrackerPtr t)
+void MultiTracker::update(unsigned int total, const Detection* detections, const Mat& image)
{
- trackers.erase(t);
+ // predict trackers, update trackers using kalman filter
+ for (auto t : trackers){
+ t->predict();
+ }
+
+ // match the trackers with the detections using linear sum assignment (hungarian)
+ int row = trackers.size();
+ int col = total;
+ Eigen::MatrixXi cost_matrix = Eigen::MatrixXi::Zero(row, col);
+ for (int i = 0; i < row; i++){
+ for (int j = 0; j < col; j++){
+ if (calc_iou_ratio(trackers[i]->detection, detections[j]) < -0.1)
+ cost_matrix(i, j) = MaxCost;
+ else
+ cost_matrix(i, j) = distance(trackers[i], image, detections[j]);
+ }
+ }
+
+ Eigen::VectorXi tracker_inds, bb_inds;
+ linear_sum_assignment(cost_matrix, tracker_inds, bb_inds);
+
+ set<TrackerPtr> unmatched_trackers;
+ set<int> unmatch_bbs_indices;
+
+ for(unsigned int i = 0; i < trackers.size(); i++){
+ if (!(tracker_inds.array() == i).any()){
+ unmatched_trackers.insert(trackers[i]);
+ }
+ }
+ for (unsigned int j = 0; j < total; j++){
+ if (!(bb_inds.array() == j).any()){
+ unmatch_bbs_indices.insert(j);
+ }
+ }
+
+ // handle matched trackers
+ for (unsigned int i = 0; i < tracker_inds.size(); i++){
+ for (int j = 0; j < bb_inds.size(); j++){
+ int rr = tracker_inds(i);
+ int cc = bb_inds(j);
+ TrackerPtr tracker = trackers[rr];
+ const Detection& detect = detections[cc];
+ if (cost_matrix(rr, cc) < MaxCost){
+ tracker->correct(image, detect);
+ tracker->addPatch(createPatch(image, detect));
+ } else {
+ unmatched_trackers.insert(tracker); // failed trackers
+ unmatch_bbs_indices.insert(cc); // filed detection
+ }
+ }
+ }
+
+ // handle unmatched trackers
+ for (auto t : unmatched_trackers){
+ t->updateState(image);
+ }
+
+ // handle unmatched detections - Create new trackers
+ vector<Person> inPersons;
+ for (auto i : unmatch_bbs_indices){
+ TrackerPtr new_tracker (new Tracker(image, detections[i]));
+ new_tracker->addPatch(createPatch(image, detections[i]));
+ addTracker(new_tracker);
+ Person test; // TODO
+ inPersons.push_back(test);
+ }
+
+ // callback and notify engine - persons in
+ if (inPersons.size() > 0){
+ if (auto e = engine.lock()){
+ e->onPersonsIn(inPersons);
+ }
+ }
+
+ // Delete lost trackers
+ vector<Person> outPersons;
+ for (auto it = trackers.begin(); it < trackers.end(); it++){
+ if ((*it)->status == TrackerStatus::Delete){
+ Person test; // TODO
+ outPersons.push_back(test);
+ trackers.erase(it);
+ }
+ }
+
+ // callback and notify engine - persons out
+ if (outPersons.size() > 0){
+ if (auto e = engine.lock()){
+ e->onPersonsOut(outPersons);
+ }
+ }
}
+static cv::Mat image_crop(const cv::Mat& image, const Detection& bb)
+{
+ return image(getRectInDetection(bb));
+}
-void MultiTracker::update()
+PatchPtr MultiTracker::createPatch(const Mat& image, const Detection& detect)
{
+ PatchPtr patch(new Patch());
+
+ // calculate hog descriptors, size is 3780
+ Mat im, im2;
+ im = image_crop(image, detect);
+ resize(im, im2, PREFERRED_SIZE);
+ vector<float> feature_hog;
+ this->descriptor.compute(im2, feature_hog);
+
+ // calculate histogram, size is (64 x 45)
+ Mat hsv, hist;
+ cvtColor(im, hsv, COLOR_BGR2HSV);
+ int channels[] = {0, 1};
+ int histSize[] = {45, 64};
+ float hranges[] = {0, 180};
+ float sranges[] = {0, 256};
+ const float* ranges[] = {hranges, sranges};
+ calcHist(&hsv, 1, channels, Mat(), hist, 2, histSize, ranges, true, false);
+ patch->image_crop = im.clone();
+ patch->detection = detect;
+ std::vector<double> feature_hog_double (feature_hog.begin(), feature_hog.end()); // convert to double
+ patch->features = std::make_pair(feature_hog_double, hist);
+ return patch;
+}
+
+void MultiTracker::addTracker(TrackerPtr t)
+{
+ trackers.insert(trackers.begin(), t);
+ if(trackers.size() > MaxTrackers){
+ LOG_ERROR(TAG, "trackers reaches the maximum " + to_string(MaxTrackers));
+ trackers.pop_back();
+ }
}
projects / trackerpp.git / blobdiff