Add callback from multitracker to engine

[trackerpp.git] / src / MultiTracker.cpp

diff --git a/src/MultiTracker.cpp b/src/MultiTracker.cpp
index a7f5e59..aef5e2e 100644 (file)
--- a/src/MultiTracker.cpp
+++ b/src/MultiTracker.cpp
@@ -13,7 +13,7 @@ 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 int MaxPatch  = 5;
 
 MultiTracker::MultiTracker(EngineWPtr e)
 : engine(e)
@@ -100,30 +100,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);
         }
     }
-    // create new trackers for new detections
-    for (auto i : unmatched_detection){
-        TrackerPtr t (new Tracker(image));
-        this->trackers.push_back(t);
+
+    // 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
+            }
+        }
     }
 
-    Detection dd;
+    // handle unmatched trackers
+    for (auto t : unmatched_trackers){
+        t->updateState(image);
+    }
 
-    PatchPtr pp = createPatch(image, dd);
+    // 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);
+        }
+    }
+
+    // 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)