10 #include <cuda_runtime.h>
25 void KCF_Tracker::init(cv::Mat &img, const cv::Rect & bbox)
27 //check boundary, enforce min size
28 double x1 = bbox.x, x2 = bbox.x + bbox.width, y1 = bbox.y, y2 = bbox.y + bbox.height;
30 if (x2 > img.cols-1) x2 = img.cols - 1;
32 if (y2 > img.rows-1) y2 = img.rows - 1;
34 if (x2-x1 < 2*p_cell_size) {
35 double diff = (2*p_cell_size -x2+x1)/2.;
36 if (x1 - diff >= 0 && x2 + diff < img.cols){
39 } else if (x1 - 2*diff >= 0) {
45 if (y2-y1 < 2*p_cell_size) {
46 double diff = (2*p_cell_size -y2+y1)/2.;
47 if (y1 - diff >= 0 && y2 + diff < img.rows){
50 } else if (y1 - 2*diff >= 0) {
59 p_pose.cx = x1 + p_pose.w/2.;
60 p_pose.cy = y1 + p_pose.h/2.;
63 cv::Mat input_gray, input_rgb = img.clone();
64 if (img.channels() == 3){
65 cv::cvtColor(img, input_gray, CV_BGR2GRAY);
66 input_gray.convertTo(input_gray, CV_32FC1);
68 img.convertTo(input_gray, CV_32FC1);
70 // don't need too large image
71 if (p_pose.w * p_pose.h > 100.*100.) {
72 std::cout << "resizing image by factor of 2" << std::endl;
73 p_resize_image = true;
75 cv::resize(input_gray, input_gray, cv::Size(0,0), 0.5, 0.5, cv::INTER_AREA);
76 cv::resize(input_rgb, input_rgb, cv::Size(0,0), 0.5, 0.5, cv::INTER_AREA);
79 //compute win size + fit to fhog cell size
80 p_windows_size[0] = round(p_pose.w * (1. + p_padding) / p_cell_size) * p_cell_size;
81 p_windows_size[1] = round(p_pose.h * (1. + p_padding) / p_cell_size) * p_cell_size;
85 for (int i = -p_num_scales/2; i <= p_num_scales/2; ++i)
86 p_scales.push_back(std::pow(p_scale_step, i));
88 p_scales.push_back(1.);
92 double min_size_ratio = std::max(5.*p_cell_size/p_windows_size[0], 5.*p_cell_size/p_windows_size[1]);
93 double max_size_ratio = std::min(floor((img.cols + p_windows_size[0]/3)/p_cell_size)*p_cell_size/p_windows_size[0], floor((img.rows + p_windows_size[1]/3)/p_cell_size)*p_cell_size/p_windows_size[1]);
94 p_min_max_scale[0] = std::pow(p_scale_step, std::ceil(std::log(min_size_ratio) / log(p_scale_step)));
95 p_min_max_scale[1] = std::pow(p_scale_step, std::floor(std::log(max_size_ratio) / log(p_scale_step)));
97 std::cout << "init: img size " << img.cols << " " << img.rows << std::endl;
98 std::cout << "init: win size. " << p_windows_size[0] << " " << p_windows_size[1] << std::endl;
99 std::cout << "init: min max scales factors: " << p_min_max_scale[0] << " " << p_min_max_scale[1] << std::endl;
101 p_output_sigma = std::sqrt(p_pose.w*p_pose.h) * p_output_sigma_factor / static_cast<double>(p_cell_size);
103 #if defined(FFTW) && defined(OPENMP)
105 fftw_plan_with_nthreads(omp_get_max_threads());
108 //window weights, i.e. labels
109 p_yf = fft2(gaussian_shaped_labels(p_output_sigma, p_windows_size[0]/p_cell_size, p_windows_size[1]/p_cell_size));
110 p_cos_window = cosine_window_function(p_yf.cols, p_yf.rows);
111 //obtain a sub-window for training initial model
112 std::vector<cv::Mat> path_feat = get_features(input_rgb, input_gray, p_pose.cx, p_pose.cy, p_windows_size[0], p_windows_size[1]);
113 p_model_xf = fft2(path_feat, p_cos_window);
115 if (m_use_linearkernel) {
116 ComplexMat xfconj = p_model_xf.conj();
117 p_model_alphaf_num = xfconj.mul(p_yf);
118 p_model_alphaf_den = (p_model_xf * xfconj);
120 //Kernel Ridge Regression, calculate alphas (in Fourier domain)
121 ComplexMat kf = gaussian_correlation(p_model_xf, p_model_xf, p_kernel_sigma, true);
122 p_model_alphaf_num = p_yf * kf;
123 p_model_alphaf_den = kf * (kf + p_lambda);
125 p_model_alphaf = p_model_alphaf_num / p_model_alphaf_den;
126 // p_model_alphaf = p_yf / (kf + p_lambda); //equation for fast training
129 void KCF_Tracker::setTrackerPose(BBox_c &bbox, cv::Mat & img)
131 init(img, bbox.get_rect());
134 void KCF_Tracker::updateTrackerPosition(BBox_c &bbox)
136 if (p_resize_image) {
147 BBox_c KCF_Tracker::getBBox()
150 tmp.w *= p_current_scale;
151 tmp.h *= p_current_scale;
159 void KCF_Tracker::track(cv::Mat &img)
162 cv::Mat input_gray, input_rgb = img.clone();
163 if (img.channels() == 3){
164 cv::cvtColor(img, input_gray, CV_BGR2GRAY);
165 input_gray.convertTo(input_gray, CV_32FC1);
167 img.convertTo(input_gray, CV_32FC1);
169 // don't need too large image
170 if (p_resize_image) {
171 cv::resize(input_gray, input_gray, cv::Size(0, 0), 0.5, 0.5, cv::INTER_AREA);
172 cv::resize(input_rgb, input_rgb, cv::Size(0, 0), 0.5, 0.5, cv::INTER_AREA);
176 std::vector<cv::Mat> patch_feat;
177 double max_response = -1.;
178 cv::Mat max_response_map;
179 cv::Point2i max_response_pt;
181 std::vector<double> scale_responses;
183 if (m_use_multithreading){
184 std::vector<std::future<cv::Mat>> async_res(p_scales.size());
185 for (size_t i = 0; i < p_scales.size(); ++i) {
186 async_res[i] = std::async(std::launch::async,
187 [this, &input_gray, &input_rgb, i]() -> cv::Mat
189 std::vector<cv::Mat> patch_feat_async = get_features(input_rgb, input_gray, this->p_pose.cx, this->p_pose.cy, this->p_windows_size[0],
190 this->p_windows_size[1], this->p_current_scale * this->p_scales[i]);
191 ComplexMat zf = fft2(patch_feat_async, this->p_cos_window);
192 if (m_use_linearkernel)
193 return ifft2((p_model_alphaf * zf).sum_over_channels());
195 ComplexMat kzf = gaussian_correlation(zf, this->p_model_xf, this->p_kernel_sigma);
196 return ifft2(this->p_model_alphaf * kzf);
201 for (size_t i = 0; i < p_scales.size(); ++i) {
204 cv::Mat response = async_res[i].get();
206 double min_val, max_val;
207 cv::Point2i min_loc, max_loc;
208 cv::minMaxLoc(response, &min_val, &max_val, &min_loc, &max_loc);
210 double weight = p_scales[i] < 1. ? p_scales[i] : 1./p_scales[i];
211 if (max_val*weight > max_response) {
212 max_response = max_val*weight;
213 max_response_map = response;
214 max_response_pt = max_loc;
217 scale_responses.push_back(max_val*weight);
220 #pragma omp parallel for ordered private(patch_feat) schedule(dynamic)
221 for (size_t i = 0; i < p_scales.size(); ++i) {
222 patch_feat = get_features(input_rgb, input_gray, p_pose.cx, p_pose.cy, p_windows_size[0], p_windows_size[1], p_current_scale * p_scales[i]);
223 ComplexMat zf = fft2(patch_feat, p_cos_window);
225 if (m_use_linearkernel)
226 response = ifft2((p_model_alphaf * zf).sum_over_channels());
228 ComplexMat kzf = gaussian_correlation(zf, p_model_xf, p_kernel_sigma);
229 response = ifft2(p_model_alphaf * kzf);
232 /* target location is at the maximum response. we must take into
233 account the fact that, if the target doesn't move, the peak
234 will appear at the top-left corner, not at the center (this is
235 discussed in the paper). the responses wrap around cyclically. */
236 double min_val, max_val;
237 cv::Point2i min_loc, max_loc;
238 cv::minMaxLoc(response, &min_val, &max_val, &min_loc, &max_loc);
240 double weight = p_scales[i] < 1. ? p_scales[i] : 1./p_scales[i];
243 if (max_val*weight > max_response) {
244 max_response = max_val*weight;
245 max_response_map = response;
246 max_response_pt = max_loc;
251 scale_responses.push_back(max_val*weight);
255 //sub pixel quadratic interpolation from neighbours
256 if (max_response_pt.y > max_response_map.rows / 2) //wrap around to negative half-space of vertical axis
257 max_response_pt.y = max_response_pt.y - max_response_map.rows;
258 if (max_response_pt.x > max_response_map.cols / 2) //same for horizontal axis
259 max_response_pt.x = max_response_pt.x - max_response_map.cols;
261 cv::Point2f new_location(max_response_pt.x, max_response_pt.y);
263 if (m_use_subpixel_localization)
264 new_location = sub_pixel_peak(max_response_pt, max_response_map);
266 p_pose.cx += p_current_scale*p_cell_size*new_location.x;
267 p_pose.cy += p_current_scale*p_cell_size*new_location.y;
268 if (p_pose.cx < 0) p_pose.cx = 0;
269 if (p_pose.cx > img.cols-1) p_pose.cx = img.cols-1;
270 if (p_pose.cy < 0) p_pose.cy = 0;
271 if (p_pose.cy > img.rows-1) p_pose.cy = img.rows-1;
273 //sub grid scale interpolation
274 double new_scale = p_scales[scale_index];
275 if (m_use_subgrid_scale)
276 new_scale = sub_grid_scale(scale_responses, scale_index);
278 p_current_scale *= new_scale;
280 if (p_current_scale < p_min_max_scale[0])
281 p_current_scale = p_min_max_scale[0];
282 if (p_current_scale > p_min_max_scale[1])
283 p_current_scale = p_min_max_scale[1];
285 //obtain a subwindow for training at newly estimated target position
286 patch_feat = get_features(input_rgb, input_gray, p_pose.cx, p_pose.cy, p_windows_size[0], p_windows_size[1], p_current_scale);
287 ComplexMat xf = fft2(patch_feat, p_cos_window);
289 //subsequent frames, interpolate model
290 p_model_xf = p_model_xf * (1. - p_interp_factor) + xf * p_interp_factor;
292 ComplexMat alphaf_num, alphaf_den;
294 if (m_use_linearkernel) {
295 ComplexMat xfconj = xf.conj();
296 alphaf_num = xfconj.mul(p_yf);
297 alphaf_den = (xf * xfconj);
299 //Kernel Ridge Regression, calculate alphas (in Fourier domain)
300 ComplexMat kf = gaussian_correlation(xf, xf, p_kernel_sigma, true);
301 // ComplexMat alphaf = p_yf / (kf + p_lambda); //equation for fast training
302 // p_model_alphaf = p_model_alphaf * (1. - p_interp_factor) + alphaf * p_interp_factor;
303 alphaf_num = p_yf * kf;
304 alphaf_den = kf * (kf + p_lambda);
307 p_model_alphaf_num = p_model_alphaf_num * (1. - p_interp_factor) + alphaf_num * p_interp_factor;
308 p_model_alphaf_den = p_model_alphaf_den * (1. - p_interp_factor) + alphaf_den * p_interp_factor;
309 p_model_alphaf = p_model_alphaf_num / p_model_alphaf_den;
312 // ****************************************************************************
314 std::vector<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)
316 int size_x_scaled = floor(size_x*scale);
317 int size_y_scaled = floor(size_y*scale);
319 cv::Mat patch_gray = get_subwindow(input_gray, cx, cy, size_x_scaled, size_y_scaled);
320 cv::Mat patch_rgb = get_subwindow(input_rgb, cx, cy, size_x_scaled, size_y_scaled);
322 //resize to default size
324 //if we downsample use INTER_AREA interpolation
325 cv::resize(patch_gray, patch_gray, cv::Size(size_x, size_y), 0., 0., cv::INTER_AREA);
327 cv::resize(patch_gray, patch_gray, cv::Size(size_x, size_y), 0., 0., cv::INTER_LINEAR);
330 // get hog(Histogram of Oriented Gradients) features
331 std::vector<cv::Mat> hog_feat = FHoG::extract(patch_gray, 2, p_cell_size, 9);
333 //get color rgb features (simple r,g,b channels)
334 std::vector<cv::Mat> color_feat;
335 if ((m_use_color || m_use_cnfeat) && input_rgb.channels() == 3) {
336 //resize to default size
338 //if we downsample use INTER_AREA interpolation
339 cv::resize(patch_rgb, patch_rgb, cv::Size(size_x/p_cell_size, size_y/p_cell_size), 0., 0., cv::INTER_AREA);
341 cv::resize(patch_rgb, patch_rgb, cv::Size(size_x/p_cell_size, size_y/p_cell_size), 0., 0., cv::INTER_LINEAR);
345 if (m_use_color && input_rgb.channels() == 3) {
346 //use rgb color space
347 cv::Mat patch_rgb_norm;
348 patch_rgb.convertTo(patch_rgb_norm, CV_32F, 1. / 255., -0.5);
349 cv::Mat ch1(patch_rgb_norm.size(), CV_32FC1);
350 cv::Mat ch2(patch_rgb_norm.size(), CV_32FC1);
351 cv::Mat ch3(patch_rgb_norm.size(), CV_32FC1);
352 std::vector<cv::Mat> rgb = {ch1, ch2, ch3};
353 cv::split(patch_rgb_norm, rgb);
354 color_feat.insert(color_feat.end(), rgb.begin(), rgb.end());
357 if (m_use_cnfeat && input_rgb.channels() == 3) {
358 std::vector<cv::Mat> cn_feat = CNFeat::extract(patch_rgb);
359 color_feat.insert(color_feat.end(), cn_feat.begin(), cn_feat.end());
362 hog_feat.insert(hog_feat.end(), color_feat.begin(), color_feat.end());
366 cv::Mat KCF_Tracker::gaussian_shaped_labels(double sigma, int dim1, int dim2)
368 cv::Mat labels(dim2, dim1, CV_32FC1);
369 int range_y[2] = {-dim2 / 2, dim2 - dim2 / 2};
370 int range_x[2] = {-dim1 / 2, dim1 - dim1 / 2};
372 double sigma_s = sigma*sigma;
374 for (int y = range_y[0], j = 0; y < range_y[1]; ++y, ++j){
375 float * row_ptr = labels.ptr<float>(j);
377 for (int x = range_x[0], i = 0; x < range_x[1]; ++x, ++i){
378 row_ptr[i] = std::exp(-0.5 * (y_s + x*x) / sigma_s);//-1/2*e^((y^2+x^2)/sigma^2)
382 //rotate so that 1 is at top-left corner (see KCF paper for explanation)
383 cv::Mat rot_labels = circshift(labels, range_x[0], range_y[0]);
384 //sanity check, 1 at top left corner
385 assert(rot_labels.at<float>(0,0) >= 1.f - 1e-10f);
390 cv::Mat KCF_Tracker::circshift(const cv::Mat &patch, int x_rot, int y_rot)
392 cv::Mat rot_patch(patch.size(), CV_32FC1);
393 cv::Mat tmp_x_rot(patch.size(), CV_32FC1);
395 //circular rotate x-axis
397 //move part that does not rotate over the edge
398 cv::Range orig_range(-x_rot, patch.cols);
399 cv::Range rot_range(0, patch.cols - (-x_rot));
400 patch(cv::Range::all(), orig_range).copyTo(tmp_x_rot(cv::Range::all(), rot_range));
403 orig_range = cv::Range(0, -x_rot);
404 rot_range = cv::Range(patch.cols - (-x_rot), patch.cols);
405 patch(cv::Range::all(), orig_range).copyTo(tmp_x_rot(cv::Range::all(), rot_range));
406 }else if (x_rot > 0){
407 //move part that does not rotate over the edge
408 cv::Range orig_range(0, patch.cols - x_rot);
409 cv::Range rot_range(x_rot, patch.cols);
410 patch(cv::Range::all(), orig_range).copyTo(tmp_x_rot(cv::Range::all(), rot_range));
413 orig_range = cv::Range(patch.cols - x_rot, patch.cols);
414 rot_range = cv::Range(0, x_rot);
415 patch(cv::Range::all(), orig_range).copyTo(tmp_x_rot(cv::Range::all(), rot_range));
416 }else { //zero rotation
417 //move part that does not rotate over the edge
418 cv::Range orig_range(0, patch.cols);
419 cv::Range rot_range(0, patch.cols);
420 patch(cv::Range::all(), orig_range).copyTo(tmp_x_rot(cv::Range::all(), rot_range));
423 //circular rotate y-axis
425 //move part that does not rotate over the edge
426 cv::Range orig_range(-y_rot, patch.rows);
427 cv::Range rot_range(0, patch.rows - (-y_rot));
428 tmp_x_rot(orig_range, cv::Range::all()).copyTo(rot_patch(rot_range, cv::Range::all()));
431 orig_range = cv::Range(0, -y_rot);
432 rot_range = cv::Range(patch.rows - (-y_rot), patch.rows);
433 tmp_x_rot(orig_range, cv::Range::all()).copyTo(rot_patch(rot_range, cv::Range::all()));
434 }else if (y_rot > 0){
435 //move part that does not rotate over the edge
436 cv::Range orig_range(0, patch.rows - y_rot);
437 cv::Range rot_range(y_rot, patch.rows);
438 tmp_x_rot(orig_range, cv::Range::all()).copyTo(rot_patch(rot_range, cv::Range::all()));
441 orig_range = cv::Range(patch.rows - y_rot, patch.rows);
442 rot_range = cv::Range(0, y_rot);
443 tmp_x_rot(orig_range, cv::Range::all()).copyTo(rot_patch(rot_range, cv::Range::all()));
444 }else { //zero rotation
445 //move part that does not rotate over the edge
446 cv::Range orig_range(0, patch.rows);
447 cv::Range rot_range(0, patch.rows);
448 tmp_x_rot(orig_range, cv::Range::all()).copyTo(rot_patch(rot_range, cv::Range::all()));
454 ComplexMat KCF_Tracker::fft2(const cv::Mat &input)
456 cv::Mat complex_result;
458 cv::Mat flip_h,imag_h;
460 cv::cuda::HostMem hostmem_input(input, cv::cuda::HostMem::SHARED);
461 cv::cuda::HostMem hostmem_real(cv::Size(input.cols,input.rows/2+1), CV_32FC2, cv::cuda::HostMem::SHARED);
463 cv::cuda::dft(hostmem_input,hostmem_real,hostmem_input.size(),0,stream);
464 stream.waitForCompletion();
466 cv::Mat real_h = hostmem_real.createMatHeader();
468 //create reversed copy of result and merge them
469 cv::flip(hostmem_real,flip_h,1);
470 flip_h(cv::Range(0, flip_h.rows), cv::Range(1, flip_h.cols)).copyTo(imag_h);
472 std::vector<cv::Mat> matarray = {real_h,imag_h};
474 cv::hconcat(matarray,complex_result);
477 // Prepare variables and FFTW plan for float precision FFT
488 float* outdata = new float[2*width * height];
490 // data_in = fftwf_alloc_real(width * height);
493 #if defined(FFTW) && defined(ASYNC)
494 std::unique_lock<std::mutex> lock_i(fftw_init);
496 fft = fftwf_alloc_complex((width/2+1) * height);
497 plan_f=fftwf_plan_dft_r2c_2d( height , width , (float*)input.data , fft , FFTW_ESTIMATE );
498 #if defined(FFTW) && defined(ASYNC)
502 // Prepare input data
503 // for(int i = 0,k=0; i < height; ++i) {
504 // const float* row = input.ptr<float>(i);
505 // for(int j = 0; j < width; j++) {
506 // data_in[k]=(float)row[j];
512 fftwf_execute( plan_f );
514 // Get output data to right format
516 for(int i = 0, k = 0,l=0 ; i < height; i++ ) {
517 for(int j = 0 ; j < width2 ; j++ ) {
519 outdata[i * width2 + j] = (float)fft[k][0];
520 outdata[i * width2 + j+1] = (float)fft[k][1];
527 outdata[i * width2 + j] = (float)fft[l][0];
528 outdata[i * width2 + j+1] = (float)fft[l][1];
534 cv::Mat tmp(height,width,CV_32FC2,outdata);
536 // Destroy FFTW plan and variables
539 #if defined(FFTW) && defined(ASYNC)
540 std::unique_lock<std::mutex> lock_d(fftw_destroy);
542 fftwf_destroy_plan(plan_f);
543 fftwf_free(fft); /*fftwf_free(data_in);*/
544 #if defined(FFTW) && defined(ASYNC)
549 #if !defined OPENCV_CUFFT || !defined FFTW
550 cv::dft(input, complex_result, cv::DFT_COMPLEX_OUTPUT);
553 //extraxt x and y channels
555 cv::split(complex_result, xy);
557 //calculate angle and magnitude
558 cv::Mat magnitude, angle;
559 cv::cartToPolar(xy[0], xy[1], magnitude, angle, true);
561 //translate magnitude to range [0;1]
563 cv::minMaxLoc(magnitude, 0, &mag_max);
564 magnitude.convertTo(magnitude, -1, 1.0 / mag_max);
567 cv::Mat _hsv[3], hsv;
569 _hsv[1] = cv::Mat::ones(angle.size(), CV_32F);
571 cv::merge(_hsv, 3, hsv);
573 //convert to BGR and show
574 cv::Mat bgr;//CV_32FC3 matrix
575 cv::cvtColor(hsv, bgr, cv::COLOR_HSV2BGR);
576 cv::resize(bgr, bgr, cv::Size(600,600));
577 cv::imshow("DFT", bgr);
580 return ComplexMat(complex_result);
583 ComplexMat KCF_Tracker::fft2(const std::vector<cv::Mat> &input, const cv::Mat &cos_window)
585 int n_channels = input.size();
586 cv::Mat complex_result;
589 cv::Mat flip_h,imag_h;
590 cv::cuda::GpuMat src_gpu;
591 cv::cuda::HostMem hostmem_real(cv::Size(input[0].cols,input[0].rows/2+1), CV_32FC2, cv::cuda::HostMem::SHARED);
594 // Prepare variables and FFTW plan for float precision FFT
600 int width, height, width2;
602 width = input[0].cols;
603 height = input[0].rows;
606 float* outdata = new float[2*width * height];
607 cv::Mat in_img = cv::Mat::zeros(height, width, CV_32FC1);
608 // data_in = fftwf_alloc_real(width * height);
611 #if defined(FFTW) && defined(ASYNC)
612 std::unique_lock<std::mutex> lock_i(fftw_init);
614 fft = fftwf_alloc_complex((width/2+1) * height);
615 plan_f=fftwf_plan_dft_r2c_2d( height , width , (float*) in_img.data , fft , FFTW_ESTIMATE );
616 #if defined(FFTW) && defined(ASYNC)
622 ComplexMat result(input[0].rows, input[0].cols, n_channels);
623 for (int i = 0; i < n_channels; ++i){
625 cv::cuda::HostMem hostmem_input(input[i], cv::cuda::HostMem::SHARED);
626 cv::cuda::multiply(hostmem_input,p_cos_window_d,src_gpu);
627 cv::cuda::dft(src_gpu,hostmem_real,src_gpu.size(),0,stream);
628 stream.waitForCompletion();
630 cv::Mat real_h = hostmem_real.createMatHeader();
632 //create reversed copy of result and merge them
633 cv::flip(hostmem_real,flip_h,1);
634 flip_h(cv::Range(0, flip_h.rows), cv::Range(1, flip_h.cols)).copyTo(imag_h);
636 std::vector<cv::Mat> matarray = {real_h,imag_h};
638 cv::hconcat(matarray,complex_result);
641 // Prepare input data
642 cv::Mat in_img = input[i].mul(cos_window);
643 // for(int x = 0,k=0; x< height; ++x) {
644 // const float* row = in_img.ptr<float>(x);
645 // for(int j = 0; j < width; j++) {
646 // data_in[k]=(float)row[j];
652 fftwf_execute( plan_f );
654 // Get output data to right format
655 for(int x = 0, k = 0,l=0 ; x < height; ++x ) {
656 for(int j = 0 ; j < width2 ; j++ ) {
658 outdata[x* width2 + j] = (float)fft[k][0];
659 outdata[x * width2 + j+1] = (float)fft[k][1];
665 outdata[x * width2 + j] = (float)fft[l][0];
666 outdata[x * width2 + j+1] = (float)fft[l][1];
671 cv::Mat tmp(height,width,CV_32FC2,outdata);
672 complex_result = tmp;
675 #if !defined OPENCV_CUFFT || !defined FFTW
676 cv::dft(input[i].mul(cos_window), complex_result, cv::DFT_COMPLEX_OUTPUT);
679 result.set_channel(i, complex_result);
682 // Destroy FFT plans and variables
685 #if defined(FFTW) && defined(ASYNC)
686 std::unique_lock<std::mutex> lock_d(fftw_destroy);
688 fftwf_destroy_plan(plan_f);
689 fftwf_free(fft); /*fftwf_free(data_in);*/
690 #if defined(FFTW) && defined(ASYNC)
698 cv::Mat KCF_Tracker::ifft2(const ComplexMat &inputf)
701 // Prepare variables and FFTW plan for float precision IFFT
702 fftwf_complex *data_in;
709 if (inputf.n_channels == 1){
711 cv::Mat input=inputf.to_cv_mat() ;
716 float* outdata = new float[width * height];
719 #if defined(FFTW) && defined(ASYNC)
720 std::unique_lock<std::mutex> lock_i(fftw_init);
722 data_in = fftwf_alloc_complex(2*(width/2+1) * height);
723 ifft = fftwf_alloc_real(width * height);
724 plan_if=fftwf_plan_dft_c2r_2d( height , width , data_in , ifft , FFTW_MEASURE );
725 #if defined(FFTW) && defined(ASYNC)
730 for(int x = 0,k=0; x< height; ++x) {
731 const float* row = input.ptr<float>(x);
732 for(int j = 0; j < width; j++) {
733 data_in[k][0]=(float)row[j];
734 data_in[k][1]=(float)row[j+1];
742 fftwf_execute( plan_if );
744 // Get output data to right format
745 for(int x = 0,k=0; x< height; ++x) {
746 for(int j = 0; j < width; j++) {
747 outdata[k]=(float)ifft[x*width+j]/(float)(width*height);
753 cv::Mat tmp(height,width,CV_32FC1,outdata);
755 // Destroy FFTW plans and variables
758 #if defined(FFTW) && defined(ASYNC)
759 std::unique_lock<std::mutex> lock_d(fftw_destroy);
761 fftwf_destroy_plan(plan_if);
762 fftwf_free(ifft); fftwf_free(data_in);
763 #if defined(FFTW) && defined(ASYNC)
768 cv::dft(inputf.to_cv_mat(),real_result, cv::DFT_INVERSE | cv::DFT_REAL_OUTPUT | cv::DFT_SCALE);
772 std::vector<cv::Mat> mat_channels = inputf.to_cv_mat_vector();
773 std::vector<cv::Mat> ifft_mats(inputf.n_channels);
775 width = mat_channels[0].cols;
776 height = mat_channels[0].rows;
778 float* outdata = new float[width * height];
781 #if defined(FFTW) && defined(ASYNC)
782 std::unique_lock<std::mutex> lock_i(fftw_init);
784 data_in = fftwf_alloc_complex(2*(width/2+1) * height);
785 ifft = fftwf_alloc_real(width * height);
786 plan_if=fftwf_plan_dft_c2r_2d( height , width , data_in , ifft , FFTW_MEASURE );
787 #if defined(FFTW) && defined(ASYNC)
792 for (int i = 0; i < inputf.n_channels; ++i) {
795 for(int x = 0,k=0; x< height; ++x) {
796 const float* row = mat_channels[i].ptr<float>(x);
797 for(int j = 0; j < width; j++) {
798 data_in[k][0]=(float)row[j];
799 data_in[k][1]=(float)row[j+1];
807 fftwf_execute( plan_if );
809 // Get output data to right format
810 for(int x = 0,k=0; x< height; ++x) {
811 for(int j = 0; j < width; j++) {
812 outdata[k]=(float)ifft[x*width+j]/(float)(width*height);
818 cv::Mat tmp(height,width,CV_32FC1,outdata);
823 cv::dft(mat_channels[i], ifft_mats[i], cv::DFT_INVERSE | cv::DFT_REAL_OUTPUT | cv::DFT_SCALE);
827 // Destroy FFTW plans and variables
830 #if defined(FFTW) && defined(ASYNC)
831 std::unique_lock<std::mutex> lock_d(fftw_destroy);
833 fftwf_destroy_plan(plan_if);
834 fftwf_free(ifft); fftwf_free(data_in);
835 #if defined(FFTW) && defined(ASYNC)
840 cv::merge(ifft_mats, real_result);
845 //hann window actually (Power-of-cosine windows)
846 cv::Mat KCF_Tracker::cosine_window_function(int dim1, int dim2)
848 cv::Mat m1(1, dim1, CV_32FC1), m2(dim2, 1, CV_32FC1);
849 double N_inv = 1./(static_cast<double>(dim1)-1.);
850 for (int i = 0; i < dim1; ++i)
851 m1.at<float>(i) = 0.5*(1. - std::cos(2. * CV_PI * static_cast<double>(i) * N_inv));
852 N_inv = 1./(static_cast<double>(dim2)-1.);
853 for (int i = 0; i < dim2; ++i)
854 m2.at<float>(i) = 0.5*(1. - std::cos(2. * CV_PI * static_cast<double>(i) * N_inv));
857 cv::cuda::createContinuous(cv::Size(ret.cols,ret.rows),CV_32FC1,p_cos_window_d);
858 p_cos_window_d.upload(ret);
863 // Returns sub-window of image input centered at [cx, cy] coordinates),
864 // with size [width, height]. If any pixels are outside of the image,
865 // they will replicate the values at the borders.
866 cv::Mat KCF_Tracker::get_subwindow(const cv::Mat &input, int cx, int cy, int width, int height)
870 int x1 = cx - width/2;
871 int y1 = cy - height/2;
872 int x2 = cx + width/2;
873 int y2 = cy + height/2;
876 if (x1 >= input.cols || y1 >= input.rows || x2 < 0 || y2 < 0) {
877 patch.create(height, width, input.type());
882 int top = 0, bottom = 0, left = 0, right = 0;
884 //fit to image coordinates, set border extensions;
893 if (x2 >= input.cols) {
894 right = x2 - input.cols + width % 2;
899 if (y2 >= input.rows) {
900 bottom = y2 - input.rows + height % 2;
905 if (x2 - x1 == 0 || y2 - y1 == 0)
906 patch = cv::Mat::zeros(height, width, CV_32FC1);
909 cv::copyMakeBorder(input(cv::Range(y1, y2), cv::Range(x1, x2)), patch, top, bottom, left, right, cv::BORDER_REPLICATE);
910 // imshow( "copyMakeBorder", patch);
915 assert(patch.cols == width && patch.rows == height);
920 ComplexMat KCF_Tracker::gaussian_correlation(const ComplexMat &xf, const ComplexMat &yf, double sigma, bool auto_correlation)
922 float xf_sqr_norm = xf.sqr_norm();
923 float yf_sqr_norm = auto_correlation ? xf_sqr_norm : yf.sqr_norm();
925 ComplexMat xyf = auto_correlation ? xf.sqr_mag() : xf * yf.conj();
927 //ifft2 and sum over 3rd dimension, we dont care about individual channels
928 cv::Mat xy_sum(xf.rows, xf.cols, CV_32FC1);
930 cv::Mat ifft2_res = ifft2(xyf);
931 for (int y = 0; y < xf.rows; ++y) {
932 float * row_ptr = ifft2_res.ptr<float>(y);
933 float * row_ptr_sum = xy_sum.ptr<float>(y);
934 for (int x = 0; x < xf.cols; ++x){
935 row_ptr_sum[x] = std::accumulate((row_ptr + x*ifft2_res.channels()), (row_ptr + x*ifft2_res.channels() + ifft2_res.channels()), 0.f);
939 float numel_xf_inv = 1.f/(xf.cols * xf.rows * xf.n_channels);
941 cv::exp(- 1.f / (sigma * sigma) * cv::max((xf_sqr_norm + yf_sqr_norm - 2 * xy_sum) * numel_xf_inv, 0), tmp);
946 float get_response_circular(cv::Point2i & pt, cv::Mat & response)
951 x = response.cols + x;
953 y = response.rows + y;
954 if (x >= response.cols)
955 x = x - response.cols;
956 if (y >= response.rows)
957 y = y - response.rows;
959 return response.at<float>(y,x);
962 cv::Point2f KCF_Tracker::sub_pixel_peak(cv::Point & max_loc, cv::Mat & response)
964 //find neighbourhood of max_loc (response is circular)
968 cv::Point2i p1(max_loc.x-1, max_loc.y-1), p2(max_loc.x, max_loc.y-1), p3(max_loc.x+1, max_loc.y-1);
969 cv::Point2i p4(max_loc.x-1, max_loc.y), p5(max_loc.x+1, max_loc.y);
970 cv::Point2i p6(max_loc.x-1, max_loc.y+1), p7(max_loc.x, max_loc.y+1), p8(max_loc.x+1, max_loc.y+1);
972 // fit 2d quadratic function f(x, y) = a*x^2 + b*x*y + c*y^2 + d*x + e*y + f
973 cv::Mat A = (cv::Mat_<float>(9, 6) <<
974 p1.x*p1.x, p1.x*p1.y, p1.y*p1.y, p1.x, p1.y, 1.f,
975 p2.x*p2.x, p2.x*p2.y, p2.y*p2.y, p2.x, p2.y, 1.f,
976 p3.x*p3.x, p3.x*p3.y, p3.y*p3.y, p3.x, p3.y, 1.f,
977 p4.x*p4.x, p4.x*p4.y, p4.y*p4.y, p4.x, p4.y, 1.f,
978 p5.x*p5.x, p5.x*p5.y, p5.y*p5.y, p5.x, p5.y, 1.f,
979 p6.x*p6.x, p6.x*p6.y, p6.y*p6.y, p6.x, p6.y, 1.f,
980 p7.x*p7.x, p7.x*p7.y, p7.y*p7.y, p7.x, p7.y, 1.f,
981 p8.x*p8.x, p8.x*p8.y, p8.y*p8.y, p8.x, p8.y, 1.f,
982 max_loc.x*max_loc.x, max_loc.x*max_loc.y, max_loc.y*max_loc.y, max_loc.x, max_loc.y, 1.f);
983 cv::Mat fval = (cv::Mat_<float>(9, 1) <<
984 get_response_circular(p1, response),
985 get_response_circular(p2, response),
986 get_response_circular(p3, response),
987 get_response_circular(p4, response),
988 get_response_circular(p5, response),
989 get_response_circular(p6, response),
990 get_response_circular(p7, response),
991 get_response_circular(p8, response),
992 get_response_circular(max_loc, response));
994 cv::solve(A, fval, x, cv::DECOMP_SVD);
996 double a = x.at<float>(0), b = x.at<float>(1), c = x.at<float>(2),
997 d = x.at<float>(3), e = x.at<float>(4);
999 cv::Point2f sub_peak(max_loc.x, max_loc.y);
1000 if (b > 0 || b < 0) {
1001 sub_peak.y = ((2.f * a * e) / b - d) / (b - (4 * a * c) / b);
1002 sub_peak.x = (-2 * c * sub_peak.y - e) / b;
1008 double KCF_Tracker::sub_grid_scale(std::vector<double> & responses, int index)
1011 if (index < 0 || index > (int)p_scales.size()-1) {
1012 // interpolate from all values
1013 // fit 1d quadratic function f(x) = a*x^2 + b*x + c
1014 A.create(p_scales.size(), 3, CV_32FC1);
1015 fval.create(p_scales.size(), 1, CV_32FC1);
1016 for (size_t i = 0; i < p_scales.size(); ++i) {
1017 A.at<float>(i, 0) = p_scales[i] * p_scales[i];
1018 A.at<float>(i, 1) = p_scales[i];
1019 A.at<float>(i, 2) = 1;
1020 fval.at<float>(i) = responses[i];
1023 //only from neighbours
1024 if (index == 0 || index == (int)p_scales.size()-1)
1025 return p_scales[index];
1027 A = (cv::Mat_<float>(3, 3) <<
1028 p_scales[index-1] * p_scales[index-1], p_scales[index-1], 1,
1029 p_scales[index] * p_scales[index], p_scales[index], 1,
1030 p_scales[index+1] * p_scales[index+1], p_scales[index+1], 1);
1031 fval = (cv::Mat_<float>(3, 1) << responses[index-1], responses[index], responses[index+1]);
1035 cv::solve(A, fval, x, cv::DECOMP_SVD);
1036 double a = x.at<float>(0), b = x.at<float>(1);
1037 double scale = p_scales[index];
1039 scale = -b / (2 * a);