+void KCF_Tracker::resizeImgs(cv::Mat &input_rgb, cv::Mat &input_gray)
+{
+ if (p_resize_image) {
+ cv::resize(input_gray, input_gray, cv::Size(0, 0), p_downscale_factor, p_downscale_factor, cv::INTER_AREA);
+ cv::resize(input_rgb, input_rgb, cv::Size(0, 0), p_downscale_factor, p_downscale_factor, cv::INTER_AREA);
+ }
+}
+
+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;
+ const auto &vec = IF_BIG_BATCH(d->threadctxs[0].max, d->threadctxs);
+
+#ifndef BIG_BATCH
+ auto max_it = std::max_element(vec.begin(), vec.end(),
+ [](const ThreadCtx &a, const ThreadCtx &b)
+ { return a.max.response < b.max.response; });
+#else
+ auto max_it = std::max_element(vec.begin(), vec.end(),
+ [](const ThreadCtx::Max &a, const ThreadCtx::Max &b)
+ { return a.response < b.response; });
+#endif
+ assert(max_it != vec.end());
+ max = max_it->IF_BIG_BATCH(response, max.response);
+
+ max_idx = std::distance(vec.begin(), max_it);
+
+ cv::Point2i max_response_pt = IF_BIG_BATCH(max_it->loc, max_it->max.loc);
+ cv::Mat max_response_map = IF_BIG_BATCH(d->threadctxs[0].response.plane(max_idx),
+ max_it->response.plane(0));
+
+ DEBUG_PRINTM(max_response_map);
+ DEBUG_PRINT(max_response_pt);
+
+ 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 {
+ new_location = max_response_pt;
+ }
+ DEBUG_PRINT(new_location);
+
+ 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, 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;
+ 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 {
+ 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);
+ //drawCross(tmp, cross, false);
+ }
+ 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;
+ }
+ // 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);
+ }
+ }
+ cv::namedWindow("KCF visual debug", CV_WINDOW_AUTOSIZE);
+ cv::imshow("KCF visual debug", all_responses);
+ }
+
+ return max;
+}
+