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 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 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 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 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 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 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
