www.pudn.com > colortracker.rar > colorfeatsearch.cpp

#include "pp_preprocessor.h" 
#include "colorfeatsearch.h" 
#include  
#include  
 
template  
static inline double max(T x, U y){ return static_cast(x > y ? x : y);} 
template  
static inline T min(T x, U y){ return static_cast(x < y ? x : y);} 
 
#include  
 
/* ================================================================ */ 
 
void MPColorFeatSearch::initPyramids(int imageWidth, int imageHeight, double scaleUpPct, double minSizePct, int minScaleUp, double shiftPct, int minStride) 
{  
	/* create pyramids of posterior images per scale */ 
	m_priors1.set_imagedims(imageWidth, imageHeight); 
	m_priors2.set_imagedims(imageWidth, imageHeight); 
	m_priors1.compute_scales(scaleUpPct, minSizePct*imageWidth, minScaleUp); 
	m_priors1.initPyramid(shiftPct, minStride); 
	m_priors2.compute_scales(scaleUpPct, minSizePct*imageWidth, minScaleUp); 
	m_priors2.initPyramid(shiftPct, minStride); 
	 
	m_recycle = false;	 
} 
 
/* ================================================================ */ 
 
void MPColorFeatSearch::zeroPyramids() 
{  
	/* create pyramids of posterior images per scale */ 
	m_priors1.makezero(); 
	m_priors2.makezero(); 
} 
 
/* ================================================================ */ 
 
double MPColorFeatSearch::searchFeature(TIntegral &sumlikrat, double liknorm, TBox& besthyp, int scaledev, double hr, double shiftPct, double scaleUpPct, double minSizePct, int minScaleUp, int minStride) { 
	int imageWidth = sumlikrat.getImWidth(); 
	int imageHeight = sumlikrat.getImHeight(); 
 
	static const double minlog = -500; 
 
	/* swap the image pyramids after every frame instead of having to reallocate (reset) each time */ 
	if (m_recycle) { 
		m_priors_t = &m_priors1; 
		m_priors_tnew = &m_priors2; 
		m_recycle = false; 
	} else { 
		m_priors_t = &m_priors2; 
		m_priors_tnew = &m_priors1; 
		m_recycle = true; 
	}	 
	 
	static double priorsum = 1.0; 
	double postsum = 0; 
 
	int numscales = m_priors_t->get_numScales(); 
 
	UniformWindow window(scaledev, numscales); 
 
	m_priors_tnew->makezero(); 
	int numhyp = m_priors_t->get_numhyp(); 
	 
	double prevPostT = minlog; 
	static double maxLogPostT = minlog; 
 
	int scaleind; 
	 
	/* fix this, needs to be shiftPct*scale */ 
	double shift = (shiftPct > minStride) ? shiftPct : minStride; 
	 
	double unboost = 1.0/1.0; 
	static int frames = 0; 
	 
	double oneoverpriorsum = 1.0/priorsum; 
 
	/* this loop determines the max log posterior hypothesis  
		 in order to offset logpost values to be in range of exp function */ 
	for (scaleind=0; scaleind < numscales; scaleind++) { 
		HypothesesPerScale &scaleptr_t = m_priors_t->get_hypothesesPerScale(scaleind); 
		double &scale_t = scaleptr_t.m_scale; 
 
		for (int j=0; j < scaleptr_t.m_numY; j++) { 
			for (int i=0; i < scaleptr_t.m_numX; i++) { 
				double prior_t = 0; 
				if (frames == 0) prior_t = scaleptr_t.m_intimage.getImPixel(i, j); 
				else prior_t = scaleptr_t.m_intimage.getIntPixel(i, j); 
 
				prior_t *= oneoverpriorsum; 
				/* fix this, needs to be shiftPct*scale */ 
				double shift = (shiftPct > minStride) ? shiftPct : minStride; 
  
				int h_x = i * shift; 
				int h_y = j * shift; 
 
				/* get the likelihood ratio for hypothesis h at time t */ 
#ifdef	SPIKETEST 
#ifdef	ONESPIKE 
				double loglikrat = -50; 
 				if (frames == 0 && scaleind == numscales/2 
												&& i == scaleptr_t.m_numX/2 && j == scaleptr_t.m_numY/2) 
					loglikrat = 50; 
#else		ONESPIKE 
				double loglikrat = -50; 
				if (frames == 0) { 
					double alpha = -2.0; 
					if (scaleind == 4 && i == 6 && j == 6) { 
						int numh = scale_t*scale_t; 
						loglikrat = alpha * numh; 
					} else if (scaleind == 6 && i == 12 && j == 12) { 
						int numh = scale_t*scale_t; 
						loglikrat = alpha * numh;	 
					} 
				} 
#endif	ONESPIKE 
#else		SPIKETEST 
				//unboost = 1/(scale_t*scale_t) 
				double loglikrat = getBoxSum(sumlikrat, h_x, h_y, scale_t)*unboost;				 
#endif	SPIKETEST 
 
				double logprior_t = log(prior_t); 
				double logpost_t = loglikrat + logprior_t; 
				 
				/* max code */ 
				if (logpost_t >= maxLogPostT) maxLogPostT = logpost_t; 
			} 
		} 
	} 
 
	/* this loop determines the normalizing constant for the posterior */ 
	for (scaleind=0; scaleind < numscales; scaleind++) { 
		HypothesesPerScale &scaleptr_t = m_priors_t->get_hypothesesPerScale(scaleind); 
		double &scale_t = scaleptr_t.m_scale; 
 
		for (int j=0; j < scaleptr_t.m_numY; j++) { 
			for (int i=0; i < scaleptr_t.m_numX; i++) { 
				double prior_t = 0; 
				if (frames == 0) prior_t = scaleptr_t.m_intimage.getImPixel(i, j); 
				else prior_t = scaleptr_t.m_intimage.getIntPixel(i, j); 
 
				prior_t *= oneoverpriorsum; 
				/* fix this, needs to be shiftPct*scale */ 
				double shift = (shiftPct > minStride) ? shiftPct : minStride; 
  
				int h_x = i * shift; 
				int h_y = j * shift; 
 
				/* get the likelihood ratio for hypothesis h at time t */ 
#ifdef	SPIKETEST 
#ifdef	ONESPIKE 
				double loglikrat = -50; 
 				if (frames == 0 && scaleind == numscales/2 
												&& i == scaleptr_t.m_numX/2 && j == scaleptr_t.m_numY/2) 
					loglikrat = 50; 
#else		ONESPIKE 
				double loglikrat = -50; 
				if (frames == 0) { 
					double alpha = -2.0; 
					if (scaleind == 4 && i == 6 && j == 6) { 
						int numh = scale_t*scale_t; 
						loglikrat = alpha * numh; 
					} else if (scaleind == 6 && i == 12 && j == 12) { 
						int numh = scale_t*scale_t; 
						loglikrat = alpha * numh;	 
					} 
				} 
#endif	ONESPIKE 
#else		SPIKETEST 
				//unboost = 1/((scale_t+1)*(scale_t+1)); 
				double loglikrat = getBoxSum(sumlikrat, h_x, h_y, scale_t)*unboost;				 
#endif	SPIKETEST 
 
				double logprior_t = log(prior_t); 
				double logpost_t = loglikrat + logprior_t - maxLogPostT; 
				logpost_t = max(logpost_t, minlog); 
				postsum += exp(logpost_t); 
			} 
		} 
	} 
 
	/* calculates posterior probabilities and projects to predictive distribution for time t+1 */ 
	for (scaleind=0; scaleind < numscales; scaleind++) { 
		HypothesesPerScale &scaleptr_t = m_priors_t->get_hypothesesPerScale(scaleind); 
		HypothesesPerScale &scaleptr_tnew = m_priors_tnew->get_hypothesesPerScale(scaleind); 
		double &scale_t = scaleptr_t.m_scale; 
		 
		for (int j=0; j < scaleptr_t.m_numY; j++) { 
			for (int i=0; i < scaleptr_t.m_numX; i++) { 
				double prior_t = 0; 
			 
				if (frames == 0) prior_t = scaleptr_t.m_intimage.getImPixel(i, j); 
				else prior_t = scaleptr_t.m_intimage.getIntPixel(i, j); 
 
				prior_t *= oneoverpriorsum; 
 
				/* fix this, needs to be shiftPct*scale */ 
				double shift = (shiftPct > minStride) ? shiftPct : minStride; 
  
				int h_x = i * shift; 
				int h_y = j * shift; 
				 
 				window.set_scalebounds(scaleind); 
				 
				/* get the likelihood ratio for hypothesis h at time t */ 
#ifdef	SPIKETEST 
#ifdef	ONESPIKE 
				double loglikrat = -50; 
 				if (frames == 0 && scaleind == numscales/2 
												&& i == scaleptr_t.m_numX/2 && j == scaleptr_t.m_numY/2) 
					loglikrat = 50; 
#else		ONESPIKE 
				double loglikrat = -50; 
				if (frames == 0) { 
					double alpha = -2.0; 
					if (scaleind == 4 && i == 6 && j == 6) { 
						int numh = scale_t*scale_t; 
						loglikrat = alpha * numh; 
					} else if (scaleind == 6 && i == 12 && j == 12) { 
						int numh = scale_t*scale_t; 
						loglikrat = alpha * numh;	 
					} 
				} 
#endif	ONESPIKE 
#else		SPIKETEST 
				//unboost = 1/((scale_t+1)*(scale_t+1)); 
				double loglikrat = getBoxSum(sumlikrat, h_x, h_y, scale_t)*unboost;				 
#endif	SPIKETEST 
 
				double logprior_t = log(prior_t); 
				double logpost_t = (loglikrat + logprior_t) - maxLogPostT; 
 
				logpost_t = max(logpost_t, minlog); 
 
				logpost_t = min(logpost_t, -minlog); 
				double post_t = exp(logpost_t); 
 
				post_t /= postsum; 
 
#ifdef	SPIKETEST 
#ifndef	ONESPIKE 
				if (frames == 0) { 
					if (scaleind == 4 && i == 6 && j == 6) 
						printf("H1: llr = %g\tn = %g\tpost = %g\n", loglikrat, scale_t*scale_t, post_t); 
					else if (scaleind == 6 && i == 12 && j == 12) 
						printf("H2: llr = %g\tn = %g\tpost = %g\n", loglikrat, scale_t*scale_t, post_t); 
				} 
#endif	ONESPIKE 
#endif	SPIKETEST 
 
				/* max code */ 
				if (post_t >= prevPostT) { 
					besthyp.x = h_x;  
					besthyp.y = h_y;  
					besthyp.size = scale_t;  
					prevPostT = post_t; 
				} 
 
				int innerscaleind, boxsum; 
				TBox lw; 
 
				/* determine normalizing constant for p(h_t+1 | h_t) */ 
				int numh = 0; 
				for (innerscaleind=0; innerscaleind < m_priors_tnew->get_numScales(); innerscaleind++) { 
					HypothesesPerScale &scaleptr = m_priors_tnew->get_hypothesesPerScale(innerscaleind); 
					if (true) { 
// 					if (((int)scale_t % 2 != 0 && (int)scaleptr.m_scale % 2 != 0) || 
// 							((int)scale_t % 2 == 0 && (int)scaleptr.m_scale % 2 == 0) ) { 
// 					if ((int)scaleptr.m_scale % 2 != 0) { 
						if (window.is_validscale(innerscaleind)) {					 
							lw.scale = scaleptr.m_scale; 
							window.get_location_window(scale_t, lw, hr, h_x, h_y, shiftPct, minStride,scaleptr.m_numX,scaleptr.m_numY); 
							boxsum = getBoxSum(scaleptr.m_intones, lw.x, lw.y, lw.size); 
							numh += boxsum; 
						} 
					} 
				} 
 
				/* compute normalized prior for t+1 */ 
				double val = (post_t/static_cast(numh)); 
				for (innerscaleind=0; innerscaleind < m_priors_tnew->get_numScales(); innerscaleind++) { 
					HypothesesPerScale &scaleptr = m_priors_tnew->get_hypothesesPerScale(innerscaleind); 
					if (true) { 
// 					if (((int)scale_t % 2 != 0 && (int)scaleptr.m_scale % 2 != 0) || 
// 							((int)scale_t % 2 == 0 && (int)scaleptr.m_scale % 2 == 0) ) { 
// 					if ((int)scaleptr.m_scale % 2 != 0) { 
						if (window.is_validscale(innerscaleind)) { 
							lw.scale = scaleptr.m_scale; 
							window.get_location_window(scale_t, lw, hr, h_x, h_y, shiftPct, minStride, scaleptr.m_numX,scaleptr.m_numY);			 
							addBoxCorners(scaleptr.m_intimage, lw.x, lw.y, lw.size, val); 
						} 
					} 
				} 
			} 
		} 
	} 
 
	// #ifdef SIMULATE 
// 	bool headerflag_dd = false; 
// 	if (frames == 0) headerflag_dd = true; 
// 	ScalePyramid priorsnewcopy_dd; 
// 	m_priors_tnew->copy(priorsnewcopy_dd); 
// 	for (scaleind=0; scaleind < numscales; scaleind++) { 
// 		HypothesesPerScale &scaleptr_tnew = priorsnewcopy_dd.get_hypothesesPerScale(scaleind); 
// 		char ddfile[100];//, iifileraw[100]; 
// 		sprintf(ddfile, "temp/ddimage_scale%d.bin", scaleind); 
// // 		sprintf(iifileraw, "temp/iiimage_frame%d_scale%d.txt", frames, scaleind); 
// 		scaleptr_tnew.m_intimage.printIntBin(ddfile, headerflag_dd); 
// // 		scaleptr_tnew.m_intimage.printInt(iifileraw); 
// 	} 
// #endif 
 
	/* determine normalizing constant for prior (so prior sums to 1) */ 
	priorsum = 0; 
	for (scaleind=0; scaleind < numscales; scaleind++) { 
		HypothesesPerScale &scaleptr_tnew = m_priors_tnew->get_hypothesesPerScale(scaleind); 
 
		/* integrating the derivative image gives us unnormalized probabilities */ 
		scaleptr_tnew.m_intimage.integrate(); 
 
		double sum = 0; 
		for (int z=0;z < scaleptr_tnew.m_intimage.getIntNumPix(); z++) { 
			scaleptr_tnew.m_intimage.getArray()[z] = fabs(scaleptr_tnew.m_intimage.getArray()[z]); 
			sum += scaleptr_tnew.m_intimage.getArray()[z]; 
		} 
		priorsum += sum; 
	} 
 
#ifdef SIMULATE 
	bool headerflag = false; 
	if (frames == 0) headerflag = true; 
	ScalePyramid priorsnewcopy; 
	m_priors_tnew->copy(priorsnewcopy); 
	for (scaleind=0; scaleind < numscales; scaleind++) { 
		HypothesesPerScale &scaleptr_tnew = priorsnewcopy.get_hypothesesPerScale(scaleind); 
		char iifile[100];//, iifileraw[100]; 
		sprintf(iifile, "temp/iiimage_scale%d.bin", scaleind); 
// 		sprintf(iifileraw, "temp/iiimage_frame%d_scale%d.txt", frames, scaleind); 
		scaleptr_tnew.m_intimage.printIntBin(iifile, headerflag); 
// 		scaleptr_tnew.m_intimage.printInt(iifileraw); 
	} 
#endif 
 
	frames++; 
	return(0); 
} 
 
/* ================================================================ */ 
 
void MPColorFeatSearch::addBoxCorners(TIntegral &ii, const double i, const double j, const double size, const double val) {         
	int a = static_cast(i); 
	int b = static_cast(j); 
	 
	int numx = ii.getImWidth(); 
	int numy = ii.getImHeight(); 
	int a0 =  max(a,0); 
	int b0 =  max(b,0); 
	ASSERT(a0 < numx/*+1*/ && b0 < numy/*+1*/); 
	int a1 = min(a + size, numx/*+1*/);  
	int b1 = min(b + size, numy/*+1*/);  
	ASSERT(a1 > 0 && b1 > 0); 
	ii.setIntPixel(a0, b0, val + ii.getIntPixel(a0, b0)); 
	ii.setIntPixel(a1, b0, -val + ii.getIntPixel(a1, b0)); 
	ii.setIntPixel(a0, b1, -val + ii.getIntPixel(a0, b1)); 
	ii.setIntPixel(a1, b1, val + ii.getIntPixel(a1, b1)); 
 
// 	int numx = ii.getImWidth(); 
// 	int numy = ii.getImHeight(); 
// 	ASSERT(a0 < numx && b0 < numy); 
// 	int a1 = min(a0 + size, numx);  
// 	int b1 = min(b0 + size, numy);  
} 
 
/* ================================================================ */ 
 
double MPColorFeatSearch::getBoxSum(const TIntegral &ii, const double i, const double j, const double size) {         
	int a = static_cast(i); 
	int b = static_cast(j); 
	 
	const int numx = ii.getIntWidth(); 
	const int numy = ii.getIntHeight(); 
	const int a0 =  max(a,0); 
	const int b0 =  max(b,0); 
	ASSERT(a0 < numx && b0 < numy); 
	const int a1 = min(a + size, numx);  
	const int b1 = min(b + size, numy); 	 
	ASSERT(a1 > 0 && b1 > 0); 
	return( ii.getIntPixel(a0, b0) - ii.getIntPixel(a1, b0) - ii.getIntPixel(a0, b1) + ii.getIntPixel(a1, b1) ); 
} 
 
/* ================================================================ */