separate pytwrpper from predictorWrappery

[trackerpp.git] / src / MultiTracker.cpp

diff --git a/src/MultiTracker.cpp b/src/MultiTracker.cpp
index a7f5e59..bbfc6f3 100644 (file)
--- a/src/MultiTracker.cpp
+++ b/src/MultiTracker.cpp
@@ -11,15 +11,15 @@ using namespace std;
 
 static const std::string TAG = "MultiTracker";
 static const cv::Size PREFERRED_SIZE = Size(64, 128);
-
 static const double MaxCost  = 100000;
-static const int MaxPath = 5;
+static const double ProbThreshold = 0.05;
 
 MultiTracker::MultiTracker(EngineWPtr e)
 : engine(e)
 {
     LOG_DEBUG(TAG, "init - loading model.pkl");
-    predictor = PredictorWrapper::create("./python", "./python/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};
 }
@@ -74,8 +74,11 @@ double MultiTracker::distance(TrackerPtr tracker, const cv::Mat& image, const De
         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 float calc_iou_ratio(const Detection& d1, const Detection& d2)
@@ -85,6 +88,12 @@ static float calc_iou_ratio(const Detection& d1, const Detection& d2)
 
 void MultiTracker::update(unsigned int total, const Detection* detections, const Mat& image)
 {
+    // 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);
@@ -100,30 +109,75 @@ void MultiTracker::update(unsigned int total, const Detection* detections, const
     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)