Fix scale weight calculation in big batch mode

[hercules2020/kcf.git] / src / kcf.cpp

diff --git a/src/kcf.cpp b/src/kcf.cpp
index 01c86e3f4733725e564b6e2d136eb6dec7b805a3..d46e2c6f60aca6c4ed4f9e524d6c63b0e66d41de 100644 (file)
--- a/src/kcf.cpp
+++ b/src/kcf.cpp
@@ -346,11 +346,10 @@ double KCF_Tracker::findMaxReponse(uint &max_idx, cv::Point2d &new_location) con
     }
     DEBUG_PRINT(new_location);
 
-    if (m_visual_debug) {
-        const bool rgb = true;
+    if (m_visual_debug != vd::NONE) {
         const bool fit = 1;
-        int w = fit ? 100 : (rgb ? fit_size.width  : feature_size.width);
-        int h = fit ? 100 : (rgb ? fit_size.height : feature_size.height);
+        int w = fit ? 100 : (m_visual_debug == vd::PATCH ? fit_size.width  : feature_size.width);
+        int h = fit ? 100 : (m_visual_debug == vd::PATCH ? fit_size.height : feature_size.height);
         cv::Mat all_responses((h + 1) * p_num_scales - 1,
                               (w + 1) * p_num_angles - 1, CV_32FC3, cv::Scalar::all(0));
         for (size_t i = 0; i < p_num_scales; ++i) {
@@ -359,7 +358,7 @@ double KCF_Tracker::findMaxReponse(uint &max_idx, cv::Point2d &new_location) con
                 cv::Mat tmp;
                 cv::Point2d cross = threadctx.IF_BIG_BATCH(max(i, j), max).loc;
                 cross = wrapAroundFreq(cross, max_response_map);
-                if (rgb) {
+                if (m_visual_debug == vd::PATCH ) {
                     threadctx.dbg_patch IF_BIG_BATCH((i, j),)
                             .convertTo(tmp, all_responses.type(), 1.0 / 255);
                     cross.x = cross.x / fit_size.width  * tmp.cols + tmp.cols / 2;
@@ -370,6 +369,7 @@ double KCF_Tracker::findMaxReponse(uint &max_idx, cv::Point2d &new_location) con
                     tmp /= max; // Normalize to 1
                     cross += cv::Point2d(tmp.size())/2;
                     tmp = circshift(tmp, -tmp.cols/2, -tmp.rows/2);
+                    //drawCross(tmp, cross, false);
                 }
                 bool green = false;
                 if (&*max_it == &IF_BIG_BATCH(threadctx.max(i, j), threadctx)) {
@@ -377,9 +377,10 @@ double KCF_Tracker::findMaxReponse(uint &max_idx, cv::Point2d &new_location) con
                     cross = new_location + cv::Point2d(tmp.size())/2;
                     green = true;
                 }
-                cross.x *= double(w)/tmp.cols;
-                cross.y *= double(h)/tmp.rows;
-                cv::resize(tmp, tmp, cv::Size(w, h));
+                // Move to the center of pixes (if scaling up) and scale
+                cross.x = (cross.x + 0.5) * double(w)/tmp.cols;
+                cross.y = (cross.y + 0.5) * double(h)/tmp.rows;
+                cv::resize(tmp, tmp, cv::Size(w, h)); //, 0, 0, cv::INTER_NEAREST);
                 drawCross(tmp, cross, green);
                 cv::Mat resp_roi(all_responses, cv::Rect(j * (w+1), i * (h+1), w, h));
                 tmp.copyTo(resp_roi);
@@ -493,7 +494,7 @@ void ThreadCtx::track(const KCF_Tracker &kcf, cv::Mat &input_rgb, cv::Mat &input
     for (size_t i = 0; i < max.size(); ++i) {
         cv::minMaxLoc(response.plane(i), &min_val, &max_val, &min_loc, &max_loc);
         DEBUG_PRINT(max_loc);
-        double weight = kcf.p_scales[i] < 1. ? kcf.p_scales[i] : 1. / kcf.p_scales[i];
+        double weight = max.scale(i) < 1. ? max.scale(i) : 1. / max.scale(i);
         max[i].response = max_val * weight;
         max[i].loc = max_loc;
     }
@@ -832,9 +833,12 @@ cv::Point2f KCF_Tracker::sub_pixel_peak(cv::Point &max_loc, cv::Mat &response) c
     float a = x.at<float>(0), b = x.at<float>(1), c = x.at<float>(2), d = x.at<float>(3), e = x.at<float>(4);
 
     cv::Point2f sub_peak(max_loc.x, max_loc.y);
-    if (b > 0 || b < 0) {
+    if (4 * a * c - b * b > p_floating_error) {
         sub_peak.y = ((2.f * a * e) / b - d) / (b - (4 * a * c) / b);
         sub_peak.x = (-2 * c * sub_peak.y - e) / b;
+        if (fabs(sub_peak.x - max_loc.x) > 1 ||
+            fabs(sub_peak.y - max_loc.y) > 1)
+            sub_peak = max_loc;
     }
 
     return sub_peak;
@@ -845,7 +849,7 @@ double KCF_Tracker::sub_grid_scale(uint max_index)
     cv::Mat A, fval;
     const auto &vec = d->IF_BIG_BATCH(threadctxs[0].max, threadctxs);
     uint index = vec.getScaleIdx(max_index);
-    uint angle_idx = vec.getAngleIdx(index);
+    uint angle_idx = vec.getAngleIdx(max_index);
 
     if (index >= vec.size()) {
         // interpolate from all values