void crossCorr( const Mat& img, const Mat& _templ, Mat& corr,
Size corrsize, int ctype,
Point anchor, double delta, int borderType )
{
const double blockScale = 4.5;
const int minBlockSize = 256;
std::vector<uchar> buf;
Mat templ = _templ;
int depth = img.depth(), cn = img.channels();
int tdepth = templ.depth(), tcn = templ.channels();
int cdepth = CV_MAT_DEPTH(ctype), ccn = CV_MAT_CN(ctype);
CV_Assert( img.dims <= 2 && templ.dims <= 2 && corr.dims <= 2 );
if( depth != tdepth && tdepth != std::max(CV_32F, depth) )
{
_templ.convertTo(templ, std::max(CV_32F, depth));
tdepth = templ.depth();
}
CV_Assert( depth == tdepth || tdepth == CV_32F);
CV_Assert( corrsize.height <= img.rows + templ.rows - 1 &&
corrsize.width <= img.cols + templ.cols - 1 );
CV_Assert( ccn == 1 || delta == 0 );
corr.create(corrsize, ctype);
int maxDepth = depth > CV_8S ? CV_64F : std::max(std::max(CV_32F, tdepth), cdepth);
Size blocksize, dftsize;
blocksize.width = cvRound(templ.cols*blockScale);
blocksize.width = std::max( blocksize.width, minBlockSize - templ.cols + 1 );
blocksize.width = std::min( blocksize.width, corr.cols );
blocksize.height = cvRound(templ.rows*blockScale);
blocksize.height = std::max( blocksize.height, minBlockSize - templ.rows + 1 );
blocksize.height = std::min( blocksize.height, corr.rows );
dftsize.width = std::max(getOptimalDFTSize(blocksize.width + templ.cols - 1), 2);
dftsize.height = getOptimalDFTSize(blocksize.height + templ.rows - 1);
if( dftsize.width <= 0 || dftsize.height <= 0 )
CV_Error( CV_StsOutOfRange, "the input arrays are too big" );
// recompute block size
blocksize.width = dftsize.width - templ.cols + 1;
blocksize.width = MIN( blocksize.width, corr.cols );
blocksize.height = dftsize.height - templ.rows + 1;
blocksize.height = MIN( blocksize.height, corr.rows );
Mat dftTempl( dftsize.height*tcn, dftsize.width, maxDepth );
Mat dftImg( dftsize, maxDepth );
int i, k, bufSize = 0;
if( tcn > 1 && tdepth != maxDepth )
bufSize = templ.cols*templ.rows*CV_ELEM_SIZE(tdepth);
if( cn > 1 && depth != maxDepth )
bufSize = std::max( bufSize, (blocksize.width + templ.cols - 1)*
(blocksize.height + templ.rows - 1)*CV_ELEM_SIZE(depth));
if( (ccn > 1 || cn > 1) && cdepth != maxDepth )
bufSize = std::max( bufSize, blocksize.width*blocksize.height*CV_ELEM_SIZE(cdepth));
buf.resize(bufSize);
// compute DFT of each template plane
for( k = 0; k < tcn; k++ )
{
int yofs = k*dftsize.height;
Mat src = templ;
Mat dst(dftTempl, Rect(0, yofs, dftsize.width, dftsize.height));
Mat dst1(dftTempl, Rect(0, yofs, templ.cols, templ.rows));
if( tcn > 1 )
{
src = tdepth == maxDepth ? dst1 : Mat(templ.size(), tdepth, &buf[0]);
int pairs[] = {k, 0};
mixChannels(&templ, 1, &src, 1, pairs, 1);
}
if( dst1.data != src.data )
src.convertTo(dst1, dst1.depth());
if( dst.cols > templ.cols )
{
Mat part(dst, Range(0, templ.rows), Range(templ.cols, dst.cols));
part = Scalar::all(0);
}
dft(dst, dst, 0, templ.rows);
}
int tileCountX = (corr.cols + blocksize.width - 1)/blocksize.width;
int tileCountY = (corr.rows + blocksize.height - 1)/blocksize.height;
int tileCount = tileCountX * tileCountY;
Size wholeSize = img.size();
Point roiofs(0,0);
Mat img0 = img;
if( !(borderType & BORDER_ISOLATED) )
{
img.locateROI(wholeSize, roiofs);
img0.adjustROI(roiofs.y, wholeSize.height-img.rows-roiofs.y,
roiofs.x, wholeSize.width-img.cols-roiofs.x);
}
borderType |= BORDER_ISOLATED;
// calculate correlation by blocks
for( i = 0; i < tileCount; i++ )
{
int x = (i%tileCountX)*blocksize.width;
int y = (i/tileCountX)*blocksize.height;
Size bsz(std::min(blocksize.width, corr.cols - x),
std::min(blocksize.height, corr.rows - y));
Size dsz(bsz.width + templ.cols - 1, bsz.height + templ.rows - 1);
int x0 = x - anchor.x + roiofs.x, y0 = y - anchor.y + roiofs.y;
int x1 = std::max(0, x0), y1 = std::max(0, y0);
int x2 = std::min(img0.cols, x0 + dsz.width);
int y2 = std::min(img0.rows, y0 + dsz.height);
Mat src0(img0, Range(y1, y2), Range(x1, x2));
Mat dst(dftImg, Rect(0, 0, dsz.width, dsz.height));
Mat dst1(dftImg, Rect(x1-x0, y1-y0, x2-x1, y2-y1));
Mat cdst(corr, Rect(x, y, bsz.width, bsz.height));
for( k = 0; k < cn; k++ )
{
Mat src = src0;
dftImg = Scalar::all(0);
if( cn > 1 )
{
src = depth == maxDepth ? dst1 : Mat(y2-y1, x2-x1, depth, &buf[0]);
int pairs[] = {k, 0};
mixChannels(&src0, 1, &src, 1, pairs, 1);
}
if( dst1.data != src.data )
src.convertTo(dst1, dst1.depth());
if( x2 - x1 < dsz.width || y2 - y1 < dsz.height )
copyMakeBorder(dst1, dst, y1-y0, dst.rows-dst1.rows-(y1-y0),
x1-x0, dst.cols-dst1.cols-(x1-x0), borderType);
dft( dftImg, dftImg, 0, dsz.height );
Mat dftTempl1(dftTempl, Rect(0, tcn > 1 ? k*dftsize.height : 0,
dftsize.width, dftsize.height));
mulSpectrums(dftImg, dftTempl1, dftImg, 0, true);
dft( dftImg, dftImg, DFT_INVERSE + DFT_SCALE, bsz.height );
src = dftImg(Rect(0, 0, bsz.width, bsz.height));
if( ccn > 1 )
{
if( cdepth != maxDepth )
{
Mat plane(bsz, cdepth, &buf[0]);
src.convertTo(plane, cdepth, 1, delta);
src = plane;
}
int pairs[] = {0, k};
mixChannels(&src, 1, &cdst, 1, pairs, 1);
}
else
{
if( k == 0 )
src.convertTo(cdst, cdepth, 1, delta);
else
{
if( maxDepth != cdepth )
{
Mat plane(bsz, cdepth, &buf[0]);
src.convertTo(plane, cdepth);
src = plane;
}
add(src, cdst, cdst);
}
}
}
}
}
int main(int argc, char **argv){
int opcion; //Opcion para el getopt
int vflag=0, rflag=0, nflag=0, glfag=0, iflag=0, mflag=0, oflag=0; //Flags para el getopt
float r=0.5, g=1.0;
int n=2;
string nombreImagen;
string nombreMascara;
string nombreSalida = "output.png";
Mat imagen, padded, complexImg, filter, filterAux, imagenSalida, filterSalida, imagenFrecuencias, imagenFrecuenciasSinOrden, imagenHSV;
Mat complexAux;
Mat salida;
Mat imagenPasoBaja;
Mat mascara;
vector<Mat> canales;
while((opcion=getopt(argc, argv, "vr:n:g:i:o:m:")) !=-1 ){
switch(opcion){
case 'v':
vflag=1;
break;
case 'r':
rflag=1;
r=atof(optarg);
if(r<0 || r>1){
cout << "Valor de 'r' introducido invalido" << endl;
exit(-1);
}
break;
case 'n':
nflag=1;
n = atoi(optarg);
if(n<0 || n>10){
cout << "Valor de 'n' introducido invalido" << endl;
exit(-1);
}
break;
case 'g':
glfag=1;
g = atof(optarg);
if(g<0.0 || g>5.0){
cout << "Valor de 'g' introducido invalido" << endl;
exit(-1);
}
break;
case 'i':
iflag=1;
nombreImagen = optarg;
break;
case 'm':
mflag=1;
nombreMascara=optarg;
break;
case 'o':
oflag=1;
nombreSalida=optarg;
break;
case '?':
//Algo ha ido mal
help();
exit(-1);
break;
default:
help();
exit(-1);
break;
}
}
//Primero cargaremos la imagen
if(iflag==1){
imagen = imread(nombreImagen, CV_LOAD_IMAGE_ANYDEPTH);
if(imagen.empty()){
cout << "Imagen especificada invalida" << endl;
exit(-1);
}else{
cout << "Imagen cargada con exito" << endl;
if(vflag==1){
namedWindow("Imagen", CV_WINDOW_AUTOSIZE);
imshow("Imagen", imagen);
waitKey(0);
destroyWindow("Imagen");
}
}
}else{
cout << "La imagen es necesaria" << endl;
exit(-1);
}
//Calculamos r
r=(r)*(sqrt(pow((imagen.rows),2.0)+pow((imagen.cols),2.0))/2);
int M = getOptimalDFTSize(imagen.rows);
int N = getOptimalDFTSize(imagen.cols);
//Miramos si tiene mascara para cargarla
if(mflag==1){
//Cargamos la mascara
mascara = imread(nombreMascara, 0);
if(mascara.empty()){
cout << "Mascara especificada invalida" << endl;
exit(-1);
}else{
cout << "Mascara cargada con exito" << endl;
}
}
//Ahora miramos los canales para hacer cosas distintas dependiendo
if(imagen.channels()==1){
//Imagen monocromatica
imagen.convertTo(imagenPasoBaja,CV_32F, 1.0/255.0);
copyMakeBorder(imagenPasoBaja, padded, 0, M-imagenPasoBaja.rows, 0, N - imagenPasoBaja.cols, BORDER_CONSTANT, Scalar::all(0));
Mat planes[] = {Mat_<float>(padded), Mat::zeros(padded.size(), CV_32F)};
merge(planes, 2, complexImg);
dft(complexImg, complexImg);
filter = complexImg.clone();
filterAux = complexImg.clone();
complexAux = complexImg.clone();
shiftDFT(complexImg);
shiftDFT(complexAux);
butterworth(filter, r, n);
butterworth(filterAux, r, 0);
mulSpectrums(complexImg, filter, complexImg, 0);
mulSpectrums(complexAux, filterAux, complexAux, 0);
shiftDFT(complexImg);
shiftDFT(complexAux);
//Falta hacer lo de poder mostrarla
imagenFrecuencias = create_spectrum(complexImg);
imagenFrecuenciasSinOrden = create_spectrum(complexAux);
//Hacemos la inversa
idft(complexImg, complexImg, DFT_SCALE);
split(complexImg, planes);
normalize(planes[0], imagenSalida, 0, 1, CV_MINMAX);
split(filter, planes);
normalize(planes[0], filterSalida, 0, 1, CV_MINMAX);
salida = imagenPasoBaja.clone();
if(mflag==1){
//Con mascara procesaremos pixel por pixel
//Recorremos la imagen
for(int i=0; i<imagen.rows; i++){
for(int j=0; j<imagen.cols;j++){
if(mascara.at<uchar>(i,j)!=0){
salida.at<float>(i,j) = (g+1)*(imagenPasoBaja.at<float>(i,j)) - (g*imagenSalida.at<float>(i,j));
}
}
}
}else{
//Sin mascara lo haremos de forma inmediata
for(int i=0; i<imagen.rows; i++){
for(int j=0; j<imagen.cols;j++){
salida.at<float>(i,j) = ((g+1)*imagenPasoBaja.at<float>(i,j)) - (g*imagenSalida.at<float>(i,j));
}
}
}
salida.convertTo(salida, CV_8U, 255.0, 0.0);
if(vflag==1){
imshow("Imagen final", salida);
imshow("Filtro Butterworth", filterSalida);
imshow("Espectro", imagenFrecuencias);
imshow("Espectro de imagen sin orden", imagenFrecuenciasSinOrden);
waitKey(0);
}
}else{
//Spliteamos la imagen en canales
cvtColor(imagen, imagenHSV, CV_BGR2HSV);
split(imagenHSV, canales);
Mat temporal;
canales[2].convertTo(imagenPasoBaja, CV_32F, 1.0/255.0);
copyMakeBorder(imagenPasoBaja, padded, 0, M-imagenPasoBaja.rows, 0, N - imagenPasoBaja.cols, BORDER_CONSTANT, Scalar::all(0));
Mat planes[] = {Mat_<float>(padded), Mat::zeros(padded.size(), CV_32F)};
merge(planes, 2, complexImg);
dft(complexImg, complexImg);
filter = complexImg.clone();
shiftDFT(complexImg);
butterworth(filter, r, n);
mulSpectrums(complexImg, filter, complexImg, 0);
shiftDFT(complexImg);
//Falta hacer lo de poder mostrarla
imagenFrecuencias = create_spectrum(complexImg);
//Hacemos la inversa
idft(complexImg, complexImg, DFT_SCALE);
split(complexImg, planes);
normalize(planes[0], imagenSalida, 0, 1, CV_MINMAX);
split(filter, planes);
normalize(planes[0], filterSalida, 0, 1, CV_MINMAX);
Mat salida = imagen.clone();
canales[2] = imagenPasoBaja.clone();
if(mflag==1){
//Con mascara
for(int i=0; i<canales[2].rows; i++){
for(int j=0; j<canales[2].cols;j++){
if(mascara.at<uchar>(i,j)!=0){
canales[2].at<float>(i,j) = ((g+1)*imagenPasoBaja.at<float>(i,j)) - (g*imagenSalida.at<float>(i,j));
}
}
}
}else{
//Sin mascara
for(int i=0; i<canales[2].rows; i++){
for(int j=0; j<canales[2].cols;j++){
canales[2].at<float>(i,j) = ((g+1)*imagenPasoBaja.at<float>(i,j)) - (g*imagenSalida.at<float>(i,j));
}
}
}
canales[2].convertTo(canales[2], CV_8U, 255.0, 0.0);
merge(canales, salida);
cvtColor(salida, salida, CV_HSV2BGR);
salida.convertTo(salida, CV_8U, 255.0, 0.0);
if(vflag==1){
imshow("Imagen final", salida);
imshow("Filtro Butterworth", filterSalida);
imshow("Espectro", imagenFrecuencias);
imshow("Espectro de imagen sin orden", imagenFrecuenciasSinOrden);
waitKey(0);
}
}
//Y escribimos la imagen a fichero
imwrite(nombreSalida, salida);
return 0;
}
Point2d phaseCorrelate(InputArray _src1, InputArray _src2, InputArray _window, double* response)
{
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;
}
// perform window multiplication if available
if(!paddedWin.empty())
{
// apply window to both images before proceeding...
multiply(paddedWin, padded1, padded1);
multiply(paddedWin, padded2, padded2);
}
// execute phase correlation equation
// Reference: http://en.wikipedia.org/wiki/Phase_correlation
cv::Mat FFT1, FFT2;
dft(padded1, FFT1, DFT_COMPLEX_OUTPUT);
dft(padded2, FFT2, DFT_COMPLEX_OUTPUT);
// // high-pass filter
// cv::Mat hpFilter = 1-paddedWin;
// phasecorrelation::fftShift(hpFilter);
// for(int i=0; i<paddedWin.rows; i++){
// for(int j=0; j<paddedWin.cols; j++){
// FFT1.at<cv::Vec2f>(i,j) *= hpFilter.at<float>(i,j);
// FFT2.at<cv::Vec2f>(i,j) *= hpFilter.at<float>(i,j);
// }
// }
cv::Mat P;
cv::mulSpectrums(FFT1, FFT2, P, DFT_COMPLEX_OUTPUT, true);
cv::Mat Pm(P.size(), CV_32F);
// NOTE: memleak in magSpectrums when using it with complex output!
//phasecorrelation::magSpectrums(P, Pm);
for(int i=0; i<P.rows; i++){
for(int j=0; j<P.cols; j++){
cv::Vec2f e = P.at<cv::Vec2f>(i, j);
Pm.at<float>(i, j) = cv::sqrt(e[0]*e[0] + e[1]*e[1]);
}
}
//phasecorrelation::divSpectrums(P, Pm, C, 0, false); // FF* / |FF*| (phase correlation equation completed here...)
for(int i=0; i<P.rows; i++){
for(int j=0; j<P.cols; j++){
P.at<cv::Vec2f>(i, j) /= (Pm.at<float>(i, j) + DBL_EPSILON);
}
}
cv::Mat C(P.size(), CV_32F);
cv::dft(P, C, cv::DFT_INVERSE + cv::DFT_REAL_OUTPUT);
phasecorrelation::fftShift(C); // shift the energy to the center of the frame.
//cvtools::writeMat(C, "C.mat", "C");
// 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 = phasecorrelation::weightedCentroid(C, peakLoc, Size(3, 3), response);
// max response is M*N (not exactly, might be slightly larger due to rounding errors)
if(response)
*response /= M*N;
// adjust shift relative to image center...
Point2d center((double)padded1.cols / 2.0, (double)padded1.rows / 2.0);
return (center - t);
}
Mat make_dft(const Mat& IRGB) {
Mat I;
cvtColor(IRGB, I, CV_RGB2GRAY);
Mat padded; //expand input image to optimal size
int m = getOptimalDFTSize(I.rows);
int n = getOptimalDFTSize(I.cols); // on the border add zero values
copyMakeBorder(I, padded, 0, m - I.rows, 0, n - I.cols, BORDER_CONSTANT, Scalar::all(0));
Mat planes[] = { Mat_<float>(padded), Mat::zeros(padded.size(), CV_32F) };
Mat complexI;
merge(planes, 2, complexI); // Add to the expanded another plane with zeros
dft(complexI, complexI); // this way the result may fit in the source matrix
// compute the magnitude and switch to logarithmic scale
// => log(1 + sqrt(Re(DFT(I))^2 + Im(DFT(I))^2))
split(complexI, planes); // planes[0] = Re(DFT(I), planes[1] = Im(DFT(I))
magnitude(planes[0], planes[1], planes[0]); // planes[0] = magnitude
Mat magI = planes[0];
magI += Scalar::all(1); // switch to logarithmic scale
log(magI, magI);
// crop the spectrum, if it has an odd number of rows or columns
magI = magI(Rect(0, 0, magI.cols & -2, magI.rows & -2));
// rearrange the quadrants of Fourier image so that the origin is at the image center
int cx = magI.cols / 2;
int cy = magI.rows / 2;
Mat q0(magI, Rect(0, 0, cx, cy)); // Top-Left - Create a ROI per quadrant
Mat q1(magI, Rect(cx, 0, cx, cy)); // Top-Right
Mat q2(magI, Rect(0, cy, cx, cy)); // Bottom-Left
Mat q3(magI, Rect(cx, cy, cx, cy)); // Bottom-Right
Mat tmp; // swap quadrants (Top-Left with Bottom-Right)
q0.copyTo(tmp);
q3.copyTo(q0);
tmp.copyTo(q3);
q1.copyTo(tmp); // swap quadrant (Top-Right with Bottom-Left)
q2.copyTo(q1);
tmp.copyTo(q2);
normalize(magI, magI, 0, 1, CV_MINMAX);
/*
int lowThreshold = 100;
int ratio = 3;
int kernel_size = 3;
Mat detected_edges;
Mat gray;
Mat sharpened;
magI.convertTo(gray, CV_8U);
cv::GaussianBlur(gray, sharpened, cv::Size(0, 0), 3);
cv::addWeighted(gray, 1.5, sharpened, -0.5, 0, sharpened);
sharpened.convertTo(magI, CV_32F);*/
return magI;
}