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)
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)
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()
{
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;
+}
+