Visual Servoing Platform version 3.5.0
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
54vpScale::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
65vpScale::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
118double 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
167double 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 throw(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 Euclidian space R^d
241double 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(vpException(vpException::fatalError, "Error in vpScale::KernelDensityGradient_EPANECHNIKOV: wrong dimension"));
258 }
259
260 // return sumX*(dimension+2)/(n*pow(bandwidth,
261 // (double)dimension)*c*vpMath::sqr(bandwidth));
262 return sumX * (dimension + 2) / (n * bandwidth * c * vpMath::sqr(bandwidth));
263}
unsigned int getRows() const
Definition: vpArray2D.h:289
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ fatalError
Fatal error.
Definition: vpException.h:96
static double sqr(double x)
Definition: vpMath.h:116
double KernelDensity(vpColVector &error, unsigned int position)
Definition: vpScale.cpp:118
double KernelDensityGradient_EPANECHNIKOV(double X, unsigned int n)
Definition: vpScale.cpp:241
double KernelDensityGradient(vpColVector &error, unsigned int position)
Definition: vpScale.cpp:167
double MeanShift(vpColVector &error)
Definition: vpScale.cpp:81
double KernelDensity_EPANECHNIKOV(vpColVector &X)
Definition: vpScale.cpp:214
virtual ~vpScale(void)
Destructor.
Definition: vpScale.cpp:77
vpScale()
Constructor.
Definition: vpScale.cpp:54