Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
vpScale.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 *****************************************************************************/
36 
41 #include <cmath> // std::fabs
42 #include <limits> // numeric_limits
43 #include <stdlib.h>
44 #include <visp3/core/vpColVector.h>
45 #include <visp3/core/vpMath.h>
46 #include <visp3/core/vpScale.h>
47 
48 #define DEBUG_LEVEL2 0
49 
51 vpScale::vpScale() : bandwidth(0.02), dimension(1)
52 {
53 #if (DEBUG_LEVEL2)
54  std::cout << "vpScale constructor reached" << std::endl;
55 #endif
56 #if (DEBUG_LEVEL2)
57  std::cout << "vpScale constructor finished" << std::endl;
58 #endif
59 }
60 
62 vpScale::vpScale(double kernel_bandwidth, unsigned int dim) : bandwidth(kernel_bandwidth), dimension(dim)
63 
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 
75 
76 // Calculate the modes of the density for the distribution
77 // and their associated errors
79 {
80 
81  unsigned int n = error.getRows() / dimension;
82  vpColVector density(n);
83  vpColVector density_gradient(n);
84  vpColVector mean_shift(n);
85 
86  unsigned int increment = 1;
87 
88  // choose smallest error as start point
89  unsigned int i = 0;
90  while (error[i] < 0 && error[i] < error[i + 1])
91  i++;
92 
93  // Do mean shift until no shift
94  while (increment >= 1 && i < n) {
95  increment = 0;
96  density[i] = KernelDensity(error, i);
97  density_gradient[i] = KernelDensityGradient(error, i);
98  mean_shift[i] = vpMath::sqr(bandwidth) * density_gradient[i] / ((dimension + 2) * density[i]);
99 
100  double tmp_shift = mean_shift[i];
101 
102  // Do mean shift
103  while (tmp_shift > 0 && tmp_shift > error[i] - error[i + 1]) {
104  i++;
105  increment++;
106  tmp_shift -= (error[i] - error[i - 1]);
107  }
108  }
109 
110  return error[i];
111 }
112 
113 // Calculate the density of each point in the error vector
114 // Requires ordered set of errors
115 double vpScale::KernelDensity(vpColVector &error, unsigned int position)
116 {
117 
118  unsigned int n = error.getRows() / dimension;
119  double density = 0;
120  double Ke = 1;
121  unsigned int j = position;
122 
123  vpColVector X(dimension);
124 
125  // Use each error in the bandwidth to calculate
126  // the local density of error i
127  // First treat larger errors
128  // while(Ke !=0 && j<=n)
129  while (std::fabs(Ke) > std::numeric_limits<double>::epsilon() && j <= n) {
130  // Create vector of errors corresponding to each dimension of a feature
131  for (unsigned int i = 0; i < dimension; i++) {
132  X[i] = (error[position] - error[j]) / bandwidth;
133  position++;
134  j++;
135  }
136  position -= dimension; // reset position
137 
139  density += Ke;
140  }
141 
142  Ke = 1;
143  j = position;
144  // Then treat smaller errors
145  // while(Ke !=0 && j>=dimension)
146  while (std::fabs(Ke) > std::numeric_limits<double>::epsilon() && j >= dimension) {
147  // Create vector of errors corresponding to each dimension of a feature
148  for (unsigned int i = 0; i < dimension; i++) {
149  X[i] = (error[position] - error[j]) / bandwidth;
150  position++;
151  j--;
152  }
153  position -= dimension; // reset position
154 
156  density += Ke;
157  }
158 
159  density *= 1 / (n * bandwidth);
160 
161  return density;
162 }
163 
164 double vpScale::KernelDensityGradient(vpColVector &error, unsigned int position)
165 {
166 
167  unsigned int n = error.getRows() / dimension;
168  double density_gradient = 0;
169  double sum_delta = 0;
170  double delta = 0;
171 
172  double inside_kernel = 1;
173  unsigned int j = position;
174  // Use each error in the bandwidth to calculate
175  // the local density gradient
176  // First treat larger errors than current
177  // while(inside_kernel !=0 && j<=n)
178  while (std::fabs(inside_kernel) > std::numeric_limits<double>::epsilon() && j <= n) {
179  delta = error[position] - error[j];
180  if (vpMath::sqr(delta / bandwidth) < 1) {
181  inside_kernel = 1;
182  sum_delta += error[j] - error[position];
183  j++;
184  } else {
185  inside_kernel = 0;
186  }
187  }
188 
189  inside_kernel = 1;
190  j = position;
191  // Then treat smaller errors than current
192  // while(inside_kernel !=0 && j>=dimension)
193  while (std::fabs(inside_kernel) > std::numeric_limits<double>::epsilon() && j >= dimension) {
194  delta = error[position] - error[j];
195  if (vpMath::sqr(delta / bandwidth) < 1) {
196  inside_kernel = 1;
197  sum_delta += error[j] - error[position];
198  j--;
199  } else {
200  inside_kernel = 0;
201  }
202  }
203 
204  density_gradient = KernelDensityGradient_EPANECHNIKOV(sum_delta, n);
205 
206  return density_gradient;
207 }
208 
209 // Epanechnikov_kernel for an d dimensional Euclidean space R^d
211 {
212 
213  double XtX = X * X;
214  double c; // Volume of an n dimensional unit sphere
215 
216  switch (dimension) {
217  case 1:
218  c = 2;
219  break;
220  case 2:
221  c = M_PI;
222  break;
223  case 3:
224  c = 4 * M_PI / 3;
225  break;
226  default:
227  throw(
228  vpException(vpException::fatalError, "Error in vpScale::KernelDensityGradient_EPANECHNIKOV: wrong dimension"));
229  }
230 
231  if (XtX < 1)
232  return 1 / (2 * c) * (dimension + 2) * (1 - XtX);
233  else
234  return 0;
235 }
236 
237 // Epanechnikov_kernel for an d dimensional Euclidean space R^d
238 double vpScale::KernelDensityGradient_EPANECHNIKOV(double sumX, unsigned int n)
239 {
240 
241  double c; // Volume of an n dimensional unit sphere
242 
243  switch (dimension) {
244  case 1:
245  c = 2;
246  break;
247  case 2:
248  c = M_PI;
249  break;
250  case 3:
251  c = 4 * M_PI / 3;
252  break;
253  default:
254  throw(
255  vpException(vpException::fatalError, "Error in vpScale::KernelDensityGradient_EPANECHNIKOV: wrong dimension"));
256  }
257 
258  // return sumX*(dimension+2)/(n*pow(bandwidth,
259  // (double)dimension)*c*vpMath::sqr(bandwidth));
260  return sumX * (dimension + 2) / (n * bandwidth * c * vpMath::sqr(bandwidth));
261 }
unsigned int getRows() const
Definition: vpArray2D.h:284
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ fatalError
Fatal error.
Definition: vpException.h:84
static double sqr(double x)
Definition: vpMath.h:201
double KernelDensity(vpColVector &error, unsigned int position)
Definition: vpScale.cpp:115
double KernelDensityGradient_EPANECHNIKOV(double X, unsigned int n)
Definition: vpScale.cpp:238
double KernelDensityGradient(vpColVector &error, unsigned int position)
Definition: vpScale.cpp:164
double MeanShift(vpColVector &error)
Definition: vpScale.cpp:78
double KernelDensity_EPANECHNIKOV(vpColVector &X)
Definition: vpScale.cpp:210
virtual ~vpScale(void)
Destructor.
Definition: vpScale.cpp:74
vpScale()
Constructor.
Definition: vpScale.cpp:51