1 // This file is part of OpenCV project.
2 // It is subject to the license terms in the LICENSE file found in the top-level directory
3 // of this distribution and at http://opencv.org/license.html.
4 
5 #include "precomp.hpp"
6 
7 #include "trackerCSRTUtils.hpp"
8 
9 namespace cv {
10 
circshift(Mat matrix,int dx,int dy)11 Mat circshift(Mat matrix, int dx, int dy)
12 {
13     Mat matrix_out = matrix.clone();
14     int idx_y = 0;
15     int idx_x = 0;
16     for(int i=0; i<matrix.rows; i++) {
17         for(int j=0; j<matrix.cols; j++) {
18             idx_y = modul(i+dy+1, matrix.rows);
19             idx_x = modul(j+dx+1, matrix.cols);
20             matrix_out.at<float>(idx_y, idx_x) = matrix.at<float>(i,j);
21         }
22     }
23     return matrix_out;
24 }
25 
gaussian_shaped_labels(const float sigma,const int w,const int h)26 Mat gaussian_shaped_labels(const float sigma, const int w, const int h)
27 {
28     // create 2D Gaussian peak, convert to Fourier space and stores it into the yf
29     Mat y = Mat::zeros(h, w, CV_32F);
30     float w2 = static_cast<float>(cvFloor(w / 2));
31     float h2 = static_cast<float>(cvFloor(h / 2));
32 
33     // calculate for each pixel separatelly
34     for(int i=0; i<y.rows; i++) {
35         for(int j=0; j<y.cols; j++) {
36             y.at<float>(i,j) = (float)exp((-0.5 / pow(sigma, 2)) * (pow((i+1-h2), 2) + pow((j+1-w2), 2)));
37         }
38     }
39     // wrap-around with the circulat shifting
40     y = circshift(y, -cvFloor(y.cols / 2), -cvFloor(y.rows / 2));
41     Mat yf;
42     dft(y, yf, DFT_COMPLEX_OUTPUT);
43     return yf;
44 }
45 
fourier_transform_features(const std::vector<Mat> & M)46 std::vector<Mat> fourier_transform_features(const std::vector<Mat> &M)
47 {
48     std::vector<Mat> out(M.size());
49     Mat channel;
50     // iterate over channels and convert them to Fourier domain
51     for(size_t k = 0; k < M.size(); k++) {
52         M[k].convertTo(channel, CV_32F);
53         dft(channel, channel, DFT_COMPLEX_OUTPUT);
54         out[k] = (channel);
55     }
56     return out;
57 }
58 
divide_complex_matrices(const Mat & A,const Mat & B)59 Mat divide_complex_matrices(const Mat &A, const Mat &B)
60 {
61     std::vector<Mat> va,vb;
62     split(A, va);
63     split(B, vb);
64 
65     Mat a = va.at(0);
66     Mat b = va.at(1);
67     Mat c = vb.at(0);
68     Mat d = vb.at(1);
69 
70     Mat div = c.mul(c) + d.mul(d);
71     Mat real_part = (a.mul(c) + b.mul(d));
72     Mat im_part = (b.mul(c) - a.mul(d));
73     divide(real_part, div, real_part);
74     divide(im_part, div, im_part);
75 
76     std::vector<Mat> tmp(2);
77     tmp[0] = real_part;
78     tmp[1] = im_part;
79     Mat res;
80     merge(tmp, res);
81     return res;
82 }
83 
get_subwindow(const Mat & image,const Point2f center,const int w,const int h,Rect * valid_pixels)84 Mat get_subwindow(
85         const Mat &image,
86         const Point2f center,
87         const int w,
88         const int h,
89         Rect *valid_pixels)
90 {
91     int startx = cvFloor(center.x) + 1 - (cvFloor(w/2));
92     int starty = cvFloor(center.y) + 1 - (cvFloor(h/2));
93     Rect roi(startx, starty, w, h);
94     int padding_left = 0, padding_right = 0, padding_top = 0, padding_bottom = 0;
95     if(roi.x < 0) {
96         padding_left = -roi.x;
97         roi.x = 0;
98     }
99     if(roi.y < 0) {
100         padding_top = -roi.y;
101         roi.y = 0;
102     }
103     roi.width -= padding_left;
104     roi.height-= padding_top;
105     if(roi.x + roi.width >= image.cols) {
106         padding_right = roi.x + roi.width - image.cols;
107         roi.width = image.cols - roi.x;
108     }
109     if(roi.y + roi.height >= image.rows) {
110         padding_bottom = roi.y + roi.height - image.rows;
111         roi.height = image.rows - roi.y;
112     }
113     Mat subwin = image(roi).clone();
114     copyMakeBorder(subwin, subwin, padding_top, padding_bottom, padding_left, padding_right, BORDER_REPLICATE);
115 
116     if(valid_pixels != NULL) {
117         *valid_pixels = Rect(padding_left, padding_top, roi.width, roi.height);
118     }
119     return subwin;
120 }
121 
subpixel_peak(const Mat & response,const std::string & s,const Point2f & p)122 float subpixel_peak(const Mat &response, const std::string &s, const Point2f &p)
123 {
124     int i_p0, i_p_l, i_p_r;     // indexes in response
125     float p0, p_l, p_r;         // values in response
126 
127     if(s.compare("vertical") == 0) {
128         // neighbouring rows
129         i_p0 = cvRound(p.y);
130         i_p_l = modul(cvRound(p.y) - 1, response.rows);
131         i_p_r = modul(cvRound(p.y) + 1, response.rows);
132         int px = static_cast<int>(p.x);
133         p0 = response.at<float>(i_p0, px);
134         p_l = response.at<float>(i_p_l, px);
135         p_r = response.at<float>(i_p_r, px);
136     } else if(s.compare("horizontal") == 0) {
137         // neighbouring cols
138         i_p0 = cvRound(p.x);
139         i_p_l = modul(cvRound(p.x) - 1, response.cols);
140         i_p_r = modul(cvRound(p.x) + 1, response.cols);
141         int py = static_cast<int>(p.y);
142         p0 = response.at<float>(py, i_p0);
143         p_l = response.at<float>(py, i_p_l);
144         p_r = response.at<float>(py, i_p_r);
145     } else {
146         std::cout << "Warning: unknown subpixel peak direction!" << std::endl;
147         return 0;
148     }
149     float delta = 0.5f * (p_r - p_l) / (2*p0 - p_r - p_l);
150     if(!std::isfinite(delta)) {
151         delta = 0;
152     }
153 
154     return delta;
155 }
156 
chebpoly(const int n,const float x)157 inline float chebpoly(const int n, const float x)
158 {
159     float res;
160     if (fabs(x) <= 1)
161         res = cos(n*acos(x));
162     else
163         res = cosh(n*acosh(x));
164     return res;
165 }
166 
chebwin(int N,const float atten)167 static Mat chebwin(int N, const float atten)
168 {
169     Mat out(N , 1, CV_32FC1);
170     int nn, i;
171     float M, n, sum = 0, max=0;
172     float tg = static_cast<float>(pow(10,atten/20.0f));  /* 1/r term [2], 10^gamma [2] */
173     float x0 = cosh((1.0f/(N-1))*acosh(tg));
174     M = (N-1)/2.0f;
175     if(N%2==0)
176         M = M + 0.5f; /* handle even length windows */
177     for(nn=0; nn<(N/2+1); nn++) {
178         n = nn-M;
179         sum = 0;
180         for(i=1; i<=M; i++){
181             sum += chebpoly(N-1,x0*static_cast<float>(cos(CV_PI*i/N))) *
182                 static_cast<float>(cos(2.0f*n*CV_PI*i/N));
183         }
184         out.at<float>(nn,0) = tg + 2*sum;
185         out.at<float>(N-nn-1,0) = out.at<float>(nn,0) ;
186         if(out.at<float>(nn,0) > max)
187             max = out.at<float>(nn,0);
188     }
189     for(nn=0; nn<N; nn++)
190         out.at<float>(nn,0) /= max; /* normalize everything */
191 
192     return out;
193 }
194 
195 
modified_bessel(int order,double x)196 static double modified_bessel(int order, double x)
197 {
198     //  sum m=0:inf 1/(m! * Gamma(m + order + 1)) * (x/2)^(2m + order)
199     const double eps = 1e-13;
200     double result = 0;
201     double m = 0;
202     double gamma = 1.0;
203     for(int i = 2; i <= order; ++i)
204         gamma *= i;
205     double term = pow(x,order) / (pow(2,order) * gamma);
206 
207     while(term  > eps * result) {
208         result += term;
209         //calculate new term in series
210         ++m;
211         term *= (x*x) / (4*m*(m+order));
212     }
213     return result;
214 }
215 
get_hann_win(Size sz)216 Mat get_hann_win(Size sz)
217 {
218     Mat hann_rows = Mat::ones(sz.height, 1, CV_32F);
219     Mat hann_cols = Mat::ones(1, sz.width, CV_32F);
220     int NN = sz.height - 1;
221     if(NN != 0) {
222         for (int i = 0; i < hann_rows.rows; ++i) {
223             hann_rows.at<float>(i,0) = (float)(1.0/2.0 * (1.0 - cos(2*CV_PI*i/NN)));
224         }
225     }
226     NN = sz.width - 1;
227     if(NN != 0) {
228         for (int i = 0; i < hann_cols.cols; ++i) {
229             hann_cols.at<float>(0,i) = (float)(1.0/2.0 * (1.0 - cos(2*CV_PI*i/NN)));
230         }
231     }
232     return hann_rows * hann_cols;
233 }
234 
get_kaiser_win(Size sz,float alpha)235 Mat get_kaiser_win(Size sz, float alpha)
236 {
237     Mat kaiser_rows = Mat::ones(sz.height, 1, CV_32F);
238     Mat kaiser_cols = Mat::ones(1, sz.width, CV_32F);
239 
240     int N = sz.height - 1;
241     double shape = alpha;
242     double den = 1.0 / modified_bessel(0, shape);
243 
244     for(int n = 0; n <= N; ++n) {
245         double K = (2.0 * n * 1.0/N) - 1.0;
246         double x = sqrt(1.0 - (K * K));
247         kaiser_rows.at<float>(n,0) = static_cast<float>(modified_bessel(0, shape * x) * den);
248     }
249 
250     N = sz.width - 1;
251     for(int n = 0; n <= N; ++n) {
252         double K = (2.0 * n * 1.0/N) - 1.0;
253         double x = sqrt(1.0 - (K * K));
254         kaiser_cols.at<float>(0,n) = static_cast<float>(modified_bessel(0, shape * x) * den);
255     }
256 
257     return kaiser_rows * kaiser_cols;
258 }
259 
get_chebyshev_win(Size sz,float attenuation)260 Mat get_chebyshev_win(Size sz, float attenuation)
261 {
262     Mat cheb_rows = chebwin(sz.height, attenuation);
263     Mat cheb_cols = chebwin(sz.width, attenuation).t();
264     return cheb_rows * cheb_cols;
265 }
266 
computeHOG32D(const Mat & imageM,Mat & featM,const int sbin,const int pad_x,const int pad_y)267 static void computeHOG32D(const Mat &imageM, Mat &featM, const int sbin, const int pad_x, const int pad_y)
268 {
269     const int dimHOG = 32;
270     CV_Assert(pad_x >= 0);
271     CV_Assert(pad_y >= 0);
272     CV_Assert(imageM.channels() == 3);
273     CV_Assert(imageM.depth() == CV_64F);
274 
275     // epsilon to avoid division by zero
276     const double eps = 0.0001;
277     // number of orientations
278     const int numOrient = 18;
279     // unit vectors to compute gradient orientation
280     const double uu[9] = {1.000, 0.9397, 0.7660, 0.5000, 0.1736, -0.1736, -0.5000, -0.7660, -0.9397};
281     const double vv[9] = {0.000, 0.3420, 0.6428, 0.8660, 0.9848,  0.9848,  0.8660,  0.6428,  0.3420};
282 
283     // image size
284     const Size imageSize = imageM.size();
285     // block size
286     // int bW = cvRound((double)imageSize.width/(double)sbin);
287     // int bH = cvRound((double)imageSize.height/(double)sbin);
288     int bW = cvFloor((double)imageSize.width/(double)sbin);
289     int bH = cvFloor((double)imageSize.height/(double)sbin);
290     const Size blockSize(bW, bH);
291     // size of HOG features
292     int oW = max(blockSize.width-2, 0) + 2*pad_x;
293     int oH = max(blockSize.height-2, 0) + 2*pad_y;
294     Size outSize = Size(oW, oH);
295     // size of visible
296     const Size visible = blockSize*sbin;
297 
298     // initialize historgram, norm, output feature matrices
299     Mat histM = Mat::zeros(Size(blockSize.width*numOrient, blockSize.height), CV_64F);
300     Mat normM = Mat::zeros(Size(blockSize.width, blockSize.height), CV_64F);
301     featM = Mat::zeros(Size(outSize.width*dimHOG, outSize.height), CV_64F);
302 
303     // get the stride of each matrix
304     const size_t imStride = imageM.step1();
305     const size_t histStride = histM.step1();
306     const size_t normStride = normM.step1();
307     const size_t featStride = featM.step1();
308 
309     // calculate the zero offset
310     const double* im = imageM.ptr<double>(0);
311     double* const hist = histM.ptr<double>(0);
312     double* const norm = normM.ptr<double>(0);
313     double* const feat = featM.ptr<double>(0);
314 
315     for (int y = 1; y < visible.height - 1; y++)
316     {
317         for (int x = 1; x < visible.width - 1; x++)
318         {
319             // OpenCV uses an interleaved format: BGR-BGR-BGR
320             const double* s = im + 3*min(x, imageM.cols-2) + min(y, imageM.rows-2)*imStride;
321 
322             // blue image channel
323             double dyb = *(s+imStride) - *(s-imStride);
324             double dxb = *(s+3) - *(s-3);
325             double vb = dxb*dxb + dyb*dyb;
326 
327             // green image channel
328             s += 1;
329             double dyg = *(s+imStride) - *(s-imStride);
330             double dxg = *(s+3) - *(s-3);
331             double vg = dxg*dxg + dyg*dyg;
332 
333             // red image channel
334             s += 1;
335             double dy = *(s+imStride) - *(s-imStride);
336             double dx = *(s+3) - *(s-3);
337             double v = dx*dx + dy*dy;
338 
339             // pick the channel with the strongest gradient
340             if (vg > v) { v = vg; dx = dxg; dy = dyg; }
341             if (vb > v) { v = vb; dx = dxb; dy = dyb; }
342 
343             // snap to one of the 18 orientations
344             double best_dot = 0;
345             int best_o = 0;
346             for (int o = 0; o < (int)numOrient/2; o++)
347             {
348                 double dot =  uu[o]*dx + vv[o]*dy;
349                 if (dot > best_dot)
350                 {
351                     best_dot = dot;
352                     best_o = o;
353                 }
354                 else if (-dot > best_dot)
355                 {
356                     best_dot = -dot;
357                     best_o = o + (int)(numOrient/2);
358                 }
359             }
360 
361             // add to 4 historgrams around pixel using bilinear interpolation
362             double yp =  ((double)y+0.5)/(double)sbin - 0.5;
363             double xp =  ((double)x+0.5)/(double)sbin - 0.5;
364             int iyp = (int)cvFloor(yp);
365             int ixp = (int)cvFloor(xp);
366             double vy0 = yp - iyp;
367             double vx0 = xp - ixp;
368             double vy1 = 1.0 - vy0;
369             double vx1 = 1.0 - vx0;
370             v = sqrt(v);
371 
372             // fill the value into the 4 neighborhood cells
373             if (iyp >= 0 && ixp >= 0)
374                 *(hist + iyp*histStride + ixp*numOrient + best_o) += vy1*vx1*v;
375 
376             if (iyp >= 0 && ixp+1 < blockSize.width)
377                 *(hist + iyp*histStride + (ixp+1)*numOrient + best_o) += vx0*vy1*v;
378 
379             if (iyp+1 < blockSize.height && ixp >= 0)
380                 *(hist + (iyp+1)*histStride + ixp*numOrient + best_o) += vy0*vx1*v;
381 
382             if (iyp+1 < blockSize.height && ixp+1 < blockSize.width)
383                 *(hist + (iyp+1)*histStride + (ixp+1)*numOrient + best_o) += vy0*vx0*v;
384 
385         } // for y
386     } // for x
387 
388     // compute the energy in each block by summing over orientation
389     for (int y = 0; y < blockSize.height; y++)
390     {
391         const double* src = hist + y*histStride;
392         double* dst = norm + y*normStride;
393         double const* const dst_end = dst + blockSize.width;
394         // for each cell
395         while (dst < dst_end)
396         {
397             *dst = 0;
398             for (int o = 0; o < (int)(numOrient/2); o++)
399             {
400                 *dst += (*src + *(src + numOrient/2))*
401                     (*src + *(src + numOrient/2));
402                 src++;
403             }
404             dst++;
405             src += numOrient/2;
406         }
407     }
408 
409     // compute the features
410     for (int y = pad_y; y < outSize.height - pad_y; y++)
411     {
412         for (int x = pad_x; x < outSize.width - pad_x; x++)
413         {
414             double* dst = feat + y*featStride + x*dimHOG;
415             double* p, n1, n2, n3, n4;
416             const double* src;
417 
418             p = norm + (y - pad_y + 1)*normStride + (x - pad_x + 1);
419             n1 = 1.0f / sqrt(*p + *(p + 1) + *(p + normStride) + *(p + normStride + 1) + eps);
420             p = norm + (y - pad_y)*normStride + (x - pad_x + 1);
421             n2 = 1.0f / sqrt(*p + *(p + 1) + *(p + normStride) + *(p + normStride + 1) + eps);
422             p = norm + (y- pad_y + 1)*normStride + x - pad_x;
423             n3 = 1.0f / sqrt(*p + *(p + 1) + *(p + normStride) + *(p + normStride + 1) + eps);
424             p = norm + (y - pad_y)*normStride + x - pad_x;
425             n4 = 1.0f / sqrt(*p + *(p + 1) + *(p + normStride) + *(p + normStride + 1) + eps);
426 
427             double t1 = 0.0, t2 = 0.0, t3 = 0.0, t4 = 0.0;
428 
429             // contrast-sesitive features
430             src = hist + (y - pad_y + 1)*histStride + (x - pad_x + 1)*numOrient;
431             for (int o = 0; o < numOrient; o++)
432             {
433                 double val = *src;
434                 double h1 = min(val*n1, 0.2);
435                 double h2 = min(val*n2, 0.2);
436                 double h3 = min(val*n3, 0.2);
437                 double h4 = min(val*n4, 0.2);
438                 *(dst++) = 0.5 * (h1 + h2 + h3 + h4);
439 
440                 src++;
441                 t1 += h1;
442                 t2 += h2;
443                 t3 += h3;
444                 t4 += h4;
445             }
446 
447             // contrast-insensitive features
448             src =  hist + (y - pad_y + 1)*histStride + (x - pad_x + 1)*numOrient;
449             for (int o = 0; o < numOrient/2; o++)
450             {
451                 double sum = *src + *(src + numOrient/2);
452                 double h1 = min(sum * n1, 0.2);
453                 double h2 = min(sum * n2, 0.2);
454                 double h3 = min(sum * n3, 0.2);
455                 double h4 = min(sum * n4, 0.2);
456                 *(dst++) = 0.5 * (h1 + h2 + h3 + h4);
457                 src++;
458             }
459 
460             // texture features
461             *(dst++) = 0.2357 * t1;
462             *(dst++) = 0.2357 * t2;
463             *(dst++) = 0.2357 * t3;
464             *(dst++) = 0.2357 * t4;
465             // truncation feature
466             *dst = 0;
467         }// for x
468     }// for y
469     // Truncation features
470     for (int m = 0; m < featM.rows; m++)
471     {
472         for (int n = 0; n < featM.cols; n += dimHOG)
473         {
474             if (m > pad_y - 1 && m < featM.rows - pad_y && n > pad_x*dimHOG - 1 && n < featM.cols - pad_x*dimHOG)
475                 continue;
476 
477             featM.at<double>(m, n + dimHOG - 1) = 1;
478         } // for x
479     }// for y
480 }
481 
get_features_hog(const Mat & im,const int bin_size)482 std::vector<Mat> get_features_hog(const Mat &im, const int bin_size)
483 {
484     Mat hogmatrix;
485     Mat im_;
486     im.convertTo(im_, CV_64FC3, 1.0/255.0);
487     computeHOG32D(im_,hogmatrix,bin_size,1,1);
488     hogmatrix.convertTo(hogmatrix, CV_32F);
489     Size hog_size = im.size();
490     hog_size.width /= bin_size;
491     hog_size.height /= bin_size;
492     Mat hogc(hog_size, CV_32FC(32), hogmatrix.data);
493     std::vector<Mat> features;
494     split(hogc, features);
495     return features;
496 }
497 
get_features_cn(const Mat & ppatch_data,const Size & output_size)498 std::vector<Mat> get_features_cn(const Mat &ppatch_data, const Size &output_size) {
499     Mat patch_data = ppatch_data.clone();
500     Vec3b & pixel = patch_data.at<Vec3b>(0,0);
501     unsigned index;
502 
503     Mat cnFeatures = Mat::zeros(patch_data.rows,patch_data.cols,CV_32FC(10));
504 
505     for(int i=0;i<patch_data.rows;i++){
506         for(int j=0;j<patch_data.cols;j++){
507             pixel=patch_data.at<Vec3b>(i,j);
508             index=(unsigned)(cvFloor((float)pixel[2]/8)+32*cvFloor((float)pixel[1]/8)+32*32*cvFloor((float)pixel[0]/8));
509 
510             //copy the values
511             for(int k=0;k<10;k++){
512                 cnFeatures.at<Vec<float,10> >(i,j)[k]=(float)ColorNames[index][k];
513             }
514         }
515     }
516     std::vector<Mat> result;
517     split(cnFeatures, result);
518     for (size_t i = 0; i < result.size(); i++) {
519         if (output_size.width > 0 && output_size.height > 0) {
520             resize(result.at(i), result.at(i), output_size, INTER_CUBIC);
521         }
522     }
523     return result;
524 }
525 
get_features_rgb(const Mat & patch,const Size & output_size)526 std::vector<Mat> get_features_rgb(const Mat &patch, const Size &output_size)
527 {
528     std::vector<Mat> channels;
529     split(patch, channels);
530     for(size_t k=0; k<channels.size(); k++) {
531         channels[k].convertTo(channels[k], CV_32F, 1.0/255.0, -0.5);
532         channels[k] = channels[k] - mean(channels[k])[0];
533         resize(channels[k], channels[k], output_size, INTER_CUBIC);
534     }
535     return channels;
536 }
537 
get_max(const Mat & m)538 double get_max(const Mat &m)
539 {
540     double val;
541     minMaxLoc(m, NULL, &val, NULL, NULL);
542     return val;
543 }
544 
get_min(const Mat & m)545 double get_min(const Mat &m)
546 {
547     double val;
548     minMaxLoc(m, &val, NULL, NULL, NULL);
549     return val;
550 }
551 
bgr2hsv(const Mat & img)552 Mat bgr2hsv(const Mat &img)
553 {
554     Mat hsv_img;
555     cvtColor(img, hsv_img, COLOR_BGR2HSV);
556     std::vector<Mat> hsv_img_channels;
557     split(hsv_img, hsv_img_channels);
558     hsv_img_channels.at(0).convertTo(hsv_img_channels.at(0), CV_8UC1, 255.0 / 180.0);
559     merge(hsv_img_channels, hsv_img);
560     return hsv_img;
561 }
562 
563 } //cv namespace
564