ViSP  2.10.0
vpRobust.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRobust.cpp 5126 2015-01-05 22:07:11Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * M-Estimator and various influence function.
36  *
37  * Authors:
38  * Andrew Comport
39  * Jean Laneurit
40  *
41  *****************************************************************************/
42 
50 #include <visp/vpDebug.h>
51 #include <visp/vpColVector.h>
52 #include <visp/vpMath.h>
53 
54 #include <visp/vpRobust.h>
55 #include <stdio.h>
56 #include <string.h>
57 #include <stdlib.h>
58 #include <cmath> // std::fabs
59 #include <limits> // numeric_limits
60 
61 #define vpITMAX 100
62 #define vpEPS 3.0e-7
63 #define vpCST 1
64 
65 
66 // ===================================================================
72 vpRobust::vpRobust(unsigned int n_data)
73  : normres(), sorted_normres(), sorted_residues(), NoiseThreshold(0.0017), sig_prev(0), it(0), swap(0), size(n_data)
74 {
75  vpCDEBUG(2) << "vpRobust constructor reached" << std::endl;
76 
77  normres.resize(n_data);
78  sorted_normres.resize(n_data);
79  sorted_residues.resize(n_data);
80  // NoiseThreshold=0.0017; //Can not be more accurate than 1 pixel
81 }
82 
88 void vpRobust::resize(unsigned int n_data){
89 
90  if(n_data!=size){
91  normres.resize(n_data);
92  sorted_normres.resize(n_data);
93  sorted_residues.resize(n_data);
94  size=n_data;
95  }
96 
97 }
98 
99 // ===================================================================
135 // ===================================================================
137  const vpColVector &residues,
138  vpColVector &weights)
139 {
140 
141  double med=0; // median
142  double normmedian=0; // Normalized median
143  double sigma=0;// Standard Deviation
144 
145  // resize vector only if the size of residue vector has changed
146  unsigned int n_data = residues.getRows();
147  resize(n_data);
148 
149  sorted_residues = residues;
150 
151  unsigned int ind_med = (unsigned int)(ceil(n_data/2.0))-1;
152 
153  // Calculate median
154  med = select(sorted_residues, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
155  //residualMedian = med ;
156 
157  // Normalize residues
158  for(unsigned int i=0; i<n_data; i++)
159  {
160  normres[i] = (fabs(residues[i]- med));
161  sorted_normres[i] = (fabs(sorted_residues[i]- med));
162 
163  }
164 
165  // Calculate MAD
166  normmedian = select(sorted_normres, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
167  //normalizedResidualMedian = normmedian ;
168  // 1.48 keeps scale estimate consistent for a normal probability dist.
169  sigma = 1.4826*normmedian; // median Absolute Deviation
170 
171  // Set a minimum threshold for sigma
172  // (when sigma reaches the level of noise in the image)
173  if(sigma < NoiseThreshold)
174  {
175  sigma= NoiseThreshold;
176  }
177 
178  switch (method)
179  {
180  case TUKEY :
181  {
182  psiTukey(sigma, normres,weights);
183 
184  vpCDEBUG(2) << "Tukey's function computed" << std::endl;
185  break ;
186 
187  }
188  case CAUCHY :
189  {
190  psiCauchy(sigma, normres,weights);
191  break ;
192  }
193  /* case MCLURE :
194  {
195  psiMcLure(sigma, normres);
196  break ;
197  }*/
198  case HUBER :
199  {
200  psiHuber(sigma, normres,weights);
201  break ;
202  }
203  }
204 }
205 
206 
207 
209  const vpColVector &residues,
210  const vpColVector& all_residues,
211  vpColVector &weights)
212 {
213 
214 
215  double normmedian=0; // Normalized median
216  double sigma=0;// Standard Deviation
217 
218  unsigned int n_all_data = all_residues.getRows();
219  vpColVector all_normres(n_all_data);
220 
221  // compute median with the residues vector, return all_normres which are the normalized all_residues vector.
222  normmedian = computeNormalizedMedian(all_normres,residues,all_residues,weights);
223 
224 
225  // 1.48 keeps scale estimate consistent for a normal probability dist.
226  sigma = 1.4826*normmedian; // Median Absolute Deviation
227 
228  // Set a minimum threshold for sigma
229  // (when sigma reaches the level of noise in the image)
230  if(sigma < NoiseThreshold)
231  {
232  sigma= NoiseThreshold;
233  }
234 
235 
236  switch (method)
237  {
238  case TUKEY :
239  {
240  psiTukey(sigma, all_normres,weights);
241 
242  vpCDEBUG(2) << "Tukey's function computed" << std::endl;
243  break ;
244 
245  }
246  case CAUCHY :
247  {
248  psiCauchy(sigma, all_normres,weights);
249  break ;
250  }
251  /* case MCLURE :
252  {
253  psiMcLure(sigma, all_normres);
254  break ;
255  }*/
256  case HUBER :
257  {
258  psiHuber(sigma, all_normres,weights);
259  break ;
260  }
261 
262 
263  };
264 }
265 
266 
267 
268 double vpRobust::computeNormalizedMedian(vpColVector &all_normres,
269  const vpColVector &residues,
270  const vpColVector &all_residues,
271  const vpColVector & weights
272  )
273 {
274  double med=0;
275  double normmedian=0;
276 
277  unsigned int n_all_data = all_residues.getRows();
278  unsigned int n_data = residues.getRows();
279 
280  // resize vector only if the size of residue vector has changed
281  resize(n_data);
282 
283  sorted_residues = residues;
284  vpColVector no_null_weight_residues;
285  no_null_weight_residues.resize(n_data);
286 
287  //all_normres.resize(n_all_data); // Normalized Residue
288  //vpColVector sorted_normres(n_data); // Normalized Residue
289  //vpColVector sorted_residues = residues;
290  //vpColVector sorted_residues;
291 
292 
293  unsigned int index =0;
294  for(unsigned int j=0;j<n_data;j++)
295  {
296  //if(weights[j]!=0)
297  if(std::fabs(weights[j]) > std::numeric_limits<double>::epsilon())
298  {
299  no_null_weight_residues[index]=residues[j];
300  index++;
301  }
302  }
303  sorted_residues.resize(index);
304  memcpy(sorted_residues.data,no_null_weight_residues.data,index*sizeof(double));
305  n_data=index;
306 
307  vpCDEBUG(2) << "vpRobust MEstimator reached. No. data = " << n_data
308  << std::endl;
309 
310  // Calculate Median
311  // Be careful to not use the rejected residues for the
312  // calculation.
313 
314  unsigned int ind_med = (unsigned int)(ceil(n_data/2.0))-1;
315  med = select(sorted_residues, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
316 
317  unsigned int i;
318  // Normalize residues
319  for(i=0; i<n_all_data; i++)
320  {
321  all_normres[i] = (fabs(all_residues[i]- med));
322  }
323 
324  for(i=0; i<n_data; i++)
325  {
326  sorted_normres[i] = (fabs(sorted_residues[i]- med));
327  }
328  // MAD calculated only on first iteration
329 
330  //normmedian = Median(normres, weights);
331  //normmedian = Median(normres);
332  normmedian = select(sorted_normres, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
333 
334  return normmedian;
335 }
336 
337 // ===================================================================
345 // ===================================================================
348 {
349 
350  double med=0; // Median
351  double normmedian=0; // Normalized Median
352  double sigma=0; // Standard Deviation
353 
354  unsigned int n_data = residues.getRows();
355  vpColVector norm_res(n_data); // Normalized Residue
356  vpColVector w(n_data);
357 
358  vpCDEBUG(2) << "vpRobust MEstimator reached. No. data = " << n_data
359  << std::endl;
360 
361  // Calculate Median
362  unsigned int ind_med = (unsigned int)(ceil(n_data/2.0))-1;
363  med = select(residues, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
364 
365  // Normalize residues
366  for(unsigned int i=0; i<n_data; i++)
367  norm_res[i] = (fabs(residues[i]- med));
368 
369  // Check for various methods.
370  // For Huber compute Simultaneous scale estimate
371  // For Others use MAD calculated on first iteration
372  if(it==0)
373  {
374  normmedian = select(norm_res, 0, (int)n_data-1, (int)ind_med/*(int)n_data/2*/);
375  // 1.48 keeps scale estimate consistent for a normal probability dist.
376  sigma = 1.4826*normmedian; // Median Absolute Deviation
377  }
378  else
379  {
380  // compute simultaneous scale estimate
381  sigma = simultscale(residues);
382  }
383 
384  // Set a minimum threshold for sigma
385  // (when sigma reaches the level of noise in the image)
386  if(sigma < NoiseThreshold)
387  {
388  sigma= NoiseThreshold;
389  }
390 
391  vpCDEBUG(2) << "MAD and C computed" << std::endl;
392 
393  psiHuber(sigma, norm_res,w);
394 
395  sig_prev = sigma;
396 
397  return w;
398 }
399 
400 double
401 vpRobust::scale(vpRobustEstimatorType method, vpColVector &x)
402 {
403  unsigned int p = 6; //Number of parameters to be estimated.
404  unsigned int n = x.getRows();
405  double sigma2=0;
406  /* long */ double Expectation=0;
407  /* long */ double Sum_chi=0;
408  /* long*/ double chiTmp =0;
409 
410  for(unsigned int i=0; i<n; i++)
411  {
412 
413  chiTmp = constrainedChi(method, x[i]);
414  Expectation += chiTmp*(1-erf(chiTmp));
415  Sum_chi += chiTmp;
416 
417 #ifdef VP_DEBUG
418 #if VP_DEBUG_MODE == 3
419  {
420  std::cout << "erf = " << 1-erf(chiTmp) << std::endl;
421  std::cout << "x[i] = " << x[i] <<std::endl;
422  std::cout << "chi = " << chiTmp << std::endl;
423  std::cout << "Sum chi = " << chiTmp*vpMath::sqr(sig_prev) << std::endl;
424  std::cout << "Expectation = " << chiTmp*(1-erf(chiTmp)) << std::endl;
425  //getchar();
426  }
427 #endif
428 #endif
429  }
430 
431 
432  sigma2 = Sum_chi*vpMath::sqr(sig_prev)/((n-p)*Expectation);
433 
434 #ifdef VP_DEBUG
435 #if VP_DEBUG_MODE == 3
436  {
437  std::cout << "Expectation = " << Expectation << std::endl;
438  std::cout << "Sum chi = " << Sum_chi << std::endl;
439  std::cout << "sig_prev" << sig_prev << std::endl;
440  std::cout << "sig_out" << sqrt(fabs(sigma2)) << std::endl;
441  }
442 #endif
443 #endif
444 
445  return sqrt(fabs(sigma2));
446 
447 }
448 
449 
450 double
451 vpRobust::simultscale(vpColVector &x)
452 {
453  unsigned int p = 6; //Number of parameters to be estimated.
454  unsigned int n = x.getRows();
455  double sigma2=0;
456  /* long */ double Expectation=0;
457  /* long */ double Sum_chi=0;
458  /* long */ double chiTmp =0;
459 
460  for(unsigned int i=0; i<n; i++)
461  {
462 
463  chiTmp = simult_chi_huber(x[i]);
464  Expectation += chiTmp*(1-erf(chiTmp));
465  Sum_chi += chiTmp;
466 
467 #ifdef VP_DEBUG
468 #if VP_DEBUG_MODE == 3
469  {
470  std::cout << "erf = " << 1-erf(chiTmp) << std::endl;
471  std::cout << "x[i] = " << x[i] <<std::endl;
472  std::cout << "chi = " << chiTmp << std::endl;
473  std::cout << "Sum chi = " << chiTmp*vpMath::sqr(sig_prev) << std::endl;
474  std::cout << "Expectation = " << chiTmp*(1-erf(chiTmp)) << std::endl;
475  //getchar();
476  }
477 #endif
478 #endif
479  }
480 
481 
482  sigma2 = Sum_chi*vpMath::sqr(sig_prev)/((n-p)*Expectation);
483 
484 #ifdef VP_DEBUG
485 #if VP_DEBUG_MODE == 3
486  {
487  std::cout << "Expectation = " << Expectation << std::endl;
488  std::cout << "Sum chi = " << Sum_chi << std::endl;
489  std::cout << "sig_prev" << sig_prev << std::endl;
490  std::cout << "sig_out" << sqrt(fabs(sigma2)) << std::endl;
491  }
492 #endif
493 #endif
494 
495  return sqrt(fabs(sigma2));
496 
497 }
498 
499 double
500 vpRobust::constrainedChi(vpRobustEstimatorType method, double x)
501 {
502  switch (method)
503  {
504  case TUKEY :
505  return constrainedChiTukey(x);
506  case CAUCHY :
507  return constrainedChiCauchy(x);
508  case HUBER :
509  return constrainedChiHuber(x);
510  };
511 
512  return -1;
513 
514 }
515 
516 
517 double
518 vpRobust::constrainedChiTukey(double x)
519 {
520  double sct=0;
521  double a=4.7;
522  double s=sig_prev;
523  //double epsillon=0.5;
524 
525  if(fabs(x) <= 4.7*sig_prev)
526  {
527  //sct = (vpMath::sqr(s*a-x)*vpMath::sqr(s*a+x)*vpMath::sqr(x))/(s*vpMath::sqr(vpMath::sqr(a*vpMath::sqr(s))));
529  }
530  else
531  sct = -1/s;
532 
533  return sct;
534 }
535 
536 
537 double
538 vpRobust::constrainedChiCauchy(double x)
539 {
540  double sct = 0;
541  //double u = x/sig_prev;
542  double s = sig_prev;
543  double b = 2.3849;
544 
545  sct = -1*(vpMath::sqr(x)*b)/(s*(vpMath::sqr(s*b)+vpMath::sqr(x)));
546 
547  return sct;
548 }
549 
550 double
551 vpRobust::constrainedChiHuber(double x)
552 {
553  double sct=0;
554  double u = x/sig_prev;
555  double c = 1.2107; //1.345;
556 
557  if(fabs(u) <= c)
558  sct = vpMath::sqr(u);
559  else
560  sct = vpMath::sqr(c);
561 
562  return sct;
563 }
564 
565 double
566 vpRobust::simult_chi_huber(double x)
567 {
568  double sct=0;
569  double u = x/sig_prev;
570  double c = 1.2107; //1.345;
571 
572  if(fabs(u) <= c)
573  {
574  //sct = 0.5*vpMath::sqr(u);
575  sct = vpMath::sqr(u);
576  }
577  else
578  {
579  //sct = 0.5*vpMath::sqr(c);
580  sct = vpMath::sqr(c);
581  }
582 
583  return sct;
584 }
585 
594 void vpRobust::psiTukey(double sig, vpColVector &x, vpColVector & weights)
595 {
596 
597  unsigned int n_data = x.getRows();
598  double cst_const = vpCST*4.6851;
599 
600  for(unsigned int i=0; i<n_data; i++)
601  {
602  //if(sig==0 && weights[i]!=0)
603  if(std::fabs(sig) <= std::numeric_limits<double>::epsilon() && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
604  {
605  weights[i]=1;
606  continue;
607  }
608 
609  double xi_sig = x[i]/sig;
610 
611  //if((fabs(xi_sig)<=(cst_const)) && weights[i]!=0)
612  if((std::fabs(xi_sig)<=(cst_const)) && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
613  {
614  weights[i] = vpMath::sqr(1-vpMath::sqr(xi_sig/cst_const));
615  //w[i] = vpMath::sqr(1-vpMath::sqr(x[i]/sig/4.7));
616  }
617  else
618  {
619  //Outlier - could resize list of points tracked here?
620  weights[i] = 0;
621  }
622  }
623 }
624 
632 void vpRobust::psiHuber(double sig, vpColVector &x, vpColVector &weights)
633 {
634  double c = 1.2107; //1.345;
635  unsigned int n_data = x.getRows();
636 
637  for(unsigned int i=0; i<n_data; i++)
638  {
639  //if(weights[i]!=0)
640  if(std::fabs(weights[i]) > std::numeric_limits<double>::epsilon())
641  {
642  double xi_sig = x[i]/sig;
643  if(fabs(xi_sig)<=c)
644  weights[i] = 1;
645  else
646  weights[i] = c/fabs(xi_sig);
647  }
648  }
649 }
650 
659 void vpRobust::psiCauchy(double sig, vpColVector &x, vpColVector &weights)
660 {
661  unsigned int n_data = x.getRows();
662  double const_sig = 2.3849*sig;
663 
664  //Calculate Cauchy's equation
665  for(unsigned int i=0; i<n_data; i++)
666  {
667  weights[i] = 1/(1+vpMath::sqr(x[i]/(const_sig)));
668 
669  // If one coordinate is an outlier the other is too!
670  // w[i] < 0.01 is a threshold to be set
671  /*if(w[i] < 0.01)
672  {
673  if(i%2 == 0)
674  {
675  w[i+1] = w[i];
676  i++;
677  }
678  else
679  w[i-1] = w[i];
680  }*/
681  }
682 }
683 
684 
692 void vpRobust::psiMcLure(double sig, vpColVector &r,vpColVector &weights)
693 {
694  unsigned int n_data = r.getRows();
695 
696  //McLure's function
697  for(unsigned int i=0; i<n_data; i++)
698  {
699  weights[i] = 1/(vpMath::sqr(1+vpMath::sqr(r[i]/sig)));
700  //w[i] = 2*mad/vpMath::sqr((mad+r[i]*r[i]));//odobez
701 
702  // If one coordinate is an outlier the other is too!
703  // w[i] < 0.01 is a threshold to be set
704  /*if(w[i] < 0.01)
705  {
706  if(i%2 == 0)
707  {
708  w[i+1] = w[i];
709  i++;
710  }
711  else
712  w[i-1] = w[i];
713  }*/
714  }
715 }
716 
717 
724 int
725 vpRobust::partition(vpColVector &a, int l, int r)
726 {
727  int i = l-1;
728  int j = r;
729  double v = a[(unsigned int)r];
730 
731  for (;;)
732  {
733  while (a[(unsigned int)++i] < v) ;
734  while (v < a[(unsigned int)--j]) if (j == l) break;
735  if (i >= j) break;
736  exch(a[(unsigned int)i], a[(unsigned int)j]);
737  }
738  exch(a[(unsigned int)i], a[(unsigned int)r]);
739  return i;
740 }
741 
749 double
750 vpRobust::select(vpColVector &a, int l, int r, int k)
751 {
752  while (r > l)
753  {
754  int i = partition(a, l, r);
755  if (i >= k) r = i-1;
756  if (i <= k) l = i+1;
757  }
758  return a[(unsigned int)k];
759 }
760 
761 
762 double
763 vpRobust::erf(double x)
764 {
765  return x < 0.0 ? -gammp(0.5,x*x) : gammp(0.5,x*x);
766 }
767 
768 double
769 vpRobust::gammp(double a, double x)
770 {
771  double gamser=0.,gammcf=0.,gln;
772 
773  if (x < 0.0 || a <= 0.0)
774  std::cout << "Invalid arguments in routine GAMMP";
775  if (x < (a+1.0))
776  {
777  gser(&gamser,a,x,&gln);
778  return gamser;
779  }
780  else
781  {
782  gcf(&gammcf,a,x,&gln);
783  return 1.0-gammcf;
784  }
785 }
786 
787 void
788 vpRobust::gser(double *gamser, double a, double x, double *gln)
789 {
790  double sum,del,ap;
791 
792  *gln=gammln(a);
793  if (x <= 0.0)
794  {
795  if (x < 0.0)
796  std::cout << "x less than 0 in routine GSER";
797  *gamser=0.0;
798  return;
799  }
800  else
801  {
802  ap=a;
803  del=sum=1.0/a;
804  for (int n=1; n<=vpITMAX; n++)
805  {
806  ap += 1.0;
807  del *= x/ap;
808  sum += del;
809  if (fabs(del) < fabs(sum)*vpEPS)
810  {
811  *gamser=sum*exp(-x+a*log(x)-(*gln));
812  return;
813  }
814  }
815  std::cout << "a too large, vpITMAX too small in routine GSER";
816  return;
817  }
818 }
819 
820 void
821 vpRobust::gcf(double *gammcf, double a, double x, double *gln)
822 {
823  double gold=0.0,g,fac=1.0,b1=1.0;
824  double b0=0.0,anf,ana,an,a1,a0=1.0;
825 
826  *gln=gammln(a);
827  a1=x;
828  for (int n=1; n<=vpITMAX; n++)
829  {
830  an=(double) n;
831  ana=an-a;
832  a0=(a1+a0*ana)*fac;
833  b0=(b1+b0*ana)*fac;
834  anf=an*fac;
835  a1=x*a0+anf*a1;
836  b1=x*b0+anf*b1;
837  //if (a1)
838  if (std::fabs(a1) > std::numeric_limits<double>::epsilon())
839  {
840  fac=1.0/a1;
841  g=b1*fac;
842  if (fabs((g-gold)/g) < vpEPS)
843  {
844  *gammcf=exp(-x+a*log(x)-(*gln))*g;
845  return;
846  }
847  gold=g;
848  }
849  }
850  std::cout << "a too large, vpITMAX too small in routine GCF";
851 }
852 
853 double
854 vpRobust::gammln(double xx)
855 {
856  double x,tmp,ser;
857  static double cof[6]={76.18009173,-86.50532033,24.01409822,
858  -1.231739516,0.120858003e-2,-0.536382e-5};
859 
860  x=xx-1.0;
861  tmp=x+5.5;
862  tmp -= (x+0.5)*log(tmp);
863  ser=1.0;
864  for (int j=0; j<=5; j++)
865  {
866  x += 1.0;
867  ser += cof[j]/x;
868  }
869  return -tmp+log(2.50662827465*ser);
870 }
871 
872 
873 
874 // double
875 // vpRobust::median(const vpColVector &v)
876 // {
877 // int i,j;
878 // int inf, sup;
879 // int n = v.getRows() ;
880 // vpColVector infsup(n) ;
881 // vpColVector eq(n) ;
882 //
883 // for (i=0;i<n;i++)
884 // {
885 // // We compute the number of elements superior to the current value (sup)
886 // // the number of elements inferior (inf) to the current value and
887 // // the number of elements equal to the current value (eq)
888 // inf = sup = 0;
889 // for (j=0;j<n;j++)
890 // {
891 // if (i != j)
892 // {
893 // if (v[i] <= v[j]) inf++;
894 // if (v[i] >= v[j]) sup++;
895 // if (v[i] == v[j]) eq[i]++;
896 // }
897 // }
898 // // We compute then difference between inf and sup
899 // // the median should be for |inf-sup| = 0 (1 if an even number of element)
900 // // which means that there are the same number of element in the array
901 // // that are greater and smaller that this value.
902 // infsup[i] = abs(inf-sup);
903 // }
904 //
905 // // seek for the smaller value of |inf-sup| (should be 0 or 1)
906 // int imin = 0 ; // index of the median in the array
907 // //double eqmax = 0 ; // count of equal values
908 // // min cannot be greater than the number of element
909 // double min = n;
910 //
911 // // number of medians
912 // int mediancount = 0;
913 // // array of medians
914 // int *medianindex = new int[n];
915 //
916 // for (i=0; i<n; i++)
917 // {
918 // if(infsup[i] < min)
919 // {
920 // min = infsup[i];
921 // imin = i ;
922 //
923 // //reset count of median values
924 // mediancount=0;
925 // medianindex[mediancount]=i;
926 // }
927 // else if(infsup[i]==min) //If there is another median
928 // {
929 // mediancount++;
930 // medianindex[mediancount]=i;
931 // }
932 // }
933 //
934 // // Choose smalest data to be the median
935 // /*for(i=0; i<mediancount+1; i++)
936 // {
937 // //Choose the value with the greatest count
938 // if(eq[medianindex[i]] > eqmax)
939 // {
940 // eqmax = eq[medianindex[i]];
941 // imin = medianindex[i];
942 // }
943 // //If we have identical counts
944 // // Choose smalest data to be the median
945 // //if(v[medianindex[i]] < v[imin])
946 // // imin = medianindex[i];
947 // }*/
948 //
949 // // return the median
950 // delete []medianindex;
951 // return(v[imin]);
952 // }
953 //
954 // // Calculate median only for the residues which have
955 // // not be rejected. i.e. weight=0
956 // double
957 // vpRobust::median(const vpColVector &v, vpColVector &weights)
958 // {
959 // int i,j;
960 // int inf, sup;
961 // int n = v.getRows() ;
962 // vpColVector infsup(n) ;
963 // vpColVector eq(n) ;
964 //
965 // for (i=0;i<n;i++)
966 // {
967 // if(weights[i]!=0)
968 // {
969 // // We compute the number of elements superior to the current value (sup)
970 // // the number of elements inferior (inf) to the current value and
971 // // the number of elements equal to the current value (eq)
972 // inf = sup = 0;
973 // for (j=0;j<n;j++)
974 // {
975 // if (weights[j]!=0 && i!=j)
976 // {
977 // if (v[i] <= v[j]) inf++;
978 // if (v[i] >= v[j]) sup++;
979 // if (v[i] == v[j]) eq[i]++;
980 // }
981 // }
982 // // We compute then difference between inf and sup
983 // // the median should be for |inf-sup| = 0 (1 if an even number of element)
984 // // which means that there are the same number of element in the array
985 // // that are greater and smaller that this value.
986 // infsup[i] = abs(inf-sup);
987 // }
988 // }
989 //
990 // // seek for the smaller value of |inf-sup| (should be 0 or 1)
991 // int imin = 0 ; // index of the median in the array
992 // //double eqmax = 0 ; // count of equal values
993 // // min cannot be greater than the number of element
994 // double min = n;
995 //
996 // for (i=0; i<n; i++)
997 // {
998 // if(weights[i]!=0)
999 // {
1000 // if(infsup[i] < min)
1001 // {
1002 // min = infsup[i];
1003 // imin = i ;
1004 // }
1005 // }
1006 // }
1007 //
1008 // // return the median
1009 // return(v[imin]);
1010 // }
1011 
1012 
1013 #undef vpITMAX
1014 #undef vpEPS
1015 #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:136
double * data
address of the first element of the data array
Definition: vpMatrix.h:118
vpColVector simultMEstimator(vpColVector &residues)
Simult Mestimator.
Definition: vpRobust.cpp:347
static double sqr(double x)
Definition: vpMath.h:106
vpRobust(unsigned int n_data)
Default Constructor.
Definition: vpRobust.cpp:72
#define vpCDEBUG(level)
Definition: vpDebug.h:506
Class that provides a data structure for the column vectors as well as a set of operations on these v...
Definition: vpColVector.h:72
unsigned int getRows() const
Return the number of rows of the matrix.
Definition: vpMatrix.h:161
void resize(unsigned int n_data)
Resize containers for sort methods.
Definition: vpRobust.cpp:88
vpRobustEstimatorType
Enumeration of influence functions.
Definition: vpRobust.h:68
void resize(const unsigned int i, const bool flagNullify=true)
Definition: vpColVector.h:98