1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008-2011, William Lucas
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35 #include "precomp.hpp"
36 #include <vector>
37
38 namespace cv
39 {
40
magSpectrums(InputArray _src,OutputArray _dst)41 static void magSpectrums( InputArray _src, OutputArray _dst)
42 {
43 Mat src = _src.getMat();
44 int depth = src.depth(), cn = src.channels(), type = src.type();
45 int rows = src.rows, cols = src.cols;
46 int j, k;
47
48 CV_Assert( type == CV_32FC1 || type == CV_32FC2 || type == CV_64FC1 || type == CV_64FC2 );
49
50 if(src.depth() == CV_32F)
51 _dst.create( src.rows, src.cols, CV_32FC1 );
52 else
53 _dst.create( src.rows, src.cols, CV_64FC1 );
54
55 Mat dst = _dst.getMat();
56 dst.setTo(0);//Mat elements are not equal to zero by default!
57
58 bool is_1d = (rows == 1 || (cols == 1 && src.isContinuous() && dst.isContinuous()));
59
60 if( is_1d )
61 cols = cols + rows - 1, rows = 1;
62
63 int ncols = cols*cn;
64 int j0 = cn == 1;
65 int j1 = ncols - (cols % 2 == 0 && cn == 1);
66
67 if( depth == CV_32F )
68 {
69 const float* dataSrc = src.ptr<float>();
70 float* dataDst = dst.ptr<float>();
71
72 size_t stepSrc = src.step/sizeof(dataSrc[0]);
73 size_t stepDst = dst.step/sizeof(dataDst[0]);
74
75 if( !is_1d && cn == 1 )
76 {
77 for( k = 0; k < (cols % 2 ? 1 : 2); k++ )
78 {
79 if( k == 1 )
80 dataSrc += cols - 1, dataDst += cols - 1;
81 dataDst[0] = dataSrc[0]*dataSrc[0];
82 if( rows % 2 == 0 )
83 dataDst[(rows-1)*stepDst] = dataSrc[(rows-1)*stepSrc]*dataSrc[(rows-1)*stepSrc];
84
85 for( j = 1; j <= rows - 2; j += 2 )
86 {
87 dataDst[j*stepDst] = (float)std::sqrt((double)dataSrc[j*stepSrc]*dataSrc[j*stepSrc] +
88 (double)dataSrc[(j+1)*stepSrc]*dataSrc[(j+1)*stepSrc]);
89 }
90
91 if( k == 1 )
92 dataSrc -= cols - 1, dataDst -= cols - 1;
93 }
94 }
95
96 for( ; rows--; dataSrc += stepSrc, dataDst += stepDst )
97 {
98 if( is_1d && cn == 1 )
99 {
100 dataDst[0] = dataSrc[0]*dataSrc[0];
101 if( cols % 2 == 0 )
102 dataDst[j1] = dataSrc[j1]*dataSrc[j1];
103 }
104
105 for( j = j0; j < j1; j += 2 )
106 {
107 dataDst[j] = (float)std::sqrt((double)dataSrc[j]*dataSrc[j] + (double)dataSrc[j+1]*dataSrc[j+1]);
108 }
109 }
110 }
111 else
112 {
113 const double* dataSrc = src.ptr<double>();
114 double* dataDst = dst.ptr<double>();
115
116 size_t stepSrc = src.step/sizeof(dataSrc[0]);
117 size_t stepDst = dst.step/sizeof(dataDst[0]);
118
119 if( !is_1d && cn == 1 )
120 {
121 for( k = 0; k < (cols % 2 ? 1 : 2); k++ )
122 {
123 if( k == 1 )
124 dataSrc += cols - 1, dataDst += cols - 1;
125 dataDst[0] = dataSrc[0]*dataSrc[0];
126 if( rows % 2 == 0 )
127 dataDst[(rows-1)*stepDst] = dataSrc[(rows-1)*stepSrc]*dataSrc[(rows-1)*stepSrc];
128
129 for( j = 1; j <= rows - 2; j += 2 )
130 {
131 dataDst[j*stepDst] = std::sqrt(dataSrc[j*stepSrc]*dataSrc[j*stepSrc] +
132 dataSrc[(j+1)*stepSrc]*dataSrc[(j+1)*stepSrc]);
133 }
134
135 if( k == 1 )
136 dataSrc -= cols - 1, dataDst -= cols - 1;
137 }
138 }
139
140 for( ; rows--; dataSrc += stepSrc, dataDst += stepDst )
141 {
142 if( is_1d && cn == 1 )
143 {
144 dataDst[0] = dataSrc[0]*dataSrc[0];
145 if( cols % 2 == 0 )
146 dataDst[j1] = dataSrc[j1]*dataSrc[j1];
147 }
148
149 for( j = j0; j < j1; j += 2 )
150 {
151 dataDst[j] = std::sqrt(dataSrc[j]*dataSrc[j] + dataSrc[j+1]*dataSrc[j+1]);
152 }
153 }
154 }
155 }
156
divSpectrums(InputArray _srcA,InputArray _srcB,OutputArray _dst,int flags,bool conjB)157 static void divSpectrums( InputArray _srcA, InputArray _srcB, OutputArray _dst, int flags, bool conjB)
158 {
159 Mat srcA = _srcA.getMat(), srcB = _srcB.getMat();
160 int depth = srcA.depth(), cn = srcA.channels(), type = srcA.type();
161 int rows = srcA.rows, cols = srcA.cols;
162 int j, k;
163
164 CV_Assert( type == srcB.type() && srcA.size() == srcB.size() );
165 CV_Assert( type == CV_32FC1 || type == CV_32FC2 || type == CV_64FC1 || type == CV_64FC2 );
166
167 _dst.create( srcA.rows, srcA.cols, type );
168 Mat dst = _dst.getMat();
169
170 bool is_1d = (flags & DFT_ROWS) || (rows == 1 || (cols == 1 &&
171 srcA.isContinuous() && srcB.isContinuous() && dst.isContinuous()));
172
173 if( is_1d && !(flags & DFT_ROWS) )
174 cols = cols + rows - 1, rows = 1;
175
176 int ncols = cols*cn;
177 int j0 = cn == 1;
178 int j1 = ncols - (cols % 2 == 0 && cn == 1);
179
180 if( depth == CV_32F )
181 {
182 const float* dataA = srcA.ptr<float>();
183 const float* dataB = srcB.ptr<float>();
184 float* dataC = dst.ptr<float>();
185 float eps = FLT_EPSILON; // prevent div0 problems
186
187 size_t stepA = srcA.step/sizeof(dataA[0]);
188 size_t stepB = srcB.step/sizeof(dataB[0]);
189 size_t stepC = dst.step/sizeof(dataC[0]);
190
191 if( !is_1d && cn == 1 )
192 {
193 for( k = 0; k < (cols % 2 ? 1 : 2); k++ )
194 {
195 if( k == 1 )
196 dataA += cols - 1, dataB += cols - 1, dataC += cols - 1;
197 dataC[0] = dataA[0] / (dataB[0] + eps);
198 if( rows % 2 == 0 )
199 dataC[(rows-1)*stepC] = dataA[(rows-1)*stepA] / (dataB[(rows-1)*stepB] + eps);
200 if( !conjB )
201 for( j = 1; j <= rows - 2; j += 2 )
202 {
203 double denom = (double)dataB[j*stepB]*dataB[j*stepB] +
204 (double)dataB[(j+1)*stepB]*dataB[(j+1)*stepB] + (double)eps;
205
206 double re = (double)dataA[j*stepA]*dataB[j*stepB] +
207 (double)dataA[(j+1)*stepA]*dataB[(j+1)*stepB];
208
209 double im = (double)dataA[(j+1)*stepA]*dataB[j*stepB] -
210 (double)dataA[j*stepA]*dataB[(j+1)*stepB];
211
212 dataC[j*stepC] = (float)(re / denom);
213 dataC[(j+1)*stepC] = (float)(im / denom);
214 }
215 else
216 for( j = 1; j <= rows - 2; j += 2 )
217 {
218
219 double denom = (double)dataB[j*stepB]*dataB[j*stepB] +
220 (double)dataB[(j+1)*stepB]*dataB[(j+1)*stepB] + (double)eps;
221
222 double re = (double)dataA[j*stepA]*dataB[j*stepB] -
223 (double)dataA[(j+1)*stepA]*dataB[(j+1)*stepB];
224
225 double im = (double)dataA[(j+1)*stepA]*dataB[j*stepB] +
226 (double)dataA[j*stepA]*dataB[(j+1)*stepB];
227
228 dataC[j*stepC] = (float)(re / denom);
229 dataC[(j+1)*stepC] = (float)(im / denom);
230 }
231 if( k == 1 )
232 dataA -= cols - 1, dataB -= cols - 1, dataC -= cols - 1;
233 }
234 }
235
236 for( ; rows--; dataA += stepA, dataB += stepB, dataC += stepC )
237 {
238 if( is_1d && cn == 1 )
239 {
240 dataC[0] = dataA[0] / (dataB[0] + eps);
241 if( cols % 2 == 0 )
242 dataC[j1] = dataA[j1] / (dataB[j1] + eps);
243 }
244
245 if( !conjB )
246 for( j = j0; j < j1; j += 2 )
247 {
248 double denom = (double)(dataB[j]*dataB[j] + dataB[j+1]*dataB[j+1] + eps);
249 double re = (double)(dataA[j]*dataB[j] + dataA[j+1]*dataB[j+1]);
250 double im = (double)(dataA[j+1]*dataB[j] - dataA[j]*dataB[j+1]);
251 dataC[j] = (float)(re / denom);
252 dataC[j+1] = (float)(im / denom);
253 }
254 else
255 for( j = j0; j < j1; j += 2 )
256 {
257 double denom = (double)(dataB[j]*dataB[j] + dataB[j+1]*dataB[j+1] + eps);
258 double re = (double)(dataA[j]*dataB[j] - dataA[j+1]*dataB[j+1]);
259 double im = (double)(dataA[j+1]*dataB[j] + dataA[j]*dataB[j+1]);
260 dataC[j] = (float)(re / denom);
261 dataC[j+1] = (float)(im / denom);
262 }
263 }
264 }
265 else
266 {
267 const double* dataA = srcA.ptr<double>();
268 const double* dataB = srcB.ptr<double>();
269 double* dataC = dst.ptr<double>();
270 double eps = DBL_EPSILON; // prevent div0 problems
271
272 size_t stepA = srcA.step/sizeof(dataA[0]);
273 size_t stepB = srcB.step/sizeof(dataB[0]);
274 size_t stepC = dst.step/sizeof(dataC[0]);
275
276 if( !is_1d && cn == 1 )
277 {
278 for( k = 0; k < (cols % 2 ? 1 : 2); k++ )
279 {
280 if( k == 1 )
281 dataA += cols - 1, dataB += cols - 1, dataC += cols - 1;
282 dataC[0] = dataA[0] / (dataB[0] + eps);
283 if( rows % 2 == 0 )
284 dataC[(rows-1)*stepC] = dataA[(rows-1)*stepA] / (dataB[(rows-1)*stepB] + eps);
285 if( !conjB )
286 for( j = 1; j <= rows - 2; j += 2 )
287 {
288 double denom = dataB[j*stepB]*dataB[j*stepB] +
289 dataB[(j+1)*stepB]*dataB[(j+1)*stepB] + eps;
290
291 double re = dataA[j*stepA]*dataB[j*stepB] +
292 dataA[(j+1)*stepA]*dataB[(j+1)*stepB];
293
294 double im = dataA[(j+1)*stepA]*dataB[j*stepB] -
295 dataA[j*stepA]*dataB[(j+1)*stepB];
296
297 dataC[j*stepC] = re / denom;
298 dataC[(j+1)*stepC] = im / denom;
299 }
300 else
301 for( j = 1; j <= rows - 2; j += 2 )
302 {
303 double denom = dataB[j*stepB]*dataB[j*stepB] +
304 dataB[(j+1)*stepB]*dataB[(j+1)*stepB] + eps;
305
306 double re = dataA[j*stepA]*dataB[j*stepB] -
307 dataA[(j+1)*stepA]*dataB[(j+1)*stepB];
308
309 double im = dataA[(j+1)*stepA]*dataB[j*stepB] +
310 dataA[j*stepA]*dataB[(j+1)*stepB];
311
312 dataC[j*stepC] = re / denom;
313 dataC[(j+1)*stepC] = im / denom;
314 }
315 if( k == 1 )
316 dataA -= cols - 1, dataB -= cols - 1, dataC -= cols - 1;
317 }
318 }
319
320 for( ; rows--; dataA += stepA, dataB += stepB, dataC += stepC )
321 {
322 if( is_1d && cn == 1 )
323 {
324 dataC[0] = dataA[0] / (dataB[0] + eps);
325 if( cols % 2 == 0 )
326 dataC[j1] = dataA[j1] / (dataB[j1] + eps);
327 }
328
329 if( !conjB )
330 for( j = j0; j < j1; j += 2 )
331 {
332 double denom = dataB[j]*dataB[j] + dataB[j+1]*dataB[j+1] + eps;
333 double re = dataA[j]*dataB[j] + dataA[j+1]*dataB[j+1];
334 double im = dataA[j+1]*dataB[j] - dataA[j]*dataB[j+1];
335 dataC[j] = re / denom;
336 dataC[j+1] = im / denom;
337 }
338 else
339 for( j = j0; j < j1; j += 2 )
340 {
341 double denom = dataB[j]*dataB[j] + dataB[j+1]*dataB[j+1] + eps;
342 double re = dataA[j]*dataB[j] - dataA[j+1]*dataB[j+1];
343 double im = dataA[j+1]*dataB[j] + dataA[j]*dataB[j+1];
344 dataC[j] = re / denom;
345 dataC[j+1] = im / denom;
346 }
347 }
348 }
349
350 }
351
fftShift(InputOutputArray _out)352 static void fftShift(InputOutputArray _out)
353 {
354 Mat out = _out.getMat();
355
356 if(out.rows == 1 && out.cols == 1)
357 {
358 // trivially shifted.
359 return;
360 }
361
362 std::vector<Mat> planes;
363 split(out, planes);
364
365 int xMid = out.cols >> 1;
366 int yMid = out.rows >> 1;
367
368 bool is_1d = xMid == 0 || yMid == 0;
369
370 if(is_1d)
371 {
372 xMid = xMid + yMid;
373
374 for(size_t i = 0; i < planes.size(); i++)
375 {
376 Mat tmp;
377 Mat half0(planes[i], Rect(0, 0, xMid, 1));
378 Mat half1(planes[i], Rect(xMid, 0, xMid, 1));
379
380 half0.copyTo(tmp);
381 half1.copyTo(half0);
382 tmp.copyTo(half1);
383 }
384 }
385 else
386 {
387 for(size_t i = 0; i < planes.size(); i++)
388 {
389 // perform quadrant swaps...
390 Mat tmp;
391 Mat q0(planes[i], Rect(0, 0, xMid, yMid));
392 Mat q1(planes[i], Rect(xMid, 0, xMid, yMid));
393 Mat q2(planes[i], Rect(0, yMid, xMid, yMid));
394 Mat q3(planes[i], Rect(xMid, yMid, xMid, yMid));
395
396 q0.copyTo(tmp);
397 q3.copyTo(q0);
398 tmp.copyTo(q3);
399
400 q1.copyTo(tmp);
401 q2.copyTo(q1);
402 tmp.copyTo(q2);
403 }
404 }
405
406 merge(planes, out);
407 }
408
weightedCentroid(InputArray _src,cv::Point peakLocation,cv::Size weightBoxSize,double * response)409 static Point2d weightedCentroid(InputArray _src, cv::Point peakLocation, cv::Size weightBoxSize, double* response)
410 {
411 Mat src = _src.getMat();
412
413 int type = src.type();
414 CV_Assert( type == CV_32FC1 || type == CV_64FC1 );
415
416 int minr = peakLocation.y - (weightBoxSize.height >> 1);
417 int maxr = peakLocation.y + (weightBoxSize.height >> 1);
418 int minc = peakLocation.x - (weightBoxSize.width >> 1);
419 int maxc = peakLocation.x + (weightBoxSize.width >> 1);
420
421 Point2d centroid;
422 double sumIntensity = 0.0;
423
424 // clamp the values to min and max if needed.
425 if(minr < 0)
426 {
427 minr = 0;
428 }
429
430 if(minc < 0)
431 {
432 minc = 0;
433 }
434
435 if(maxr > src.rows - 1)
436 {
437 maxr = src.rows - 1;
438 }
439
440 if(maxc > src.cols - 1)
441 {
442 maxc = src.cols - 1;
443 }
444
445 if(type == CV_32FC1)
446 {
447 const float* dataIn = src.ptr<float>();
448 dataIn += minr*src.cols;
449 for(int y = minr; y <= maxr; y++)
450 {
451 for(int x = minc; x <= maxc; x++)
452 {
453 centroid.x += (double)x*dataIn[x];
454 centroid.y += (double)y*dataIn[x];
455 sumIntensity += (double)dataIn[x];
456 }
457
458 dataIn += src.cols;
459 }
460 }
461 else
462 {
463 const double* dataIn = src.ptr<double>();
464 dataIn += minr*src.cols;
465 for(int y = minr; y <= maxr; y++)
466 {
467 for(int x = minc; x <= maxc; x++)
468 {
469 centroid.x += (double)x*dataIn[x];
470 centroid.y += (double)y*dataIn[x];
471 sumIntensity += dataIn[x];
472 }
473
474 dataIn += src.cols;
475 }
476 }
477
478 if(response)
479 *response = sumIntensity;
480
481 sumIntensity += DBL_EPSILON; // prevent div0 problems...
482
483 centroid.x /= sumIntensity;
484 centroid.y /= sumIntensity;
485
486 return centroid;
487 }
488
489 }
490
phaseCorrelate(InputArray _src1,InputArray _src2,InputArray _window,double * response)491 cv::Point2d cv::phaseCorrelate(InputArray _src1, InputArray _src2, InputArray _window, double* response)
492 {
493 Mat src1 = _src1.getMat();
494 Mat src2 = _src2.getMat();
495 Mat window = _window.getMat();
496
497 CV_Assert( src1.type() == src2.type());
498 CV_Assert( src1.type() == CV_32FC1 || src1.type() == CV_64FC1 );
499 CV_Assert( src1.size == src2.size);
500
501 if(!window.empty())
502 {
503 CV_Assert( src1.type() == window.type());
504 CV_Assert( src1.size == window.size);
505 }
506
507 int M = getOptimalDFTSize(src1.rows);
508 int N = getOptimalDFTSize(src1.cols);
509
510 Mat padded1, padded2, paddedWin;
511
512 if(M != src1.rows || N != src1.cols)
513 {
514 copyMakeBorder(src1, padded1, 0, M - src1.rows, 0, N - src1.cols, BORDER_CONSTANT, Scalar::all(0));
515 copyMakeBorder(src2, padded2, 0, M - src2.rows, 0, N - src2.cols, BORDER_CONSTANT, Scalar::all(0));
516
517 if(!window.empty())
518 {
519 copyMakeBorder(window, paddedWin, 0, M - window.rows, 0, N - window.cols, BORDER_CONSTANT, Scalar::all(0));
520 }
521 }
522 else
523 {
524 padded1 = src1;
525 padded2 = src2;
526 paddedWin = window;
527 }
528
529 Mat FFT1, FFT2, P, Pm, C;
530
531 // perform window multiplication if available
532 if(!paddedWin.empty())
533 {
534 // apply window to both images before proceeding...
535 multiply(paddedWin, padded1, padded1);
536 multiply(paddedWin, padded2, padded2);
537 }
538
539 // execute phase correlation equation
540 // Reference: http://en.wikipedia.org/wiki/Phase_correlation
541 dft(padded1, FFT1, DFT_REAL_OUTPUT);
542 dft(padded2, FFT2, DFT_REAL_OUTPUT);
543
544 mulSpectrums(FFT1, FFT2, P, 0, true);
545
546 magSpectrums(P, Pm);
547 divSpectrums(P, Pm, C, 0, false); // FF* / |FF*| (phase correlation equation completed here...)
548
549 idft(C, C); // gives us the nice peak shift location...
550
551 fftShift(C); // shift the energy to the center of the frame.
552
553 // locate the highest peak
554 Point peakLoc;
555 minMaxLoc(C, NULL, NULL, NULL, &peakLoc);
556
557 // get the phase shift with sub-pixel accuracy, 5x5 window seems about right here...
558 Point2d t;
559 t = weightedCentroid(C, peakLoc, Size(5, 5), response);
560
561 // max response is M*N (not exactly, might be slightly larger due to rounding errors)
562 if(response)
563 *response /= M*N;
564
565 // adjust shift relative to image center...
566 Point2d center((double)padded1.cols / 2.0, (double)padded1.rows / 2.0);
567
568 return (center - t);
569 }
570
571
createHanningWindow(OutputArray _dst,cv::Size winSize,int type)572 void cv::createHanningWindow(OutputArray _dst, cv::Size winSize, int type)
573 {
574 CV_Assert( type == CV_32FC1 || type == CV_64FC1 );
575
576 _dst.create(winSize, type);
577 Mat dst = _dst.getMat();
578
579 int rows = dst.rows, cols = dst.cols;
580
581 AutoBuffer<double> _wc(cols);
582 double * const wc = (double *)_wc;
583
584 double coeff0 = 2.0 * CV_PI / (double)(cols - 1), coeff1 = 2.0f * CV_PI / (double)(rows - 1);
585 for(int j = 0; j < cols; j++)
586 wc[j] = 0.5 * (1.0 - cos(coeff0 * j));
587
588 if(dst.depth() == CV_32F)
589 {
590 for(int i = 0; i < rows; i++)
591 {
592 float* dstData = dst.ptr<float>(i);
593 double wr = 0.5 * (1.0 - cos(coeff1 * i));
594 for(int j = 0; j < cols; j++)
595 dstData[j] = (float)(wr * wc[j]);
596 }
597 }
598 else
599 {
600 for(int i = 0; i < rows; i++)
601 {
602 double* dstData = dst.ptr<double>(i);
603 double wr = 0.5 * (1.0 - cos(coeff1 * i));
604 for(int j = 0; j < cols; j++)
605 dstData[j] = wr * wc[j];
606 }
607 }
608
609 // perform batch sqrt for SSE performance gains
610 cv::sqrt(dst, dst);
611 }
612