// FindPeak:
// This version of findPeak just determines the maxium amplitude value and checks
// whether it is actually the peak for the neighborhood
bool findPeak(float *s, int size, float *peakX, float *peakY)
{
float max=-10000000.0;
int ii, kk, bestX=0, bestY=0;
float bestLocX, bestLocY;
// Search for the amplitude peak
for (ii=0; ii<size; ii++)
for (kk=0; kk<size; kk++)
if (s[ii*size+kk] > max) {
bestX = ii;
bestY = kk;
max = s[ii*size+kk];
}
topOffPeak(s,bestX,bestY,size,&bestLocX,&bestLocY);
// Output our guess.
//*peakX = bestLocX - size/2;
//*peakY = bestLocY - size/2;
*peakX = bestLocX;
*peakY = bestLocY;
return TRUE;
}
/*FindPeak: search correlation image for coherence peak.*/
static void findPeak(float *corrImage,float *dx,float *dy,float *doubt,
int nl, int ns, int chipX, int chipY,
int searchX, int searchY)
{
float biggestNearby=-100000000000.0;
int delX=15+chipX/8,delY=15+chipY/8;
int closeX=5+chipX/16,closeY=5+chipY/16;
/*Search for the peak with the highest correlation strength*/
int x,y,bestX=0,bestY=0;
float bestMatch=-100000000000.0;
float bestLocX=delX,bestLocY=delY;
//asfPrintStatus(" Searching for Peak (largest offset=%d lines "
// "& %d samples)\n",
// searchY,searchX);
for (y=chipY-searchY;y<chipY+searchY;y++) {
for (x=chipX-searchX;x<chipX+searchX;x++) {
int index=ns*modY(y,nl)+modX(x,ns);
if (bestMatch<corrImage[index]) {
bestMatch=corrImage[index];
bestX=x;bestY=y;
}
}
}
topOffPeak(corrImage,bestX,bestY,ns,&bestLocX,&bestLocY,nl,ns);
/*Compute the doubt in our offset guess, by
finding the largest of several pixels around our guess.*/
for (y=-delY;y<=delY;y++) {
for (x=-delX;x<=delX;x++) {
if ((abs(y)>closeY)||(abs(x)>closeX)) {
float cor=corrImage[modY(bestY+y,nl)*ns+modX(bestX+x,ns)];
if (biggestNearby<cor) {
biggestNearby=cor;
}
}
}
}
*doubt=biggestNearby/bestMatch;
if (*doubt<0) *doubt=0;
/*Output our guess:*/
bestLocX-=chipX;
bestLocY-=chipY;
*dx=bestLocX;
*dy=bestLocY;
}
/*FindPeak:
This function computes a correlation peak, with doubt level, between
the two amplitude images at the given points.
*/
bool findPeak(int x1, int y1, char *szImg1, int x2, int y2, char *szImg2,
float *peakX, float *peakY, float *doubt)
{
meta_parameters *meta;
static float *peaks, *s=NULL, *t, *product;
int x, y, srcIndex;
int mX,mY; /* Invariant: 2^mX=ns; 2^mY=nl. */
#define modX(x) ((x+srcSize)%srcSize) /* Return x, wrapped to [0..srcSize-1] */
#define modY(y) ((y+srcSize)%srcSize) /* Return y, wrapped to [0..srcSize-1] */
float aveChip=0;
float scaleFact=1.0/(srcSize*srcSize);
float sum=0, stdDev;
meta = meta_read(szImg1);
/* Allocate working arrays */
if (s == NULL) {
s = (float *)(MALLOC(srcSize*srcSize*sizeof(float)));
t = (float *)(MALLOC(srcSize*srcSize*sizeof(float)));
product = (float *)(MALLOC(srcSize*srcSize*sizeof(float)));
peaks=(float *)MALLOC(sizeof(float)*srcSize*srcSize);
}
/* At each grid point, read in a chunk of each image...*/
readSubset(szImg1, srcSize, srcSize, x1-srcSize/2+1, y1-srcSize/2+1, s);
readSubset(szImg2, srcSize, srcSize, x2-srcSize/2+1, y2-srcSize/2+1, t);
/* Compute average brightness of chip */
for(y=0;y<srcSize;y++)
{
srcIndex=y*srcSize;
for(x=0;x<srcSize;x++) {
sum += s[x+srcIndex];
}
}
aveChip = sum/(float)srcSize*srcSize;
stdDev = sqrt(sum/(srcSize*srcSize-1));
if (stdDev < 0.1) return FALSE;
/* Subtract average brightness from chip 2 */
for(y=0;y<srcSize;y++)
{
srcIndex=y*srcSize;
for(x=0;x<srcSize;x++)
t[x+srcIndex]=(t[x+srcIndex]-aveChip)*scaleFact;
}
/* Add average brightness to chip 1 */
for(y=0;y<srcSize;y++)
{
srcIndex=y*srcSize;
for(x=0;x<srcSize;x++)
s[x+srcIndex]=s[x+srcIndex]-aveChip;
}
/* Do the FFT and back */
mX = (int)(log(srcSize)/log(2.0)+0.5);
mY = (int)(log(srcSize)/log(2.0)+0.5);
fft2dInit(mY,mX); /* Initialization */
rfft2d(s,mY,mX); /* FFT chip 1 */
rfft2d(t,mY,mX); /* FFT chip 2 */
for(y=0;y<srcSize;y++) /* Conjugate chip 2 */ {
srcIndex=y*srcSize;
if (y<2) x=1; else x=0;
for (;x<srcSize/2;x++)
t[srcIndex+2*x+1]*=-1.0;
}
rspect2dprod(s,t,product,srcSize,srcSize); /* Complex product */
for (y=0;y<4;y++) /* Zero out the low frequencies of the correlation chip */
{
srcIndex=y*srcSize;
for (x=0;x<8;x++) product[srcIndex+x]=0;
srcIndex=srcSize*(srcSize-1-y);
for (x=0;x<8;x++) product[srcIndex+x]=0;
}
rifft2d(product,mY,mX); /* Inverse FFT */
/* Set up search chip size. */
chipDX = srcSize*3/4;
chipDY = srcSize*3/4;
chipX = srcSize/8;
chipY = srcSize/8;
searchX = srcSize*3/8;
searchY = srcSize*3/8;
/* Find peak */
float biggestNearby=-100000000000.0;
int delX=15+chipX/8,delY=15+chipY/8;
int closeX=5+chipX/16,closeY=5+chipY/16;
/* Search for the peak with the highest correlation strength */
int bestX,bestY;
float bestMatch=-100000000000.0;
float bestLocX=delX,bestLocY=delY;
for (y=chipY-searchY; y<chipY+searchY; y++)
for (x=chipX-searchX; x<chipX+searchX; x++)
{
int index = srcSize * modY(y) + modX(x);
if (bestMatch < product[index])
{
bestMatch = product[index];
bestX = x;
bestY = y;
}
}
topOffPeak(product,bestX,bestY,srcSize,&bestLocX,&bestLocY);
/* Compute the doubt in our offset guess, by
finding the largest of several pixels around our guess. */
for (y=-delY; y<=delY; y++)
for (x=-delX; x<=delX; x++)
if ((abs(y)>closeY) || (abs(x)>closeX))
{
float cor = product[modY(bestY+y)*srcSize+modX(bestX+x)];
if (biggestNearby < cor)
biggestNearby = cor;
}
*doubt=biggestNearby/bestMatch;
if (*doubt<0) *doubt = 0;
/* Clean up */
meta_free(meta);
/* Output our guess. */
*peakX = bestLocX;
*peakY = bestLocY;
return TRUE;
}
