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: ParticleFilterFrameworkFloat.cpp 00037 // Author: Pedram Azad 00038 // Date: 07.05.2009 00039 // **************************************************************************** 00040 00041 00042 // **************************************************************************** 00043 // Includes 00044 // **************************************************************************** 00045 00046 #include <new> // for explicitly using correct new/delete operators on VC DSPs 00047 00048 #include "ParticleFilterFrameworkFloat.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 CParticleFilterFrameworkFloat::CParticleFilterFrameworkFloat(int nParticles, int nDimension) 00063 { 00064 m_nParticles = nParticles; 00065 m_nDimension = nDimension; 00066 00067 mean_configuration = new float[nDimension]; 00068 last_configuration = new float[nDimension]; 00069 00070 sigma = new float[nDimension]; 00071 lower_limit = new float[nDimension]; 00072 upper_limit = new float[nDimension]; 00073 00074 int i; 00075 00076 // allocate memory 00077 s = new float*[nParticles]; 00078 for (i = 0; i < nParticles; i++) 00079 s[i] = new float[nDimension]; 00080 00081 s_temp = new float*[nParticles]; 00082 for (i = 0; i < nParticles; i++) 00083 s_temp[i] = new float[nDimension]; 00084 00085 c = new double[nParticles]; 00086 pi = new double[nParticles]; 00087 00088 temp = new float[nDimension]; 00089 00090 for (i = 0; i < nParticles; i++) 00091 { 00092 c[i] = 0.0; 00093 pi[i] = 0.0; 00094 } 00095 } 00096 00097 CParticleFilterFrameworkFloat::~CParticleFilterFrameworkFloat() 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 CParticleFilterFrameworkFloat::GetConfiguration(float *pBestConfiguration, float fMeanFactor) 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 * double(fMeanFactor); 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 CParticleFilterFrameworkFloat::GetBestConfiguration(float *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 CParticleFilterFrameworkFloat::GetMeanConfiguration(float *pMeanConfiguration) 00173 { 00174 for (int i = 0; i < m_nDimension; i++) 00175 pMeanConfiguration[i] = mean_configuration[i]; 00176 } 00177 00178 void CParticleFilterFrameworkFloat::GetPredictedConfiguration(float *pPredictedConfiguration) 00179 { 00180 for (int i = 0; i < m_nDimension; i++) 00181 pPredictedConfiguration[i] = mean_configuration[i] + 0.8f * (mean_configuration[i] - last_configuration[i]); 00182 } 00183 00184 int CParticleFilterFrameworkFloat::PickBaseSample() 00185 { 00186 double choice = uniform_random() * c_total; 00187 00188 int low = 0; 00189 int high = m_nParticles; 00190 00191 while (high > (low + 1)) 00192 { 00193 int middle = (high + low) >> 1; 00194 00195 if (choice > c[middle]) 00196 low = middle; 00197 else 00198 high = middle; 00199 } 00200 00201 return low; 00202 } 00203 00204 double CParticleFilterFrameworkFloat::ParticleFilter(float *pResultMeanConfiguration, float dSigmaFactor) 00205 { 00206 // push previous state through process model 00207 // use dynamic model and add noise 00208 PredictNewBases(dSigmaFactor); 00209 00210 // apply bayesian measurement weighting 00211 c_total = 0.0; 00212 int i; 00213 for (i = 0; i < m_nParticles; i++) 00214 { 00215 // update model (calculate forward kinematics) 00216 UpdateModel(i); 00217 00218 // evaluate likelihood function (compare edges,...) 00219 pi[i] = CalculateProbability(false); 00220 } 00221 00222 CalculateFinalProbabilities(); 00223 00224 for (i = 0; i < m_nParticles; i++) 00225 { 00226 c[i] = c_total; 00227 c_total += pi[i]; 00228 } 00229 00230 // normalize 00231 const double factor = 1.0 / c_total; 00232 for (i = 0; i < m_nParticles; i++) 00233 pi[i] *= factor; 00234 00235 CalculateMean(); 00236 00237 GetMeanConfiguration(pResultMeanConfiguration); 00238 00239 return CalculateProbabilityForConfiguration(pResultMeanConfiguration); 00240 } 00241 00242 void CParticleFilterFrameworkFloat::CalculateMean() 00243 { 00244 int i; 00245 00246 for (i = 0; i < m_nDimension; i++) 00247 { 00248 last_configuration[i] = mean_configuration[i]; 00249 mean_configuration[i] = 0.0f; 00250 } 00251 00252 for (i = 0; i < m_nParticles; i++) 00253 { 00254 for (int j = 0; j < m_nDimension; j++) 00255 mean_configuration[j] += float(pi[i] * s[i][j]); 00256 } 00257 } 00258 00259 double CParticleFilterFrameworkFloat::CalculateProbabilityForConfiguration(const float *pConfiguration) 00260 { 00261 // little "trick" for calculating the probability for the mean configuration 00262 // without introducing a new virtual function by using s[0] 00263 00264 int i; 00265 00266 for (i = 0; i < m_nDimension; i++) 00267 { 00268 temp[i] = s[0][i]; 00269 s[0][i] = pConfiguration[i]; 00270 } 00271 00272 UpdateModel(0); 00273 00274 for (i = 0; i < m_nDimension; i++) 00275 s[0][i] = temp[i]; 00276 00277 const double dResult = CalculateProbability(true); 00278 00279 for (i = 0; i < m_nDimension; i++) 00280 s[0][i] = temp[i]; 00281 00282 return dResult; 00283 }