Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRobust.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 * M-Estimator and various influence function.
33 *
34 * Authors:
35 * Andrew Comport
36 * Jean Laneurit
37 *
38*****************************************************************************/
39
44#include <algorithm> // std::swap
45#include <cmath> // std::fabs
46#include <limits> // numeric_limits
47#include <stdio.h>
48#include <stdlib.h>
49#include <string.h>
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
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
71vpRobust::vpRobust(const vpRobust &other) { *this = other; }
72
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)
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
114void 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// ===================================================================
137void 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
196void 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
223void 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
245void 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
262int vpRobust::partition(vpColVector &a, int l, int r)
263{
264 int i = l - 1;
265 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
289double vpRobust::select(vpColVector &a, int l, int r, int k)
290{
291 while (r > l) {
292 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
312vpRobust::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
327void 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
366double 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
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
462double 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
524double 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
538double 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 *
551 } else
552 sct = -1 / s;
553
554 return sct;
555}
556
557double 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
569double 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
583double 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)
601double vpRobust::erf(double x) { return x < 0.0 ? -gammp(0.5, x * x) : gammp(0.5, x * x); }
602
603double 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
618void 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
644void 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
673double 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
Type * data
Address of the first element of the data array.
Definition vpArray2D.h:144
unsigned int getRows() const
Definition vpArray2D.h:290
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
static double sqr(double x)
Definition vpMath.h:124
Contains an M-estimator and various influence function.
Definition vpRobust.h:83
vpRobustEstimatorType
Enumeration of influence functions.
Definition vpRobust.h:86
@ HUBER
Huber influence function.
Definition vpRobust.h:89
@ TUKEY
Tukey influence function.
Definition vpRobust.h:87
@ CAUCHY
Cauchy influence function.
Definition vpRobust.h:88
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition vpRobust.cpp:137
vpRobust & operator=(const vpRobust &other)
Definition vpRobust.cpp:76
vp_deprecated vpColVector simultMEstimator(vpColVector &residues)
Definition vpRobust.cpp:421
#define vpCDEBUG(level)
Definition vpDebug.h:506