added phaseCorrelate function by Will Lucas.

modules/imgproc/doc/motion_analysis_and_object_tracking.rst

@@ -139,3 +139,67 @@ The function supports multi-channel images. Each channel is processed independen
     :ocv:func:`accumulate`,
     :ocv:func:`accumulateSquare`,
     :ocv:func:`accumulateProduct` 
+
+
+
+phaseCorrelate
+-------------------------------
+The function is used to detect translational shifts that occur between two images. The operation takes advantage of the Fourier shift theorem for detecting the translational shift in the frequency domain. It can be used for fast image registration as well as motion esitimation. For more information please see http://http://en.wikipedia.org/wiki/Phase\_correlation .
+
+Calculates the cross-power spectrum of two supplied source arrays. The arrays are padded if needed with ``getOptimalDFTSize`` .
+
+.. ocv:function:: Point2d phaseCorrelate(InputArray src1, InputArray src2, InputArray window = noArray())
+    :param src1: Source floating point array (CV_32FC1 or CV_64FC1)
+    :param src2: Source floating point array (CV_32FC1 or CV_64FC1)
+    :param window: Floating point array with windowing coefficients to reduce edge effects (optional).
+    :param result: Detected phase shift (sub-pixel) between the two arrays.
+
+The function performs the following equations
+
+*
+    First it applies a Hanning window (see http://en.wikipedia.org/wiki/Hann\_function) to each image to remove possible edge effects. This window is cached until the array size changes to speed up processing time.
+
+*
+    Next it computes the forward DFTs of each source array:
+    ..math:
+        \mathbf{G}_a = \mathcal{F}\{src_1\}, \; \mathbf{G}_b = \mathcal{F}\{src_2\}
+    where
+    :math:`\mathcal{F} is the forward DFT.`
+
+*
+    It then computes the cross-power spectrum of each frequency domain array:
+    ..math:
+        R = \frac{ \mathbf{G}_a \mathbf{G}_b^*}{|\mathbf{G}_a \mathbf{G}_b^*|}
+
+*
+    Next the cross-correlation is converted back into the time domain via the inverse DFT:
+    ..math:
+        r = \mathcal{F}^{-1}\{R\}
+*
+    Finally, it computes the peak location and computes a 5x5 weighted centroid around the peak to achieve sub-pixel accuracy.
+    ..math:
+        (\Delta x, \Delta y) = \textrm{weighted_centroid}\{\arg \max_{(x, y)}\{r\}\}
+
+.. seealso::
+    :ocv:func:`dft`,
+    :ocv:func:`getOptimalDFTSize`,
+    :ocv:func:`idft`,
+    :ocv:func:`mulSpectrums`
+    :ocv:func:`createHanningWindow`
+
+createHanningWindow
+-------------------------------
+This function computes a Hanning window coefficients in two dimensions. See http://en.wikipedia.org/wiki/Hann\_function and http://en.wikipedia.org/wiki/Window\_function for more information.
+
+.. ocv:function:: void createHanningWindow(OutputArray dst, Size winSize, int type)
+    :param dst: Destination array to place Hann coefficients in
+    :param winSize: The window size specifications
+    :param type: Created array type
+
+::
+    // create hanning window of size 100x100 and type CV_32F 
+    Mat hann;
+    createHanningWindow(hann, Size(100, 100), CV_32F);
+
+.. seealso::
+    :ocv:func:`phaseCorrelate`

modules/imgproc/include/opencv2/imgproc/imgproc.hpp

@@ -1132,6 +1132,14 @@ struct CvLSHOperations
   virtual int hash_lookup(lsh_hash h, int l, int* ret_i, int ret_i_max) = 0;
 };
 
+namespace cv
+{
+
+CV_EXPORTS_W cv::Point2d phaseCorrelate(InputArray _src1, InputArray _src2, InputArray window = noArray());
+CV_EXPORTS_W void createHanningWindow(OutputArray _dst, cv::Size winSize, int type);
+
+}
+
 #endif /* __cplusplus */
 
 #endif

modules/imgproc/src/phasecorr.cpp

@@ -0,0 +1,349 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2008-2011, William Lucas
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#include "precomp.hpp"
+#include <vector>
+
+namespace cv
+{
+
+static void divComplex(InputArray _src1, InputArray _src2, OutputArray _dst)
+{
+    Mat src1 = _src1.getMat();
+    Mat src2 = _src2.getMat();
+    
+    CV_Assert( src1.type() == src2.type() && src1.size() == src2.size());
+    CV_Assert( src1.type() == CV_32FC2 || src1.type() == CV_64FC2 );
+    
+    _dst.create(src1.size(), src1.type());
+    Mat dst  = _dst.getMat();
+    
+    int length = src1.rows*src1.cols;
+    
+    if(src1.depth() == CV_32F)
+    {
+        const float* dataA = (const float*)src1.data;
+        const float* dataB = (const float*)src2.data;
+        float* dataC = (float*)dst.data;
+        float eps = FLT_EPSILON; // prevent div0 problems
+        
+        for(int j = 0; j < length - 1; j += 2)
+        {
+            double denom = (double)(dataB[j]*dataB[j] + dataB[j+1]*dataB[j+1] + eps);
+            double re = (double)(dataA[j]*dataB[j] + dataA[j+1]*dataB[j+1]);
+            double im = (double)(dataA[j+1]*dataB[j] - dataA[j]*dataB[j+1]);
+            dataC[j] = (float)(re / denom);
+            dataC[j+1] = (float)(im / denom);
+        }
+    }
+    else
+    {
+        const double* dataA = (const double*)src1.data;
+        const double* dataB = (const double*)src2.data;
+        double* dataC = (double*)dst.data;
+        double eps = DBL_EPSILON; // prevent div0 problems
+        
+        for(int j = 0; j < length - 1; j += 2)
+        {
+            double denom = dataB[j]*dataB[j] + dataB[j+1]*dataB[j+1] + eps;
+            double re = dataA[j]*dataB[j] + dataA[j+1]*dataB[j+1];
+            double im = dataA[j+1]*dataB[j] - dataA[j]*dataB[j+1];
+            dataC[j] = re / denom;
+            dataC[j+1] = im / denom;
+        }
+    }
+}
+
+static void absComplex(InputArray _src, OutputArray _dst)
+{
+    Mat src = _src.getMat();
+    
+    CV_Assert( src.type() == CV_32FC2 || src.type() == CV_64FC2 );
+    
+    vector<Mat> planes;
+    split(src, planes);
+    
+    magnitude(planes[0], planes[1], planes[0]);
+    planes[1] = Mat::zeros(planes[0].size(), planes[0].type());
+    
+    merge(planes, _dst);
+}
+
+static void fftShift(InputOutputArray _out)
+{
+    Mat out = _out.getMat();
+    
+    vector<Mat> planes;
+    split(out, planes);
+    
+    int xMid = out.cols >> 1;
+    int yMid = out.rows >> 1;
+    
+    for(size_t i = 0; i < planes.size(); i++)
+    {
+        // perform quadrant swaps...
+        Mat tmp;
+        Mat q0(planes[i], Rect(0,    0,    xMid, yMid));
+        Mat q1(planes[i], Rect(xMid, 0,    xMid, yMid));
+        Mat q2(planes[i], Rect(0,    yMid, xMid, yMid));
+        Mat q3(planes[i], Rect(xMid, yMid, xMid, yMid));
+        
+        q0.copyTo(tmp);
+        q3.copyTo(q0);
+        tmp.copyTo(q3);
+        
+        q1.copyTo(tmp);
+        q2.copyTo(q1);
+        tmp.copyTo(q2);
+    }
+    
+    merge(planes, out);
+}
+
+Point2d weightedCentroid(InputArray _src, cv::Point peakLocation, cv::Size weightBoxSize)
+{
+    Mat src = _src.getMat();
+    
+    int type = src.type();
+    CV_Assert( type == CV_32FC1 || type == CV_64FC1 );
+    
+    int minr = peakLocation.y - (weightBoxSize.height >> 1);
+    int maxr = peakLocation.y + (weightBoxSize.height >> 1);
+    int minc = peakLocation.x - (weightBoxSize.width  >> 1);
+    int maxc = peakLocation.x + (weightBoxSize.width  >> 1);
+    
+    Point2d centroid;
+    double sumIntensity = 0.0;
+    
+    // clamp the values to min and max if needed.
+    if(minr < 0)
+    {
+        minr = 0;
+    }
+    
+    if(minc < 0)
+    {
+        minc = 0;
+    }
+    
+    if(maxr > src.rows - 1)
+    {
+        maxr = src.rows - 1;
+    }
+    
+    if(maxc > src.cols - 1)
+    {
+        maxc = src.cols - 1;
+    }
+    
+    if(type == CV_32FC1)
+    {
+        const float* dataIn = (const float*)src.data;
+        dataIn += minr*src.cols;
+        for(int y = minr; y <= maxr; y++)
+        {
+            for(int x = minc; x <= maxc; x++)
+            {
+                centroid.x   += (double)x*dataIn[x];
+                centroid.y   += (double)y*dataIn[x];
+                sumIntensity += (double)dataIn[x];
+            }
+            
+            dataIn += src.cols;
+        }
+    }
+    else
+    {
+        const double* dataIn = (const double*)src.data;
+        dataIn += minr*src.cols;
+        for(int y = minr; y <= maxr; y++)
+        {
+            for(int x = minc; x <= maxc; x++)
+            {
+                centroid.x   += (double)x*dataIn[x];
+                centroid.y   += (double)y*dataIn[x];
+                sumIntensity += dataIn[x];
+            }
+            
+            dataIn += src.cols;
+        }
+    }
+    
+    sumIntensity += DBL_EPSILON; // prevent div0 problems...
+    
+    centroid.x /= sumIntensity;
+    centroid.y /= sumIntensity;
+    
+    return centroid;
+}
+    
+}
+
+cv::Point2d cv::phaseCorrelate(InputArray _src1, InputArray _src2, InputArray _window)
+{
+    Mat src1 = _src1.getMat();
+    Mat src2 = _src2.getMat();
+    Mat window = _window.getMat();
+
+    CV_Assert( src1.type() == src2.type());
+    CV_Assert( src1.type() == CV_32FC1 || src1.type() == CV_64FC1 );
+    CV_Assert( src1.size == src2.size);
+
+    if(!window.empty())
+    {
+        CV_Assert( src1.type() == window.type());
+        CV_Assert( src1.size == window.size);
+    }
+
+    int M = getOptimalDFTSize(src1.rows);
+    int N = getOptimalDFTSize(src1.cols);
+
+    Mat padded1, padded2, paddedWin;
+
+    if(M != src1.rows || N != src1.cols)
+    {
+        copyMakeBorder(src1, padded1, 0, M - src1.rows, 0, N - src1.cols, BORDER_CONSTANT, Scalar::all(0));
+        copyMakeBorder(src2, padded2, 0, M - src2.rows, 0, N - src2.cols, BORDER_CONSTANT, Scalar::all(0));
+
+        if(!window.empty())
+        {
+            copyMakeBorder(window, paddedWin, 0, M - window.rows, 0, N - window.cols, BORDER_CONSTANT, Scalar::all(0));
+        }
+    }
+    else
+    {
+        padded1 = src1;
+        padded2 = src2;
+        paddedWin = window;
+    }
+
+    Mat FFT1, FFT2, P, Pm, C;
+
+    // perform window multiplication if available
+    if(!paddedWin.empty())
+    {
+        // apply window to both images before proceeding...
+        multiply(paddedWin, padded1, padded1);
+        multiply(paddedWin, padded2, padded2);
+    }
+
+    // TODO should be able to improve speed by switching to CCS packed matrices
+    vector<Mat> cplx1, cplx2;
+    cplx1.push_back(padded1);
+    cplx1.push_back(Mat::zeros(padded1.size(), padded1.type()));
+    merge(cplx1, FFT1);
+
+    cplx2.push_back(padded2);
+    cplx2.push_back(Mat::zeros(padded2.size(), padded2.type()));
+    merge(cplx2, FFT2);
+
+    // execute phase correlation equation
+    // Reference: http://en.wikipedia.org/wiki/Phase_correlation
+    dft(FFT1, FFT1);
+    dft(FFT2, FFT2);
+
+    mulSpectrums(FFT1, FFT2, P, 0, true);
+
+    // TODO these two functions need to be changed to work with CCS packed matrices...
+    absComplex(P, Pm);
+    divComplex(P, Pm, C); // FF* / |FF*| (phase correlation equation completed here...)
+
+    idft(C, C); // gives us the nice peak shift location...
+
+    vector<Mat> Cplanes;
+    split(C, Cplanes);
+    C = Cplanes[0]; // use only the real plane since that's all that's left...
+
+    fftShift(C); // shift the energy to the center of the frame.
+
+    // locate the highest peak
+    Point peakLoc;
+    minMaxLoc(C, NULL, NULL, NULL, &peakLoc);
+
+    // get the phase shift with sub-pixel accuracy, 5x5 window seems about right here...
+    Point2d t;
+    t = weightedCentroid(C, peakLoc, Size(5, 5));
+
+    // adjust shift relative to image center...
+    Point2d center((double)src1.cols / 2.0, (double)src1.rows / 2.0);
+
+    return (center - t);
+}
+
+
+void cv::createHanningWindow(OutputArray _dst, cv::Size winSize, int type)
+{
+    CV_Assert( type == CV_32FC1 || type == CV_64FC1 );
+
+    _dst.create(winSize, type);
+    Mat dst = _dst.getMat();
+
+    int rows = dst.rows;
+    int cols = dst.cols;
+
+    if(dst.depth() == CV_32F)
+    {
+        float* dstData = (float*)dst.data;
+
+        for(int i = 0; i < rows; i++)
+        {
+            double wr = 0.5 * (1.0f - cos(2.0f * M_PI * (double)i / (double)(rows - 1)));
+            for(int j = 0; j < cols; j++)
+            {
+                double wc = 0.5 * (1.0f - cos(2.0f * M_PI * (double)j / (double)(cols - 1)));
+                dstData[i*cols + j] = wr * wc;
+            }
+        }
+
+        // perform batch sqrt for SSE performance gains
+        cv::sqrt(dst, dst);
+    }
+    else
+    {
+        double* dstData = (double*)dst.data;
+
+        for(int i = 0; i < rows; i++)
+        {
+            double wr = 0.5 * (1.0 - cos(2.0 * M_PI * (double)i / (double)(rows - 1)));
+            for(int j = 0; j < cols; j++)
+            {
+                double wc = 0.5 * (1.0 - cos(2.0 * M_PI * (double)j / (double)(cols - 1)));
+                dstData[i*cols + j] = wr * wc;
+            }
+        }
+
+        // perform batch sqrt for SSE performance gains
+        cv::sqrt(dst, dst);
+    }
+}

modules/imgproc/test/test_pc.cpp

@@ -0,0 +1,89 @@
+/*M///////////////////////////////////////////////////////////////////////////////////////
+//
+//  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
+//
+//  By downloading, copying, installing or using the software you agree to this license.
+//  If you do not agree to this license, do not download, install,
+//  copy or use the software.
+//
+//
+//                           License Agreement
+//                For Open Source Computer Vision Library
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+// the use of this software, even if advised of the possibility of such damage.
+//
+//M*/
+
+#include "test_precomp.hpp"
+
+using namespace cv;
+using namespace std;
+
+namespace cvtest
+{
+
+/// phase correlation
+class CV_PhaseCorrelatorTest : public cvtest::ArrayTest
+{
+public:
+    CV_PhaseCorrelatorTest();
+protected:
+    void run( int );
+};
+
+CV_PhaseCorrelatorTest::CV_PhaseCorrelatorTest() {}
+
+void CV_PhaseCorrelatorTest::run( int )
+{
+    ts->set_failed_test_info(cvtest::TS::OK);
+
+    Mat r1 = Mat::ones(Size(128, 128), CV_64F);
+    Mat r2 = Mat::ones(Size(128, 128), CV_64F);
+
+    double expectedShiftX = -10.0;
+    double expectedShiftY = -20.0;
+
+    // draw 10x10 rectangles @ (100, 100) and (90, 80) should see ~(-10, -20) shift here...
+    cv::rectangle(r1, Point(100, 100), Point(110, 110), Scalar(0, 0, 0), CV_FILLED);
+    cv::rectangle(r2, Point(90, 80), Point(100, 90), Scalar(0, 0, 0), CV_FILLED);
+
+    Mat hann;
+    createHanningWindow(hann, r1.size(), CV_64F);
+    Point2d phaseShift = phaseCorrelate(r1, r2, hann);
+
+    // test accuracy should be less than 1 pixel...
+    if((expectedShiftX - phaseShift.x) >= 1 || (expectedShiftY - phaseShift.y) >= 1)
+    {
+         ts->set_failed_test_info( cvtest::TS::FAIL_BAD_ACCURACY );
+    }
+}
+
+TEST(Imgproc_PhaseCorrelatorTest, accuracy) { CV_PhaseCorrelatorTest test; test.safe_run(); }
+
+}

samples/cpp/phase_corr.cpp

@@ -0,0 +1,49 @@
+#include "opencv2/core/core.hpp"
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+
+using namespace cv;
+
+int main(int argc, char* argv[])
+{
+    VideoCapture video(0);
+    Mat frame, curr, prev, curr64f, prev64f, hann;
+    int key = 0;
+
+    do
+    {
+        video >> frame;
+        cvtColor(frame, curr, CV_RGB2GRAY);
+
+        if(prev.empty())
+        {
+            prev = curr.clone();
+            createHanningWindow(hann, curr.size(), CV_64F);
+        }
+
+        prev.convertTo(prev64f, CV_64F);
+        curr.convertTo(curr64f, CV_64F);
+
+        Point2d shift = phaseCorrelate(prev64f, curr64f, hann);
+        double radius = cv::sqrt(shift.x*shift.x + shift.y*shift.y);
+
+        if(radius > 5)
+        {
+            // draw a circle and line indicating the shift direction...
+            Point center(curr.cols >> 1, curr.rows >> 1);
+            cv::circle(frame, center, (int)radius, cv::Scalar(0, 255, 0), 3, CV_AA);
+            cv::line(frame, center, Point(center.x + (int)shift.x, center.y + (int)shift.y), cv::Scalar(0, 255, 0), 3, CV_AA);
+        }
+
+        imshow("phase shift", frame);
+        key = waitKey(2);
+
+        prev = curr.clone();
+    } while((char)key != 27); // Esc to exit...
+
+    return 0;
+}
+
+
+
+