51 #include <visp3/core/vpColVector.h>
52 #include <visp3/core/vpDebug.h>
53 #include <visp3/core/vpMath.h>
54 #include <visp3/core/vpRobust.h>
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)
77 m_normres = other.m_normres;
78 m_sorted_normres = other.m_sorted_normres;
79 m_sorted_residues = other.m_sorted_residues;
80 m_mad_min = other.m_mad_min;
82 m_mad_prev = other.m_mad_prev;
83 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
84 m_iter = other.m_iter;
86 m_size = other.m_size;
90 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
96 m_normres = std::move(other.m_normres);
97 m_sorted_normres = std::move(other.m_sorted_normres);
98 m_sorted_residues = std::move(other.m_sorted_residues);
99 m_mad_min = std::move(other.m_mad_min);
100 m_mad_prev = std::move(other.m_mad_prev);
101 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
102 m_iter = std::move(other.m_iter);
104 m_size = std::move(other.m_size);
113 void vpRobust::resize(
unsigned int n_data)
115 if (n_data != m_size) {
117 m_sorted_normres.
resize(n_data);
118 m_sorted_residues.
resize(n_data);
139 double normmedian = 0;
142 unsigned int n_data = residues.
getRows();
143 weights.
resize(n_data,
false);
146 m_sorted_residues = residues;
148 unsigned int ind_med =
static_cast<unsigned int>(ceil(n_data / 2.0)) - 1;
151 med = select(m_sorted_residues, 0, n_data - 1, ind_med);
155 for (
unsigned int i = 0; i < n_data; ++i) {
156 m_normres[i] = (fabs(residues[i] - med));
157 m_sorted_normres[i] = (fabs(m_sorted_residues[i] - med));
161 normmedian = select(m_sorted_normres, 0, n_data - 1, ind_med);
164 m_mad = 1.4826 * normmedian;
168 if (m_mad < m_mad_min) {
173 psiTukey(m_mad, m_normres, weights);
177 psiCauchy(m_mad, m_normres, weights);
181 psiHuber(m_mad, m_normres, weights);
186 std::cout <<
"MEstimator: method not recognised - id = " << method << std::endl;
200 unsigned int n_data = x.
getRows();
201 double C = sig * 4.6851;
204 for (
unsigned int i = 0; i < n_data; ++i) {
205 double xi = x[i] / C;
228 double C = sig * 1.2107;
229 unsigned int n_data = x.
getRows();
231 for (
unsigned int i = 0; i < n_data; ++i) {
232 double xi = x[i] / C;
234 weights[i] = std::fabs(1. / xi);
252 unsigned int n_data = x.
getRows();
253 double C = sig * 2.3849;
256 for (
unsigned int i = 0; i < n_data; ++i) {
267 int vpRobust::partition(
vpColVector &a,
int l,
int r)
281 std::swap(a[i], a[j]);
283 std::swap(a[i], a[r]);
294 double vpRobust::select(
vpColVector &a,
int l,
int r,
int k)
297 int i = partition(a, l, r);
311 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
320 : m_normres(), m_sorted_normres(), m_sorted_residues(), m_mad_min(0.0017), m_mad_prev(0),
321 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
324 m_size(n_data), m_mad(0)
326 vpCDEBUG(2) <<
"vpRobust constructor reached" << std::endl;
329 m_sorted_normres.
resize(n_data);
330 m_sorted_residues.
resize(n_data);
337 double normmedian = 0;
339 unsigned int n_all_data = all_residues.
getRows();
344 normmedian = computeNormalizedMedian(all_normres, residues, all_residues, weights);
347 m_mad = 1.4826 * normmedian;
351 if (m_mad < m_mad_min) {
357 psiTukey(m_mad, all_normres, weights);
359 vpCDEBUG(2) <<
"Tukey's function computed" << std::endl;
363 psiCauchy(m_mad, all_normres, weights);
367 psiHuber(m_mad, all_normres, weights);
377 double normmedian = 0;
379 unsigned int n_all_data = all_residues.
getRows();
380 unsigned int n_data = residues.
getRows();
385 m_sorted_residues = residues;
387 no_null_weight_residues.
resize(n_data);
389 unsigned int index = 0;
390 for (
unsigned int j = 0; j < n_data; ++j) {
392 if (std::fabs(weights[j]) > std::numeric_limits<double>::epsilon()) {
393 no_null_weight_residues[index] = residues[j];
397 m_sorted_residues.
resize(index);
398 memcpy(m_sorted_residues.
data, no_null_weight_residues.
data, index *
sizeof(
double));
405 unsigned int ind_med = (
unsigned int)(ceil(n_data / 2.0)) - 1;
406 med = select(m_sorted_residues, 0, n_data - 1, ind_med);
409 for (
unsigned int i = 0; i < n_all_data; ++i) {
410 all_normres[i] = (fabs(all_residues[i] - med));
413 for (
unsigned int i = 0; i < n_data; ++i) {
414 m_sorted_normres[i] = (fabs(m_sorted_residues[i] - med));
417 normmedian = select(m_sorted_normres, 0, n_data - 1, ind_med);
432 unsigned int n_data = residues.
getRows();
437 unsigned int ind_med = (
unsigned int)(ceil(n_data / 2.0)) - 1;
438 med = select(residues, 0, n_data - 1, ind_med );
441 for (
unsigned int i = 0; i < n_data; ++i)
442 norm_res[i] = (fabs(residues[i] - med));
448 double normmedian = select(norm_res, 0, n_data - 1, ind_med);
450 m_mad = 1.4826 * normmedian;
454 m_mad = simultscale(residues);
459 if (m_mad < m_mad_min) {
463 psiHuber(m_mad, norm_res, w);
475 double Expectation = 0;
478 for (
unsigned int i = 0; i < n; ++i) {
480 double chiTmp = simult_chi_huber(x[i]);
481 #if defined(VISP_HAVE_FUNC_STD_ERFC)
482 Expectation += chiTmp * std::erfc(chiTmp);
483 #elif defined(VISP_HAVE_FUNC_ERFC)
484 Expectation += chiTmp * erfc(chiTmp);
486 Expectation += chiTmp * (1 - erf(chiTmp));
491 #if VP_DEBUG_MODE == 3
493 #if defined(VISP_HAVE_FUNC_STD_ERFC)
494 std::cout <<
"erf = " << std::erfc(chiTmp) << std::endl;
495 #elif defined(VISP_HAVE_FUNC_ERFC)
496 std::cout <<
"erf = " << erfc(chiTmp) << std::endl;
498 std::cout <<
"erf = " << (1 - erf(chiTmp)) << std::endl;
500 std::cout <<
"x[i] = " << x[i] << std::endl;
501 std::cout <<
"chi = " << chiTmp << std::endl;
502 std::cout <<
"Sum chi = " << chiTmp *
vpMath::sqr(m_mad_prev) << std::endl;
503 #if defined(VISP_HAVE_FUNC_STD_ERFC)
504 std::cout <<
"Expectation = " << chiTmp * std::erfc(chiTmp) << std::endl;
505 #elif defined(VISP_HAVE_FUNC_ERFC)
506 std::cout <<
"Expectation = " << chiTmp * erfc(chiTmp) << std::endl;
508 std::cout <<
"Expectation = " << chiTmp * (1 - erf(chiTmp)) << std::endl;
516 sigma2 = Sum_chi *
vpMath::sqr(m_mad_prev) / ((n - p) * Expectation);
519 #if VP_DEBUG_MODE == 3
521 std::cout <<
"Expectation = " << Expectation << std::endl;
522 std::cout <<
"Sum chi = " << Sum_chi << std::endl;
523 std::cout <<
"MAD prev" << m_mad_prev << std::endl;
524 std::cout <<
"sig_out" << sqrt(fabs(sigma2)) << std::endl;
529 return sqrt(fabs(sigma2));
532 double vpRobust::constrainedChi(vpRobustEstimatorType method,
double x)
536 return constrainedChiTukey(x);
538 return constrainedChiCauchy(x);
540 return constrainedChiHuber(x);
546 double vpRobust::constrainedChiTukey(
double x)
549 double s = m_mad_prev;
552 if (fabs(x) <= 4.7 * m_mad_prev) {
566 double vpRobust::constrainedChiCauchy(
double x)
570 double s = m_mad_prev;
578 double vpRobust::constrainedChiHuber(
double x)
581 double u = x / m_mad_prev;
592 double vpRobust::simult_chi_huber(
double x)
595 double u = x / m_mad_prev;
610 #if !defined(VISP_HAVE_FUNC_ERFC) && !defined(VISP_HAVE_FUNC_STD_ERFC)
611 double vpRobust::erf(
double x) {
return x < 0.0 ? -gammp(0.5, x * x) : gammp(0.5, x * x); }
613 double vpRobust::gammp(
double a,
double x)
615 double gamser = 0., gammcf = 0., gln;
617 if (x < 0.0 || a <= 0.0)
618 std::cout <<
"Invalid arguments in routine GAMMP";
620 gser(&gamser, a, x, &gln);
624 gcf(&gammcf, a, x, &gln);
629 void vpRobust::gser(
double *gamser,
double a,
double x,
double *gln)
634 std::cout <<
"x less than 0 in routine GSER";
640 double sum = 1.0 / a;
642 for (
int n = 1; n <= vpITMAX; ++n) {
646 if (fabs(del) < fabs(sum) * vpEPS) {
647 *gamser = sum * exp(-x + a * log(x) - (*gln));
651 std::cout <<
"a too large, vpITMAX too small in routine GSER";
656 void vpRobust::gcf(
double *gammcf,
double a,
double x,
double *gln)
658 double gold = 0.0, g, fac = 1.0, b1 = 1.0;
659 double b0 = 0.0, a1, a0 = 1.0;
663 for (
int n = 1; n <= vpITMAX; ++n) {
664 double an =
static_cast<double>(n);
666 a0 = (a1 + a0 * ana) * fac;
667 b0 = (b1 + b0 * ana) * fac;
668 double anf = an * fac;
669 a1 = x * a0 + anf * a1;
670 b1 = x * b0 + anf * b1;
672 if (std::fabs(a1) > std::numeric_limits<double>::epsilon()) {
675 if (fabs((g - gold) / g) < vpEPS) {
676 *gammcf = exp(-x + a * log(x) - (*gln)) * g;
682 std::cout <<
"a too large, vpITMAX too small in routine GCF";
685 double vpRobust::gammln(
double xx)
688 static double cof[6] = { 76.18009173, -86.50532033, 24.01409822, -1.231739516, 0.120858003e-2, -0.536382e-5 };
692 tmp -= (x + 0.5) * log(tmp);
694 for (
int j = 0; j <= 5; ++j) {
698 return -tmp + log(2.50662827465 * ser);
Type * data
Address of the first element of the data array.
unsigned int getRows() const
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
static double sqr(double x)
Contains an M-estimator and various influence function.
vpRobustEstimatorType
Enumeration of influence functions.
@ HUBER
Huber influence function.
@ TUKEY
Tukey influence function.
@ CAUCHY
Cauchy influence function.
void MEstimator(const vpRobustEstimatorType method, const vpColVector &residues, vpColVector &weights)
vpRobust & operator=(const vpRobust &other)
vp_deprecated vpColVector simultMEstimator(vpColVector &residues)