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