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