X-Git-Url: http://47.100.26.94:8080/?a=blobdiff_plain;f=src%2FMultiTracker.cpp;h=bbfc6f3b4520dcfac5509c2eb27539227092ff0f;hb=81741fb5b3fe86bf29a130f367ea102e3aa99b0b;hp=c87395ef3415c1da78da878eaa40aa2b9fcc568d;hpb=b5f0328f4054d19fcf8a6b870d5448be8087d29c;p=trackerpp.git diff --git a/src/MultiTracker.cpp b/src/MultiTracker.cpp index c87395e..bbfc6f3 100644 --- a/src/MultiTracker.cpp +++ b/src/MultiTracker.cpp @@ -3,6 +3,7 @@ #include #include "hungarian.h" #include "Logger.h" +#include "Utils.h" using namespace suanzi; using namespace cv; @@ -10,19 +11,17 @@ using namespace std; static const std::string TAG = "MultiTracker"; static const cv::Size PREFERRED_SIZE = Size(64, 128); - -#define MaxCost 100000 +static const double MaxCost = 100000; +static const double ProbThreshold = 0.05; MultiTracker::MultiTracker(EngineWPtr e) : engine(e) { LOG_DEBUG(TAG, "init - loading model.pkl"); - predictor = PredictorWrapper::create("./python/model.pkl"); + predictor = PredictorWrapperPtr(new PredictorWrapper()); + predictor->load("./resources/model.pkl"); predictor->dump(); this->descriptor = {Size(64, 128), Size(16, 16), Size(8, 8), Size(8, 8), 9}; - - std::vector ff (40, 1); - double prob = predictor->predict(4, ff); } MultiTracker::~MultiTracker() @@ -31,25 +30,6 @@ MultiTracker::~MultiTracker() trackers.clear(); } -static Rect getRectInDetection(const Detection& d) -{ - Rect r; - r.x = d.center_x - d.width / 2; - r.y = d.center_y - d.height / 2; - r.width = d.width; - r.height = d.height; - return r; -} - -static double calc_iou_ratio(const Detection& d1, const Detection& d2) -{ - Rect r1 = getRectInDetection (d1); - Rect r2 = getRectInDetection (d2); - Rect r_inner = r1 & r1; - Rect r_union = r1 | r2; - return 1.0 * r_inner.area() / r_union.area(); -} - static std::vector similarity(const PatchPtr p1, const PatchPtr p2) { std::vector feature; @@ -79,12 +59,11 @@ static std::vector similarity(const PatchPtr p1, const PatchPtr p2) double center_distance = sqrt(pow((d1.center_x - d2.center_x), 2) + pow((d1.center_y - d2.center_y), 2)); feature.push_back(center_distance / (d1.width + d1.height + d2.width + d2.height) * 4); - feature.push_back(calc_iou_ratio(d1, d2)); + feature.push_back(calc_iou_ratio(getRectInDetection(d1), getRectInDetection(d2))); return feature; } - double MultiTracker::distance(TrackerPtr tracker, const cv::Mat& image, const Detection& d) { PatchPtr patch = createPatch(image, d); @@ -95,31 +74,34 @@ 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(Tracker::MaxPatch - 1, features); // TODO why is MaxPatch-1 + if (prob > ProbThreshold) + return -log(prob); + else + return MaxCost; } -static long cc = 0; +static float calc_iou_ratio(const Detection& d1, const Detection& d2) +{ + return calc_iou_ratio(getRectInDetection(d1), getRectInDetection(d2)); +} void MultiTracker::update(unsigned int total, const Detection* detections, const Mat& image) { - ////// - if ((cc % 50) == 0){ - if (EnginePtr e = engine.lock()){ - e->onStatusChanged(); - } + // predict trackers, update trackers using kalman filter + for (auto t : trackers){ + t->predict(); } - cc++; - ////// + // 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], detections[j]) < -0.1) - // cost_matrix(i, j) = MaxCost; - //else + 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]); } } @@ -127,30 +109,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); + } + } + + // 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 + } } } - // create new trackers for new detections - for (auto i : unmatched_detection){ - TrackerPtr t (new Tracker(image)); - this->trackers.push_back(t); + + // handle unmatched trackers + for (auto t : unmatched_trackers){ + t->updateState(image); + } + + // 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); + } } - Detection dd; + // 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); + } + } - PatchPtr pp = createPatch(image, dd); + // 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)