Move patch capture for visual debug after scale step

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

diff --git a/src/kcf.cpp b/src/kcf.cpp
index 64baea78d4f3c8105459c9ec1c03dbe082631791..399ef4ef2d572b8cbac5a464fe7d1a95f775a83d 100644 (file)
--- a/src/kcf.cpp
+++ b/src/kcf.cpp
@@ -426,7 +426,8 @@ void KCF_Tracker::track(cv::Mat &img)
     uint max_idx;
     max_response = findMaxReponse(max_idx, new_location);
 
-    double angle_change = d->IF_BIG_BATCH(threadctxs[0].max, threadctxs).angle(max_idx);
+    double angle_change = m_use_subgrid_angle ? sub_grid_angle(max_idx)
+                                              : d->IF_BIG_BATCH(threadctxs[0].max, threadctxs).angle(max_idx);
     p_current_angle += angle_change;
 
     new_location.x = new_location.x * cos(-p_current_angle/180*M_PI) + new_location.y * sin(-p_current_angle/180*M_PI);
@@ -494,7 +495,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;
     }
@@ -520,9 +521,6 @@ cv::Mat KCF_Tracker::get_features(cv::Mat &input_rgb, cv::Mat &input_gray, cv::M
     cv::Mat patch_gray = get_subwindow(input_gray, cx, cy, scaled.width, scaled.height, angle);
     cv::Mat patch_rgb = get_subwindow(input_rgb, cx, cy, scaled.width, scaled.height, angle);
 
-    if (dbg_patch)
-        patch_rgb.copyTo(*dbg_patch);
-
     // resize to default size
     if (scaled.area() > fit_size.area()) {
         // if we downsample use  INTER_AREA interpolation
@@ -547,6 +545,9 @@ cv::Mat KCF_Tracker::get_features(cv::Mat &input_rgb, cv::Mat &input_gray, cv::M
         }
     }
 
+    if (dbg_patch)
+        patch_rgb.copyTo(*dbg_patch);
+
     if (m_use_color && input_rgb.channels() == 3) {
         // use rgb color space
         cv::Mat patch_rgb_norm;
@@ -833,9 +834,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;
@@ -889,3 +893,52 @@ double KCF_Tracker::sub_grid_scale(uint max_index)
         scale = -b / (2 * a);
     return scale;
 }
+
+double KCF_Tracker::sub_grid_angle(uint max_index)
+{
+    cv::Mat A, fval;
+    const auto &vec = d->IF_BIG_BATCH(threadctxs[0].max, threadctxs);
+    uint scale_idx = vec.getScaleIdx(max_index);
+    uint index = vec.getAngleIdx(max_index);
+
+    if (index >= vec.size()) {
+        // interpolate from all values
+        // fit 1d quadratic function f(x) = a*x^2 + b*x + c
+        A.create(p_angles.size(), 3, CV_32FC1);
+        fval.create(p_angles.size(), 1, CV_32FC1);
+        for (size_t i = 0; i < p_angles.size(); ++i) {
+            A.at<float>(i, 0) = float(p_angles[i] * p_angles[i]);
+            A.at<float>(i, 1) = float(p_angles[i]);
+            A.at<float>(i, 2) = 1;
+            fval.at<float>(i) = d->IF_BIG_BATCH(threadctxs[0].max[i].response, threadctxs(scale_idx, i).max.response);
+        }
+    } else {
+        // only from neighbours
+        if (index == 0 || index == p_angles.size() - 1)
+           return p_angles[index];
+
+        A = (cv::Mat_<float>(3, 3) <<
+             p_angles[index - 1] * p_angles[index - 1], p_angles[index - 1], 1,
+             p_angles[index + 0] * p_angles[index + 0], p_angles[index + 0], 1,
+             p_angles[index + 1] * p_angles[index + 1], p_angles[index + 1], 1);
+#ifdef BIG_BATCH
+        fval = (cv::Mat_<float>(3, 1) <<
+                d->threadctxs[0].max(scale_idx, index - 1).response,
+                d->threadctxs[0].max(scale_idx, index + 0).response,
+                d->threadctxs[0].max(scale_idx, index + 1).response);
+#else
+        fval = (cv::Mat_<float>(3, 1) <<
+                d->threadctxs(scale_idx, index - 1).max.response,
+                d->threadctxs(scale_idx, index + 0).max.response,
+                d->threadctxs(scale_idx, index + 1).max.response);
+#endif
+    }
+
+    cv::Mat x;
+    cv::solve(A, fval, x, cv::DECOMP_SVD);
+    float a = x.at<float>(0), b = x.at<float>(1);
+    double angle = p_angles[index];
+    if (a > 0 || a < 0)
+        angle = -b / (2 * a);
+    return angle;
+}