Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
51vpScale::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
62vpScale::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
115double 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
164double 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
238double 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:290
Implementation of column vector and the associated operations.
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:124
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