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 * M-Estimator and various influence function.
33 *
34 * Authors:
35 * Andrew Comport
36 * Jean Laneurit
37 *
38 *****************************************************************************/
39
40 /*!
41 \file vpRobust.cpp
42 */
43
44 #include <cmath> // std::fabs
45 #include <limits> // numeric_limits
46 #include <stdio.h>
47 #include <stdlib.h>
48 #include <string.h>
49 #include <algorithm> // std::swap
50
51 #include <visp3/core/vpColVector.h>
52 #include <visp3/core/vpDebug.h>
53 #include <visp3/core/vpMath.h>
54 #include <visp3/core/vpRobust.h>
55
56 /*!
57 Default constructor.
58 */
vpRobust()59 vpRobust::vpRobust()
60 : m_normres(), m_sorted_normres(), m_sorted_residues(), m_mad_min(0.0017), m_mad_prev(0),
61 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
62 m_iter(0),
63 #endif
64 m_size(0), m_mad(0)
65 {
66 }
67
68 /*!
69 Copy constructor.
70 */
vpRobust(const vpRobust & other)71 vpRobust::vpRobust(const vpRobust &other) { *this = other; }
72
73 /*!
74 Copy operator.
75 */
operator =(const vpRobust & other)76 vpRobust &vpRobust::operator=(const vpRobust &other)
77 {
78 m_normres = other.m_normres;
79 m_sorted_normres = other.m_sorted_normres;
80 m_sorted_residues = other.m_sorted_residues;
81 m_mad_min = other.m_mad_min;
82 m_mad = other.m_mad;
83 m_mad_prev = other.m_mad_prev;
84 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
85 m_iter = other.m_iter;
86 #endif
87 m_size = other.m_size;
88 return *this;
89 }
90
91 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
92 /*!
93 Move operator.
94 */
operator =(const vpRobust && other)95 vpRobust &vpRobust::operator=(const vpRobust &&other)
96 {
97 m_normres = std::move(other.m_normres);
98 m_sorted_normres = std::move(other.m_sorted_normres);
99 m_sorted_residues = std::move(other.m_sorted_residues);
100 m_mad_min = std::move(other.m_mad_min);
101 m_mad_prev = std::move(other.m_mad_prev);
102 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
103 m_iter = std::move(other.m_iter);
104 #endif
105 m_size = std::move(other.m_size);
106 return *this;
107 }
108 #endif
109
110 /*!
111 Resize containers.
112 \param n_data : size of input data vector.
113 */
resize(unsigned int n_data)114 void vpRobust::resize(unsigned int n_data)
115 {
116 if (n_data != m_size) {
117 m_normres.resize(n_data);
118 m_sorted_normres.resize(n_data);
119 m_sorted_residues.resize(n_data);
120 m_size = n_data;
121 }
122 }
123
124 // ===================================================================
125 /*!
126
127 Calculate an M-estimate given a particular influence function using MAD
128 (Median Absolute Deviation) as a scale estimate at each iteration.
129
130 \param[in] method : Type of influence function.
131
132 \param[in] residues : Vector of residues \f$ r \f$ of the parameters to estimate.
133
134 \param[out] weights : Vector of weights \f$w(r)\f$. Values are in [0, 1]. A value near zero
135 means that the data is an outlier.
136 */
MEstimator(const vpRobustEstimatorType method,const vpColVector & residues,vpColVector & weights)137 void vpRobust::MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
138 {
139 double med = 0; // median
140 double normmedian = 0; // Normalized median
141
142 // resize vector only if the size of residue vector has changed
143 unsigned int n_data = residues.getRows();
144 weights.resize(n_data, false);
145 resize(n_data);
146
147 m_sorted_residues = residues;
148
149 unsigned int ind_med = (unsigned int)(ceil(n_data / 2.0)) - 1;
150
151 // Calculate median
152 med = select(m_sorted_residues, 0, n_data - 1, ind_med);
153 // residualMedian = med ;
154
155 // Normalize residues
156 for (unsigned int i = 0; i < n_data; i++) {
157 m_normres[i] = (fabs(residues[i] - med));
158 m_sorted_normres[i] = (fabs(m_sorted_residues[i] - med));
159 }
160
161 // Calculate MAD
162 normmedian = select(m_sorted_normres, 0, n_data - 1, ind_med);
163 // normalizedResidualMedian = normmedian ;
164 // 1.48 keeps scale estimate consistent for a normal probability dist.
165 m_mad = 1.4826 * normmedian; // median Absolute Deviation
166
167 // Set a minimum threshold for sigma
168 // (when sigma reaches the level of noise in the image)
169 if (m_mad < m_mad_min) {
170 m_mad = m_mad_min;
171 }
172 switch (method) {
173 case TUKEY: {
174 psiTukey(m_mad, m_normres, weights);
175 break;
176 }
177 case CAUCHY: {
178 psiCauchy(m_mad, m_normres, weights);
179 break;
180 }
181 case HUBER: {
182 psiHuber(m_mad, m_normres, weights);
183 break;
184 }
185 }
186 }
187
188 /*!
189 Calculation of Tukey's influence function.
190
191 \param sigma : sigma parameters.
192 \param x : normalized residue vector.
193 \param weights : weight vector.
194 */
195
psiTukey(double sig,const vpColVector & x,vpColVector & weights)196 void vpRobust::psiTukey(double sig, const vpColVector &x, vpColVector &weights)
197 {
198 unsigned int n_data = x.getRows();
199 double C = sig * 4.6851;
200
201 // Here we consider that sig cannot be equal to 0
202 for (unsigned int i = 0; i < n_data; i++) {
203 double xi = x[i] / C;
204 xi *= xi;
205
206 if (xi > 1.) {
207 weights[i] = 0;
208 } else {
209 xi = 1 - xi;
210 xi *= xi;
211 weights[i] = xi;
212 }
213 }
214 }
215
216 /*!
217 Calculation of Tukey's influence function.
218
219 \param sigma : sigma parameters.
220 \param x : normalized residue vector.
221 \param weights : weight vector.
222 */
psiHuber(double sig,const vpColVector & x,vpColVector & weights)223 void vpRobust::psiHuber(double sig, const vpColVector &x, vpColVector &weights)
224 {
225 double C = sig * 1.2107;
226 unsigned int n_data = x.getRows();
227
228 for (unsigned int i = 0; i < n_data; i++) {
229 double xi = x[i] / C;
230 if (fabs(xi) > 1.)
231 weights[i] = std::fabs(1./xi);
232 else
233 weights[i] = 1;
234 }
235 }
236
237 /*!
238 Calculation of Cauchy's influence function.
239
240 \param sigma : sigma parameter.
241 \param x : normalized residue vector.
242 \param weights : weight vector.
243 */
244
psiCauchy(double sig,const vpColVector & x,vpColVector & weights)245 void vpRobust::psiCauchy(double sig, const vpColVector &x, vpColVector &weights)
246 {
247 unsigned int n_data = x.getRows();
248 double C = sig * 2.3849;
249
250 // Calculate Cauchy's equation
251 for (unsigned int i = 0; i < n_data; i++) {
252 weights[i] = 1. / (1. + vpMath::sqr(x[i] / (C)));
253 }
254 }
255
256 /*!
257 Partition function.
258 \param a : vector to be sorted.
259 \param l : first value to be considered.
260 \param r : last value to be considered.
261 */
partition(vpColVector & a,unsigned int l,unsigned int r)262 int vpRobust::partition(vpColVector &a, unsigned int l, unsigned int r)
263 {
264 unsigned int i = l - 1;
265 unsigned int j = r;
266 double v = a[r];
267
268 for (;;) {
269 while (a[++i] < v)
270 ;
271 while (v < a[--j])
272 if (j == l)
273 break;
274 if (i >= j)
275 break;
276 std::swap(a[i], a[j]);
277 }
278 std::swap(a[i], a[r]);
279 return i;
280 }
281
282 /*!
283 \brief Sort a part of a vector and select a value of this new vector.
284 \param a : vector to be sorted
285 \param l : first value to be considered
286 \param r : last value to be considered
287 \param k : value to be selected
288 */
select(vpColVector & a,unsigned int l,unsigned int r,unsigned int k)289 double vpRobust::select(vpColVector &a, unsigned int l, unsigned int r, unsigned int k)
290 {
291 while (r > l) {
292 unsigned int i = partition(a, l, r);
293 if (i >= k)
294 r = i - 1;
295 if (i <= k)
296 l = i + 1;
297 }
298 return a[k];
299 }
300
301 /**********************
302 * Below are deprecated functions
303 */
304 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
305 #define vpITMAX 100
306 #define vpEPS 3.0e-7
307
308 /*!
309 \deprecated You should rather use the default constructor.
310 \param n_data : Size of the data vector.
311 */
vpRobust(unsigned int n_data)312 vpRobust::vpRobust(unsigned int n_data)
313 : m_normres(), m_sorted_normres(), m_sorted_residues(), m_mad_min(0.0017), m_mad_prev(0),
314 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
315 m_iter(0),
316 #endif
317 m_size(n_data), m_mad(0)
318 {
319 vpCDEBUG(2) << "vpRobust constructor reached" << std::endl;
320
321 m_normres.resize(n_data);
322 m_sorted_normres.resize(n_data);
323 m_sorted_residues.resize(n_data);
324 // m_mad_min=0.0017; //Can not be more accurate than 1 pixel
325 }
326
MEstimator(const vpRobustEstimatorType method,const vpColVector & residues,const vpColVector & all_residues,vpColVector & weights)327 void vpRobust::MEstimator(const vpRobustEstimatorType method, const vpColVector &residues,
328 const vpColVector &all_residues, vpColVector &weights)
329 {
330 double normmedian = 0; // Normalized median
331
332 unsigned int n_all_data = all_residues.getRows();
333 vpColVector all_normres(n_all_data);
334
335 // compute median with the residues vector, return all_normres which are the
336 // normalized all_residues vector.
337 normmedian = computeNormalizedMedian(all_normres, residues, all_residues, weights);
338
339 // 1.48 keeps scale estimate consistent for a normal probability dist.
340 m_mad = 1.4826 * normmedian; // Median Absolute Deviation
341
342 // Set a minimum threshold for sigma
343 // (when sigma reaches the level of noise in the image)
344 if (m_mad < m_mad_min) {
345 m_mad = m_mad_min;
346 }
347
348 switch (method) {
349 case TUKEY: {
350 psiTukey(m_mad, all_normres, weights);
351
352 vpCDEBUG(2) << "Tukey's function computed" << std::endl;
353 break;
354 }
355 case CAUCHY: {
356 psiCauchy(m_mad, all_normres, weights);
357 break;
358 }
359 case HUBER: {
360 psiHuber(m_mad, all_normres, weights);
361 break;
362 }
363 };
364 }
365
computeNormalizedMedian(vpColVector & all_normres,const vpColVector & residues,const vpColVector & all_residues,const vpColVector & weights)366 double vpRobust::computeNormalizedMedian(vpColVector &all_normres, const vpColVector &residues,
367 const vpColVector &all_residues, const vpColVector &weights)
368 {
369 double med = 0;
370 double normmedian = 0;
371
372 unsigned int n_all_data = all_residues.getRows();
373 unsigned int n_data = residues.getRows();
374
375 // resize vector only if the size of residue vector has changed
376 resize(n_data);
377
378 m_sorted_residues = residues;
379 vpColVector no_null_weight_residues;
380 no_null_weight_residues.resize(n_data);
381
382 unsigned int index = 0;
383 for (unsigned int j = 0; j < n_data; j++) {
384 // if(weights[j]!=0)
385 if (std::fabs(weights[j]) > std::numeric_limits<double>::epsilon()) {
386 no_null_weight_residues[index] = residues[j];
387 index++;
388 }
389 }
390 m_sorted_residues.resize(index);
391 memcpy(m_sorted_residues.data, no_null_weight_residues.data, index * sizeof(double));
392 n_data = index;
393
394 // Calculate Median
395 // Be careful to not use the rejected residues for the
396 // calculation.
397
398 unsigned int ind_med = (unsigned int)(ceil(n_data / 2.0)) - 1;
399 med = select(m_sorted_residues, 0, n_data - 1, ind_med);
400
401 // Normalize residues
402 for (unsigned int i = 0; i < n_all_data; i++) {
403 all_normres[i] = (fabs(all_residues[i] - med));
404 }
405
406 for (unsigned int i = 0; i < n_data; i++) {
407 m_sorted_normres[i] = (fabs(m_sorted_residues[i] - med));
408 }
409 // MAD calculated only on first iteration
410 normmedian = select(m_sorted_normres, 0, n_data - 1, ind_med);
411
412 return normmedian;
413 }
414
415 /*!
416 * \deprecated This function is useless.
417 * Calculate an Mestimate with a simultaneous scale estimate using HUBER's influence function
418 * \param[in] residues : Vector of residues. The content of the vector is changed.
419 * \return Returns a vector of weights associated to each residue.
420 */
simultMEstimator(vpColVector & residues)421 vpColVector vpRobust::simultMEstimator(vpColVector &residues)
422 {
423 double med = 0; // Median
424
425 unsigned int n_data = residues.getRows();
426 vpColVector norm_res(n_data); // Normalized Residue
427 vpColVector w(n_data);
428
429 // Calculate Median
430 unsigned int ind_med = (unsigned int)(ceil(n_data / 2.0)) - 1;
431 med = select(residues, 0, n_data - 1, ind_med /*(int)n_data/2*/);
432
433 // Normalize residues
434 for (unsigned int i = 0; i < n_data; i++)
435 norm_res[i] = (fabs(residues[i] - med));
436
437 // Check for various methods.
438 // For Huber compute Simultaneous scale estimate
439 // For Others use MAD calculated on first iteration
440 if (m_iter == 0) {
441 double normmedian = select(norm_res, 0, n_data - 1, ind_med); // Normalized Median
442 // 1.48 keeps scale estimate consistent for a normal probability dist.
443 m_mad = 1.4826 * normmedian; // Median Absolute Deviation
444 } else {
445 // compute simultaneous scale estimate
446 m_mad = simultscale(residues);
447 }
448
449 // Set a minimum threshold for sigma
450 // (when sigma reaches the level of noise in the image)
451 if (m_mad < m_mad_min) {
452 m_mad = m_mad_min;
453 }
454
455 psiHuber(m_mad, norm_res, w);
456
457 m_mad_prev = m_mad;
458
459 return w;
460 }
461
simultscale(const vpColVector & x)462 double vpRobust::simultscale(const vpColVector &x)
463 {
464 unsigned int p = 6; // Number of parameters to be estimated.
465 unsigned int n = x.getRows();
466 double sigma2 = 0;
467 /* long */ double Expectation = 0;
468 /* long */ double Sum_chi = 0;
469
470 for (unsigned int i = 0; i < n; i++) {
471
472 double chiTmp = simult_chi_huber(x[i]);
473 #if defined(VISP_HAVE_FUNC_STD_ERFC)
474 Expectation += chiTmp * std::erfc(chiTmp);
475 #elif defined(VISP_HAVE_FUNC_ERFC)
476 Expectation += chiTmp * erfc(chiTmp);
477 #else
478 Expectation += chiTmp * (1 - erf(chiTmp));
479 #endif
480 Sum_chi += chiTmp;
481
482 #ifdef VP_DEBUG
483 #if VP_DEBUG_MODE == 3
484 {
485 #if defined(VISP_HAVE_FUNC_STD_ERFC)
486 std::cout << "erf = " << std::erfc(chiTmp) << std::endl;
487 #elif defined(VISP_HAVE_FUNC_ERFC)
488 std::cout << "erf = " << erfc(chiTmp) << std::endl;
489 #else
490 std::cout << "erf = " << (1 - erf(chiTmp)) << std::endl;
491 #endif
492 std::cout << "x[i] = " << x[i] << std::endl;
493 std::cout << "chi = " << chiTmp << std::endl;
494 std::cout << "Sum chi = " << chiTmp * vpMath::sqr(m_mad_prev) << std::endl;
495 #if defined(VISP_HAVE_FUNC_STD_ERFC)
496 std::cout << "Expectation = " << chiTmp * std::erfc(chiTmp) << std::endl;
497 #elif defined(VISP_HAVE_FUNC_ERFC)
498 std::cout << "Expectation = " << chiTmp * erfc(chiTmp) << std::endl;
499 #else
500 std::cout << "Expectation = " << chiTmp * (1 - erf(chiTmp)) << std::endl;
501 #endif
502 // getchar();
503 }
504 #endif
505 #endif
506 }
507
508 sigma2 = Sum_chi * vpMath::sqr(m_mad_prev) / ((n - p) * Expectation);
509
510 #ifdef VP_DEBUG
511 #if VP_DEBUG_MODE == 3
512 {
513 std::cout << "Expectation = " << Expectation << std::endl;
514 std::cout << "Sum chi = " << Sum_chi << std::endl;
515 std::cout << "MAD prev" << m_mad_prev << std::endl;
516 std::cout << "sig_out" << sqrt(fabs(sigma2)) << std::endl;
517 }
518 #endif
519 #endif
520
521 return sqrt(fabs(sigma2));
522 }
523
constrainedChi(vpRobustEstimatorType method,double x)524 double vpRobust::constrainedChi(vpRobustEstimatorType method, double x)
525 {
526 switch (method) {
527 case TUKEY:
528 return constrainedChiTukey(x);
529 case CAUCHY:
530 return constrainedChiCauchy(x);
531 case HUBER:
532 return constrainedChiHuber(x);
533 };
534
535 return -1;
536 }
537
constrainedChiTukey(double x)538 double vpRobust::constrainedChiTukey(double x)
539 {
540 double sct = 0;
541 double s = m_mad_prev;
542 // double epsillon=0.5;
543
544 if (fabs(x) <= 4.7 * m_mad_prev) {
545 double a = 4.7;
546 // sct =
547 // (vpMath::sqr(s*a-x)*vpMath::sqr(s*a+x)*vpMath::sqr(x))/(s*vpMath::sqr(vpMath::sqr(a*vpMath::sqr(s))));
548 sct = (vpMath::sqr(s * a) * x - s * vpMath::sqr(s * a) - x * vpMath::sqr(x)) *
549 (vpMath::sqr(s * a) * x + s * vpMath::sqr(s * a) - x * vpMath::sqr(x)) / s *
550 vpMath::sqr(vpMath::sqr(vpMath::sqr(s))) / vpMath::sqr(vpMath::sqr(a));
551 } else
552 sct = -1 / s;
553
554 return sct;
555 }
556
constrainedChiCauchy(double x)557 double vpRobust::constrainedChiCauchy(double x)
558 {
559 double sct = 0;
560 // double u = x/m_mad_prev;
561 double s = m_mad_prev;
562 double b = 2.3849;
563
564 sct = -1 * (vpMath::sqr(x) * b) / (s * (vpMath::sqr(s * b) + vpMath::sqr(x)));
565
566 return sct;
567 }
568
constrainedChiHuber(double x)569 double vpRobust::constrainedChiHuber(double x)
570 {
571 double sct = 0;
572 double u = x / m_mad_prev;
573 double c = 1.2107; // 1.345;
574
575 if (fabs(u) <= c)
576 sct = vpMath::sqr(u);
577 else
578 sct = vpMath::sqr(c);
579
580 return sct;
581 }
582
simult_chi_huber(double x)583 double vpRobust::simult_chi_huber(double x)
584 {
585 double sct = 0;
586 double u = x / m_mad_prev;
587 double c = 1.2107; // 1.345;
588
589 if (fabs(u) <= c) {
590 // sct = 0.5*vpMath::sqr(u);
591 sct = vpMath::sqr(u);
592 } else {
593 // sct = 0.5*vpMath::sqr(c);
594 sct = vpMath::sqr(c);
595 }
596
597 return sct;
598 }
599
600 #if !defined(VISP_HAVE_FUNC_ERFC) && !defined(VISP_HAVE_FUNC_STD_ERFC)
erf(double x)601 double vpRobust::erf(double x) { return x < 0.0 ? -gammp(0.5, x * x) : gammp(0.5, x * x); }
602
gammp(double a,double x)603 double vpRobust::gammp(double a, double x)
604 {
605 double gamser = 0., gammcf = 0., gln;
606
607 if (x < 0.0 || a <= 0.0)
608 std::cout << "Invalid arguments in routine GAMMP";
609 if (x < (a + 1.0)) {
610 gser(&gamser, a, x, &gln);
611 return gamser;
612 } else {
613 gcf(&gammcf, a, x, &gln);
614 return 1.0 - gammcf;
615 }
616 }
617
gser(double * gamser,double a,double x,double * gln)618 void vpRobust::gser(double *gamser, double a, double x, double *gln)
619 {
620 *gln = gammln(a);
621 if (x <= 0.0) {
622 if (x < 0.0)
623 std::cout << "x less than 0 in routine GSER";
624 *gamser = 0.0;
625 return;
626 } else {
627 double ap = a;
628 double sum = 1.0 / a;
629 double del = sum;
630 for (int n = 1; n <= vpITMAX; n++) {
631 ap += 1.0;
632 del *= x / ap;
633 sum += del;
634 if (fabs(del) < fabs(sum) * vpEPS) {
635 *gamser = sum * exp(-x + a * log(x) - (*gln));
636 return;
637 }
638 }
639 std::cout << "a too large, vpITMAX too small in routine GSER";
640 return;
641 }
642 }
643
gcf(double * gammcf,double a,double x,double * gln)644 void vpRobust::gcf(double *gammcf, double a, double x, double *gln)
645 {
646 double gold = 0.0, g, fac = 1.0, b1 = 1.0;
647 double b0 = 0.0, a1, a0 = 1.0;
648
649 *gln = gammln(a);
650 a1 = x;
651 for (int n = 1; n <= vpITMAX; n++) {
652 double an = (double)n;
653 double ana = an - a;
654 a0 = (a1 + a0 * ana) * fac;
655 b0 = (b1 + b0 * ana) * fac;
656 double anf = an * fac;
657 a1 = x * a0 + anf * a1;
658 b1 = x * b0 + anf * b1;
659 // if (a1)
660 if (std::fabs(a1) > std::numeric_limits<double>::epsilon()) {
661 fac = 1.0 / a1;
662 g = b1 * fac;
663 if (fabs((g - gold) / g) < vpEPS) {
664 *gammcf = exp(-x + a * log(x) - (*gln)) * g;
665 return;
666 }
667 gold = g;
668 }
669 }
670 std::cout << "a too large, vpITMAX too small in routine GCF";
671 }
672
gammln(double xx)673 double vpRobust::gammln(double xx)
674 {
675 double x, tmp, ser;
676 static double cof[6] = {76.18009173, -86.50532033, 24.01409822, -1.231739516, 0.120858003e-2, -0.536382e-5};
677
678 x = xx - 1.0;
679 tmp = x + 5.5;
680 tmp -= (x + 0.5) * log(tmp);
681 ser = 1.0;
682 for (int j = 0; j <= 5; j++) {
683 x += 1.0;
684 ser += cof[j] / x;
685 }
686 return -tmp + log(2.50662827465 * ser);
687 }
688 #endif
689
690 #undef vpITMAX
691 #undef vpEPS
692
693 #endif
694
695