double Explorer<Correl>::exploreTranslation(image::Image const& im1, image::Image const& im2_, int xmin, int xmax, int xstep, int ymin, int ymax, int ystep, double &xres, double &yres, float const* weightMatrix)
{
cv::Rect roi = im1.getROI();
// image::Image im2(im2_, cv::Rect(0,0,im2_.width(),im2_.height()));
image::Image im2(im2_);
double score;
double best_score = -1.;
int bestx = -1, besty = -1;
if (xmin < 0) xmin = 0; if (xmax >= im2.width ()) xmax = im2.width ()-1;
if (ymin < 0) ymin = 0; if (ymax >= im2.height()) ymax = im2.height()-1;
int sa_w = (xmax-xmin+1), sa_h = (ymax-ymin+1); // search area
if (sa_w < 5) xstep = 1; if (sa_h < 5) ystep = 1;
int nresults = (sa_w+2)*(sa_h+2);
double *results = new double[nresults]; // add 1 border for interpolation
for(int i = 0; i < nresults; i++) results[i] = -1e6;
// explore
for(int y = ymin; y <= ymax; y += ystep)
for(int x = xmin; x <= xmax; x += xstep)
DO_CORRELATION(im1, im2, weightMatrix, x, y, score, best_score, bestx, besty, roi);
// refine
// JFR_DEBUG("refine (" << bestx << "," << besty << " " << best_score << ")");
// TODO refine several local maxima
// TODO refine by dichotomy for large steps ?
int newbestx = bestx, newbesty = besty;
for(int y = besty-ystep+1; y <= besty+ystep-1; y++)
for(int x = bestx-xstep+1; x <= bestx+xstep-1; x++)
{
if (x == bestx && y == besty) continue;
DO_CORRELATION(im1, im2, weightMatrix, x, y, score, best_score, newbestx, newbesty, roi);
}
// ensure that all values that will be used by interpolation are computed
int newnewbestx = newbestx, newnewbesty = newbesty;
/* if (((newbestx == bestx-xstep+1 || newbestx == bestx+xstep-1) && (newbesty-ymin)%ystep) ||
((newbesty == besty-ystep+1 || newbesty == besty+ystep-1) && (newbestx-xmin)%xstep))
{
if (newbestx == bestx-xstep+1) DO_CORRELATION(im1, im2, weightMatrix, newbestx-1, newbesty, score, best_score, newnewbestx, newnewbesty, roi);
if (newbestx == bestx+xstep-1) DO_CORRELATION(im1, im2, weightMatrix, newbestx+1, newbesty, score, best_score, newnewbestx, newnewbesty, roi);
if (newbesty == besty-ystep+1) DO_CORRELATION(im1, im2, weightMatrix, newbestx, newbesty-1, score, best_score, newnewbestx, newnewbesty, roi);
if (newbesty == besty+ystep-1) DO_CORRELATION(im1, im2, weightMatrix, newbestx, newbesty+1, score, best_score, newnewbestx, newnewbesty, roi);
}*/
// JFR_DEBUG("extra interpol (" << newbestx << "," << newbesty << " " << best_score << ")");
do {
newbestx = newnewbestx, newbesty = newnewbesty;
if (newbestx>0 && RESULTS(newbesty,newbestx-1)<-1e5)
DO_CORRELATION(im1, im2, weightMatrix, newbestx-1, newbesty, score, best_score, newnewbestx, newnewbesty, roi);
if (newbestx<im2.width()-1 && RESULTS(newbesty,newbestx+1)<-1e5)
DO_CORRELATION(im1, im2, weightMatrix, newbestx+1, newbesty, score, best_score, newnewbestx, newnewbesty, roi);
if (newbesty>0 && RESULTS(newbesty-1,newbestx)<-1e5)
DO_CORRELATION(im1, im2, weightMatrix, newbestx, newbesty-1, score, best_score, newnewbestx, newnewbesty, roi);
if (newbesty<im2.height()-1 && RESULTS(newbesty+1,newbestx)<-1e5)
DO_CORRELATION(im1, im2, weightMatrix, newbestx, newbesty+1, score, best_score, newnewbestx, newnewbesty, roi);
} while (newbestx != newnewbestx || newbesty != newnewbesty);
// FIXME this could go out of bounds
// JFR_DEBUG("final : " << newnewbestx << "," << newnewbesty << " " << best_score);
bestx = newbestx;
besty = newbesty;
// TODO interpolate the score as well
// interpolate x
double a1 = RESULTS(besty,bestx-1), a2 = RESULTS(besty,bestx-0), a3 = RESULTS(besty,bestx+1);
if (a1 > -1e5 && a3 > -1e5) jmath::parabolicInterpolation(a1,a2,a3, xres); else xres = 0;
// JFR_DEBUG("interpolating " << a1 << " " << a2 << " " << a3 << " gives shift " << xres << " plus " << bestx+0.5);
xres += bestx+0.5;
// interpolate y
a1 = RESULTS(besty-1,bestx), a2 = RESULTS(besty-0,bestx), a3 = RESULTS(besty+1,bestx);
if (a1 > -1e5 && a3 > -1e5) jmath::parabolicInterpolation(a1,a2,a3, yres); else yres = 0;
// JFR_DEBUG("interpolating " << a1 << " " << a2 << " " << a3 << " gives shift " << yres << " plus " << besty+0.5);
yres += besty+0.5;
delete[] results;
return best_score;
}
int main(int argc, char *argv[])
{
int ii;
unsigned char *map;
int value;
int index;
int result;
bool ok;
map = BM_ALLOC(HUGE);
if ( map == NULL )
{
fprintf(stderr, "CANNOT ALLOCATE MEMORY FOR BIT MAP OF %u BITS\n", HUGE);
goto Error;
}
else
{
printf("ALLOCATED AN ARRAY OF %u BITS\n", HUGE);
}
printf("SETTING ARRAY TO ALL '1'S\n");
BM_ALL(map, 1, HUGE);
result = BM_TEST(map, 12) ; printf(" bit %12u is %c/%c %s byte:%10u is 0x%02x\n", 12, RESULTS('T'), 0, map[0]);
printf("SETTING ARRAY TO ALL '0'S\n");
BM_ALL(map, 0, HUGE);
result = BM_TEST(map, 12) ; printf(" bit %12u is %c/%c %s byte:%10u is 0x%02x\n", 12, RESULTS('F'), 0, map[0]);
// Assign value to the bitmap map at position index:
value = 11111;
index = HUGE - 1; // the bitmap is indexed like a C array.. 0 to size-1
printf("ASSIGN %d AT %u\n", value, index);
BM_ASSIGN(map, value, index);
result = BM_TEST(map, index) ; printf(" bit %12u is %c/%c %s byte:%10u is 0x%02x\n", index, RESULTS('T'), index / 8, map[index / 8]);
value = 0;
printf("ASSIGN %d AT %u\n", value, index);
BM_ASSIGN(map, value, index);
result = BM_TEST(map, index) ; printf(" bit %12u is %c/%c %s byte:%10u is 0x%02x\n", index, RESULTS('F'), index / 8, map[index / 8]);
printf("EVERY EVEN BIT 0, EVERY ODD BIT 1...\n");
for ( ii = 0; ii < HUGE; ii += 2 )
{
BM_CLR(map, ii);
BM_SET(map, ii + 1);
}
result = BM_TEST(map, 3456) ; printf(" bit %12u is %c/%c %s byte:%10u is 0x%02x\n", 3456, RESULTS('F'), 3456 / 8, map[3456 / 8]);
result = BM_TEST(map, 4567) ; printf(" bit %12u is %c/%c %s byte:%10u is 0x%02x\n", 4567, RESULTS('T'), 4567 / 8, map[4567 / 8]);
result = BM_ANY(map, 1, HUGE) ; printf(" checking for any '1's: %c/%c %s\n", RESULTS('T'));
result = BM_ANY(map, 0, HUGE) ; printf(" checking for any '0's: %c/%c %s\n", RESULTS('T'));
ok = TRUE;
for ( ii = 0; ii < HUGE; ii += 2 )
{
if ( ii & 1 )
{
if ( ! BM_TEST(map, ii) ) { printf(" FAIL: scan test: bit %u should be 1 byte:%10u is 0x%02x\n", ii, ii / 8, map[ii / 8]); ok = FALSE; break; }
}
else
{
if ( BM_TEST(map, ii) ) { printf(" FAIL: scan test: bit %u should be 0 byte:%10u is 0x%02x\n", ii, ii / 8, map[ii / 8]); ok = FALSE; break; }
}
}
if ( ok ) printf(" scan test... OK\n");
printf("EVERY ODD BIT 0, EVERY EVEN BIT 1...\n");
for ( ii = 0; ii < HUGE; ii += 2 )
{
BM_SET(map, ii);
BM_CLR(map, ii + 1);
}
result = BM_TEST(map, 3456) ; printf(" bit %12u is %c/%c %s byte:%10u is 0x%02x\n", 3456, RESULTS('T'), 3456 / 8, map[3456 / 8]);
result = BM_TEST(map, 4567) ; printf(" bit %12u is %c/%c %s byte:%10u is 0x%02x\n", 4567, RESULTS('F'), 4567 / 8, map[4567 / 8]);
result = BM_ANY(map, 1, HUGE) ; printf(" checking for any '1's: %c/%c %s\n", RESULTS('T'));
result = BM_ANY(map, 0, HUGE) ; printf(" checking for any '0's: %c/%c %s\n", RESULTS('T'));
ok = TRUE;
for ( ii = 0; ii < HUGE; ii += 2 )
{
if ( ii & 1 )
{
if ( BM_TEST(map, ii) ) { printf(" FAIL: scan test: bit %u should be 0 byte:%10u is 0x%02x\n", ii, ii / 8, map[ii / 8]); ok = FALSE; break; }
}
else
{
if ( ! BM_TEST(map, ii) ) { printf(" FAIL: scan test: bit %u should be 1 byte:%10u is 0x%02x\n", ii, ii / 8, map[ii / 8]); ok = FALSE; break; }
}
}
if ( ok ) printf(" scan test... OK\n");
printf("SETTING ARRAY TO ALL '1'S\n");
BM_ALL(map, 1, HUGE);
result = BM_ANY(map, 1, HUGE) ; printf(" checking for any '1's: %c/%c %s\n", RESULTS('T'));
result = BM_ANY(map, 0, HUGE) ; printf(" checking for any '0's: %c/%c %s\n", RESULTS('F'));
printf("SETTING ARRAY TO ALL '0'S\n");
BM_ALL(map, 0, HUGE);
result = BM_ANY(map, 1, HUGE) ; printf(" checking for any '1's: %c/%c %s\n", RESULTS('F'));
result = BM_ANY(map, 0, HUGE) ; printf(" checking for any '0's: %c/%c %s\n", RESULTS('T'));
printf("SETTING BIT %u TO '1'\n", HUGE - 2);
BM_SET(map, HUGE - 2);
result = BM_ANY(map, 1, HUGE) ; printf(" checking for any '1's: %c/%c %s\n", RESULTS('T'));
result = BM_ANY(map, 0, HUGE) ; printf(" checking for any '0's: %c/%c %s\n", RESULTS('T'));
printf("SETTING ARRAY TO ALL '1'S\n");
BM_ALL(map, 1, HUGE);
printf("SETTING BIT %u TO '0'\n", HUGE - 2);
BM_CLR(map, HUGE - 2);
result = BM_ANY(map, 1, HUGE) ; printf(" checking for any '1's: %c/%c %s\n", RESULTS('T'));
result = BM_ANY(map, 0, HUGE) ; printf(" checking for any '0's: %c/%c %s\n", RESULTS('T'));
// Freeing space requested: We can just use free(), or for closing the circle:
BM_FREE(map);
return 0;
Error:
return 1;
}
