Visual Servoing Platform  version 3.5.0 under development (2022-02-15)
vpRobust.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  * 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 
71 vpRobust::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)
92 
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 
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 // ===================================================================
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 
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 
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 
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 
262 int 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 
289 double 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 
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 
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 
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 
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 
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 
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 
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 *
551  } else
552  sct = -1 / s;
553 
554  return sct;
555 }
556 
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 
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 
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)
601 double vpRobust::erf(double x) { return x < 0.0 ? -gammp(0.5, x * x) : gammp(0.5, x * x); }
602 
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 
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 
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 
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
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Definition: vpRobust.cpp:137
vpRobust()
Definition: vpRobust.cpp:59
unsigned int getRows() const
Definition: vpArray2D.h:289
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
Huber influence function.
Definition: vpRobust.h:95
vp_deprecated vpColVector simultMEstimator(vpColVector &residues)
Definition: vpRobust.cpp:421
static double sqr(double x)
Definition: vpMath.h:116
#define vpCDEBUG(level)
Definition: vpDebug.h:511
vpRobust & operator=(const vpRobust &other)
Definition: vpRobust.cpp:76
void resize(unsigned int i, bool flagNullify=true)
Definition: vpColVector.h:310
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
Contains an M-estimator and various influence function.
Definition: vpRobust.h:88
Tukey influence function.
Definition: vpRobust.h:93
Cauchy influence function.
Definition: vpRobust.h:94
vpRobustEstimatorType
Enumeration of influence functions.
Definition: vpRobust.h:92