00001 /*!@file BayesFilters/ParticleFilter.C Particle Filter */ 00002 // //////////////////////////////////////////////////////////////////// // 00003 // The iLab Neuromorphic Vision C++ Toolkit - Copyright (C) 2001 by the // 00004 // University of Southern California (USC) and the iLab at USC. // 00005 // See http://iLab.usc.edu for information about this project. // 00006 // //////////////////////////////////////////////////////////////////// // 00007 // Major portions of the iLab Neuromorphic Vision Toolkit are protected // 00008 // under the U.S. patent ``Computation of Intrinsic Perceptual Saliency // 00009 // in Visual Environments, and Applications'' by Christof Koch and // 00010 // Laurent Itti, California Institute of Technology, 2001 (patent // 00011 // pending; application number 09/912,225 filed July 23, 2001; see // 00012 // http://pair.uspto.gov/cgi-bin/final/home.pl for current status). // 00013 // //////////////////////////////////////////////////////////////////// // 00014 // This file is part of the iLab Neuromorphic Vision C++ Toolkit. // 00015 // // 00016 // The iLab Neuromorphic Vision C++ Toolkit is free software; you can // 00017 // redistribute it and/or modify it under the terms of the GNU General // 00018 // Public License as published by the Free Software Foundation; either // 00019 // version 2 of the License, or (at your option) any later version. // 00020 // // 00021 // The iLab Neuromorphic Vision C++ Toolkit is distributed in the hope // 00022 // that it will be useful, but WITHOUT ANY WARRANTY; without even the // 00023 // implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR // 00024 // PURPOSE. See the GNU General Public License for more details. // 00025 // // 00026 // You should have received a copy of the GNU General Public License // 00027 // along with the iLab Neuromorphic Vision C++ Toolkit; if not, write // 00028 // to the Free Software Foundation, Inc., 59 Temple Place, Suite 330, // 00029 // Boston, MA 02111-1307 USA. // 00030 // //////////////////////////////////////////////////////////////////// // 00031 // 00032 // Primary maintainer for this file: Lior Elazary 00033 // $HeadURL: $ 00034 // $Id: $ 00035 00036 #ifndef ParticleFilter_C_DEFINED 00037 #define ParticleFilter_C_DEFINED 00038 00039 #include "BayesFilters/ParticleFilter.H" 00040 #include "Util/MathFunctions.H" 00041 00042 // ###################################################################### 00043 00044 ParticleFilter::ParticleFilter(int numStates, int numObservations, int numParticles) : 00045 itsNumStates(numStates), 00046 itsNumObservations(numObservations) 00047 { 00048 00049 //Initialize the particles 00050 itsParticles.resize(numParticles); 00051 double totalCumlProb = 0; 00052 for(uint i=0; i<itsParticles.size(); i++) 00053 { 00054 double weight = 1.0; 00055 itsParticles[1].state = Image<double>(1,itsNumStates,NO_INIT); 00056 itsParticles[1].weight = weight; 00057 itsParticles[1].cumlProb = i; 00058 totalCumlProb += weight; 00059 } 00060 largestCumlProb = totalCumlProb; 00061 00062 double priorMean = 0.0; 00063 double priorSigma = 0.2; 00064 00065 //Init the state from a Gaussian 00066 for(uint i=0; i<itsParticles.size(); i++) 00067 itsParticles[i].state.setVal(0,0, priorMean + priorSigma*gaussianRand()); 00068 00069 } 00070 00071 void ParticleFilter::predictState() 00072 { 00073 for(uint i=0; i<itsParticles.size(); i++) 00074 { 00075 int particle = pickParticleToSample(); 00076 Image<double> newState = getNextState(itsParticles[particle].state); 00077 itsParticles[particle].state = newState; 00078 } 00079 } 00080 00081 00082 /* This is binary search using cumulative probabilities to pick a base 00083 sample. The use of this routine makes Condensation O(NlogN) where N 00084 is the number of samples. It is probably better to pick base 00085 samples deterministically, since then the algorithm is O(N) and 00086 probably marginally more efficient, but this routine is kept here 00087 for conceptual simplicity and because it maps better to the 00088 published literature. */ 00089 int ParticleFilter::pickParticleToSample(void) 00090 { 00091 double choice = uniformRandom() * largestCumlProb; 00092 int low, middle, high; 00093 00094 low = 0; 00095 high = itsParticles.size(); 00096 00097 while (high>(low+1)) { 00098 middle = (high+low)/2; 00099 if (choice > itsParticles[middle].cumlProb) 00100 low = middle; 00101 else high = middle; 00102 } 00103 00104 return low; 00105 } 00106 00107 00108 00109 double ParticleFilter::getLikelihood(const Image<double>& z, const Image<double>& X) 00110 { 00111 00112 Image<double> predZ = getObservation(X); 00113 double val = predZ[0] - z[0]; 00114 double sigma = 0.03; 00115 00116 //Evaluate using a Gaussian distribution 00117 /* This private definition is used for portability */ 00118 static const double PI = 3.14159265358979323846; 00119 00120 return 1.0/(sqrt(2.0*PI) * sigma) * 00121 exp(-0.5 * (val*val / (sigma*sigma))); 00122 00123 } 00124 00125 00126 void ParticleFilter::update(const Image<double>& z) 00127 { 00128 00129 double cumlTotal = 0; 00130 for(uint i=0; i<itsParticles.size(); i++) 00131 { 00132 double weight = getLikelihood(z, itsParticles[i].state); 00133 itsParticles[i].weight = weight; 00134 itsParticles[i].cumlProb = cumlTotal; 00135 cumlTotal += weight; 00136 } 00137 largestCumlProb = cumlTotal; 00138 00139 } 00140 00141 00142 // ###################################################################### 00143 /* So things look consistent in everyone's emacs... */ 00144 /* Local Variables: */ 00145 /* indent-tabs-mode: nil */ 00146 /* End: */ 00147 00148 #endif