20 #include <Eigen/Dense> 37 (
std * 2.506628274631000502415765284811);
49 return dim * pow(1.0 - 2.0 / (9 * dim) +
61 for (
unsigned int i = 2; i <=
n; i++) ret *= i;
73 for (
unsigned int i = 2; i <=
n; i++) retLog += ::log((
double)
n);
85 static const double a[6] = {-3.969683028665376e+01, 2.209460984245205e+02,
86 -2.759285104469687e+02, 1.383577518672690e+02,
87 -3.066479806614716e+01, 2.506628277459239e+00};
88 static const double b[5] = {-5.447609879822406e+01, 1.615858368580409e+02,
89 -1.556989798598866e+02, 6.680131188771972e+01,
90 -1.328068155288572e+01};
91 static const double c[6] = {-7.784894002430293e-03, -3.223964580411365e-01,
92 -2.400758277161838e+00, -2.549732539343734e+00,
93 4.374664141464968e+00, 2.938163982698783e+00};
94 static const double d[4] = {7.784695709041462e-03, 3.224671290700398e-01,
95 2.445134137142996e+00, 3.754408661907416e+00};
108 (((((
a[0] *
t +
a[1]) *
t +
a[2]) *
t +
a[3]) *
t +
a[4]) *
t +
110 (((((
b[0] *
t +
b[1]) *
t +
b[2]) *
t +
b[3]) *
t +
b[4]) *
t + 1);
115 t = sqrt(-2 * ::log(
q));
116 u = (((((
c[0] *
t +
c[1]) *
t +
c[2]) *
t +
c[3]) *
t +
c[4]) *
t +
118 ((((d[0] *
t + d[1]) *
t + d[2]) *
t + d[3]) *
t + 1);
125 t =
t * 2.506628274631000502415765284811 *
127 u = u -
t / (1 + u *
t / 2);
129 return (
p > 0.5 ? -u : u);
137 static const double a[5] = {1.161110663653770e-002, 3.951404679838207e-001,
138 2.846603853776254e+001, 1.887426188426510e+002,
139 3.209377589138469e+003};
140 static const double b[5] = {1.767766952966369e-001, 8.344316438579620e+000,
141 1.725514762600375e+002, 1.813893686502485e+003,
142 8.044716608901563e+003};
143 static const double c[9] = {
144 2.15311535474403846e-8, 5.64188496988670089e-1, 8.88314979438837594e00,
145 6.61191906371416295e01, 2.98635138197400131e02, 8.81952221241769090e02,
146 1.71204761263407058e03, 2.05107837782607147e03, 1.23033935479799725E03};
147 static const double d[9] = {
148 1.00000000000000000e00, 1.57449261107098347e01, 1.17693950891312499e02,
149 5.37181101862009858e02, 1.62138957456669019e03, 3.29079923573345963e03,
150 4.36261909014324716e03, 3.43936767414372164e03, 1.23033935480374942e03};
151 static const double p[6] = {1.63153871373020978e-2, 3.05326634961232344e-1,
152 3.60344899949804439e-1, 1.25781726111229246e-1,
153 1.60837851487422766e-2, 6.58749161529837803e-4};
154 static const double q[6] = {1.00000000000000000e00, 2.56852019228982242e00,
155 1.87295284992346047e00, 5.27905102951428412e-1,
156 6.05183413124413191e-2, 2.33520497626869185e-3};
163 if (
y <= 0.46875 * 1.4142135623730950488016887242097)
167 y = u * ((((
a[0] *
z +
a[1]) *
z +
a[2]) *
z +
a[3]) *
z +
a[4]) /
168 ((((
b[0] *
z +
b[1]) *
z +
b[2]) *
z +
b[3]) *
z +
b[4]);
172 z = ::exp(-
y *
y / 2) / 2;
176 y =
y / 1.4142135623730950488016887242097;
177 y = ((((((((
c[0] *
y +
c[1]) *
y +
c[2]) *
y +
c[3]) *
y +
c[4]) *
y +
186 / ((((((((d[0] *
y + d[1]) *
y + d[2]) *
y + d[3]) *
y + d[4]) *
y +
200 z =
z * 1.4142135623730950488016887242097 /
y;
203 (((((
p[0] *
y +
p[1]) *
y +
p[2]) *
y +
p[3]) *
y +
p[4]) *
y +
205 (((((
q[0] *
y +
q[1]) *
y +
q[2]) *
y +
q[3]) *
y +
q[4]) *
y +
207 y =
z * (0.564189583547756286948 -
y);
209 return (u < 0.0 ?
y : 1 -
y);
218 if (!std::getline(f, str))
return false;
220 const char*
s = str.c_str();
222 char *nextTok, *context;
223 const char* delim =
" \t";
227 while (nextTok !=
nullptr)
229 d.push_back(atoi(nextTok));
242 if (!std::getline(f, str))
return false;
244 const char*
s = str.c_str();
246 char *nextTok, *context;
247 const char* delim =
" \t";
251 while (nextTok !=
nullptr)
253 d.push_back(atof(nextTok));
271 if (!logWeights.
size())
277 double SUM1 = 0, SUM2 = 0, tmpVal;
279 for (itLW = logWeights.
begin(), itLL = logLikelihoods.
begin();
280 itLW != logWeights.
end(); itLW++, itLL++)
282 tmpVal = *itLW - lw_max;
283 SUM1 += std::exp(tmpVal);
284 SUM2 += std::exp(tmpVal + *itLL - ll_max);
287 double res = -std::log(SUM1) + std::log(SUM2) + ll_max;
300 size_t N = logLikelihoods.
size();
306 for (
size_t i = 0; i < N; i++) SUM1 += exp(logLikelihoods[i] - ll_max);
308 double res = log(SUM1) - log(static_cast<double>(N)) + ll_max;
318 if (angles.
empty())
return 0;
320 int W_phi_R = 0, W_phi_L = 0;
321 double phi_R = 0, phi_L = 0;
327 double phi = angles[i];
328 if (abs(phi) > 1.5707963267948966192313216916398)
331 if (phi < 0) phi = (
M_2PI + phi);
347 if (W_phi_L) phi_L /=
static_cast<double>(W_phi_L);
348 if (W_phi_R) phi_R /=
static_cast<double>(W_phi_R);
354 return ((phi_L * W_phi_L + phi_R * W_phi_R) / (W_phi_L + W_phi_R));
359 const string& style,
const size_t& nEllipsePoints)
368 cov2, mean2, stdCount, style, nEllipsePoints);
375 const string& style,
const size_t& nEllipsePoints)
384 std::vector<float> X, Y, COS, SIN;
385 std::vector<float>::iterator
x,
y, Cos, Sin;
389 X.resize(nEllipsePoints);
390 Y.resize(nEllipsePoints);
391 COS.resize(nEllipsePoints);
392 SIN.resize(nEllipsePoints);
395 for (Cos = COS.begin(), Sin = SIN.begin(), ang = 0; Cos != COS.end();
396 ++Cos, ++Sin, ang += (
M_2PI / (nEllipsePoints - 1)))
398 *Cos = (float)cos(ang);
399 *Sin = (float)sin(ang);
403 const auto eigVec = eigensolver.eigenvectors();
404 auto eigVal = eigensolver.eigenvalues();
406 eigVal = eigVal.array().sqrt().matrix();
407 Eigen::MatrixXd M = eigVal * eigVec.transpose();
411 for (
x = X.begin(),
y = Y.begin(), Cos = COS.begin(), Sin = SIN.begin();
412 x != X.end(); ++
x, ++
y, ++Cos, ++Sin)
414 *
x = (float)(
mean[0] + stdCount * (*Cos * M(0, 0) + *Sin * M(1, 0)));
415 *
y = (float)(
mean[1] + stdCount * (*Cos * M(0, 1) + *Sin * M(1, 1)));
421 for (
x = X.begin();
x != X.end(); ++
x)
424 if (
x != (X.end() - 1)) str +=
format(
",");
427 for (
y = Y.begin();
y != Y.end(); ++
y)
430 if (
y != (Y.end() - 1)) str +=
format(
",");
433 str +=
format(
"],'%s');\n", style.c_str());
442 const double x,
const double x0,
const double y0,
const double x1,
443 const double y1,
bool wrap2pi)
449 const double Ax = x1 - x0;
450 const double Ay = y1 - y0;
452 double r = y0 + Ay * (
x - x0) / Ax;
466 const std::vector<double>& inV, std::vector<double>& outV,
467 const int& _winSize,
const int& numberOfSigmas)
470 ASSERT_((
int)inV.size() >= _winSize);
472 size_t winSize = _winSize;
477 size_t sz = inV.size();
480 std::vector<double> aux(winSize);
481 size_t mpoint = winSize / 2;
482 for (
size_t k = 0; k < sz; ++k)
486 size_t idx_to_start =
487 std::max(
size_t(0), k - mpoint);
491 aux.resize(n_elements);
492 for (
size_t m = idx_to_start,
n = 0; m < idx_to_start + n_elements;
496 std::sort(aux.begin(), aux.end());
498 size_t auxSz = aux.size();
499 size_t auxMPoint = auxSz / 2;
500 outV[k] = (auxSz % 2) ? (aux[auxMPoint])
501 : (0.5 * (aux[auxMPoint - 1] +
516 T arg, T& lans, T& dans, T& pans,
unsigned int& j)
521 lans = lans + std::log(arg / j);
522 dans = std::exp(lans);
526 dans = dans * arg / j;
533 unsigned int degreesOfFreedom,
double noncentrality,
double arg,
double eps)
536 noncentrality >= 0.0 && arg >= 0.0 &&
eps > 0.0,
537 "noncentralChi2PDF_CDF(): parameters must be positive.");
539 if (arg == 0.0 && degreesOfFreedom > 0)
return std::make_pair(0.0, 0.0);
542 double b1 = 0.5 * noncentrality, ao = std::exp(-
b1), eps2 =
eps / ao,
543 lnrtpi2 = 0.22579135264473, probability, density, lans, dans, pans,
545 unsigned int maxit = 500, i, m;
546 if (degreesOfFreedom % 2)
549 lans = -0.5 * (arg + std::log(arg)) - lnrtpi2;
550 dans = std::exp(lans);
551 pans = std::erf(std::sqrt(arg / 2.0));
557 dans = std::exp(lans);
562 if (degreesOfFreedom == 0)
565 degreesOfFreedom = 2;
567 sum = 1.0 / ao - 1.0 - am;
569 probability = 1.0 + am * pans;
574 degreesOfFreedom = degreesOfFreedom - 1;
576 sum = 1.0 / ao - 1.0;
577 while (i < degreesOfFreedom)
579 degreesOfFreedom = degreesOfFreedom + 1;
584 for (++m; m < maxit; ++m)
589 density = density + am * dans;
591 probability = probability + hold;
592 if ((pans *
sum < eps2) && (hold < eps2))
break;
594 if (m == maxit)
THROW_EXCEPTION(
"noncentralChi2PDF_CDF(): no convergence.");
595 return std::make_pair(
596 0.5 * ao * density,
std::min(1.0, std::max(0.0, ao * probability)));
600 unsigned int degreesOfFreedom,
double arg,
double accuracy)
603 degreesOfFreedom, 0.0, arg, accuracy)
608 unsigned int degreesOfFreedom,
double noncentrality,
double arg)
610 const double a = degreesOfFreedom + noncentrality;
611 const double b = (
a + noncentrality) /
square(
a);
613 (std::pow((
double)arg /
a, 1.0 / 3.0) - (1.0 - 2.0 / 9.0 *
b)) /
614 std::sqrt(2.0 / 9.0 *
b);
615 return 0.5 * (1.0 + std::erf(
t / std::sqrt(2.0)));
633 if (
v.size() > 0)
s.WriteBufferFixEndianness(&
v[0],
v.size());
639 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 "CMatrixDynamic<double>".
#define THROW_EXCEPTION(msg)
double normalCDF(double p)
Evaluates the Gaussian cumulative density function.
GLdouble GLdouble GLdouble GLdouble q
size_type size() const
Get a 2-vector with [NROWS NCOLS] (as in MATLAB command size(x))
This file implements miscelaneous matrix and matrix/vector operations, and internal functions in mrpt...
void WriteAs(const TYPE_FROM_ACTUAL &value)
#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...
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.
CONTAINER::Scalar maximum(const CONTAINER &v)
#define ASSERTMSG_(f, __ERROR_MSG)
Defines an assertion mechanism.
typename vec_t::const_iterator const_iterator
CMatrixDouble cov(const MATRIX &v)
Computes the covariance matrix from a list of samples in an NxM matrix, where each row is a sample...
GLsizei const GLchar ** string
T wrapToPi(T a)
Modifies the given angle to translate it into the ]-pi,pi] range.
size_type rows() const
Number of rows in the matrix.
size_type cols() const
Number of columns in the matrix.
mrpt::serialization::CArchive & operator>>(mrpt::serialization::CArchive &in, CMatrixD::Ptr &pObj)
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
mrpt::serialization::CArchive & operator<<(mrpt::serialization::CArchive &s, const CVectorFloat &a)
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.
double noncentralChi2CDF(unsigned int degreesOfFreedom, double noncentrality, double arg)
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".
EIGEN_MAP asEigen()
Get as an Eigen-compatible Eigen::Map object.
This template class provides the basic functionality for a general 2D any-size, resizable container o...
GLubyte GLubyte GLubyte a
#define MRPT_UNUSED_PARAM(a)
Determines whether this is an X86 or AMD64 platform.