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