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

/* 
 *  MPColorTracker.cpp 
 *   
 * 
 *  Created by Josh Susskind 2003. 
 *  Copyright (c) 2003 Machine Perception Laboratory 
 *  University of California San Diego. 
 *  Please read the disclaimer and notes about redistribution 
 *  at the end of this file. 
 * 
 *  Authors: Josh Susskind, Bret Fortenberry 
 */ 
#ifdef WIN32 
#pragma warning(disable:4786) 
#endif 
 
#include "Images/color.h" 
#include "MPColorTracker.h" 
#include  
#include  
 
#ifdef PREDICTIVE 
enum {MAXPRIOREFFECT = 0, MINPRIOREFFECT = 999999999}; 
#else 
enum {MAXPRIOREFFECT = 999999999, MINPRIOREFFECT = 0}; 
#endif 
 
#define SHOWPROBS 0 
#define SHOWMPI 1 
 
/* ================================================================ */ 
 
MPColorTracker::MPColorTracker() { 
	m_llikeMPI = -.5; 
	m_NumFrames = 0; 
	m_successTrackerSize = 30;  /* NEW VERSION */ 
	m_successTrackerIndex = 0; 
	m_successTracker = new double[m_successTrackerSize]; 
	for (int i = 0; i < m_successTrackerSize; i++) 
		m_successTracker[i] = 1.0; 
	m_successMeasure = 1.0; 
	//m_CommThread = new CommThread(); 
	m_MPI = new MPISearchThread(); 
	 
	//if (MPConfigure::intval("colorupdate")) 
		m_MPI->Start();// start the feature tracker thread 
 
// 	m_LogProbColorGivenFace.InitColorModel(1); 
// 	m_LogProbColorGivenBack.InitColorModel(0); 
//  
 
 	m_MPI->GetColorModel(&m_LogProbColorGivenFace, &m_LogProbColorGivenBack, &m_resetTracker, &m_llikeMPI); 
// 	m_resetTracker = 1; 
	 
// 	m_DownSample = 2; 
// 	m_smooth_param = 0.5; 
 
	m_mBox.x = 0; 
	m_mBox.y = 0; 
	m_mBox.size = 0; 
	 
#ifdef PREDICTIVE 
// 	m_hr = m_scaledev = MINPRIOREFFECT;  
// 	m_minScaleUp = 1; 
// 	m_minStride = 1; 
// 	m_minSizePct = 0; 
// 	m_stridePct = 0; 
// 	m_scaleUpPct = 0; 
// #else 
// 	m_vBox.x = MINPRIOREFFECT; 
// 	m_vBox.y = MINPRIOREFFECT; 
// 	m_vBox.size = MINPRIOREFFECT;  
#else 
 
  m_searchHeight = m_searchWidth = 2; 
  m_colorSearch.CreateData(320, 240, m_searchHeight, m_searchWidth); //if image size is incorrect it is okay 
#endif 
} 
 
/* ================================================================ */ 
 
MPColorTracker::~MPColorTracker() { 
	 
	delete m_MPI; 
	//delete m_CommThread; 
} 
 
/* ================================================================ */   
 
int MPColorTracker::InitStreaming() { 
	return 1; 
} 
 
/* ================================================================ */ 
 
int MPColorTracker::EndStreaming() { 
	return 1; 
} 
 
/* ================================================================ */ 
 
int MPColorTracker::DoSearch() { 
	/*RGBTRIPLE * pData = (RGBTRIPLE *)m_ImageWrapper.rgbimage; 
	int imgWidth = m_ImageWrapper.width; 
	int imgHeight = m_ImageWrapper.height; 
	*/ 
	RGBTRIPLE * pData = m_colorImage.array; 
	int imgWidth = m_colorImage.width; 
	int imgHeight = m_colorImage.height; 
	 
	/* whether or not a box has been drawn by checking to see if the second coordinate is a positive number */ 
	if (m_results.x2 == -1) { 
		/* assigning the width and height of the image as the coordinates for the bottom right of the box */ 
		m_results.x2 = imgWidth; 
		m_results.y2 = imgHeight; 
		m_success = 0; 
	} 
	 
	int &downsample = m_csearchparams.decimation_factor; 
 
	if (!m_Collection->empty()) { 
		long framenum = -1; 
		int x = 0, y = 0, w = 0, h = 0; 
		for (int i = 0; i < m_Collection->size(); i++) { 
			m_Collection->get_keyframe(framenum, x, y, w, h, i); 
			if (m_NumFrames == framenum) { 
				m_resetTracker=1; 
				break; 
			} 
		} 
	} 
 
#ifndef PREDICTIVE 
	TSquare &vbox = m_csearchparams.vbox; 
#endif 
	double gridScanX = imgWidth / downsample; 
	double gridScanY = imgHeight / downsample; 
 
////#ifdef PREDICTIVE 
////	if (m_likRatImageInt.getImWidth() != gridScanX || m_likRatImageInt.getImHeight() != gridScanY) 
////		m_likRatImageInt.setSize(gridScanX, gridScanY); // because image size may change frame to frame, otherwise put in constructor 
////#else 
 
	if (m_likRatImage.width != gridScanX || m_likRatImage.height != gridScanY) 
		m_likRatImage.setSize(static_cast(gridScanX),static_cast(gridScanY)); // because image size may change frame to frame, otherwise put in constructor 
////#endif 
 
	/* Sample from grid to build log probability image */ 
	RGBTRIPLE *pPixel = pData;	/* For grid sampling, holds a pointer to the current pixel */ 
	 
	double *likptr; 
////#ifdef PREDICTIVE 
////	likptr = m_likRatImageInt.getArray(); 
////#else 
	likptr = m_likRatImage.array; 
////#endif 
#ifdef WIN32 
	for (int y = imgHeight-1; y >= 0; y-=downsample) { 
#else 
	for (int y = 0; y < imgHeight; y+=downsample) { 
#endif 
		pPixel = pData + y * imgWidth; 
		for (int x = 0; x < gridScanX; x++) { 
			*likptr++ = m_LogProbColorGivenFace.get_prob(*pPixel) - m_LogProbColorGivenBack.get_prob(*pPixel); 
			pPixel += downsample; 
		} 
	} 
 
 
#ifdef PREDICTIVE 
	int &minScaleUp = m_csearchparams.m_min_scale_up; /* in pixels */ 
	int &minStride = m_csearchparams.m_min_stride; /* in pixels */ 
	float &minSizePct = m_csearchparams.m_min_scale_pct; 
	float &stridePct = m_csearchparams.m_stride_pct;  
	float &scaleUpPct = m_csearchparams.m_scale_up_pct; 
 	int minSize = static_cast(minSizePct*(double)imgWidth); 
 	if (minSize == 0) minSize = 1; 
#endif 
	 
	 
	/* if the color tracker size is 0 (for initial frame)  
		 set color tracker box to middle of screen and 1/4 size of window */ 
	if (!m_mBox.size) { 
		m_mBox.x = (int)(imgWidth/2) - (int)(imgWidth/8); 
		m_mBox.y = (int)(imgHeight/2) - (int)(imgWidth/8); 
		m_mBox.size = (int)(imgWidth/4); 
	} 
	 
#ifdef PREDICTIVE 
	if (!firsttime++) 
		m_colorFeatSearch.initPyramids(imgWidth, imgHeight, scaleUpPct, minSizePct, minScaleUp, stridePct, minStride); 
#endif 
 
	double likrat = 0; 
	 
	/* Search for maximum likelihood image */ 
 
#ifdef PREDICTIVE 
 
	RIntegral likRatImageInt(m_likRatImage); 
 
	TBoxtbox; 
	tbox.x = m_mBox.x; 
	tbox.y = m_mBox.y; 
	tbox.size = m_mBox.size; 
	 
	float &hr = m_csearchparams.m_location_window; 
	float &scaledev = m_csearchparams.m_scale_window; 
	likrat = m_colorFeatSearch.searchFeature(likRatImageInt, NULL, tbox, scaledev, hr, stridePct, scaleUpPct, minSize, minScaleUp, minStride); 
 
	// set priors (precisions) smaller # = stronger prior, 0 size = infinite prior 
	scaledev = 2; 
	hr = 1.2f; 
 
	m_mBox.x = tbox.x; 
	m_mBox.y = tbox.y; 
	m_mBox.size = tbox.size; 
 
#else 
  int LaplacianPrior =1; // If zero we get Gaussian prior 
  double llikeThresh = -100000; // effectively no threshold 
  const ProbColorSearch::SearchType searchType = ProbColorSearch::Max; 
	 
	likrat = m_colorSearch.probSearch(searchType, m_likRatImage, m_searchHeight, m_mBox, vbox, llikeThresh, LaplacianPrior); 
 
	// set priors smaller # = weaker prior, 0 size = no prior 
  vbox.x = 2.5; vbox.y = 2.5; vbox.size = 2.5; 
#endif 
 
	/* set per pixel liklihoods */ 
	likrat /= (m_mBox.size * m_mBox.size);// * featureWidth/featureHeight); /* compute likelihood per pixel */ 
 
	// We do an exponential smoothing on the solution found by the color search. 
	static double SmoothBox_x = m_mBox.x, SmoothBox_y = m_mBox.y, SmoothBox_size = m_mBox.size; 
	float &smoothingeffect = m_csearchparams.smooth_result; 
	SmoothBox_x = (1-smoothingeffect)*m_mBox.x + smoothingeffect*SmoothBox_x; 
	SmoothBox_y = (1-smoothingeffect)*m_mBox.y + smoothingeffect*SmoothBox_y; 
	SmoothBox_size = (1-smoothingeffect)*m_mBox.size + smoothingeffect*SmoothBox_size; 
	/* *********** deal with box alignment problems !!! ************** */ 
	int bBoxHeight = (int)((SmoothBox_size) * downsample); 
	//	int bBoxHeight = (int)((SmoothBox_size-1) * downsample+1); 
	int bBoxLength = (int) ((double) bBoxHeight);// * featureWidth/featureHeight); 
	m_results.x1 = (int)((SmoothBox_x) * downsample);// + margin) * downsample); 
	m_results.x2 = m_results.x1 + (bBoxLength); 
	//	m_results.x2 = m_results.x1 + (bBoxLength-1); 
	m_results.y1 = (int)(SmoothBox_y * downsample); 
	m_results.y2 = m_results.y1 + (bBoxHeight); 
	//	m_results.y2 = m_results.y1 + (bBoxHeight-1); 
	/* *********** deal with this subtraction!!! ************** */ 
	 
	/* decide success of tracker based on likelihood ratio */ 
	static double likThreshold = m_llikeMPI; 
	static int framesMissed = 0; 
	framesMissed++; 
	static const double a = 0.5; 
	likThreshold = a * likThreshold + (1-a) * m_llikeMPI; 
	m_activation = likrat - 0.5*likThreshold; 
	if (likrat >  0.7*likThreshold ) 
		framesMissed = 0; 
	m_success = (framesMissed < 30); 
	 
	/* **************************************** 
		 logic to detect when there is no face 
		 ignored so that we always report a face */ 
	m_success = 1; 
	/* **************************************** */ 
	 
	if (! m_success) m_resetTracker = 1; 
	 
	if (m_resetTracker)   { 
#ifndef PREDICTIVE 
		TSquare &vbox = m_csearchparams.vbox; 
		vbox.x=MINPRIOREFFECT; 
		vbox.y=MINPRIOREFFECT; 
		vbox.size=MINPRIOREFFECT; 
#endif 
		m_resetTracker = 0; 
	} 
	 
	return m_success; 
} 
 
/* ================================================================ */ 
 
void MPColorTracker::ShowProbs() { 
	RGBTRIPLE *temp = m_colorImage.array; 
	int imgWidth = m_colorImage.width; 
	int imgHeight = m_colorImage.height; 
	const double div_by = 1/(255-127); 
	 
	for (int y = 0 ; y < imgHeight; y++) { 
		for (int x = 0 ; x < imgWidth; x++,temp++) { 
			double val = 0.; 
			val = (log(m_LogProbColorGivenFace.get_prob(*temp)) - log(m_LogProbColorGivenBack.get_prob(*temp)))*div_by;;	 
			if (val > 255) val = 255; 
			if (val < 0 ) val = 0; 
			if ( val > 127){ 
				temp->rgbtRed = static_cast(0.6*temp->rgbtRed+0.4*val); 
				temp->rgbtBlue = static_cast(0.6*temp->rgbtBlue); 
				temp->rgbtGreen = static_cast(0.6*temp->rgbtGreen); 
			} 
			if (val< 127){ 
				temp->rgbtBlue = static_cast(0.6*val+0.4*temp->rgbtBlue); 
				temp->rgbtGreen = static_cast(0.6*temp->rgbtGreen); 
				temp->rgbtRed = static_cast(0.6*temp->rgbtRed); 
			} 
		} 
	} 
} 
 
/* ================================================================ */ 
 
void MPColorTracker::ShowProbs(const bool &type) { 
	RGBTRIPLE *temp = m_colorImage.array; 
	int imgWidth = m_colorImage.width; 
	int imgHeight = m_colorImage.height; 
	 
	for (int y = 0 ; y < imgHeight; y++) { 
		for (int x = 0 ; x < imgWidth; x++,temp++) { 
			double val = 0.; 
			 
			double scalingfactor = 25; 
			if (type) 
				val = exp(m_LogProbColorGivenFace.get_prob(*temp)) * 255 * scalingfactor; 
			else 
				val = exp(m_LogProbColorGivenBack.get_prob(*temp)) * 255 * scalingfactor; 
			 
			if (val > 255) val = 255; 
			 
			if (type) 
				temp->rgbtRed = static_cast(val); 
			else 
				temp->rgbtBlue = static_cast(val); 
		} 
	} 
} 
 
/* ================================================================ */ 
 
void MPColorTracker::MPIUpdate() { 
	int imgWidth = m_colorImage.width; 
	int imgHeight = m_colorImage.height; 
	int numPixels = imgWidth*imgHeight; 
	m_MPI->GetColorModel(&m_LogProbColorGivenFace, &m_LogProbColorGivenBack, &m_resetTracker, &m_llikeMPI); 
	m_MPI->PutData(m_colorImage.array, imgWidth, imgHeight, numPixels, m_results.getMeanX(), m_results.getMeanY(), m_curFrame); 
} 
 
/* ================================================================ */ 
 
void MPColorTracker::WriteBitmap(char *directory) { 
#ifdef WIN32 
	char filename[256]; 
	sprintf(filename, "%s\\face_%d.bmp", directory, m_NumFrames); 
	MPImageIO imageIO; 
 	imageIO.WriteBitmap((unsigned char*)m_colorImage.array, m_colorImage.width*m_colorImage.height, m_colorImage.width, m_colorImage.height, filename); 
#endif 
} 
 
/* ================================================================ */ 
 
void MPColorTracker::PutImage(void *img, int &w, int &h, long & frameCount) { 
	RImage setImage((RGBTRIPLE *)img, w, h); 
	m_colorImage = setImage; 
	m_curFrame = frameCount; 
} 
 
/* ================================================================ */ 
 
void MPColorTracker::SetKeyFrameCollection(MPKeyFrameCollection *collection) { 
	m_Collection = collection; 
} 
 
 
 
/* 
 *  
 * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 
 *  
 *    1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 
 *    2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 
 *    3. The name of the author may not be used to endorse or promote products derived from this software without specific prior written permission. 
 *  
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 
 *  
 */