1 #include "MultiTracker.h"
9 using namespace suanzi;
13 static const std::string TAG = "MultiTracker";
14 static const cv::Size PREFERRED_SIZE = Size(64, 128);
15 static const double MaxCost = 100000;
16 static const double ProbThreshold = 0.05;
17 static const int MaxTrackers = 100;
19 MultiTracker::MultiTracker(EngineWPtr e)
22 LOG_DEBUG(TAG, "init - loading model.pkl");
23 predictor = PredictorWrapperPtr(new PredictorWrapper());
24 predictor->load("./resources/model.pkl");
26 this->descriptor = {Size(64, 128), Size(16, 16), Size(8, 8), Size(8, 8), 9};
29 MultiTracker::~MultiTracker()
35 static std::vector<double> similarity(const PatchPtr p1, const PatchPtr p2)
37 std::vector<double> feature;
38 cv::Mat im1(PREFERRED_SIZE, p1->image_crop.type());
39 cv::Mat im2(PREFERRED_SIZE, p2->image_crop.type());
40 cv::resize(p1->image_crop, im1, im1.size());
41 cv::resize(p2->image_crop, im2, im2.size());
43 cv::Mat result = Mat::zeros(1,1, CV_64F);
45 cv::matchTemplate(im1, im2, result, CV_TM_CCOEFF_NORMED);
46 feature.push_back(result.at<double>(0, 0));
47 cv::matchTemplate(im1, im2, result, CV_TM_CCORR_NORMED);
48 feature.push_back(result.at<double>(0, 0));
51 vector<double>& f1_hog = p1->features.first; Mat f1_hue = p1->features.second;
52 vector<double>& f2_hog = p1->features.first; Mat f2_hue = p1->features.second;
53 feature.push_back(distance_cosine(Eigen::Map<Eigen::VectorXd>(f1_hog.data(), f1_hog.size()),
54 Eigen::Map<Eigen::VectorXd>(f2_hog.data(), f2_hog.size())));
55 feature.push_back(distance_euclidean(Eigen::Map<Eigen::VectorXd>(f1_hog.data(), f1_hog.size()),
56 Eigen::Map<Eigen::VectorXd>(f2_hog.data(), f2_hog.size())));
57 feature.push_back(compareHist(f1_hue, f2_hue, HISTCMP_CORREL));
58 feature.push_back(compareHist(f1_hue, f2_hue, HISTCMP_HELLINGER));
60 Detection& d1 = p1->detection;
61 Detection& d2 = p2->detection;
63 double center_distance = sqrt(pow((d1.center_x - d2.center_x), 2) + pow((d1.center_y - d2.center_y), 2));
64 feature.push_back(center_distance / (d1.width + d1.height + d2.width + d2.height) * 4);
66 feature.push_back(calc_iou_ratio(getRectInDetection(d1), getRectInDetection(d2)));
68 // for (auto i : feature){
71 // throw overflow_error("Nan in feature");
79 double MultiTracker::distance(TrackerPtr tracker, const cv::Mat& image, const Detection& d)
81 PatchPtr patch = createPatch(image, d);
82 std::vector<double> features;
84 std::vector<double> ss;
85 for (auto i : tracker->patches){
86 ss = similarity(i, patch);
87 features.insert(features.end(), ss.begin(), ss.end());
89 double prob = predictor->predict(tracker->patches.size() - 1, features); // TODO why is MaxPatch-1
90 if (prob > ProbThreshold)
96 static float calc_iou_ratio(const Detection& d1, const Detection& d2)
98 return calc_iou_ratio(getRectInDetection(d1), getRectInDetection(d2));
101 void MultiTracker::update(unsigned int total, const Detection* detections, const Mat& image)
103 // predict trackers, update trackers using kalman filter
104 for (auto t : trackers){
108 // match the trackers with the detections using linear sum assignment (hungarian)
109 int row = trackers.size();
111 Eigen::MatrixXi cost_matrix = Eigen::MatrixXi::Zero(row, col);
112 for (int i = 0; i < row; i++){
113 for (int j = 0; j < col; j++){
114 if (calc_iou_ratio(trackers[i]->detection, detections[j]) < -0.1)
115 cost_matrix(i, j) = MaxCost;
117 cost_matrix(i, j) = distance(trackers[i], image, detections[j]);
121 Eigen::VectorXi tracker_inds, bb_inds;
122 linear_sum_assignment(cost_matrix, tracker_inds, bb_inds);
124 set<TrackerPtr> unmatched_trackers;
125 set<int> unmatch_bbs_indices;
127 for(unsigned int i = 0; i < trackers.size(); i++){
128 if (!(tracker_inds.array() == i).any()){
129 unmatched_trackers.insert(trackers[i]);
132 for (unsigned int j = 0; j < total; j++){
133 if (!(bb_inds.array() == j).any()){
134 unmatch_bbs_indices.insert(j);
138 // handle matched trackers
139 for (unsigned int i = 0; i < tracker_inds.size(); i++){
140 for (int j = 0; j < bb_inds.size(); j++){
141 int rr = tracker_inds(i);
143 TrackerPtr tracker = trackers[rr];
144 const Detection& detect = detections[cc];
145 if (cost_matrix(rr, cc) < MaxCost){
146 tracker->correct(image, detect);
147 tracker->addPatch(createPatch(image, detect));
149 unmatched_trackers.insert(tracker); // failed trackers
150 unmatch_bbs_indices.insert(cc); // filed detection
155 // handle unmatched trackers
156 for (auto t : unmatched_trackers){
157 t->updateState(image);
160 // handle unmatched detections - Create new trackers
161 vector<Person> inPersons;
162 for (auto i : unmatch_bbs_indices){
163 TrackerPtr new_tracker (new Tracker(image, detections[i]));
164 new_tracker->addPatch(createPatch(image, detections[i]));
165 addTracker(new_tracker);
167 inPersons.push_back(test);
170 // callback and notify engine - persons in
171 if (inPersons.size() > 0){
172 if (auto e = engine.lock()){
173 e->onPersonsIn(inPersons);
177 // Delete lost trackers
178 vector<Person> outPersons;
179 for (auto it = trackers.begin(); it < trackers.end(); it++){
180 if ((*it)->status == TrackerStatus::Delete){
182 outPersons.push_back(test);
187 // callback and notify engine - persons out
188 if (outPersons.size() > 0){
189 if (auto e = engine.lock()){
190 e->onPersonsOut(outPersons);
195 static cv::Mat image_crop(const cv::Mat& image, const Detection& bb)
197 return image(getRectInDetection(bb));
200 PatchPtr MultiTracker::createPatch(const Mat& image, const Detection& detect)
202 PatchPtr patch(new Patch());
204 // calculate hog descriptors, size is 3780
206 im = image_crop(image, detect);
207 resize(im, im2, PREFERRED_SIZE);
208 vector<float> feature_hog;
209 this->descriptor.compute(im2, feature_hog);
211 // calculate histogram, size is (64 x 45)
213 cvtColor(im, hsv, COLOR_BGR2HSV);
214 int channels[] = {0, 1};
215 int histSize[] = {45, 64};
216 float hranges[] = {0, 180};
217 float sranges[] = {0, 256};
218 const float* ranges[] = {hranges, sranges};
219 calcHist(&hsv, 1, channels, Mat(), hist, 2, histSize, ranges, true, false);
221 patch->image_crop = im.clone();
222 patch->detection = detect;
223 std::vector<double> feature_hog_double (feature_hog.begin(), feature_hog.end()); // convert to double
224 patch->features = std::make_pair(feature_hog_double, hist);
228 void MultiTracker::addTracker(TrackerPtr t)
230 trackers.insert(trackers.begin(), t);
231 if(trackers.size() > MaxTrackers){
232 LOG_ERROR(TAG, "trackers reaches the maximum " + to_string(MaxTrackers));