Vadim Pisarevsky
14 years ago
parent
c26b005371
commit
32ed1bf858
5 changed files with 559 additions and 0 deletions
@ -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); |
||||
} |
||||
} |
||||
@ -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(); } |
||||
|
||||
} |
||||
@ -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; |
||||
} |
||||
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in new issue