35 return ::exp(-0.5 *
square((
x - mu) / std)) /
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());