+ // predict trackers, update trackers using kalman filter
+ for (auto t : trackers){
+ t->predict();
+ }
+
+ // match the trackers with the detections using linear sum assignment (hungarian)
+ int row = trackers.size();
+ int col = total;
+ Eigen::MatrixXi cost_matrix = Eigen::MatrixXi::Zero(row, col);
+ for (int i = 0; i < row; i++){
+ for (int j = 0; j < col; j++){
+ if (calc_iou_ratio(trackers[i]->detection, detections[j]) < -0.1)
+ cost_matrix(i, j) = MaxCost;
+ else
+ cost_matrix(i, j) = distance(trackers[i], image, detections[j]);
+ }
+ }
+
+ Eigen::VectorXi tracker_inds, bb_inds;
+ linear_sum_assignment(cost_matrix, tracker_inds, bb_inds);
+
+ set<TrackerPtr> unmatched_trackers;
+ set<int> unmatch_bbs_indices;
+
+ for(unsigned int i = 0; i < trackers.size(); i++){
+ if (!(tracker_inds.array() == i).any()){
+ unmatched_trackers.insert(trackers[i]);
+ }
+ }
+ for (unsigned int j = 0; j < total; j++){
+ if (!(bb_inds.array() == j).any()){
+ unmatch_bbs_indices.insert(j);
+ }
+ }
+
+ // 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
+ }
+ }
+ }
+
+ // handle unmatched trackers
+ for (auto t : unmatched_trackers){
+ t->updateState(image);
+ }
+
+ // 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);
+ }
+ }