Allow specifying visual debug mode from command line

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

diff --git a/src/kcf.cpp b/src/kcf.cpp
index 13ff374debb80f84de93c816cbfbaae9b95a28a9..5fbcdf09c21bae89ee1d8aeed8065a2c286a0945 100644 (file)
--- a/src/kcf.cpp
+++ b/src/kcf.cpp
@@ -294,6 +294,22 @@ void KCF_Tracker::resizeImgs(cv::Mat &input_rgb, cv::Mat &input_gray)
     }
 }
 
+static void drawCross(cv::Mat &img, cv::Point center, bool green)
+{
+    cv::Scalar col = green ? cv::Scalar(0, 1, 0) : cv::Scalar(0, 0, 1);
+    cv::line(img, cv::Point(center.x, 0), cv::Point(center.x, img.size().height), col);
+    cv::line(img, cv::Point(0, center.y), cv::Point(img.size().height, center.y), col);
+}
+
+static cv::Point2d wrapAroundFreq(cv::Point2d pt, cv::Mat &resp_map)
+{
+    if (pt.y > resp_map.rows / 2) // wrap around to negative half-space of vertical axis
+        pt.y = pt.y - resp_map.rows;
+    if (pt.x > resp_map.cols / 2) // same for horizontal axis
+        pt.x = pt.x - resp_map.cols;
+    return pt;
+}
+
 double KCF_Tracker::findMaxReponse(uint &max_idx, cv::Point2d &new_location) const
 {
     double max;
@@ -320,13 +336,9 @@ double KCF_Tracker::findMaxReponse(uint &max_idx, cv::Point2d &new_location) con
     DEBUG_PRINTM(max_response_map);
     DEBUG_PRINT(max_response_pt);
 
-    // sub pixel quadratic interpolation from neighbours
-    if (max_response_pt.y > max_response_map.rows / 2) // wrap around to negative half-space of vertical axis
-        max_response_pt.y = max_response_pt.y - max_response_map.rows;
-    if (max_response_pt.x > max_response_map.cols / 2) // same for horizontal axis
-        max_response_pt.x = max_response_pt.x - max_response_map.cols;
-
+    max_response_pt = wrapAroundFreq(max_response_pt, max_response_map);
 
+    // sub pixel quadratic interpolation from neighbours
     if (m_use_subpixel_localization) {
         new_location = sub_pixel_peak(max_response_pt, max_response_map);
     } else {
@@ -334,24 +346,40 @@ double KCF_Tracker::findMaxReponse(uint &max_idx, cv::Point2d &new_location) con
     }
     DEBUG_PRINT(new_location);
 
-    if (m_visual_debug) {
-        const bool rgb = true;
-        int type = rgb ? d->threadctxs[0].IF_BIG_BATCH(dbg_patch[0], dbg_patch).type()
-                       : d->threadctxs[0].response.type();
-        int w = true ? 100 : (rgb ? fit_size.width  : feature_size.width);
-        int h = true ? 100 : (rgb ? fit_size.height : feature_size.height);
+    if (m_visual_debug != vd::NONE) {
+        const bool fit = 1;
+        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, type, cv::Scalar::all(0));
+                              (w + 1) * p_num_angles - 1, CV_32FC3, cv::Scalar::all(0));
         for (size_t i = 0; i < p_num_scales; ++i) {
             for (size_t j = 0; j < p_num_angles; ++j) {
+                auto &threadctx = d->IF_BIG_BATCH(threadctxs[0], threadctxs(i, j));
                 cv::Mat tmp;
-                if (rgb) {
-                    tmp = d->IF_BIG_BATCH(threadctxs[0], threadctxs(i, j)).dbg_patch IF_BIG_BATCH((i, j),);
+                cv::Point2d cross = threadctx.IF_BIG_BATCH(max(i, j), max).loc;
+                cross = wrapAroundFreq(cross, max_response_map);
+                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;
+                    cross.y = cross.y / fit_size.height * tmp.rows + tmp.rows / 2;
                 } else {
-                    tmp = d->IF_BIG_BATCH(threadctxs[0], threadctxs(i, j)).response.plane(IF_BIG_BATCH(d->threadctxs[0].max.getIdx(i, j), 0));
+                    cv::cvtColor(threadctx.response.plane(IF_BIG_BATCH(threadctx.max.getIdx(i, j), 0)),
+                            tmp, cv::COLOR_GRAY2BGR);
+                    tmp /= max; // Normalize to 1
+                    cross += cv::Point2d(tmp.size())/2;
                     tmp = circshift(tmp, -tmp.cols/2, -tmp.rows/2);
                 }
+                bool green = false;
+                if (&*max_it == &IF_BIG_BATCH(threadctx.max(i, j), threadctx)) {
+                    // Show the green cross at position of sub-pixel interpolation (if enabled)
+                    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));
+                drawCross(tmp, cross, green);
                 cv::Mat resp_roi(all_responses, cv::Rect(j * (w+1), i * (h+1), w, h));
                 tmp.copyTo(resp_roi);
             }
@@ -396,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;
 
@@ -413,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);
@@ -565,8 +598,8 @@ cv::Mat KCF_Tracker::gaussian_shaped_labels(double sigma, int dim1, int dim2)
 
 cv::Mat KCF_Tracker::circshift(const cv::Mat &patch, int x_rot, int y_rot) const
 {
-    cv::Mat rot_patch(patch.size(), CV_32FC1);
-    cv::Mat tmp_x_rot(patch.size(), CV_32FC1);
+    cv::Mat rot_patch(patch.size(), patch.type());
+    cv::Mat tmp_x_rot(patch.size(), patch.type());
 
     // circular rotate x-axis
     if (x_rot < 0) {