The Machine Perception Toolbox (MPT)

00001 /*
00002  *  MPColorTracker.cpp
00003  *  
00004  *
00005  *  Created by Josh Susskind 2003.
00006  *  Copyright (c) 2003 Machine Perception Laboratory
00007  *  University of California San Diego.
00008  *  Please read the disclaimer and notes about redistribution
00009  *  at the end of this file.
00010  *
00011  *  Authors: Josh Susskind, Bret Fortenberry
00012  */
00013 #ifdef WIN32
00014 #pragma warning(disable:4786)
00015 #endif
00016 
00017 #include "Images/color.h"
00018 #include "MPColorTracker.h"
00019 #include <cmath>
00020 #include <stdio.h>
00021 
00022 #ifdef PREDICTIVE
00023 enum {MAXPRIOREFFECT = 0, MINPRIOREFFECT = 999999999};
00024 #else
00025 enum {MAXPRIOREFFECT = 999999999, MINPRIOREFFECT = 0};
00026 #endif
00027 
00028 #define SHOWPROBS 0
00029 #define SHOWMPI 1
00030 
00031 /* ================================================================ */
00032 
00033 MPColorTracker::MPColorTracker() {
00034         m_llikeMPI = -.5;
00035         m_NumFrames = 0;
00036         m_successTrackerSize = 30;  /* NEW VERSION */
00037         m_successTrackerIndex = 0;
00038         m_successTracker = new double[m_successTrackerSize];
00039         for (int i = 0; i < m_successTrackerSize; i++)
00040                 m_successTracker[i] = 1.0;
00041         m_successMeasure = 1.0;
00042         //m_CommThread = new CommThread();
00043         m_MPI = new MPISearchThread();
00044         
00045         //if (MPConfigure::intval("colorupdate"))
00046                 m_MPI->Start();// start the feature tracker thread
00047 
00048 //      m_LogProbColorGivenFace.InitColorModel(1);
00049 //      m_LogProbColorGivenBack.InitColorModel(0);
00050 // 
00051 
00052         m_MPI->GetColorModel(&m_LogProbColorGivenFace, &m_LogProbColorGivenBack, &m_resetTracker, &m_llikeMPI);
00053 //      m_resetTracker = 1;
00054         
00055 //      m_DownSample = 2;
00056 //      m_smooth_param = 0.5;
00057 
00058         m_mBox.x = 0;
00059         m_mBox.y = 0;
00060         m_mBox.size = 0;
00061         
00062 #ifdef PREDICTIVE
00063 //      m_hr = m_scaledev = MINPRIOREFFECT; 
00064 //      m_minScaleUp = 1;
00065 //      m_minStride = 1;
00066 //      m_minSizePct = 0;
00067 //      m_stridePct = 0;
00068 //      m_scaleUpPct = 0;
00069 // #else
00070 //      m_vBox.x = MINPRIOREFFECT;
00071 //      m_vBox.y = MINPRIOREFFECT;
00072 //      m_vBox.size = MINPRIOREFFECT; 
00073 #else
00074 
00075   m_searchHeight = m_searchWidth = 2;
00076   m_colorSearch.CreateData(320, 240, m_searchHeight, m_searchWidth); //if image size is incorrect it is okay
00077 #endif
00078 }
00079 
00080 /* ================================================================ */
00081 
00082 MPColorTracker::~MPColorTracker() {
00083         
00084         delete m_MPI;
00085         //delete m_CommThread;
00086 }
00087 
00088 /* ================================================================ */  
00089 
00090 int MPColorTracker::InitStreaming() {
00091         return 1;
00092 }
00093 
00094 /* ================================================================ */
00095 
00096 int MPColorTracker::EndStreaming() {
00097         return 1;
00098 }
00099 
00100 /* ================================================================ */
00101 
00102 int MPColorTracker::DoSearch() {
00103         /*RGBTRIPLE * pData = (RGBTRIPLE *)m_ImageWrapper.rgbimage;
00104         int imgWidth = m_ImageWrapper.width;
00105         int imgHeight = m_ImageWrapper.height;
00106         */
00107         RGBTRIPLE * pData = m_colorImage.array;
00108         int imgWidth = m_colorImage.width;
00109         int imgHeight = m_colorImage.height;
00110         
00111         /* whether or not a box has been drawn by checking to see if the second coordinate is a positive number */
00112         if (m_results.x2 == -1) {
00113                 /* assigning the width and height of the image as the coordinates for the bottom right of the box */
00114                 m_results.x2 = imgWidth;
00115                 m_results.y2 = imgHeight;
00116                 m_success = 0;
00117         }
00118         
00119         int &downsample = m_csearchparams.decimation_factor;
00120 
00121         if (!m_Collection->empty()) {
00122                 long framenum = -1;
00123                 int x = 0, y = 0, w = 0, h = 0;
00124                 for (int i = 0; i < m_Collection->size(); i++) {
00125                         m_Collection->get_keyframe(framenum, x, y, w, h, i);
00126                         if (m_NumFrames == framenum) {
00127                                 m_resetTracker=1;
00128                                 break;
00129                         }
00130                 }
00131         }
00132 
00133 #ifndef PREDICTIVE
00134         TSquare<double> &vbox = m_csearchparams.vbox;
00135 #endif
00136         double gridScanX = imgWidth / downsample;
00137         double gridScanY = imgHeight / downsample;
00138 
00143 
00144         if (m_likRatImage.width != gridScanX || m_likRatImage.height != gridScanY)
00145                 m_likRatImage.setSize(static_cast<int>(gridScanX),static_cast<int>(gridScanY)); // because image size may change frame to frame, otherwise put in constructor
00147 
00148         /* Sample from grid to build log probability image */
00149         RGBTRIPLE *pPixel = pData;      /* For grid sampling, holds a pointer to the current pixel */
00150         
00151         double *likptr;
00155         likptr = m_likRatImage.array;
00157 #ifdef WIN32
00158         for (int y = imgHeight-1; y >= 0; y-=downsample) {
00159 #else
00160         for (int y = 0; y < imgHeight; y+=downsample) {
00161 #endif
00162                 pPixel = pData + y * imgWidth;
00163                 for (int x = 0; x < gridScanX; x++) {
00164                         *likptr++ = m_LogProbColorGivenFace.get_prob(*pPixel) - m_LogProbColorGivenBack.get_prob(*pPixel);
00165                         pPixel += downsample;
00166                 }
00167         }
00168 
00169 
00170 #ifdef PREDICTIVE
00171         int &minScaleUp = m_csearchparams.m_min_scale_up; /* in pixels */
00172         int &minStride = m_csearchparams.m_min_stride; /* in pixels */
00173         float &minSizePct = m_csearchparams.m_min_scale_pct;
00174         float &stridePct = m_csearchparams.m_stride_pct; 
00175         float &scaleUpPct = m_csearchparams.m_scale_up_pct;
00176         int minSize = static_cast<int>(minSizePct*(double)imgWidth);
00177         if (minSize == 0) minSize = 1;
00178 #endif
00179         
00180         
00181         /* if the color tracker size is 0 (for initial frame) 
00182                  set color tracker box to middle of screen and 1/4 size of window */
00183         if (!m_mBox.size) {
00184                 m_mBox.x = (int)(imgWidth/2) - (int)(imgWidth/8);
00185                 m_mBox.y = (int)(imgHeight/2) - (int)(imgWidth/8);
00186                 m_mBox.size = (int)(imgWidth/4);
00187         }
00188         
00189 #ifdef PREDICTIVE
00190         if (!firsttime++)
00191                 m_colorFeatSearch.initPyramids(imgWidth, imgHeight, scaleUpPct, minSizePct, minScaleUp, stridePct, minStride);
00192 #endif
00193 
00194         double likrat = 0;
00195         
00196         /* Search for maximum likelihood image */
00197 
00198 #ifdef PREDICTIVE
00199 
00200         RIntegral<double> likRatImageInt(m_likRatImage);
00201 
00202         TBox<double>tbox;
00203         tbox.x = m_mBox.x;
00204         tbox.y = m_mBox.y;
00205         tbox.size = m_mBox.size;
00206         
00207         float &hr = m_csearchparams.m_location_window;
00208         float &scaledev = m_csearchparams.m_scale_window;
00209         likrat = m_colorFeatSearch.searchFeature(likRatImageInt, NULL, tbox, scaledev, hr, stridePct, scaleUpPct, minSize, minScaleUp, minStride);
00210 
00211         // set priors (precisions) smaller # = stronger prior, 0 size = infinite prior
00212         scaledev = 2;
00213         hr = 1.2f;
00214 
00215         m_mBox.x = tbox.x;
00216         m_mBox.y = tbox.y;
00217         m_mBox.size = tbox.size;
00218 
00219 #else
00220   int LaplacianPrior =1; // If zero we get Gaussian prior
00221   double llikeThresh = -100000; // effectively no threshold
00222   const ProbColorSearch<double>::SearchType searchType = ProbColorSearch<double>::Max;
00223         
00224         likrat = m_colorSearch.probSearch(searchType, m_likRatImage, m_searchHeight, m_mBox, vbox, llikeThresh, LaplacianPrior);
00225 
00226         // set priors smaller # = weaker prior, 0 size = no prior
00227   vbox.x = 2.5; vbox.y = 2.5; vbox.size = 2.5;
00228 #endif
00229 
00230         /* set per pixel liklihoods */
00231         likrat /= (m_mBox.size * m_mBox.size);// * featureWidth/featureHeight); /* compute likelihood per pixel */
00232 
00233         // We do an exponential smoothing on the solution found by the color search.
00234         static double SmoothBox_x = m_mBox.x, SmoothBox_y = m_mBox.y, SmoothBox_size = m_mBox.size;
00235         float &smoothingeffect = m_csearchparams.smooth_result;
00236         SmoothBox_x = (1-smoothingeffect)*m_mBox.x + smoothingeffect*SmoothBox_x;
00237         SmoothBox_y = (1-smoothingeffect)*m_mBox.y + smoothingeffect*SmoothBox_y;
00238         SmoothBox_size = (1-smoothingeffect)*m_mBox.size + smoothingeffect*SmoothBox_size;
00239         /* *********** deal with box alignment problems !!! ************** */
00240         int bBoxHeight = (int)((SmoothBox_size) * downsample);
00241         //      int bBoxHeight = (int)((SmoothBox_size-1) * downsample+1);
00242         int bBoxLength = (int) ((double) bBoxHeight);// * featureWidth/featureHeight);
00243         m_results.x1 = (int)((SmoothBox_x) * downsample);// + margin) * downsample);
00244         m_results.x2 = m_results.x1 + (bBoxLength);
00245         //      m_results.x2 = m_results.x1 + (bBoxLength-1);
00246         m_results.y1 = (int)(SmoothBox_y * downsample);
00247         m_results.y2 = m_results.y1 + (bBoxHeight);
00248         //      m_results.y2 = m_results.y1 + (bBoxHeight-1);
00249         /* *********** deal with this subtraction!!! ************** */
00250         
00251         /* decide success of tracker based on likelihood ratio */
00252         static double likThreshold = m_llikeMPI;
00253         static int framesMissed = 0;
00254         framesMissed++;
00255         static const double a = 0.5;
00256         likThreshold = a * likThreshold + (1-a) * m_llikeMPI;
00257         m_activation = likrat - 0.5*likThreshold;
00258         if (likrat >  0.7*likThreshold )
00259                 framesMissed = 0;
00260         m_success = (framesMissed < 30);
00261         
00262         /* ****************************************
00263                  logic to detect when there is no face
00264                  ignored so that we always report a face */
00265         m_success = 1;
00266         /* **************************************** */
00267         
00268         if (! m_success) m_resetTracker = 1;
00269         
00270         if (m_resetTracker)   {
00271 #ifndef PREDICTIVE
00272                 TSquare<double> &vbox = m_csearchparams.vbox;
00273                 vbox.x=MINPRIOREFFECT;
00274                 vbox.y=MINPRIOREFFECT;
00275                 vbox.size=MINPRIOREFFECT;
00276 #endif
00277                 m_resetTracker = 0;
00278         }
00279         
00280         return m_success;
00281 }
00282 
00283 /* ================================================================ */
00284 
00285 void MPColorTracker::ShowProbs() {
00286         RGBTRIPLE *temp = m_colorImage.array;
00287         int imgWidth = m_colorImage.width;
00288         int imgHeight = m_colorImage.height;
00289         const double div_by = 1/(255-127);
00290         
00291         for (int y = 0 ; y < imgHeight; y++) {
00292                 for (int x = 0 ; x < imgWidth; x++,temp++) {
00293                         double val = 0.;
00294                         val = (log(m_LogProbColorGivenFace.get_prob(*temp)) - log(m_LogProbColorGivenBack.get_prob(*temp)))*div_by;;    
00295                         if (val > 255) val = 255;
00296                         if (val < 0 ) val = 0;
00297                         if ( val > 127){
00298                                 temp->rgbtRed = static_cast<unsigned char>(0.6*temp->rgbtRed+0.4*val);
00299                                 temp->rgbtBlue = static_cast<unsigned char>(0.6*temp->rgbtBlue);
00300                                 temp->rgbtGreen = static_cast<unsigned char>(0.6*temp->rgbtGreen);
00301                         }
00302                         if (val< 127){
00303                                 temp->rgbtBlue = static_cast<unsigned char>(0.6*val+0.4*temp->rgbtBlue);
00304                                 temp->rgbtGreen = static_cast<unsigned char>(0.6*temp->rgbtGreen);
00305                                 temp->rgbtRed = static_cast<unsigned char>(0.6*temp->rgbtRed);
00306                         }
00307                 }
00308         }
00309 }
00310 
00311 /* ================================================================ */
00312 
00313 void MPColorTracker::ShowProbs(const bool &type) {
00314         RGBTRIPLE *temp = m_colorImage.array;
00315         int imgWidth = m_colorImage.width;
00316         int imgHeight = m_colorImage.height;
00317         
00318         for (int y = 0 ; y < imgHeight; y++) {
00319                 for (int x = 0 ; x < imgWidth; x++,temp++) {
00320                         double val = 0.;
00321                         
00322                         double scalingfactor = 25;
00323                         if (type)
00324                                 val = exp(m_LogProbColorGivenFace.get_prob(*temp)) * 255 * scalingfactor;
00325                         else
00326                                 val = exp(m_LogProbColorGivenBack.get_prob(*temp)) * 255 * scalingfactor;
00327                         
00328                         if (val > 255) val = 255;
00329                         
00330                         if (type)
00331                                 temp->rgbtRed = static_cast<unsigned char>(val);
00332                         else
00333                                 temp->rgbtBlue = static_cast<unsigned char>(val);
00334                 }
00335         }
00336 }
00337 
00338 /* ================================================================ */
00339 
00340 void MPColorTracker::MPIUpdate() {
00341         int imgWidth = m_colorImage.width;
00342         int imgHeight = m_colorImage.height;
00343         int numPixels = imgWidth*imgHeight;
00344         m_MPI->GetColorModel(&m_LogProbColorGivenFace, &m_LogProbColorGivenBack, &m_resetTracker, &m_llikeMPI);
00345         m_MPI->PutData(m_colorImage.array, imgWidth, imgHeight, numPixels, m_results.getMeanX(), m_results.getMeanY(), m_curFrame);
00346 }
00347 
00348 /* ================================================================ */
00349 
00350 void MPColorTracker::WriteBitmap(char *directory) {
00351 #ifdef WIN32
00352         char filename[256];
00353         sprintf(filename, "%s\\face_%d.bmp", directory, m_NumFrames);
00354         MPImageIO imageIO;
00355         imageIO.WriteBitmap((unsigned char*)m_colorImage.array, m_colorImage.width*m_colorImage.height, m_colorImage.width, m_colorImage.height, filename);
00356 #endif
00357 }
00358 
00359 /* ================================================================ */
00360 
00361 void MPColorTracker::PutImage(void *img, int &w, int &h, long & frameCount) {
00362         RImage<RGBTRIPLE> setImage((RGBTRIPLE *)img, w, h);
00363         m_colorImage = setImage;
00364         m_curFrame = frameCount;
00365 }
00366 
00367 /* ================================================================ */
00368 
00369 void MPColorTracker::SetKeyFrameCollection(MPKeyFrameCollection *collection) {
00370         m_Collection = collection;
00371 }
00372 
00373 
00374 
00375 /*
00376  * 
00377  * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
00378  * 
00379  *    1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
00380  *    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.
00381  *    3. The name of the author may not be used to endorse or promote products derived from this software without specific prior written permission.
00382  * 
00383  * 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.
00384  * 
00385  */