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