#include <algorithm>
#include "hungarian.h"
#include "Logger.h"
+#include "Utils.h"
using namespace suanzi;
using namespace cv;
static const std::string TAG = "MultiTracker";
static const cv::Size PREFERRED_SIZE = Size(64, 128);
-
-#define MaxCost 100000
+static const double MaxCost = 100000;
+static const double ProbThreshold = 0.05;
MultiTracker::MultiTracker(EngineWPtr e)
: engine(e)
{
LOG_DEBUG(TAG, "init - loading model.pkl");
- predictor = PredictorWrapper::create("./python/model.pkl");
+ predictor = PredictorWrapper::create("./python", "./python/model.pkl");
predictor->dump();
this->descriptor = {Size(64, 128), Size(16, 16), Size(8, 8), Size(8, 8), 9};
-
- std::vector<double> ff (40, 1);
- double prob = predictor->predict(4, ff);
}
MultiTracker::~MultiTracker()
trackers.clear();
}
-static Rect getRectInDetection(const Detection& d)
-{
- Rect r;
- r.x = d.center_x - d.width / 2;
- r.y = d.center_y - d.height / 2;
- r.width = d.width;
- r.height = d.height;
- return r;
-}
-
-static double calc_iou_ratio(const Detection& d1, const Detection& d2)
-{
- Rect r1 = getRectInDetection (d1);
- Rect r2 = getRectInDetection (d2);
- Rect r_inner = r1 & r1;
- Rect r_union = r1 | r2;
- return 1.0 * r_inner.area() / r_union.area();
-}
-
static std::vector<double> similarity(const PatchPtr p1, const PatchPtr p2)
{
std::vector<double> feature;
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(d1, d2));
+ feature.push_back(calc_iou_ratio(getRectInDetection(d1), getRectInDetection(d2)));
return feature;
}
-
double MultiTracker::distance(TrackerPtr tracker, const cv::Mat& image, const Detection& d)
{
PatchPtr patch = createPatch(image, d);
ss = similarity(i, patch);
features.insert(features.end(), ss.begin(), ss.end());
}
- double prob = predictor->predict(4, features);
- return prob;
+ double prob = predictor->predict(Tracker::MaxPatch - 1, features); // TODO why is MaxPatch-1
+ if (prob > ProbThreshold)
+ return -log(prob);
+ else
+ return MaxCost;
}
-static long cc = 0;
+static float calc_iou_ratio(const Detection& d1, const Detection& d2)
+{
+ return calc_iou_ratio(getRectInDetection(d1), getRectInDetection(d2));
+}
void MultiTracker::update(unsigned int total, const Detection* detections, const Mat& image)
{
- //////
- if ((cc % 50) == 0){
- if (EnginePtr e = engine.lock()){
- e->onStatusChanged();
- }
+ // predict trackers, update trackers using kalman filter
+ for (auto t : trackers){
+ t->predict();
}
- cc++;
- //////
+ // 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], detections[j]) < -0.1)
- // cost_matrix(i, j) = MaxCost;
- //else
+ 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);
- // handle unmatched trackers
- //vector<TrackerPtr> unmatched_trackers;
- for (int i = 0; i < row; i++){
+ set<TrackerPtr> unmatched_trackers;
+ set<int> unmatch_bbs_indices;
+
+ for(unsigned int i = 0; i < trackers.size(); i++){
if (!(tracker_inds.array() == i).any()){
- trackers[i]->updateState(image);
+ unmatched_trackers.insert(trackers[i]);
}
}
-
- // handle unmatched detections
- vector<int> unmatched_detection;
- for(int j = 0; j < col; j++){
+ for (unsigned int j = 0; j < total; j++){
if (!(bb_inds.array() == j).any()){
- unmatched_detection.push_back(j);
+ 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
+ }
}
}
- // create new trackers for new detections
- for (auto i : unmatched_detection){
- TrackerPtr t (new Tracker(image));
- this->trackers.push_back(t);
+
+ // 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]));
+ this->trackers.push_back(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);
+ }
}
- Detection dd;
+ // 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);
+ }
+ }
- PatchPtr pp = createPatch(image, dd);
+ // 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)