00001 // **************************************************************************** 00002 // This file is part of the Integrating Vision Toolkit (IVT). 00003 // 00004 // The IVT is maintained by the Karlsruhe Institute of Technology (KIT) 00005 // (www.kit.edu) in cooperation with the company Keyetech (www.keyetech.de). 00006 // 00007 // Copyright (C) 2014 Karlsruhe Institute of Technology (KIT). 00008 // All rights reserved. 00009 // 00010 // Redistribution and use in source and binary forms, with or without 00011 // modification, are permitted provided that the following conditions are met: 00012 // 00013 // 1. Redistributions of source code must retain the above copyright 00014 // notice, this list of conditions and the following disclaimer. 00015 // 00016 // 2. Redistributions in binary form must reproduce the above copyright 00017 // notice, this list of conditions and the following disclaimer in the 00018 // documentation and/or other materials provided with the distribution. 00019 // 00020 // 3. Neither the name of the KIT nor the names of its contributors may be 00021 // used to endorse or promote products derived from this software 00022 // without specific prior written permission. 00023 // 00024 // THIS SOFTWARE IS PROVIDED BY THE KIT AND CONTRIBUTORS “AS IS” AND ANY 00025 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00026 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00027 // DISCLAIMED. IN NO EVENT SHALL THE KIT OR CONTRIBUTORS BE LIABLE FOR ANY 00028 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00029 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00030 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00031 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00032 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF 00033 // THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00034 // **************************************************************************** 00035 // **************************************************************************** 00036 // Filename: ParticleFilterFramework.cpp 00037 // Author: Pedram Azad 00038 // Date: 2004 00039 // **************************************************************************** 00040 00041 00042 // **************************************************************************** 00043 // Includes 00044 // **************************************************************************** 00045 00046 #include <new> // for explicitly using correct new/delete operators on VC DSPs 00047 00048 #include "ParticleFilterFramework.h" 00049 00050 #include "Helpers/helpers.h" 00051 00052 #include <math.h> 00053 #include <stdio.h> 00054 #include <float.h> 00055 00056 00057 00058 // **************************************************************************** 00059 // Constructor / Destructor 00060 // **************************************************************************** 00061 00062 CParticleFilterFramework::CParticleFilterFramework(int nParticles, int nDimension) 00063 { 00064 m_nParticles = nParticles; 00065 m_nDimension = nDimension; 00066 00067 mean_configuration = new double[nDimension]; 00068 last_configuration = new double[nDimension]; 00069 00070 sigma = new double[nDimension]; 00071 lower_limit = new double[nDimension]; 00072 upper_limit = new double[nDimension]; 00073 00074 int i; 00075 00076 // allocate memory 00077 s = new double*[nParticles]; 00078 for (i = 0; i < nParticles; i++) 00079 s[i] = new double[nDimension]; 00080 00081 s_temp = new double*[nParticles]; 00082 for (i = 0; i < nParticles; i++) 00083 s_temp[i] = new double[nDimension]; 00084 00085 c = new double[nParticles]; 00086 pi = new double[nParticles]; 00087 00088 temp = new double[nDimension]; 00089 00090 for (i = 0; i < nParticles; i++) 00091 { 00092 c[i] = 0.0; 00093 pi[i] = 0.0; 00094 } 00095 } 00096 00097 CParticleFilterFramework::~CParticleFilterFramework() 00098 { 00099 delete [] mean_configuration; 00100 delete [] last_configuration; 00101 delete [] sigma; 00102 delete [] lower_limit; 00103 delete [] upper_limit; 00104 00105 int i; 00106 00107 for (i = 0; i < m_nParticles; i++) 00108 delete [] s[i]; 00109 delete [] s; 00110 00111 for (i = 0; i < m_nParticles; i++) 00112 delete [] s_temp[i]; 00113 delete [] s_temp; 00114 00115 delete [] c; 00116 delete [] pi; 00117 00118 delete [] temp; 00119 } 00120 00121 00122 // **************************************************************************** 00123 // Methods 00124 // **************************************************************************** 00125 00126 void CParticleFilterFramework::GetConfiguration(double *pBestConfiguration, double dMeanFactor) 00127 { 00128 double mean = 0; 00129 int nConfigurations = 0, i; 00130 00131 for (i = 0; i < m_nDimension; i++) 00132 pBestConfiguration[i] = 0; 00133 00134 for (i = 0; i < m_nParticles; i++) 00135 mean += pi[i]; 00136 00137 mean /= m_nParticles * dMeanFactor; 00138 00139 for (i = 0; i < m_nParticles; i++) 00140 if (pi[i] > mean) 00141 { 00142 for (int j = 0; j < m_nDimension; j++) 00143 pBestConfiguration[j] += s[i][j]; 00144 00145 nConfigurations++; 00146 } 00147 00148 if (nConfigurations > 0) 00149 { 00150 for (i = 0; i < m_nDimension; i++) 00151 pBestConfiguration[i] /= nConfigurations; 00152 } 00153 } 00154 00155 void CParticleFilterFramework::GetBestConfiguration(double *pBestConfiguration) 00156 { 00157 double max = -DBL_MAX; 00158 int best_i, i; 00159 00160 for (i = 0; i < m_nParticles; i++) 00161 if (pi[i] > max) 00162 { 00163 best_i = i; 00164 max = pi[i]; 00165 } 00166 00167 00168 for (i = 0; i < m_nDimension; i++) 00169 pBestConfiguration[i] = s[best_i][i]; 00170 } 00171 00172 void CParticleFilterFramework::GetMeanConfiguration(double *pMeanConfiguration) 00173 { 00174 for (int i = 0; i < m_nDimension; i++) 00175 pMeanConfiguration[i] = mean_configuration[i]; 00176 } 00177 00178 void CParticleFilterFramework::GetPredictedConfiguration(double *pPredictedConfiguration) 00179 { 00180 for (int i = 0; i < m_nDimension; i++) 00181 pPredictedConfiguration[i] = mean_configuration[i] + 0.8 * (mean_configuration[i] - last_configuration[i]); 00182 } 00183 00184 00185 int CParticleFilterFramework::PickBaseSample() 00186 { 00187 double choice = uniform_random() * c_total; 00188 00189 int low = 0; 00190 int high = m_nParticles; 00191 00192 while (high > (low + 1)) 00193 { 00194 int middle = (high + low) / 2; 00195 00196 if (choice > c[middle]) 00197 low = middle; 00198 else 00199 high = middle; 00200 } 00201 00202 return low; 00203 } 00204 00205 double CParticleFilterFramework::ParticleFilter(double *pResultMeanConfiguration, double dSigmaFactor) 00206 { 00207 // push previous state through process model 00208 // use dynamic model and add noise 00209 PredictNewBases(dSigmaFactor); 00210 00211 // apply bayesian measurement weighting 00212 c_total = 0; 00213 int i; 00214 for (i = 0; i < m_nParticles; i++) 00215 { 00216 // update model (calculate forward kinematics) 00217 UpdateModel(i); 00218 00219 // evaluate likelihood function (compare edges,...) 00220 pi[i] = CalculateProbability(false); 00221 } 00222 00223 CalculateFinalProbabilities(); 00224 00225 for (i = 0; i < m_nParticles; i++) 00226 { 00227 c[i] = c_total; 00228 c_total += pi[i]; 00229 } 00230 00231 // normalize 00232 const double factor = 1 / c_total; 00233 for (i = 0; i < m_nParticles; i++) 00234 pi[i] *= factor; 00235 00236 CalculateMean(); 00237 00238 GetMeanConfiguration(pResultMeanConfiguration); 00239 00240 return CalculateProbabilityForConfiguration(pResultMeanConfiguration); 00241 } 00242 00243 void CParticleFilterFramework::CalculateMean() 00244 { 00245 int i; 00246 double max = -1; 00247 00248 for (i = 0; i < m_nDimension; i++) 00249 { 00250 last_configuration[i] = mean_configuration[i]; 00251 mean_configuration[i] = 0; 00252 } 00253 00254 for (i = 0; i < m_nParticles; i++) 00255 { 00256 for (int j = 0; j < m_nDimension; j++) 00257 mean_configuration[j] += pi[i] * s[i][j]; 00258 } 00259 } 00260 00261 double CParticleFilterFramework::CalculateProbabilityForConfiguration(const double *pConfiguration) 00262 { 00263 // little "trick" for calculating the probability for the mean configuration 00264 // without introducing a new virtual function by using s[0] 00265 00266 int i; 00267 00268 for (i = 0; i < m_nDimension; i++) 00269 { 00270 temp[i] = s[0][i]; 00271 s[0][i] = pConfiguration[i]; 00272 } 00273 00274 UpdateModel(0); 00275 00276 for (i = 0; i < m_nDimension; i++) 00277 s[0][i] = temp[i]; 00278 00279 const double dResult = CalculateProbability(true); 00280 00281 for (i = 0; i < m_nDimension; i++) 00282 s[0][i] = temp[i]; 00283 00284 return dResult; 00285 }