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; }