Fix issue boostpython not stopped by Ctrl+c

[trackerpp.git] / src / MultiTracker.cpp

diff --git a/src/MultiTracker.cpp b/src/MultiTracker.cpp
index 2206af3..01e60c5 100644 (file)
--- a/src/MultiTracker.cpp
+++ b/src/MultiTracker.cpp
@@ -1,38 +1,105 @@
 #include "MultiTracker.h"
 #include "Metrics.h"
+#include <algorithm>
+#include "hungarian.h"
+#include "Logger.h"
 
 using namespace suanzi;
+using namespace cv;
+using namespace Eigen;
 
-MultiTracker::MultiTracker(Metrics* m) : metrics(m)
+static const std::string TAG = "MultiTracker";
+MultiTracker::MultiTracker(MetricsPtr m) : metrics(m)
 {
+    LOG_DEBUG(TAG, "init - load model.pkl");
+    predictor = PredictorWrapper::create("./python/model.pkl");
+    predictor->dump();
 }
 
 
 MultiTracker::~MultiTracker()
 {
-    delete metrics;
     trackers.clear();
 }
+//
+//TrackerPtr MultiTracker::createTracker(int id)
+//{
+//    TrackerPtr t (new Tracker(id));
+//    addTracker(t);
+//    return t;
+//}
+//
+//void MultiTracker::addTracker(TrackerPtr t)
+//{
+//    trackers.push_back(t);
+//}
+//
+void MultiTracker::removeTracker(TrackerPtr t)
+{
+//    trackers.erase(t);
+}
 
-Tracker* MultiTracker::createTracker(int id)
+void MultiTracker::initNewTrackers(cv::Mat& iamge)
 {
-    Tracker* t = new Tracker(id);
-    addTracker(t);
-    return t;
 }
 
-void MultiTracker::addTracker(Tracker* t)
+
+void MultiTracker::correctTrackers(MetricsPtr m, Mat& image)
 {
-    trackers.insert(t);
 }
 
-void MultiTracker::removeTracker(Tracker *t)
+void calculate_edistance()
 {
-    trackers.erase(t);
 }
 
+#define MaxCost  100000
 
-void MultiTracker::update()
+static double distance(TrackerPtr t, const cv::Mat& image, const Detection& d)
 {
+    return 0.1;
+}
+
+void MultiTracker::update(unsigned int total, const Detection* detections, const Mat& image)
+{
+    // correct_trackers
+    // Generate cost matrix
+    int row = trackers.size();
+    int col = total;
+    MatrixXi cost_matrix = MatrixXi::Zero(row, col);
+    for (int i = 0; i < row; i++){
+        for (int j = 0; j < col; j++){
+            // TODO
+            // int cost = MaxCost;
+            cost_matrix(i, j) = distance(trackers[i], image, detections[j]);
+        }
+    }
+
+    // assignment
+    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++){
+        if (!(tracker_inds.array() == i).any()){
+            unmatched_trackers.push_back(trackers[i]);
+        }
+    }
+    for (auto t : unmatched_trackers){
+        t->updateState(image);
+    }
+
 
+    // handle unmatched detections
+    vector<int> unmatched_detection;
+    for(int j = 0; j < col; j++){
+        if (!(bb_inds.array() == j).any()){
+            unmatched_detection.push_back(j);
+        }
+    }
+    // create new trackers for new detections
+    for (auto i : unmatched_detection){
+        TrackerPtr t (new Tracker(image));
+        this->trackers.push_back(t);
+    }
 }