36 (
std * 2.506628274631000502415765284811);
48 return dim * pow(1.0 - 2.0 / (9 * dim) +
60 for (
unsigned int i = 2; i <=
n; i++) ret *= i;
72 for (
unsigned int i = 2; i <=
n; i++) retLog += ::log((
double)
n);
84 static const double a[6] = {-3.969683028665376e+01, 2.209460984245205e+02,
85 -2.759285104469687e+02, 1.383577518672690e+02,
86 -3.066479806614716e+01, 2.506628277459239e+00};
87 static const double b[5] = {-5.447609879822406e+01, 1.615858368580409e+02,
88 -1.556989798598866e+02, 6.680131188771972e+01,
89 -1.328068155288572e+01};
90 static const double c[6] = {-7.784894002430293e-03, -3.223964580411365e-01,
91 -2.400758277161838e+00, -2.549732539343734e+00,
92 4.374664141464968e+00, 2.938163982698783e+00};
93 static const double d[4] = {7.784695709041462e-03, 3.224671290700398e-01,
94 2.445134137142996e+00, 3.754408661907416e+00};
106 u = u * (((((
a[0] *
t +
a[1]) *
t +
a[2]) *
t +
a[3]) *
t +
a[4]) *
t +
108 (((((
b[0] *
t +
b[1]) *
t +
b[2]) *
t +
b[3]) *
t +
b[4]) *
t + 1);
113 t = sqrt(-2 * ::log(
q));
114 u = (((((
c[0] *
t +
c[1]) *
t +
c[2]) *
t +
c[3]) *
t +
c[4]) *
t +
116 ((((d[0] *
t + d[1]) *
t + d[2]) *
t + d[3]) *
t + 1);
123 t =
t * 2.506628274631000502415765284811 *
125 u = u -
t / (1 + u *
t / 2);
127 return (
p > 0.5 ? -u : u);
135 static const double a[5] = {1.161110663653770e-002, 3.951404679838207e-001,
136 2.846603853776254e+001, 1.887426188426510e+002,
137 3.209377589138469e+003};
138 static const double b[5] = {1.767766952966369e-001, 8.344316438579620e+000,
139 1.725514762600375e+002, 1.813893686502485e+003,
140 8.044716608901563e+003};
141 static const double c[9] = {
142 2.15311535474403846e-8, 5.64188496988670089e-1, 8.88314979438837594e00,
143 6.61191906371416295e01, 2.98635138197400131e02, 8.81952221241769090e02,
144 1.71204761263407058e03, 2.05107837782607147e03, 1.23033935479799725E03};
145 static const double d[9] = {
146 1.00000000000000000e00, 1.57449261107098347e01, 1.17693950891312499e02,
147 5.37181101862009858e02, 1.62138957456669019e03, 3.29079923573345963e03,
148 4.36261909014324716e03, 3.43936767414372164e03, 1.23033935480374942e03};
149 static const double p[6] = {1.63153871373020978e-2, 3.05326634961232344e-1,
150 3.60344899949804439e-1, 1.25781726111229246e-1,
151 1.60837851487422766e-2, 6.58749161529837803e-4};
152 static const double q[6] = {1.00000000000000000e00, 2.56852019228982242e00,
153 1.87295284992346047e00, 5.27905102951428412e-1,
154 6.05183413124413191e-2, 2.33520497626869185e-3};
161 if (
y <= 0.46875 * 1.4142135623730950488016887242097)
165 y = u * ((((
a[0] *
z +
a[1]) *
z +
a[2]) *
z +
a[3]) *
z +
a[4]) /
166 ((((
b[0] *
z +
b[1]) *
z +
b[2]) *
z +
b[3]) *
z +
b[4]);
170 z = ::exp(-
y *
y / 2) / 2;
174 y =
y / 1.4142135623730950488016887242097;
175 y = ((((((((
c[0] *
y +
c[1]) *
y +
c[2]) *
y +
c[3]) *
y +
c[4]) *
y +
184 / ((((((((d[0] *
y + d[1]) *
y + d[2]) *
y + d[3]) *
y + d[4]) *
y +
198 z =
z * 1.4142135623730950488016887242097 /
y;
200 y =
y * (((((
p[0] *
y +
p[1]) *
y +
p[2]) *
y +
p[3]) *
y +
p[4]) *
y +
202 (((((
q[0] *
y +
q[1]) *
y +
q[2]) *
y +
q[3]) *
y +
q[4]) *
y +
204 y =
z * (0.564189583547756286948 -
y);
206 return (u < 0.0 ?
y : 1 -
y);
215 if (!std::getline(f, str))
return false;
217 const char*
s = str.c_str();
219 char *nextTok, *context;
220 const char* delim =
" \t";
224 while (nextTok !=
nullptr)
226 d.push_back(atoi(nextTok));
239 if (!std::getline(f, str))
return false;
241 const char*
s = str.c_str();
243 char *nextTok, *context;
244 const char* delim =
" \t";
248 while (nextTok !=
nullptr)
250 d.push_back(atof(nextTok));
266 ASSERT_(logWeights.size() == logLikelihoods.size());
268 if (!logWeights.size())
274 double SUM1 = 0, SUM2 = 0, tmpVal;
276 for (itLW = logWeights.begin(), itLL = logLikelihoods.begin();
277 itLW != logWeights.end(); itLW++, itLL++)
279 tmpVal = *itLW - lw_max;
280 SUM1 += std::exp(tmpVal);
281 SUM2 += std::exp(tmpVal + *itLL - ll_max);
284 double res = -std::log(SUM1) + std::log(SUM2) + ll_max;
297 size_t N = logLikelihoods.size();
303 for (
size_t i = 0; i < N; i++) SUM1 += exp(logLikelihoods[i] - ll_max);
305 double res = log(SUM1) - log(static_cast<double>(N)) + ll_max;
315 if (angles.empty())
return 0;
317 int W_phi_R = 0, W_phi_L = 0;
318 double phi_R = 0, phi_L = 0;
322 for (CVectorDouble::Index i = 0; i < angles.size(); i++)
324 double phi = angles[i];
325 if (abs(phi) > 1.5707963267948966192313216916398)
328 if (phi < 0) phi = (
M_2PI + phi);
344 if (W_phi_L) phi_L /=
static_cast<double>(W_phi_L);
345 if (W_phi_R) phi_R /=
static_cast<double>(W_phi_R);
351 return ((phi_L * W_phi_L + phi_R * W_phi_R) / (W_phi_L + W_phi_R));
356 const string& style,
const size_t& nEllipsePoints)
365 cov2, mean2, stdCount, style, nEllipsePoints);
372 const string& style,
const size_t& nEllipsePoints)
381 std::vector<float> X, Y, COS, SIN;
387 X.resize(nEllipsePoints);
388 Y.resize(nEllipsePoints);
389 COS.resize(nEllipsePoints);
390 SIN.resize(nEllipsePoints);
393 for (Cos = COS.begin(), Sin = SIN.begin(), ang = 0; Cos != COS.end();
394 ++Cos, ++Sin, ang += (
M_2PI / (nEllipsePoints - 1)))
396 *Cos = (float)cos(ang);
397 *Sin = (float)sin(ang);
400 cov.eigenVectors(eigVec, eigVal);
401 eigVal = eigVal.array().sqrt().matrix();
402 M = eigVal * eigVec.adjoint();
406 for (
x = X.begin(),
y = Y.begin(), Cos = COS.begin(), Sin = SIN.begin();
407 x != X.end(); ++
x, ++
y, ++Cos, ++Sin)
409 *
x = (float)(
mean[0] + stdCount * (*Cos * M(0, 0) + *Sin * M(1, 0)));
410 *
y = (float)(
mean[1] + stdCount * (*Cos * M(0, 1) + *Sin * M(1, 1)));
416 for (
x = X.begin();
x != X.end(); ++
x)
419 if (
x != (X.end() - 1)) str +=
format(
",");
422 for (
y = Y.begin();
y != Y.end(); ++
y)
425 if (
y != (Y.end() - 1)) str +=
format(
",");
428 str +=
format(
"],'%s');\n", style.c_str());
432 std::cerr <<
"The matrix that led to error was: " << std::endl
433 <<
cov << std::endl;)
437 const double x,
const double x0,
const double y0,
const double x1,
438 const double y1,
bool wrap2pi)
444 const double Ax = x1 - x0;
445 const double Ay = y1 - y0;
447 double r = y0 + Ay * (
x - x0) / Ax;
461 const std::vector<double>& inV, std::vector<double>& outV,
462 const int& _winSize,
const int& numberOfSigmas)
465 ASSERT_((
int)inV.size() >= _winSize);
467 size_t winSize = _winSize;
472 size_t sz = inV.size();
475 std::vector<double> aux(winSize);
476 size_t mpoint = winSize / 2;
477 for (
size_t k = 0; k < sz; ++k)
481 size_t idx_to_start =
482 std::max(
size_t(0), k - mpoint);
486 aux.resize(n_elements);
487 for (
size_t m = idx_to_start,
n = 0; m < idx_to_start + n_elements;
491 std::sort(aux.begin(), aux.end());
493 size_t auxSz = aux.size();
494 size_t auxMPoint = auxSz / 2;
495 outV[k] = (auxSz % 2) ? (aux[auxMPoint])
496 : (0.5 * (aux[auxMPoint - 1] +
511 T arg, T& lans, T& dans, T& pans,
unsigned int& j)
516 lans = lans + std::log(arg / j);
517 dans = std::exp(lans);
521 dans = dans * arg / j;
528 unsigned int degreesOfFreedom,
double noncentrality,
double arg,
double eps)
531 noncentrality >= 0.0 && arg >= 0.0 &&
eps > 0.0,
532 "noncentralChi2PDF_CDF(): parameters must be positive.");
534 if (arg == 0.0 && degreesOfFreedom > 0)
return std::make_pair(0.0, 0.0);
537 double b1 = 0.5 * noncentrality, ao = std::exp(-
b1), eps2 =
eps / ao,
538 lnrtpi2 = 0.22579135264473, probability, density, lans, dans, pans,
540 unsigned int maxit = 500, i, m;
541 if (degreesOfFreedom % 2)
544 lans = -0.5 * (arg + std::log(arg)) - lnrtpi2;
545 dans = std::exp(lans);
546 pans = std::erf(std::sqrt(arg / 2.0));
552 dans = std::exp(lans);
557 if (degreesOfFreedom == 0)
560 degreesOfFreedom = 2;
562 sum = 1.0 / ao - 1.0 - am;
564 probability = 1.0 + am * pans;
569 degreesOfFreedom = degreesOfFreedom - 1;
571 sum = 1.0 / ao - 1.0;
572 while (i < degreesOfFreedom)
574 degreesOfFreedom = degreesOfFreedom + 1;
579 for (++m; m < maxit; ++m)
584 density = density + am * dans;
586 probability = probability + hold;
587 if ((pans *
sum < eps2) && (hold < eps2))
break;
589 if (m == maxit)
THROW_EXCEPTION(
"noncentralChi2PDF_CDF(): no convergence.");
590 return std::make_pair(
591 0.5 * ao * density,
std::min(1.0, std::max(0.0, ao * probability)));
595 unsigned int degreesOfFreedom,
double arg,
double accuracy)
598 degreesOfFreedom, 0.0, arg, accuracy)
603 unsigned int degreesOfFreedom,
double noncentrality,
double arg)
605 const double a = degreesOfFreedom + noncentrality;
606 const double b = (
a + noncentrality) /
square(
a);
608 (std::pow((
double)arg /
a, 1.0 / 3.0) - (1.0 - 2.0 / 9.0 *
b)) /
609 std::sqrt(2.0 / 9.0 *
b);
610 return 0.5 * (1.0 + std::erf(
t / std::sqrt(2.0)));
628 if (
v.size() > 0)
s.WriteBufferFixEndianness(&
v[0],
v.size());
634 if (
v.size() > 0)
s.WriteBufferFixEndianness(&
v[0],
v.size());
double averageWrap2Pi(const CVectorDouble &angles)
Computes the average of a sequence of angles in radians taking into account the correct wrapping in t...
std::string MATLAB_plotCovariance2D(const CMatrixFloat &cov22, const CVectorFloat &mean, const float &stdCount, const std::string &style=std::string("b"), const size_t &nEllipsePoints=30)
Generates a string with the MATLAB commands required to plot an confidence interval (ellipse) for a 2...
This class is a "CSerializable" wrapper for "CMatrixTemplateNumeric<double>".
#define THROW_EXCEPTION(msg)
double normalCDF(double p)
Evaluates the Gaussian cumulative density function.
GLdouble GLdouble GLdouble GLdouble q
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
#define MRPT_END_WITH_CLEAN_UP(stuff)
T square(const T x)
Inline function for the square of a number.
double factorial(unsigned int n)
Computes the factorial of an integer number and returns it as a double value (internally it uses loga...
double normalQuantile(double p)
Evaluates the Gaussian distribution quantile for the probability value p=[0,1].
#define ASSERT_(f)
Defines an assertion mechanism.
This base provides a set of functions for maths stuff.
std::pair< double, double > noncentralChi2PDF_CDF(unsigned int degreesOfFreedom, double noncentrality, double arg, double eps=1e-7)
Returns the 'exact' PDF (first) and CDF (second) of a Non-central chi-squared probability distributio...
#define MRPT_CHECK_NORMAL_NUMBER(v)
Throws an exception if the number is NaN, IND, or +/-INF, or return the same number otherwise...
double averageLogLikelihood(const CVectorDouble &logLikelihoods)
A numerically-stable method to compute average likelihood values with strongly different ranges (unwe...
std::ostream & operator<<(std::ostream &o, const TPoint2D &p)
bool loadVector(std::istream &f, std::vector< int > &d)
Loads one row of a text file as a numerical std::vector.
double chi2PDF(unsigned int degreesOfFreedom, double arg, double accuracy=1e-7)
char * strtok(char *str, const char *strDelimit, char **context) noexcept
An OS-independent method for tokenizing a string.
CONTAINER::Scalar sum(const CONTAINER &v)
Computes the sum of all the elements.
uint64_t factorial64(unsigned int n)
Computes the factorial of an integer number and returns it as a 64-bit integer number.
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
CONTAINER::Scalar maximum(const CONTAINER &v)
GLsizei const GLchar ** string
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
double chi2CDF(unsigned int degreesOfFreedom, double arg)
void medianFilter(const std::vector< double > &inV, std::vector< double > &outV, const int &winSize, const int &numberOfSigmas=2)
void noncentralChi2OneIteration(T arg, T &lans, T &dans, T &pans, unsigned int &j)
unsigned __int64 uint64_t
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Virtual base class for "archives": classes abstracting I/O streams.
double chi2inv(double P, unsigned int dim=1)
The "quantile" of the Chi-Square distribution, for dimension "dim" and probability 0<P<1 (the inverse...
GLdouble GLdouble GLdouble r
std::string format(const char *fmt,...) MRPT_printf_format_check(1
A std::string version of C sprintf.
double mean(const CONTAINER &v)
Computes the mean value of a vector.
A matrix of dynamic size.
double noncentralChi2CDF(unsigned int degreesOfFreedom, double noncentrality, double arg)
Eigen::Matrix< typename MATRIX::Scalar, MATRIX::ColsAtCompileTime, MATRIX::ColsAtCompileTime > cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
double interpolate2points(const double x, const double x0, const double y0, const double x1, const double y1, bool wrap2pi=false)
Linear interpolation/extrapolation: evaluates at "x" the line (x0,y0)-(x1,y1).
#define THROW_EXCEPTION_FMT(_FORMAT_STRING,...)
unsigned __int32 uint32_t
size_t ReadBufferFixEndianness(T *ptr, size_t ElementCount)
Reads a sequence of elemental datatypes, taking care of reordering their bytes from the MRPT stream s...
double normalPDF(double x, double mu, double std)
Evaluates the univariate normal (Gaussian) distribution at a given point "x".
GLubyte GLubyte GLubyte a
const Scalar * const_iterator
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrix::Ptr &pObj)
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.