Go to the documentation of this file.
63 #define KOLMOG_CUTOVER 0.82
78 #define SM_UPPER_MAX_TERMS 3
79 #define SM_UPPERSUM_MIN_N 10
85 #define CLIP(X, LOW, HIGH) ((X) < LOW ? LOW : MIN(X, HIGH))
87 #define MIN(a,b) (((a) < (b)) ? (a) : (b))
90 #define MAX(a,b) (((a) < (b)) ? (b) : (a))
100 #define LOGSQRT2PI 0.91893853320467274178032973640561764
110 #define RETURN_3PROBS(PSF, PCDF, PDF) \
116 static const double _xtol = DBL_EPSILON;
117 static const double _rtol = 2*DBL_EPSILON;
130 #define valueD dd_to_double
131 #define add_dd dd_add_d_d
132 #define sub_dd dd_sub_d_d
133 #define mul_dd dd_mul_d_d
135 #define div_dd dd_div_d_d
136 #define add_DD dd_add
137 #define sub_DD dd_sub
138 #define mul_DD dd_mul
139 #define div_DD dd_div
140 #define add_Dd dd_add_dd_d
141 #define add_dD dd_add_d_dd
142 #define sub_Dd dd_sub_dd_d
143 #define sub_dD dd_sub_d_dd
144 #define mul_Dd dd_mul_dd_d
145 #define mul_dD dd_mul_d_dd
146 #define div_Dd dd_div_dd_d
147 #define div_dD dd_div_d_dd
148 #define frexpD dd_frexp
149 #define ldexpD dd_ldexp
151 #define log1pD dd_log1p
186 double u =
exp(logu8/8);
192 double logP = logu8/8 +
log(
w);
196 double u8 =
exp(logu8);
197 double u8cub =
pow(u8, 3);
219 double logv = -2*
x*
x;
220 double v =
exp(logv);
248 cdf =
CLIP(cdf, 0, 1);
260 double xmax = INFINITY;
262 double a = xmin,
b = xmax;
264 if (!(psf >= 0.0 && pcdf >= 0.0 && pcdf <= 1.0 && psf <= 1.0)) {
268 if (
fabs(1.0 - pcdf - psf) > 4* DBL_EPSILON) {
281 double logpcdf =
log(pcdf);
300 const double jiggerb = 256 * DBL_EPSILON;
301 double pba = psf/(1.0 -
exp(-4))/2, pbb = psf * (1 - jiggerb)/2;
314 q0 = 1 +
p3 * (1 +
p3 * (4 +
p2 *(-1 +
p*(22 +
p2* (-13 + 140 *
p)))));
318 if (x < a || x >
b) {
328 double df = ((pcdf < 0.5) ? (pcdf - probs.
cdf) : (probs.
sf - psf));
335 if (df > 0 &&
x >
a) {
337 }
else if (df < 0 &&
x <
b) {
342 if (
fabs(dfdx) <= 0.0) {
356 if (
x >=
a &&
x <=
b) {
360 if ((
x ==
a) || (
x ==
b)) {
363 if (
x ==
a ||
x ==
b) {
446 double q = ldexp(
x, 1-DBL_MANT_DIG);
460 modNX(
int n,
double x,
int *pNXFloor,
double *pNX)
472 alphaD =
sub_DD(nxD, nxfloorD);
500 man2 =
frexpD(man2, &expt);
524 ans =
pow(
a.x[0],
m);
527 if (
fabs(adj) > 1
e-8) {
528 if (
fabs(adj) < 1
e-4) {
530 adj += (
m*r) * ((
m-1)/2.0 * r);
549 #define _MAX_EXPONENT 960
551 #define RETURN_M_E(MAND, EXPT) \
563 int q, r, y2mE, y2rE, y2mqE;
591 double lgAns =
m * lg2y;
599 ans =
frexpD(ans1, &ansE);
612 ansE += y2mqE + y2rE;
672 if (0.5 <=
X.x[0] &&
X.x[0] <= 1.5) {
706 ansE = Cexpt + t1E + t2E;
723 int nxfl, n1mxfl, n1mxceil;
726 if (!(
n > 0 &&
x >= 0.0 &&
x <= 1.0)) {
740 n1mxfl =
n - nxfl - (
alpha == 0 ? 0 : 1);
752 if (nxfl == 0 || (nxfl == 1 &&
alpha == 0)) {
754 pdf = (nx + 1) *
t / (1+
x);
778 double logp = -
pow(6.0*
n*
x+1, 2)/18.0/
n;
787 pdf = (6.0*
n*
x+1) * 2 * sf/3;
796 int nUpperTerms =
n - n1mxceil + 1;
797 bUseUpperSum = (nUpperTerms <= 1 &&
x < 0.5);
798 bUseUpperSum = (bUseUpperSum ||
801 && (
x <= 0.5 /
sqrt(
n))));
805 int start=0,
step=1, nTerms=n1mxfl+1;
810 double2 Aj, dAj, t1, t2, dAjCoeff;
816 nTerms =
n - n1mxceil + 1;
823 dAjCoeff =
add_DD(dAjCoeff, oneOverX);
830 dAjCoeff =
div_Dd(dAjCoeff,
x);
831 dAjCoeff =
add_DD(dAjCoeff, oneOverX);
834 dAj =
mul_DD(Aj, dAjCoeff);
835 AjSum =
add_DD(AjSum, Aj);
836 dAjSum =
add_DD(dAjSum, dAj);
841 for (
j = firstJ;
j < nTerms;
j += 1) {
850 dAjCoeff =
add_DD(dAjCoeff, oneOverX);
851 dAj =
mul_DD(Aj, dAjCoeff);
854 AjSum =
add_DD(AjSum, Aj);
855 dAjSum =
add_DD(dAjSum, dAj);
860 && (
j != nTerms - 1)) {
879 assert (nx != 1 ||
alpha > 0);
893 cdf =
CLIP(cdf, 0, 1);
911 int function_calls = 0;
913 double maxlogpcdf, psfrootn;
916 if (!(
n > 0 && psf >= 0.0 && pcdf >= 0.0 && pcdf <= 1.0 && psf <= 1.0)) {
920 if (
fabs(1.0 - pcdf - psf) > 4* DBL_EPSILON) {
937 psfrootn =
pow(psf, 1.0 /
n);
939 if (
n < 150 &&
n*psfrootn <= 1) {
945 logpcdf = (pcdf < 0.5 ?
log(pcdf) :
log1p(-psf));
952 if (logpcdf <= maxlogpcdf) {
972 z0 = (z0*z0 +
R *
exp(1-z0))/(1+z0);
974 a = xmin*(1 - 4 * DBL_EPSILON);
976 b = xmax * (1 + 4 * DBL_EPSILON);
983 double xmin = 1 - psfrootn;
984 double logpsf = (psf < 0.5 ?
log(psf) :
log1p(-pcdf));
985 double xmax =
sqrt(-logpsf / (2.0
L *
n));
986 double xmax6 = xmax - 1.0L / (6 *
n);
990 a *= 1 - 4 * DBL_EPSILON;
991 b *= 1 + 4 * DBL_EPSILON;
992 a =
MAX(xmin, 1.0/
n);
993 b =
MIN(xmax, 1-1.0/
n);
996 if (x < a || x >
b) {
1013 double dfdx,
x0 =
x, deltax, df;
1019 df = ((pcdf < 0.5) ? (pcdf - probs.
cdf) : (probs.
sf - psf));
1026 if (df > 0 &&
x >
a) {
1028 }
else if (df < 0 &&
x <
b) {
1051 if ((
a <=
x) && (
x <=
b) && (
fabs(2 * deltax) <=
fabs(dxold) ||
fabs(dxold) < 256 * DBL_EPSILON)) {
1109 if (!(
n > 0 &&
d >= 0.0 &&
d <= 1.0)) {
static double2 dd_floor(const double2 a)
static int dd_to_int(const double2 a)
static double modNX(int n, double x, int *pNXFloor, double *pNX)
static double pow4(double a, double b, double c, double d, int m)
struct ThreeProbs ThreeProbs
Array< double, 1, 3 > e(1./3., 0.5, 2.)
static const double d[K][N]
#define CLIP(X, LOW, HIGH)
static double _smirnovi(int n, double psf, double pcdf)
#define RETURN_M_E(MAND, EXPT)
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy x
double smirnovi(int n, double p)
static void updateBinomial(double2 *Cman, int *Cexpt, int n, int j)
static int _within_tol(double x, double y, double atol, double rtol)
const EIGEN_DEVICE_FUNC LogReturnType log() const
#define RETURN_3PROBS(PSF, PCDF, PDF)
const EIGEN_DEVICE_FUNC ExpReturnType exp() const
static double2 dd_inv(const double2 a)
static double2 dd_add_d_d(double a, double b)
double smirnovci(int n, double p)
static const double _xtol
Matrix< SCALARA, Dynamic, Dynamic, opt_A > A
static void computeAv(int n, double x, int v, double2 Cman, int Cexpt, double2 *pt1, double2 *pt2, double2 *pAv)
double smirnovc(int n, double d)
double smirnovp(int n, double d)
static double logpow4(double a, double b, double c, double d, int m)
double smirnov(int n, double d)
double kolmogci(double p)
EIGEN_DEVICE_FUNC const Scalar & q
static double2 pow4_D(double a, double b, double c, double d, int m)
static double dd_to_double(const double2 a)
const int SMIRNOV_MAX_COMPUTE_N
static double2 pow2Scaled_D(double2 a, int m, int *pExponent)
def step(data, isam, result, truth, currPoseIndex, isamArgs=())
static double dd_hi(const double2 a)
static int dd_isfinite(const double2 a)
static int dd_is_zero(const double2 a)
static const double _rtol
static const int MIN_EXPABLE
static double pow2(double a, double b, int m)
static double _kolmogi(double psf, double pcdf)
static double nextPowerOf2(double x)
const Eigen::CwiseBinaryOp< Eigen::internal::scalar_pow_op< typename Derived::Scalar, typename ExponentDerived::Scalar >, const Derived, const ExponentDerived > pow(const Eigen::ArrayBase< Derived > &x, const Eigen::ArrayBase< ExponentDerived > &exponents)
static ThreeProbs _kolmogorov(double x)
Matrix< Scalar, Dynamic, Dynamic > C
static ThreeProbs _smirnov(int n, double x)
#define SM_UPPER_MAX_TERMS
void sf_error(const char *func_name, sf_error_t code, const char *fmt,...)
Array< int, Dynamic, 1 > v
double kolmogorov(double x)
static double2 logpow4_D(double a, double b, double c, double d, int m)
#define SM_UPPERSUM_MIN_N
static double2 pow_D(double2 a, int m)
Jet< T, N > sqrt(const Jet< T, N > &f)
Rot2 R(Rot2::fromAngle(0.1))
static int dd_is_negative(const double2 a)
gtsam
Author(s):
autogenerated on Fri Nov 1 2024 03:33:01