ViSP  2.10.0
vpScale.cpp
1 /****************************************************************************
2  *
3  * $Id: vpScale.cpp 5126 2015-01-05 22:07:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Median Absolute Deviation (MAD), MPDE, Mean shift kernel density estimation.
36  *
37  * Authors:
38  * Andrew Comport
39  *
40  *****************************************************************************/
41 
49 #include <visp/vpColVector.h>
50 #include <visp/vpMath.h>
51 #include <visp/vpScale.h>
52 #include <stdlib.h>
53 #include <cmath> // std::fabs
54 #include <limits> // numeric_limits
55 
56 #define DEBUG_LEVEL2 0
57 
58 
59 
60 
63  : bandwidth(0.02), dimension(1), kernel_type(EPANECHNIKOV)
64 {
65 #if (DEBUG_LEVEL2)
66  std::cout << "vpScale constructor reached" << std::endl;
67 #endif
68 #if (DEBUG_LEVEL2)
69  std::cout << "vpScale constructor finished" << std::endl;
70 #endif
71 
72 }
73 
75 vpScale::vpScale(double kernel_bandwidth, unsigned int dim, int type)
76  : bandwidth(kernel_bandwidth), dimension(dim), kernel_type(type)
77 
78 {
79 #if (DEBUG_LEVEL2)
80  std::cout << "vpScale constructor reached" << std::endl;
81 #endif
82 #if (DEBUG_LEVEL2)
83  std::cout << "vpScale constructor finished" << std::endl;
84 #endif
85 }
86 
89 {
90 }
91 
92 // Calculate the modes of the density for the distribution
93 // and their associated errors
94 double
96 {
97 
98  unsigned int n = error.getRows()/dimension;
99  vpColVector density(n);
100  vpColVector density_gradient(n);
101  vpColVector mean_shift(n);
102 
103  unsigned int increment=1;
104 
105  // choose smallest error as start point
106  unsigned int i=0;
107  while(error[i]<0 && error[i]<error[i+1])
108  i++;
109 
110  // Do mean shift until no shift
111  while(increment >= 1 && i<n)
112  {
113  increment=0;
114  density[i] = KernelDensity(error, i);
115  density_gradient[i] = KernelDensityGradient(error, i);
116  mean_shift[i]=vpMath::sqr(bandwidth)*density_gradient[i]/((dimension+2)*density[i]);
117 
118  double tmp_shift = mean_shift[i];
119 
120  // Do mean shift
121  while(tmp_shift>0 && tmp_shift>error[i]-error[i+1])
122  {
123  i++;
124  increment++;
125  tmp_shift-=(error[i]-error[i-1]);
126  }
127  }
128 
129  return error[i];
130 
131 }
132 
133 // Calculate the density of each point in the error vector
134 // Requires ordered set of errors
135 double
136 vpScale::KernelDensity(vpColVector &error, unsigned int position)
137 {
138 
139  unsigned int n = error.getRows()/dimension;
140  double density=0;
141  double Ke = 1;
142  unsigned int j=position;
143 
144  vpColVector X(dimension);
145 
146 
147  // Use each error in the bandwidth to calculate
148  // the local density of error i
149  // First treat larger errors
150  //while(Ke !=0 && j<=n)
151  while(std::fabs(Ke) > std::numeric_limits<double>::epsilon() && j<=n)
152  {
153  //Create vector of errors corresponding to each dimension of a feature
154  for(unsigned int i=0; i<dimension; i++)
155  {
156  X[i]=(error[position]-error[j])/bandwidth;
157  position++;
158  j++;
159  }
160  position-=dimension; // reset position
161 
163  density+=Ke;
164  }
165 
166  Ke = 1;
167  j=position;
168  // Then treat smaller errors
169  //while(Ke !=0 && j>=dimension)
170  while(std::fabs(Ke) > std::numeric_limits<double>::epsilon() && j>=dimension)
171  {
172  //Create vector of errors corresponding to each dimension of a feature
173  for(unsigned int i=0; i<dimension; i++)
174  {
175  X[i]=(error[position]-error[j])/bandwidth;
176  position++;
177  j--;
178  }
179  position-=dimension; // reset position
180 
182  density+=Ke;
183  }
184 
185  density*=1/(n*bandwidth);
186 
187  return density;
188 
189 }
190 
191 double
192 vpScale::KernelDensityGradient(vpColVector &error, unsigned int position)
193 {
194 
195  unsigned int n = error.getRows()/dimension;
196  double density_gradient=0;
197  double sum_delta=0;
198  double delta=0;
199  int nx=0;
200 
201  double inside_kernel = 1;
202  unsigned int j=position;
203  // Use each error in the bandwidth to calculate
204  // the local density gradient
205  // First treat larger errors than current
206  //while(inside_kernel !=0 && j<=n)
207  while(std::fabs(inside_kernel) > std::numeric_limits<double>::epsilon() && j<=n)
208  {
209  delta = error[position]-error[j];
210  if(vpMath::sqr(delta/bandwidth)<1)
211  {
212  inside_kernel = 1;
213  sum_delta+=error[j]-error[position];
214  j++;
215  nx++;
216  }
217  else
218  inside_kernel = 0;
219  }
220 
221  inside_kernel = 1;
222  j=position;
223  // Then treat smaller errors than current
224  //while(inside_kernel !=0 && j>=dimension)
225  while(std::fabs(inside_kernel) > std::numeric_limits<double>::epsilon() && j>=dimension)
226  {
227  delta = error[position]-error[j];
228  if(vpMath::sqr(delta/bandwidth)<1)
229  {
230  inside_kernel = 1;
231  sum_delta+=error[j]-error[position];
232  j--;
233  nx++;
234  }
235  else
236  inside_kernel = 0;
237  }
238 
239  density_gradient = KernelDensityGradient_EPANECHNIKOV(sum_delta, n);
240 
241 
242  return density_gradient;
243 
244 }
245 
246 
247 //Epanechnikov_kernel for an d dimensional Euclidian space R^d
248 double
250 {
251 
252  double XtX= X*X;
253  double c; // Volume of an n dimensional unit sphere
254 
255  switch (dimension)
256  {
257  case 1:
258  c = 2;
259  break;
260  case 2:
261  c = M_PI;
262  break;
263  case 3:
264  c = 4*M_PI/3;
265  break;
266  default:
267  std::cout << "ERROR in vpScale::Kernel_EPANECHNIKOV : wrong dimension" << std::endl;
268  exit(1);
269  }
270 
271  if(XtX < 1)
272  return 1/(2*c)*(dimension+2)*(1-XtX);
273  else
274  return 0;
275 
276 }
277 
278 
279 //Epanechnikov_kernel for an d dimensional Euclidian space R^d
280 double
282 {
283 
284  double c; // Volume of an n dimensional unit sphere
285 
286  switch (dimension)
287  {
288  case 1:
289  c = 2;
290  break;
291  case 2:
292  c = M_PI;
293  break;
294  case 3:
295  c = 4*M_PI/3;
296  break;
297  default:
298  std::cout << "ERROR in vpScale::Kernel_EPANECHNIKOV : wrong dimension" << std::endl;
299  exit(1);
300  }
301 
302  //return sumX*(dimension+2)/(n*pow(bandwidth, (double)dimension)*c*vpMath::sqr(bandwidth));
303  return sumX*(dimension+2)/(n*bandwidth*c*vpMath::sqr(bandwidth));
304 
305 }
306 
double KernelDensityGradient_EPANECHNIKOV(double X, unsigned int n)
Definition: vpScale.cpp:281
double KernelDensityGradient(vpColVector &error, unsigned int position)
Definition: vpScale.cpp:192
static double sqr(double x)
Definition: vpMath.h:106
virtual ~vpScale(void)
Destructor.
Definition: vpScale.cpp:88
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
double MeanShift(vpColVector &error)
Definition: vpScale.cpp:95
double KernelDensity(vpColVector &error, unsigned int position)
Definition: vpScale.cpp:136
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
vpScale()
Constructor.
Definition: vpScale.cpp:62
double KernelDensity_EPANECHNIKOV(vpColVector &X)
Definition: vpScale.cpp:249