Visual Servoing Platform  version 3.2.0 under development (2019-01-22)
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 <visp3/core/vpColVector.h>
45 #include <visp3/core/vpDebug.h>
46 #include <visp3/core/vpMath.h>
47 
48 #include <cmath> // std::fabs
49 #include <limits> // numeric_limits
50 #include <stdio.h>
51 #include <stdlib.h>
52 #include <string.h>
53 #include <visp3/core/vpRobust.h>
54 
55 #define vpITMAX 100
56 #define vpEPS 3.0e-7
57 #define vpCST 1
58 
59 // ===================================================================
65 vpRobust::vpRobust(unsigned int n_data)
66  : normres(), sorted_normres(), sorted_residues(), NoiseThreshold(0.0017), sig_prev(0), it(0), swap(0), size(n_data)
67 {
68  vpCDEBUG(2) << "vpRobust constructor reached" << std::endl;
69 
70  normres.resize(n_data);
71  sorted_normres.resize(n_data);
72  sorted_residues.resize(n_data);
73  // NoiseThreshold=0.0017; //Can not be more accurate than 1 pixel
74 }
75 
80  : normres(), sorted_normres(), sorted_residues(), NoiseThreshold(0.0017), sig_prev(0), it(0), swap(0), size(0)
81 {
82 }
83 
87 vpRobust::vpRobust(const vpRobust &other) { *this = other; }
88 
93 {
94  normres = other.normres;
95  sorted_normres = other.sorted_normres;
96  sorted_residues = other.sorted_residues;
97  NoiseThreshold = other.NoiseThreshold;
98  sig_prev = other.sig_prev;
99  it = other.it;
100  swap = other.swap;
101  size = other.size;
102  return *this;
103 }
104 
105 #ifdef VISP_HAVE_CPP11_COMPATIBILITY
106 
110 {
111  normres = std::move(other.normres);
112  sorted_normres = std::move(other.sorted_normres);
113  sorted_residues = std::move(other.sorted_residues);
114  NoiseThreshold = std::move(other.NoiseThreshold);
115  sig_prev = std::move(other.sig_prev);
116  it = std::move(other.it);
117  swap = std::move(other.swap);
118  size = std::move(other.size);
119  return *this;
120 }
121 #endif
122 
128 void vpRobust::resize(unsigned int n_data)
129 {
130 
131  if (n_data != size) {
132  normres.resize(n_data);
133  sorted_normres.resize(n_data);
134  sorted_residues.resize(n_data);
135  size = n_data;
136  }
137 }
138 
139 // ===================================================================
175 // ===================================================================
176 void vpRobust::MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
177 {
178 
179  double med = 0; // median
180  double normmedian = 0; // Normalized median
181  double sigma = 0; // Standard Deviation
182 
183  // resize vector only if the size of residue vector has changed
184  unsigned int n_data = residues.getRows();
185  resize(n_data);
186 
187  sorted_residues = residues;
188 
189  unsigned int ind_med = (unsigned int)(ceil(n_data / 2.0)) - 1;
190 
191  // Calculate median
192  med = select(sorted_residues, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/);
193  // residualMedian = med ;
194 
195  // Normalize residues
196  for (unsigned int i = 0; i < n_data; i++) {
197  normres[i] = (fabs(residues[i] - med));
198  sorted_normres[i] = (fabs(sorted_residues[i] - med));
199  }
200 
201  // Calculate MAD
202  normmedian = select(sorted_normres, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/);
203  // normalizedResidualMedian = normmedian ;
204  // 1.48 keeps scale estimate consistent for a normal probability dist.
205  sigma = 1.4826 * normmedian; // median Absolute Deviation
206 
207  // Set a minimum threshold for sigma
208  // (when sigma reaches the level of noise in the image)
209  if (sigma < NoiseThreshold) {
210  sigma = NoiseThreshold;
211  }
212 
213  switch (method) {
214  case TUKEY: {
215  psiTukey(sigma, normres, weights);
216 
217  vpCDEBUG(2) << "Tukey's function computed" << std::endl;
218  break;
219  }
220  case CAUCHY: {
221  psiCauchy(sigma, normres, weights);
222  break;
223  }
224  case HUBER: {
225  psiHuber(sigma, normres, weights);
226  break;
227  }
228  }
229 }
230 
231 void vpRobust::MEstimator(const vpRobustEstimatorType method, const vpColVector &residues,
232  const vpColVector &all_residues, vpColVector &weights)
233 {
234 
235  double normmedian = 0; // Normalized median
236  double sigma = 0; // Standard Deviation
237 
238  unsigned int n_all_data = all_residues.getRows();
239  vpColVector all_normres(n_all_data);
240 
241  // compute median with the residues vector, return all_normres which are the
242  // normalized all_residues vector.
243  normmedian = computeNormalizedMedian(all_normres, residues, all_residues, weights);
244 
245  // 1.48 keeps scale estimate consistent for a normal probability dist.
246  sigma = 1.4826 * normmedian; // Median Absolute Deviation
247 
248  // Set a minimum threshold for sigma
249  // (when sigma reaches the level of noise in the image)
250  if (sigma < NoiseThreshold) {
251  sigma = NoiseThreshold;
252  }
253 
254  switch (method) {
255  case TUKEY: {
256  psiTukey(sigma, all_normres, weights);
257 
258  vpCDEBUG(2) << "Tukey's function computed" << std::endl;
259  break;
260  }
261  case CAUCHY: {
262  psiCauchy(sigma, all_normres, weights);
263  break;
264  }
265  case HUBER: {
266  psiHuber(sigma, all_normres, weights);
267  break;
268  }
269  };
270 }
271 
272 double vpRobust::computeNormalizedMedian(vpColVector &all_normres, const vpColVector &residues,
273  const vpColVector &all_residues, const vpColVector &weights)
274 {
275  double med = 0;
276  double normmedian = 0;
277 
278  unsigned int n_all_data = all_residues.getRows();
279  unsigned int n_data = residues.getRows();
280 
281  // resize vector only if the size of residue vector has changed
282  resize(n_data);
283 
284  sorted_residues = residues;
285  vpColVector no_null_weight_residues;
286  no_null_weight_residues.resize(n_data);
287 
288  // all_normres.resize(n_all_data); // Normalized Residue
289  // vpColVector sorted_normres(n_data); // Normalized Residue
290  // vpColVector sorted_residues = residues;
291  // vpColVector sorted_residues;
292 
293  unsigned int index = 0;
294  for (unsigned int j = 0; j < n_data; j++) {
295  // if(weights[j]!=0)
296  if (std::fabs(weights[j]) > std::numeric_limits<double>::epsilon()) {
297  no_null_weight_residues[index] = residues[j];
298  index++;
299  }
300  }
301  sorted_residues.resize(index);
302  memcpy(sorted_residues.data, no_null_weight_residues.data, index * sizeof(double));
303  n_data = index;
304 
305  vpCDEBUG(2) << "vpRobust MEstimator reached. No. data = " << n_data << std::endl;
306 
307  // Calculate Median
308  // Be careful to not use the rejected residues for the
309  // calculation.
310 
311  unsigned int ind_med = (unsigned int)(ceil(n_data / 2.0)) - 1;
312  med = select(sorted_residues, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/);
313 
314  unsigned int i;
315  // Normalize residues
316  for (i = 0; i < n_all_data; i++) {
317  all_normres[i] = (fabs(all_residues[i] - med));
318  }
319 
320  for (i = 0; i < n_data; i++) {
321  sorted_normres[i] = (fabs(sorted_residues[i] - med));
322  }
323  // MAD calculated only on first iteration
324 
325  // normmedian = Median(normres, weights);
326  // normmedian = Median(normres);
327  normmedian = select(sorted_normres, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/);
328 
329  return normmedian;
330 }
331 
332 // ===================================================================
340 // ===================================================================
342 {
343 
344  double med = 0; // Median
345  double sigma = 0; // Standard Deviation
346 
347  unsigned int n_data = residues.getRows();
348  vpColVector norm_res(n_data); // Normalized Residue
349  vpColVector w(n_data);
350 
351  vpCDEBUG(2) << "vpRobust MEstimator reached. No. data = " << n_data << std::endl;
352 
353  // Calculate Median
354  unsigned int ind_med = (unsigned int)(ceil(n_data / 2.0)) - 1;
355  med = select(residues, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/);
356 
357  // Normalize residues
358  for (unsigned int i = 0; i < n_data; i++)
359  norm_res[i] = (fabs(residues[i] - med));
360 
361  // Check for various methods.
362  // For Huber compute Simultaneous scale estimate
363  // For Others use MAD calculated on first iteration
364  if (it == 0) {
365  double normmedian = select(norm_res, 0, (int)n_data - 1, (int)ind_med /*(int)n_data/2*/); // Normalized Median
366  // 1.48 keeps scale estimate consistent for a normal probability dist.
367  sigma = 1.4826 * normmedian; // Median Absolute Deviation
368  } else {
369  // compute simultaneous scale estimate
370  sigma = simultscale(residues);
371  }
372 
373  // Set a minimum threshold for sigma
374  // (when sigma reaches the level of noise in the image)
375  if (sigma < NoiseThreshold) {
376  sigma = NoiseThreshold;
377  }
378 
379  vpCDEBUG(2) << "MAD and C computed" << std::endl;
380 
381  psiHuber(sigma, norm_res, w);
382 
383  sig_prev = sigma;
384 
385  return w;
386 }
387 
388 double vpRobust::simultscale(vpColVector &x)
389 {
390  unsigned int p = 6; // Number of parameters to be estimated.
391  unsigned int n = x.getRows();
392  double sigma2 = 0;
393  /* long */ double Expectation = 0;
394  /* long */ double Sum_chi = 0;
395 
396  for (unsigned int i = 0; i < n; i++) {
397 
398  double chiTmp = simult_chi_huber(x[i]);
399 #if defined(VISP_HAVE_FUNC_STD_ERFC)
400  Expectation += chiTmp * std::erfc(chiTmp);
401 #elif defined(VISP_HAVE_FUNC_ERFC)
402  Expectation += chiTmp * erfc(chiTmp);
403 #else
404  Expectation += chiTmp * (1 - erf(chiTmp));
405 #endif
406  Sum_chi += chiTmp;
407 
408 #ifdef VP_DEBUG
409 #if VP_DEBUG_MODE == 3
410  {
411 #if defined(VISP_HAVE_FUNC_STD_ERFC)
412  std::cout << "erf = " << std::erfc(chiTmp) << std::endl;
413 #elif defined(VISP_HAVE_FUNC_ERFC)
414  std::cout << "erf = " << erfc(chiTmp) << std::endl;
415 #else
416  std::cout << "erf = " << (1 - erf(chiTmp)) << std::endl;
417 #endif
418  std::cout << "x[i] = " << x[i] << std::endl;
419  std::cout << "chi = " << chiTmp << std::endl;
420  std::cout << "Sum chi = " << chiTmp * vpMath::sqr(sig_prev) << std::endl;
421 #if defined(VISP_HAVE_FUNC_STD_ERFC)
422  std::cout << "Expectation = " << chiTmp * std::erfc(chiTmp) << std::endl;
423 #elif defined(VISP_HAVE_FUNC_ERFC)
424  std::cout << "Expectation = " << chiTmp * erfc(chiTmp) << std::endl;
425 #else
426  std::cout << "Expectation = " << chiTmp * (1 - erf(chiTmp)) << std::endl;
427 #endif
428  // getchar();
429  }
430 #endif
431 #endif
432  }
433 
434  sigma2 = Sum_chi * vpMath::sqr(sig_prev) / ((n - p) * Expectation);
435 
436 #ifdef VP_DEBUG
437 #if VP_DEBUG_MODE == 3
438  {
439  std::cout << "Expectation = " << Expectation << std::endl;
440  std::cout << "Sum chi = " << Sum_chi << std::endl;
441  std::cout << "sig_prev" << sig_prev << std::endl;
442  std::cout << "sig_out" << sqrt(fabs(sigma2)) << std::endl;
443  }
444 #endif
445 #endif
446 
447  return sqrt(fabs(sigma2));
448 }
449 
450 double vpRobust::constrainedChi(vpRobustEstimatorType method, double x)
451 {
452  switch (method) {
453  case TUKEY:
454  return constrainedChiTukey(x);
455  case CAUCHY:
456  return constrainedChiCauchy(x);
457  case HUBER:
458  return constrainedChiHuber(x);
459  };
460 
461  return -1;
462 }
463 
464 double vpRobust::constrainedChiTukey(double x)
465 {
466  double sct = 0;
467  double s = sig_prev;
468  // double epsillon=0.5;
469 
470  if (fabs(x) <= 4.7 * sig_prev) {
471  double a = 4.7;
472  // sct =
473  // (vpMath::sqr(s*a-x)*vpMath::sqr(s*a+x)*vpMath::sqr(x))/(s*vpMath::sqr(vpMath::sqr(a*vpMath::sqr(s))));
474  sct = (vpMath::sqr(s * a) * x - s * vpMath::sqr(s * a) - x * vpMath::sqr(x)) *
475  (vpMath::sqr(s * a) * x + s * vpMath::sqr(s * a) - x * vpMath::sqr(x)) / s *
477  } else
478  sct = -1 / s;
479 
480  return sct;
481 }
482 
483 double vpRobust::constrainedChiCauchy(double x)
484 {
485  double sct = 0;
486  // double u = x/sig_prev;
487  double s = sig_prev;
488  double b = 2.3849;
489 
490  sct = -1 * (vpMath::sqr(x) * b) / (s * (vpMath::sqr(s * b) + vpMath::sqr(x)));
491 
492  return sct;
493 }
494 
495 double vpRobust::constrainedChiHuber(double x)
496 {
497  double sct = 0;
498  double u = x / sig_prev;
499  double c = 1.2107; // 1.345;
500 
501  if (fabs(u) <= c)
502  sct = vpMath::sqr(u);
503  else
504  sct = vpMath::sqr(c);
505 
506  return sct;
507 }
508 
509 double vpRobust::simult_chi_huber(double x)
510 {
511  double sct = 0;
512  double u = x / sig_prev;
513  double c = 1.2107; // 1.345;
514 
515  if (fabs(u) <= c) {
516  // sct = 0.5*vpMath::sqr(u);
517  sct = vpMath::sqr(u);
518  } else {
519  // sct = 0.5*vpMath::sqr(c);
520  sct = vpMath::sqr(c);
521  }
522 
523  return sct;
524 }
525 
534 void vpRobust::psiTukey(double sig, vpColVector &x, vpColVector &weights)
535 {
536 
537  unsigned int n_data = x.getRows();
538  double cst_const = vpCST * 4.6851;
539 
540  for (unsigned int i = 0; i < n_data; i++) {
541  // if(sig==0 && weights[i]!=0)
542  if (std::fabs(sig) <= std::numeric_limits<double>::epsilon() &&
543  std::fabs(weights[i]) > std::numeric_limits<double>::epsilon()) {
544  weights[i] = 1;
545  continue;
546  }
547 
548  double xi_sig = x[i] / sig;
549 
550  // if((fabs(xi_sig)<=(cst_const)) && weights[i]!=0)
551  if ((std::fabs(xi_sig) <= (cst_const)) && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon()) {
552  weights[i] = vpMath::sqr(1 - vpMath::sqr(xi_sig / cst_const));
553  // w[i] = vpMath::sqr(1-vpMath::sqr(x[i]/sig/4.7));
554  } else {
555  // Outlier - could resize list of points tracked here?
556  weights[i] = 0;
557  }
558  }
559 }
560 
568 void vpRobust::psiHuber(double sig, vpColVector &x, vpColVector &weights)
569 {
570  double c = 1.2107; // 1.345;
571  unsigned int n_data = x.getRows();
572 
573  for (unsigned int i = 0; i < n_data; i++) {
574  // if(weights[i]!=0)
575  if (std::fabs(weights[i]) > std::numeric_limits<double>::epsilon()) {
576  double xi_sig = x[i] / sig;
577  if (fabs(xi_sig) <= c)
578  weights[i] = 1;
579  else
580  weights[i] = c / fabs(xi_sig);
581  }
582  }
583 }
584 
593 void vpRobust::psiCauchy(double sig, vpColVector &x, vpColVector &weights)
594 {
595  unsigned int n_data = x.getRows();
596  double const_sig = 2.3849 * sig;
597 
598  // Calculate Cauchy's equation
599  for (unsigned int i = 0; i < n_data; i++) {
600  weights[i] = 1 / (1 + vpMath::sqr(x[i] / (const_sig)));
601 
602  // If one coordinate is an outlier the other is too!
603  // w[i] < 0.01 is a threshold to be set
604  /*if(w[i] < 0.01)
605  {
606  if(i%2 == 0)
607  {
608  w[i+1] = w[i];
609  i++;
610  }
611  else
612  w[i-1] = w[i];
613  }*/
614  }
615 }
616 
623 int vpRobust::partition(vpColVector &a, int l, int r)
624 {
625  int i = l - 1;
626  int j = r;
627  double v = a[(unsigned int)r];
628 
629  for (;;) {
630  while (a[(unsigned int)++i] < v)
631  ;
632  while (v < a[(unsigned int)--j])
633  if (j == l)
634  break;
635  if (i >= j)
636  break;
637  exch(a[(unsigned int)i], a[(unsigned int)j]);
638  }
639  exch(a[(unsigned int)i], a[(unsigned int)r]);
640  return i;
641 }
642 
650 double vpRobust::select(vpColVector &a, int l, int r, int k)
651 {
652  while (r > l) {
653  int i = partition(a, l, r);
654  if (i >= k)
655  r = i - 1;
656  if (i <= k)
657  l = i + 1;
658  }
659  return a[(unsigned int)k];
660 }
661 
662 #if !defined(VISP_HAVE_FUNC_ERFC) && !defined(VISP_HAVE_FUNC_STD_ERFC)
663 double vpRobust::erf(double x) { return x < 0.0 ? -gammp(0.5, x * x) : gammp(0.5, x * x); }
664 
665 double vpRobust::gammp(double a, double x)
666 {
667  double gamser = 0., gammcf = 0., gln;
668 
669  if (x < 0.0 || a <= 0.0)
670  std::cout << "Invalid arguments in routine GAMMP";
671  if (x < (a + 1.0)) {
672  gser(&gamser, a, x, &gln);
673  return gamser;
674  } else {
675  gcf(&gammcf, a, x, &gln);
676  return 1.0 - gammcf;
677  }
678 }
679 
680 void vpRobust::gser(double *gamser, double a, double x, double *gln)
681 {
682  *gln = gammln(a);
683  if (x <= 0.0) {
684  if (x < 0.0)
685  std::cout << "x less than 0 in routine GSER";
686  *gamser = 0.0;
687  return;
688  } else {
689  double ap = a;
690  double sum = 1.0 / a;
691  double del = sum;
692  for (int n = 1; n <= vpITMAX; n++) {
693  ap += 1.0;
694  del *= x / ap;
695  sum += del;
696  if (fabs(del) < fabs(sum) * vpEPS) {
697  *gamser = sum * exp(-x + a * log(x) - (*gln));
698  return;
699  }
700  }
701  std::cout << "a too large, vpITMAX too small in routine GSER";
702  return;
703  }
704 }
705 
706 void vpRobust::gcf(double *gammcf, double a, double x, double *gln)
707 {
708  double gold = 0.0, g, fac = 1.0, b1 = 1.0;
709  double b0 = 0.0, a1, a0 = 1.0;
710 
711  *gln = gammln(a);
712  a1 = x;
713  for (int n = 1; n <= vpITMAX; n++) {
714  double an = (double)n;
715  double ana = an - a;
716  a0 = (a1 + a0 * ana) * fac;
717  b0 = (b1 + b0 * ana) * fac;
718  double anf = an * fac;
719  a1 = x * a0 + anf * a1;
720  b1 = x * b0 + anf * b1;
721  // if (a1)
722  if (std::fabs(a1) > std::numeric_limits<double>::epsilon()) {
723  fac = 1.0 / a1;
724  g = b1 * fac;
725  if (fabs((g - gold) / g) < vpEPS) {
726  *gammcf = exp(-x + a * log(x) - (*gln)) * g;
727  return;
728  }
729  gold = g;
730  }
731  }
732  std::cout << "a too large, vpITMAX too small in routine GCF";
733 }
734 
735 double vpRobust::gammln(double xx)
736 {
737  double x, tmp, ser;
738  static double cof[6] = {76.18009173, -86.50532033, 24.01409822, -1.231739516, 0.120858003e-2, -0.536382e-5};
739 
740  x = xx - 1.0;
741  tmp = x + 5.5;
742  tmp -= (x + 0.5) * log(tmp);
743  ser = 1.0;
744  for (int j = 0; j <= 5; j++) {
745  x += 1.0;
746  ser += cof[j] / x;
747  }
748  return -tmp + log(2.50662827465 * ser);
749 }
750 #endif
751 
752 // double
753 // vpRobust::median(const vpColVector &v)
754 // {
755 // int i,j;
756 // int inf, sup;
757 // int n = v.getRows() ;
758 // vpColVector infsup(n) ;
759 // vpColVector eq(n) ;
760 //
761 // for (i=0;i<n;i++)
762 // {
763 // // We compute the number of elements superior to the current value
764 // (sup)
765 // // the number of elements inferior (inf) to the current value and
766 // // the number of elements equal to the current value (eq)
767 // inf = sup = 0;
768 // for (j=0;j<n;j++)
769 // {
770 // if (i != j)
771 // {
772 // if (v[i] <= v[j]) inf++;
773 // if (v[i] >= v[j]) sup++;
774 // if (v[i] == v[j]) eq[i]++;
775 // }
776 // }
777 // // We compute then difference between inf and sup
778 // // the median should be for |inf-sup| = 0 (1 if an even number of
779 // element)
780 // // which means that there are the same number of element in the array
781 // // that are greater and smaller that this value.
782 // infsup[i] = abs(inf-sup);
783 // }
784 //
785 // // seek for the smaller value of |inf-sup| (should be 0 or 1)
786 // int imin = 0 ; // index of the median in the array
787 // //double eqmax = 0 ; // count of equal values
788 // // min cannot be greater than the number of element
789 // double min = n;
790 //
791 // // number of medians
792 // int mediancount = 0;
793 // // array of medians
794 // int *medianindex = new int[n];
795 //
796 // for (i=0; i<n; i++)
797 // {
798 // if(infsup[i] < min)
799 // {
800 // min = infsup[i];
801 // imin = i ;
802 //
803 // //reset count of median values
804 // mediancount=0;
805 // medianindex[mediancount]=i;
806 // }
807 // else if(infsup[i]==min) //If there is another median
808 // {
809 // mediancount++;
810 // medianindex[mediancount]=i;
811 // }
812 // }
813 //
814 // // Choose smalest data to be the median
815 // /*for(i=0; i<mediancount+1; i++)
816 // {
817 // //Choose the value with the greatest count
818 // if(eq[medianindex[i]] > eqmax)
819 // {
820 // eqmax = eq[medianindex[i]];
821 // imin = medianindex[i];
822 // }
823 // //If we have identical counts
824 // // Choose smalest data to be the median
825 // //if(v[medianindex[i]] < v[imin])
826 // // imin = medianindex[i];
827 // }*/
828 //
829 // // return the median
830 // delete []medianindex;
831 // return(v[imin]);
832 // }
833 //
834 // // Calculate median only for the residues which have
835 // // not be rejected. i.e. weight=0
836 // double
837 // vpRobust::median(const vpColVector &v, vpColVector &weights)
838 // {
839 // int i,j;
840 // int inf, sup;
841 // int n = v.getRows() ;
842 // vpColVector infsup(n) ;
843 // vpColVector eq(n) ;
844 //
845 // for (i=0;i<n;i++)
846 // {
847 // if(weights[i]!=0)
848 // {
849 // // We compute the number of elements superior to the current value
850 // (sup)
851 // // the number of elements inferior (inf) to the current value and
852 // // the number of elements equal to the current value (eq)
853 // inf = sup = 0;
854 // for (j=0;j<n;j++)
855 // {
856 // if (weights[j]!=0 && i!=j)
857 // {
858 // if (v[i] <= v[j]) inf++;
859 // if (v[i] >= v[j]) sup++;
860 // if (v[i] == v[j]) eq[i]++;
861 // }
862 // }
863 // // We compute then difference between inf and sup
864 // // the median should be for |inf-sup| = 0 (1 if an even number of
865 // element)
866 // // which means that there are the same number of element in the array
867 // // that are greater and smaller that this value.
868 // infsup[i] = abs(inf-sup);
869 // }
870 // }
871 //
872 // // seek for the smaller value of |inf-sup| (should be 0 or 1)
873 // int imin = 0 ; // index of the median in the array
874 // //double eqmax = 0 ; // count of equal values
875 // // min cannot be greater than the number of element
876 // double min = n;
877 //
878 // for (i=0; i<n; i++)
879 // {
880 // if(weights[i]!=0)
881 // {
882 // if(infsup[i] < min)
883 // {
884 // min = infsup[i];
885 // imin = i ;
886 // }
887 // }
888 // }
889 //
890 // // return the median
891 // return(v[imin]);
892 // }
893 
894 #undef vpITMAX
895 #undef vpEPS
896 #undef vpCST
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Definition: vpRobust.cpp:176
vpRobust()
Definition: vpRobust.cpp:79
Type * data
Address of the first element of the data array.
Definition: vpArray2D.h:84
vpColVector simultMEstimator(vpColVector &residues)
Simult Mestimator.
Definition: vpRobust.cpp:341
static double sqr(double x)
Definition: vpMath.h:108
#define vpCDEBUG(level)
Definition: vpDebug.h:511
vpRobust & operator=(const vpRobust &other)
Definition: vpRobust.cpp:92
unsigned int getRows() const
Definition: vpArray2D.h:156
Implementation of column vector and the associated operations.
Definition: vpColVector.h:72
Contains an M-Estimator and various influence function.
Definition: vpRobust.h:58
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:128
vpRobustEstimatorType
Enumeration of influence functions.
Definition: vpRobust.h:63
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:244