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