44 #include <visp3/core/vpColVector.h> 45 #include <visp3/core/vpDebug.h> 46 #include <visp3/core/vpMath.h> 53 #include <visp3/core/vpRobust.h> 66 : normres(), sorted_normres(), sorted_residues(), NoiseThreshold(0.0017), sig_prev(0), it(0), swap(0), size(n_data)
68 vpCDEBUG(2) <<
"vpRobust constructor reached" << std::endl;
71 sorted_normres.
resize(n_data);
72 sorted_residues.
resize(n_data);
80 : normres(), sorted_normres(), sorted_residues(), NoiseThreshold(0.0017), sig_prev(0), it(0), swap(0), size(0)
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;
105 #ifdef VISP_HAVE_CPP11_COMPATIBILITY 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);
131 if (n_data != size) {
133 sorted_normres.
resize(n_data);
134 sorted_residues.
resize(n_data);
180 double normmedian = 0;
184 unsigned int n_data = residues.
getRows();
187 sorted_residues = residues;
189 unsigned int ind_med = (
unsigned int)(ceil(n_data / 2.0)) - 1;
192 med = select(sorted_residues, 0, (
int)n_data - 1, (
int)ind_med );
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));
202 normmedian = select(sorted_normres, 0, (
int)n_data - 1, (
int)ind_med );
205 sigma = 1.4826 * normmedian;
209 if (sigma < NoiseThreshold) {
210 sigma = NoiseThreshold;
215 psiTukey(sigma, normres, weights);
217 vpCDEBUG(2) <<
"Tukey's function computed" << std::endl;
221 psiCauchy(sigma, normres, weights);
225 psiHuber(sigma, normres, weights);
235 double normmedian = 0;
238 unsigned int n_all_data = all_residues.
getRows();
243 normmedian = computeNormalizedMedian(all_normres, residues, all_residues, weights);
246 sigma = 1.4826 * normmedian;
250 if (sigma < NoiseThreshold) {
251 sigma = NoiseThreshold;
256 psiTukey(sigma, all_normres, weights);
258 vpCDEBUG(2) <<
"Tukey's function computed" << std::endl;
262 psiCauchy(sigma, all_normres, weights);
266 psiHuber(sigma, all_normres, weights);
276 double normmedian = 0;
278 unsigned int n_all_data = all_residues.
getRows();
279 unsigned int n_data = residues.
getRows();
284 sorted_residues = residues;
286 no_null_weight_residues.
resize(n_data);
293 unsigned int index = 0;
294 for (
unsigned int j = 0; j < n_data; j++) {
296 if (std::fabs(weights[j]) > std::numeric_limits<double>::epsilon()) {
297 no_null_weight_residues[index] = residues[j];
301 sorted_residues.
resize(index);
302 memcpy(sorted_residues.
data, no_null_weight_residues.
data, index *
sizeof(
double));
305 vpCDEBUG(2) <<
"vpRobust MEstimator reached. No. data = " << n_data << std::endl;
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 );
316 for (i = 0; i < n_all_data; i++) {
317 all_normres[i] = (fabs(all_residues[i] - med));
320 for (i = 0; i < n_data; i++) {
321 sorted_normres[i] = (fabs(sorted_residues[i] - med));
327 normmedian = select(sorted_normres, 0, (
int)n_data - 1, (
int)ind_med );
347 unsigned int n_data = residues.
getRows();
351 vpCDEBUG(2) <<
"vpRobust MEstimator reached. No. data = " << n_data << std::endl;
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 );
358 for (
unsigned int i = 0; i < n_data; i++)
359 norm_res[i] = (fabs(residues[i] - med));
365 double normmedian = select(norm_res, 0, (
int)n_data - 1, (
int)ind_med );
367 sigma = 1.4826 * normmedian;
370 sigma = simultscale(residues);
375 if (sigma < NoiseThreshold) {
376 sigma = NoiseThreshold;
379 vpCDEBUG(2) <<
"MAD and C computed" << std::endl;
381 psiHuber(sigma, norm_res, w);
393 double Expectation = 0;
396 for (
unsigned int i = 0; i < n; i++) {
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);
404 Expectation += chiTmp * (1 - erf(chiTmp));
409 #if VP_DEBUG_MODE == 3 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;
416 std::cout <<
"erf = " << (1 - erf(chiTmp)) << std::endl;
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;
426 std::cout <<
"Expectation = " << chiTmp * (1 - erf(chiTmp)) << std::endl;
434 sigma2 = Sum_chi *
vpMath::sqr(sig_prev) / ((n - p) * Expectation);
437 #if VP_DEBUG_MODE == 3 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;
447 return sqrt(fabs(sigma2));
454 return constrainedChiTukey(x);
456 return constrainedChiCauchy(x);
458 return constrainedChiHuber(x);
464 double vpRobust::constrainedChiTukey(
double x)
470 if (fabs(x) <= 4.7 * sig_prev) {
483 double vpRobust::constrainedChiCauchy(
double x)
495 double vpRobust::constrainedChiHuber(
double x)
498 double u = x / sig_prev;
509 double vpRobust::simult_chi_huber(
double x)
512 double u = x / sig_prev;
537 unsigned int n_data = x.
getRows();
538 double cst_const = vpCST * 4.6851;
540 for (
unsigned int i = 0; i < n_data; i++) {
542 if (std::fabs(sig) <= std::numeric_limits<double>::epsilon() &&
543 std::fabs(weights[i]) > std::numeric_limits<double>::epsilon()) {
548 double xi_sig = x[i] / sig;
551 if ((std::fabs(xi_sig) <= (cst_const)) && std::fabs(weights[i]) > std::numeric_limits<double>::epsilon()) {
571 unsigned int n_data = x.
getRows();
573 for (
unsigned int i = 0; i < n_data; i++) {
575 if (std::fabs(weights[i]) > std::numeric_limits<double>::epsilon()) {
576 double xi_sig = x[i] / sig;
577 if (fabs(xi_sig) <= c)
580 weights[i] = c / fabs(xi_sig);
595 unsigned int n_data = x.
getRows();
596 double const_sig = 2.3849 * sig;
599 for (
unsigned int i = 0; i < n_data; i++) {
600 weights[i] = 1 / (1 +
vpMath::sqr(x[i] / (const_sig)));
623 int vpRobust::partition(
vpColVector &a,
int l,
int r)
627 double v = a[(
unsigned int)r];
630 while (a[(
unsigned int)++i] < v)
632 while (v < a[(
unsigned int)--j])
637 exch(a[(
unsigned int)i], a[(
unsigned int)j]);
639 exch(a[(
unsigned int)i], a[(
unsigned int)r]);
650 double vpRobust::select(
vpColVector &a,
int l,
int r,
int k)
653 int i = partition(a, l, r);
659 return a[(
unsigned int)k];
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); }
665 double vpRobust::gammp(
double a,
double x)
667 double gamser = 0., gammcf = 0., gln;
669 if (x < 0.0 || a <= 0.0)
670 std::cout <<
"Invalid arguments in routine GAMMP";
672 gser(&gamser, a, x, &gln);
675 gcf(&gammcf, a, x, &gln);
680 void vpRobust::gser(
double *gamser,
double a,
double x,
double *gln)
685 std::cout <<
"x less than 0 in routine GSER";
690 double sum = 1.0 / a;
692 for (
int n = 1; n <= vpITMAX; n++) {
696 if (fabs(del) < fabs(sum) * vpEPS) {
697 *gamser = sum * exp(-x + a * log(x) - (*gln));
701 std::cout <<
"a too large, vpITMAX too small in routine GSER";
706 void vpRobust::gcf(
double *gammcf,
double a,
double x,
double *gln)
708 double gold = 0.0, g, fac = 1.0, b1 = 1.0;
709 double b0 = 0.0, a1, a0 = 1.0;
713 for (
int n = 1; n <= vpITMAX; n++) {
714 double an = (double)n;
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;
722 if (std::fabs(a1) > std::numeric_limits<double>::epsilon()) {
725 if (fabs((g - gold) / g) < vpEPS) {
726 *gammcf = exp(-x + a * log(x) - (*gln)) * g;
732 std::cout <<
"a too large, vpITMAX too small in routine GCF";
735 double vpRobust::gammln(
double xx)
738 static double cof[6] = {76.18009173, -86.50532033, 24.01409822, -1.231739516, 0.120858003e-2, -0.536382e-5};
742 tmp -= (x + 0.5) * log(tmp);
744 for (
int j = 0; j <= 5; j++) {
748 return -tmp + log(2.50662827465 * ser);
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
Compute the weights according a residue vector and a PsiFunction.
Type * data
Address of the first element of the data array.
vpColVector simultMEstimator(vpColVector &residues)
Simult Mestimator.
static double sqr(double x)
vpRobust & operator=(const vpRobust &other)
unsigned int getRows() const
Implementation of column vector and the associated operations.
Contains an M-Estimator and various influence function.
void resize(unsigned int n_data)
Resize containers for sort methods.
vpRobustEstimatorType
Enumeration of influence functions.
void resize(const unsigned int i, const bool flagNullify=true)