Add assertion about found maximum response

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

diff --git a/src/kcf.cpp b/src/kcf.cpp
index 5aeb1612d047f4cfa502ccfa943ba8af6b2c46a7..2136387fc10dc84c58627a3e377fde0dfc67c802 100644 (file)
--- a/src/kcf.cpp
+++ b/src/kcf.cpp
@@ -4,6 +4,7 @@
 #include <algorithm>
 #include "threadctx.hpp"
 #include "debug.h"
+#include <limits>
 
 #ifdef FFTW
 #include "fft_fftw.h"
@@ -34,7 +35,7 @@ void clamp2(T& n, const T& lower, const T& upper)
     n = std::max(lower, std::min(n, upper));
 }
 
-#if CV_VERSION_EPOCH < 3
+#if CV_MAJOR_VERSION < 3
 template<typename _Tp> static inline
 cv::Size_<_Tp> operator / (const cv::Size_<_Tp>& a, _Tp b)
 {
@@ -297,6 +298,8 @@ void KCF_Tracker::resizeImgs(cv::Mat &input_rgb, cv::Mat &input_gray)
 double KCF_Tracker::findMaxReponse(uint &max_idx, cv::Point2d &new_location) const
 {
     double max = -1.;
+    max_idx = std::numeric_limits<uint>::max();
+
 #ifndef BIG_BATCH
     for (uint j = 0; j < d.threadctxs.size(); ++j) {
         if (d.threadctxs[j].max.response > max) {
@@ -312,6 +315,26 @@ double KCF_Tracker::findMaxReponse(uint &max_idx, cv::Point2d &new_location) con
         }
     }
 #endif
+    assert(max_idx < IF_BIG_BATCH(p_scales.size(), d.threadctxs.size()));
+
+    if (m_visual_debug) {
+        int w = 100; //feature_size.width;
+        int h = 100; //feature_size.height;
+        cv::Mat all_responses(h * p_num_scales, w * p_num_angles,
+                              d.threadctxs[0].response.type(), cv::Scalar::all(0));
+        for (size_t i = 0; i < p_num_scales; ++i) {
+            for (size_t j = 0; j < p_num_angles; ++j) {
+                cv::Mat tmp = d.threadctxs[IF_BIG_BATCH(0, p_num_angles * i + j)].response.plane(IF_BIG_BATCH(p_num_angles * i + j, 0));
+                tmp = circshift(tmp, -tmp.cols/2, -tmp.rows/2);
+                cv::resize(tmp, tmp, cv::Size(w, h));
+                cv::Mat resp_roi(all_responses, cv::Rect(j * w, i * h, w, h));
+                tmp.copyTo(resp_roi);
+            }
+        }
+        cv::namedWindow("All responses", CV_WINDOW_AUTOSIZE);
+        cv::imshow("All responses", all_responses);
+    }
+
     cv::Point2i &max_response_pt = IF_BIG_BATCH(d.threadctxs[0].max[max_idx].loc,        d.threadctxs[max_idx].max.loc);
     cv::Mat max_response_map     = IF_BIG_BATCH(d.threadctxs[0].response.plane(max_idx), d.threadctxs[max_idx].response.plane(0));
 
@@ -527,7 +550,7 @@ cv::Mat KCF_Tracker::gaussian_shaped_labels(double sigma, int dim1, int dim2)
     return rot_labels;
 }
 
-cv::Mat KCF_Tracker::circshift(const cv::Mat &patch, int x_rot, int y_rot)
+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);