From a80807eb35cc0ff70a9afdd6b7db2f38cb686683 Mon Sep 17 00:00:00 2001 From: Peng Li Date: Mon, 23 Jul 2018 12:17:39 +0800 Subject: [PATCH] Add callback from multitracker to engine --- README.md | 14 +++------- include/Tracker.h | 3 +- src/Engine.cpp | 9 ------ src/MultiTracker.cpp | 77 +++++++++++++++++++++++++++++++++++++++++----------- src/Tracker.cpp | 25 +++++++++++++---- 5 files changed, 86 insertions(+), 42 deletions(-) diff --git a/README.md b/README.md index b40c9ee..e167559 100644 --- 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 --- 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 patches; Detection detection; diff --git a/src/Engine.cpp b/src/Engine.cpp index 4f67103..16a704d 100644 --- a/src/Engine.cpp +++ b/src/Engine.cpp @@ -127,12 +127,3 @@ void Engine::onPersonsIn(const std::vector& p) PersonsInfoPtr pp = std::make_shared>(p); eventThread.enqueue(new PersonInEventWorkItem(this->observer_list, pp)); } - -void Engine::onStatusChanged() -{ - Person pm, p2; - std::vector 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 --- 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 unmatched_trackers; - for (int i = 0; i < row; i++){ + set unmatched_trackers; + set 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 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 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 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 --- 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_(4, 4) << + this->kf.transitionMatrix = (Mat_(4, 4) << 1, 0, 1, 0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0, 0, 1); - this->kf.measurementMatrix = (Mat_(2, 2) << + this->kf.measurementMatrix = (Mat_(2, 2) << 1, 0, 0, 0, 0, 1, 0, 0); - this->kf.processNoiseCov = 1e-5 * Mat_::eye(4, 4); - this->kf.measurementNoiseCov = 1e-1 * Mat_::ones(2, 2); - this->kf.errorCovPost = 1. * Mat_::ones(4, 4); + this->kf.processNoiseCov = 1e-5 * Mat_::eye(4, 4); + this->kf.measurementNoiseCov = 1e-1 * Mat_::ones(2, 2); + this->kf.errorCovPost = 1. * Mat_::ones(4, 4); + //this->kf.statePre = 0.1 * Matx_::randn(4, 1); + //??? TODO + randn(this->kf.statePre, Scalar::all(0), Scalar::all(0.1)); + this->kf.statePost = (Mat_(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; +} + -- 2.11.0