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;
}
double Zncc::computeTpl(image::Image const& im1_, image::Image const& im2_, float const* weightMatrix)
{
// preconds
JFR_PRECOND( im1_.depth() == depth, "Image 1 depth is different from the template parameter" );
JFR_PRECOND( im2_.depth() == depth, "Image 2 depth is different from the template parameter" );
JFR_PRECOND( im1_.channels() == im2_.channels(), "The channels number of both images are different" );
JFR_PRECOND( !useWeightMatrix || weightMatrix, "Template parameter tells to use weightMatrix but no one is given" );
// adjust ROIs to match size, assuming that it is reduced when set out of the image
// FIXME weightMatrix should be a cv::Mat in order to have a ROI too, and to adjust it
cv::Size size1; cv::Rect roi1 = im1_.getROI(size1);
cv::Size size2; cv::Rect roi2 = im2_.getROI(size2);
int dw = roi1.width - roi2.width, dh = roi1.height - roi2.height;
if (dw != 0)
{
cv::Rect &roiA = (dw<0 ? roi1 : roi2), &roiB = (dw<0 ? roi2 : roi1);
cv::Size &sizeA = (dw<0 ? size1 : size2);
if (roiA.x == 0) { roiB.x += dw; roiB.width -= dw; } else
if (roiA.x+roiA.width == sizeA.width) { roiB.width -= dw; }
}
if (dh != 0)
{
cv::Rect &roiA = (dh<0 ? roi1 : roi2), &roiB = (dh<0 ? roi2 : roi1);
cv::Size &sizeA = (dh<0 ? size1 : size2);
if (roiA.y == 0) { roiB.y += dh; roiB.height -= dh; } else
if (roiA.y+roiA.height == sizeA.height) { roiB.height -= dh; }
}
image::Image im1(im1_); im1.setROI(roi1);
image::Image im2(im2_); im2.setROI(roi2);
// some variables initialization
int height = im1.height();
int width = im1.width();
int step1 = im1.step1() - width;
int step2 = im2.step1() - width;
double mean1 = 0., mean2 = 0.;
double sigma1 = 0., sigma2 = 0., sigma12 = 0.;
double zncc_sum = 0.;
double zncc_count = 0.;
double zncc_total = 0.;
worktype const* im1ptr = reinterpret_cast<worktype const*>(im1.data());
worktype const* im2ptr = reinterpret_cast<worktype const*>(im2.data());
float const* wptr = weightMatrix;
double w;
// start the loops
for(int i = 0; i < height; ++i)
{
for(int j = 0; j < width; ++j)
{
worktype im1v = *(im1ptr++);
worktype im2v = *(im2ptr++);
if (useWeightMatrix) w = *(wptr++); else w = 1;
if (useBornes) zncc_total += w;
//std::cout << "will correl ? " << useBornes << ", " << (int)im1v << ", " << (int)im2v << std::endl;
if (!useBornes || (im1v != borneinf && im1v != bornesup && im2v != borneinf && im2v != bornesup))
{
//std::cout << "correl one pixel" << std::endl;
#if 0
double im1vw, im2vw;
if (useWeightMatrix)
{ im1vw = im1v * w; im2vw = im2v * w; } else
{ im1vw = im1v; im2vw = im2v; }
zncc_count += w;
mean1 += im1vw;
mean2 += im2vw;
sigma1 += im1v * im1vw;
sigma2 += im2v * im2vw;
zncc_sum += im1v * im2vw;
#else
zncc_count += w;
mean1 += im1v * w;
mean2 += im2v * w;
sigma1 += im1v * im1v * w;
sigma2 += im2v * im2v * w;
zncc_sum += im1v * im2v * w;
#endif
}
}
im1ptr += step1;
im2ptr += step2;
}
if (useBornes) if (zncc_count / zncc_total < 0.75)
{ /*std::cout << "zncc failed: " << zncc_count << "," << zncc_total << std::endl;*/ return -3; }
// finish
mean1 /= zncc_count;
mean2 /= zncc_count;
sigma1 = sigma1/zncc_count - mean1*mean1;
sigma2 = sigma2/zncc_count - mean2*mean2;
sigma1 = sigma1 > 0.0 ? sqrt(sigma1) : 0.0; // test for numerical rounding errors to avoid nan
sigma2 = sigma2 > 0.0 ? sqrt(sigma2) : 0.0;
sigma12 = sigma1*sigma2;
// std::cout << "normal: zncc_sum " << zncc_sum << ", count " << zncc_count << ", mean12 " << mean1*mean2 << ", sigma12 " << sigma1*sigma2 << std::endl;
zncc_sum = (sigma12 < 1e-6 ? -1 : (zncc_sum/zncc_count - mean1*mean2) / sigma12);
JFR_ASSERT(zncc_sum >= -1.01, "");
return zncc_sum;
}