Add callback from multitracker to engine

diff --git a/README.md b/README.md
index b40c9ee..e167559 100644 (file)
--- a/README.md
+++ b/README.md
@@ -21,17 +21,7 @@ As boost has already
 `sudo ./b2 install link=static cxxflags=-fPIC --with-filesystem --with-test --with-log --with-program_options --with-python`
 
 
-## Build 
-
-`make all`
-
-### Run
-
-`./main`
-
-
 ## Build with `scons`
-install scons tool `apt-get install scons`
 
 - Run `scons` to build the `src/` and `main`. It will generate a library
 `libtracker.a` and an executable file `main` under current folder.
@@ -40,3 +30,7 @@ install scons tool `apt-get install scons`
 which is used to run the unit test, will be generated under `test`
 
 - Run `scons --all -c` to clean.
+
+### Run 
+
+`./main`

diff --git a/include/Tracker.h b/include/Tracker.h
index aaa3d1b..d528670 100644 (file)
--- a/include/Tracker.h
+++ b/include/Tracker.h
@@ -24,10 +24,11 @@ namespace suanzi {
     class Tracker
     {
     public:
-        Tracker(const cv::Mat& image, int id = 0);
+        Tracker(const cv::Mat& image, const Detection& d, int id = 0);
         virtual ~Tracker();
         void updateState(const cv::Mat& image);
         void addPatch(PatchPtr p);
+        void correct(const cv::Mat& image, const Detection& d);
         TrackerStatus status;
         std::vector<PatchPtr> patches;
         Detection detection;

diff --git a/src/Engine.cpp b/src/Engine.cpp
index 4f67103..16a704d 100644 (file)
--- a/src/Engine.cpp
+++ b/src/Engine.cpp
@@ -127,12 +127,3 @@ void Engine::onPersonsIn(const std::vector<Person>& p)
     PersonsInfoPtr pp = std::make_shared<std::vector<Person>>(p);
     eventThread.enqueue(new PersonInEventWorkItem(this->observer_list, pp));
 }
-
-void Engine::onStatusChanged()
-{
-    Person pm, p2;
-    std::vector<Person> ps;
-    ps.push_back(pm);
-    ps.push_back(p2);
-    onPersonsOut(ps);
-}

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)

diff --git a/src/Tracker.cpp b/src/Tracker.cpp
index 8ab2414..e9353bd 100644 (file)
--- a/src/Tracker.cpp
+++ b/src/Tracker.cpp
@@ -6,25 +6,29 @@ using namespace std;
 
 static const int MaxLost = 5;
 
-Tracker::Tracker(const cv::Mat& image,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_<float>(4, 4) << 
+    this->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_<float>(2, 2) << 
+    this->kf.measurementMatrix = (Mat_<double>(2, 2) << 
                                                 1, 0, 0, 0,
                                                 0, 1, 0, 0);
 
-    this->kf.processNoiseCov = 1e-5 * Mat_<float>::eye(4, 4);
-    this->kf.measurementNoiseCov = 1e-1 * Mat_<float>::ones(2, 2);
-    this->kf.errorCovPost = 1. * Mat_<float>::ones(4, 4);
+    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);
+    //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);
 }
 
 Tracker::~Tracker()
@@ -51,3 +55,12 @@ void Tracker::addPatch(PatchPtr p)
 {
     this->patches.push_back(p);
 }
+
+void Tracker::correct(const cv::Mat& image, const Detection& detection)
+{
+    //kf.correct();
+    preStatus = status;
+    status = TrackerStatus::Active;
+    last_active = age;
+}
+