X-Git-Url: http://rtime.felk.cvut.cz/gitweb/hercules2020/kcf.git/blobdiff_plain/2ab68626cf544342170dd2a339af2eb7ea7d95d9..f498ac81b844b016e80ce5ac694c43107a418625:/src/kcf.cpp diff --git a/src/kcf.cpp b/src/kcf.cpp index 2b5f937..5fbcdf0 100644 --- a/src/kcf.cpp +++ b/src/kcf.cpp @@ -4,6 +4,7 @@ #include #include "threadctx.hpp" #include "debug.h" +#include #ifdef FFTW #include "fft_fftw.h" @@ -40,26 +41,40 @@ cv::Size_<_Tp> operator / (const cv::Size_<_Tp>& a, _Tp b) { return cv::Size_<_Tp>(a.width / b, a.height / b); } + +template static inline +cv::Point_<_Tp> operator / (const cv::Point_<_Tp>& a, double b) +{ + return cv::Point_<_Tp>(a.x / b, a.y / b); +} + #endif class Kcf_Tracker_Private { friend KCF_Tracker; + + Kcf_Tracker_Private(const KCF_Tracker &kcf) : kcf(kcf) {} + + const KCF_Tracker &kcf; +#ifdef BIG_BATCH std::vector threadctxs; +#else + ScaleRotVector threadctxs{kcf.p_scales, kcf.p_angles}; +#endif }; KCF_Tracker::KCF_Tracker(double padding, double kernel_sigma, double lambda, double interp_factor, double output_sigma_factor, int cell_size) : p_cell_size(cell_size), fft(*new FFT()), p_padding(padding), p_output_sigma_factor(output_sigma_factor), p_kernel_sigma(kernel_sigma), - p_lambda(lambda), p_interp_factor(interp_factor), d(*new Kcf_Tracker_Private) + p_lambda(lambda), p_interp_factor(interp_factor) { } -KCF_Tracker::KCF_Tracker() : fft(*new FFT()), d(*new Kcf_Tracker_Private) {} +KCF_Tracker::KCF_Tracker() : fft(*new FFT()) {} KCF_Tracker::~KCF_Tracker() { delete &fft; - delete &d; } void KCF_Tracker::train(cv::Mat input_rgb, cv::Mat input_gray, double interp_factor) @@ -67,36 +82,30 @@ void KCF_Tracker::train(cv::Mat input_rgb, cv::Mat input_gray, double interp_fac TRACE(""); // obtain a sub-window for training - // TODO: Move Mats outside from here - MatScaleFeats patch_feats(1, p_num_of_feats, feature_size); - DEBUG_PRINT(patch_feats); - MatScaleFeats temp(1, p_num_of_feats, feature_size); - get_features(input_rgb, input_gray, p_current_center.x, p_current_center.y, + get_features(input_rgb, input_gray, nullptr, p_current_center.x, p_current_center.y, p_windows_size.width, p_windows_size.height, - p_current_scale).copyTo(patch_feats.scale(0)); - DEBUG_PRINT(patch_feats); - fft.forward_window(patch_feats, p_xf, temp); - DEBUG_PRINTM(p_xf); - p_model_xf = p_model_xf * (1. - interp_factor) + p_xf * interp_factor; - DEBUG_PRINTM(p_model_xf); - - ComplexMat alphaf_num, alphaf_den; + p_current_scale, p_current_angle).copyTo(model->patch_feats.scale(0)); + DEBUG_PRINT(model->patch_feats); + fft.forward_window(model->patch_feats, model->xf, model->temp); + DEBUG_PRINTM(model->xf); + model->model_xf = model->model_xf * (1. - interp_factor) + model->xf * interp_factor; + DEBUG_PRINTM(model->model_xf); if (m_use_linearkernel) { - ComplexMat xfconj = p_xf.conj(); - alphaf_num = xfconj.mul(p_yf); - alphaf_den = (p_xf * xfconj); + ComplexMat xfconj = model->xf.conj(); + model->model_alphaf_num = xfconj.mul(model->yf); + model->model_alphaf_den = (model->xf * xfconj); } else { // Kernel Ridge Regression, calculate alphas (in Fourier domain) cv::Size sz(Fft::freq_size(feature_size)); ComplexMat kf(sz.height, sz.width, 1); - (*gaussian_correlation)(kf, p_model_xf, p_model_xf, p_kernel_sigma, true, *this); + (*gaussian_correlation)(kf, model->model_xf, model->model_xf, p_kernel_sigma, true, *this); DEBUG_PRINTM(kf); - p_model_alphaf_num = p_yf * kf; - p_model_alphaf_den = kf * (kf + p_lambda); + model->model_alphaf_num = model->yf * kf; + model->model_alphaf_den = kf * (kf + p_lambda); } - p_model_alphaf = p_model_alphaf_num / p_model_alphaf_den; - DEBUG_PRINTM(p_model_alphaf); + model->model_alphaf = model->model_alphaf_num / model->model_alphaf_den; + DEBUG_PRINTM(model->model_alphaf); // p_model_alphaf = p_yf / (kf + p_lambda); //equation for fast training } @@ -182,42 +191,32 @@ void KCF_Tracker::init(cv::Mat &img, const cv::Rect &bbox, int fit_size_x, int f feature_size = fit_size / p_cell_size; p_scales.clear(); - for (int i = -int(p_num_scales) / 2; i <= int(p_num_scales) / 2; ++i) + for (int i = -int(p_num_scales - 1) / 2; i <= int(p_num_scales) / 2; ++i) p_scales.push_back(std::pow(p_scale_step, i)); -#ifdef CUFFT - if (Fft::freq_size(feature_size).area() > 1024) { - std::cerr << "Window after forward FFT is too big for CUDA kernels. Plese use -f to set " - "the window dimensions so its size is less or equal to " - << 1024 * p_cell_size * p_cell_size * 2 + 1 - << " pixels. Currently the size of the window is: " << fit_size - << " which is " << fit_size.area() << " pixels. " << std::endl; - std::exit(EXIT_FAILURE); - } + p_angles.clear(); + for (int i = -int(p_num_angles - 1) / 2; i <= int(p_num_angles) / 2; ++i) + p_angles.push_back(i * p_angle_step); +#ifdef CUFFT if (m_use_linearkernel) { std::cerr << "cuFFT supports only Gaussian kernel." << std::endl; std::exit(EXIT_FAILURE); } #endif -#if defined(CUFFT) || defined(FFTW) - uint width = feature_size.width / 2 + 1; -#else - uint width = feature_size.width; -#endif - p_model_xf.create(feature_size.height, width, p_num_of_feats); - p_yf.create(feature_size.height, width, 1); - p_xf.create(feature_size.height, width, p_num_of_feats); + model.reset(new Model(feature_size, p_num_of_feats)); + d.reset(new Kcf_Tracker_Private(*this)); #ifndef BIG_BATCH for (auto scale: p_scales) - d.threadctxs.emplace_back(feature_size, p_num_of_feats, scale); + for (auto angle : p_angles) + d->threadctxs.emplace_back(feature_size, p_num_of_feats, scale, angle); #else - d.threadctxs.emplace_back(feature_size, p_num_of_feats, p_num_scales); + d->threadctxs.emplace_back(feature_size, p_num_of_feats, p_scales, p_angles); #endif - gaussian_correlation.reset(new GaussianCorrelation(1, feature_size)); + gaussian_correlation.reset(new GaussianCorrelation(1, p_num_of_feats, feature_size)); p_current_center = p_init_pose.center(); p_current_scale = 1.; @@ -240,14 +239,14 @@ void KCF_Tracker::init(cv::Mat &img, const cv::Rect &bbox, int fit_size_x, int f p_output_sigma = std::sqrt(p_init_pose.w * p_init_pose.h * double(fit_size.area()) / p_windows_size.area()) * p_output_sigma_factor / p_cell_size; - fft.init(feature_size.width, feature_size.height, p_num_of_feats, p_num_scales); + fft.init(feature_size.width, feature_size.height, p_num_of_feats, p_num_scales * p_num_angles); fft.set_window(MatDynMem(cosine_window_function(feature_size.width, feature_size.height))); // window weights, i.e. labels MatScales gsl(1, feature_size); gaussian_shaped_labels(p_output_sigma, feature_size.width, feature_size.height).copyTo(gsl.plane(0)); - fft.forward(gsl, p_yf); - DEBUG_PRINTM(p_yf); + fft.forward(gsl, model->yf); + DEBUG_PRINTM(model->yf); // train initial model train(input_rgb, input_gray, 1.0); @@ -274,6 +273,7 @@ BBox_c KCF_Tracker::getBBox() tmp.cy = p_current_center.y; tmp.w = p_init_pose.w * p_current_scale; tmp.h = p_init_pose.h * p_current_scale; + tmp.a = p_current_angle; if (p_resize_image) tmp.scale(1 / p_downscale_factor); @@ -294,76 +294,100 @@ 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 = -1.; + double max; + const auto &vec = IF_BIG_BATCH(d->threadctxs[0].max, d->threadctxs); + #ifndef BIG_BATCH - for (uint j = 0; j < d.threadctxs.size(); ++j) { - if (d.threadctxs[j].max.response > max) { - max = d.threadctxs[j].max.response; - max_idx = j; - } - } + auto max_it = std::max_element(vec.begin(), vec.end(), + [](const ThreadCtx &a, const ThreadCtx &b) + { return a.max.response < b.max.response; }); #else - for (uint j = 0; j < p_scales.size(); ++j) { - if (d.threadctxs[0].max[j].response > max) { - max = d.threadctxs[0].max[j].response; - max_idx = j; - } - } + 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); - if (m_visual_debug) { - cv::Mat all_responses(cv::Size(p_num_angles * 100, p_num_scales * 100), - d.threadctxs[max_idx].response.plane(0).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 in_roi(all_responses, cv::Rect(j * 100, i * 100, 100, 100)); - cv::Mat copy_response = d.threadctxs[p_num_angles * i + j].response.plane(0).clone(); - - copy_response = copy_response(cv::Rect(0, 0, copy_response.cols & -2, copy_response.rows & -2)); - - int cx = copy_response.cols / 2; - int cy = copy_response.rows / 2; - cv::Mat q0(copy_response, cv::Rect(0, 0, cx, cy)); - cv::Mat q1(copy_response, cv::Rect(cx, 0, cx, cy)); - cv::Mat q2(copy_response, cv::Rect(0, cy, cx, cy)); - cv::Mat q3(copy_response, cv::Rect(cx, cy, cx, cy)); - cv::Mat tmp; - q0.copyTo(tmp); - q3.copyTo(q0); - tmp.copyTo(q3); - q1.copyTo(tmp); - q2.copyTo(q1); - tmp.copyTo(q2); - - copy_response.copyTo(in_roi); - } - } - cv::namedWindow("All responses", CV_WINDOW_AUTOSIZE); - cv::imshow("All responses", all_responses); - cv::waitKey(); - } + max_idx = std::distance(vec.begin(), max_it); - 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)); + 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); - // 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 { 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); + } + 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); + } + } + cv::namedWindow("KCF visual debug", CV_WINDOW_AUTOSIZE); + cv::imshow("KCF visual debug", all_responses); + } + return max; } @@ -383,23 +407,29 @@ void KCF_Tracker::track(cv::Mat &img) resizeImgs(input_rgb, input_gray); #ifdef ASYNC - for (auto &it : d.threadctxs) + for (auto &it : d->threadctxs) it.async_res = std::async(std::launch::async, [this, &input_gray, &input_rgb, &it]() -> void { it.track(*this, input_rgb, input_gray); }); - for (auto const &it : d.threadctxs) + for (auto const &it : d->threadctxs) it.async_res.wait(); #else // !ASYNC NORMAL_OMP_PARALLEL_FOR - for (uint i = 0; i < d.threadctxs.size(); ++i) - d.threadctxs[i].track(*this, input_rgb, input_gray); + for (uint i = 0; i < d->threadctxs.size(); ++i) + d->threadctxs[i].track(*this, input_rgb, input_gray); #endif cv::Point2d new_location; 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; @@ -412,11 +442,12 @@ void KCF_Tracker::track(cv::Mat &img) if (m_use_subgrid_scale) { p_current_scale *= sub_grid_scale(max_idx); } else { - p_current_scale *= p_scales[max_idx]; + p_current_scale *= d->IF_BIG_BATCH(threadctxs[0].max, threadctxs).scale(max_idx); } clamp2(p_current_scale, p_min_max_scale[0], p_min_max_scale[1]); + // train at newly estimated target position train(input_rgb, input_gray, p_interp_factor); } @@ -426,11 +457,13 @@ void ThreadCtx::track(const KCF_Tracker &kcf, cv::Mat &input_rgb, cv::Mat &input TRACE(""); BIG_BATCH_OMP_PARALLEL_FOR - for (uint i = 0; i < IF_BIG_BATCH(kcf.p_num_scales, 1); ++i) + for (uint i = 0; i < IF_BIG_BATCH(max.size(), 1); ++i) { - kcf.get_features(input_rgb, input_gray, kcf.p_current_center.x, kcf.p_current_center.y, + kcf.get_features(input_rgb, input_gray, &dbg_patch IF_BIG_BATCH([i],), + kcf.p_current_center.x, kcf.p_current_center.y, kcf.p_windows_size.width, kcf.p_windows_size.height, - kcf.p_current_scale * IF_BIG_BATCH(kcf.p_scales[i], scale)) + kcf.p_current_scale * IF_BIG_BATCH(max.scale(i), scale), + kcf.p_current_angle + IF_BIG_BATCH(max.angle(i), angle)) .copyTo(patch_feats.scale(i)); DEBUG_PRINT(patch_feats.scale(i)); } @@ -439,11 +472,11 @@ void ThreadCtx::track(const KCF_Tracker &kcf, cv::Mat &input_rgb, cv::Mat &input DEBUG_PRINTM(zf); if (kcf.m_use_linearkernel) { - kzf = zf.mul(kcf.p_model_alphaf).sum_over_channels(); + kzf = zf.mul(kcf.model->model_alphaf).sum_over_channels(); } else { - gaussian_correlation(kzf, zf, kcf.p_model_xf, kcf.p_kernel_sigma, false, kcf); + gaussian_correlation(kzf, zf, kcf.model->model_xf, kcf.p_kernel_sigma, false, kcf); DEBUG_PRINTM(kzf); - kzf = kzf.mul(kcf.p_model_alphaf); + kzf = kzf.mul(kcf.model->model_alphaf); } kcf.fft.inverse(kzf, response); @@ -456,7 +489,7 @@ void ThreadCtx::track(const KCF_Tracker &kcf, cv::Mat &input_rgb, cv::Mat &input double min_val, max_val; cv::Point2i min_loc, max_loc; #ifdef BIG_BATCH - for (size_t i = 0; i < kcf.p_scales.size(); ++i) { + 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]; @@ -477,13 +510,16 @@ void ThreadCtx::track(const KCF_Tracker &kcf, cv::Mat &input_rgb, cv::Mat &input // **************************************************************************** -cv::Mat KCF_Tracker::get_features(cv::Mat &input_rgb, cv::Mat &input_gray, int cx, int cy, - int size_x, int size_y, double scale) const +cv::Mat KCF_Tracker::get_features(cv::Mat &input_rgb, cv::Mat &input_gray, cv::Mat *dbg_patch, + int cx, int cy, int size_x, int size_y, double scale, double angle) const { cv::Size scaled = cv::Size(floor(size_x * scale), floor(size_y * scale)); - cv::Mat patch_gray = get_subwindow(input_gray, cx, cy, scaled.width, scaled.height); - cv::Mat patch_rgb = get_subwindow(input_rgb, cx, cy, scaled.width, scaled.height); + 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()) { @@ -560,10 +596,10 @@ 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); + 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) { @@ -641,14 +677,18 @@ cv::Mat KCF_Tracker::cosine_window_function(int dim1, int dim2) // Returns sub-window of image input centered at [cx, cy] coordinates), // with size [width, height]. If any pixels are outside of the image, // they will replicate the values at the borders. -cv::Mat KCF_Tracker::get_subwindow(const cv::Mat &input, int cx, int cy, int width, int height) const +cv::Mat KCF_Tracker::get_subwindow(const cv::Mat &input, int cx, int cy, int width, int height, double angle) const { cv::Mat patch; - int x1 = cx - width / 2; - int y1 = cy - height / 2; - int x2 = cx + width / 2; - int y2 = cy + height / 2; + cv::Size sz(width, height); + cv::RotatedRect rr(cv::Point2f(cx, cy), sz, angle); + cv::Rect bb = rr.boundingRect(); + + int x1 = bb.tl().x; + int y1 = bb.tl().y; + int x2 = bb.br().x; + int y2 = bb.br().y; // out of image if (x1 >= input.cols || y1 >= input.rows || x2 < 0 || y2 < 0) { @@ -689,6 +729,12 @@ cv::Mat KCF_Tracker::get_subwindow(const cv::Mat &input, int cx, int cy, int wid // cv::waitKey(); } + cv::Point2f src_pts[4]; + cv::RotatedRect(cv::Point2f(patch.size()) / 2.0, sz, angle).points(src_pts); + cv::Point2f dst_pts[3] = { cv::Point2f(0, height), cv::Point2f(0, 0), cv::Point2f(width, 0)}; + auto rot = cv::getAffineTransform(src_pts, dst_pts); + cv::warpAffine(patch, patch, rot, sz); + // sanity check assert(patch.cols == width && patch.rows == height); @@ -699,12 +745,19 @@ void KCF_Tracker::GaussianCorrelation::operator()(ComplexMat &result, const Comp double sigma, bool auto_correlation, const KCF_Tracker &kcf) { TRACE(""); + DEBUG_PRINTM(xf); + DEBUG_PRINT(xf_sqr_norm.num_elem); xf.sqr_norm(xf_sqr_norm); + for (uint s = 0; s < xf.n_scales; ++s) + DEBUG_PRINT(xf_sqr_norm[s]); if (auto_correlation) { yf_sqr_norm = xf_sqr_norm; } else { + DEBUG_PRINTM(yf); yf.sqr_norm(yf_sqr_norm); } + for (uint s = 0; s < yf.n_scales; ++s) + DEBUG_PRINTM(yf_sqr_norm[s]); xyf = auto_correlation ? xf.sqr_mag() : xf * yf.conj(); // xf.muln(yf.conj()); DEBUG_PRINTM(xyf); @@ -713,12 +766,6 @@ void KCF_Tracker::GaussianCorrelation::operator()(ComplexMat &result, const Comp DEBUG_PRINTM(xyf_sum); kcf.fft.inverse(xyf_sum, ifft_res); DEBUG_PRINTM(ifft_res); -#ifdef CUFFT - // FIXME - cuda_gaussian_correlation(ifft_res.deviceMem(), k.deviceMem(), xf_sqr_norm.deviceMem(), - auto_correlation ? xf_sqr_norm.deviceMem() : yf_sqr_norm.deviceMem(), sigma, - xf.n_channels, xf.n_scales, kcf.feature_size.height, kcf.feature_size.width); -#else float numel_xf_inv = 1.f / (xf.cols * xf.rows * (xf.channels() / xf.n_scales)); for (uint i = 0; i < xf.n_scales; ++i) { @@ -728,7 +775,7 @@ void KCF_Tracker::GaussianCorrelation::operator()(ComplexMat &result, const Comp * numel_xf_inv, 0), plane); DEBUG_PRINTM(plane); } -#endif + kcf.fft.forward(ifft_res, result); } @@ -792,10 +839,14 @@ cv::Point2f KCF_Tracker::sub_pixel_peak(cv::Point &max_loc, cv::Mat &response) c return sub_peak; } -double KCF_Tracker::sub_grid_scale(uint index) +double KCF_Tracker::sub_grid_scale(uint max_index) { cv::Mat A, fval; - if (index >= p_scales.size()) { + const auto &vec = d->IF_BIG_BATCH(threadctxs[0].max, threadctxs); + uint index = vec.getScaleIdx(max_index); + uint angle_idx = vec.getAngleIdx(index); + + if (index >= vec.size()) { // interpolate from all values // fit 1d quadratic function f(x) = a*x^2 + b*x + c A.create(p_scales.size(), 3, CV_32FC1); @@ -804,7 +855,7 @@ double KCF_Tracker::sub_grid_scale(uint index) A.at(i, 0) = float(p_scales[i] * p_scales[i]); A.at(i, 1) = float(p_scales[i]); A.at(i, 2) = 1; - fval.at(i) = d.threadctxs.back().IF_BIG_BATCH(max[i].response, max.response); + fval.at(i) = d->IF_BIG_BATCH(threadctxs[0].max[i].response, threadctxs(i, angle_idx).max.response); } } else { // only from neighbours @@ -817,14 +868,14 @@ double KCF_Tracker::sub_grid_scale(uint index) p_scales[index + 1] * p_scales[index + 1], p_scales[index + 1], 1); #ifdef BIG_BATCH fval = (cv::Mat_(3, 1) << - d.threadctxs.back().max[index - 1].response, - d.threadctxs.back().max[index + 0].response, - d.threadctxs.back().max[index + 1].response); + d->threadctxs[0].max(index - 1, angle_idx).response, + d->threadctxs[0].max(index + 0, angle_idx).response, + d->threadctxs[0].max(index + 1, angle_idx).response); #else fval = (cv::Mat_(3, 1) << - d.threadctxs[index - 1].max.response, - d.threadctxs[index + 0].max.response, - d.threadctxs[index + 1].max.response); + d->threadctxs(index - 1, angle_idx).max.response, + d->threadctxs(index + 0, angle_idx).max.response, + d->threadctxs(index + 1, angle_idx).max.response); #endif }