IVT
ParticleFilterFrameworkFloat.cpp
Go to the documentation of this file.
1 // ****************************************************************************
2 // This file is part of the Integrating Vision Toolkit (IVT).
3 //
4 // The IVT is maintained by the Karlsruhe Institute of Technology (KIT)
5 // (www.kit.edu) in cooperation with the company Keyetech (www.keyetech.de).
6 //
7 // Copyright (C) 2014 Karlsruhe Institute of Technology (KIT).
8 // All rights reserved.
9 //
10 // Redistribution and use in source and binary forms, with or without
11 // modification, are permitted provided that the following conditions are met:
12 //
13 // 1. Redistributions of source code must retain the above copyright
14 // notice, this list of conditions and the following disclaimer.
15 //
16 // 2. Redistributions in binary form must reproduce the above copyright
17 // notice, this list of conditions and the following disclaimer in the
18 // documentation and/or other materials provided with the distribution.
19 //
20 // 3. Neither the name of the KIT nor the names of its contributors may be
21 // used to endorse or promote products derived from this software
22 // without specific prior written permission.
23 //
24 // THIS SOFTWARE IS PROVIDED BY THE KIT AND CONTRIBUTORS “AS IS” AND ANY
25 // EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
26 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
27 // DISCLAIMED. IN NO EVENT SHALL THE KIT OR CONTRIBUTORS BE LIABLE FOR ANY
28 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
29 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
31 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
32 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
33 // THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
34 // ****************************************************************************
35 // ****************************************************************************
36 // Filename: ParticleFilterFrameworkFloat.cpp
37 // Author: Pedram Azad
38 // Date: 07.05.2009
39 // ****************************************************************************
40 
41 
42 // ****************************************************************************
43 // Includes
44 // ****************************************************************************
45 
46 #include <new> // for explicitly using correct new/delete operators on VC DSPs
47 
49 
50 #include "Helpers/helpers.h"
51 
52 #include <math.h>
53 #include <stdio.h>
54 #include <float.h>
55 
56 
57 
58 // ****************************************************************************
59 // Constructor / Destructor
60 // ****************************************************************************
61 
63 {
64  m_nParticles = nParticles;
65  m_nDimension = nDimension;
66 
67  mean_configuration = new float[nDimension];
68  last_configuration = new float[nDimension];
69 
70  sigma = new float[nDimension];
71  lower_limit = new float[nDimension];
72  upper_limit = new float[nDimension];
73 
74  int i;
75 
76  // allocate memory
77  s = new float*[nParticles];
78  for (i = 0; i < nParticles; i++)
79  s[i] = new float[nDimension];
80 
81  s_temp = new float*[nParticles];
82  for (i = 0; i < nParticles; i++)
83  s_temp[i] = new float[nDimension];
84 
85  c = new double[nParticles];
86  pi = new double[nParticles];
87 
88  temp = new float[nDimension];
89 
90  for (i = 0; i < nParticles; i++)
91  {
92  c[i] = 0.0;
93  pi[i] = 0.0;
94  }
95 }
96 
98 {
99  delete [] mean_configuration;
100  delete [] last_configuration;
101  delete [] sigma;
102  delete [] lower_limit;
103  delete [] upper_limit;
104 
105  int i;
106 
107  for (i = 0; i < m_nParticles; i++)
108  delete [] s[i];
109  delete [] s;
110 
111  for (i = 0; i < m_nParticles; i++)
112  delete [] s_temp[i];
113  delete [] s_temp;
114 
115  delete [] c;
116  delete [] pi;
117 
118  delete [] temp;
119 }
120 
121 
122 // ****************************************************************************
123 // Methods
124 // ****************************************************************************
125 
126 void CParticleFilterFrameworkFloat::GetConfiguration(float *pBestConfiguration, float fMeanFactor)
127 {
128  double mean = 0;
129  int nConfigurations = 0, i;
130 
131  for (i = 0; i < m_nDimension; i++)
132  pBestConfiguration[i] = 0;
133 
134  for (i = 0; i < m_nParticles; i++)
135  mean += pi[i];
136 
137  mean /= m_nParticles * double(fMeanFactor);
138 
139  for (i = 0; i < m_nParticles; i++)
140  if (pi[i] > mean)
141  {
142  for (int j = 0; j < m_nDimension; j++)
143  pBestConfiguration[j] += s[i][j];
144 
145  nConfigurations++;
146  }
147 
148  if (nConfigurations > 0)
149  {
150  for (i = 0; i < m_nDimension; i++)
151  pBestConfiguration[i] /= nConfigurations;
152  }
153 }
154 
156 {
157  double max = -DBL_MAX;
158  int best_i, i;
159 
160  for (i = 0; i < m_nParticles; i++)
161  if (pi[i] > max)
162  {
163  best_i = i;
164  max = pi[i];
165  }
166 
167 
168  for (i = 0; i < m_nDimension; i++)
169  pBestConfiguration[i] = s[best_i][i];
170 }
171 
173 {
174  for (int i = 0; i < m_nDimension; i++)
175  pMeanConfiguration[i] = mean_configuration[i];
176 }
177 
179 {
180  for (int i = 0; i < m_nDimension; i++)
181  pPredictedConfiguration[i] = mean_configuration[i] + 0.8f * (mean_configuration[i] - last_configuration[i]);
182 }
183 
185 {
186  double choice = uniform_random() * c_total;
187 
188  int low = 0;
189  int high = m_nParticles;
190 
191  while (high > (low + 1))
192  {
193  int middle = (high + low) >> 1;
194 
195  if (choice > c[middle])
196  low = middle;
197  else
198  high = middle;
199  }
200 
201  return low;
202 }
203 
204 double CParticleFilterFrameworkFloat::ParticleFilter(float *pResultMeanConfiguration, float dSigmaFactor)
205 {
206  // push previous state through process model
207  // use dynamic model and add noise
208  PredictNewBases(dSigmaFactor);
209 
210  // apply bayesian measurement weighting
211  c_total = 0.0;
212  int i;
213  for (i = 0; i < m_nParticles; i++)
214  {
215  // update model (calculate forward kinematics)
216  UpdateModel(i);
217 
218  // evaluate likelihood function (compare edges,...)
219  pi[i] = CalculateProbability(false);
220  }
221 
223 
224  for (i = 0; i < m_nParticles; i++)
225  {
226  c[i] = c_total;
227  c_total += pi[i];
228  }
229 
230  // normalize
231  const double factor = 1.0 / c_total;
232  for (i = 0; i < m_nParticles; i++)
233  pi[i] *= factor;
234 
235  CalculateMean();
236 
237  GetMeanConfiguration(pResultMeanConfiguration);
238 
239  return CalculateProbabilityForConfiguration(pResultMeanConfiguration);
240 }
241 
243 {
244  int i;
245 
246  for (i = 0; i < m_nDimension; i++)
247  {
249  mean_configuration[i] = 0.0f;
250  }
251 
252  for (i = 0; i < m_nParticles; i++)
253  {
254  for (int j = 0; j < m_nDimension; j++)
255  mean_configuration[j] += float(pi[i] * s[i][j]);
256  }
257 }
258 
260 {
261  // little "trick" for calculating the probability for the mean configuration
262  // without introducing a new virtual function by using s[0]
263 
264  int i;
265 
266  for (i = 0; i < m_nDimension; i++)
267  {
268  temp[i] = s[0][i];
269  s[0][i] = pConfiguration[i];
270  }
271 
272  UpdateModel(0);
273 
274  for (i = 0; i < m_nDimension; i++)
275  s[0][i] = temp[i];
276 
277  const double dResult = CalculateProbability(true);
278 
279  for (i = 0; i < m_nDimension; i++)
280  s[0][i] = temp[i];
281 
282  return dResult;
283 }
double uniform_random()
Definition: helpers.cpp:251
virtual void GetPredictedConfiguration(float *pPredictedConfiguration)
virtual void GetBestConfiguration(float *pBestConfiguration)
virtual double CalculateProbability(bool bSeparateCall=true)=0
CParticleFilterFrameworkFloat(int nParticles, int nDimension)
virtual void PredictNewBases(float fSigmaFactor)=0
virtual void GetConfiguration(float *pBestConfiguration, float fMeanFactor)
virtual void UpdateModel(int nParticle)=0
double ParticleFilter(float *pResultMeanConfiguration, float fSigmaFactor=1.0f)
virtual void GetMeanConfiguration(float *pMeanConfiguration)
double CalculateProbabilityForConfiguration(const float *pConfiguration)