Fix sub_grid_scale calculation

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

diff --git a/src/kcf.cpp b/src/kcf.cpp
index 3961d9c52c9c015994bfa7b9beeecdf155ae3516..66689250c0af5e8779a26efd01731c7005eecb4e 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;
@@ -425,6 +424,12 @@ 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);
+    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);
+    new_location.y = new_location.y * cos(-p_current_angle/180*M_PI) - new_location.x * sin(-p_current_angle/180*M_PI);
+
     new_location.x *= double(p_windows_size.width) / fit_size.width;
     new_location.y *= double(p_windows_size.height) / fit_size.height;
 
@@ -442,7 +447,6 @@ void KCF_Tracker::track(cv::Mat &img)
 
     clamp2(p_current_scale, p_min_max_scale[0], p_min_max_scale[1]);
 
-    p_current_angle += d->IF_BIG_BATCH(threadctxs[0].max, threadctxs).angle(max_idx);
 
     // train at newly estimated target position
     train(input_rgb, input_gray, p_interp_factor);
@@ -840,7 +844,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