From be96459d3e098508417f07ed0e1952b6e05215c4 Mon Sep 17 00:00:00 2001
From: Peng Li <seudut@gmail.com>
Date: Mon, 23 Jul 2018 14:21:56 +0800
Subject: [PATCH] finish tracker

---
 .gitignore               |  4 ++++
 include/Tracker.h        | 13 ++++++-------
 src/MultiTracker.cpp     | 14 ++++++++++++--
 src/PredictorWrapper.cpp |  2 --
 src/Tracker.cpp          | 25 ++++++++++++++++---------
 5 files changed, 38 insertions(+), 20 deletions(-)

diff --git a/.gitignore b/.gitignore
index c7db8d3..8e7feea 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,3 +2,7 @@ src/*.o
 *.o
 .sconsign.dblite
 *.pyc
+main
+temp/
+test/TestMain
+libtracker.a
diff --git a/include/Tracker.h b/include/Tracker.h
index d528670..bc36dba 100644
--- a/include/Tracker.h
+++ b/include/Tracker.h
@@ -29,19 +29,18 @@ namespace suanzi {
         void updateState(const cv::Mat& image);
         void addPatch(PatchPtr p);
         void correct(const cv::Mat& image, const Detection& d);
-        TrackerStatus status;
+        void predict();
         std::vector<PatchPtr> patches;
         Detection detection;
+        TrackerStatus status = TrackerStatus::Fire;
 
     private:
-        TrackerStatus preStatus;
+        TrackerStatus preStatus = TrackerStatus::Fire;
         int id;
-        int age;
-        int last_active;
-        cv::KalmanFilter kf = {4,2};
+        int age = 1;
+        int last_active = 1;
+        cv::KalmanFilter KF = {4,2};
     };
-
-
 }
 
 #endif /* _TRACKER_H_ */
diff --git a/src/MultiTracker.cpp b/src/MultiTracker.cpp
index aef5e2e..547a195 100644
--- a/src/MultiTracker.cpp
+++ b/src/MultiTracker.cpp
@@ -14,6 +14,7 @@ static const cv::Size PREFERRED_SIZE = Size(64, 128);
 
 static const double MaxCost  = 100000;
 static const int MaxPatch  = 5;
+static const double ProbThreshold = 0.05;
 
 MultiTracker::MultiTracker(EngineWPtr e)
 : engine(e)
@@ -74,8 +75,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(MaxPatch - 1, features); // TODO ???
+    if (prob > ProbThreshold)
+        return -log(prob);
+    else 
+        return MaxCost;
 }
 
 static float calc_iou_ratio(const Detection& d1, const Detection& d2)
@@ -85,6 +89,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
     int row = trackers.size();
     int col = total;
     Eigen::MatrixXi cost_matrix = Eigen::MatrixXi::Zero(row, col);
diff --git a/src/PredictorWrapper.cpp b/src/PredictorWrapper.cpp
index 44e5fed..ebe7579 100644
--- a/src/PredictorWrapper.cpp
+++ b/src/PredictorWrapper.cpp
@@ -39,10 +39,8 @@ PredictorWrapperPtr PredictorWrapper::create(const std::string& python_dir, cons
 void PredictorWrapper::dump()
 {
     LOG_DEBUG(TAG, "dump");
-    cout << "dump" << endl;
     std::string ss = "";
     try{
-        cout << "==== before" << endl;
         py::object ret = this->dump_func();
         ss = py::extract<std::string>(ret);
     } catch (boost::python::error_already_set const &){
diff --git a/src/Tracker.cpp b/src/Tracker.cpp
index e9353bd..b7da363 100644
--- a/src/Tracker.cpp
+++ b/src/Tracker.cpp
@@ -6,29 +6,30 @@ using namespace std;
 
 static const int MaxLost = 5;
 
-Tracker::Tracker(const cv::Mat& image, const Detection& detection, int id) : id(id)
+Tracker::Tracker(const cv::Mat& image, const Detection& detection, int id) 
+: id(id)
 {
     status = TrackerStatus::Fire;
     preStatus = TrackerStatus::Fire;
 
     // TODO: Kalman filter
-    this->kf.transitionMatrix = (Mat_<double>(4, 4) << 
+    KF.transitionMatrix = (Mat_<double>(4, 4) << 
                                                 1, 0, 1, 0,
                                                 0, 1, 0, 1,
                                                 0, 0, 1, 0,
                                                 0, 0, 0, 1);
 
-    this->kf.measurementMatrix = (Mat_<double>(2, 2) << 
+    KF.measurementMatrix = (Mat_<double>(2, 2) << 
                                                 1, 0, 0, 0,
                                                 0, 1, 0, 0);
 
-    this->kf.processNoiseCov = 1e-5 * Mat_<double>::eye(4, 4);
-    this->kf.measurementNoiseCov = 1e-1 * Mat_<double>::ones(2, 2);
-    this->kf.errorCovPost = 1. * Mat_<double>::ones(4, 4);
+    KF.processNoiseCov = 1e-5 * Mat_<double>::eye(4, 4);
+    KF.measurementNoiseCov = 1e-1 * Mat_<double>::ones(2, 2);
+    KF.errorCovPost = 1. * Mat_<double>::ones(4, 4);
     //this->kf.statePre = 0.1 * Matx_<int, 4, 1>::randn(4, 1);
     //??? TODO
-    randn(this->kf.statePre, Scalar::all(0), Scalar::all(0.1));
-    this->kf.statePost = (Mat_<double>(4, 1) << detection.center_x, detection.center_y, 0, 0);
+    randn(KF.statePre, Scalar::all(0), Scalar::all(0.1));
+    KF.statePost = (Mat_<double>(4, 1) << detection.center_x, detection.center_y, 0, 0);
 }
 
 Tracker::~Tracker()
@@ -58,9 +59,15 @@ void Tracker::addPatch(PatchPtr p)
 
 void Tracker::correct(const cv::Mat& image, const Detection& detection)
 {
-    //kf.correct();
+    // detection.center_x,   detection.center_y, 
+    // KF.correct(detect.center_x, detect.center_y);
     preStatus = status;
     status = TrackerStatus::Active;
     last_active = age;
 }
 
+void Tracker::predict()
+{
+    age++;
+    //detection = KF.predict();
+}
-- 
2.11.0