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;
		}
Exemple #2
0
	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;
	}