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