From d0b3f1dd25a7e7b7e4e49bbde024e9fa00170ac9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 May 2023 14:52:13 -0400 Subject: [PATCH 01/33] code for computing quantile of chi-squared distribution --- gtsam/nonlinear/GncHelpers.h | 516 +++++++++++++++++++++++++++++++++++ 1 file changed, 516 insertions(+) create mode 100644 gtsam/nonlinear/GncHelpers.h diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h new file mode 100644 index 0000000000..399da2c99a --- /dev/null +++ b/gtsam/nonlinear/GncHelpers.h @@ -0,0 +1,516 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file GncHelpers.h + * @brief Helper functions for use with the GncOptimizer + * @author Varun Agrawal + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/// Template type for numeric limits +template +using LIM = std::numeric_limits; + +template +using return_t = + typename std::conditional::value, double, T>::type; + +template +using common_t = typename std::common_type::type; + +template +using common_return_t = return_t>; + +/// Check if integer is odd +constexpr bool is_odd(const long long int x) noexcept { return (x & 1U) != 0; } + +/// Templated check for NaN +template +constexpr bool is_nan(const T x) noexcept { + return x != x; +} + +/// @brief Gauss-Legendre quadrature: 50 points +static const long double gauss_legendre_50_points[50] = { + -0.03109833832718887611232898966595L, 0.03109833832718887611232898966595L, + -0.09317470156008614085445037763960L, 0.09317470156008614085445037763960L, + -0.15489058999814590207162862094111L, 0.15489058999814590207162862094111L, + -0.21600723687604175684728453261710L, 0.21600723687604175684728453261710L, + -0.27628819377953199032764527852113L, 0.27628819377953199032764527852113L, + -0.33550024541943735683698825729107L, 0.33550024541943735683698825729107L, + -0.39341431189756512739422925382382L, 0.39341431189756512739422925382382L, + -0.44980633497403878914713146777838L, 0.44980633497403878914713146777838L, + -0.50445814490746420165145913184914L, 0.50445814490746420165145913184914L, + -0.55715830451465005431552290962580L, 0.55715830451465005431552290962580L, + -0.60770292718495023918038179639183L, 0.60770292718495023918038179639183L, + -0.65589646568543936078162486400368L, 0.65589646568543936078162486400368L, + -0.70155246870682225108954625788366L, 0.70155246870682225108954625788366L, + -0.74449430222606853826053625268219L, 0.74449430222606853826053625268219L, + -0.78455583290039926390530519634099L, 0.78455583290039926390530519634099L, + -0.82158207085933594835625411087394L, 0.82158207085933594835625411087394L, + -0.85542976942994608461136264393476L, 0.85542976942994608461136264393476L, + -0.88596797952361304863754098246675L, 0.88596797952361304863754098246675L, + -0.91307855665579189308973564277166L, 0.91307855665579189308973564277166L, + -0.93665661894487793378087494727250L, 0.93665661894487793378087494727250L, + -0.95661095524280794299774564415662L, 0.95661095524280794299774564415662L, + -0.97286438510669207371334410460625L, 0.97286438510669207371334410460625L, + -0.98535408404800588230900962563249L, 0.98535408404800588230900962563249L, + -0.99403196943209071258510820042069L, 0.99403196943209071258510820042069L, + -0.99886640442007105018545944497422L, 0.99886640442007105018545944497422L}; + +/// @brief Gauss-Legendre quadrature: 50 weights +static const long double gauss_legendre_50_weights[50] = { + 0.06217661665534726232103310736061L, 0.06217661665534726232103310736061L, + 0.06193606742068324338408750978083L, 0.06193606742068324338408750978083L, + 0.06145589959031666375640678608392L, 0.06145589959031666375640678608392L, + 0.06073797084177021603175001538481L, 0.06073797084177021603175001538481L, + 0.05978505870426545750957640531259L, 0.05978505870426545750957640531259L, + 0.05860084981322244583512243663085L, 0.05860084981322244583512243663085L, + 0.05718992564772838372302931506599L, 0.05718992564772838372302931506599L, + 0.05555774480621251762356742561227L, 0.05555774480621251762356742561227L, + 0.05371062188899624652345879725566L, 0.05371062188899624652345879725566L, + 0.05165570306958113848990529584010L, 0.05165570306958113848990529584010L, + 0.04940093844946631492124358075143L, 0.04940093844946631492124358075143L, + 0.04695505130394843296563301363499L, 0.04695505130394843296563301363499L, + 0.04432750433880327549202228683039L, 0.04432750433880327549202228683039L, + 0.04152846309014769742241197896407L, 0.04152846309014769742241197896407L, + 0.03856875661258767524477015023639L, 0.03856875661258767524477015023639L, + 0.03545983561514615416073461100098L, 0.03545983561514615416073461100098L, + 0.03221372822357801664816582732300L, 0.03221372822357801664816582732300L, + 0.02884299358053519802990637311323L, 0.02884299358053519802990637311323L, + 0.02536067357001239044019487838544L, 0.02536067357001239044019487838544L, + 0.02178024317012479298159206906269L, 0.02178024317012479298159206906269L, + 0.01811556071348939035125994342235L, 0.01811556071348939035125994342235L, + 0.01438082276148557441937890892732L, 0.01438082276148557441937890892732L, + 0.01059054838365096926356968149924L, 0.01059054838365096926356968149924L, + 0.00675979919574540150277887817799L, 0.00675979919574540150277887817799L, + 0.00290862255315514095840072434286L, 0.00290862255315514095840072434286L}; + +namespace internal { + +/// 50 point Gauss-Legendre quadrature +template +constexpr T incomplete_gamma_quad_inp_vals(const T lb, const T ub, + const int counter) noexcept { + return (ub - lb) * gauss_legendre_50_points[counter] / T(2) + + (ub + lb) / T(2); +} + +template +constexpr T incomplete_gamma_quad_weight_vals(const T lb, const T ub, + const int counter) noexcept { + return (ub - lb) * gauss_legendre_50_weights[counter] / T(2); +} + +template +constexpr T incomplete_gamma_quad_fn(const T x, const T a, + const T lg_term) noexcept { + return exp(-x + (a - T(1)) * log(x) - lg_term); +} + +template +constexpr T incomplete_gamma_quad_recur(const T lb, const T ub, const T a, + const T lg_term, + const int counter) noexcept { + return (counter < 49 ? // if + incomplete_gamma_quad_fn( + incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * + incomplete_gamma_quad_weight_vals(lb, ub, counter) + + incomplete_gamma_quad_recur(lb, ub, a, lg_term, counter + 1) + : + // else + incomplete_gamma_quad_fn( + incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * + incomplete_gamma_quad_weight_vals(lb, ub, counter)); +} + +template +constexpr T incomplete_gamma_quad_lb(const T a, const T z) noexcept { + // break integration into ranges + return (a > T(1000) ? std::max(T(0), std::min(z, a) - 11 * sqrt(a)) + : a > T(800) ? std::max(T(0), std::min(z, a) - 11 * sqrt(a)) + : a > T(500) ? std::max(T(0), std::min(z, a) - 10 * sqrt(a)) + : a > T(300) ? std::max(T(0), std::min(z, a) - 10 * sqrt(a)) + : a > T(100) ? std::max(T(0), std::min(z, a) - 9 * sqrt(a)) + : a > T(90) ? std::max(T(0), std::min(z, a) - 9 * sqrt(a)) + : a > T(70) ? std::max(T(0), std::min(z, a) - 8 * sqrt(a)) + : a > T(50) ? std::max(T(0), std::min(z, a) - 7 * sqrt(a)) + : a > T(40) ? std::max(T(0), std::min(z, a) - 6 * sqrt(a)) + : a > T(30) ? std::max(T(0), std::min(z, a) - 5 * sqrt(a)) + : std::max(T(0), std::min(z, a) - 4 * sqrt(a))); +} + +template +constexpr T incomplete_gamma_quad_ub(const T a, const T z) noexcept { + return (a > T(1000) ? std::min(z, a + 10 * sqrt(a)) + : a > T(800) ? std::min(z, a + 10 * sqrt(a)) + : a > T(500) ? std::min(z, a + 9 * sqrt(a)) + : a > T(300) ? std::min(z, a + 9 * sqrt(a)) + : a > T(100) ? std::min(z, a + 8 * sqrt(a)) + : a > T(90) ? std::min(z, a + 8 * sqrt(a)) + : a > T(70) ? std::min(z, a + 7 * sqrt(a)) + : a > T(50) ? std::min(z, a + 6 * sqrt(a)) + : std::min(z, a + 5 * sqrt(a))); +} + +template +constexpr T incomplete_gamma_quad(const T a, const T z) noexcept { + return incomplete_gamma_quad_recur(incomplete_gamma_quad_lb(a, z), + incomplete_gamma_quad_ub(a, z), a, + lgamma(a), 0); +} + +// reverse cf expansion +// see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ + +template +constexpr T incomplete_gamma_cf_2_recur(const T a, const T z, + const int depth) noexcept { + return (depth < 100 ? (1 + (depth - 1) * 2 - a + z) + + depth * (a - depth) / + incomplete_gamma_cf_2_recur(a, z, depth + 1) + : (1 + (depth - 1) * 2 - a + z)); +} + +template +constexpr T incomplete_gamma_cf_2( + const T a, + const T z) noexcept { // lower (regularized) incomplete gamma function + return (T(1.0) - exp(a * log(z) - z - lgamma(a)) / + incomplete_gamma_cf_2_recur(a, z, 1)); +} + +// cf expansion +// see: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ + +template +constexpr T incomplete_gamma_cf_1_coef(const T a, const T z, + const int depth) noexcept { + return (is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z + : T(depth) / T(2) * z); +} + +template +constexpr T incomplete_gamma_cf_1_recur(const T a, const T z, + const int depth) noexcept { + return (depth < 55 ? // if + (a + depth - 1) + incomplete_gamma_cf_1_coef(a, z, depth) / + incomplete_gamma_cf_1_recur(a, z, depth + 1) + : + // else + (a + depth - 1)); +} + +template +constexpr T incomplete_gamma_cf_1( + const T a, + const T z) noexcept { // lower (regularized) incomplete gamma function + return (exp(a * log(z) - z - lgamma(a)) / + incomplete_gamma_cf_1_recur(a, z, 1)); +} + +// + +template +constexpr T incomplete_gamma_check(const T a, const T z) noexcept { + return ( // NaN check + (is_nan(a) || is_nan(z)) ? LIM::quiet_NaN() : + // + a < T(0) ? LIM::quiet_NaN() + : + // + LIM::min() > z ? T(0) + : + // + LIM::min() > a ? T(1) + : + // cf or quadrature + (a < T(10)) && (z - a < T(10)) ? incomplete_gamma_cf_1(a, z) + : (a < T(10)) || (z / a > T(3)) ? incomplete_gamma_cf_2(a, z) + : + // else + incomplete_gamma_quad(a, z)); +} + +template > +constexpr TC incomplete_gamma_type_check(const T1 a, const T2 p) noexcept { + return incomplete_gamma_check(static_cast(a), static_cast(p)); +} + +} // namespace internal + +/** + * Compile-time regularized lower incomplete gamma function + * + * @param a a real-valued, non-negative input. + * @param x a real-valued, non-negative input. + * + * @return the regularized lower incomplete gamma function evaluated at (\c a, + * \c x), \f[ \frac{\gamma(a,x)}{\Gamma(a)} = \frac{1}{\Gamma(a)} \int_0^x + * t^{a-1} \exp(-t) dt \f] When \c a is not too large, the value is computed + * using the continued fraction representation of the upper incomplete gamma + * function, \f$ \Gamma(a,x) \f$, using \f[ \Gamma(a,x) = \Gamma(a) - + * \dfrac{x^a\exp(-x)}{a - \dfrac{ax}{a + 1 + \dfrac{x}{a + 2 - \dfrac{(a+1)x}{a + * + 3 + \dfrac{2x}{a + 4 - \ddots}}}}} \f] where \f$ \gamma(a,x) \f$ and \f$ + * \Gamma(a,x) \f$ are connected via \f[ \frac{\gamma(a,x)}{\Gamma(a)} + + * \frac{\Gamma(a,x)}{\Gamma(a)} = 1 \f] When \f$ a > 10 \f$, a 50-point + * Gauss-Legendre quadrature scheme is employed. + */ +template +constexpr common_return_t incomplete_gamma(const T1 a, + const T2 x) noexcept { + return internal::incomplete_gamma_type_check(a, x); +} + +namespace internal { + +template +class IncompleteGammaInverse { + /** + * @brief Compute an initial value for the inverse gamma function which is + * then iteratively updated. + * + * @param a + * @param p + * @return constexpr T + */ + static constexpr T initial_val(const T a, const T p) noexcept { + if (a > T(1)) { + // Inverse gamma function initial value when a > 1.0 + const T t_val = p > T(0.5) ? sqrt(-2 * log(T(1) - p)) : sqrt(-2 * log(p)); + const T sgn_term = p > T(0.5) ? T(-1) : T(1); + const T initial_val_1 = + t_val - + (T(2.515517L) + T(0.802853L) * t_val + T(0.010328L) * t_val * t_val) / + (T(1) + T(1.432788L) * t_val + T(0.189269L) * t_val * t_val + + T(0.001308L) * t_val * t_val * t_val); + const T signed_initial_val_1 = sgn_term * initial_val_1; + + return std::max( + T(1e-04), + a * pow(T(1) - T(1) / (9 * a) - signed_initial_val_1 / (3 * sqrt(a)), + 3)); + } else { + // Inverse gamma function initial value when a <= 1.0 + T t_val = T(1) - T(0.253) * a - T(0.12) * a * a; + if (p < t_val) { + return pow(p / t_val, T(1) / a); + } else { + return T(1) - log(T(1) - (p - t_val) / (T(1) - t_val)); + } + } + } + + /** + * @brief Compute the error value `f(x)` which we can use for root-finding. + * + * @param value + * @param a + * @param p + * @return constexpr T + */ + static constexpr T err_val(const T value, const T a, const T p) noexcept { + return (incomplete_gamma(a, value) - p); + } + + /** + * @brief Derivative of the incomplete gamma function w.r.t. value + * + * @param value + * @param a + * @param log_val + * @return constexpr T + */ + static constexpr T derivative(const T value, const T a, + const T lg_val) noexcept { + return (exp(-value + (a - T(1)) * log(value) - lg_val)); + } + + /** + * @brief Second derivative of the incomplete gamma function w.r.t. value + * + * @param value + * @param a + * @param derivative + * @return constexpr T + */ + static constexpr T second_derivative(const T value, const T a, + const T derivative) noexcept { + return (derivative * ((a - T(1)) / value - T(1))); + } + + /** + * @brief Compute \f[ \frac{f(x_n)}{f'(x_n)} \f] as part + * of the update denominator. + * + * @param value + * @param a + * @param p + * @param derivative + * @return constexpr T + */ + static constexpr T ratio_val_1(const T value, const T a, const T p, + const T derivative) noexcept { + return (err_val(value, a, p) / derivative); + } + + /** + * @brief Compute \f[ \frac{f''(x_n)}{f'(x_n)} \f] as part + * of the update denominator. + * + * @param value + * @param a + * @param derivative + * @return constexpr T + */ + static constexpr T ratio_val_2(const T value, const T a, + const T derivative) noexcept { + return (second_derivative(value, a, derivative) / derivative); + } + + /** + * @brief Halley's method update step + * + * @param ratio_val_1 + * @param ratio_val_2 + * @return constexpr T + */ + static constexpr T halley(const T ratio_val_1, const T ratio_val_2) noexcept { + return (ratio_val_1 / + std::max(T(0.8), std::min(T(1.2), T(1) - T(0.5) * ratio_val_1 * + ratio_val_2))); + } + /** + * @brief Recursive method for computing the iterative solution for the + * incomplete inverse gamma function. + * + * @param value + * @param a + * @param p + * @param derivative + * @param lg_val + * @param iter_count + * @return constexpr T + */ + static constexpr T recurse(const T value, const T a, const T p, + const T derivative, const T lg_val, + const int iter_count) noexcept { + return decision(value, a, p, + halley(ratio_val_1(value, a, p, derivative), + ratio_val_2(value, a, derivative)), + lg_val, iter_count); + } + + static constexpr T decision(const T value, const T a, const T p, + const T direc, const T lg_val, + const int iter_count) noexcept { + const int GAMMA_INV_MAX_ITER = 35; + if (iter_count <= GAMMA_INV_MAX_ITER) { + return recurse(value - direc, a, p, derivative(value, a, lg_val), lg_val, + iter_count + 1); + } else { + return value - direc; + } + } + + /** + * @brief Start point for numerical computation of the incomplete gamma + * inverse funtion. + * + * @param initial_val Initial value guess + * @param a + * @param p + * @param lg_val + * @return constexpr T + */ + static constexpr T begin(const T initial_val, const T a, const T p, + const T lg_val) noexcept { + return recurse(initial_val, a, p, derivative(initial_val, a, lg_val), + lg_val, 1); + } + + public: + /** + * @brief Compute the percent point function for the Gamma distribution. + * + * @param a + * @param p + * @return constexpr T + */ + static constexpr T compute(const T a, const T p) noexcept { + // Perform checks on the input and return the corresponding best answer + if (isnan(a) || isnan(p)) { // NaN check + return LIM::quiet_NaN(); + } else if (LIM::min() > p) { // Check lower bound + return T(0); + } else if (p > T(1)) { // Check upper bound + return LIM::quiet_NaN(); + } else if (LIM::min() > abs(T(1) - p)) { + return LIM::infinity(); + } else if (LIM::min() > a) { // Check lower bound for degrees of freedom + return T(0); + } else { + return begin(initial_val(a, p), a, p, lgamma(a)); + } + } +}; + +} // namespace internal + +/** + * Compile-time inverse incomplete gamma function + * + * Compute the value \f$ x \f$ + * such that \f[ f(x) := \frac{\gamma(a,x)}{\Gamma(a)} - p \f] equal to zero, + * for a given \c p. + * + * We find this root using Halley's method: + * \f[ x_{n+1} = x_n - \frac{f(x_n)/f'(x_n)}{1 - 0.5 \frac{f(x_n)}{f'(x_n)} + * \frac{f''(x_n)}{f'(x_n)} } \f] where + * \f[ \frac{\partial}{\partial x} \left(\frac{\gamma(a,x)}{\Gamma(a)}\right) = + * \frac{1}{\Gamma(a)} x^{a-1} \exp(-x) \f] \f[ \frac{\partial^2}{\partial x^2} + * \left(\frac{\gamma(a,x)}{\Gamma(a)}\right) = \frac{1}{\Gamma(a)} x^{a-1} + * \exp(-x) \left( \frac{a-1}{x} - 1 \right) \f] + * + * @param a The degrees of freedom for the gamma distribution. + * @param p The quantile value for computing the percent point function. + * + * @return Computes the inverse incomplete gamma function. + */ +template +constexpr common_return_t incomplete_gamma_inv(const T1 a, + const T2 p) noexcept { + using TC = common_return_t; + return internal::IncompleteGammaInverse::compute(static_cast(a), + static_cast(p)); +} + +/** + * @brief Compute the quantile function of the Chi squared distribution. + * + * @param dofs Degrees of freedom + * @param alpha Quantile value + * @return constexpr double + */ +constexpr double chi_squared_quantile(const size_t dofs, const double alpha) { + // The quantile function of the Chi-squared distribution is the quantile of + // the specific (inverse) incomplete Gamma distribution + return 2 * incomplete_gamma_inv(dofs * 0.5, alpha); +} + +} // namespace gtsam From 8201c77b30b3d69d0a9b775b8eb164be9650ae3c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 May 2023 15:37:46 -0400 Subject: [PATCH 02/33] refactor IncompleteGamma class --- gtsam/nonlinear/GncHelpers.h | 267 +++++++++++++++++------------------ 1 file changed, 131 insertions(+), 136 deletions(-) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 399da2c99a..38185249c4 100644 --- a/gtsam/nonlinear/GncHelpers.h +++ b/gtsam/nonlinear/GncHelpers.h @@ -103,154 +103,147 @@ static const long double gauss_legendre_50_weights[50] = { namespace internal { -/// 50 point Gauss-Legendre quadrature template -constexpr T incomplete_gamma_quad_inp_vals(const T lb, const T ub, - const int counter) noexcept { - return (ub - lb) * gauss_legendre_50_points[counter] / T(2) + - (ub + lb) / T(2); -} - -template -constexpr T incomplete_gamma_quad_weight_vals(const T lb, const T ub, - const int counter) noexcept { - return (ub - lb) * gauss_legendre_50_weights[counter] / T(2); -} - -template -constexpr T incomplete_gamma_quad_fn(const T x, const T a, - const T lg_term) noexcept { - return exp(-x + (a - T(1)) * log(x) - lg_term); -} - -template -constexpr T incomplete_gamma_quad_recur(const T lb, const T ub, const T a, - const T lg_term, - const int counter) noexcept { - return (counter < 49 ? // if - incomplete_gamma_quad_fn( - incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * - incomplete_gamma_quad_weight_vals(lb, ub, counter) + - incomplete_gamma_quad_recur(lb, ub, a, lg_term, counter + 1) - : - // else - incomplete_gamma_quad_fn( - incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * - incomplete_gamma_quad_weight_vals(lb, ub, counter)); -} - -template -constexpr T incomplete_gamma_quad_lb(const T a, const T z) noexcept { - // break integration into ranges - return (a > T(1000) ? std::max(T(0), std::min(z, a) - 11 * sqrt(a)) - : a > T(800) ? std::max(T(0), std::min(z, a) - 11 * sqrt(a)) - : a > T(500) ? std::max(T(0), std::min(z, a) - 10 * sqrt(a)) - : a > T(300) ? std::max(T(0), std::min(z, a) - 10 * sqrt(a)) - : a > T(100) ? std::max(T(0), std::min(z, a) - 9 * sqrt(a)) - : a > T(90) ? std::max(T(0), std::min(z, a) - 9 * sqrt(a)) - : a > T(70) ? std::max(T(0), std::min(z, a) - 8 * sqrt(a)) - : a > T(50) ? std::max(T(0), std::min(z, a) - 7 * sqrt(a)) - : a > T(40) ? std::max(T(0), std::min(z, a) - 6 * sqrt(a)) - : a > T(30) ? std::max(T(0), std::min(z, a) - 5 * sqrt(a)) - : std::max(T(0), std::min(z, a) - 4 * sqrt(a))); -} +class IncompleteGamma { + /// 50 point Gauss-Legendre quadrature + static constexpr T quadrature_inp_vals(const T lb, const T ub, + const int counter) noexcept { + return (ub - lb) * gauss_legendre_50_points[counter] / T(2) + + (ub + lb) / T(2); + } -template -constexpr T incomplete_gamma_quad_ub(const T a, const T z) noexcept { - return (a > T(1000) ? std::min(z, a + 10 * sqrt(a)) - : a > T(800) ? std::min(z, a + 10 * sqrt(a)) - : a > T(500) ? std::min(z, a + 9 * sqrt(a)) - : a > T(300) ? std::min(z, a + 9 * sqrt(a)) - : a > T(100) ? std::min(z, a + 8 * sqrt(a)) - : a > T(90) ? std::min(z, a + 8 * sqrt(a)) - : a > T(70) ? std::min(z, a + 7 * sqrt(a)) - : a > T(50) ? std::min(z, a + 6 * sqrt(a)) - : std::min(z, a + 5 * sqrt(a))); -} + static constexpr T quadrature_weight_vals(const T lb, const T ub, + const int counter) noexcept { + return (ub - lb) * gauss_legendre_50_weights[counter] / T(2); + } -template -constexpr T incomplete_gamma_quad(const T a, const T z) noexcept { - return incomplete_gamma_quad_recur(incomplete_gamma_quad_lb(a, z), - incomplete_gamma_quad_ub(a, z), a, - lgamma(a), 0); -} + static constexpr T quadrature_fn(const T x, const T a, + const T lg_term) noexcept { + return exp(-x + (a - T(1)) * log(x) - lg_term); + } -// reverse cf expansion -// see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ + static constexpr T quadrature_recur(const T lb, const T ub, const T a, + const T lg_term, + const int counter) noexcept { + if (counter < 49) { + return quadrature_fn(quadrature_inp_vals(lb, ub, counter), a, lg_term) * + quadrature_weight_vals(lb, ub, counter) + + quadrature_recur(lb, ub, a, lg_term, counter + 1); + } else { + return quadrature_fn(quadrature_inp_vals(lb, ub, counter), a, lg_term) * + quadrature_weight_vals(lb, ub, counter); + } + } -template -constexpr T incomplete_gamma_cf_2_recur(const T a, const T z, - const int depth) noexcept { - return (depth < 100 ? (1 + (depth - 1) * 2 - a + z) + - depth * (a - depth) / - incomplete_gamma_cf_2_recur(a, z, depth + 1) - : (1 + (depth - 1) * 2 - a + z)); -} + static constexpr T quadrature_lb(const T a, const T z) noexcept { + // break integration into ranges + return a > T(1000) ? std::max(T(0), std::min(z, a) - 11 * sqrt(a)) + : a > T(800) ? std::max(T(0), std::min(z, a) - 11 * sqrt(a)) + : a > T(500) ? std::max(T(0), std::min(z, a) - 10 * sqrt(a)) + : a > T(300) ? std::max(T(0), std::min(z, a) - 10 * sqrt(a)) + : a > T(100) ? std::max(T(0), std::min(z, a) - 9 * sqrt(a)) + : a > T(90) ? std::max(T(0), std::min(z, a) - 9 * sqrt(a)) + : a > T(70) ? std::max(T(0), std::min(z, a) - 8 * sqrt(a)) + : a > T(50) ? std::max(T(0), std::min(z, a) - 7 * sqrt(a)) + : a > T(40) ? std::max(T(0), std::min(z, a) - 6 * sqrt(a)) + : a > T(30) ? std::max(T(0), std::min(z, a) - 5 * sqrt(a)) + : std::max(T(0), std::min(z, a) - 4 * sqrt(a)); + } -template -constexpr T incomplete_gamma_cf_2( - const T a, - const T z) noexcept { // lower (regularized) incomplete gamma function - return (T(1.0) - exp(a * log(z) - z - lgamma(a)) / - incomplete_gamma_cf_2_recur(a, z, 1)); -} + static constexpr T quadrature_ub(const T a, const T z) noexcept { + return a > T(1000) ? std::min(z, a + 10 * sqrt(a)) + : a > T(800) ? std::min(z, a + 10 * sqrt(a)) + : a > T(500) ? std::min(z, a + 9 * sqrt(a)) + : a > T(300) ? std::min(z, a + 9 * sqrt(a)) + : a > T(100) ? std::min(z, a + 8 * sqrt(a)) + : a > T(90) ? std::min(z, a + 8 * sqrt(a)) + : a > T(70) ? std::min(z, a + 7 * sqrt(a)) + : a > T(50) ? std::min(z, a + 6 * sqrt(a)) + : std::min(z, a + 5 * sqrt(a)); + } -// cf expansion -// see: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ + static constexpr T quadrature(const T a, const T z) noexcept { + return quadrature_recur(quadrature_lb(a, z), quadrature_ub(a, z), a, + lgamma(a), 0); + } -template -constexpr T incomplete_gamma_cf_1_coef(const T a, const T z, - const int depth) noexcept { - return (is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z - : T(depth) / T(2) * z); -} + // reverse cf expansion + // see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ + static constexpr T cf_2_recur(const T a, const T z, + const int depth) noexcept { + if (depth < 100) { + return (1 + (depth - 1) * 2 - a + z) + + depth * (a - depth) / cf_2_recur(a, z, depth + 1); + } else { + return 1 + (depth - 1) * 2 - a + z; + } + } -template -constexpr T incomplete_gamma_cf_1_recur(const T a, const T z, - const int depth) noexcept { - return (depth < 55 ? // if - (a + depth - 1) + incomplete_gamma_cf_1_coef(a, z, depth) / - incomplete_gamma_cf_1_recur(a, z, depth + 1) - : - // else - (a + depth - 1)); -} + /** + * @brief Lower (regularized) incomplete gamma function + * + * @param a + * @param z + * @return constexpr T + */ + static constexpr T cf_2(const T a, const T z) noexcept { + return T(1.0) - exp(a * log(z) - z - lgamma(a)) / cf_2_recur(a, z, 1); + } -template -constexpr T incomplete_gamma_cf_1( - const T a, - const T z) noexcept { // lower (regularized) incomplete gamma function - return (exp(a * log(z) - z - lgamma(a)) / - incomplete_gamma_cf_1_recur(a, z, 1)); -} + // continued fraction expansion + // see: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ + static constexpr T cf_1_coef(const T a, const T z, const int depth) noexcept { + return (is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z + : T(depth) / T(2) * z); + } -// + static constexpr T cf_1_recur(const T a, const T z, + const int depth) noexcept { + if (depth < 55) { + return (a + depth - 1) + + cf_1_coef(a, z, depth) / cf_1_recur(a, z, depth + 1); + } else { + return (a + depth - 1); + } + } -template -constexpr T incomplete_gamma_check(const T a, const T z) noexcept { - return ( // NaN check - (is_nan(a) || is_nan(z)) ? LIM::quiet_NaN() : - // - a < T(0) ? LIM::quiet_NaN() - : - // - LIM::min() > z ? T(0) - : - // - LIM::min() > a ? T(1) - : - // cf or quadrature - (a < T(10)) && (z - a < T(10)) ? incomplete_gamma_cf_1(a, z) - : (a < T(10)) || (z / a > T(3)) ? incomplete_gamma_cf_2(a, z) - : - // else - incomplete_gamma_quad(a, z)); -} + /** + * @brief Lower (regularized) incomplete gamma function + * + * @param a + * @param z + * @return constexpr T + */ + static constexpr T cf_1(const T a, const T z) noexcept { + return exp(a * log(z) - z - lgamma(a)) / cf_1_recur(a, z, 1); + } -template > -constexpr TC incomplete_gamma_type_check(const T1 a, const T2 p) noexcept { - return incomplete_gamma_check(static_cast(a), static_cast(p)); -} + public: + /** + * @brief Compute the CDF for the Gamma distribution. + * + * @param a + * @param z + * @return constexpr T + */ + static constexpr T compute(const T a, const T z) noexcept { + if (is_nan(a) || is_nan(z)) { // NaN check + return LIM::quiet_NaN(); + } else if (a < T(0)) { + return LIM::quiet_NaN(); + } else if (LIM::min() > z) { + return T(0); + } else if (LIM::min() > a) { + return T(1); + } else if (a < T(10) && z - a < T(10)) { + return cf_1(a, z); + } else if (a < T(10) || z / a > T(3)) { + return cf_2(a, z); + } else { + return quadrature(a, z); + } + } +}; } // namespace internal @@ -274,7 +267,9 @@ constexpr TC incomplete_gamma_type_check(const T1 a, const T2 p) noexcept { template constexpr common_return_t incomplete_gamma(const T1 a, const T2 x) noexcept { - return internal::incomplete_gamma_type_check(a, x); + using TC = common_return_t; + return internal::IncompleteGamma::compute(static_cast(a), + static_cast(x)); } namespace internal { From d5771609a4475b7af937b8d2c7140653d93554fe Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 May 2023 15:54:42 -0400 Subject: [PATCH 03/33] Simplified IncompleteGamma --- gtsam/nonlinear/GncHelpers.h | 69 ++++++++++++++++++++---------------- 1 file changed, 38 insertions(+), 31 deletions(-) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 38185249c4..92e2599a5c 100644 --- a/gtsam/nonlinear/GncHelpers.h +++ b/gtsam/nonlinear/GncHelpers.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { @@ -105,13 +106,14 @@ namespace internal { template class IncompleteGamma { - /// 50 point Gauss-Legendre quadrature + /// 50 point Gauss-Legendre quadrature values static constexpr T quadrature_inp_vals(const T lb, const T ub, const int counter) noexcept { return (ub - lb) * gauss_legendre_50_points[counter] / T(2) + (ub + lb) / T(2); } + /// 50 point Gauss-Legendre quadrature weights static constexpr T quadrature_weight_vals(const T lb, const T ub, const int counter) noexcept { return (ub - lb) * gauss_legendre_50_weights[counter] / T(2); @@ -122,19 +124,6 @@ class IncompleteGamma { return exp(-x + (a - T(1)) * log(x) - lg_term); } - static constexpr T quadrature_recur(const T lb, const T ub, const T a, - const T lg_term, - const int counter) noexcept { - if (counter < 49) { - return quadrature_fn(quadrature_inp_vals(lb, ub, counter), a, lg_term) * - quadrature_weight_vals(lb, ub, counter) + - quadrature_recur(lb, ub, a, lg_term, counter + 1); - } else { - return quadrature_fn(quadrature_inp_vals(lb, ub, counter), a, lg_term) * - quadrature_weight_vals(lb, ub, counter); - } - } - static constexpr T quadrature_lb(const T a, const T z) noexcept { // break integration into ranges return a > T(1000) ? std::max(T(0), std::min(z, a) - 11 * sqrt(a)) @@ -163,12 +152,27 @@ class IncompleteGamma { } static constexpr T quadrature(const T a, const T z) noexcept { - return quadrature_recur(quadrature_lb(a, z), quadrature_ub(a, z), a, - lgamma(a), 0); + T lb = quadrature_lb(a, z); + T ub = quadrature_ub(a, z); + T lg_term = lgamma(a); + T value = quadrature_fn(quadrature_inp_vals(lb, ub, 49), a, lg_term) * + quadrature_weight_vals(lb, ub, 49); + for (size_t counter = 48; counter >= 0; counter--) { + value += quadrature_fn(quadrature_inp_vals(lb, ub, counter), a, lg_term) * + quadrature_weight_vals(lb, ub, counter); + } + return value; } - // reverse cf expansion - // see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ + /** + * @brief Reverse continued fraction expansion + * See: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ + * + * @param a + * @param z + * @param depth + * @return constexpr T + */ static constexpr T cf_2_recur(const T a, const T z, const int depth) noexcept { if (depth < 100) { @@ -186,22 +190,25 @@ class IncompleteGamma { * @param z * @return constexpr T */ - static constexpr T cf_2(const T a, const T z) noexcept { + static constexpr T continued_fraction_2(const T a, const T z) noexcept { return T(1.0) - exp(a * log(z) - z - lgamma(a)) / cf_2_recur(a, z, 1); } - // continued fraction expansion - // see: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ - static constexpr T cf_1_coef(const T a, const T z, const int depth) noexcept { - return (is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z - : T(depth) / T(2) * z); - } - + /** + * @brief Continued fraction expansion of Gamma function + * See: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ + * + * @param a + * @param z + * @param depth + * @return constexpr T + */ static constexpr T cf_1_recur(const T a, const T z, const int depth) noexcept { if (depth < 55) { - return (a + depth - 1) + - cf_1_coef(a, z, depth) / cf_1_recur(a, z, depth + 1); + T cf_coef = is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z + : T(depth) / T(2) * z; + return (a + depth - 1) + cf_coef / cf_1_recur(a, z, depth + 1); } else { return (a + depth - 1); } @@ -214,7 +221,7 @@ class IncompleteGamma { * @param z * @return constexpr T */ - static constexpr T cf_1(const T a, const T z) noexcept { + static constexpr T continued_fraction_1(const T a, const T z) noexcept { return exp(a * log(z) - z - lgamma(a)) / cf_1_recur(a, z, 1); } @@ -236,9 +243,9 @@ class IncompleteGamma { } else if (LIM::min() > a) { return T(1); } else if (a < T(10) && z - a < T(10)) { - return cf_1(a, z); + return continued_fraction_1(a, z); } else if (a < T(10) || z / a > T(3)) { - return cf_2(a, z); + return continued_fraction_2(a, z); } else { return quadrature(a, z); } From 7ce5684e058cf533d1712205d92e778bfa426644 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 May 2023 16:16:20 -0400 Subject: [PATCH 04/33] remove recursion for Halley update --- gtsam/nonlinear/GncHelpers.h | 85 +++++++++++++----------------------- 1 file changed, 31 insertions(+), 54 deletions(-) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 92e2599a5c..1340958a35 100644 --- a/gtsam/nonlinear/GncHelpers.h +++ b/gtsam/nonlinear/GncHelpers.h @@ -168,7 +168,7 @@ class IncompleteGamma { * @brief Reverse continued fraction expansion * See: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ * - * @param a + * @param a Degrees of freedom * @param z * @param depth * @return constexpr T @@ -186,7 +186,7 @@ class IncompleteGamma { /** * @brief Lower (regularized) incomplete gamma function * - * @param a + * @param a Degrees of freedom * @param z * @return constexpr T */ @@ -198,7 +198,7 @@ class IncompleteGamma { * @brief Continued fraction expansion of Gamma function * See: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ * - * @param a + * @param a Degrees of freedom * @param z * @param depth * @return constexpr T @@ -217,7 +217,7 @@ class IncompleteGamma { /** * @brief Lower (regularized) incomplete gamma function * - * @param a + * @param a Degrees of freedom * @param z * @return constexpr T */ @@ -229,7 +229,7 @@ class IncompleteGamma { /** * @brief Compute the CDF for the Gamma distribution. * - * @param a + * @param a Degrees of freedom * @param z * @return constexpr T */ @@ -287,7 +287,7 @@ class IncompleteGammaInverse { * @brief Compute an initial value for the inverse gamma function which is * then iteratively updated. * - * @param a + * @param a Degrees of freedom * @param p * @return constexpr T */ @@ -322,7 +322,7 @@ class IncompleteGammaInverse { * @brief Compute the error value `f(x)` which we can use for root-finding. * * @param value - * @param a + * @param a Degrees of freedom * @param p * @return constexpr T */ @@ -334,12 +334,12 @@ class IncompleteGammaInverse { * @brief Derivative of the incomplete gamma function w.r.t. value * * @param value - * @param a + * @param a Degrees of freedom * @param log_val * @return constexpr T */ - static constexpr T derivative(const T value, const T a, - const T lg_val) noexcept { + static constexpr T first_derivative(const T value, const T a, + const T lg_val) noexcept { return (exp(-value + (a - T(1)) * log(value) - lg_val)); } @@ -347,7 +347,7 @@ class IncompleteGammaInverse { * @brief Second derivative of the incomplete gamma function w.r.t. value * * @param value - * @param a + * @param a Degrees of freedom * @param derivative * @return constexpr T */ @@ -361,7 +361,7 @@ class IncompleteGammaInverse { * of the update denominator. * * @param value - * @param a + * @param a Degrees of freedom * @param p * @param derivative * @return constexpr T @@ -376,7 +376,7 @@ class IncompleteGammaInverse { * of the update denominator. * * @param value - * @param a + * @param a Degrees of freedom * @param derivative * @return constexpr T */ @@ -386,7 +386,7 @@ class IncompleteGammaInverse { } /** - * @brief Halley's method update step + * @brief Halley's method update delta * * @param ratio_val_1 * @param ratio_val_2 @@ -397,60 +397,37 @@ class IncompleteGammaInverse { std::max(T(0.8), std::min(T(1.2), T(1) - T(0.5) * ratio_val_1 * ratio_val_2))); } + /** - * @brief Recursive method for computing the iterative solution for the + * @brief Compute the iterative solution for the * incomplete inverse gamma function. * - * @param value - * @param a + * @param initial_val Initial value guess + * @param a Degrees of freedom * @param p - * @param derivative * @param lg_val - * @param iter_count * @return constexpr T */ - static constexpr T recurse(const T value, const T a, const T p, - const T derivative, const T lg_val, - const int iter_count) noexcept { - return decision(value, a, p, - halley(ratio_val_1(value, a, p, derivative), - ratio_val_2(value, a, derivative)), - lg_val, iter_count); - } - - static constexpr T decision(const T value, const T a, const T p, - const T direc, const T lg_val, - const int iter_count) noexcept { + static constexpr T find_root(const T initial_val, const T a, const T p, + const T lg_val) noexcept { const int GAMMA_INV_MAX_ITER = 35; - if (iter_count <= GAMMA_INV_MAX_ITER) { - return recurse(value - direc, a, p, derivative(value, a, lg_val), lg_val, - iter_count + 1); - } else { - return value - direc; + T x = initial_val; + T derivative = first_derivative(initial_val, a, lg_val); + for (size_t counter = 1; counter <= GAMMA_INV_MAX_ITER; counter++) { + T direc = halley(ratio_val_1(x, a, p, derivative), + ratio_val_2(x, a, derivative)); + derivative = first_derivative(x, a, lg_val); + x = x - direc; } - } - - /** - * @brief Start point for numerical computation of the incomplete gamma - * inverse funtion. - * - * @param initial_val Initial value guess - * @param a - * @param p - * @param lg_val - * @return constexpr T - */ - static constexpr T begin(const T initial_val, const T a, const T p, - const T lg_val) noexcept { - return recurse(initial_val, a, p, derivative(initial_val, a, lg_val), - lg_val, 1); + return x - halley(ratio_val_1(x, a, p, derivative), + ratio_val_2(x, a, derivative)); } public: /** * @brief Compute the percent point function for the Gamma distribution. * - * @param a + * @param a Degrees of freedom * @param p * @return constexpr T */ @@ -467,7 +444,7 @@ class IncompleteGammaInverse { } else if (LIM::min() > a) { // Check lower bound for degrees of freedom return T(0); } else { - return begin(initial_val(a, p), a, p, lgamma(a)); + return find_root(initial_val(a, p), a, p, lgamma(a)); } } }; From a807127b512e41704712bdca53922a3bf2224309 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 11 May 2023 13:17:17 -0400 Subject: [PATCH 05/33] update GncOptimizer and make it available --- gtsam/CMakeLists.txt | 1 - gtsam/nonlinear/GncOptimizer.h | 5 ++--- tests/CMakeLists.txt | 1 - 3 files changed, 2 insertions(+), 5 deletions(-) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index efd0e9dc23..40028ad001 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -59,7 +59,6 @@ endif() # if GTSAM_USE_BOOST_FEATURES is not set, then we need to exclude the following: if(NOT GTSAM_USE_BOOST_FEATURES) list (APPEND excluded_sources - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/GncOptimizer.h" "${CMAKE_CURRENT_SOURCE_DIR}/inference/graph.h" "${CMAKE_CURRENT_SOURCE_DIR}/inference/graph-inl.h" ) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index d59eb43718..414bee5eb4 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -26,9 +26,9 @@ #pragma once +#include #include #include -#include namespace gtsam { /* @@ -36,8 +36,7 @@ namespace gtsam { * Equivalent to chi2inv in Matlab. */ static double Chi2inv(const double alpha, const size_t dofs) { - boost::math::chi_squared_distribution chi2(dofs); - return boost::math::quantile(chi2, alpha); + return chi_squared_quantile(dofs, alpha); } /* ************************************************************************* */ diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index d7b68c4eca..66812d6bb0 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -8,7 +8,6 @@ if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # might not be best test - Richar endif() if (NOT GTSAM_USE_BOOST_FEATURES) - list(APPEND excluded_tests "testGncOptimizer.cpp") list(APPEND excluded_tests "testGraph.cpp") endif() From 6fb3f0f209dcfa52245469814f6820c29248cd07 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 11 May 2023 13:58:55 -0400 Subject: [PATCH 06/33] use templated is_nan check --- gtsam/nonlinear/GncHelpers.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 1340958a35..6852e81313 100644 --- a/gtsam/nonlinear/GncHelpers.h +++ b/gtsam/nonlinear/GncHelpers.h @@ -18,7 +18,6 @@ #pragma once #include -#include #include namespace gtsam { @@ -433,7 +432,7 @@ class IncompleteGammaInverse { */ static constexpr T compute(const T a, const T p) noexcept { // Perform checks on the input and return the corresponding best answer - if (isnan(a) || isnan(p)) { // NaN check + if (is_nan(a) || is_nan(p)) { // NaN check return LIM::quiet_NaN(); } else if (LIM::min() > p) { // Check lower bound return T(0); From d614fda81f229b3b7404f5c163e4f23f86bbba60 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 11 May 2023 18:41:33 -0400 Subject: [PATCH 07/33] try older version --- gtsam/nonlinear/GncHelpers.h | 656 +++++++++++++++++------------------ 1 file changed, 326 insertions(+), 330 deletions(-) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 6852e81313..2dac9ac5e0 100644 --- a/gtsam/nonlinear/GncHelpers.h +++ b/gtsam/nonlinear/GncHelpers.h @@ -18,7 +18,6 @@ #pragma once #include -#include namespace gtsam { @@ -103,153 +102,154 @@ static const long double gauss_legendre_50_weights[50] = { namespace internal { +/// 50 point Gauss-Legendre quadrature template -class IncompleteGamma { - /// 50 point Gauss-Legendre quadrature values - static constexpr T quadrature_inp_vals(const T lb, const T ub, - const int counter) noexcept { - return (ub - lb) * gauss_legendre_50_points[counter] / T(2) + - (ub + lb) / T(2); - } - - /// 50 point Gauss-Legendre quadrature weights - static constexpr T quadrature_weight_vals(const T lb, const T ub, - const int counter) noexcept { - return (ub - lb) * gauss_legendre_50_weights[counter] / T(2); - } - - static constexpr T quadrature_fn(const T x, const T a, - const T lg_term) noexcept { - return exp(-x + (a - T(1)) * log(x) - lg_term); - } - - static constexpr T quadrature_lb(const T a, const T z) noexcept { - // break integration into ranges - return a > T(1000) ? std::max(T(0), std::min(z, a) - 11 * sqrt(a)) - : a > T(800) ? std::max(T(0), std::min(z, a) - 11 * sqrt(a)) - : a > T(500) ? std::max(T(0), std::min(z, a) - 10 * sqrt(a)) - : a > T(300) ? std::max(T(0), std::min(z, a) - 10 * sqrt(a)) - : a > T(100) ? std::max(T(0), std::min(z, a) - 9 * sqrt(a)) - : a > T(90) ? std::max(T(0), std::min(z, a) - 9 * sqrt(a)) - : a > T(70) ? std::max(T(0), std::min(z, a) - 8 * sqrt(a)) - : a > T(50) ? std::max(T(0), std::min(z, a) - 7 * sqrt(a)) - : a > T(40) ? std::max(T(0), std::min(z, a) - 6 * sqrt(a)) - : a > T(30) ? std::max(T(0), std::min(z, a) - 5 * sqrt(a)) - : std::max(T(0), std::min(z, a) - 4 * sqrt(a)); - } - - static constexpr T quadrature_ub(const T a, const T z) noexcept { - return a > T(1000) ? std::min(z, a + 10 * sqrt(a)) - : a > T(800) ? std::min(z, a + 10 * sqrt(a)) - : a > T(500) ? std::min(z, a + 9 * sqrt(a)) - : a > T(300) ? std::min(z, a + 9 * sqrt(a)) - : a > T(100) ? std::min(z, a + 8 * sqrt(a)) - : a > T(90) ? std::min(z, a + 8 * sqrt(a)) - : a > T(70) ? std::min(z, a + 7 * sqrt(a)) - : a > T(50) ? std::min(z, a + 6 * sqrt(a)) - : std::min(z, a + 5 * sqrt(a)); - } - - static constexpr T quadrature(const T a, const T z) noexcept { - T lb = quadrature_lb(a, z); - T ub = quadrature_ub(a, z); - T lg_term = lgamma(a); - T value = quadrature_fn(quadrature_inp_vals(lb, ub, 49), a, lg_term) * - quadrature_weight_vals(lb, ub, 49); - for (size_t counter = 48; counter >= 0; counter--) { - value += quadrature_fn(quadrature_inp_vals(lb, ub, counter), a, lg_term) * - quadrature_weight_vals(lb, ub, counter); - } - return value; - } - - /** - * @brief Reverse continued fraction expansion - * See: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ - * - * @param a Degrees of freedom - * @param z - * @param depth - * @return constexpr T - */ - static constexpr T cf_2_recur(const T a, const T z, - const int depth) noexcept { - if (depth < 100) { - return (1 + (depth - 1) * 2 - a + z) + - depth * (a - depth) / cf_2_recur(a, z, depth + 1); - } else { - return 1 + (depth - 1) * 2 - a + z; - } - } - - /** - * @brief Lower (regularized) incomplete gamma function - * - * @param a Degrees of freedom - * @param z - * @return constexpr T - */ - static constexpr T continued_fraction_2(const T a, const T z) noexcept { - return T(1.0) - exp(a * log(z) - z - lgamma(a)) / cf_2_recur(a, z, 1); - } - - /** - * @brief Continued fraction expansion of Gamma function - * See: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ - * - * @param a Degrees of freedom - * @param z - * @param depth - * @return constexpr T - */ - static constexpr T cf_1_recur(const T a, const T z, - const int depth) noexcept { - if (depth < 55) { - T cf_coef = is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z - : T(depth) / T(2) * z; - return (a + depth - 1) + cf_coef / cf_1_recur(a, z, depth + 1); - } else { - return (a + depth - 1); - } - } - - /** - * @brief Lower (regularized) incomplete gamma function - * - * @param a Degrees of freedom - * @param z - * @return constexpr T - */ - static constexpr T continued_fraction_1(const T a, const T z) noexcept { - return exp(a * log(z) - z - lgamma(a)) / cf_1_recur(a, z, 1); - } - - public: - /** - * @brief Compute the CDF for the Gamma distribution. - * - * @param a Degrees of freedom - * @param z - * @return constexpr T - */ - static constexpr T compute(const T a, const T z) noexcept { - if (is_nan(a) || is_nan(z)) { // NaN check - return LIM::quiet_NaN(); - } else if (a < T(0)) { - return LIM::quiet_NaN(); - } else if (LIM::min() > z) { - return T(0); - } else if (LIM::min() > a) { - return T(1); - } else if (a < T(10) && z - a < T(10)) { - return continued_fraction_1(a, z); - } else if (a < T(10) || z / a > T(3)) { - return continued_fraction_2(a, z); - } else { - return quadrature(a, z); - } - } -}; +constexpr T incomplete_gamma_quad_inp_vals(const T lb, const T ub, + const int counter) noexcept { + return (ub - lb) * gauss_legendre_50_points[counter] / T(2) + + (ub + lb) / T(2); +} + +template +constexpr T incomplete_gamma_quad_weight_vals(const T lb, const T ub, + const int counter) noexcept { + return (ub - lb) * gauss_legendre_50_weights[counter] / T(2); +} + +template +constexpr T incomplete_gamma_quad_fn(const T x, const T a, + const T lg_term) noexcept { + return exp(-x + (a - T(1)) * log(x) - lg_term); +} + +template +constexpr T incomplete_gamma_quad_recur(const T lb, const T ub, const T a, + const T lg_term, + const int counter) noexcept { + return (counter < 49 ? // if + incomplete_gamma_quad_fn( + incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * + incomplete_gamma_quad_weight_vals(lb, ub, counter) + + incomplete_gamma_quad_recur(lb, ub, a, lg_term, counter + 1) + : + // else + incomplete_gamma_quad_fn( + incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * + incomplete_gamma_quad_weight_vals(lb, ub, counter)); +} + +template +constexpr T incomplete_gamma_quad_lb(const T a, const T z) noexcept { + // break integration into ranges + return (a > T(1000) ? std::max(T(0), std::min(z, a) - 11 * sqrt(a)) + : a > T(800) ? std::max(T(0), std::min(z, a) - 11 * sqrt(a)) + : a > T(500) ? std::max(T(0), std::min(z, a) - 10 * sqrt(a)) + : a > T(300) ? std::max(T(0), std::min(z, a) - 10 * sqrt(a)) + : a > T(100) ? std::max(T(0), std::min(z, a) - 9 * sqrt(a)) + : a > T(90) ? std::max(T(0), std::min(z, a) - 9 * sqrt(a)) + : a > T(70) ? std::max(T(0), std::min(z, a) - 8 * sqrt(a)) + : a > T(50) ? std::max(T(0), std::min(z, a) - 7 * sqrt(a)) + : a > T(40) ? std::max(T(0), std::min(z, a) - 6 * sqrt(a)) + : a > T(30) ? std::max(T(0), std::min(z, a) - 5 * sqrt(a)) + : std::max(T(0), std::min(z, a) - 4 * sqrt(a))); +} + +template +constexpr T incomplete_gamma_quad_ub(const T a, const T z) noexcept { + return (a > T(1000) ? std::min(z, a + 10 * sqrt(a)) + : a > T(800) ? std::min(z, a + 10 * sqrt(a)) + : a > T(500) ? std::min(z, a + 9 * sqrt(a)) + : a > T(300) ? std::min(z, a + 9 * sqrt(a)) + : a > T(100) ? std::min(z, a + 8 * sqrt(a)) + : a > T(90) ? std::min(z, a + 8 * sqrt(a)) + : a > T(70) ? std::min(z, a + 7 * sqrt(a)) + : a > T(50) ? std::min(z, a + 6 * sqrt(a)) + : std::min(z, a + 5 * sqrt(a))); +} + +template +constexpr T incomplete_gamma_quad(const T a, const T z) noexcept { + return incomplete_gamma_quad_recur(incomplete_gamma_quad_lb(a, z), + incomplete_gamma_quad_ub(a, z), a, + lgamma(a), 0); +} + +// reverse cf expansion +// see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ + +template +constexpr T incomplete_gamma_cf_2_recur(const T a, const T z, + const int depth) noexcept { + return (depth < 100 ? (1 + (depth - 1) * 2 - a + z) + + depth * (a - depth) / + incomplete_gamma_cf_2_recur(a, z, depth + 1) + : (1 + (depth - 1) * 2 - a + z)); +} + +template +constexpr T incomplete_gamma_cf_2( + const T a, + const T z) noexcept { // lower (regularized) incomplete gamma function + return (T(1.0) - exp(a * log(z) - z - lgamma(a)) / + incomplete_gamma_cf_2_recur(a, z, 1)); +} + +// cf expansion +// see: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ + +template +constexpr T incomplete_gamma_cf_1_coef(const T a, const T z, + const int depth) noexcept { + return (is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z + : T(depth) / T(2) * z); +} + +template +constexpr T incomplete_gamma_cf_1_recur(const T a, const T z, + const int depth) noexcept { + return (depth < 55 ? // if + (a + depth - 1) + incomplete_gamma_cf_1_coef(a, z, depth) / + incomplete_gamma_cf_1_recur(a, z, depth + 1) + : + // else + (a + depth - 1)); +} + +template +constexpr T incomplete_gamma_cf_1( + const T a, + const T z) noexcept { // lower (regularized) incomplete gamma function + return (exp(a * log(z) - z - lgamma(a)) / + incomplete_gamma_cf_1_recur(a, z, 1)); +} + +// + +template +constexpr T incomplete_gamma_check(const T a, const T z) noexcept { + return ( // NaN check + (is_nan(a) || is_nan(z)) ? LIM::quiet_NaN() : + // + a < T(0) ? LIM::quiet_NaN() + : + // + LIM::min() > z ? T(0) + : + // + LIM::min() > a ? T(1) + : + // cf or quadrature + (a < T(10)) && (z - a < T(10)) ? incomplete_gamma_cf_1(a, z) + : (a < T(10)) || (z / a > T(3)) ? incomplete_gamma_cf_2(a, z) + : + // else + incomplete_gamma_quad(a, z)); +} + +template > +constexpr TC incomplete_gamma_type_check(const T1 a, const T2 p) noexcept { + return incomplete_gamma_check(static_cast(a), static_cast(p)); +} } // namespace internal @@ -270,212 +270,208 @@ class IncompleteGamma { * \frac{\Gamma(a,x)}{\Gamma(a)} = 1 \f] When \f$ a > 10 \f$, a 50-point * Gauss-Legendre quadrature scheme is employed. */ + template constexpr common_return_t incomplete_gamma(const T1 a, const T2 x) noexcept { - using TC = common_return_t; - return internal::IncompleteGamma::compute(static_cast(a), - static_cast(x)); + return internal::incomplete_gamma_type_check(a, x); } namespace internal { template -class IncompleteGammaInverse { - /** - * @brief Compute an initial value for the inverse gamma function which is - * then iteratively updated. - * - * @param a Degrees of freedom - * @param p - * @return constexpr T - */ - static constexpr T initial_val(const T a, const T p) noexcept { - if (a > T(1)) { - // Inverse gamma function initial value when a > 1.0 - const T t_val = p > T(0.5) ? sqrt(-2 * log(T(1) - p)) : sqrt(-2 * log(p)); - const T sgn_term = p > T(0.5) ? T(-1) : T(1); - const T initial_val_1 = - t_val - +constexpr T incomplete_gamma_inv_decision(const T value, const T a, const T p, + const T direc, const T lg_val, + const int iter_count) noexcept; + +// +// initial value for Halley's method +template +constexpr T incomplete_gamma_inv_t_val_1(const T p) noexcept { // a > 1.0 + return (p > T(0.5) ? sqrt(-2 * log(T(1) - p)) : sqrt(-2 * log(p))); +} + +template +constexpr T incomplete_gamma_inv_t_val_2(const T a) noexcept { // a <= 1.0 + return (T(1) - T(0.253) * a - T(0.12) * a * a); +} + +// +template +constexpr T incomplete_gamma_inv_initial_val_1_int_begin( + const T t_val) noexcept { // internal for a > 1.0 + return (t_val - (T(2.515517L) + T(0.802853L) * t_val + T(0.010328L) * t_val * t_val) / (T(1) + T(1.432788L) * t_val + T(0.189269L) * t_val * t_val + - T(0.001308L) * t_val * t_val * t_val); - const T signed_initial_val_1 = sgn_term * initial_val_1; - - return std::max( - T(1e-04), - a * pow(T(1) - T(1) / (9 * a) - signed_initial_val_1 / (3 * sqrt(a)), - 3)); - } else { - // Inverse gamma function initial value when a <= 1.0 - T t_val = T(1) - T(0.253) * a - T(0.12) * a * a; - if (p < t_val) { - return pow(p / t_val, T(1) / a); - } else { - return T(1) - log(T(1) - (p - t_val) / (T(1) - t_val)); - } - } - } - - /** - * @brief Compute the error value `f(x)` which we can use for root-finding. - * - * @param value - * @param a Degrees of freedom - * @param p - * @return constexpr T - */ - static constexpr T err_val(const T value, const T a, const T p) noexcept { - return (incomplete_gamma(a, value) - p); - } - - /** - * @brief Derivative of the incomplete gamma function w.r.t. value - * - * @param value - * @param a Degrees of freedom - * @param log_val - * @return constexpr T - */ - static constexpr T first_derivative(const T value, const T a, - const T lg_val) noexcept { - return (exp(-value + (a - T(1)) * log(value) - lg_val)); - } - - /** - * @brief Second derivative of the incomplete gamma function w.r.t. value - * - * @param value - * @param a Degrees of freedom - * @param derivative - * @return constexpr T - */ - static constexpr T second_derivative(const T value, const T a, - const T derivative) noexcept { - return (derivative * ((a - T(1)) / value - T(1))); - } - - /** - * @brief Compute \f[ \frac{f(x_n)}{f'(x_n)} \f] as part - * of the update denominator. - * - * @param value - * @param a Degrees of freedom - * @param p - * @param derivative - * @return constexpr T - */ - static constexpr T ratio_val_1(const T value, const T a, const T p, - const T derivative) noexcept { - return (err_val(value, a, p) / derivative); - } - - /** - * @brief Compute \f[ \frac{f''(x_n)}{f'(x_n)} \f] as part - * of the update denominator. - * - * @param value - * @param a Degrees of freedom - * @param derivative - * @return constexpr T - */ - static constexpr T ratio_val_2(const T value, const T a, - const T derivative) noexcept { - return (second_derivative(value, a, derivative) / derivative); - } - - /** - * @brief Halley's method update delta - * - * @param ratio_val_1 - * @param ratio_val_2 - * @return constexpr T - */ - static constexpr T halley(const T ratio_val_1, const T ratio_val_2) noexcept { - return (ratio_val_1 / - std::max(T(0.8), std::min(T(1.2), T(1) - T(0.5) * ratio_val_1 * - ratio_val_2))); - } - - /** - * @brief Compute the iterative solution for the - * incomplete inverse gamma function. - * - * @param initial_val Initial value guess - * @param a Degrees of freedom - * @param p - * @param lg_val - * @return constexpr T - */ - static constexpr T find_root(const T initial_val, const T a, const T p, - const T lg_val) noexcept { - const int GAMMA_INV_MAX_ITER = 35; - T x = initial_val; - T derivative = first_derivative(initial_val, a, lg_val); - for (size_t counter = 1; counter <= GAMMA_INV_MAX_ITER; counter++) { - T direc = halley(ratio_val_1(x, a, p, derivative), - ratio_val_2(x, a, derivative)); - derivative = first_derivative(x, a, lg_val); - x = x - direc; - } - return x - halley(ratio_val_1(x, a, p, derivative), - ratio_val_2(x, a, derivative)); - } - - public: - /** - * @brief Compute the percent point function for the Gamma distribution. - * - * @param a Degrees of freedom - * @param p - * @return constexpr T - */ - static constexpr T compute(const T a, const T p) noexcept { - // Perform checks on the input and return the corresponding best answer - if (is_nan(a) || is_nan(p)) { // NaN check - return LIM::quiet_NaN(); - } else if (LIM::min() > p) { // Check lower bound - return T(0); - } else if (p > T(1)) { // Check upper bound - return LIM::quiet_NaN(); - } else if (LIM::min() > abs(T(1) - p)) { - return LIM::infinity(); - } else if (LIM::min() > a) { // Check lower bound for degrees of freedom - return T(0); - } else { - return find_root(initial_val(a, p), a, p, lgamma(a)); - } - } -}; + T(0.001308L) * t_val * t_val * t_val)); +} + +template +constexpr T incomplete_gamma_inv_initial_val_1_int_end( + const T value_inp, const T a) noexcept { // internal for a > 1.0 + return std::max( + T(1E-04), a * pow(T(1) - T(1) / (9 * a) - value_inp / (3 * sqrt(a)), 3)); +} + +template +constexpr T incomplete_gamma_inv_initial_val_1( + const T a, const T t_val, const T sgn_term) noexcept { // a > 1.0 + return incomplete_gamma_inv_initial_val_1_int_end( + sgn_term * incomplete_gamma_inv_initial_val_1_int_begin(t_val), a); +} + +template +constexpr T incomplete_gamma_inv_initial_val_2( + const T a, const T p, const T t_val) noexcept { // a <= 1.0 + return (p < t_val ? // if + pow(p / t_val, T(1) / a) + : + // else + T(1) - log(T(1) - (p - t_val) / (T(1) - t_val))); +} + +// initial value + +template +constexpr T incomplete_gamma_inv_initial_val(const T a, const T p) noexcept { + return (a > T(1) ? // if + incomplete_gamma_inv_initial_val_1( + a, incomplete_gamma_inv_t_val_1(p), p > T(0.5) ? T(-1) : T(1)) + : + // else + incomplete_gamma_inv_initial_val_2( + a, p, incomplete_gamma_inv_t_val_2(a))); +} + +// +// Halley recursion + +template +constexpr T incomplete_gamma_inv_err_val( + const T value, const T a, const T p) noexcept { // err_val = f(x) + return (incomplete_gamma(a, value) - p); +} + +template +constexpr T incomplete_gamma_inv_deriv_1( + const T value, const T a, + const T lg_val) noexcept { // derivative of the incomplete gamma function + // w.r.t. x + return (exp(-value + (a - T(1)) * log(value) - lg_val)); +} + +template +constexpr T incomplete_gamma_inv_deriv_2( + const T value, const T a, + const T deriv_1) noexcept { // second derivative of the incomplete gamma + // function w.r.t. x + return (deriv_1 * ((a - T(1)) / value - T(1))); +} + +template +constexpr T incomplete_gamma_inv_ratio_val_1(const T value, const T a, + const T p, + const T deriv_1) noexcept { + return (incomplete_gamma_inv_err_val(value, a, p) / deriv_1); +} + +template +constexpr T incomplete_gamma_inv_ratio_val_2(const T value, const T a, + const T deriv_1) noexcept { + return (incomplete_gamma_inv_deriv_2(value, a, deriv_1) / deriv_1); +} + +template +constexpr T incomplete_gamma_inv_halley(const T ratio_val_1, + const T ratio_val_2) noexcept { + return (ratio_val_1 / + std::max(T(0.8), std::min(T(1.2), T(1) - T(0.5) * ratio_val_1 * + ratio_val_2))); +} + +template +constexpr T incomplete_gamma_inv_recur(const T value, const T a, const T p, + const T deriv_1, const T lg_val, + const int iter_count) noexcept { + return incomplete_gamma_inv_decision( + value, a, p, + incomplete_gamma_inv_halley( + incomplete_gamma_inv_ratio_val_1(value, a, p, deriv_1), + incomplete_gamma_inv_ratio_val_2(value, a, deriv_1)), + lg_val, iter_count); +} + +template +constexpr T incomplete_gamma_inv_decision(const T value, const T a, const T p, + const T direc, const T lg_val, + const int iter_count) noexcept { +// return( abs(direc) > GCEM_INCML_GAMMA_INV_TOL ? +// incomplete_gamma_inv_recur(value - direc, a, p, +// incomplete_gamma_inv_deriv_1(value,a,lg_val), lg_val) : value - direc ); +#define INCML_GAMMA_INV_MAX_ITER 35 + return (iter_count <= INCML_GAMMA_INV_MAX_ITER ? // if + incomplete_gamma_inv_recur( + value - direc, a, p, + incomplete_gamma_inv_deriv_1(value, a, lg_val), lg_val, + iter_count + 1) + : + // else + value - direc); +} + +template +constexpr T incomplete_gamma_inv_begin(const T initial_val, const T a, + const T p, const T lg_val) noexcept { + return incomplete_gamma_inv_recur( + initial_val, a, p, incomplete_gamma_inv_deriv_1(initial_val, a, lg_val), + lg_val, 1); +} + +template +constexpr T incomplete_gamma_inv_check(const T a, const T p) noexcept { + return ( // NaN check + (is_nan(a) || is_nan(p)) ? LIM::quiet_NaN() : + // + LIM::min() > p ? T(0) + : p > T(1) ? LIM::quiet_NaN() + : LIM::min() > abs(T(1) - p) ? LIM::infinity() + : + // + LIM::min() > a ? T(0) + : + // else + incomplete_gamma_inv_begin(incomplete_gamma_inv_initial_val(a, p), a, + p, lgamma(a))); +} + +template > +constexpr TC incomplete_gamma_inv_type_check(const T1 a, const T2 p) noexcept { + return incomplete_gamma_inv_check(static_cast(a), static_cast(p)); +} } // namespace internal /** * Compile-time inverse incomplete gamma function * - * Compute the value \f$ x \f$ - * such that \f[ f(x) := \frac{\gamma(a,x)}{\Gamma(a)} - p \f] equal to zero, - * for a given \c p. + * @param a a real-valued, non-negative input. + * @param p a real-valued input with values in the unit-interval. * - * We find this root using Halley's method: - * \f[ x_{n+1} = x_n - \frac{f(x_n)/f'(x_n)}{1 - 0.5 \frac{f(x_n)}{f'(x_n)} - * \frac{f''(x_n)}{f'(x_n)} } \f] where - * \f[ \frac{\partial}{\partial x} \left(\frac{\gamma(a,x)}{\Gamma(a)}\right) = - * \frac{1}{\Gamma(a)} x^{a-1} \exp(-x) \f] \f[ \frac{\partial^2}{\partial x^2} + * @return Computes the inverse incomplete gamma function, a value \f$ x \f$ + * such that \f[ f(x) := \frac{\gamma(a,x)}{\Gamma(a)} - p \f] equal to zero, + * for a given \c p. GCE-Math finds this root using Halley's method: \f[ x_{n+1} + * = x_n - \frac{f(x_n)/f'(x_n)}{1 - 0.5 \frac{f(x_n)}{f'(x_n)} + * \frac{f''(x_n)}{f'(x_n)} } \f] where \f[ \frac{\partial}{\partial x} + * \left(\frac{\gamma(a,x)}{\Gamma(a)}\right) = \frac{1}{\Gamma(a)} x^{a-1} + * \exp(-x) \f] \f[ \frac{\partial^2}{\partial x^2} * \left(\frac{\gamma(a,x)}{\Gamma(a)}\right) = \frac{1}{\Gamma(a)} x^{a-1} * \exp(-x) \left( \frac{a-1}{x} - 1 \right) \f] - * - * @param a The degrees of freedom for the gamma distribution. - * @param p The quantile value for computing the percent point function. - * - * @return Computes the inverse incomplete gamma function. */ + template constexpr common_return_t incomplete_gamma_inv(const T1 a, const T2 p) noexcept { - using TC = common_return_t; - return internal::IncompleteGammaInverse::compute(static_cast(a), - static_cast(p)); + return internal::incomplete_gamma_inv_type_check(a, p); } /** From 64c28504adf2a6abb272141a616728be0e4e76ce Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Jul 2023 12:54:03 -0400 Subject: [PATCH 08/33] switch from IndexVector to FastVector now that pybind/stl.h is enabled --- gtsam/nonlinear/GncParams.h | 11 ++++------- tests/testGncOptimizer.cpp | 22 +++++++++++----------- 2 files changed, 15 insertions(+), 18 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 10ac80663a..b792fa0581 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -73,13 +73,10 @@ class GncParams { double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level - //TODO(Varun) replace IndexVector with vector once pybind11/stl.h is globally enabled. - /// Use IndexVector for inliers and outliers since it is fast + wrapping - using IndexVector = FastVector; ///< Slots in the factor graph corresponding to measurements that we know are inliers - IndexVector knownInliers = IndexVector(); + FastVector knownInliers; ///< Slots in the factor graph corresponding to measurements that we know are outliers - IndexVector knownOutliers = IndexVector(); + FastVector knownOutliers; /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { @@ -120,7 +117,7 @@ class GncParams { * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and * only apply GNC to prune outliers from the loop closures. * */ - void setKnownInliers(const IndexVector& knownIn) { + void setKnownInliers(const FastVector& knownIn) { for (size_t i = 0; i < knownIn.size(); i++){ knownInliers.push_back(knownIn[i]); } @@ -131,7 +128,7 @@ class GncParams { * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. * */ - void setKnownOutliers(const IndexVector& knownOut) { + void setKnownOutliers(const FastVector& knownOut) { for (size_t i = 0; i < knownOut.size(); i++){ knownOutliers.push_back(knownOut[i]); } diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 5424a5744f..28261683bc 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { Values initial; initial.insert(X(1), p0); - GncParams::IndexVector knownInliers; + FastVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) { Values initial; initial.insert(X(1), p0); - GncParams::IndexVector knownInliers; + FastVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) { Values initial; initial.insert(X(1), p0); - GncParams::IndexVector knownInliers; + FastVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -761,7 +761,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // GNC // Note: in difficult instances, we set the odometry measurements to be // inliers, but this problem is simple enought to succeed even without that - // assumption GncParams::IndexVector knownInliers; + // assumption; GncParams gncParams; auto gnc = GncOptimizer>(*graph, *initial, gncParams); @@ -782,12 +782,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers (check early stopping // when all measurements are known to be inliers or outliers) { - GncParams::IndexVector knownInliers; + FastVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); - GncParams::IndexVector knownOutliers; + FastVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -811,11 +811,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers { - GncParams::IndexVector knownInliers; + FastVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - GncParams::IndexVector knownOutliers; + FastVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -839,7 +839,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // only known outliers { - GncParams::IndexVector knownOutliers; + FastVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -914,11 +914,11 @@ TEST(GncOptimizer, setWeights) { // initialize weights and also set known inliers/outliers { GncParams gncParams; - GncParams::IndexVector knownInliers; + FastVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - GncParams::IndexVector knownOutliers; + FastVector knownOutliers; knownOutliers.push_back(3); gncParams.setKnownInliers(knownInliers); gncParams.setKnownOutliers(knownOutliers); From a5fd9c120b3159812c196836f82e537cf6b43c07 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Jul 2023 12:54:32 -0400 Subject: [PATCH 09/33] fix chi_squared_quantile --- gtsam/nonlinear/GncHelpers.h | 168 +++++++++++------------ gtsam/nonlinear/tests/testGncHelpers.cpp | 37 +++++ 2 files changed, 121 insertions(+), 84 deletions(-) create mode 100644 gtsam/nonlinear/tests/testGncHelpers.cpp diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 2dac9ac5e0..7a27a35305 100644 --- a/gtsam/nonlinear/GncHelpers.h +++ b/gtsam/nonlinear/GncHelpers.h @@ -29,9 +29,11 @@ template using return_t = typename std::conditional::value, double, T>::type; +/// Get common type amongst all arguments template using common_t = typename std::common_type::type; +/// Helper template for finding common return type template using common_return_t = return_t>; @@ -126,16 +128,14 @@ template constexpr T incomplete_gamma_quad_recur(const T lb, const T ub, const T a, const T lg_term, const int counter) noexcept { - return (counter < 49 ? // if - incomplete_gamma_quad_fn( - incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * - incomplete_gamma_quad_weight_vals(lb, ub, counter) + - incomplete_gamma_quad_recur(lb, ub, a, lg_term, counter + 1) - : - // else - incomplete_gamma_quad_fn( - incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * - incomplete_gamma_quad_weight_vals(lb, ub, counter)); + T val = incomplete_gamma_quad_fn( + incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * + incomplete_gamma_quad_weight_vals(lb, ub, counter); + if (counter < 49) { + return val + incomplete_gamma_quad_recur(lb, ub, a, lg_term, counter + 1); + } else { + return val; + } } template @@ -180,10 +180,13 @@ constexpr T incomplete_gamma_quad(const T a, const T z) noexcept { template constexpr T incomplete_gamma_cf_2_recur(const T a, const T z, const int depth) noexcept { - return (depth < 100 ? (1 + (depth - 1) * 2 - a + z) + - depth * (a - depth) / - incomplete_gamma_cf_2_recur(a, z, depth + 1) - : (1 + (depth - 1) * 2 - a + z)); + T val = 1 + (depth - 1) * 2 - a + z; + if (depth < 100) { + return val + + depth * (a - depth) / incomplete_gamma_cf_2_recur(a, z, depth + 1); + } else { + return val; + } } template @@ -200,50 +203,49 @@ constexpr T incomplete_gamma_cf_2( template constexpr T incomplete_gamma_cf_1_coef(const T a, const T z, const int depth) noexcept { - return (is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z - : T(depth) / T(2) * z); + if (is_odd(depth)) { + return -(a - 1 + T(depth + 1) / T(2)) * z; + } else { + return T(depth) / T(2) * z; + } } template constexpr T incomplete_gamma_cf_1_recur(const T a, const T z, const int depth) noexcept { - return (depth < 55 ? // if - (a + depth - 1) + incomplete_gamma_cf_1_coef(a, z, depth) / - incomplete_gamma_cf_1_recur(a, z, depth + 1) - : - // else - (a + depth - 1)); + T val = a + depth - 1; + if (depth < 55) { + return val + incomplete_gamma_cf_1_coef(a, z, depth) / + incomplete_gamma_cf_1_recur(a, z, depth + 1); + } else { + return val; + } } +/// lower (regularized) incomplete gamma function template -constexpr T incomplete_gamma_cf_1( - const T a, - const T z) noexcept { // lower (regularized) incomplete gamma function - return (exp(a * log(z) - z - lgamma(a)) / - incomplete_gamma_cf_1_recur(a, z, 1)); +constexpr T incomplete_gamma_cf_1(const T a, const T z) noexcept { + return exp(a * log(z) - z - lgamma(a)) / incomplete_gamma_cf_1_recur(a, z, 1); } -// - +/// Perform NaN check template constexpr T incomplete_gamma_check(const T a, const T z) noexcept { - return ( // NaN check - (is_nan(a) || is_nan(z)) ? LIM::quiet_NaN() : - // - a < T(0) ? LIM::quiet_NaN() - : - // - LIM::min() > z ? T(0) - : - // - LIM::min() > a ? T(1) - : - // cf or quadrature - (a < T(10)) && (z - a < T(10)) ? incomplete_gamma_cf_1(a, z) - : (a < T(10)) || (z / a > T(3)) ? incomplete_gamma_cf_2(a, z) - : - // else - incomplete_gamma_quad(a, z)); + if (is_nan(a) || is_nan(z)) { + return LIM::quiet_NaN(); + } else if (a < T(0)) { + return LIM::quiet_NaN(); + } else if (LIM::min() > z) { + return T(0); + } else if (LIM::min() > a) { + return T(1); + } else if (a < T(10) && z - a < T(10)) { // cf or quadrature + return incomplete_gamma_cf_1(a, z); + } else if (a < T(10) || z / a > T(3)) { + return incomplete_gamma_cf_2(a, z); + } else { + return incomplete_gamma_quad(a, z); + } } template > @@ -323,24 +325,24 @@ constexpr T incomplete_gamma_inv_initial_val_1( template constexpr T incomplete_gamma_inv_initial_val_2( const T a, const T p, const T t_val) noexcept { // a <= 1.0 - return (p < t_val ? // if - pow(p / t_val, T(1) / a) - : - // else - T(1) - log(T(1) - (p - t_val) / (T(1) - t_val))); + if (p < t_val) { + return pow(p / t_val, T(1) / a); + } else { + return T(1) - log(T(1) - (p - t_val) / (T(1) - t_val)); + } } -// initial value +// Initial value template constexpr T incomplete_gamma_inv_initial_val(const T a, const T p) noexcept { - return (a > T(1) ? // if - incomplete_gamma_inv_initial_val_1( - a, incomplete_gamma_inv_t_val_1(p), p > T(0.5) ? T(-1) : T(1)) - : - // else - incomplete_gamma_inv_initial_val_2( - a, p, incomplete_gamma_inv_t_val_2(a))); + if (a > T(1)) { + return incomplete_gamma_inv_initial_val_1( + a, incomplete_gamma_inv_t_val_1(p), p > T(0.5) ? T(-1) : T(1)); + } else { + return incomplete_gamma_inv_initial_val_2(a, p, + incomplete_gamma_inv_t_val_2(a)); + } } // @@ -405,18 +407,15 @@ template constexpr T incomplete_gamma_inv_decision(const T value, const T a, const T p, const T direc, const T lg_val, const int iter_count) noexcept { -// return( abs(direc) > GCEM_INCML_GAMMA_INV_TOL ? -// incomplete_gamma_inv_recur(value - direc, a, p, -// incomplete_gamma_inv_deriv_1(value,a,lg_val), lg_val) : value - direc ); -#define INCML_GAMMA_INV_MAX_ITER 35 - return (iter_count <= INCML_GAMMA_INV_MAX_ITER ? // if - incomplete_gamma_inv_recur( - value - direc, a, p, - incomplete_gamma_inv_deriv_1(value, a, lg_val), lg_val, - iter_count + 1) - : - // else - value - direc); + constexpr int INCML_GAMMA_INV_MAX_ITER = 35; + + if (iter_count <= INCML_GAMMA_INV_MAX_ITER) { + return incomplete_gamma_inv_recur( + value - direc, a, p, incomplete_gamma_inv_deriv_1(value, a, lg_val), + lg_val, iter_count + 1); + } else { + return value - direc; + } } template @@ -429,19 +428,20 @@ constexpr T incomplete_gamma_inv_begin(const T initial_val, const T a, template constexpr T incomplete_gamma_inv_check(const T a, const T p) noexcept { - return ( // NaN check - (is_nan(a) || is_nan(p)) ? LIM::quiet_NaN() : - // - LIM::min() > p ? T(0) - : p > T(1) ? LIM::quiet_NaN() - : LIM::min() > abs(T(1) - p) ? LIM::infinity() - : - // - LIM::min() > a ? T(0) - : - // else - incomplete_gamma_inv_begin(incomplete_gamma_inv_initial_val(a, p), a, - p, lgamma(a))); + if (is_nan(a) || is_nan(p)) { + return LIM::quiet_NaN(); + } else if (LIM::min() > p) { + return T(0); + } else if (p > T(1)) { + return LIM::quiet_NaN(); + } else if (LIM::min() > fabs(T(1) - p)) { + return LIM::infinity(); + } else if (LIM::min() > a) { + return T(0); + } else { + return incomplete_gamma_inv_begin(incomplete_gamma_inv_initial_val(a, p), a, + p, lgamma(a)); + } } template > diff --git a/gtsam/nonlinear/tests/testGncHelpers.cpp b/gtsam/nonlinear/tests/testGncHelpers.cpp new file mode 100644 index 0000000000..6e47f97ccb --- /dev/null +++ b/gtsam/nonlinear/tests/testGncHelpers.cpp @@ -0,0 +1,37 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/* + * @file testGncHelpers.cpp + * @date July 10, 2023 + * @author Varun Agrawal + * @brief Tests for Chi-squared distribution. + */ + +#include +#include +#include + +using namespace gtsam; + +/* ************************************************************************* */ +TEST(GncHelpers, ChiSqInv) { + double barcSq = chi_squared_quantile(2, 0.99); + EXPECT_DOUBLES_EQUAL(9.21034, barcSq, 1e-5); +} + +/* ************************************************************************* */ +int main() { + srand(time(nullptr)); + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ From 00f5596e704dcf676cb0f317b81c77ed65ca1dd8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Jul 2023 13:40:14 -0400 Subject: [PATCH 10/33] Revert "switch from IndexVector to FastVector now that pybind/stl.h is enabled" This reverts commit 64c28504adf2a6abb272141a616728be0e4e76ce. --- gtsam/nonlinear/GncParams.h | 11 +++++++---- tests/testGncOptimizer.cpp | 22 +++++++++++----------- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index b792fa0581..10ac80663a 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -73,10 +73,13 @@ class GncParams { double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level + //TODO(Varun) replace IndexVector with vector once pybind11/stl.h is globally enabled. + /// Use IndexVector for inliers and outliers since it is fast + wrapping + using IndexVector = FastVector; ///< Slots in the factor graph corresponding to measurements that we know are inliers - FastVector knownInliers; + IndexVector knownInliers = IndexVector(); ///< Slots in the factor graph corresponding to measurements that we know are outliers - FastVector knownOutliers; + IndexVector knownOutliers = IndexVector(); /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { @@ -117,7 +120,7 @@ class GncParams { * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and * only apply GNC to prune outliers from the loop closures. * */ - void setKnownInliers(const FastVector& knownIn) { + void setKnownInliers(const IndexVector& knownIn) { for (size_t i = 0; i < knownIn.size(); i++){ knownInliers.push_back(knownIn[i]); } @@ -128,7 +131,7 @@ class GncParams { * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. * */ - void setKnownOutliers(const FastVector& knownOut) { + void setKnownOutliers(const IndexVector& knownOut) { for (size_t i = 0; i < knownOut.size(); i++){ knownOutliers.push_back(knownOut[i]); } diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 28261683bc..5424a5744f 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { Values initial; initial.insert(X(1), p0); - FastVector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) { Values initial; initial.insert(X(1), p0); - FastVector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) { Values initial; initial.insert(X(1), p0); - FastVector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -761,7 +761,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // GNC // Note: in difficult instances, we set the odometry measurements to be // inliers, but this problem is simple enought to succeed even without that - // assumption; + // assumption GncParams::IndexVector knownInliers; GncParams gncParams; auto gnc = GncOptimizer>(*graph, *initial, gncParams); @@ -782,12 +782,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers (check early stopping // when all measurements are known to be inliers or outliers) { - FastVector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); - FastVector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -811,11 +811,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers { - FastVector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - FastVector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -839,7 +839,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // only known outliers { - FastVector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -914,11 +914,11 @@ TEST(GncOptimizer, setWeights) { // initialize weights and also set known inliers/outliers { GncParams gncParams; - FastVector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - FastVector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); gncParams.setKnownInliers(knownInliers); gncParams.setKnownOutliers(knownOutliers); From 7c935d9e434ce0cd504296611d40fff30d4253ff Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Jul 2023 13:43:44 -0400 Subject: [PATCH 11/33] small update to GNC IndexVector --- gtsam/nonlinear/GncParams.h | 7 +++---- tests/testGncOptimizer.cpp | 9 +++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 10ac80663a..b1237b7901 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -73,13 +73,12 @@ class GncParams { double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level - //TODO(Varun) replace IndexVector with vector once pybind11/stl.h is globally enabled. - /// Use IndexVector for inliers and outliers since it is fast + wrapping + /// Use IndexVector for inliers and outliers since it is fast using IndexVector = FastVector; ///< Slots in the factor graph corresponding to measurements that we know are inliers - IndexVector knownInliers = IndexVector(); + IndexVector knownInliers; ///< Slots in the factor graph corresponding to measurements that we know are outliers - IndexVector knownOutliers = IndexVector(); + IndexVector knownOutliers; /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 5424a5744f..4e0ebf516c 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -750,7 +750,8 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // add a few outliers SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( Vector3(0.1, 0.1, 0.01)); - graph->push_back(BetweenFactor(90, 50, Pose2(), betweenNoise)); // some arbitrary and incorrect between factor + // some arbitrary and incorrect between factor + graph->push_back(BetweenFactor(90, 50, Pose2(), betweenNoise)); /// get expected values by optimizing outlier-free graph Values expectedWithOutliers = LevenbergMarquardtOptimizer(*graph, *initial) @@ -759,9 +760,9 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // CHECK(assert_equal(expected, expectedWithOutliers, 1e-3)); // GNC - // Note: in difficult instances, we set the odometry measurements to be - // inliers, but this problem is simple enought to succeed even without that - // assumption GncParams::IndexVector knownInliers; + // NOTE: in difficult instances, we set the odometry measurements to be + // inliers, but this problem is simple enough to succeed even without that + // assumption. GncParams gncParams; auto gnc = GncOptimizer>(*graph, *initial, gncParams); From dcb42998e8b7d5f1e490ea85426de8f442a05300 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Oct 2023 05:47:43 -0400 Subject: [PATCH 12/33] rename GncHelpers to chiSquaredInverse and move to internal directory --- gtsam/nonlinear/{GncHelpers.h => internal/chiSquaredInverse.hpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename gtsam/nonlinear/{GncHelpers.h => internal/chiSquaredInverse.hpp} (100%) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/internal/chiSquaredInverse.hpp similarity index 100% rename from gtsam/nonlinear/GncHelpers.h rename to gtsam/nonlinear/internal/chiSquaredInverse.hpp From e8817ae3ea1c748abfe9a45aa4e0f67a85adcc94 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Oct 2023 08:25:56 -0400 Subject: [PATCH 13/33] rename other files accordingly --- .../internal/{chiSquaredInverse.hpp => chiSquaredInverse.h} | 0 .../tests/{testGncHelpers.cpp => testChiSquaredInverse.cpp} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename gtsam/nonlinear/internal/{chiSquaredInverse.hpp => chiSquaredInverse.h} (100%) rename gtsam/nonlinear/tests/{testGncHelpers.cpp => testChiSquaredInverse.cpp} (100%) diff --git a/gtsam/nonlinear/internal/chiSquaredInverse.hpp b/gtsam/nonlinear/internal/chiSquaredInverse.h similarity index 100% rename from gtsam/nonlinear/internal/chiSquaredInverse.hpp rename to gtsam/nonlinear/internal/chiSquaredInverse.h diff --git a/gtsam/nonlinear/tests/testGncHelpers.cpp b/gtsam/nonlinear/tests/testChiSquaredInverse.cpp similarity index 100% rename from gtsam/nonlinear/tests/testGncHelpers.cpp rename to gtsam/nonlinear/tests/testChiSquaredInverse.cpp From a46a78d413206cc6f712524dcc13bcdeb263cbe8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Oct 2023 08:27:53 -0400 Subject: [PATCH 14/33] update paths --- gtsam/nonlinear/GncOptimizer.h | 2 +- gtsam/nonlinear/internal/chiSquaredInverse.h | 2 +- gtsam/nonlinear/tests/testChiSquaredInverse.cpp | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 21d4e826be..45c674401d 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -26,9 +26,9 @@ #pragma once -#include #include #include +#include namespace gtsam { /* diff --git a/gtsam/nonlinear/internal/chiSquaredInverse.h b/gtsam/nonlinear/internal/chiSquaredInverse.h index 7a27a35305..3729bf754e 100644 --- a/gtsam/nonlinear/internal/chiSquaredInverse.h +++ b/gtsam/nonlinear/internal/chiSquaredInverse.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GncHelpers.h + * @file chiSquaredInverse.h * @brief Helper functions for use with the GncOptimizer * @author Varun Agrawal */ diff --git a/gtsam/nonlinear/tests/testChiSquaredInverse.cpp b/gtsam/nonlinear/tests/testChiSquaredInverse.cpp index 6e47f97ccb..65c3b417d9 100644 --- a/gtsam/nonlinear/tests/testChiSquaredInverse.cpp +++ b/gtsam/nonlinear/tests/testChiSquaredInverse.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file testGncHelpers.cpp + * @file testChiSquaredInverse.cpp * @date July 10, 2023 * @author Varun Agrawal * @brief Tests for Chi-squared distribution. @@ -18,12 +18,12 @@ #include #include -#include +#include using namespace gtsam; /* ************************************************************************* */ -TEST(GncHelpers, ChiSqInv) { +TEST(ChiSquaredInverse, ChiSqInv) { double barcSq = chi_squared_quantile(2, 0.99); EXPECT_DOUBLES_EQUAL(9.21034, barcSq, 1e-5); } From c8a0cdc543587799292302d804afde01ddd9a4d2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Oct 2023 07:00:34 -0400 Subject: [PATCH 15/33] much improved chi_squared_quantile, incremental update --- gtsam/nonlinear/GncOptimizer.h | 2 +- gtsam/nonlinear/internal/chiSquaredInverse.h | 521 +++--------------- .../nonlinear/tests/testChiSquaredInverse.cpp | 2 +- 3 files changed, 89 insertions(+), 436 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 45c674401d..edcc6f0bb6 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -36,7 +36,7 @@ namespace gtsam { * Equivalent to chi2inv in Matlab. */ static double Chi2inv(const double alpha, const size_t dofs) { - return chi_squared_quantile(dofs, alpha); + return internal::chi_squared_quantile(dofs, alpha); } /* ************************************************************************* */ diff --git a/gtsam/nonlinear/internal/chiSquaredInverse.h b/gtsam/nonlinear/internal/chiSquaredInverse.h index 3729bf754e..5286942847 100644 --- a/gtsam/nonlinear/internal/chiSquaredInverse.h +++ b/gtsam/nonlinear/internal/chiSquaredInverse.h @@ -11,480 +11,133 @@ /** * @file chiSquaredInverse.h - * @brief Helper functions for use with the GncOptimizer + * @brief This file contains an implementation of the Chi Squared inverse + * function, which is implemented similar to Boost with additional template + * parameter helpers. + * + * A lot of this code has been picked up from + * https://www.boost.org/doc/libs/1_83_0/boost/math/special_functions/detail/igamma_inverse.hpp + * https://www.boost.org/doc/libs/1_83_0/boost/math/tools/roots.hpp + * * @author Varun Agrawal */ #pragma once -#include - -namespace gtsam { - -/// Template type for numeric limits -template -using LIM = std::numeric_limits; - -template -using return_t = - typename std::conditional::value, double, T>::type; - -/// Get common type amongst all arguments -template -using common_t = typename std::common_type::type; - -/// Helper template for finding common return type -template -using common_return_t = return_t>; +#include +#include +#include -/// Check if integer is odd -constexpr bool is_odd(const long long int x) noexcept { return (x & 1U) != 0; } - -/// Templated check for NaN -template -constexpr bool is_nan(const T x) noexcept { - return x != x; -} +#include -/// @brief Gauss-Legendre quadrature: 50 points -static const long double gauss_legendre_50_points[50] = { - -0.03109833832718887611232898966595L, 0.03109833832718887611232898966595L, - -0.09317470156008614085445037763960L, 0.09317470156008614085445037763960L, - -0.15489058999814590207162862094111L, 0.15489058999814590207162862094111L, - -0.21600723687604175684728453261710L, 0.21600723687604175684728453261710L, - -0.27628819377953199032764527852113L, 0.27628819377953199032764527852113L, - -0.33550024541943735683698825729107L, 0.33550024541943735683698825729107L, - -0.39341431189756512739422925382382L, 0.39341431189756512739422925382382L, - -0.44980633497403878914713146777838L, 0.44980633497403878914713146777838L, - -0.50445814490746420165145913184914L, 0.50445814490746420165145913184914L, - -0.55715830451465005431552290962580L, 0.55715830451465005431552290962580L, - -0.60770292718495023918038179639183L, 0.60770292718495023918038179639183L, - -0.65589646568543936078162486400368L, 0.65589646568543936078162486400368L, - -0.70155246870682225108954625788366L, 0.70155246870682225108954625788366L, - -0.74449430222606853826053625268219L, 0.74449430222606853826053625268219L, - -0.78455583290039926390530519634099L, 0.78455583290039926390530519634099L, - -0.82158207085933594835625411087394L, 0.82158207085933594835625411087394L, - -0.85542976942994608461136264393476L, 0.85542976942994608461136264393476L, - -0.88596797952361304863754098246675L, 0.88596797952361304863754098246675L, - -0.91307855665579189308973564277166L, 0.91307855665579189308973564277166L, - -0.93665661894487793378087494727250L, 0.93665661894487793378087494727250L, - -0.95661095524280794299774564415662L, 0.95661095524280794299774564415662L, - -0.97286438510669207371334410460625L, 0.97286438510669207371334410460625L, - -0.98535408404800588230900962563249L, 0.98535408404800588230900962563249L, - -0.99403196943209071258510820042069L, 0.99403196943209071258510820042069L, - -0.99886640442007105018545944497422L, 0.99886640442007105018545944497422L}; +// TODO(Varun) remove +#include -/// @brief Gauss-Legendre quadrature: 50 weights -static const long double gauss_legendre_50_weights[50] = { - 0.06217661665534726232103310736061L, 0.06217661665534726232103310736061L, - 0.06193606742068324338408750978083L, 0.06193606742068324338408750978083L, - 0.06145589959031666375640678608392L, 0.06145589959031666375640678608392L, - 0.06073797084177021603175001538481L, 0.06073797084177021603175001538481L, - 0.05978505870426545750957640531259L, 0.05978505870426545750957640531259L, - 0.05860084981322244583512243663085L, 0.05860084981322244583512243663085L, - 0.05718992564772838372302931506599L, 0.05718992564772838372302931506599L, - 0.05555774480621251762356742561227L, 0.05555774480621251762356742561227L, - 0.05371062188899624652345879725566L, 0.05371062188899624652345879725566L, - 0.05165570306958113848990529584010L, 0.05165570306958113848990529584010L, - 0.04940093844946631492124358075143L, 0.04940093844946631492124358075143L, - 0.04695505130394843296563301363499L, 0.04695505130394843296563301363499L, - 0.04432750433880327549202228683039L, 0.04432750433880327549202228683039L, - 0.04152846309014769742241197896407L, 0.04152846309014769742241197896407L, - 0.03856875661258767524477015023639L, 0.03856875661258767524477015023639L, - 0.03545983561514615416073461100098L, 0.03545983561514615416073461100098L, - 0.03221372822357801664816582732300L, 0.03221372822357801664816582732300L, - 0.02884299358053519802990637311323L, 0.02884299358053519802990637311323L, - 0.02536067357001239044019487838544L, 0.02536067357001239044019487838544L, - 0.02178024317012479298159206906269L, 0.02178024317012479298159206906269L, - 0.01811556071348939035125994342235L, 0.01811556071348939035125994342235L, - 0.01438082276148557441937890892732L, 0.01438082276148557441937890892732L, - 0.01059054838365096926356968149924L, 0.01059054838365096926356968149924L, - 0.00675979919574540150277887817799L, 0.00675979919574540150277887817799L, - 0.00290862255315514095840072434286L, 0.00290862255315514095840072434286L}; +namespace gtsam { namespace internal { -/// 50 point Gauss-Legendre quadrature -template -constexpr T incomplete_gamma_quad_inp_vals(const T lb, const T ub, - const int counter) noexcept { - return (ub - lb) * gauss_legendre_50_points[counter] / T(2) + - (ub + lb) / T(2); -} - -template -constexpr T incomplete_gamma_quad_weight_vals(const T lb, const T ub, - const int counter) noexcept { - return (ub - lb) * gauss_legendre_50_weights[counter] / T(2); -} - -template -constexpr T incomplete_gamma_quad_fn(const T x, const T a, - const T lg_term) noexcept { - return exp(-x + (a - T(1)) * log(x) - lg_term); -} - -template -constexpr T incomplete_gamma_quad_recur(const T lb, const T ub, const T a, - const T lg_term, - const int counter) noexcept { - T val = incomplete_gamma_quad_fn( - incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * - incomplete_gamma_quad_weight_vals(lb, ub, counter); - if (counter < 49) { - return val + incomplete_gamma_quad_recur(lb, ub, a, lg_term, counter + 1); - } else { - return val; - } -} - -template -constexpr T incomplete_gamma_quad_lb(const T a, const T z) noexcept { - // break integration into ranges - return (a > T(1000) ? std::max(T(0), std::min(z, a) - 11 * sqrt(a)) - : a > T(800) ? std::max(T(0), std::min(z, a) - 11 * sqrt(a)) - : a > T(500) ? std::max(T(0), std::min(z, a) - 10 * sqrt(a)) - : a > T(300) ? std::max(T(0), std::min(z, a) - 10 * sqrt(a)) - : a > T(100) ? std::max(T(0), std::min(z, a) - 9 * sqrt(a)) - : a > T(90) ? std::max(T(0), std::min(z, a) - 9 * sqrt(a)) - : a > T(70) ? std::max(T(0), std::min(z, a) - 8 * sqrt(a)) - : a > T(50) ? std::max(T(0), std::min(z, a) - 7 * sqrt(a)) - : a > T(40) ? std::max(T(0), std::min(z, a) - 6 * sqrt(a)) - : a > T(30) ? std::max(T(0), std::min(z, a) - 5 * sqrt(a)) - : std::max(T(0), std::min(z, a) - 4 * sqrt(a))); -} - -template -constexpr T incomplete_gamma_quad_ub(const T a, const T z) noexcept { - return (a > T(1000) ? std::min(z, a + 10 * sqrt(a)) - : a > T(800) ? std::min(z, a + 10 * sqrt(a)) - : a > T(500) ? std::min(z, a + 9 * sqrt(a)) - : a > T(300) ? std::min(z, a + 9 * sqrt(a)) - : a > T(100) ? std::min(z, a + 8 * sqrt(a)) - : a > T(90) ? std::min(z, a + 8 * sqrt(a)) - : a > T(70) ? std::min(z, a + 7 * sqrt(a)) - : a > T(50) ? std::min(z, a + 6 * sqrt(a)) - : std::min(z, a + 5 * sqrt(a))); -} - -template -constexpr T incomplete_gamma_quad(const T a, const T z) noexcept { - return incomplete_gamma_quad_recur(incomplete_gamma_quad_lb(a, z), - incomplete_gamma_quad_ub(a, z), a, - lgamma(a), 0); -} - -// reverse cf expansion -// see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ - -template -constexpr T incomplete_gamma_cf_2_recur(const T a, const T z, - const int depth) noexcept { - T val = 1 + (depth - 1) * 2 - a + z; - if (depth < 100) { - return val + - depth * (a - depth) / incomplete_gamma_cf_2_recur(a, z, depth + 1); - } else { - return val; - } -} - template -constexpr T incomplete_gamma_cf_2( - const T a, - const T z) noexcept { // lower (regularized) incomplete gamma function - return (T(1.0) - exp(a * log(z) - z - lgamma(a)) / - incomplete_gamma_cf_2_recur(a, z, 1)); -} - -// cf expansion -// see: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ - -template -constexpr T incomplete_gamma_cf_1_coef(const T a, const T z, - const int depth) noexcept { - if (is_odd(depth)) { - return -(a - 1 + T(depth + 1) / T(2)) * z; - } else { - return T(depth) / T(2) * z; - } -} - -template -constexpr T incomplete_gamma_cf_1_recur(const T a, const T z, - const int depth) noexcept { - T val = a + depth - 1; - if (depth < 55) { - return val + incomplete_gamma_cf_1_coef(a, z, depth) / - incomplete_gamma_cf_1_recur(a, z, depth + 1); - } else { - return val; - } -} - -/// lower (regularized) incomplete gamma function -template -constexpr T incomplete_gamma_cf_1(const T a, const T z) noexcept { - return exp(a * log(z) - z - lgamma(a)) / incomplete_gamma_cf_1_recur(a, z, 1); -} - -/// Perform NaN check -template -constexpr T incomplete_gamma_check(const T a, const T z) noexcept { - if (is_nan(a) || is_nan(z)) { - return LIM::quiet_NaN(); - } else if (a < T(0)) { +T gamma_p_inv_imp(const T a, const T p) { + if (is_nan(a) || is_nan(p)) { return LIM::quiet_NaN(); - } else if (LIM::min() > z) { - return T(0); - } else if (LIM::min() > a) { - return T(1); - } else if (a < T(10) && z - a < T(10)) { // cf or quadrature - return incomplete_gamma_cf_1(a, z); - } else if (a < T(10) || z / a > T(3)) { - return incomplete_gamma_cf_2(a, z); - } else { - return incomplete_gamma_quad(a, z); + if (a <= T(0)) { + throw std::runtime_error( + "Argument a in the incomplete gamma function inverse must be >= 0."); + } + } else if (p < T(0) || p > T(1)) { + throw std::runtime_error( + "Probability must be in the range [0,1] in the incomplete gamma " + "function inverse."); + } else if (p == T(0)) { + return 0; } -} -template > -constexpr TC incomplete_gamma_type_check(const T1 a, const T2 p) noexcept { - return incomplete_gamma_check(static_cast(a), static_cast(p)); -} - -} // namespace internal - -/** - * Compile-time regularized lower incomplete gamma function - * - * @param a a real-valued, non-negative input. - * @param x a real-valued, non-negative input. - * - * @return the regularized lower incomplete gamma function evaluated at (\c a, - * \c x), \f[ \frac{\gamma(a,x)}{\Gamma(a)} = \frac{1}{\Gamma(a)} \int_0^x - * t^{a-1} \exp(-t) dt \f] When \c a is not too large, the value is computed - * using the continued fraction representation of the upper incomplete gamma - * function, \f$ \Gamma(a,x) \f$, using \f[ \Gamma(a,x) = \Gamma(a) - - * \dfrac{x^a\exp(-x)}{a - \dfrac{ax}{a + 1 + \dfrac{x}{a + 2 - \dfrac{(a+1)x}{a - * + 3 + \dfrac{2x}{a + 4 - \ddots}}}}} \f] where \f$ \gamma(a,x) \f$ and \f$ - * \Gamma(a,x) \f$ are connected via \f[ \frac{\gamma(a,x)}{\Gamma(a)} + - * \frac{\Gamma(a,x)}{\Gamma(a)} = 1 \f] When \f$ a > 10 \f$, a 50-point - * Gauss-Legendre quadrature scheme is employed. - */ - -template -constexpr common_return_t incomplete_gamma(const T1 a, - const T2 x) noexcept { - return internal::incomplete_gamma_type_check(a, x); -} - -namespace internal { - -template -constexpr T incomplete_gamma_inv_decision(const T value, const T a, const T p, - const T direc, const T lg_val, - const int iter_count) noexcept; - -// -// initial value for Halley's method -template -constexpr T incomplete_gamma_inv_t_val_1(const T p) noexcept { // a > 1.0 - return (p > T(0.5) ? sqrt(-2 * log(T(1) - p)) : sqrt(-2 * log(p))); -} - -template -constexpr T incomplete_gamma_inv_t_val_2(const T a) noexcept { // a <= 1.0 - return (T(1) - T(0.253) * a - T(0.12) * a * a); -} - -// -template -constexpr T incomplete_gamma_inv_initial_val_1_int_begin( - const T t_val) noexcept { // internal for a > 1.0 - return (t_val - - (T(2.515517L) + T(0.802853L) * t_val + T(0.010328L) * t_val * t_val) / - (T(1) + T(1.432788L) * t_val + T(0.189269L) * t_val * t_val + - T(0.001308L) * t_val * t_val * t_val)); -} - -template -constexpr T incomplete_gamma_inv_initial_val_1_int_end( - const T value_inp, const T a) noexcept { // internal for a > 1.0 - return std::max( - T(1E-04), a * pow(T(1) - T(1) / (9 * a) - value_inp / (3 * sqrt(a)), 3)); -} - -template -constexpr T incomplete_gamma_inv_initial_val_1( - const T a, const T t_val, const T sgn_term) noexcept { // a > 1.0 - return incomplete_gamma_inv_initial_val_1_int_end( - sgn_term * incomplete_gamma_inv_initial_val_1_int_begin(t_val), a); -} - -template -constexpr T incomplete_gamma_inv_initial_val_2( - const T a, const T p, const T t_val) noexcept { // a <= 1.0 - if (p < t_val) { - return pow(p / t_val, T(1) / a); - } else { - return T(1) - log(T(1) - (p - t_val) / (T(1) - t_val)); + // TODO + // Get an initial guess (https://dl.acm.org/doi/abs/10.1145/22721.23109) + // T guess = find_inverse_gamma(a, p, 1 - p); + bool has_10_digits = false; + boost::math::policies::policy<> pol; + T guess = boost::math::detail::find_inverse_gamma(a, p, 1 - p, pol, + &has_10_digits); + + T lower = LIM::min(); + if (guess <= lower) { + guess = LIM::min(); } -} -// Initial value - -template -constexpr T incomplete_gamma_inv_initial_val(const T a, const T p) noexcept { - if (a > T(1)) { - return incomplete_gamma_inv_initial_val_1( - a, incomplete_gamma_inv_t_val_1(p), p > T(0.5) ? T(-1) : T(1)); + // TODO + // Number of Halley iterations + // The default used in Boost is 200 + // uint_fast16_t max_iter = 200; + + // The number of digits to converge to. + // This is an arbitrary number, + // but Boost does more sophisticated things + // using the first derivative. + // unsigned digits = 40; + + // // Perform Halley iteration for root-finding to get a more refined answer + // guess = halley_iterate(gamma_p_inverse_func(a, p, false), guess, lower, + // LIM::max(), digits, max_iter); + unsigned digits = + boost::math::policies::digits>(); + if (digits < 30) { + digits *= 2; + digits /= 3; } else { - return incomplete_gamma_inv_initial_val_2(a, p, - incomplete_gamma_inv_t_val_2(a)); + digits /= 2; + digits -= 1; } -} - -// -// Halley recursion - -template -constexpr T incomplete_gamma_inv_err_val( - const T value, const T a, const T p) noexcept { // err_val = f(x) - return (incomplete_gamma(a, value) - p); -} - -template -constexpr T incomplete_gamma_inv_deriv_1( - const T value, const T a, - const T lg_val) noexcept { // derivative of the incomplete gamma function - // w.r.t. x - return (exp(-value + (a - T(1)) * log(value) - lg_val)); -} - -template -constexpr T incomplete_gamma_inv_deriv_2( - const T value, const T a, - const T deriv_1) noexcept { // second derivative of the incomplete gamma - // function w.r.t. x - return (deriv_1 * ((a - T(1)) / value - T(1))); -} - -template -constexpr T incomplete_gamma_inv_ratio_val_1(const T value, const T a, - const T p, - const T deriv_1) noexcept { - return (incomplete_gamma_inv_err_val(value, a, p) / deriv_1); -} - -template -constexpr T incomplete_gamma_inv_ratio_val_2(const T value, const T a, - const T deriv_1) noexcept { - return (incomplete_gamma_inv_deriv_2(value, a, deriv_1) / deriv_1); -} - -template -constexpr T incomplete_gamma_inv_halley(const T ratio_val_1, - const T ratio_val_2) noexcept { - return (ratio_val_1 / - std::max(T(0.8), std::min(T(1.2), T(1) - T(0.5) * ratio_val_1 * - ratio_val_2))); -} - -template -constexpr T incomplete_gamma_inv_recur(const T value, const T a, const T p, - const T deriv_1, const T lg_val, - const int iter_count) noexcept { - return incomplete_gamma_inv_decision( - value, a, p, - incomplete_gamma_inv_halley( - incomplete_gamma_inv_ratio_val_1(value, a, p, deriv_1), - incomplete_gamma_inv_ratio_val_2(value, a, deriv_1)), - lg_val, iter_count); -} - -template -constexpr T incomplete_gamma_inv_decision(const T value, const T a, const T p, - const T direc, const T lg_val, - const int iter_count) noexcept { - constexpr int INCML_GAMMA_INV_MAX_ITER = 35; - - if (iter_count <= INCML_GAMMA_INV_MAX_ITER) { - return incomplete_gamma_inv_recur( - value - direc, a, p, incomplete_gamma_inv_deriv_1(value, a, lg_val), - lg_val, iter_count + 1); - } else { - return value - direc; + if ((a < T(0.125)) && (fabs(boost::math::gamma_p_derivative(a, guess, pol)) > + 1 / sqrt(boost::math::tools::epsilon()))) + digits = + boost::math::policies::digits>() - 2; + // + // Go ahead and iterate: + // + std::uintmax_t max_iter = boost::math::policies::get_max_root_iterations< + boost::math::policies::policy<>>(); + guess = boost::math::tools::halley_iterate( + boost::math::detail::gamma_p_inverse_func< + T, boost::math::policies::policy<>>(a, p, false), + guess, lower, boost::math::tools::max_value(), digits, max_iter); + + if (guess == lower) { + throw std::runtime_error( + "Expected result known to be non-zero, but is smaller than the " + "smallest available number."); } -} - -template -constexpr T incomplete_gamma_inv_begin(const T initial_val, const T a, - const T p, const T lg_val) noexcept { - return incomplete_gamma_inv_recur( - initial_val, a, p, incomplete_gamma_inv_deriv_1(initial_val, a, lg_val), - lg_val, 1); -} - -template -constexpr T incomplete_gamma_inv_check(const T a, const T p) noexcept { - if (is_nan(a) || is_nan(p)) { - return LIM::quiet_NaN(); - } else if (LIM::min() > p) { - return T(0); - } else if (p > T(1)) { - return LIM::quiet_NaN(); - } else if (LIM::min() > fabs(T(1) - p)) { - return LIM::infinity(); - } else if (LIM::min() > a) { - return T(0); - } else { - return incomplete_gamma_inv_begin(incomplete_gamma_inv_initial_val(a, p), a, - p, lgamma(a)); - } -} -template > -constexpr TC incomplete_gamma_inv_type_check(const T1 a, const T2 p) noexcept { - return incomplete_gamma_inv_check(static_cast(a), static_cast(p)); + return guess; } -} // namespace internal - /** - * Compile-time inverse incomplete gamma function + * Compile-time check for inverse incomplete gamma function * * @param a a real-valued, non-negative input. * @param p a real-valued input with values in the unit-interval. - * - * @return Computes the inverse incomplete gamma function, a value \f$ x \f$ - * such that \f[ f(x) := \frac{\gamma(a,x)}{\Gamma(a)} - p \f] equal to zero, - * for a given \c p. GCE-Math finds this root using Halley's method: \f[ x_{n+1} - * = x_n - \frac{f(x_n)/f'(x_n)}{1 - 0.5 \frac{f(x_n)}{f'(x_n)} - * \frac{f''(x_n)}{f'(x_n)} } \f] where \f[ \frac{\partial}{\partial x} - * \left(\frac{\gamma(a,x)}{\Gamma(a)}\right) = \frac{1}{\Gamma(a)} x^{a-1} - * \exp(-x) \f] \f[ \frac{\partial^2}{\partial x^2} - * \left(\frac{\gamma(a,x)}{\Gamma(a)}\right) = \frac{1}{\Gamma(a)} x^{a-1} - * \exp(-x) \left( \frac{a-1}{x} - 1 \right) \f] */ - template constexpr common_return_t incomplete_gamma_inv(const T1 a, const T2 p) noexcept { - return internal::incomplete_gamma_inv_type_check(a, p); + return internal::gamma_p_inv_imp(static_cast>(a), + static_cast>(p)); } /** - * @brief Compute the quantile function of the Chi squared distribution. + * @brief Compute the quantile function of the Chi-Squared distribution. * * @param dofs Degrees of freedom * @param alpha Quantile value - * @return constexpr double + * @return double */ -constexpr double chi_squared_quantile(const size_t dofs, const double alpha) { +double chi_squared_quantile(const double dofs, const double alpha) { // The quantile function of the Chi-squared distribution is the quantile of // the specific (inverse) incomplete Gamma distribution - return 2 * incomplete_gamma_inv(dofs * 0.5, alpha); + return 2 * incomplete_gamma_inv(dofs / 2, alpha); } +} // namespace internal + } // namespace gtsam diff --git a/gtsam/nonlinear/tests/testChiSquaredInverse.cpp b/gtsam/nonlinear/tests/testChiSquaredInverse.cpp index 65c3b417d9..8da4df5c2e 100644 --- a/gtsam/nonlinear/tests/testChiSquaredInverse.cpp +++ b/gtsam/nonlinear/tests/testChiSquaredInverse.cpp @@ -24,7 +24,7 @@ using namespace gtsam; /* ************************************************************************* */ TEST(ChiSquaredInverse, ChiSqInv) { - double barcSq = chi_squared_quantile(2, 0.99); + double barcSq = internal::chi_squared_quantile(2, 0.99); EXPECT_DOUBLES_EQUAL(9.21034, barcSq, 1e-5); } From bebb275489ae6030353148eef881fba9c24ca9b7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Oct 2023 10:21:49 -0400 Subject: [PATCH 16/33] compute initial guess for inverse gamma value --- gtsam/nonlinear/internal/chiSquaredInverse.h | 304 +++++++++++++++++-- 1 file changed, 275 insertions(+), 29 deletions(-) diff --git a/gtsam/nonlinear/internal/chiSquaredInverse.h b/gtsam/nonlinear/internal/chiSquaredInverse.h index 5286942847..dc85958460 100644 --- a/gtsam/nonlinear/internal/chiSquaredInverse.h +++ b/gtsam/nonlinear/internal/chiSquaredInverse.h @@ -24,9 +24,9 @@ #pragma once -#include -#include -#include +#include +#include +#include #include @@ -37,6 +37,270 @@ namespace gtsam { namespace internal { +/** + * @brief Polynomial evaluation with runtime size. + * + * @tparam T + * @tparam U + */ +template +inline U evaluate_polynomial(const T* poly, U const& z, std::size_t count) { + assert(count > 0); + U sum = static_cast(poly[count - 1]); + for (int i = static_cast(count) - 2; i >= 0; --i) { + sum *= z; + sum += static_cast(poly[i]); + } + return sum; +} + +/** + * @brief Computation of the Incomplete Gamma Function Ratios and their Inverse. + * + * Reference: + * ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR. + * ACM Transactions on Mathematical Software, Vol. 12, No. 4, + * December 1986, Pages 377-393. + * + * See equation 32. + * + * @tparam T + * @param p + * @param q + * @return T + */ +template +T find_inverse_s(T p, T q) { + T t; + if (p < T(0.5)) { + t = sqrt(-2 * log(p)); + } else { + t = sqrt(-2 * log(q)); + } + static const double a[4] = {3.31125922108741, 11.6616720288968, + 4.28342155967104, 0.213623493715853}; + static const double b[5] = {1, 6.61053765625462, 6.40691597760039, + 1.27364489782223, 0.3611708101884203e-1}; + T s = t - internal::evaluate_polynomial(a, t, 4) / + internal::evaluate_polynomial(b, t, 5); + if (p < T(0.5)) s = -s; + return s; +} + +/** + * @brief Computation of the Incomplete Gamma Function Ratios and their Inverse. + * + * Reference: + * ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR. + * ACM Transactions on Mathematical Software, Vol. 12, No. 4, + * December 1986, Pages 377-393. + * + * See equation 34. + * + * @tparam T + * @param a + * @param x + * @param N + * @param tolerance + * @return T + */ +template +T didonato_SN(T a, T x, unsigned N, T tolerance = 0) { + T sum = 1; + if (N >= 1) { + T partial = x / (a + 1); + sum += partial; + for (unsigned i = 2; i <= N; ++i) { + partial *= x / (a + i); + sum += partial; + if (partial < tolerance) break; + } + } + return sum; +} + +/** + * @brief Compute the initial inverse gamma value guess. + * + * We use the implementation in this paper: + * Computation of the Incomplete Gamma Function Ratios and their Inverse + * ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR. + * ACM Transactions on Mathematical Software, Vol. 12, No. 4, + * December 1986, Pages 377-393. + * + * @tparam T + * @param a + * @param p + * @param q + * @param p_has_10_digits + * @return T + */ +template +T find_inverse_gamma(T a, T p, T q, bool* p_has_10_digits) { + T result; + *p_has_10_digits = false; + + // TODO(Varun) replace with egamma_v in C++20 + // Euler-Mascheroni constant + double euler = 0.577215664901532860606512090082402431042159335939923598805; + + if (a == 1) { + result = -log(q); + } else if (a < 1) { + T g = std::tgamma(a); + T b = q * g; + + if ((b > T(0.6)) || ((b >= T(0.45)) && (a >= T(0.3)))) { + // DiDonato & Morris Eq 21: + // + // There is a slight variation from DiDonato and Morris here: + // the first form given here is unstable when p is close to 1, + // making it impossible to compute the inverse of Q(a,x) for small + // q. Fortunately the second form works perfectly well in this case. + T u; + if ((b * q > T(1e-8)) && (q > T(1e-5))) { + u = pow(p * g * a, 1 / a); + } else { + u = exp((-q / a) - euler); + } + result = u / (1 - (u / (a + 1))); + + } else if ((a < 0.3) && (b >= 0.35)) { + // DiDonato & Morris Eq 22: + T t = exp(-euler - b); + T u = t * exp(t); + result = t * exp(u); + + } else if ((b > 0.15) || (a >= 0.3)) { + // DiDonato & Morris Eq 23: + T y = -log(b); + T u = y - (1 - a) * log(y); + result = y - (1 - a) * log(u) - log(1 + (1 - a) / (1 + u)); + + } else if (b > 0.1) { + // DiDonato & Morris Eq 24: + T y = -log(b); + T u = y - (1 - a) * log(y); + result = y - (1 - a) * log(u) - + log((u * u + 2 * (3 - a) * u + (2 - a) * (3 - a)) / + (u * u + (5 - a) * u + 2)); + + } else { + // DiDonato & Morris Eq 25: + T y = -log(b); + T c1 = (a - 1) * log(y); + T c1_2 = c1 * c1; + T c1_3 = c1_2 * c1; + T c1_4 = c1_2 * c1_2; + T a_2 = a * a; + T a_3 = a_2 * a; + + T c2 = (a - 1) * (1 + c1); + T c3 = (a - 1) * (-(c1_2 / 2) + (a - 2) * c1 + (3 * a - 5) / 2); + T c4 = (a - 1) * ((c1_3 / 3) - (3 * a - 5) * c1_2 / 2 + + (a_2 - 6 * a + 7) * c1 + (11 * a_2 - 46 * a + 47) / 6); + T c5 = (a - 1) * (-(c1_4 / 4) + (11 * a - 17) * c1_3 / 6 + + (-3 * a_2 + 13 * a - 13) * c1_2 + + (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2 + + (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12); + + T y_2 = y * y; + T y_3 = y_2 * y; + T y_4 = y_2 * y_2; + result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4); + + if (b < 1e-28f) *p_has_10_digits = true; + } + } else { + // DiDonato and Morris Eq 31: + T s = find_inverse_s(p, q); + + T s_2 = s * s; + T s_3 = s_2 * s; + T s_4 = s_2 * s_2; + T s_5 = s_4 * s; + T ra = sqrt(a); + + T w = a + s * ra + (s * s - 1) / 3; + w += (s_3 - 7 * s) / (36 * ra); + w -= (3 * s_4 + 7 * s_2 - 16) / (810 * a); + w += (9 * s_5 + 256 * s_3 - 433 * s) / (38880 * a * ra); + + if ((a >= 500) && (fabs(1 - w / a) < 1e-6)) { + result = w; + *p_has_10_digits = true; + + } else if (p > 0.5) { + if (w < 3 * a) { + result = w; + + } else { + T D = (std::max)(T(2), T(a * (a - 1))); + T lg = std::lgamma(a); + T lb = log(q) + lg; + if (lb < -D * T(2.3)) { + // DiDonato and Morris Eq 25: + T y = -lb; + T c1 = (a - 1) * log(y); + T c1_2 = c1 * c1; + T c1_3 = c1_2 * c1; + T c1_4 = c1_2 * c1_2; + T a_2 = a * a; + T a_3 = a_2 * a; + + T c2 = (a - 1) * (1 + c1); + T c3 = (a - 1) * (-(c1_2 / 2) + (a - 2) * c1 + (3 * a - 5) / 2); + T c4 = + (a - 1) * ((c1_3 / 3) - (3 * a - 5) * c1_2 / 2 + + (a_2 - 6 * a + 7) * c1 + (11 * a_2 - 46 * a + 47) / 6); + T c5 = (a - 1) * (-(c1_4 / 4) + (11 * a - 17) * c1_3 / 6 + + (-3 * a_2 + 13 * a - 13) * c1_2 + + (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2 + + (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12); + + T y_2 = y * y; + T y_3 = y_2 * y; + T y_4 = y_2 * y_2; + result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4); + + } else { + // DiDonato and Morris Eq 33: + T u = -lb + (a - 1) * log(w) - log(1 + (1 - a) / (1 + w)); + result = -lb + (a - 1) * log(u) - log(1 + (1 - a) / (1 + u)); + } + } + } else { + T z = w; + T ap1 = a + 1; + T ap2 = a + 2; + if (w < 0.15f * ap1) { + // DiDonato and Morris Eq 35: + T v = log(p) + std::lgamma(ap1); + z = exp((v + w) / a); + s = std::log1p(z / ap1 * (1 + z / ap2)); + z = exp((v + z - s) / a); + s = std::log1p(z / ap1 * (1 + z / ap2)); + z = exp((v + z - s) / a); + s = std::log1p(z / ap1 * (1 + z / ap2 * (1 + z / (a + 3)))); + z = exp((v + z - s) / a); + } + + if ((z <= 0.01 * ap1) || (z > 0.7 * ap1)) { + result = z; + if (z <= T(0.002) * ap1) *p_has_10_digits = true; + + } else { + // DiDonato and Morris Eq 36: + T ls = log(didonato_SN(a, z, 100, T(1e-4))); + T v = log(p) + std::lgamma(ap1); + z = exp((v + z - ls) / a); + result = z * (1 - (a * log(z) - z - v + ls) / (a - z)); + } + } + } + return result; +} + template T gamma_p_inv_imp(const T a, const T p) { if (is_nan(a) || is_nan(p)) { @@ -53,13 +317,9 @@ T gamma_p_inv_imp(const T a, const T p) { return 0; } - // TODO // Get an initial guess (https://dl.acm.org/doi/abs/10.1145/22721.23109) - // T guess = find_inverse_gamma(a, p, 1 - p); bool has_10_digits = false; - boost::math::policies::policy<> pol; - T guess = boost::math::detail::find_inverse_gamma(a, p, 1 - p, pol, - &has_10_digits); + T guess = find_inverse_gamma(a, p, 1 - p, &has_10_digits); T lower = LIM::min(); if (guess <= lower) { @@ -67,35 +327,21 @@ T gamma_p_inv_imp(const T a, const T p) { } // TODO + // The number of digits to converge to. + // This is an arbitrary but reasonable number, + // though Boost does more sophisticated things + // using the first derivative. + unsigned digits = 25; + // Number of Halley iterations // The default used in Boost is 200 // uint_fast16_t max_iter = 200; - // The number of digits to converge to. - // This is an arbitrary number, - // but Boost does more sophisticated things - // using the first derivative. - // unsigned digits = 40; - // // Perform Halley iteration for root-finding to get a more refined answer // guess = halley_iterate(gamma_p_inverse_func(a, p, false), guess, lower, // LIM::max(), digits, max_iter); - unsigned digits = - boost::math::policies::digits>(); - if (digits < 30) { - digits *= 2; - digits /= 3; - } else { - digits /= 2; - digits -= 1; - } - if ((a < T(0.125)) && (fabs(boost::math::gamma_p_derivative(a, guess, pol)) > - 1 / sqrt(boost::math::tools::epsilon()))) - digits = - boost::math::policies::digits>() - 2; - // + // Go ahead and iterate: - // std::uintmax_t max_iter = boost::math::policies::get_max_root_iterations< boost::math::policies::policy<>>(); guess = boost::math::tools::halley_iterate( From 6f386168a459db94f6cdda9e15ef3da499dc873d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Oct 2023 11:09:35 -0400 Subject: [PATCH 17/33] gamma inverse functional --- gtsam/nonlinear/internal/Gamma.h | 94 ++++++++++++++++++++ gtsam/nonlinear/internal/Utils.h | 49 ++++++++++ gtsam/nonlinear/internal/chiSquaredInverse.h | 19 ++-- 3 files changed, 152 insertions(+), 10 deletions(-) create mode 100644 gtsam/nonlinear/internal/Gamma.h create mode 100644 gtsam/nonlinear/internal/Utils.h diff --git a/gtsam/nonlinear/internal/Gamma.h b/gtsam/nonlinear/internal/Gamma.h new file mode 100644 index 0000000000..325fb7eeef --- /dev/null +++ b/gtsam/nonlinear/internal/Gamma.h @@ -0,0 +1,94 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Gamma.h + * @brief Gamma and Gamma Inverse functions + * + * A lot of this code has been picked up from + * https://www.boost.org/doc/libs/1_83_0/boost/math/special_functions/detail/igamma_inverse.hpp + * + * @author Varun Agrawal + */ + +#pragma once + +#include + +#include + +namespace gtsam { + +namespace internal { + +/** + * @brief Functional to compute the gamma inverse. + * Mainly used with Halley iteration. + * + * @tparam T + */ +template +struct gamma_p_inverse_func { + gamma_p_inverse_func(T a_, T p_, bool inv) : a(a_), p(p_), invert(inv) { + /* + If p is too near 1 then P(x) - p suffers from cancellation + errors causing our root-finding algorithms to "thrash", better + to invert in this case and calculate Q(x) - (1-p) instead. + + Of course if p is *very* close to 1, then the answer we get will + be inaccurate anyway (because there's not enough information in p) + but at least we will converge on the (inaccurate) answer quickly. + */ + if (p > T(0.9)) { + p = 1 - p; + invert = !invert; + } + } + + std::tuple operator()(const T& x) const { + // Calculate P(x) - p and the first two derivates, or if the invert + // flag is set, then Q(x) - q and it's derivatives. + T f, f1; + T ft; + boost::math::policies::policy<> pol; + f = static_cast(boost::math::detail::gamma_incomplete_imp( + a, x, true, invert, pol, &ft)); + f1 = ft; + T f2; + T div = (a - x - 1) / x; + f2 = f1; + + if (fabs(div) > 1) { + if (internal::LIM::max() / fabs(div) < f2) { + // overflow: + f2 = -internal::LIM::max() / 2; + } else { + f2 *= div; + } + } else { + f2 *= div; + } + + if (invert) { + f1 = -f1; + f2 = -f2; + } + + return std::make_tuple(static_cast(f - p), f1, f2); + } + + private: + T a, p; + bool invert; +}; + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/nonlinear/internal/Utils.h b/gtsam/nonlinear/internal/Utils.h new file mode 100644 index 0000000000..23573346cb --- /dev/null +++ b/gtsam/nonlinear/internal/Utils.h @@ -0,0 +1,49 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Utils.h + * @brief Utilities for the Chi Squared inverse and related operations. + * @author Varun Agrawal + */ + +#pragma once + +namespace gtsam { +namespace internal { + +/// Template type for numeric limits +template +using LIM = std::numeric_limits; + +template +using return_t = + typename std::conditional::value, double, T>::type; + +/// Get common type amongst all arguments +template +using common_t = typename std::common_type::type; + +/// Helper template for finding common return type +template +using common_return_t = return_t>; + +/// Check if integer is odd +constexpr bool is_odd(const long long int x) noexcept { return (x & 1U) != 0; } + +/// Templated check for NaN +template +constexpr bool is_nan(const T x) noexcept { + return x != x; +} + +} // namespace internal +} // namespace gtsam diff --git a/gtsam/nonlinear/internal/chiSquaredInverse.h b/gtsam/nonlinear/internal/chiSquaredInverse.h index dc85958460..b7744ffa22 100644 --- a/gtsam/nonlinear/internal/chiSquaredInverse.h +++ b/gtsam/nonlinear/internal/chiSquaredInverse.h @@ -25,13 +25,13 @@ #pragma once #include -#include #include #include // TODO(Varun) remove -#include +// #include +#include namespace gtsam { @@ -320,13 +320,15 @@ T gamma_p_inv_imp(const T a, const T p) { // Get an initial guess (https://dl.acm.org/doi/abs/10.1145/22721.23109) bool has_10_digits = false; T guess = find_inverse_gamma(a, p, 1 - p, &has_10_digits); + if (has_10_digits) { + return guess; + } T lower = LIM::min(); if (guess <= lower) { guess = LIM::min(); } - // TODO // The number of digits to converge to. // This is an arbitrary but reasonable number, // though Boost does more sophisticated things @@ -334,20 +336,17 @@ T gamma_p_inv_imp(const T a, const T p) { unsigned digits = 25; // Number of Halley iterations - // The default used in Boost is 200 - // uint_fast16_t max_iter = 200; + uintmax_t max_iter = 200; + // TODO // // Perform Halley iteration for root-finding to get a more refined answer // guess = halley_iterate(gamma_p_inverse_func(a, p, false), guess, lower, // LIM::max(), digits, max_iter); // Go ahead and iterate: - std::uintmax_t max_iter = boost::math::policies::get_max_root_iterations< - boost::math::policies::policy<>>(); guess = boost::math::tools::halley_iterate( - boost::math::detail::gamma_p_inverse_func< - T, boost::math::policies::policy<>>(a, p, false), - guess, lower, boost::math::tools::max_value(), digits, max_iter); + internal::gamma_p_inverse_func(a, p, false), guess, lower, + LIM::max(), digits, max_iter); if (guess == lower) { throw std::runtime_error( From 25ebdd54fc23ca0624a42540e941cd04abc7eb9e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Oct 2023 15:15:02 -0400 Subject: [PATCH 18/33] add gamma_p_inverse_func --- gtsam/nonlinear/internal/chiSquaredInverse.h | 72 ++++++++++++++++++-- 1 file changed, 67 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/internal/chiSquaredInverse.h b/gtsam/nonlinear/internal/chiSquaredInverse.h index b7744ffa22..7577721dc3 100644 --- a/gtsam/nonlinear/internal/chiSquaredInverse.h +++ b/gtsam/nonlinear/internal/chiSquaredInverse.h @@ -24,13 +24,12 @@ #pragma once -#include #include #include // TODO(Varun) remove -// #include +#include #include namespace gtsam { @@ -301,6 +300,65 @@ T find_inverse_gamma(T a, T p, T q, bool* p_has_10_digits) { return result; } +/** + * @brief Functional to compute the gamma inverse. + * Mainly used with Halley iteration. + * + * @tparam T + */ +template +struct gamma_p_inverse_func { + gamma_p_inverse_func(T a_, T p_, bool inv) : a(a_), p(p_), invert(inv) { + /* + If p is too near 1 then P(x) - p suffers from cancellation + errors causing our root-finding algorithms to "thrash", better + to invert in this case and calculate Q(x) - (1-p) instead. + + Of course if p is *very* close to 1, then the answer we get will + be inaccurate anyway (because there's not enough information in p) + but at least we will converge on the (inaccurate) answer quickly. + */ + if (p > T(0.9)) { + p = 1 - p; + invert = !invert; + } + } + + std::tuple operator()(const T& x) const { + // Calculate P(x) - p and the first two derivates, or if the invert + // flag is set, then Q(x) - q and it's derivatives. + T f, f1; + T ft; + f = static_cast(gamma_incomplete_imp(a, x, true, invert, &ft)); + f1 = ft; + T f2; + T div = (a - x - 1) / x; + f2 = f1; + + if (fabs(div) > 1) { + if (internal::LIM::max() / fabs(div) < f2) { + // overflow: + f2 = -internal::LIM::max() / 2; + } else { + f2 *= div; + } + } else { + f2 *= div; + } + + if (invert) { + f1 = -f1; + f2 = -f2; + } + + return std::make_tuple(static_cast(f - p), f1, f2); + } + + private: + T a, p; + bool invert; +}; + template T gamma_p_inv_imp(const T a, const T p) { if (is_nan(a) || is_nan(p)) { @@ -339,14 +397,18 @@ T gamma_p_inv_imp(const T a, const T p) { uintmax_t max_iter = 200; // TODO - // // Perform Halley iteration for root-finding to get a more refined answer + // Perform Halley iteration for root-finding to get a more refined answer // guess = halley_iterate(gamma_p_inverse_func(a, p, false), guess, lower, // LIM::max(), digits, max_iter); // Go ahead and iterate: + // guess = boost::math::tools::halley_iterate( + // internal::gamma_p_inverse_func(a, p, false), guess, lower, + // LIM::max(), digits, max_iter); guess = boost::math::tools::halley_iterate( - internal::gamma_p_inverse_func(a, p, false), guess, lower, - LIM::max(), digits, max_iter); + boost::math::detail::gamma_p_inverse_func< + T, boost::math::policies::policy<>>(a, p, false), + guess, lower, boost::math::tools::max_value(), digits, max_iter); if (guess == lower) { throw std::runtime_error( From 203a84dc0ec32526f9e98af68c10a4191a57e5f9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Dec 2023 17:12:51 -0500 Subject: [PATCH 19/33] add more gamma functions --- gtsam/nonlinear/internal/Gamma.h | 383 ++++++++++++++++++- gtsam/nonlinear/internal/chiSquaredInverse.h | 69 +--- 2 files changed, 384 insertions(+), 68 deletions(-) diff --git a/gtsam/nonlinear/internal/Gamma.h b/gtsam/nonlinear/internal/Gamma.h index 325fb7eeef..edd2b20717 100644 --- a/gtsam/nonlinear/internal/Gamma.h +++ b/gtsam/nonlinear/internal/Gamma.h @@ -29,6 +29,385 @@ namespace gtsam { namespace internal { +template +inline constexpr T log_max_value() { + return log(LIM::max()); +} + +/** + * @brief Incomplete gamma functions follow + * + * @tparam T + */ +template +struct upper_incomplete_gamma_fract { + private: + T z, a; + int k; + + public: + typedef std::pair result_type; + + upper_incomplete_gamma_fract(T a1, T z1) : z(z1 - a1 + 1), a(a1), k(0) {} + + result_type operator()() { + ++k; + z += 2; + return result_type(k * (a - k), z); + } +}; + +template +inline T upper_gamma_fraction(T a, T z, T eps) { + // Multiply result by z^a * e^-z to get the full + // upper incomplete integral. Divide by tgamma(z) + // to normalise. + upper_incomplete_gamma_fract f(a, z); + return 1 / (z - a + 1 + boost::math::tools::continued_fraction_a(f, eps)); +} + +/** + * @brief Main incomplete gamma entry point, handles all four incomplete + * gamma's: + * + * @tparam T + * @tparam Policy + * @param a + * @param x + * @param normalised + * @param invert + * @param pol + * @param p_derivative + * @return T + */ +template +T gamma_incomplete_imp(T a, T x, bool normalised, bool invert, + const Policy& pol, T* p_derivative) { + if (a <= 0) { + throw std::runtime_error( + "Argument a to the incomplete gamma function must be greater than " + "zero"); + } + if (x < 0) { + throw std::runtime_error( + "Argument x to the incomplete gamma function must be >= 0"); + } + + typedef typename boost::math::lanczos::lanczos::type lanczos_type; + + T result = 0; // Just to avoid warning C4701: potentially uninitialized local + // variable 'result' used + + if (a >= boost::math::max_factorial::value && !normalised) { + // + // When we're computing the non-normalized incomplete gamma + // and a is large the result is rather hard to compute unless + // we use logs. There are really two options - if x is a long + // way from a in value then we can reliably use methods 2 and 4 + // below in logarithmic form and go straight to the result. + // Otherwise we let the regularized gamma take the strain + // (the result is unlikely to underflow in the central region anyway) + // and combine with lgamma in the hopes that we get a finite result. + // + if (invert && (a * 4 < x)) { + // This is method 4 below, done in logs: + result = a * log(x) - x; + if (p_derivative) *p_derivative = exp(result); + result += log(upper_gamma_fraction( + a, x, boost::math::policies::get_epsilon())); + } else if (!invert && (a > 4 * x)) { + // This is method 2 below, done in logs: + result = a * log(x) - x; + if (p_derivative) *p_derivative = exp(result); + T init_value = 0; + result += log( + boost::math::detail::lower_gamma_series(a, x, pol, init_value) / a); + } else { + result = gamma_incomplete_imp(a, x, true, invert, pol, p_derivative); + if (result == 0) { + if (invert) { + // Try http://functions.wolfram.com/06.06.06.0039.01 + result = 1 + 1 / (12 * a) + 1 / (288 * a * a); + result = log(result) - a + (a - 0.5f) * log(a) + log(sqrt(2 * M_PI)); + if (p_derivative) *p_derivative = exp(a * log(x) - x); + } else { + // This is method 2 below, done in logs, we're really outside the + // range of this method, but since the result is almost certainly + // infinite, we should probably be OK: + result = a * log(x) - x; + if (p_derivative) *p_derivative = exp(result); + T init_value = 0; + result += log( + boost::math::detail::lower_gamma_series(a, x, pol, init_value) / + a); + } + } else { + result = log(result) + boost::math::lgamma(a, pol); + } + } + if (result > log_max_value()) { + throw std::overflow_error( + "gamma_incomplete_imp: result is larger than log of max value"); + } + + return exp(result); + } + + BOOST_MATH_ASSERT((p_derivative == nullptr) || normalised); + + bool is_int, is_half_int; + bool is_small_a = (a < 30) && (a <= x + 1) && (x < log_max_value()); + if (is_small_a) { + T fa = floor(a); + is_int = (fa == a); + is_half_int = is_int ? false : (fabs(fa - a) == 0.5f); + } else { + is_int = is_half_int = false; + } + + int eval_method; + + if (is_int && (x > 0.6)) { + // calculate Q via finite sum: + invert = !invert; + eval_method = 0; + } else if (is_half_int && (x > 0.2)) { + // calculate Q via finite sum for half integer a: + invert = !invert; + eval_method = 1; + } else if ((x < boost::math::tools::root_epsilon()) && (a > 1)) { + eval_method = 6; + } else if ((x > 1000) && ((a < x) || (fabs(a - 50) / x < 1))) { + // calculate Q via asymptotic approximation: + invert = !invert; + eval_method = 7; + } else if (x < T(0.5)) { + // + // Changeover criterion chosen to give a changeover at Q ~ 0.33 + // + if (T(-0.4) / log(x) < a) { + eval_method = 2; + } else { + eval_method = 3; + } + } else if (x < T(1.1)) { + // + // Changeover here occurs when P ~ 0.75 or Q ~ 0.25: + // + if (x * 0.75f < a) { + eval_method = 2; + } else { + eval_method = 3; + } + } else { + // + // Begin by testing whether we're in the "bad" zone + // where the result will be near 0.5 and the usual + // series and continued fractions are slow to converge: + // + bool use_temme = false; + if (normalised && std::numeric_limits::is_specialized && (a > 20)) { + T sigma = fabs((x - a) / a); + if ((a > 200) && (boost::math::policies::digits() <= 113)) { + // + // This limit is chosen so that we use Temme's expansion + // only if the result would be larger than about 10^-6. + // Below that the regular series and continued fractions + // converge OK, and if we use Temme's method we get increasing + // errors from the dominant erfc term as it's (inexact) argument + // increases in magnitude. + // + if (20 / a > sigma * sigma) use_temme = true; + } else if (boost::math::policies::digits() <= 64) { + // Note in this zone we can't use Temme's expansion for + // types longer than an 80-bit real: + // it would require too many terms in the polynomials. + if (sigma < 0.4) use_temme = true; + } + } + if (use_temme) { + eval_method = 5; + } else { + // + // Regular case where the result will not be too close to 0.5. + // + // Changeover here occurs at P ~ Q ~ 0.5 + // Note that series computation of P is about x2 faster than continued + // fraction calculation of Q, so try and use the CF only when really + // necessary, especially for small x. + // + if (x - (1 / (3 * x)) < a) { + eval_method = 2; + } else { + eval_method = 4; + invert = !invert; + } + } + } + + switch (eval_method) { + case 0: { + result = boost::math::detail::finite_gamma_q(a, x, pol, p_derivative); + if (!normalised) result *= boost::math::tgamma(a, pol); + break; + } + case 1: { + result = + boost::math::detail::finite_half_gamma_q(a, x, p_derivative, pol); + if (!normalised) result *= boost::math::tgamma(a, pol); + if (p_derivative && (*p_derivative == 0)) + *p_derivative = boost::math::detail::regularised_gamma_prefix( + a, x, pol, lanczos_type()); + break; + } + case 2: { + // Compute P: + result = normalised ? boost::math::detail::regularised_gamma_prefix( + a, x, pol, lanczos_type()) + : boost::math::detail::full_igamma_prefix(a, x, pol); + if (p_derivative) *p_derivative = result; + if (result != 0) { + // + // If we're going to be inverting the result then we can + // reduce the number of series evaluations by quite + // a few iterations if we set an initial value for the + // series sum based on what we'll end up subtracting it from + // at the end. + // Have to be careful though that this optimization doesn't + // lead to spurious numeric overflow. Note that the + // scary/expensive overflow checks below are more often + // than not bypassed in practice for "sensible" input + // values: + // + T init_value = 0; + bool optimised_invert = false; + if (invert) { + init_value = (normalised ? 1 : boost::math::tgamma(a, pol)); + if (normalised || (result >= 1) || + (LIM::max() * result > init_value)) { + init_value /= result; + if (normalised || (a < 1) || (LIM::max() / a > init_value)) { + init_value *= -a; + optimised_invert = true; + } else + init_value = 0; + } else + init_value = 0; + } + result *= + boost::math::detail::lower_gamma_series(a, x, pol, init_value) / a; + if (optimised_invert) { + invert = false; + result = -result; + } + } + break; + } + case 3: { + // Compute Q: + invert = !invert; + T g; + result = boost::math::detail::tgamma_small_upper_part( + a, x, pol, &g, invert, p_derivative); + invert = false; + if (normalised) result /= g; + break; + } + case 4: { + // Compute Q: + result = normalised ? boost::math::detail::regularised_gamma_prefix( + a, x, pol, lanczos_type()) + : boost::math::detail::full_igamma_prefix(a, x, pol); + if (p_derivative) *p_derivative = result; + if (result != 0) + result *= upper_gamma_fraction( + a, x, boost::math::policies::get_epsilon()); + break; + } + case 5: { + // + // Use compile time dispatch to the appropriate + // Temme asymptotic expansion. This may be dead code + // if T does not have numeric limits support, or has + // too many digits for the most precise version of + // these expansions, in that case we'll be calling + // an empty function. + // + typedef typename boost::math::policies::precision::type + precision_type; + + typedef std::integral_constant + tag_type; + + result = boost::math::detail::igamma_temme_large( + a, x, pol, static_cast(nullptr)); + if (x >= a) invert = !invert; + if (p_derivative) + *p_derivative = boost::math::detail::regularised_gamma_prefix( + a, x, pol, lanczos_type()); + break; + } + case 6: { + // x is so small that P is necessarily very small too, + // use + // http://functions.wolfram.com/GammaBetaErf/GammaRegularized/06/01/05/01/01/ + if (!normalised) + result = pow(x, a) / (a); + else { +#ifndef BOOST_NO_EXCEPTIONS + try { +#endif + result = pow(x, a) / boost::math::tgamma(a + 1, pol); +#ifndef BOOST_NO_EXCEPTIONS + } catch (const std::overflow_error&) { + result = 0; + } +#endif + } + result *= 1 - a * x / (a + 1); + if (p_derivative) + *p_derivative = boost::math::detail::regularised_gamma_prefix( + a, x, pol, lanczos_type()); + break; + } + case 7: { + // x is large, + // Compute Q: + result = normalised ? boost::math::detail::regularised_gamma_prefix( + a, x, pol, lanczos_type()) + : boost::math::detail::full_igamma_prefix(a, x, pol); + if (p_derivative) *p_derivative = result; + result /= x; + if (result != 0) + result *= boost::math::detail::incomplete_tgamma_large_x(a, x, pol); + break; + } + } + + if (normalised && (result > 1)) result = 1; + if (invert) { + T gam = normalised ? 1 : boost::math::tgamma(a, pol); + result = gam - result; + } + if (p_derivative) { + // + // Need to convert prefix term to derivative: + // + if ((x < 1) && (LIM::max() * x < *p_derivative)) { + // overflow, just return an arbitrarily large value: + *p_derivative = LIM::max() / 2; + } + + *p_derivative /= x; + } + + return result; +} + /** * @brief Functional to compute the gamma inverse. * Mainly used with Halley iteration. @@ -59,8 +438,8 @@ struct gamma_p_inverse_func { T f, f1; T ft; boost::math::policies::policy<> pol; - f = static_cast(boost::math::detail::gamma_incomplete_imp( - a, x, true, invert, pol, &ft)); + f = static_cast( + internal::gamma_incomplete_imp(a, x, true, invert, pol, &ft)); f1 = ft; T f2; T div = (a - x - 1) / x; diff --git a/gtsam/nonlinear/internal/chiSquaredInverse.h b/gtsam/nonlinear/internal/chiSquaredInverse.h index 7577721dc3..d4f79147af 100644 --- a/gtsam/nonlinear/internal/chiSquaredInverse.h +++ b/gtsam/nonlinear/internal/chiSquaredInverse.h @@ -24,12 +24,12 @@ #pragma once +#include #include #include // TODO(Varun) remove -#include #include namespace gtsam { @@ -300,65 +300,6 @@ T find_inverse_gamma(T a, T p, T q, bool* p_has_10_digits) { return result; } -/** - * @brief Functional to compute the gamma inverse. - * Mainly used with Halley iteration. - * - * @tparam T - */ -template -struct gamma_p_inverse_func { - gamma_p_inverse_func(T a_, T p_, bool inv) : a(a_), p(p_), invert(inv) { - /* - If p is too near 1 then P(x) - p suffers from cancellation - errors causing our root-finding algorithms to "thrash", better - to invert in this case and calculate Q(x) - (1-p) instead. - - Of course if p is *very* close to 1, then the answer we get will - be inaccurate anyway (because there's not enough information in p) - but at least we will converge on the (inaccurate) answer quickly. - */ - if (p > T(0.9)) { - p = 1 - p; - invert = !invert; - } - } - - std::tuple operator()(const T& x) const { - // Calculate P(x) - p and the first two derivates, or if the invert - // flag is set, then Q(x) - q and it's derivatives. - T f, f1; - T ft; - f = static_cast(gamma_incomplete_imp(a, x, true, invert, &ft)); - f1 = ft; - T f2; - T div = (a - x - 1) / x; - f2 = f1; - - if (fabs(div) > 1) { - if (internal::LIM::max() / fabs(div) < f2) { - // overflow: - f2 = -internal::LIM::max() / 2; - } else { - f2 *= div; - } - } else { - f2 *= div; - } - - if (invert) { - f1 = -f1; - f2 = -f2; - } - - return std::make_tuple(static_cast(f - p), f1, f2); - } - - private: - T a, p; - bool invert; -}; - template T gamma_p_inv_imp(const T a, const T p) { if (is_nan(a) || is_nan(p)) { @@ -402,13 +343,9 @@ T gamma_p_inv_imp(const T a, const T p) { // LIM::max(), digits, max_iter); // Go ahead and iterate: - // guess = boost::math::tools::halley_iterate( - // internal::gamma_p_inverse_func(a, p, false), guess, lower, - // LIM::max(), digits, max_iter); guess = boost::math::tools::halley_iterate( - boost::math::detail::gamma_p_inverse_func< - T, boost::math::policies::policy<>>(a, p, false), - guess, lower, boost::math::tools::max_value(), digits, max_iter); + internal::gamma_p_inverse_func(a, p, false), guess, lower, + LIM::max(), digits, max_iter); if (guess == lower) { throw std::runtime_error( From 87c572912e98b9ee95d8fc854bd490b31b8635ed Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Dec 2023 18:03:02 -0500 Subject: [PATCH 20/33] more code --- gtsam/nonlinear/internal/Gamma.h | 78 +++++++++++++++++++++++++++++--- 1 file changed, 71 insertions(+), 7 deletions(-) diff --git a/gtsam/nonlinear/internal/Gamma.h b/gtsam/nonlinear/internal/Gamma.h index edd2b20717..6c63ff7cbf 100644 --- a/gtsam/nonlinear/internal/Gamma.h +++ b/gtsam/nonlinear/internal/Gamma.h @@ -24,6 +24,7 @@ #include #include +#include namespace gtsam { @@ -34,6 +35,72 @@ inline constexpr T log_max_value() { return log(LIM::max()); } +/** + * @brief Upper gamma fraction for integer a + * + * @param a + * @param x + * @param pol + * @param pderivative + * @return template + */ +template +inline T finite_gamma_q(T a, T x, Policy const& pol, T* pderivative = 0) { + // Calculates normalised Q when a is an integer: + T e = exp(-x); + T sum = e; + if (sum != 0) { + T term = sum; + for (unsigned n = 1; n < a; ++n) { + term /= n; + term *= x; + sum += term; + } + } + if (pderivative) { + *pderivative = e * pow(x, a) / + boost::math::unchecked_factorial(std::trunc(T(a - 1))); + } + return sum; +} + +/** + * @brief Upper gamma fraction for half integer a + * + * @tparam T + * @tparam Policy + * @param a + * @param x + * @param p_derivative + * @param pol + * @return T + */ +template +T finite_half_gamma_q(T a, T x, T* p_derivative, const Policy& pol) { + // Calculates normalised Q when a is a half-integer: + T e = boost::math::erfc(sqrt(x), pol); + if ((e != 0) && (a > 1)) { + T term = exp(-x) / sqrt(M_PI * x); + term *= x; + static const T half = T(1) / 2; + term /= half; + T sum = term; + for (unsigned n = 2; n < a; ++n) { + term /= n - half; + term *= x; + sum += term; + } + e += sum; + if (p_derivative) { + *p_derivative = 0; + } + } else if (p_derivative) { + // We'll be dividing by x later, so calculate derivative * x: + *p_derivative = sqrt(x) * exp(-x) / sqrt(M_PI); + } + return e; +} + /** * @brief Incomplete gamma functions follow * @@ -98,7 +165,8 @@ T gamma_incomplete_imp(T a, T x, bool normalised, bool invert, T result = 0; // Just to avoid warning C4701: potentially uninitialized local // variable 'result' used - if (a >= boost::math::max_factorial::value && !normalised) { + // max_factorial value for long double is 170 in Boost + if (a >= 170 && !normalised) { // // When we're computing the non-normalized incomplete gamma // and a is large the result is rather hard to compute unless @@ -153,7 +221,7 @@ T gamma_incomplete_imp(T a, T x, bool normalised, bool invert, return exp(result); } - BOOST_MATH_ASSERT((p_derivative == nullptr) || normalised); + assert((p_derivative == nullptr) || normalised); bool is_int, is_half_int; bool is_small_a = (a < 30) && (a <= x + 1) && (x < log_max_value()); @@ -247,7 +315,7 @@ T gamma_incomplete_imp(T a, T x, bool normalised, bool invert, switch (eval_method) { case 0: { - result = boost::math::detail::finite_gamma_q(a, x, pol, p_derivative); + result = finite_gamma_q(a, x, pol, p_derivative); if (!normalised) result *= boost::math::tgamma(a, pol); break; } @@ -358,15 +426,11 @@ T gamma_incomplete_imp(T a, T x, bool normalised, bool invert, if (!normalised) result = pow(x, a) / (a); else { -#ifndef BOOST_NO_EXCEPTIONS try { -#endif result = pow(x, a) / boost::math::tgamma(a + 1, pol); -#ifndef BOOST_NO_EXCEPTIONS } catch (const std::overflow_error&) { result = 0; } -#endif } result *= 1 - a * x / (a + 1); if (p_derivative) From 4326195786676cc344a713bb4eec71a37906a8bf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 07:25:55 -0500 Subject: [PATCH 21/33] cephes source code --- gtsam/3rdparty/cephes/CMakeLists.txt | 102 ++ gtsam/3rdparty/cephes/cephes.h | 163 +++ gtsam/3rdparty/cephes/cephes/airy.c | 376 ++++++ gtsam/3rdparty/cephes/cephes/bdtr.c | 241 ++++ gtsam/3rdparty/cephes/cephes/besselpoly.c | 34 + gtsam/3rdparty/cephes/cephes/beta.c | 258 ++++ gtsam/3rdparty/cephes/cephes/btdtr.c | 59 + gtsam/3rdparty/cephes/cephes/cbrt.c | 117 ++ gtsam/3rdparty/cephes/cephes/cephes_names.h | 114 ++ gtsam/3rdparty/cephes/cephes/chbevl.c | 81 ++ gtsam/3rdparty/cephes/cephes/chdtr.c | 186 +++ gtsam/3rdparty/cephes/cephes/const.c | 129 ++ gtsam/3rdparty/cephes/cephes/dawsn.c | 160 +++ gtsam/3rdparty/cephes/cephes/dd_idefs.h | 198 +++ gtsam/3rdparty/cephes/cephes/dd_real.c | 587 +++++++++ gtsam/3rdparty/cephes/cephes/dd_real.h | 143 +++ gtsam/3rdparty/cephes/cephes/dd_real_idefs.h | 557 +++++++++ gtsam/3rdparty/cephes/cephes/ellie.c | 282 +++++ gtsam/3rdparty/cephes/cephes/ellik.c | 246 ++++ gtsam/3rdparty/cephes/cephes/ellpe.c | 108 ++ gtsam/3rdparty/cephes/cephes/ellpj.c | 154 +++ gtsam/3rdparty/cephes/cephes/ellpk.c | 124 ++ gtsam/3rdparty/cephes/cephes/erfinv.c | 78 ++ gtsam/3rdparty/cephes/cephes/exp10.c | 115 ++ gtsam/3rdparty/cephes/cephes/exp2.c | 108 ++ gtsam/3rdparty/cephes/cephes/expn.c | 224 ++++ gtsam/3rdparty/cephes/cephes/expn.h | 19 + gtsam/3rdparty/cephes/cephes/fdtr.c | 216 ++++ gtsam/3rdparty/cephes/cephes/fresnl.c | 219 ++++ gtsam/3rdparty/cephes/cephes/gamma.c | 364 ++++++ gtsam/3rdparty/cephes/cephes/gammasgn.c | 25 + gtsam/3rdparty/cephes/cephes/gdtr.c | 132 ++ gtsam/3rdparty/cephes/cephes/hyp2f1.c | 569 +++++++++ gtsam/3rdparty/cephes/cephes/hyperg.c | 362 ++++++ gtsam/3rdparty/cephes/cephes/i0.c | 180 +++ gtsam/3rdparty/cephes/cephes/i1.c | 184 +++ gtsam/3rdparty/cephes/cephes/igam.c | 423 +++++++ gtsam/3rdparty/cephes/cephes/igam.h | 38 + gtsam/3rdparty/cephes/cephes/igami.c | 339 ++++++ gtsam/3rdparty/cephes/cephes/incbet.c | 369 ++++++ gtsam/3rdparty/cephes/cephes/incbi.c | 275 +++++ gtsam/3rdparty/cephes/cephes/j0.c | 246 ++++ gtsam/3rdparty/cephes/cephes/j1.c | 225 ++++ gtsam/3rdparty/cephes/cephes/jv.c | 841 +++++++++++++ gtsam/3rdparty/cephes/cephes/k0.c | 178 +++ gtsam/3rdparty/cephes/cephes/k1.c | 179 +++ gtsam/3rdparty/cephes/cephes/kn.c | 235 ++++ gtsam/3rdparty/cephes/cephes/kolmogorov.c | 1147 ++++++++++++++++++ gtsam/3rdparty/cephes/cephes/lanczos.c | 56 + gtsam/3rdparty/cephes/cephes/lanczos.h | 133 ++ gtsam/3rdparty/cephes/cephes/mconf.h | 109 ++ gtsam/3rdparty/cephes/cephes/nbdtr.c | 207 ++++ gtsam/3rdparty/cephes/cephes/ndtr.c | 305 +++++ gtsam/3rdparty/cephes/cephes/ndtri.c | 176 +++ gtsam/3rdparty/cephes/cephes/owens_t.c | 364 ++++++ gtsam/3rdparty/cephes/cephes/pdtr.c | 173 +++ gtsam/3rdparty/cephes/cephes/poch.c | 81 ++ gtsam/3rdparty/cephes/cephes/polevl.h | 165 +++ gtsam/3rdparty/cephes/cephes/psi.c | 205 ++++ gtsam/3rdparty/cephes/cephes/rgamma.c | 128 ++ gtsam/3rdparty/cephes/cephes/round.c | 63 + gtsam/3rdparty/cephes/cephes/scipy_iv.c | 654 ++++++++++ gtsam/3rdparty/cephes/cephes/sf_error.c | 45 + gtsam/3rdparty/cephes/cephes/sf_error.h | 38 + gtsam/3rdparty/cephes/cephes/shichi.c | 305 +++++ gtsam/3rdparty/cephes/cephes/sici.c | 276 +++++ gtsam/3rdparty/cephes/cephes/sindg.c | 219 ++++ gtsam/3rdparty/cephes/cephes/sinpi.c | 54 + gtsam/3rdparty/cephes/cephes/spence.c | 125 ++ gtsam/3rdparty/cephes/cephes/stdtr.c | 203 ++++ gtsam/3rdparty/cephes/cephes/struve.c | 408 +++++++ gtsam/3rdparty/cephes/cephes/tandg.c | 141 +++ gtsam/3rdparty/cephes/cephes/tukey.c | 68 ++ gtsam/3rdparty/cephes/cephes/unity.c | 190 +++ gtsam/3rdparty/cephes/cephes/yn.c | 105 ++ gtsam/3rdparty/cephes/cephes/yv.c | 46 + gtsam/3rdparty/cephes/cephes/zeta.c | 160 +++ gtsam/3rdparty/cephes/cephes/zetac.c | 345 ++++++ 78 files changed, 17256 insertions(+) create mode 100644 gtsam/3rdparty/cephes/CMakeLists.txt create mode 100644 gtsam/3rdparty/cephes/cephes.h create mode 100644 gtsam/3rdparty/cephes/cephes/airy.c create mode 100644 gtsam/3rdparty/cephes/cephes/bdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/besselpoly.c create mode 100644 gtsam/3rdparty/cephes/cephes/beta.c create mode 100644 gtsam/3rdparty/cephes/cephes/btdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/cbrt.c create mode 100644 gtsam/3rdparty/cephes/cephes/cephes_names.h create mode 100644 gtsam/3rdparty/cephes/cephes/chbevl.c create mode 100644 gtsam/3rdparty/cephes/cephes/chdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/const.c create mode 100644 gtsam/3rdparty/cephes/cephes/dawsn.c create mode 100644 gtsam/3rdparty/cephes/cephes/dd_idefs.h create mode 100644 gtsam/3rdparty/cephes/cephes/dd_real.c create mode 100644 gtsam/3rdparty/cephes/cephes/dd_real.h create mode 100644 gtsam/3rdparty/cephes/cephes/dd_real_idefs.h create mode 100644 gtsam/3rdparty/cephes/cephes/ellie.c create mode 100644 gtsam/3rdparty/cephes/cephes/ellik.c create mode 100644 gtsam/3rdparty/cephes/cephes/ellpe.c create mode 100644 gtsam/3rdparty/cephes/cephes/ellpj.c create mode 100644 gtsam/3rdparty/cephes/cephes/ellpk.c create mode 100644 gtsam/3rdparty/cephes/cephes/erfinv.c create mode 100644 gtsam/3rdparty/cephes/cephes/exp10.c create mode 100644 gtsam/3rdparty/cephes/cephes/exp2.c create mode 100644 gtsam/3rdparty/cephes/cephes/expn.c create mode 100644 gtsam/3rdparty/cephes/cephes/expn.h create mode 100644 gtsam/3rdparty/cephes/cephes/fdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/fresnl.c create mode 100644 gtsam/3rdparty/cephes/cephes/gamma.c create mode 100644 gtsam/3rdparty/cephes/cephes/gammasgn.c create mode 100644 gtsam/3rdparty/cephes/cephes/gdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/hyp2f1.c create mode 100644 gtsam/3rdparty/cephes/cephes/hyperg.c create mode 100644 gtsam/3rdparty/cephes/cephes/i0.c create mode 100644 gtsam/3rdparty/cephes/cephes/i1.c create mode 100644 gtsam/3rdparty/cephes/cephes/igam.c create mode 100644 gtsam/3rdparty/cephes/cephes/igam.h create mode 100644 gtsam/3rdparty/cephes/cephes/igami.c create mode 100644 gtsam/3rdparty/cephes/cephes/incbet.c create mode 100644 gtsam/3rdparty/cephes/cephes/incbi.c create mode 100644 gtsam/3rdparty/cephes/cephes/j0.c create mode 100644 gtsam/3rdparty/cephes/cephes/j1.c create mode 100644 gtsam/3rdparty/cephes/cephes/jv.c create mode 100644 gtsam/3rdparty/cephes/cephes/k0.c create mode 100644 gtsam/3rdparty/cephes/cephes/k1.c create mode 100644 gtsam/3rdparty/cephes/cephes/kn.c create mode 100644 gtsam/3rdparty/cephes/cephes/kolmogorov.c create mode 100644 gtsam/3rdparty/cephes/cephes/lanczos.c create mode 100644 gtsam/3rdparty/cephes/cephes/lanczos.h create mode 100644 gtsam/3rdparty/cephes/cephes/mconf.h create mode 100644 gtsam/3rdparty/cephes/cephes/nbdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/ndtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/ndtri.c create mode 100644 gtsam/3rdparty/cephes/cephes/owens_t.c create mode 100644 gtsam/3rdparty/cephes/cephes/pdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/poch.c create mode 100644 gtsam/3rdparty/cephes/cephes/polevl.h create mode 100644 gtsam/3rdparty/cephes/cephes/psi.c create mode 100644 gtsam/3rdparty/cephes/cephes/rgamma.c create mode 100644 gtsam/3rdparty/cephes/cephes/round.c create mode 100644 gtsam/3rdparty/cephes/cephes/scipy_iv.c create mode 100644 gtsam/3rdparty/cephes/cephes/sf_error.c create mode 100644 gtsam/3rdparty/cephes/cephes/sf_error.h create mode 100644 gtsam/3rdparty/cephes/cephes/shichi.c create mode 100644 gtsam/3rdparty/cephes/cephes/sici.c create mode 100644 gtsam/3rdparty/cephes/cephes/sindg.c create mode 100644 gtsam/3rdparty/cephes/cephes/sinpi.c create mode 100644 gtsam/3rdparty/cephes/cephes/spence.c create mode 100644 gtsam/3rdparty/cephes/cephes/stdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/struve.c create mode 100644 gtsam/3rdparty/cephes/cephes/tandg.c create mode 100644 gtsam/3rdparty/cephes/cephes/tukey.c create mode 100644 gtsam/3rdparty/cephes/cephes/unity.c create mode 100644 gtsam/3rdparty/cephes/cephes/yn.c create mode 100644 gtsam/3rdparty/cephes/cephes/yv.c create mode 100644 gtsam/3rdparty/cephes/cephes/zeta.c create mode 100644 gtsam/3rdparty/cephes/cephes/zetac.c diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt new file mode 100644 index 0000000000..fdc17ea614 --- /dev/null +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -0,0 +1,102 @@ +cmake_minimum_required(VERSION 3.12) +enable_testing() +project( + cephes + DESCRIPTION "Cephes Mathematical Function Library" + VERSION 1.0.0 + LANGUAGES C) + +set(CEPHES_HEADER_FILES + cephes.h + cephes/cephes_names.h + cephes/dd_idefs.h + cephes/dd_real.h + cephes/dd_real_idefs.h + cephes/expn.h + cephes/igam.h + cephes/lanczos.h + cephes/mconf.h + cephes/polevl.h + cephes/sf_error.h) + +set(CEPHES_SOURCES + cephes/airy.c + cephes/bdtr.c + cephes/besselpoly.c + cephes/beta.c + cephes/btdtr.c + cephes/cbrt.c + cephes/chbevl.c + cephes/chdtr.c + cephes/const.c + cephes/dawsn.c + cephes/dd_real.c + cephes/ellie.c + cephes/ellik.c + cephes/ellpe.c + cephes/ellpj.c + cephes/ellpk.c + cephes/erfinv.c + cephes/exp10.c + cephes/exp2.c + cephes/expn.c + cephes/fdtr.c + cephes/fresnl.c + cephes/gamma.c + cephes/gammasgn.c + cephes/gdtr.c + cephes/hyp2f1.c + cephes/hyperg.c + cephes/i0.c + cephes/i1.c + cephes/igam.c + cephes/igami.c + cephes/incbet.c + cephes/incbi.c + cephes/j0.c + cephes/j1.c + cephes/jv.c + cephes/k0.c + cephes/k1.c + cephes/kn.c + cephes/kolmogorov.c + cephes/lanczos.c + cephes/nbdtr.c + cephes/ndtr.c + cephes/ndtri.c + cephes/owens_t.c + cephes/pdtr.c + cephes/poch.c + cephes/psi.c + cephes/rgamma.c + cephes/round.c + # cephes/scipy_iv.c + cephes/sf_error.c + cephes/shichi.c + cephes/sici.c + cephes/sindg.c + cephes/sinpi.c + cephes/spence.c + cephes/stdtr.c + cephes/tandg.c + cephes/tukey.c + cephes/unity.c + cephes/yn.c + cephes/yv.c + cephes/zeta.c + cephes/zetac.c) + +# Add library source files +add_library(${PROJECT_NAME} SHARED ${CEPHES_SOURCES}) + +# Add include directory (aka headers) +target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +set_target_properties( + ${PROJECT_NAME} + PROPERTIES VERSION ${PROJECT_VERSION} + SOVERSION ${PROJECT_VERSION_MAJOR} + PUBLIC_HEADER ${CEPHES_HEADER_FILES} + C_STANDARD 99) + +install(FILES ${CEPHES_HEADER_FILES} DESTINATION include/gtsam/3rdparty/cephes) diff --git a/gtsam/3rdparty/cephes/cephes.h b/gtsam/3rdparty/cephes/cephes.h new file mode 100644 index 0000000000..629733eef0 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes.h @@ -0,0 +1,163 @@ +#ifndef CEPHES_H +#define CEPHES_H + +#include "cephes/cephes_names.h" + +#ifdef __cplusplus +extern "C" { +#endif + +extern int airy(double x, double *ai, double *aip, double *bi, double *bip); + +extern double bdtrc(double k, int n, double p); +extern double bdtr(double k, int n, double p); +extern double bdtri(double k, int n, double y); + +extern double besselpoly(double a, double lambda, double nu); + +extern double beta(double a, double b); +extern double lbeta(double a, double b); + +extern double btdtr(double a, double b, double x); + +extern double cbrt(double x); +extern double chbevl(double x, double array[], int n); +extern double chdtrc(double df, double x); +extern double chdtr(double df, double x); +extern double chdtri(double df, double y); +extern double dawsn(double xx); + +extern double ellie(double phi, double m); +extern double ellik(double phi, double m); +extern double ellpe(double x); + +extern int ellpj(double u, double m, double *sn, double *cn, double *dn, double *ph); +extern double ellpk(double x); +extern double exp10(double x); +extern double exp2(double x); + +extern double expn(int n, double x); + +extern double fdtrc(double a, double b, double x); +extern double fdtr(double a, double b, double x); +extern double fdtri(double a, double b, double y); + +extern int fresnl(double xxa, double *ssa, double *cca); +extern double Gamma(double x); +extern double lgam(double x); +extern double lgam_sgn(double x, int *sign); +extern double gammasgn(double x); + +extern double gdtr(double a, double b, double x); +extern double gdtrc(double a, double b, double x); +extern double gdtri(double a, double b, double y); + +extern double hyp2f1(double a, double b, double c, double x); +extern double hyperg(double a, double b, double x); +extern double threef0(double a, double b, double c, double x, double *err); + +extern double i0(double x); +extern double i0e(double x); +extern double i1(double x); +extern double i1e(double x); +extern double igamc(double a, double x); +extern double igam(double a, double x); +extern double igam_fac(double a, double x); +extern double igamci(double a, double q); +extern double igami(double a, double p); + +extern double incbet(double aa, double bb, double xx); +extern double incbi(double aa, double bb, double yy0); + +extern double iv(double v, double x); +extern double j0(double x); +extern double y0(double x); +extern double j1(double x); +extern double y1(double x); + +extern double jn(int n, double x); +extern double jv(double n, double x); +extern double k0(double x); +extern double k0e(double x); +extern double k1(double x); +extern double k1e(double x); +extern double kn(int nn, double x); + +extern double nbdtrc(int k, int n, double p); +extern double nbdtr(int k, int n, double p); +extern double nbdtri(int k, int n, double p); + +extern double ndtr(double a); +extern double log_ndtr(double a); +extern double erfc(double a); +extern double erf(double x); +extern double erfinv(double y); +extern double erfcinv(double y); +extern double ndtri(double y0); + +extern double pdtrc(double k, double m); +extern double pdtr(double k, double m); +extern double pdtri(int k, double y); + +extern double poch(double x, double m); + +extern double psi(double x); + +extern double rgamma(double x); +extern double round(double x); + +extern int shichi(double x, double *si, double *ci); +extern int sici(double x, double *si, double *ci); + +extern double radian(double d, double m, double s); +extern double sindg(double x); +extern double sinpi(double x); +extern double cosdg(double x); +extern double cospi(double x); + +extern double spence(double x); + +extern double stdtr(int k, double t); +extern double stdtri(int k, double p); + +extern double struve_h(double v, double x); +extern double struve_l(double v, double x); +extern double struve_power_series(double v, double x, int is_h, double *err); +extern double struve_asymp_large_z(double v, double z, int is_h, double *err); +extern double struve_bessel_series(double v, double z, int is_h, double *err); + +extern double yv(double v, double x); + +extern double tandg(double x); +extern double cotdg(double x); + +extern double log1p(double x); +extern double log1pmx(double x); +extern double expm1(double x); +extern double cosm1(double x); +extern double lgam1p(double x); + +extern double yn(int n, double x); +extern double zeta(double x, double q); +extern double zetac(double x); + +extern double smirnov(int n, double d); +extern double smirnovi(int n, double p); +extern double smirnovp(int n, double d); +extern double smirnovc(int n, double d); +extern double smirnovci(int n, double p); +extern double kolmogorov(double x); +extern double kolmogi(double p); +extern double kolmogp(double x); +extern double kolmogc(double x); +extern double kolmogci(double p); + +extern double lanczos_sum_expg_scaled(double x); + +extern double owens_t(double h, double a); + +#ifdef __cplusplus +} +#endif + +#endif /* CEPHES_H */ diff --git a/gtsam/3rdparty/cephes/cephes/airy.c b/gtsam/3rdparty/cephes/cephes/airy.c new file mode 100644 index 0000000000..95e16a55f8 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/airy.c @@ -0,0 +1,376 @@ +/* airy.c + * + * Airy function + * + * + * + * SYNOPSIS: + * + * double x, ai, aip, bi, bip; + * int airy(); + * + * airy( x, _&ai, _&aip, _&bi, _&bip ); + * + * + * + * DESCRIPTION: + * + * Solution of the differential equation + * + * y"(x) = xy. + * + * The function returns the two independent solutions Ai, Bi + * and their first derivatives Ai'(x), Bi'(x). + * + * Evaluation is by power series summation for small x, + * by rational minimax approximations for large x. + * + * + * + * ACCURACY: + * Error criterion is absolute when function <= 1, relative + * when function > 1, except * denotes relative error criterion. + * For large negative x, the absolute error increases as x^1.5. + * For large positive x, the relative error increases as x^1.5. + * + * Arithmetic domain function # trials peak rms + * IEEE -10, 0 Ai 10000 1.6e-15 2.7e-16 + * IEEE 0, 10 Ai 10000 2.3e-14* 1.8e-15* + * IEEE -10, 0 Ai' 10000 4.6e-15 7.6e-16 + * IEEE 0, 10 Ai' 10000 1.8e-14* 1.5e-15* + * IEEE -10, 10 Bi 30000 4.2e-15 5.3e-16 + * IEEE -10, 10 Bi' 30000 4.9e-15 7.3e-16 + * + */ + /* airy.c */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +static double c1 = 0.35502805388781723926; +static double c2 = 0.258819403792806798405; +static double sqrt3 = 1.732050807568877293527; +static double sqpii = 5.64189583547756286948E-1; + +extern double MACHEP; + +#ifdef UNK +#define MAXAIRY 25.77 +#endif +#ifdef IBMPC +#define MAXAIRY 103.892 +#endif +#ifdef MIEEE +#define MAXAIRY 103.892 +#endif + + +static double AN[8] = { + 3.46538101525629032477E-1, + 1.20075952739645805542E1, + 7.62796053615234516538E1, + 1.68089224934630576269E2, + 1.59756391350164413639E2, + 7.05360906840444183113E1, + 1.40264691163389668864E1, + 9.99999999999999995305E-1, +}; + +static double AD[8] = { + 5.67594532638770212846E-1, + 1.47562562584847203173E1, + 8.45138970141474626562E1, + 1.77318088145400459522E2, + 1.64234692871529701831E2, + 7.14778400825575695274E1, + 1.40959135607834029598E1, + 1.00000000000000000470E0, +}; + +static double APN[8] = { + 6.13759184814035759225E-1, + 1.47454670787755323881E1, + 8.20584123476060982430E1, + 1.71184781360976385540E2, + 1.59317847137141783523E2, + 6.99778599330103016170E1, + 1.39470856980481566958E1, + 1.00000000000000000550E0, +}; + +static double APD[8] = { + 3.34203677749736953049E-1, + 1.11810297306158156705E1, + 7.11727352147859965283E1, + 1.58778084372838313640E2, + 1.53206427475809220834E2, + 6.86752304592780337944E1, + 1.38498634758259442477E1, + 9.99999999999999994502E-1, +}; + +static double BN16[5] = { + -2.53240795869364152689E-1, + 5.75285167332467384228E-1, + -3.29907036873225371650E-1, + 6.44404068948199951727E-2, + -3.82519546641336734394E-3, +}; + +static double BD16[5] = { + /* 1.00000000000000000000E0, */ + -7.15685095054035237902E0, + 1.06039580715664694291E1, + -5.23246636471251500874E0, + 9.57395864378383833152E-1, + -5.50828147163549611107E-2, +}; + +static double BPPN[5] = { + 4.65461162774651610328E-1, + -1.08992173800493920734E0, + 6.38800117371827987759E-1, + -1.26844349553102907034E-1, + 7.62487844342109852105E-3, +}; + +static double BPPD[5] = { + /* 1.00000000000000000000E0, */ + -8.70622787633159124240E0, + 1.38993162704553213172E1, + -7.14116144616431159572E0, + 1.34008595960680518666E0, + -7.84273211323341930448E-2, +}; + +static double AFN[9] = { + -1.31696323418331795333E-1, + -6.26456544431912369773E-1, + -6.93158036036933542233E-1, + -2.79779981545119124951E-1, + -4.91900132609500318020E-2, + -4.06265923594885404393E-3, + -1.59276496239262096340E-4, + -2.77649108155232920844E-6, + -1.67787698489114633780E-8, +}; + +static double AFD[9] = { + /* 1.00000000000000000000E0, */ + 1.33560420706553243746E1, + 3.26825032795224613948E1, + 2.67367040941499554804E1, + 9.18707402907259625840E0, + 1.47529146771666414581E0, + 1.15687173795188044134E-1, + 4.40291641615211203805E-3, + 7.54720348287414296618E-5, + 4.51850092970580378464E-7, +}; + +static double AGN[11] = { + 1.97339932091685679179E-2, + 3.91103029615688277255E-1, + 1.06579897599595591108E0, + 9.39169229816650230044E-1, + 3.51465656105547619242E-1, + 6.33888919628925490927E-2, + 5.85804113048388458567E-3, + 2.82851600836737019778E-4, + 6.98793669997260967291E-6, + 8.11789239554389293311E-8, + 3.41551784765923618484E-10, +}; + +static double AGD[10] = { + /* 1.00000000000000000000E0, */ + 9.30892908077441974853E0, + 1.98352928718312140417E1, + 1.55646628932864612953E1, + 5.47686069422975497931E0, + 9.54293611618961883998E-1, + 8.64580826352392193095E-2, + 4.12656523824222607191E-3, + 1.01259085116509135510E-4, + 1.17166733214413521882E-6, + 4.91834570062930015649E-9, +}; + +static double APFN[9] = { + 1.85365624022535566142E-1, + 8.86712188052584095637E-1, + 9.87391981747398547272E-1, + 4.01241082318003734092E-1, + 7.10304926289631174579E-2, + 5.90618657995661810071E-3, + 2.33051409401776799569E-4, + 4.08718778289035454598E-6, + 2.48379932900442457853E-8, +}; + +static double APFD[9] = { + /* 1.00000000000000000000E0, */ + 1.47345854687502542552E1, + 3.75423933435489594466E1, + 3.14657751203046424330E1, + 1.09969125207298778536E1, + 1.78885054766999417817E0, + 1.41733275753662636873E-1, + 5.44066067017226003627E-3, + 9.39421290654511171663E-5, + 5.65978713036027009243E-7, +}; + +static double APGN[11] = { + -3.55615429033082288335E-2, + -6.37311518129435504426E-1, + -1.70856738884312371053E0, + -1.50221872117316635393E0, + -5.63606665822102676611E-1, + -1.02101031120216891789E-1, + -9.48396695961445269093E-3, + -4.60325307486780994357E-4, + -1.14300836484517375919E-5, + -1.33415518685547420648E-7, + -5.63803833958893494476E-10, +}; + +static double APGD[11] = { + /* 1.00000000000000000000E0, */ + 9.85865801696130355144E0, + 2.16401867356585941885E1, + 1.73130776389749389525E1, + 6.17872175280828766327E0, + 1.08848694396321495475E0, + 9.95005543440888479402E-2, + 4.78468199683886610842E-3, + 1.18159633322838625562E-4, + 1.37480673554219441465E-6, + 5.79912514929147598821E-9, +}; + +int airy(double x, double *ai, double *aip, double *bi, double *bip) +{ + double z, zz, t, f, g, uf, ug, k, zeta, theta; + int domflg; + + domflg = 0; + if (x > MAXAIRY) { + *ai = 0; + *aip = 0; + *bi = INFINITY; + *bip = INFINITY; + return (-1); + } + + if (x < -2.09) { + domflg = 15; + t = sqrt(-x); + zeta = -2.0 * x * t / 3.0; + t = sqrt(t); + k = sqpii / t; + z = 1.0 / zeta; + zz = z * z; + uf = 1.0 + zz * polevl(zz, AFN, 8) / p1evl(zz, AFD, 9); + ug = z * polevl(zz, AGN, 10) / p1evl(zz, AGD, 10); + theta = zeta + 0.25 * M_PI; + f = sin(theta); + g = cos(theta); + *ai = k * (f * uf - g * ug); + *bi = k * (g * uf + f * ug); + uf = 1.0 + zz * polevl(zz, APFN, 8) / p1evl(zz, APFD, 9); + ug = z * polevl(zz, APGN, 10) / p1evl(zz, APGD, 10); + k = sqpii * t; + *aip = -k * (g * uf + f * ug); + *bip = k * (f * uf - g * ug); + return (0); + } + + if (x >= 2.09) { /* cbrt(9) */ + domflg = 5; + t = sqrt(x); + zeta = 2.0 * x * t / 3.0; + g = exp(zeta); + t = sqrt(t); + k = 2.0 * t * g; + z = 1.0 / zeta; + f = polevl(z, AN, 7) / polevl(z, AD, 7); + *ai = sqpii * f / k; + k = -0.5 * sqpii * t / g; + f = polevl(z, APN, 7) / polevl(z, APD, 7); + *aip = f * k; + + if (x > 8.3203353) { /* zeta > 16 */ + f = z * polevl(z, BN16, 4) / p1evl(z, BD16, 5); + k = sqpii * g; + *bi = k * (1.0 + f) / t; + f = z * polevl(z, BPPN, 4) / p1evl(z, BPPD, 5); + *bip = k * t * (1.0 + f); + return (0); + } + } + + f = 1.0; + g = x; + t = 1.0; + uf = 1.0; + ug = x; + k = 1.0; + z = x * x * x; + while (t > MACHEP) { + uf *= z; + k += 1.0; + uf /= k; + ug *= z; + k += 1.0; + ug /= k; + uf /= k; + f += uf; + k += 1.0; + ug /= k; + g += ug; + t = fabs(uf / f); + } + uf = c1 * f; + ug = c2 * g; + if ((domflg & 1) == 0) + *ai = uf - ug; + if ((domflg & 2) == 0) + *bi = sqrt3 * (uf + ug); + + /* the deriviative of ai */ + k = 4.0; + uf = x * x / 2.0; + ug = z / 3.0; + f = uf; + g = 1.0 + ug; + uf /= 3.0; + t = 1.0; + + while (t > MACHEP) { + uf *= z; + ug /= k; + k += 1.0; + ug *= z; + uf /= k; + f += uf; + k += 1.0; + ug /= k; + uf /= k; + g += ug; + k += 1.0; + t = fabs(ug / g); + } + + uf = c1 * f; + ug = c2 * g; + if ((domflg & 4) == 0) + *aip = uf - ug; + if ((domflg & 8) == 0) + *bip = sqrt3 * (uf + ug); + return (0); +} diff --git a/gtsam/3rdparty/cephes/cephes/bdtr.c b/gtsam/3rdparty/cephes/cephes/bdtr.c new file mode 100644 index 0000000000..29fcdf1aff --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/bdtr.c @@ -0,0 +1,241 @@ +/* bdtr.c + * + * Binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, bdtr(); + * + * y = bdtr( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms 0 through k of the Binomial + * probability density: + * + * k + * -- ( n ) j n-j + * > ( ) p (1-p) + * -- ( j ) + * j=0 + * + * The terms are not summed directly; instead the incomplete + * beta integral is employed, according to the formula + * + * y = bdtr( k, n, p ) = incbet( n-k, k+1, 1-p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * Tested at random points (a,b,p), with p between 0 and 1. + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * For p between 0.001 and 1: + * IEEE 0,100 100000 4.3e-15 2.6e-16 + * See also incbet.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * bdtr domain k < 0 0.0 + * n < k + * x < 0, x > 1 + */ +/* bdtrc() + * + * Complemented binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, bdtrc(); + * + * y = bdtrc( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms k+1 through n of the Binomial + * probability density: + * + * n + * -- ( n ) j n-j + * > ( ) p (1-p) + * -- ( j ) + * j=k+1 + * + * The terms are not summed directly; instead the incomplete + * beta integral is employed, according to the formula + * + * y = bdtrc( k, n, p ) = incbet( k+1, n-k, p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * Tested at random points (a,b,p). + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * For p between 0.001 and 1: + * IEEE 0,100 100000 6.7e-15 8.2e-16 + * For p between 0 and .001: + * IEEE 0,100 100000 1.5e-13 2.7e-15 + * + * ERROR MESSAGES: + * + * message condition value returned + * bdtrc domain x<0, x>1, n 1 + */ + +/* bdtr() */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +double bdtrc(double k, int n, double p) { + double dk, dn; + double fk = floor(k); + + if (isnan(p) || isnan(k)) { + return NAN; + } + + if (p < 0.0 || p > 1.0 || n < fk) { + sf_error("bdtrc", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (fk < 0) { + return 1.0; + } + + if (fk == n) { + return 0.0; + } + + dn = n - fk; + if (k == 0) { + if (p < .01) + dk = -expm1(dn * log1p(-p)); + else + dk = 1.0 - pow(1.0 - p, dn); + } else { + dk = fk + 1; + dk = incbet(dk, dn, p); + } + return dk; +} + +double bdtr(double k, int n, double p) { + double dk, dn; + double fk = floor(k); + + if (isnan(p) || isnan(k)) { + return NAN; + } + + if (p < 0.0 || p > 1.0 || fk < 0 || n < fk) { + sf_error("bdtr", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (fk == n) return 1.0; + + dn = n - fk; + if (fk == 0) { + dk = pow(1.0 - p, dn); + } else { + dk = fk + 1.; + dk = incbet(dn, dk, 1.0 - p); + } + return dk; +} + +double bdtri(double k, int n, double y) { + double p, dn, dk; + double fk = floor(k); + + if (isnan(k)) { + return NAN; + } + + if (y < 0.0 || y > 1.0 || fk < 0.0 || n <= fk) { + sf_error("bdtri", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + dn = n - fk; + + if (fk == n) return 1.0; + + if (fk == 0) { + if (y > 0.8) { + p = -expm1(log1p(y - 1.0) / dn); + } else { + p = 1.0 - pow(y, 1.0 / dn); + } + } else { + dk = fk + 1; + p = incbet(dn, dk, 0.5); + if (p > 0.5) + p = incbi(dk, dn, 1.0 - y); + else + p = 1.0 - incbi(dn, dk, y); + } + return p; +} diff --git a/gtsam/3rdparty/cephes/cephes/besselpoly.c b/gtsam/3rdparty/cephes/cephes/besselpoly.c new file mode 100644 index 0000000000..a58fe20376 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/besselpoly.c @@ -0,0 +1,34 @@ +#include "mconf.h" + +#define EPS 1.0e-17 + +double besselpoly(double a, double lambda, double nu) { + + int m, factor=0; + double Sm, relerr, Sol; + double sum=0.0; + + /* Special handling for a = 0.0 */ + if (a == 0.0) { + if (nu == 0.0) return 1.0/(lambda + 1); + else return 0.0; + } + /* Special handling for negative and integer nu */ + if ((nu < 0) && (floor(nu)==nu)) { + nu = -nu; + factor = ((int) nu) % 2; + } + Sm = exp(nu*log(a))/(Gamma(nu+1)*(lambda+nu+1)); + m = 0; + do { + sum += Sm; + Sol = Sm; + Sm *= -a*a*(lambda+nu+1+2*m)/((nu+m+1)*(m+1)*(lambda+nu+1+2*m+2)); + m++; + relerr = fabs((Sm-Sol)/Sm); + } while (relerr > EPS && m < 1000); + if (!factor) + return sum; + else + return -sum; +} diff --git a/gtsam/3rdparty/cephes/cephes/beta.c b/gtsam/3rdparty/cephes/cephes/beta.c new file mode 100644 index 0000000000..c0389deea0 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/beta.c @@ -0,0 +1,258 @@ +/* beta.c + * + * Beta function + * + * + * + * SYNOPSIS: + * + * double a, b, y, beta(); + * + * y = beta( a, b ); + * + * + * + * DESCRIPTION: + * + * - - + * | (a) | (b) + * beta( a, b ) = -----------. + * - + * | (a+b) + * + * For large arguments the logarithm of the function is + * evaluated using lgam(), then exponentiated. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 8.1e-14 1.1e-14 + * + * ERROR MESSAGES: + * + * message condition value returned + * beta overflow log(beta) > MAXLOG 0.0 + * a or b <0 integer 0.0 + * + */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +#define MAXGAM 171.624376956302725 + +extern double MAXLOG; + +#define ASYMP_FACTOR 1e6 + +static double lbeta_asymp(double a, double b, int *sgn); +static double lbeta_negint(int a, double b); +static double beta_negint(int a, double b); + +double beta(double a, double b) +{ + double y; + int sign = 1; + + if (a <= 0.0) { + if (a == floor(a)) { + if (a == (int)a) { + return beta_negint((int)a, b); + } + else { + goto overflow; + } + } + } + + if (b <= 0.0) { + if (b == floor(b)) { + if (b == (int)b) { + return beta_negint((int)b, a); + } + else { + goto overflow; + } + } + } + + if (fabs(a) < fabs(b)) { + y = a; a = b; b = y; + } + + if (fabs(a) > ASYMP_FACTOR * fabs(b) && a > ASYMP_FACTOR) { + /* Avoid loss of precision in lgam(a + b) - lgam(a) */ + y = lbeta_asymp(a, b, &sign); + return sign * exp(y); + } + + y = a + b; + if (fabs(y) > MAXGAM || fabs(a) > MAXGAM || fabs(b) > MAXGAM) { + int sgngam; + y = lgam_sgn(y, &sgngam); + sign *= sgngam; /* keep track of the sign */ + y = lgam_sgn(b, &sgngam) - y; + sign *= sgngam; + y = lgam_sgn(a, &sgngam) + y; + sign *= sgngam; + if (y > MAXLOG) { + goto overflow; + } + return (sign * exp(y)); + } + + y = Gamma(y); + a = Gamma(a); + b = Gamma(b); + if (y == 0.0) + goto overflow; + + if (fabs(fabs(a) - fabs(y)) > fabs(fabs(b) - fabs(y))) { + y = b / y; + y *= a; + } + else { + y = a / y; + y *= b; + } + + return (y); + +overflow: + sf_error("beta", SF_ERROR_OVERFLOW, NULL); + return (sign * INFINITY); +} + + +/* Natural log of |beta|. */ + +double lbeta(double a, double b) +{ + double y; + int sign; + + sign = 1; + + if (a <= 0.0) { + if (a == floor(a)) { + if (a == (int)a) { + return lbeta_negint((int)a, b); + } + else { + goto over; + } + } + } + + if (b <= 0.0) { + if (b == floor(b)) { + if (b == (int)b) { + return lbeta_negint((int)b, a); + } + else { + goto over; + } + } + } + + if (fabs(a) < fabs(b)) { + y = a; a = b; b = y; + } + + if (fabs(a) > ASYMP_FACTOR * fabs(b) && a > ASYMP_FACTOR) { + /* Avoid loss of precision in lgam(a + b) - lgam(a) */ + y = lbeta_asymp(a, b, &sign); + return y; + } + + y = a + b; + if (fabs(y) > MAXGAM || fabs(a) > MAXGAM || fabs(b) > MAXGAM) { + int sgngam; + y = lgam_sgn(y, &sgngam); + sign *= sgngam; /* keep track of the sign */ + y = lgam_sgn(b, &sgngam) - y; + sign *= sgngam; + y = lgam_sgn(a, &sgngam) + y; + sign *= sgngam; + return (y); + } + + y = Gamma(y); + a = Gamma(a); + b = Gamma(b); + if (y == 0.0) { + over: + sf_error("lbeta", SF_ERROR_OVERFLOW, NULL); + return (sign * INFINITY); + } + + if (fabs(fabs(a) - fabs(y)) > fabs(fabs(b) - fabs(y))) { + y = b / y; + y *= a; + } + else { + y = a / y; + y *= b; + } + + if (y < 0) { + y = -y; + } + + return (log(y)); +} + +/* + * Asymptotic expansion for ln(|B(a, b)|) for a > ASYMP_FACTOR*max(|b|, 1). + */ +static double lbeta_asymp(double a, double b, int *sgn) +{ + double r = lgam_sgn(b, sgn); + r -= b * log(a); + + r += b*(1-b)/(2*a); + r += b*(1-b)*(1-2*b)/(12*a*a); + r += - b*b*(1-b)*(1-b)/(12*a*a*a); + + return r; +} + + +/* + * Special case for a negative integer argument + */ + +static double beta_negint(int a, double b) +{ + int sgn; + if (b == (int)b && 1 - a - b > 0) { + sgn = ((int)b % 2 == 0) ? 1 : -1; + return sgn * beta(1 - a - b, b); + } + else { + sf_error("lbeta", SF_ERROR_OVERFLOW, NULL); + return INFINITY; + } +} + +static double lbeta_negint(int a, double b) +{ + double r; + if (b == (int)b && 1 - a - b > 0) { + r = lbeta(1 - a - b, b); + return r; + } + else { + sf_error("lbeta", SF_ERROR_OVERFLOW, NULL); + return INFINITY; + } +} diff --git a/gtsam/3rdparty/cephes/cephes/btdtr.c b/gtsam/3rdparty/cephes/cephes/btdtr.c new file mode 100644 index 0000000000..fa115c7b70 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/btdtr.c @@ -0,0 +1,59 @@ + +/* btdtr.c + * + * Beta distribution + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, btdtr(); + * + * y = btdtr( a, b, x ); + * + * + * + * DESCRIPTION: + * + * Returns the area from zero to x under the beta density + * function: + * + * + * x + * - - + * | (a+b) | | a-1 b-1 + * P(x) = ---------- | t (1-t) dt + * - - | | + * | (a) | (b) - + * 0 + * + * + * This function is identical to the incomplete beta + * integral function incbet(a, b, x). + * + * The complemented function is + * + * 1 - P(1-x) = incbet( b, a, x ); + * + * + * ACCURACY: + * + * See incbet.c. + * + */ + +/* btdtr() */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +double btdtr(double a, double b, double x) +{ + + return (incbet(a, b, x)); +} diff --git a/gtsam/3rdparty/cephes/cephes/cbrt.c b/gtsam/3rdparty/cephes/cephes/cbrt.c new file mode 100644 index 0000000000..a83c078341 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/cbrt.c @@ -0,0 +1,117 @@ +/* cbrt.c + * + * Cube root + * + * + * + * SYNOPSIS: + * + * double x, y, cbrt(); + * + * y = cbrt( x ); + * + * + * + * DESCRIPTION: + * + * Returns the cube root of the argument, which may be negative. + * + * Range reduction involves determining the power of 2 of + * the argument. A polynomial of degree 2 applied to the + * mantissa, and multiplication by the cube root of 1, 2, or 4 + * approximates the root to within about 0.1%. Then Newton's + * iteration is used three times to converge to an accurate + * result. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,1e308 30000 1.5e-16 5.0e-17 + * + */ + /* cbrt.c */ + +/* + * Cephes Math Library Release 2.2: January, 1991 + * Copyright 1984, 1991 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + + +#include "mconf.h" + +static double CBRT2 = 1.2599210498948731647672; +static double CBRT4 = 1.5874010519681994747517; +static double CBRT2I = 0.79370052598409973737585; +static double CBRT4I = 0.62996052494743658238361; + +double cbrt(double x) +{ + int e, rem, sign; + double z; + + if (!cephes_isfinite(x)) + return x; + if (x == 0) + return (x); + if (x > 0) + sign = 1; + else { + sign = -1; + x = -x; + } + + z = x; + /* extract power of 2, leaving + * mantissa between 0.5 and 1 + */ + x = frexp(x, &e); + + /* Approximate cube root of number between .5 and 1, + * peak relative error = 9.2e-6 + */ + x = (((-1.3466110473359520655053e-1 * x + + 5.4664601366395524503440e-1) * x + - 9.5438224771509446525043e-1) * x + + 1.1399983354717293273738e0) * x + 4.0238979564544752126924e-1; + + /* exponent divided by 3 */ + if (e >= 0) { + rem = e; + e /= 3; + rem -= 3 * e; + if (rem == 1) + x *= CBRT2; + else if (rem == 2) + x *= CBRT4; + } + + + /* argument less than 1 */ + + else { + e = -e; + rem = e; + e /= 3; + rem -= 3 * e; + if (rem == 1) + x *= CBRT2I; + else if (rem == 2) + x *= CBRT4I; + e = -e; + } + + /* multiply by power of 2 */ + x = ldexp(x, e); + + /* Newton iteration */ + x -= (x - (z / (x * x))) * 0.33333333333333333333; + x -= (x - (z / (x * x))) * 0.33333333333333333333; + + if (sign < 0) + x = -x; + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/cephes_names.h b/gtsam/3rdparty/cephes/cephes/cephes_names.h new file mode 100644 index 0000000000..5322feb384 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/cephes_names.h @@ -0,0 +1,114 @@ +#ifndef CEPHES_NAMES_H +#define CEPHES_NAMES_H + +#define airy cephes_airy +#define bdtrc cephes_bdtrc +#define bdtr cephes_bdtr +#define bdtri cephes_bdtri +#define besselpoly cephes_besselpoly +#define beta cephes_beta +#define lbeta cephes_lbeta +#define btdtr cephes_btdtr +#define cbrt cephes_cbrt +#define chdtrc cephes_chdtrc +#define chbevl cephes_chbevl +#define chdtr cephes_chdtr +#define chdtri cephes_chdtri +#define dawsn cephes_dawsn +#define ellie cephes_ellie +#define ellik cephes_ellik +#define ellpe cephes_ellpe +#define ellpj cephes_ellpj +#define ellpk cephes_ellpk +#define exp10 cephes_exp10 +#define exp2 cephes_exp2 +#define expn cephes_expn +#define fdtrc cephes_fdtrc +#define fdtr cephes_fdtr +#define fdtri cephes_fdtri +#define fresnl cephes_fresnl +#define Gamma cephes_Gamma +#define lgam cephes_lgam +#define lgam_sgn cephes_lgam_sgn +#define gammasgn cephes_gammasgn +#define gdtr cephes_gdtr +#define gdtrc cephes_gdtrc +#define gdtri cephes_gdtri +#define hyp2f1 cephes_hyp2f1 +#define hyperg cephes_hyperg +#define i0 cephes_i0 +#define i0e cephes_i0e +#define i1 cephes_i1 +#define i1e cephes_i1e +#define igamc cephes_igamc +#define igam cephes_igam +#define igami cephes_igami +#define incbet cephes_incbet +#define incbi cephes_incbi +#define iv cephes_iv +#define j0 cephes_j0 +#define y0 cephes_y0 +#define j1 cephes_j1 +#define y1 cephes_y1 +#define jn cephes_jn +#define jv cephes_jv +#define k0 cephes_k0 +#define k0e cephes_k0e +#define k1 cephes_k1 +#define k1e cephes_k1e +#define kn cephes_kn +#define nbdtrc cephes_nbdtrc +#define nbdtr cephes_nbdtr +#define nbdtri cephes_nbdtri +#define ndtr cephes_ndtr +#define erfc cephes_erfc +#define erf cephes_erf +#define erfinv cephes_erfinv +#define erfcinv cephes_erfcinv +#define ndtri cephes_ndtri +#define pdtrc cephes_pdtrc +#define pdtr cephes_pdtr +#define pdtri cephes_pdtri +#define poch cephes_poch +#define psi cephes_psi +#define rgamma cephes_rgamma +#define riemann_zeta cephes_riemann_zeta +// #define round cephes_round +#define shichi cephes_shichi +#define sici cephes_sici +#define radian cephes_radian +#define sindg cephes_sindg +#define sinpi cephes_sinpi +#define cosdg cephes_cosdg +#define cospi cephes_cospi +#define sincos cephes_sincos +#define spence cephes_spence +#define stdtr cephes_stdtr +#define stdtri cephes_stdtri +#define struve_h cephes_struve_h +#define struve_l cephes_struve_l +#define struve_power_series cephes_struve_power_series +#define struve_asymp_large_z cephes_struve_asymp_large_z +#define struve_bessel_series cephes_struve_bessel_series +#define yv cephes_yv +#define tandg cephes_tandg +#define cotdg cephes_cotdg +#define log1p cephes_log1p +#define expm1 cephes_expm1 +#define cosm1 cephes_cosm1 +#define yn cephes_yn +#define zeta cephes_zeta +#define zetac cephes_zetac +#define smirnov cephes_smirnov +#define smirnovc cephes_smirnovc +#define smirnovi cephes_smirnovi +#define smirnovci cephes_smirnovci +#define smirnovp cephes_smirnovp +#define kolmogorov cephes_kolmogorov +#define kolmogi cephes_kolmogi +#define kolmogp cephes_kolmogp +#define kolmogc cephes_kolmogc +#define kolmogci cephes_kolmogci +#define owens_t cephes_owens_t + +#endif diff --git a/gtsam/3rdparty/cephes/cephes/chbevl.c b/gtsam/3rdparty/cephes/cephes/chbevl.c new file mode 100644 index 0000000000..a0e9c5c52a --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/chbevl.c @@ -0,0 +1,81 @@ +/* chbevl.c + * + * Evaluate Chebyshev series + * + * + * + * SYNOPSIS: + * + * int N; + * double x, y, coef[N], chebevl(); + * + * y = chbevl( x, coef, N ); + * + * + * + * DESCRIPTION: + * + * Evaluates the series + * + * N-1 + * - ' + * y = > coef[i] T (x/2) + * - i + * i=0 + * + * of Chebyshev polynomials Ti at argument x/2. + * + * Coefficients are stored in reverse order, i.e. the zero + * order term is last in the array. Note N is the number of + * coefficients, not the order. + * + * If coefficients are for the interval a to b, x must + * have been transformed to x -> 2(2x - b - a)/(b-a) before + * entering the routine. This maps x from (a, b) to (-1, 1), + * over which the Chebyshev polynomials are defined. + * + * If the coefficients are for the inverted interval, in + * which (a, b) is mapped to (1/b, 1/a), the transformation + * required is x -> 2(2ab/x - b - a)/(b-a). If b is infinity, + * this becomes x -> 4a/x - 1. + * + * + * + * SPEED: + * + * Taking advantage of the recurrence properties of the + * Chebyshev polynomials, the routine requires one more + * addition per loop than evaluating a nested polynomial of + * the same degree. + * + */ + /* chbevl.c */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1985, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" +#include + +double chbevl(double x, double array[], int n) +{ + double b0, b1, b2, *p; + int i; + + p = array; + b0 = *p++; + b1 = 0.0; + i = n - 1; + + do { + b2 = b1; + b1 = b0; + b0 = x * b1 - b2 + *p++; + } + while (--i); + + return (0.5 * (b0 - b2)); +} diff --git a/gtsam/3rdparty/cephes/cephes/chdtr.c b/gtsam/3rdparty/cephes/cephes/chdtr.c new file mode 100644 index 0000000000..d576e7a8db --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/chdtr.c @@ -0,0 +1,186 @@ +/* chdtr.c + * + * Chi-square distribution + * + * + * + * SYNOPSIS: + * + * double df, x, y, chdtr(); + * + * y = chdtr( df, x ); + * + * + * + * DESCRIPTION: + * + * Returns the area under the left hand tail (from 0 to x) + * of the Chi square probability density function with + * v degrees of freedom. + * + * + * inf. + * - + * 1 | | v/2-1 -t/2 + * P( x | v ) = ----------- | t e dt + * v/2 - | | + * 2 | (v/2) - + * x + * + * where x is the Chi-square variable. + * + * The incomplete Gamma integral is used, according to the + * formula + * + * y = chdtr( v, x ) = igam( v/2.0, x/2.0 ). + * + * + * The arguments must both be positive. + * + * + * + * ACCURACY: + * + * See igam(). + * + * ERROR MESSAGES: + * + * message condition value returned + * chdtr domain x < 0 or v < 1 0.0 + */ + /* chdtrc() + * + * Complemented Chi-square distribution + * + * + * + * SYNOPSIS: + * + * double v, x, y, chdtrc(); + * + * y = chdtrc( v, x ); + * + * + * + * DESCRIPTION: + * + * Returns the area under the right hand tail (from x to + * infinity) of the Chi square probability density function + * with v degrees of freedom: + * + * + * inf. + * - + * 1 | | v/2-1 -t/2 + * P( x | v ) = ----------- | t e dt + * v/2 - | | + * 2 | (v/2) - + * x + * + * where x is the Chi-square variable. + * + * The incomplete Gamma integral is used, according to the + * formula + * + * y = chdtr( v, x ) = igamc( v/2.0, x/2.0 ). + * + * + * The arguments must both be positive. + * + * + * + * ACCURACY: + * + * See igamc(). + * + * ERROR MESSAGES: + * + * message condition value returned + * chdtrc domain x < 0 or v < 1 0.0 + */ + /* chdtri() + * + * Inverse of complemented Chi-square distribution + * + * + * + * SYNOPSIS: + * + * double df, x, y, chdtri(); + * + * x = chdtri( df, y ); + * + * + * + * + * DESCRIPTION: + * + * Finds the Chi-square argument x such that the integral + * from x to infinity of the Chi-square density is equal + * to the given cumulative probability y. + * + * This is accomplished using the inverse Gamma integral + * function and the relation + * + * x/2 = igamci( df/2, y ); + * + * + * + * + * ACCURACY: + * + * See igami.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * chdtri domain y < 0 or y > 1 0.0 + * v < 1 + * + */ + +/* chdtr() */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +double chdtrc(double df, double x) +{ + + if (x < 0.0) + return 1.0; /* modified by T. Oliphant */ + return (igamc(df / 2.0, x / 2.0)); +} + + + +double chdtr(double df, double x) +{ + + if ((x < 0.0)) { /* || (df < 1.0) ) */ + sf_error("chdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + return (igam(df / 2.0, x / 2.0)); +} + + + +double chdtri(double df, double y) +{ + double x; + + if ((y < 0.0) || (y > 1.0)) { /* || (df < 1.0) ) */ + sf_error("chdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + x = igamci(0.5 * df, y); + return (2.0 * x); +} diff --git a/gtsam/3rdparty/cephes/cephes/const.c b/gtsam/3rdparty/cephes/cephes/const.c new file mode 100644 index 0000000000..8631554cca --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/const.c @@ -0,0 +1,129 @@ +/* const.c + * + * Globally declared constants + * + * + * + * SYNOPSIS: + * + * extern double nameofconstant; + * + * + * + * + * DESCRIPTION: + * + * This file contains a number of mathematical constants and + * also some needed size parameters of the computer arithmetic. + * The values are supplied as arrays of hexadecimal integers + * for IEEE arithmetic, and in a normal decimal scientific notation for + * other machines. The particular notation used is determined + * by a symbol (IBMPC, or UNK) defined in the include file + * mconf.h. + * + * The default size parameters are as follows. + * + * For UNK mode: + * MACHEP = 1.38777878078144567553E-17 2**-56 + * MAXLOG = 8.8029691931113054295988E1 log(2**127) + * MINLOG = -8.872283911167299960540E1 log(2**-128) + * + * For IEEE arithmetic (IBMPC): + * MACHEP = 1.11022302462515654042E-16 2**-53 + * MAXLOG = 7.09782712893383996843E2 log(2**1024) + * MINLOG = -7.08396418532264106224E2 log(2**-1022) + * + * The global symbols for mathematical constants are + * SQ2OPI = 7.9788456080286535587989E-1 sqrt( 2/pi ) + * LOGSQ2 = 3.46573590279972654709E-1 log(2)/2 + * THPIO4 = 2.35619449019234492885 3*pi/4 + * + * These lists are subject to change. + */ + +/* const.c */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +#ifdef UNK +double MACHEP = 1.11022302462515654042E-16; /* 2**-53 */ + +#ifdef DENORMAL +double MAXLOG = 7.09782712893383996732E2; /* log(DBL_MAX) */ + + /* double MINLOG = -7.44440071921381262314E2; *//* log(2**-1074) */ +double MINLOG = -7.451332191019412076235E2; /* log(2**-1075) */ +#else +double MAXLOG = 7.08396418532264106224E2; /* log 2**1022 */ +double MINLOG = -7.08396418532264106224E2; /* log 2**-1022 */ +#endif +double SQ2OPI = 7.9788456080286535587989E-1; /* sqrt( 2/pi ) */ +double LOGSQ2 = 3.46573590279972654709E-1; /* log(2)/2 */ +double THPIO4 = 2.35619449019234492885; /* 3*pi/4 */ + +#endif + +#ifdef IBMPC + /* 2**-53 = 1.11022302462515654042E-16 */ +unsigned short MACHEP[4] = { 0x0000, 0x0000, 0x0000, 0x3ca0 }; + +#ifdef DENORMAL + /* log(DBL_MAX) = 7.09782712893383996732224E2 */ +unsigned short MAXLOG[4] = { 0x39ef, 0xfefa, 0x2e42, 0x4086 }; + + /* log(2**-1074) = - -7.44440071921381262314E2 */ +/*unsigned short MINLOG[4] = {0x71c3,0x446d,0x4385,0xc087}; */ +unsigned short MINLOG[4] = { 0x3052, 0xd52d, 0x4910, 0xc087 }; +#else + /* log(2**1022) = 7.08396418532264106224E2 */ +unsigned short MAXLOG[4] = { 0xbcd2, 0xdd7a, 0x232b, 0x4086 }; + + /* log(2**-1022) = - 7.08396418532264106224E2 */ +unsigned short MINLOG[4] = { 0xbcd2, 0xdd7a, 0x232b, 0xc086 }; +#endif + /* 2**1024*(1-MACHEP) = 1.7976931348623158E308 */ +unsigned short SQ2OPI[4] = { 0x3651, 0x33d4, 0x8845, 0x3fe9 }; +unsigned short LOGSQ2[4] = { 0x39ef, 0xfefa, 0x2e42, 0x3fd6 }; +unsigned short THPIO4[4] = { 0x21d2, 0x7f33, 0xd97c, 0x4002 }; + +#endif + +#ifdef MIEEE + /* 2**-53 = 1.11022302462515654042E-16 */ +unsigned short MACHEP[4] = { 0x3ca0, 0x0000, 0x0000, 0x0000 }; + +#ifdef DENORMAL + /* log(2**1024) = 7.09782712893383996843E2 */ +unsigned short MAXLOG[4] = { 0x4086, 0x2e42, 0xfefa, 0x39ef }; + + /* log(2**-1074) = - -7.44440071921381262314E2 */ +/* unsigned short MINLOG[4] = {0xc087,0x4385,0x446d,0x71c3}; */ +unsigned short MINLOG[4] = { 0xc087, 0x4910, 0xd52d, 0x3052 }; +#else + /* log(2**1022) = 7.08396418532264106224E2 */ +unsigned short MAXLOG[4] = { 0x4086, 0x232b, 0xdd7a, 0xbcd2 }; + + /* log(2**-1022) = - 7.08396418532264106224E2 */ +unsigned short MINLOG[4] = { 0xc086, 0x232b, 0xdd7a, 0xbcd2 }; +#endif + /* 2**1024*(1-MACHEP) = 1.7976931348623158E308 */ +unsigned short SQ2OPI[4] = { 0x3fe9, 0x8845, 0x33d4, 0x3651 }; +unsigned short LOGSQ2[4] = { 0x3fd6, 0x2e42, 0xfefa, 0x39ef }; +unsigned short THPIO4[4] = { 0x4002, 0xd97c, 0x7f33, 0x21d2 }; + +#endif + +#ifndef UNK +extern unsigned short MACHEP[]; +extern unsigned short MAXLOG[]; +extern unsigned short UNDLOG[]; +extern unsigned short MINLOG[]; +extern unsigned short SQ2OPI[]; +extern unsigned short LOGSQ2[]; +extern unsigned short THPIO4[]; +#endif diff --git a/gtsam/3rdparty/cephes/cephes/dawsn.c b/gtsam/3rdparty/cephes/cephes/dawsn.c new file mode 100644 index 0000000000..7049f191ed --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dawsn.c @@ -0,0 +1,160 @@ +/* dawsn.c + * + * Dawson's Integral + * + * + * + * SYNOPSIS: + * + * double x, y, dawsn(); + * + * y = dawsn( x ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * x + * - + * 2 | | 2 + * dawsn(x) = exp( -x ) | exp( t ) dt + * | | + * - + * 0 + * + * Three different rational approximations are employed, for + * the intervals 0 to 3.25; 3.25 to 6.25; and 6.25 up. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,10 10000 6.9e-16 1.0e-16 + * + * + */ + +/* dawsn.c */ + + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" +/* Dawson's integral, interval 0 to 3.25 */ +static double AN[10] = { + 1.13681498971755972054E-11, + 8.49262267667473811108E-10, + 1.94434204175553054283E-8, + 9.53151741254484363489E-7, + 3.07828309874913200438E-6, + 3.52513368520288738649E-4, + -8.50149846724410912031E-4, + 4.22618223005546594270E-2, + -9.17480371773452345351E-2, + 9.99999999999999994612E-1, +}; + +static double AD[11] = { + 2.40372073066762605484E-11, + 1.48864681368493396752E-9, + 5.21265281010541664570E-8, + 1.27258478273186970203E-6, + 2.32490249820789513991E-5, + 3.25524741826057911661E-4, + 3.48805814657162590916E-3, + 2.79448531198828973716E-2, + 1.58874241960120565368E-1, + 5.74918629489320327824E-1, + 1.00000000000000000539E0, +}; + +/* interval 3.25 to 6.25 */ +static double BN[11] = { + 5.08955156417900903354E-1, + -2.44754418142697847934E-1, + 9.41512335303534411857E-2, + -2.18711255142039025206E-2, + 3.66207612329569181322E-3, + -4.23209114460388756528E-4, + 3.59641304793896631888E-5, + -2.14640351719968974225E-6, + 9.10010780076391431042E-8, + -2.40274520828250956942E-9, + 3.59233385440928410398E-11, +}; + +static double BD[10] = { + /* 1.00000000000000000000E0, */ + -6.31839869873368190192E-1, + 2.36706788228248691528E-1, + -5.31806367003223277662E-2, + 8.48041718586295374409E-3, + -9.47996768486665330168E-4, + 7.81025592944552338085E-5, + -4.55875153252442634831E-6, + 1.89100358111421846170E-7, + -4.91324691331920606875E-9, + 7.18466403235734541950E-11, +}; + +/* 6.25 to infinity */ +static double CN[5] = { + -5.90592860534773254987E-1, + 6.29235242724368800674E-1, + -1.72858975380388136411E-1, + 1.64837047825189632310E-2, + -4.86827613020462700845E-4, +}; + +static double CD[5] = { + /* 1.00000000000000000000E0, */ + -2.69820057197544900361E0, + 1.73270799045947845857E0, + -3.93708582281939493482E-1, + 3.44278924041233391079E-2, + -9.73655226040941223894E-4, +}; + +extern double MACHEP; + +double dawsn(double xx) +{ + double x, y; + int sign; + + + sign = 1; + if (xx < 0.0) { + sign = -1; + xx = -xx; + } + + if (xx < 3.25) { + x = xx * xx; + y = xx * polevl(x, AN, 9) / polevl(x, AD, 10); + return (sign * y); + } + + + x = 1.0 / (xx * xx); + + if (xx < 6.25) { + y = 1.0 / xx + x * polevl(x, BN, 10) / (p1evl(x, BD, 10) * xx); + return (sign * 0.5 * y); + } + + + if (xx > 1.0e9) + return ((sign * 0.5) / xx); + + /* 6.25 to infinity */ + y = 1.0 / xx + x * polevl(x, CN, 4) / (p1evl(x, CD, 5) * xx); + return (sign * 0.5 * y); +} diff --git a/gtsam/3rdparty/cephes/cephes/dd_idefs.h b/gtsam/3rdparty/cephes/cephes/dd_idefs.h new file mode 100644 index 0000000000..fec97c4780 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dd_idefs.h @@ -0,0 +1,198 @@ +/* + * include/dd_inline.h + * + * This work was supported by the Director, Office of Science, Division + * of Mathematical, Information, and Computational Sciences of the + * U.S. Department of Energy under contract numbers DE-AC03-76SF00098 and + * DE-AC02-05CH11231. + * + * Copyright (c) 2003-2009, The Regents of the University of California, + * through Lawrence Berkeley National Laboratory (subject to receipt of + * any required approvals from U.S. Dept. of Energy) All rights reserved. + * + * By downloading or using this software you are agreeing to the modified + * BSD license "BSD-LBNL-License.doc" (see LICENSE.txt). + */ +/* + * Contains small functions (suitable for inlining) in the double-double + * arithmetic package. + */ + +#ifndef _DD_IDEFS_H_ +#define _DD_IDEFS_H_ 1 + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define _DD_SPLITTER 134217729.0 // = 2^27 + 1 +#define _DD_SPLIT_THRESH 6.69692879491417e+299 // = 2^996 + +/* + ************************************************************************ + The basic routines taking double arguments, returning 1 (or 2) doubles + ************************************************************************ +*/ + +/* Computes fl(a+b) and err(a+b). Assumes |a| >= |b|. */ +static inline double +quick_two_sum(double a, double b, double *err) +{ + volatile double s = a + b; + volatile double c = s - a; + *err = b - c; + return s; +} + +/* Computes fl(a-b) and err(a-b). Assumes |a| >= |b| */ +static inline double +quick_two_diff(double a, double b, double *err) +{ + volatile double s = a - b; + volatile double c = a - s; + *err = c - b; + return s; +} + +/* Computes fl(a+b) and err(a+b). */ +static inline double +two_sum(double a, double b, double *err) +{ + volatile double s = a + b; + volatile double c = s - a; + volatile double d = b - c; + volatile double e = s - c; + *err = (a - e) + d; + return s; +} + +/* Computes fl(a-b) and err(a-b). */ +static inline double +two_diff(double a, double b, double *err) +{ + volatile double s = a - b; + volatile double c = s - a; + volatile double d = b + c; + volatile double e = s - c; + *err = (a - e) - d; + return s; +} + +/* Computes high word and lo word of a */ +static inline void +two_split(double a, double *hi, double *lo) +{ + volatile double temp, tempma; + if (a > _DD_SPLIT_THRESH || a < -_DD_SPLIT_THRESH) { + a *= 3.7252902984619140625e-09; // 2^-28 + temp = _DD_SPLITTER * a; + tempma = temp - a; + *hi = temp - tempma; + *lo = a - *hi; + *hi *= 268435456.0; // 2^28 + *lo *= 268435456.0; // 2^28 + } + else { + temp = _DD_SPLITTER * a; + tempma = temp - a; + *hi = temp - tempma; + *lo = a - *hi; + } +} + +/* Computes fl(a*b) and err(a*b). */ +static inline double +two_prod(double a, double b, double *err) +{ +#ifdef DD_FMS + volatile double p = a * b; + *err = DD_FMS(a, b, p); + return p; +#else + double a_hi, a_lo, b_hi, b_lo; + double p = a * b; + volatile double c, d; + two_split(a, &a_hi, &a_lo); + two_split(b, &b_hi, &b_lo); + c = a_hi * b_hi - p; + d = c + a_hi * b_lo + a_lo * b_hi; + *err = d + a_lo * b_lo; + return p; +#endif /* DD_FMA */ +} + +/* Computes fl(a*a) and err(a*a). Faster than the above method. */ +static inline double +two_sqr(double a, double *err) +{ +#ifdef DD_FMS + volatile double p = a * a; + *err = DD_FMS(a, a, p); + return p; +#else + double hi, lo; + volatile double c; + double q = a * a; + two_split(a, &hi, &lo); + c = hi * hi - q; + *err = (c + 2.0 * hi * lo) + lo * lo; + return q; +#endif /* DD_FMS */ +} + +static inline double +two_div(double a, double b, double *err) +{ + volatile double q1, q2; + double p1, p2; + double s, e; + + q1 = a / b; + + /* Compute a - q1 * b */ + p1 = two_prod(q1, b, &p2); + s = two_diff(a, p1, &e); + e -= p2; + + /* get next approximation */ + q2 = (s + e) / b; + + return quick_two_sum(q1, q2, err); +} + +/* Computes the nearest integer to d. */ +static inline double +two_nint(double d) +{ + if (d == floor(d)) { + return d; + } + return floor(d + 0.5); +} + +/* Computes the truncated integer. */ +static inline double +two_aint(double d) +{ + return (d >= 0.0 ? floor(d) : ceil(d)); +} + + +/* Compare a and b */ +static inline int +two_comp(const double a, const double b) +{ + /* Works for non-NAN inputs */ + return (a < b ? -1 : (a > b ? 1 : 0)); +} + + +#ifdef __cplusplus +} +#endif + +#endif /* _DD_IDEFS_H_ */ diff --git a/gtsam/3rdparty/cephes/cephes/dd_real.c b/gtsam/3rdparty/cephes/cephes/dd_real.c new file mode 100644 index 0000000000..c37f57a7b9 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dd_real.c @@ -0,0 +1,587 @@ +/* + * src/double2.cc + * + * This work was supported by the Director, Office of Science, Division + * of Mathematical, Information, and Computational Sciences of the + * U.S. Department of Energy under contract numbers DE-AC03-76SF00098 and + * DE-AC02-05CH11231. + * + * Copyright (c) 2003-2009, The Regents of the University of California, + * through Lawrence Berkeley National Laboratory (subject to receipt of + * any required approvals from U.S. Dept. of Energy) All rights reserved. + * + * By downloading or using this software you are agreeing to the modified + * BSD license "BSD-LBNL-License.doc" (see LICENSE.txt). + */ +/* + * Contains implementation of non-inlined functions of double-double + * package. Inlined functions are found in dd_real_inline.h. + */ + +/* + * This code taken from v2.3.18 of the qd package. +*/ + + +#include +#include +#include +#include + +#include "dd_real.h" + +#define _DD_REAL_INIT(A, B) {{A, B}} + +const double DD_C_EPS = 4.93038065763132e-32; // 2^-104 +const double DD_C_MIN_NORMALIZED = 2.0041683600089728e-292; // = 2^(-1022 + 53) + +/* Compile-time initialization of const double2 structs */ + +const double2 DD_C_MAX = + _DD_REAL_INIT(1.79769313486231570815e+308, 9.97920154767359795037e+291); +const double2 DD_C_SAFE_MAX = + _DD_REAL_INIT(1.7976931080746007281e+308, 9.97920154767359795037e+291); +const int _DD_C_NDIGITS = 31; + +const double2 DD_C_ZERO = _DD_REAL_INIT(0.0, 0.0); +const double2 DD_C_ONE = _DD_REAL_INIT(1.0, 0.0); +const double2 DD_C_NEGONE = _DD_REAL_INIT(-1.0, 0.0); + +const double2 DD_C_2PI = + _DD_REAL_INIT(6.283185307179586232e+00, 2.449293598294706414e-16); +const double2 DD_C_PI = + _DD_REAL_INIT(3.141592653589793116e+00, 1.224646799147353207e-16); +const double2 DD_C_PI2 = + _DD_REAL_INIT(1.570796326794896558e+00, 6.123233995736766036e-17); +const double2 DD_C_PI4 = + _DD_REAL_INIT(7.853981633974482790e-01, 3.061616997868383018e-17); +const double2 DD_C_PI16 = + _DD_REAL_INIT(1.963495408493620697e-01, 7.654042494670957545e-18); +const double2 DD_C_3PI4 = + _DD_REAL_INIT(2.356194490192344837e+00, 9.1848509936051484375e-17); + +const double2 DD_C_E = + _DD_REAL_INIT(2.718281828459045091e+00, 1.445646891729250158e-16); +const double2 DD_C_LOG2 = + _DD_REAL_INIT(6.931471805599452862e-01, 2.319046813846299558e-17); +const double2 DD_C_LOG10 = + _DD_REAL_INIT(2.302585092994045901e+00, -2.170756223382249351e-16); + +#ifdef DD_C_NAN_IS_CONST +const double2 DD_C_NAN = _DD_REAL_INIT(NAN, NAN); +const double2 DD_C_INF = _DD_REAL_INIT(INFINITY, INFINITY); +const double2 DD_C_NEGINF = _DD_REAL_INIT(-INFINITY, -INFINITY); +#endif /* NAN */ + + +/* This routine is called whenever a fatal error occurs. */ +static volatile int errCount = 0; +void +dd_error(const char *msg) +{ + errCount++; + /* if (msg) { */ + /* fprintf(stderr, "ERROR %s\n", msg); */ + /* } */ +} + + +int +get_double_expn(double x) +{ + int i = 0; + double y; + if (x == 0.0) { + return INT_MIN; + } + if (isinf(x) || isnan(x)) { + return INT_MAX; + } + + y = fabs(x); + if (y < 1.0) { + while (y < 1.0) { + y *= 2.0; + i++; + } + return -i; + } else if (y >= 2.0) { + while (y >= 2.0) { + y *= 0.5; + i++; + } + return i; + } + return 0; +} + +/* ######################################################################## */ +/* # Exponentiation */ +/* ######################################################################## */ + +/* Computes the square root of the double-double number dd. + NOTE: dd must be a non-negative number. */ + +double2 +dd_sqrt(const double2 a) +{ + /* Strategy: Use Karp's trick: if x is an approximation + to sqrt(a), then + + sqrt(a) = a*x + [a - (a*x)^2] * x / 2 (approx) + + The approximation is accurate to twice the accuracy of x. + Also, the multiplication (a*x) and [-]*x can be done with + only half the precision. + */ + double x, ax; + + if (dd_is_zero(a)) + return DD_C_ZERO; + + if (dd_is_negative(a)) { + dd_error("(dd_sqrt): Negative argument."); + return DD_C_NAN; + } + + x = 1.0 / sqrt(a.x[0]); + ax = a.x[0] * x; + return dd_add_d_d(ax, dd_sub(a, dd_sqr_d(ax)).x[0] * (x * 0.5)); +} + +/* Computes the square root of a double in double-double precision. + NOTE: d must not be negative. */ + +double2 +dd_sqrt_d(double d) +{ + return dd_sqrt(dd_create_d(d)); +} + +/* Computes the n-th root of the double-double number a. + NOTE: n must be a positive integer. + NOTE: If n is even, then a must not be negative. */ + +double2 +dd_nroot(const double2 a, int n) +{ + /* Strategy: Use Newton iteration for the function + + f(x) = x^(-n) - a + + to find its root a^{-1/n}. The iteration is thus + + x' = x + x * (1 - a * x^n) / n + + which converges quadratically. We can then find + a^{1/n} by taking the reciprocal. + */ + double2 r, x; + + if (n <= 0) { + dd_error("(dd_nroot): N must be positive."); + return DD_C_NAN; + } + + if (n % 2 == 0 && dd_is_negative(a)) { + dd_error("(dd_nroot): Negative argument."); + return DD_C_NAN; + } + + if (n == 1) { + return a; + } + if (n == 2) { + return dd_sqrt(a); + } + + if (dd_is_zero(a)) + return DD_C_ZERO; + + /* Note a^{-1/n} = exp(-log(a)/n) */ + r = dd_abs(a); + x = dd_create_d(exp(-log(r.x[0]) / n)); + + /* Perform Newton's iteration. */ + x = dd_add( + x, dd_mul(x, dd_sub_d_dd(1.0, dd_div_dd_d(dd_mul(r, dd_npwr(x, n)), + DD_STATIC_CAST(double, n))))); + if (a.x[0] < 0.0) { + x = dd_neg(x); + } + return dd_inv(x); +} + +/* Computes the n-th power of a double-double number. + NOTE: 0^0 causes an error. */ + +double2 +dd_npwr(const double2 a, int n) +{ + double2 r = a; + double2 s = DD_C_ONE; + int N = abs(n); + if (N == 0) { + if (dd_is_zero(a)) { + dd_error("(dd_npwr): Invalid argument."); + return DD_C_NAN; + } + return DD_C_ONE; + } + + if (N > 1) { + /* Use binary exponentiation */ + while (N > 0) { + if (N % 2 == 1) { + s = dd_mul(s, r); + } + N /= 2; + if (N > 0) { + r = dd_sqr(r); + } + } + } + else { + s = r; + } + + /* Compute the reciprocal if n is negative. */ + if (n < 0) { + return dd_inv(s); + } + + return s; +} + +double2 +dd_npow(const double2 a, int n) +{ + return dd_npwr(a, n); +} + +double2 +dd_pow(const double2 a, const double2 b) +{ + return dd_exp(dd_mul(b, dd_log(a))); +} + +/* ######################################################################## */ +/* # Exp/Log functions */ +/* ######################################################################## */ + +static const double2 inv_fact[] = { + {{1.66666666666666657e-01, 9.25185853854297066e-18}}, + {{4.16666666666666644e-02, 2.31296463463574266e-18}}, + {{8.33333333333333322e-03, 1.15648231731787138e-19}}, + {{1.38888888888888894e-03, -5.30054395437357706e-20}}, + {{1.98412698412698413e-04, 1.72095582934207053e-22}}, + {{2.48015873015873016e-05, 2.15119478667758816e-23}}, + {{2.75573192239858925e-06, -1.85839327404647208e-22}}, + {{2.75573192239858883e-07, 2.37677146222502973e-23}}, + {{2.50521083854417202e-08, -1.44881407093591197e-24}}, + {{2.08767569878681002e-09, -1.20734505911325997e-25}}, + {{1.60590438368216133e-10, 1.25852945887520981e-26}}, + {{1.14707455977297245e-11, 2.06555127528307454e-28}}, + {{7.64716373181981641e-13, 7.03872877733453001e-30}}, + {{4.77947733238738525e-14, 4.39920548583408126e-31}}, + {{2.81145725434552060e-15, 1.65088427308614326e-31}} +}; +//static const int n_inv_fact = sizeof(inv_fact) / sizeof(inv_fact[0]); + +/* Exponential. Computes exp(x) in double-double precision. */ + +double2 +dd_exp(const double2 a) +{ + /* Strategy: We first reduce the size of x by noting that + + exp(kr + m * log(2)) = 2^m * exp(r)^k + + where m and k are integers. By choosing m appropriately + we can make |kr| <= log(2) / 2 = 0.347. Then exp(r) is + evaluated using the familiar Taylor series. Reducing the + argument substantially speeds up the convergence. */ + + const double k = 512.0; + const double inv_k = 1.0 / k; + double m; + double2 r, s, t, p; + int i = 0; + + if (a.x[0] <= -709.0) { + return DD_C_ZERO; + } + + if (a.x[0] >= 709.0) { + return DD_C_INF; + } + + if (dd_is_zero(a)) { + return DD_C_ONE; + } + + if (dd_is_one(a)) { + return DD_C_E; + } + + m = floor(a.x[0] / DD_C_LOG2.x[0] + 0.5); + r = dd_mul_pwr2(dd_sub(a, dd_mul_dd_d(DD_C_LOG2, m)), inv_k); + + p = dd_sqr(r); + s = dd_add(r, dd_mul_pwr2(p, 0.5)); + p = dd_mul(p, r); + t = dd_mul(p, inv_fact[0]); + do { + s = dd_add(s, t); + p = dd_mul(p, r); + ++i; + t = dd_mul(p, inv_fact[i]); + } while (fabs(dd_to_double(t)) > inv_k * DD_C_EPS && i < 5); + + s = dd_add(s, t); + + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(s, DD_C_ONE); + + return dd_ldexp(s, DD_STATIC_CAST(int, m)); +} + +double2 +dd_exp_d(const double a) +{ + return dd_exp(dd_create(a, 0)); +} + + +/* Logarithm. Computes log(x) in double-double precision. + This is a natural logarithm (i.e., base e). */ +double2 +dd_log(const double2 a) +{ + /* Strategy. The Taylor series for log converges much more + slowly than that of exp, due to the lack of the factorial + term in the denominator. Hence this routine instead tries + to determine the root of the function + + f(x) = exp(x) - a + + using Newton iteration. The iteration is given by + + x' = x - f(x)/f'(x) + = x - (1 - a * exp(-x)) + = x + a * exp(-x) - 1. + + Only one iteration is needed, since Newton's iteration + approximately doubles the number of digits per iteration. */ + double2 x; + + if (dd_is_one(a)) { + return DD_C_ZERO; + } + + if (a.x[0] <= 0.0) { + dd_error("(dd_log): Non-positive argument."); + return DD_C_NAN; + } + + x = dd_create_d(log(a.x[0])); /* Initial approximation */ + + /* x = x + a * exp(-x) - 1.0; */ + x = dd_add(x, dd_sub(dd_mul(a, dd_exp(dd_neg(x))), DD_C_ONE)); + return x; +} + + +double2 +dd_log1p(const double2 a) +{ + double2 ans; + double la, elam1, ll; + if (a.x[0] <= -1.0) { + return DD_C_NEGINF; + } + la = log1p(a.x[0]); + elam1 = expm1(la); + ll = log1p(a.x[1] / (1 + a.x[0])); + if (a.x[0] > 0) { + ll -= (elam1 - a.x[0])/(elam1+1); + } + ans = dd_add_d_d(la, ll); + return ans; +} + +double2 +dd_log10(const double2 a) +{ + return dd_div(dd_log(a), DD_C_LOG10); +} + +double2 +dd_log_d(double a) +{ + return dd_log(dd_create(a, 0)); +} + + +static const double2 expm1_numer[] = { + {{-0.028127670288085938, 1.46e-37}}, + {{0.5127815691121048, -4.248816580490825e-17}}, + {{-0.0632631785207471, 4.733650586348708e-18}}, + {{0.01470328560687425, -4.57569727474415e-20}}, + {{-0.0008675686051689528, 2.340010361165805e-20}}, + {{8.812635961829116e-05, 2.619804163788941e-21}}, + {{-2.596308786770631e-06, -1.6196413688647164e-22}}, + {{1.422669108780046e-07, 1.2956999470135368e-23}}, + {{-1.5995603306536497e-09, 5.185121944095551e-26}}, + {{4.526182006900779e-11, -1.9856249941108077e-27}} +}; + +static const double2 expm1_denom[] = { + {{1.0, 0.0}}, + {{-0.4544126470907431, -2.2553855773661143e-17}}, + {{0.09682713193619222, -4.961446925746919e-19}}, + {{-0.012745248725908178, -6.0676821249478945e-19}}, + {{0.001147361387158326, 1.3575817248483204e-20}}, + {{-7.370416847725892e-05, 3.720369981570573e-21}}, + {{3.4087499397791556e-06, -3.3067348191741576e-23}}, + {{-1.1114024704296196e-07, -3.313361038199987e-24}}, + {{2.3987051614110847e-09, 1.102474920537503e-25}}, + {{-2.947734185911159e-11, -9.4795654767864e-28}}, + {{1.32220659910223e-13, 6.440648413523595e-30}} +}; + +// +// Rational approximation of expm1(x) for -1/2 < x < 1/2 +// +static double2 +expm1_rational_approx(const double2 x) +{ + const double2 Y = dd_create(1.028127670288086, 0.0); + const double2 num = dd_polyeval(expm1_numer, 9, x); + const double2 den = dd_polyeval(expm1_denom, 10, x); + return dd_add(dd_mul(x, Y), dd_mul(x, dd_div(num, den))); +} + +// +// This is a translation of Boost's `expm1_imp` for quad precision +// for use with double2. +// + +#define LOG_MAX_VALUE 709.782712893384 + +double2 +dd_expm1(const double2 x) +{ + double2 a = dd_abs(x); + if (dd_hi(a) > 0.5) { + if (dd_hi(a) > LOG_MAX_VALUE) { + if (dd_hi(x) > 0) { + return DD_C_INF; + } + return DD_C_NEGONE; + } + return dd_sub_dd_d(dd_exp(x), 1.0); + } + return expm1_rational_approx(x); +} + + +double2 +dd_rand(void) +{ + static const double m_const = 4.6566128730773926e-10; /* = 2^{-31} */ + double m = m_const; + double2 r = DD_C_ZERO; + double d; + int i; + + /* Strategy: Generate 31 bits at a time, using lrand48 + random number generator. Shift the bits, and reapeat + 4 times. */ + + for (i = 0; i < 4; i++, m *= m_const) { + // d = lrand48() * m; + d = rand() * m; + r = dd_add_dd_d(r, d); + } + + return r; +} + +/* dd_polyeval(c, n, x) + Evaluates the given n-th degree polynomial at x. + The polynomial is given by the array of (n+1) coefficients. */ + +double2 +dd_polyeval(const double2 *c, int n, const double2 x) +{ + /* Just use Horner's method of polynomial evaluation. */ + double2 r = c[n]; + int i; + + for (i = n - 1; i >= 0; i--) { + r = dd_mul(r, x); + r = dd_add(r, c[i]); + } + + return r; +} + +/* dd_polyroot(c, n, x0) + Given an n-th degree polynomial, finds a root close to + the given guess x0. Note that this uses simple Newton + iteration scheme, and does not work for multiple roots. */ + +double2 +dd_polyroot(const double2 *c, int n, const double2 x0, int max_iter, + double thresh) +{ + double2 x = x0; + double2 f; + double2 *d = DD_STATIC_CAST(double2 *, calloc(sizeof(double2), n)); + int conv = 0; + int i; + double max_c = fabs(dd_to_double(c[0])); + double v; + + if (thresh == 0.0) { + thresh = DD_C_EPS; + } + + /* Compute the coefficients of the derivatives. */ + for (i = 1; i <= n; i++) { + v = fabs(dd_to_double(c[i])); + if (v > max_c) { + max_c = v; + } + d[i - 1] = dd_mul_dd_d(c[i], DD_STATIC_CAST(double, i)); + } + thresh *= max_c; + + /* Newton iteration. */ + for (i = 0; i < max_iter; i++) { + f = dd_polyeval(c, n, x); + + if (fabs(dd_to_double(f)) < thresh) { + conv = 1; + break; + } + x = dd_sub(x, (dd_div(f, dd_polyeval(d, n - 1, x)))); + } + free(d); + + if (!conv) { + dd_error("(dd_polyroot): Failed to converge."); + return DD_C_NAN; + } + + return x; +} diff --git a/gtsam/3rdparty/cephes/cephes/dd_real.h b/gtsam/3rdparty/cephes/cephes/dd_real.h new file mode 100644 index 0000000000..4e09da1432 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dd_real.h @@ -0,0 +1,143 @@ +/* + * include/double2.h + * + * This work was supported by the Director, Office of Science, Division + * of Mathematical, Information, and Computational Sciences of the + * U.S. Department of Energy under contract numbers DE-AC03-76SF00098 and + * DE-AC02-05CH11231. + * + * Copyright (c) 2003-2009, The Regents of the University of California, + * through Lawrence Berkeley National Laboratory (subject to receipt of + * any required approvals from U.S. Dept. of Energy) All rights reserved. + * + * By downloading or using this software you are agreeing to the modified + * BSD license "BSD-LBNL-License.doc" (see LICENSE.txt). + */ +/* + * Double-double precision (>= 106-bit significand) floating point + * arithmetic package based on David Bailey's Fortran-90 double-double + * package, with some changes. See + * + * http://www.nersc.gov/~dhbailey/mpdist/mpdist.html + * + * for the original Fortran-90 version. + * + * Overall structure is similar to that of Keith Brigg's C++ double-double + * package. See + * + * http://www-epidem.plansci.cam.ac.uk/~kbriggs/doubledouble.html + * + * for more details. In particular, the fix for x86 computers is borrowed + * from his code. + * + * Yozo Hida + */ + +#ifndef _DD_REAL_H +#define _DD_REAL_H + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Some configuration defines */ + +/* If fast fused multiply-add is available, define to the correct macro for + using it. It is invoked as DD_FMA(a, b, c) to compute fl(a * b + c). + If correctly rounded multiply-add is not available (or if unsure), + keep it undefined. */ +#ifndef DD_FMA +#ifdef FP_FAST_FMA +#define DD_FMA(A, B, C) fma((A), (B), (C)) +#endif +#endif + +/* Same with fused multiply-subtract */ +#ifndef DD_FMS +#ifdef FP_FAST_FMA +#define DD_FMS(A, B, C) fma((A), (B), (-C)) +#endif +#endif + +#ifdef __cplusplus +#define DD_STATIC_CAST(T, X) (static_cast(X)) +#else +#define DD_STATIC_CAST(T, X) ((T)(X)) +#endif + +/* double2 struct definition, some external always-present double2 constants. +*/ +typedef struct double2 +{ + double x[2]; +} double2; + +extern const double DD_C_EPS; +extern const double DD_C_MIN_NORMALIZED; +extern const double2 DD_C_MAX; +extern const double2 DD_C_SAFE_MAX; +extern const int DD_C_NDIGITS; + +extern const double2 DD_C_2PI; +extern const double2 DD_C_PI; +extern const double2 DD_C_3PI4; +extern const double2 DD_C_PI2; +extern const double2 DD_C_PI4; +extern const double2 DD_C_PI16; +extern const double2 DD_C_E; +extern const double2 DD_C_LOG2; +extern const double2 DD_C_LOG10; +extern const double2 DD_C_ZERO; +extern const double2 DD_C_ONE; +extern const double2 DD_C_NEGONE; + +/* NAN definition in AIX's math.h doesn't make it qualify as constant literal. */ +#if defined(__STDC__) && defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L) && defined(NAN) && !defined(_AIX) +#define DD_C_NAN_IS_CONST +extern const double2 DD_C_NAN; +extern const double2 DD_C_INF; +extern const double2 DD_C_NEGINF; +#else +#define DD_C_NAN (dd_create(NAN, NAN)) +#define DD_C_INF (dd_create(INFINITY, INFINITY)) +#define DD_C_NEGINF (dd_create(-INFINITY, -INFINITY)) +#endif + + +/* Include the inline definitions of functions */ +#include "dd_real_idefs.h" + +/* Non-inline functions */ + +/********** Exponentiation **********/ +double2 dd_npwr(const double2 a, int n); + +/*********** Transcendental Functions ************/ +double2 dd_exp(const double2 a); +double2 dd_log(const double2 a); +double2 dd_expm1(const double2 a); +double2 dd_log1p(const double2 a); +double2 dd_log10(const double2 a); +double2 dd_log_d(double a); + +/* Returns the exponent of the double precision number. + Returns INT_MIN is x is zero, and INT_MAX if x is INF or NaN. */ +int get_double_expn(double x); + +/*********** Polynomial Functions ************/ +double2 dd_polyeval(const double2 *c, int n, const double2 x); + +/*********** Random number generator ************/ +extern double2 dd_rand(void); + + +#ifdef __cplusplus +} +#endif + + +#endif /* _DD_REAL_H */ diff --git a/gtsam/3rdparty/cephes/cephes/dd_real_idefs.h b/gtsam/3rdparty/cephes/cephes/dd_real_idefs.h new file mode 100644 index 0000000000..d2b9ac1d65 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dd_real_idefs.h @@ -0,0 +1,557 @@ +/* + * include/dd_inline.h + * + * This work was supported by the Director, Office of Science, Division + * of Mathematical, Information, and Computational Sciences of the + * U.S. Department of Energy under contract numbers DE-AC03-76SF00098 and + * DE-AC02-05CH11231. + * + * Copyright (c) 2003-2009, The Regents of the University of California, + * through Lawrence Berkeley National Laboratory (subject to receipt of + * any required approvals from U.S. Dept. of Energy) All rights reserved. + * + * By downloading or using this software you are agreeing to the modified + * BSD license "BSD-LBNL-License.doc" (see LICENSE.txt). + */ +/* + * Contains small functions (suitable for inlining) in the double-double + * arithmetic package. + */ + +#ifndef _DD_REAL_IDEFS_H_ +#define _DD_REAL_IDEFS_H_ 1 + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#include "dd_idefs.h" + +/* + ************************************************************************ + Now for the double2 routines + ************************************************************************ +*/ + +static inline double +dd_hi(const double2 a) +{ + return a.x[0]; +} + +static inline double +dd_lo(const double2 a) +{ + return a.x[1]; +} + +static inline int +dd_isfinite(const double2 a) +{ + return isfinite(a.x[0]); +} + +static inline int +dd_isinf(const double2 a) +{ + return isinf(a.x[0]); +} + +static inline int +dd_is_zero(const double2 a) +{ + return (a.x[0] == 0.0); +} + +static inline int +dd_is_one(const double2 a) +{ + return (a.x[0] == 1.0 && a.x[1] == 0.0); +} + +static inline int +dd_is_positive(const double2 a) +{ + return (a.x[0] > 0.0); +} + +static inline int +dd_is_negative(const double2 a) +{ + return (a.x[0] < 0.0); +} + +/* Cast to double. */ +static inline double +dd_to_double(const double2 a) +{ + return a.x[0]; +} + +/* Cast to int. */ +static inline int +dd_to_int(const double2 a) +{ + return DD_STATIC_CAST(int, a.x[0]); +} + +/*********** Equality and Other Comparisons ************/ +static inline int +dd_comp(const double2 a, const double2 b) +{ + int cmp = two_comp(a.x[0], b.x[0]); + if (cmp == 0) { + cmp = two_comp(a.x[1], b.x[1]); + } + return cmp; +} + +static inline int +dd_comp_dd_d(const double2 a, double b) +{ + int cmp = two_comp(a.x[0], b); + if (cmp == 0) { + cmp = two_comp(a.x[1], 0); + } + return cmp; +} + +static inline int +dd_comp_d_dd(double a, const double2 b) +{ + int cmp = two_comp(a, b.x[0]); + if (cmp == 0) { + cmp = two_comp(0.0, b.x[1]); + } + return cmp; +} + + +/*********** Creation ************/ +static inline double2 +dd_create(double hi, double lo) +{ + double2 ret = {{hi, lo}}; + return ret; +} + +static inline double2 +dd_zero(void) +{ + return DD_C_ZERO; +} + +static inline double2 +dd_create_d(double hi) +{ + double2 ret = {{hi, 0.0}}; + return ret; +} + +static inline double2 +dd_create_i(int hi) +{ + double2 ret = {{DD_STATIC_CAST(double, hi), 0.0}}; + return ret; +} + +static inline double2 +dd_create_dp(const double *d) +{ + double2 ret = {{d[0], d[1]}}; + return ret; +} + + +/*********** Unary Minus ***********/ +static inline double2 +dd_neg(const double2 a) +{ + double2 ret = {{-a.x[0], -a.x[1]}}; + return ret; +} + +/*********** Rounding ************/ +/* Round to Nearest integer */ +static inline double2 +dd_nint(const double2 a) +{ + double hi = two_nint(a.x[0]); + double lo; + + if (hi == a.x[0]) { + /* High word is an integer already. Round the low word.*/ + lo = two_nint(a.x[1]); + + /* Renormalize. This is needed if x[0] = some integer, x[1] = 1/2.*/ + hi = quick_two_sum(hi, lo, &lo); + } + else { + /* High word is not an integer. */ + lo = 0.0; + if (fabs(hi - a.x[0]) == 0.5 && a.x[1] < 0.0) { + /* There is a tie in the high word, consult the low word + to break the tie. */ + hi -= 1.0; /* NOTE: This does not cause INEXACT. */ + } + } + + return dd_create(hi, lo); +} + +static inline double2 +dd_floor(const double2 a) +{ + double hi = floor(a.x[0]); + double lo = 0.0; + + if (hi == a.x[0]) { + /* High word is integer already. Round the low word. */ + lo = floor(a.x[1]); + hi = quick_two_sum(hi, lo, &lo); + } + + return dd_create(hi, lo); +} + +static inline double2 +dd_ceil(const double2 a) +{ + double hi = ceil(a.x[0]); + double lo = 0.0; + + if (hi == a.x[0]) { + /* High word is integer already. Round the low word. */ + lo = ceil(a.x[1]); + hi = quick_two_sum(hi, lo, &lo); + } + + return dd_create(hi, lo); +} + +static inline double2 +dd_aint(const double2 a) +{ + return (a.x[0] >= 0.0) ? dd_floor(a) : dd_ceil(a); +} + +/* Absolute value */ +static inline double2 +dd_abs(const double2 a) +{ + return (a.x[0] < 0.0 ? dd_neg(a) : a); +} + +static inline double2 +dd_fabs(const double2 a) +{ + return dd_abs(a); +} + + +/*********** Normalizing ***********/ +/* double-double * (2.0 ^ expt) */ +static inline double2 +dd_ldexp(const double2 a, int expt) +{ + return dd_create(ldexp(a.x[0], expt), ldexp(a.x[1], expt)); +} + +static inline double2 +dd_frexp(const double2 a, int *expt) +{ +// r"""return b and l s.t. 0.5<=|b|<1 and 2^l == a +// 0.5<=|b[0]|<1.0 or |b[0]| == 1.0 and b[0]*b[1]<0 +// """ + int exponent; + double man = frexp(a.x[0], &exponent); + double b1 = ldexp(a.x[1], -exponent); + if (fabs(man) == 0.5 && man * b1 < 0) + { + man *=2; + b1 *= 2; + exponent -= 1; + } + *expt = exponent; + return dd_create(man, b1); +} + + +/*********** Additions ************/ +static inline double2 +dd_add_d_d(double a, double b) +{ + double s, e; + s = two_sum(a, b, &e); + return dd_create(s, e); +} + +static inline double2 +dd_add_dd_d(const double2 a, double b) +{ + double s1, s2; + s1 = two_sum(a.x[0], b, &s2); + s2 += a.x[1]; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_add_d_dd(double a, const double2 b) +{ + double s1, s2; + s1 = two_sum(a, b.x[0], &s2); + s2 += b.x[1]; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_ieee_add(const double2 a, const double2 b) +{ + /* This one satisfies IEEE style error bound, + due to K. Briggs and W. Kahan. */ + double s1, s2, t1, t2; + + s1 = two_sum(a.x[0], b.x[0], &s2); + t1 = two_sum(a.x[1], b.x[1], &t2); + s2 += t1; + s1 = quick_two_sum(s1, s2, &s2); + s2 += t2; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_sloppy_add(const double2 a, const double2 b) +{ + /* This is the less accurate version ... obeys Cray-style + error bound. */ + double s, e; + + s = two_sum(a.x[0], b.x[0], &e); + e += (a.x[1] + b.x[1]); + s = quick_two_sum(s, e, &e); + return dd_create(s, e); +} + +static inline double2 +dd_add(const double2 a, const double2 b) +{ + /* Always require IEEE-style error bounds */ + return dd_ieee_add(a, b); +} + +/*********** Subtractions ************/ +/* double-double = double - double */ +static inline double2 +dd_sub_d_d(double a, double b) +{ + double s, e; + s = two_diff(a, b, &e); + return dd_create(s, e); +} + +static inline double2 +dd_sub(const double2 a, const double2 b) +{ + return dd_ieee_add(a, dd_neg(b)); +} + +static inline double2 +dd_sub_dd_d(const double2 a, double b) +{ + double s1, s2; + s1 = two_sum(a.x[0], -b, &s2); + s2 += a.x[1]; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_sub_d_dd(double a, const double2 b) +{ + double s1, s2; + s1 = two_sum(a, -b.x[0], &s2); + s2 -= b.x[1]; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + + +/*********** Multiplications ************/ +/* double-double = double * double */ +static inline double2 +dd_mul_d_d(double a, double b) +{ + double p, e; + p = two_prod(a, b, &e); + return dd_create(p, e); +} + +/* double-double * double, where double is a power of 2. */ +static inline double2 +dd_mul_pwr2(const double2 a, double b) +{ + return dd_create(a.x[0] * b, a.x[1] * b); +} + +static inline double2 +dd_mul(const double2 a, const double2 b) +{ + double p1, p2; + p1 = two_prod(a.x[0], b.x[0], &p2); + p2 += (a.x[0] * b.x[1] + a.x[1] * b.x[0]); + p1 = quick_two_sum(p1, p2, &p2); + return dd_create(p1, p2); +} + +static inline double2 +dd_mul_dd_d(const double2 a, double b) +{ + double p1, p2, e1, e2; + p1 = two_prod(a.x[0], b, &e1); + p2 = two_prod(a.x[1], b, &e2); + p1 = quick_two_sum(p1, e2 + p2 + e1, &e1); + return dd_create(p1, e1); +} + +static inline double2 +dd_mul_d_dd(double a, const double2 b) +{ + double p1, p2, e1, e2; + p1 = two_prod(a, b.x[0], &e1); + p2 = two_prod(a, b.x[1], &e2); + p1 = quick_two_sum(p1, e2 + p2 + e1, &e1); + return dd_create(p1, e1); +} + + +/*********** Divisions ************/ +static inline double2 +dd_sloppy_div(const double2 a, const double2 b) +{ + double s1, s2; + double q1, q2; + double2 r; + + q1 = a.x[0] / b.x[0]; /* approximate quotient */ + + /* compute this - q1 * dd */ + r = dd_sub(a, dd_mul_dd_d(b, q1)); + s1 = two_diff(a.x[0], r.x[0], &s2); + s2 -= r.x[1]; + s2 += a.x[1]; + + /* get next approximation */ + q2 = (s1 + s2) / b.x[0]; + + /* renormalize */ + r.x[0] = quick_two_sum(q1, q2, &r.x[1]); + return r; +} + +static inline double2 +dd_accurate_div(const double2 a, const double2 b) +{ + double q1, q2, q3; + double2 r; + + q1 = a.x[0] / b.x[0]; /* approximate quotient */ + + r = dd_sub(a, dd_mul_dd_d(b, q1)); + + q2 = r.x[0] / b.x[0]; + r = dd_sub(r, dd_mul_dd_d(b, q2)); + + q3 = r.x[0] / b.x[0]; + + q1 = quick_two_sum(q1, q2, &q2); + r = dd_add_dd_d(dd_create(q1, q2), q3); + return r; +} + +static inline double2 +dd_div(const double2 a, const double2 b) +{ + return dd_accurate_div(a, b); +} + +static inline double2 +dd_div_d_d(double a, double b) +{ + return dd_accurate_div(dd_create_d(a), dd_create_d(b)); +} + +static inline double2 +dd_div_dd_d(const double2 a, double b) +{ + return dd_accurate_div(a, dd_create_d(b)); +} + +static inline double2 +dd_div_d_dd(double a, const double2 b) +{ + return dd_accurate_div(dd_create_d(a), b); +} + +static inline double2 +dd_inv(const double2 a) +{ + return dd_div(DD_C_ONE, a); +} + + +/********** Remainder **********/ +static inline double2 +dd_drem(const double2 a, const double2 b) +{ + double2 n = dd_nint(dd_div(a, b)); + return dd_sub(a, dd_mul(n, b)); +} + +static inline double2 +dd_divrem(const double2 a, const double2 b, double2 *r) +{ + double2 n = dd_nint(dd_div(a, b)); + *r = dd_sub(a, dd_mul(n, b)); + return n; +} + +static inline double2 +dd_fmod(const double2 a, const double2 b) +{ + double2 n = dd_aint(dd_div(a, b)); + return dd_sub(a, dd_mul(b, n)); +} + +/*********** Squaring **********/ +static inline double2 +dd_sqr(const double2 a) +{ + double p1, p2; + double s1, s2; + p1 = two_sqr(a.x[0], &p2); + p2 += 2.0 * a.x[0] * a.x[1]; + p2 += a.x[1] * a.x[1]; + s1 = quick_two_sum(p1, p2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_sqr_d(double a) +{ + double p1, p2; + p1 = two_sqr(a, &p2); + return dd_create(p1, p2); +} + +#ifdef __cplusplus +} +#endif + +#endif /* _DD_REAL_IDEFS_H_ */ diff --git a/gtsam/3rdparty/cephes/cephes/ellie.c b/gtsam/3rdparty/cephes/cephes/ellie.c new file mode 100644 index 0000000000..8a2823f3a0 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellie.c @@ -0,0 +1,282 @@ +/* ellie.c + * + * Incomplete elliptic integral of the second kind + * + * + * + * SYNOPSIS: + * + * double phi, m, y, ellie(); + * + * y = ellie( phi, m ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * + * phi + * - + * | | + * | 2 + * E(phi_\m) = | sqrt( 1 - m sin t ) dt + * | + * | | + * - + * 0 + * + * of amplitude phi and modulus m, using the arithmetic - + * geometric mean algorithm. + * + * + * + * ACCURACY: + * + * Tested at random arguments with phi in [-10, 10] and m in + * [0, 1]. + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -10,10 150000 3.3e-15 1.4e-16 + */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987, 1993 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ +/* Copyright 2014, Eric W. Moore */ + +/* Incomplete elliptic integral of second kind */ + +#include "mconf.h" + +extern double MACHEP; + +static double ellie_neg_m(double phi, double m); + +double ellie(double phi, double m) +{ + double a, b, c, e, temp; + double lphi, t, E, denom, npio2; + int d, mod, sign; + + if (cephes_isnan(phi) || cephes_isnan(m)) + return NAN; + if (m > 1.0) + return NAN; + if (cephes_isinf(phi)) + return phi; + if (cephes_isinf(m)) + return -m; + if (m == 0.0) + return (phi); + lphi = phi; + npio2 = floor(lphi / M_PI_2); + if (fmod(fabs(npio2), 2.0) == 1.0) + npio2 += 1; + lphi = lphi - npio2 * M_PI_2; + if (lphi < 0.0) { + lphi = -lphi; + sign = -1; + } + else { + sign = 1; + } + a = 1.0 - m; + E = ellpe(m); + if (a == 0.0) { + temp = sin(lphi); + goto done; + } + if (a > 1.0) { + temp = ellie_neg_m(lphi, m); + goto done; + } + + if (lphi < 0.135) { + double m11= (((((-7.0/2816.0)*m + (5.0/1056.0))*m - (7.0/2640.0))*m + + (17.0/41580.0))*m - (1.0/155925.0))*m; + double m9 = ((((-5.0/1152.0)*m + (1.0/144.0))*m - (1.0/360.0))*m + + (1.0/5670.0))*m; + double m7 = ((-m/112.0 + (1.0/84.0))*m - (1.0/315.0))*m; + double m5 = (-m/40.0 + (1.0/30))*m; + double m3 = -m/6.0; + double p2 = lphi * lphi; + + temp = ((((m11*p2 + m9)*p2 + m7)*p2 + m5)*p2 + m3)*p2*lphi + lphi; + goto done; + } + t = tan(lphi); + b = sqrt(a); + /* Thanks to Brian Fitzgerald + * for pointing out an instability near odd multiples of pi/2. */ + if (fabs(t) > 10.0) { + /* Transform the amplitude */ + e = 1.0 / (b * t); + /* ... but avoid multiple recursions. */ + if (fabs(e) < 10.0) { + e = atan(e); + temp = E + m * sin(lphi) * sin(e) - ellie(e, m); + goto done; + } + } + c = sqrt(m); + a = 1.0; + d = 1; + e = 0.0; + mod = 0; + + while (fabs(c / a) > MACHEP) { + temp = b / a; + lphi = lphi + atan(t * temp) + mod * M_PI; + denom = 1 - temp * t * t; + if (fabs(denom) > 10*MACHEP) { + t = t * (1.0 + temp) / denom; + mod = (lphi + M_PI_2) / M_PI; + } + else { + t = tan(lphi); + mod = (int)floor((lphi - atan(t))/M_PI); + } + c = (a - b) / 2.0; + temp = sqrt(a * b); + a = (a + b) / 2.0; + b = temp; + d += d; + e += c * sin(lphi); + } + + temp = E / ellpk(1.0 - m); + temp *= (atan(t) + mod * M_PI) / (d * a); + temp += e; + + done: + + if (sign < 0) + temp = -temp; + temp += npio2 * E; + return (temp); +} + +/* N.B. This will evaluate its arguments multiple times. */ +#define MAX3(a, b, c) (a > b ? (a > c ? a : c) : (b > c ? b : c)) + +/* To calculate legendre's incomplete elliptical integral of the second kind for + * negative m, we use a power series in phi for small m*phi*phi, an asymptotic + * series in m for large m*phi*phi* and the relation to Carlson's symmetric + * integrals, R_F(x,y,z) and R_D(x,y,z). + * + * E(phi, m) = sin(phi) * R_F(cos(phi)^2, 1 - m * sin(phi)^2, 1.0) + * - m * sin(phi)^3 * R_D(cos(phi)^2, 1 - m * sin(phi)^2, 1.0) / 3 + * + * = R_F(c-1, c-m, c) - m * R_D(c-1, c-m, c) / 3 + * + * where c = csc(phi)^2. We use the second form of this for (approximately) + * phi > 1/(sqrt(DBL_MAX) ~ 1e-154, where csc(phi)^2 overflows. Elsewhere we + * use the first form, accounting for the smallness of phi. + * + * The algorithm used is described in Carlson, B. C. Numerical computation of + * real or complex elliptic integrals. (1994) https://arxiv.org/abs/math/9409227 + * Most variable names reflect Carlson's usage. + * + * In this routine, we assume m < 0 and 0 > phi > pi/2. + */ +double ellie_neg_m(double phi, double m) +{ + double x, y, z, x1, y1, z1, ret, Q; + double A0f, Af, Xf, Yf, Zf, E2f, E3f, scalef; + double A0d, Ad, seriesn, seriesd, Xd, Yd, Zd, E2d, E3d, E4d, E5d, scaled; + int n = 0; + double mpp = (m*phi)*phi; + + if (-mpp < 1e-6 && phi < -m) { + return phi + (mpp*phi*phi/30.0 - mpp*mpp/40.0 - mpp/6.0)*phi; + } + + if (-mpp > 1e6) { + double sm = sqrt(-m); + double sp = sin(phi); + double cp = cos(phi); + + double a = -cosm1(phi); + double b1 = log(4*sp*sm/(1+cp)); + double b = -(0.5 + b1) / 2.0 / m; + double c = (0.75 + cp/sp/sp - b1) / 16.0 / m / m; + return (a + b + c) * sm; + } + + if (phi > 1e-153 && m > -1e200) { + double s = sin(phi); + double csc2 = 1.0 / s / s; + scalef = 1.0; + scaled = m / 3.0; + x = 1.0 / tan(phi) / tan(phi); + y = csc2 - m; + z = csc2; + } + else { + scalef = phi; + scaled = mpp * phi / 3.0; + x = 1.0; + y = 1 - mpp; + z = 1.0; + } + + if (x == y && x == z) { + return (scalef + scaled/x)/sqrt(x); + } + + A0f = (x + y + z) / 3.0; + Af = A0f; + A0d = (x + y + 3.0*z) / 5.0; + Ad = A0d; + x1 = x; y1 = y; z1 = z; seriesd = 0.0; seriesn = 1.0; + /* Carlson gives 1/pow(3*r, 1.0/6.0) for this constant. if r == eps, + * it is ~338.38. */ + Q = 400.0 * MAX3(fabs(A0f-x), fabs(A0f-y), fabs(A0f-z)); + + while (Q > fabs(Af) && Q > fabs(Ad) && n <= 100) { + double sx = sqrt(x1); + double sy = sqrt(y1); + double sz = sqrt(z1); + double lam = sx*sy + sx*sz + sy*sz; + seriesd += seriesn / (sz * (z1 + lam)); + x1 = (x1 + lam) / 4.0; + y1 = (y1 + lam) / 4.0; + z1 = (z1 + lam) / 4.0; + Af = (x1 + y1 + z1) / 3.0; + Ad = (Ad + lam) / 4.0; + n += 1; + Q /= 4.0; + seriesn /= 4.0; + } + + Xf = (A0f - x) / Af / (1 << 2*n); + Yf = (A0f - y) / Af / (1 << 2*n); + Zf = -(Xf + Yf); + + E2f = Xf*Yf - Zf*Zf; + E3f = Xf*Yf*Zf; + + ret = scalef * (1.0 - E2f/10.0 + E3f/14.0 + E2f*E2f/24.0 + - 3.0*E2f*E3f/44.0) / sqrt(Af); + + Xd = (A0d - x) / Ad / (1 << 2*n); + Yd = (A0d - y) / Ad / (1 << 2*n); + Zd = -(Xd + Yd)/3.0; + + E2d = Xd*Yd - 6.0*Zd*Zd; + E3d = (3*Xd*Yd - 8.0*Zd*Zd)*Zd; + E4d = 3.0*(Xd*Yd - Zd*Zd)*Zd*Zd; + E5d = Xd*Yd*Zd*Zd*Zd; + + ret -= scaled * (1.0 - 3.0*E2d/14.0 + E3d/6.0 + 9.0*E2d*E2d/88.0 + - 3.0*E4d/22.0 - 9.0*E2d*E3d/52.0 + 3.0*E5d/26.0) + /(1 << 2*n) / Ad / sqrt(Ad); + ret -= 3.0 * scaled * seriesd; + return ret; +} + diff --git a/gtsam/3rdparty/cephes/cephes/ellik.c b/gtsam/3rdparty/cephes/cephes/ellik.c new file mode 100644 index 0000000000..ee73e062a2 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellik.c @@ -0,0 +1,246 @@ +/* ellik.c + * + * Incomplete elliptic integral of the first kind + * + * + * + * SYNOPSIS: + * + * double phi, m, y, ellik(); + * + * y = ellik( phi, m ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * + * + * phi + * - + * | | + * | dt + * F(phi | m) = | ------------------ + * | 2 + * | | sqrt( 1 - m sin t ) + * - + * 0 + * + * of amplitude phi and modulus m, using the arithmetic - + * geometric mean algorithm. + * + * + * + * + * ACCURACY: + * + * Tested at random points with m in [0, 1] and phi as indicated. + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -10,10 200000 7.4e-16 1.0e-16 + * + * + */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ +/* Copyright 2014, Eric W. Moore */ + +/* Incomplete elliptic integral of first kind */ + +#include "mconf.h" +extern double MACHEP; + +static double ellik_neg_m(double phi, double m); + +double ellik(double phi, double m) +{ + double a, b, c, e, temp, t, K, denom, npio2; + int d, mod, sign; + + if (cephes_isnan(phi) || cephes_isnan(m)) + return NAN; + if (m > 1.0) + return NAN; + if (cephes_isinf(phi) || cephes_isinf(m)) + { + if (cephes_isinf(m) && cephes_isfinite(phi)) + return 0.0; + else if (cephes_isinf(phi) && cephes_isfinite(m)) + return phi; + else + return NAN; + } + if (m == 0.0) + return (phi); + a = 1.0 - m; + if (a == 0.0) { + if (fabs(phi) >= (double)M_PI_2) { + sf_error("ellik", SF_ERROR_SINGULAR, NULL); + return (INFINITY); + } + /* DLMF 19.6.8, and 4.23.42 */ + return asinh(tan(phi)); + } + npio2 = floor(phi / M_PI_2); + if (fmod(fabs(npio2), 2.0) == 1.0) + npio2 += 1; + if (npio2 != 0.0) { + K = ellpk(a); + phi = phi - npio2 * M_PI_2; + } + else + K = 0.0; + if (phi < 0.0) { + phi = -phi; + sign = -1; + } + else + sign = 0; + if (a > 1.0) { + temp = ellik_neg_m(phi, m); + goto done; + } + b = sqrt(a); + t = tan(phi); + if (fabs(t) > 10.0) { + /* Transform the amplitude */ + e = 1.0 / (b * t); + /* ... but avoid multiple recursions. */ + if (fabs(e) < 10.0) { + e = atan(e); + if (npio2 == 0) + K = ellpk(a); + temp = K - ellik(e, m); + goto done; + } + } + a = 1.0; + c = sqrt(m); + d = 1; + mod = 0; + + while (fabs(c / a) > MACHEP) { + temp = b / a; + phi = phi + atan(t * temp) + mod * M_PI; + denom = 1.0 - temp * t * t; + if (fabs(denom) > 10*MACHEP) { + t = t * (1.0 + temp) / denom; + mod = (phi + M_PI_2) / M_PI; + } + else { + t = tan(phi); + mod = (int)floor((phi - atan(t))/M_PI); + } + c = (a - b) / 2.0; + temp = sqrt(a * b); + a = (a + b) / 2.0; + b = temp; + d += d; + } + + temp = (atan(t) + mod * M_PI) / (d * a); + + done: + if (sign < 0) + temp = -temp; + temp += npio2 * K; + return (temp); +} + +/* N.B. This will evaluate its arguments multiple times. */ +#define MAX3(a, b, c) (a > b ? (a > c ? a : c) : (b > c ? b : c)) + +/* To calculate legendre's incomplete elliptical integral of the first kind for + * negative m, we use a power series in phi for small m*phi*phi, an asymptotic + * series in m for large m*phi*phi* and the relation to Carlson's symmetric + * integral of the first kind. + * + * F(phi, m) = sin(phi) * R_F(cos(phi)^2, 1 - m * sin(phi)^2, 1.0) + * = R_F(c-1, c-m, c) + * + * where c = csc(phi)^2. We use the second form of this for (approximately) + * phi > 1/(sqrt(DBL_MAX) ~ 1e-154, where csc(phi)^2 overflows. Elsewhere we + * use the first form, accounting for the smallness of phi. + * + * The algorithm used is described in Carlson, B. C. Numerical computation of + * real or complex elliptic integrals. (1994) https://arxiv.org/abs/math/9409227 + * Most variable names reflect Carlson's usage. + * + * In this routine, we assume m < 0 and 0 > phi > pi/2. + */ +double ellik_neg_m(double phi, double m) +{ + double x, y, z, x1, y1, z1, A0, A, Q, X, Y, Z, E2, E3, scale; + int n = 0; + double mpp = (m*phi)*phi; + + if (-mpp < 1e-6 && phi < -m) { + return phi + (-mpp*phi*phi/30.0 + 3.0*mpp*mpp/40.0 + mpp/6.0)*phi; + } + + if (-mpp > 4e7) { + double sm = sqrt(-m); + double sp = sin(phi); + double cp = cos(phi); + + double a = log(4*sp*sm/(1+cp)); + double b = -(1 + cp/sp/sp - a) / 4 / m; + return (a + b) / sm; + } + + if (phi > 1e-153 && m > -1e305) { + double s = sin(phi); + double csc2 = 1.0 / (s*s); + scale = 1.0; + x = 1.0 / (tan(phi) * tan(phi)); + y = csc2 - m; + z = csc2; + } + else { + scale = phi; + x = 1.0; + y = 1 - m*scale*scale; + z = 1.0; + } + + if (x == y && x == z) { + return scale / sqrt(x); + } + + A0 = (x + y + z) / 3.0; + A = A0; + x1 = x; y1 = y; z1 = z; + /* Carlson gives 1/pow(3*r, 1.0/6.0) for this constant. if r == eps, + * it is ~338.38. */ + Q = 400.0 * MAX3(fabs(A0-x), fabs(A0-y), fabs(A0-z)); + + while (Q > fabs(A) && n <= 100) { + double sx = sqrt(x1); + double sy = sqrt(y1); + double sz = sqrt(z1); + double lam = sx*sy + sx*sz + sy*sz; + x1 = (x1 + lam) / 4.0; + y1 = (y1 + lam) / 4.0; + z1 = (z1 + lam) / 4.0; + A = (x1 + y1 + z1) / 3.0; + n += 1; + Q /= 4; + } + X = (A0 - x) / A / (1 << 2*n); + Y = (A0 - y) / A / (1 << 2*n); + Z = -(X + Y); + + E2 = X*Y - Z*Z; + E3 = X*Y*Z; + + return scale * (1.0 - E2/10.0 + E3/14.0 + E2*E2/24.0 + - 3.0*E2*E3/44.0) / sqrt(A); +} diff --git a/gtsam/3rdparty/cephes/cephes/ellpe.c b/gtsam/3rdparty/cephes/cephes/ellpe.c new file mode 100644 index 0000000000..1ef8e0c128 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellpe.c @@ -0,0 +1,108 @@ +/* ellpe.c + * + * Complete elliptic integral of the second kind + * + * + * + * SYNOPSIS: + * + * double m, y, ellpe(); + * + * y = ellpe( m ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * + * pi/2 + * - + * | | 2 + * E(m) = | sqrt( 1 - m sin t ) dt + * | | + * - + * 0 + * + * Where m = 1 - m1, using the approximation + * + * P(x) - x log x Q(x). + * + * Though there are no singularities, the argument m1 is used + * internally rather than m for compatibility with ellpk(). + * + * E(1) = 1; E(0) = pi/2. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 1 10000 2.1e-16 7.3e-17 + * + * + * ERROR MESSAGES: + * + * message condition value returned + * ellpe domain x<0, x>1 0.0 + * + */ + +/* ellpe.c */ + +/* Elliptic integral of second kind */ + +/* + * Cephes Math Library, Release 2.1: February, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + * + * Feb, 2002: altered by Travis Oliphant + * so that it is called with argument m + * (which gets immediately converted to m1 = 1-m) + */ + +#include "mconf.h" + +static double P[] = { + 1.53552577301013293365E-4, + 2.50888492163602060990E-3, + 8.68786816565889628429E-3, + 1.07350949056076193403E-2, + 7.77395492516787092951E-3, + 7.58395289413514708519E-3, + 1.15688436810574127319E-2, + 2.18317996015557253103E-2, + 5.68051945617860553470E-2, + 4.43147180560990850618E-1, + 1.00000000000000000299E0 +}; + +static double Q[] = { + 3.27954898576485872656E-5, + 1.00962792679356715133E-3, + 6.50609489976927491433E-3, + 1.68862163993311317300E-2, + 2.61769742454493659583E-2, + 3.34833904888224918614E-2, + 4.27180926518931511717E-2, + 5.85936634471101055642E-2, + 9.37499997197644278445E-2, + 2.49999999999888314361E-1 +}; + +double ellpe(double x) +{ + x = 1.0 - x; + if (x <= 0.0) { + if (x == 0.0) + return (1.0); + sf_error("ellpe", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (x > 1.0) { + return ellpe(1.0 - 1/x) * sqrt(x); + } + return (polevl(x, P, 10) - log(x) * (x * polevl(x, Q, 9))); +} diff --git a/gtsam/3rdparty/cephes/cephes/ellpj.c b/gtsam/3rdparty/cephes/cephes/ellpj.c new file mode 100644 index 0000000000..6891a8244c --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellpj.c @@ -0,0 +1,154 @@ +/* ellpj.c + * + * Jacobian Elliptic Functions + * + * + * + * SYNOPSIS: + * + * double u, m, sn, cn, dn, phi; + * int ellpj(); + * + * ellpj( u, m, _&sn, _&cn, _&dn, _&phi ); + * + * + * + * DESCRIPTION: + * + * + * Evaluates the Jacobian elliptic functions sn(u|m), cn(u|m), + * and dn(u|m) of parameter m between 0 and 1, and real + * argument u. + * + * These functions are periodic, with quarter-period on the + * real axis equal to the complete elliptic integral + * ellpk(m). + * + * Relation to incomplete elliptic integral: + * If u = ellik(phi,m), then sn(u|m) = sin(phi), + * and cn(u|m) = cos(phi). Phi is called the amplitude of u. + * + * Computation is by means of the arithmetic-geometric mean + * algorithm, except when m is within 1e-9 of 0 or 1. In the + * latter case with m close to 1, the approximation applies + * only for phi < pi/2. + * + * ACCURACY: + * + * Tested at random points with u between 0 and 10, m between + * 0 and 1. + * + * Absolute error (* = relative error): + * arithmetic function # trials peak rms + * IEEE phi 10000 9.2e-16* 1.4e-16* + * IEEE sn 50000 4.1e-15 4.6e-16 + * IEEE cn 40000 3.6e-15 4.4e-16 + * IEEE dn 10000 1.3e-12 1.8e-14 + * + * Peak error observed in consistency check using addition + * theorem for sn(u+v) was 4e-16 (absolute). Also tested by + * the above relation to the incomplete elliptic integral. + * Accuracy deteriorates when u is large. + * + */ + +/* ellpj.c */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +/* Scipy changes: + * - 07-18-2016: improve evaluation of dn near quarter periods + */ + +#include "mconf.h" +extern double MACHEP; + +int ellpj(double u, double m, double *sn, double *cn, double *dn, double *ph) +{ + double ai, b, phi, t, twon, dnfac; + double a[9], c[9]; + int i; + + /* Check for special cases */ + if (m < 0.0 || m > 1.0 || cephes_isnan(m)) { + sf_error("ellpj", SF_ERROR_DOMAIN, NULL); + *sn = NAN; + *cn = NAN; + *ph = NAN; + *dn = NAN; + return (-1); + } + if (m < 1.0e-9) { + t = sin(u); + b = cos(u); + ai = 0.25 * m * (u - t * b); + *sn = t - ai * b; + *cn = b + ai * t; + *ph = u - ai; + *dn = 1.0 - 0.5 * m * t * t; + return (0); + } + if (m >= 0.9999999999) { + ai = 0.25 * (1.0 - m); + b = cosh(u); + t = tanh(u); + phi = 1.0 / b; + twon = b * sinh(u); + *sn = t + ai * (twon - u) / (b * b); + *ph = 2.0 * atan(exp(u)) - M_PI_2 + ai * (twon - u) / b; + ai *= t * phi; + *cn = phi - ai * (twon - u); + *dn = phi + ai * (twon + u); + return (0); + } + + /* A. G. M. scale. See DLMF 22.20(ii) */ + a[0] = 1.0; + b = sqrt(1.0 - m); + c[0] = sqrt(m); + twon = 1.0; + i = 0; + + while (fabs(c[i] / a[i]) > MACHEP) { + if (i > 7) { + sf_error("ellpj", SF_ERROR_OVERFLOW, NULL); + goto done; + } + ai = a[i]; + ++i; + c[i] = (ai - b) / 2.0; + t = sqrt(ai * b); + a[i] = (ai + b) / 2.0; + b = t; + twon *= 2.0; + } + + done: + /* backward recurrence */ + phi = twon * a[i] * u; + do { + t = c[i] * sin(phi) / a[i]; + b = phi; + phi = (asin(t) + phi) / 2.0; + } + while (--i); + + *sn = sin(phi); + t = cos(phi); + *cn = t; + dnfac = cos(phi - b); + /* See discussion after DLMF 22.20.5 */ + if (fabs(dnfac) < 0.1) { + *dn = sqrt(1 - m*(*sn)*(*sn)); + } + else { + *dn = t / dnfac; + } + *ph = phi; + return (0); +} diff --git a/gtsam/3rdparty/cephes/cephes/ellpk.c b/gtsam/3rdparty/cephes/cephes/ellpk.c new file mode 100644 index 0000000000..3842a7403a --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellpk.c @@ -0,0 +1,124 @@ +/* ellpk.c + * + * Complete elliptic integral of the first kind + * + * + * + * SYNOPSIS: + * + * double m1, y, ellpk(); + * + * y = ellpk( m1 ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * + * + * pi/2 + * - + * | | + * | dt + * K(m) = | ------------------ + * | 2 + * | | sqrt( 1 - m sin t ) + * - + * 0 + * + * where m = 1 - m1, using the approximation + * + * P(x) - log x Q(x). + * + * The argument m1 is used internally rather than m so that the logarithmic + * singularity at m = 1 will be shifted to the origin; this + * preserves maximum accuracy. + * + * K(0) = pi/2. + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,1 30000 2.5e-16 6.8e-17 + * + * ERROR MESSAGES: + * + * message condition value returned + * ellpk domain x<0, x>1 0.0 + * + */ + +/* ellpk.c */ + + +/* + * Cephes Math Library, Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +static double P[] = { + 1.37982864606273237150E-4, + 2.28025724005875567385E-3, + 7.97404013220415179367E-3, + 9.85821379021226008714E-3, + 6.87489687449949877925E-3, + 6.18901033637687613229E-3, + 8.79078273952743772254E-3, + 1.49380448916805252718E-2, + 3.08851465246711995998E-2, + 9.65735902811690126535E-2, + 1.38629436111989062502E0 +}; + +static double Q[] = { + 2.94078955048598507511E-5, + 9.14184723865917226571E-4, + 5.94058303753167793257E-3, + 1.54850516649762399335E-2, + 2.39089602715924892727E-2, + 3.01204715227604046988E-2, + 3.73774314173823228969E-2, + 4.88280347570998239232E-2, + 7.03124996963957469739E-2, + 1.24999999999870820058E-1, + 4.99999999999999999821E-1 +}; + +static double C1 = 1.3862943611198906188E0; /* log(4) */ + +extern double MACHEP; + +double ellpk(double x) +{ + + if (x < 0.0) { + sf_error("ellpk", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + if (x > 1.0) { + if (cephes_isinf(x)) { + return 0.0; + } + return ellpk(1/x)/sqrt(x); + } + + if (x > MACHEP) { + return (polevl(x, P, 10) - log(x) * polevl(x, Q, 10)); + } + else { + if (x == 0.0) { + sf_error("ellpk", SF_ERROR_SINGULAR, NULL); + return (INFINITY); + } + else { + return (C1 - 0.5 * log(x)); + } + } +} diff --git a/gtsam/3rdparty/cephes/cephes/erfinv.c b/gtsam/3rdparty/cephes/cephes/erfinv.c new file mode 100644 index 0000000000..f7f49284c1 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/erfinv.c @@ -0,0 +1,78 @@ +/* + * mconf configures NANS, INFINITYs etc. for cephes and includes some standard + * headers. Although erfinv and erfcinv are not defined in cephes, erf and erfc + * are. We want to keep the behaviour consistent for the inverse functions and + * so need to include mconf. + */ +#include "mconf.h" + +/* + * Inverse of the error function. + * + * Computes the inverse of the error function on the restricted domain + * -1 < y < 1. This restriction ensures the existence of a unique result + * such that erf(erfinv(y)) = y. + */ +double erfinv(double y) { + const double domain_lb = -1; + const double domain_ub = 1; + + const double thresh = 1e-7; + + /* + * For small arguments, use the Taylor expansion + * erf(y) = 2/\sqrt{\pi} (y - y^3 / 3 + O(y^5)), y\to 0 + * where we only retain the linear term. + * Otherwise, y + 1 loses precision for |y| << 1. + */ + if ((-thresh < y) && (y < thresh)){ + return y / M_2_SQRTPI; + } + if ((domain_lb < y) && (y < domain_ub)) { + return ndtri(0.5 * (y+1)) * M_SQRT1_2; + } + else if (y == domain_lb) { + return -INFINITY; + } + else if (y == domain_ub) { + return INFINITY; + } + else if (cephes_isnan(y)) { + sf_error("erfinv", SF_ERROR_DOMAIN, NULL); + return y; + } + else { + sf_error("erfinv", SF_ERROR_DOMAIN, NULL); + return NAN; + } +} + +/* + * Inverse of the complementary error function. + * + * Computes the inverse of the complimentary error function on the restricted + * domain 0 < y < 2. This restriction ensures the existence of a unique result + * such that erfc(erfcinv(y)) = y. + */ +double erfcinv(double y) { + const double domain_lb = 0; + const double domain_ub = 2; + + if ((domain_lb < y) && (y < domain_ub)) { + return -ndtri(0.5 * y) * M_SQRT1_2; + } + else if (y == domain_lb) { + return INFINITY; + } + else if (y == domain_ub) { + return -INFINITY; + } + else if (cephes_isnan(y)) { + sf_error("erfcinv", SF_ERROR_DOMAIN, NULL); + return y; + } + else { + sf_error("erfcinv", SF_ERROR_DOMAIN, NULL); + return NAN; + } +} diff --git a/gtsam/3rdparty/cephes/cephes/exp10.c b/gtsam/3rdparty/cephes/cephes/exp10.c new file mode 100644 index 0000000000..0a71d3c52f --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/exp10.c @@ -0,0 +1,115 @@ +/* exp10.c + * + * Base 10 exponential function + * (Common antilogarithm) + * + * + * + * SYNOPSIS: + * + * double x, y, exp10(); + * + * y = exp10( x ); + * + * + * + * DESCRIPTION: + * + * Returns 10 raised to the x power. + * + * Range reduction is accomplished by expressing the argument + * as 10**x = 2**n 10**f, with |f| < 0.5 log10(2). + * The Pade' form + * + * 1 + 2x P(x**2)/( Q(x**2) - P(x**2) ) + * + * is used to approximate 10**f. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -307,+307 30000 2.2e-16 5.5e-17 + * + * ERROR MESSAGES: + * + * message condition value returned + * exp10 underflow x < -MAXL10 0.0 + * exp10 overflow x > MAXL10 INFINITY + * + * IEEE arithmetic: MAXL10 = 308.2547155599167. + * + */ + +/* + * Cephes Math Library Release 2.2: January, 1991 + * Copyright 1984, 1991 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + + +#include "mconf.h" + +static double P[] = { + 4.09962519798587023075E-2, + 1.17452732554344059015E1, + 4.06717289936872725516E2, + 2.39423741207388267439E3, +}; + +static double Q[] = { + /* 1.00000000000000000000E0, */ + 8.50936160849306532625E1, + 1.27209271178345121210E3, + 2.07960819286001865907E3, +}; + +/* static double LOG102 = 3.01029995663981195214e-1; */ +static double LOG210 = 3.32192809488736234787e0; +static double LG102A = 3.01025390625000000000E-1; +static double LG102B = 4.60503898119521373889E-6; + +/* static double MAXL10 = 38.230809449325611792; */ +static double MAXL10 = 308.2547155599167; + +double exp10(double x) +{ + double px, xx; + short n; + + if (cephes_isnan(x)) + return (x); + if (x > MAXL10) { + return (INFINITY); + } + + if (x < -MAXL10) { /* Would like to use MINLOG but can't */ + sf_error("exp10", SF_ERROR_UNDERFLOW, NULL); + return (0.0); + } + + /* Express 10**x = 10**g 2**n + * = 10**g 10**( n log10(2) ) + * = 10**( g + n log10(2) ) + */ + px = floor(LOG210 * x + 0.5); + n = px; + x -= px * LG102A; + x -= px * LG102B; + + /* rational approximation for exponential + * of the fractional part: + * 10**x = 1 + 2x P(x**2)/( Q(x**2) - P(x**2) ) + */ + xx = x * x; + px = x * polevl(xx, P, 3); + x = px / (p1evl(xx, Q, 3) - px); + x = 1.0 + ldexp(x, 1); + + /* multiply by power of 2 */ + x = ldexp(x, n); + + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/exp2.c b/gtsam/3rdparty/cephes/cephes/exp2.c new file mode 100644 index 0000000000..14911f59c0 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/exp2.c @@ -0,0 +1,108 @@ +/* exp2.c + * + * Base 2 exponential function + * + * + * + * SYNOPSIS: + * + * double x, y, exp2(); + * + * y = exp2( x ); + * + * + * + * DESCRIPTION: + * + * Returns 2 raised to the x power. + * + * Range reduction is accomplished by separating the argument + * into an integer k and fraction f such that + * x k f + * 2 = 2 2. + * + * A Pade' form + * + * 1 + 2x P(x**2) / (Q(x**2) - x P(x**2) ) + * + * approximates 2**x in the basic range [-0.5, 0.5]. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -1022,+1024 30000 1.8e-16 5.4e-17 + * + * + * See exp.c for comments on error amplification. + * + * + * ERROR MESSAGES: + * + * message condition value returned + * exp underflow x < -MAXL2 0.0 + * exp overflow x > MAXL2 INFINITY + * + * For IEEE arithmetic, MAXL2 = 1024. + */ + + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1995 by Stephen L. Moshier + */ + + + +#include "mconf.h" + +static double P[] = { + 2.30933477057345225087E-2, + 2.02020656693165307700E1, + 1.51390680115615096133E3, +}; + +static double Q[] = { + /* 1.00000000000000000000E0, */ + 2.33184211722314911771E2, + 4.36821166879210612817E3, +}; + +#define MAXL2 1024.0 +#define MINL2 -1024.0 + +double exp2(double x) +{ + double px, xx; + short n; + + if (cephes_isnan(x)) + return (x); + if (x > MAXL2) { + return (INFINITY); + } + + if (x < MINL2) { + return (0.0); + } + + xx = x; /* save x */ + /* separate into integer and fractional parts */ + px = floor(x + 0.5); + n = px; + x = x - px; + + /* rational approximation + * exp2(x) = 1 + 2xP(xx)/(Q(xx) - P(xx)) + * where xx = x**2 + */ + xx = x * x; + px = x * polevl(xx, P, 2); + x = px / (p1evl(xx, Q, 2) - px); + x = 1.0 + ldexp(x, 1); + + /* scale by power of 2 */ + x = ldexp(x, n); + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/expn.c b/gtsam/3rdparty/cephes/cephes/expn.c new file mode 100644 index 0000000000..2a6ee14c09 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/expn.c @@ -0,0 +1,224 @@ +/* expn.c + * + * Exponential integral En + * + * + * + * SYNOPSIS: + * + * int n; + * double x, y, expn(); + * + * y = expn( n, x ); + * + * + * + * DESCRIPTION: + * + * Evaluates the exponential integral + * + * inf. + * - + * | | -xt + * | e + * E (x) = | ---- dt. + * n | n + * | | t + * - + * 1 + * + * + * Both n and x must be nonnegative. + * + * The routine employs either a power series, a continued + * fraction, or an asymptotic formula depending on the + * relative values of n and x. + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 10000 1.7e-15 3.6e-16 + * + */ + +/* expn.c */ + +/* Cephes Math Library Release 1.1: March, 1985 + * Copyright 1985 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 */ + +/* Sources + * [1] NIST, "The Digital Library of Mathematical Functions", dlmf.nist.gov + */ + +/* Scipy changes: + * - 09-10-2016: improved asymptotic expansion for large n + */ + +#include "mconf.h" +#include "polevl.h" +#include "expn.h" + +#define EUL 0.57721566490153286060 +#define BIG 1.44115188075855872E+17 +extern double MACHEP, MAXLOG; + +static double expn_large_n(int, double); + + +double expn(int n, double x) +{ + double ans, r, t, yk, xk; + double pk, pkm1, pkm2, qk, qkm1, qkm2; + double psi, z; + int i, k; + static double big = BIG; + + if (isnan(x)) { + return NAN; + } + else if (n < 0 || x < 0) { + sf_error("expn", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x > MAXLOG) { + return (0.0); + } + + if (x == 0.0) { + if (n < 2) { + sf_error("expn", SF_ERROR_SINGULAR, NULL); + return (INFINITY); + } + else { + return (1.0 / (n - 1.0)); + } + } + + if (n == 0) { + return (exp(-x) / x); + } + + /* Asymptotic expansion for large n, DLMF 8.20(ii) */ + if (n > 50) { + ans = expn_large_n(n, x); + goto done; + } + + if (x > 1.0) { + goto cfrac; + } + + /* Power series expansion, DLMF 8.19.8 */ + psi = -EUL - log(x); + for (i = 1; i < n; i++) { + psi = psi + 1.0 / i; + } + + z = -x; + xk = 0.0; + yk = 1.0; + pk = 1.0 - n; + if (n == 1) { + ans = 0.0; + } else { + ans = 1.0 / pk; + } + do { + xk += 1.0; + yk *= z / xk; + pk += 1.0; + if (pk != 0.0) { + ans += yk / pk; + } + if (ans != 0.0) + t = fabs(yk / ans); + else + t = 1.0; + } while (t > MACHEP); + k = xk; + t = n; + r = n - 1; + ans = (pow(z, r) * psi / Gamma(t)) - ans; + goto done; + + /* Continued fraction, DLMF 8.19.17 */ + cfrac: + k = 1; + pkm2 = 1.0; + qkm2 = x; + pkm1 = 1.0; + qkm1 = x + n; + ans = pkm1 / qkm1; + + do { + k += 1; + if (k & 1) { + yk = 1.0; + xk = n + (k - 1) / 2; + } else { + yk = x; + xk = k / 2; + } + pk = pkm1 * yk + pkm2 * xk; + qk = qkm1 * yk + qkm2 * xk; + if (qk != 0) { + r = pk / qk; + t = fabs((ans - r) / r); + ans = r; + } else { + t = 1.0; + } + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + if (fabs(pk) > big) { + pkm2 /= big; + pkm1 /= big; + qkm2 /= big; + qkm1 /= big; + } + } while (t > MACHEP); + + ans *= exp(-x); + + done: + return (ans); +} + + +/* Asymptotic expansion for large n, DLMF 8.20(ii) */ +static double expn_large_n(int n, double x) +{ + int k; + double p = n; + double lambda = x/p; + double multiplier = 1/p/(lambda + 1)/(lambda + 1); + double fac = 1; + double res = 1; /* A[0] = 1 */ + double expfac, term; + + expfac = exp(-lambda*p)/(lambda + 1)/p; + if (expfac == 0) { + sf_error("expn", SF_ERROR_UNDERFLOW, NULL); + return 0; + } + + /* Do the k = 1 term outside the loop since A[1] = 1 */ + fac *= multiplier; + res += fac; + + for (k = 2; k < nA; k++) { + fac *= multiplier; + term = fac*polevl(lambda, A[k], Adegs[k]); + res += term; + if (fabs(term) < MACHEP*fabs(res)) { + break; + } + } + + return expfac*res; +} diff --git a/gtsam/3rdparty/cephes/cephes/expn.h b/gtsam/3rdparty/cephes/cephes/expn.h new file mode 100644 index 0000000000..8ced026877 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/expn.h @@ -0,0 +1,19 @@ +/* This file was automatically generated by _precompute/expn_asy.py. + * Do not edit it manually! + */ +#define nA 13 +static const double A0[] = {1.00000000000000000}; +static const double A1[] = {1.00000000000000000}; +static const double A2[] = {-2.00000000000000000, 1.00000000000000000}; +static const double A3[] = {6.00000000000000000, -8.00000000000000000, 1.00000000000000000}; +static const double A4[] = {-24.0000000000000000, 58.0000000000000000, -22.0000000000000000, 1.00000000000000000}; +static const double A5[] = {120.000000000000000, -444.000000000000000, 328.000000000000000, -52.0000000000000000, 1.00000000000000000}; +static const double A6[] = {-720.000000000000000, 3708.00000000000000, -4400.00000000000000, 1452.00000000000000, -114.000000000000000, 1.00000000000000000}; +static const double A7[] = {5040.00000000000000, -33984.0000000000000, 58140.0000000000000, -32120.0000000000000, 5610.00000000000000, -240.000000000000000, 1.00000000000000000}; +static const double A8[] = {-40320.0000000000000, 341136.000000000000, -785304.000000000000, 644020.000000000000, -195800.000000000000, 19950.0000000000000, -494.000000000000000, 1.00000000000000000}; +static const double A9[] = {362880.000000000000, -3733920.00000000000, 11026296.0000000000, -12440064.0000000000, 5765500.00000000000, -1062500.00000000000, 67260.0000000000000, -1004.00000000000000, 1.00000000000000000}; +static const double A10[] = {-3628800.00000000000, 44339040.0000000000, -162186912.000000000, 238904904.000000000, -155357384.000000000, 44765000.0000000000, -5326160.00000000000, 218848.000000000000, -2026.00000000000000, 1.00000000000000000}; +static const double A11[] = {39916800.0000000000, -568356480.000000000, 2507481216.00000000, -4642163952.00000000, 4002695088.00000000, -1648384304.00000000, 314369720.000000000, -25243904.0000000000, 695038.000000000000, -4072.00000000000000, 1.00000000000000000}; +static const double A12[] = {-479001600.000000000, 7827719040.00000000, -40788301824.0000000, 92199790224.0000000, -101180433024.000000, 56041398784.0000000, -15548960784.0000000, 2051482776.00000000, -114876376.000000000, 2170626.00000000000, -8166.00000000000000, 1.00000000000000000}; +static const double *A[] = {A0, A1, A2, A3, A4, A5, A6, A7, A8, A9, A10, A11, A12}; +static const int Adegs[] = {0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}; diff --git a/gtsam/3rdparty/cephes/cephes/fdtr.c b/gtsam/3rdparty/cephes/cephes/fdtr.c new file mode 100644 index 0000000000..9c119ed8f7 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/fdtr.c @@ -0,0 +1,216 @@ +/* fdtr.c + * + * F distribution + * + * + * + * SYNOPSIS: + * + * double df1, df2; + * double x, y, fdtr(); + * + * y = fdtr( df1, df2, x ); + * + * DESCRIPTION: + * + * Returns the area from zero to x under the F density + * function (also known as Snedcor's density or the + * variance ratio density). This is the density + * of x = (u1/df1)/(u2/df2), where u1 and u2 are random + * variables having Chi square distributions with df1 + * and df2 degrees of freedom, respectively. + * + * The incomplete beta integral is used, according to the + * formula + * + * P(x) = incbet( df1/2, df2/2, (df1*x/(df2 + df1*x) ). + * + * + * The arguments a and b are greater than zero, and x is + * nonnegative. + * + * ACCURACY: + * + * Tested at random points (a,b,x). + * + * x a,b Relative error: + * arithmetic domain domain # trials peak rms + * IEEE 0,1 0,100 100000 9.8e-15 1.7e-15 + * IEEE 1,5 0,100 100000 6.5e-15 3.5e-16 + * IEEE 0,1 1,10000 100000 2.2e-11 3.3e-12 + * IEEE 1,5 1,10000 100000 1.1e-11 1.7e-13 + * See also incbet.c. + * + * + * ERROR MESSAGES: + * + * message condition value returned + * fdtr domain a<0, b<0, x<0 0.0 + * + */ + +/* fdtrc() + * + * Complemented F distribution + * + * + * + * SYNOPSIS: + * + * double df1, df2; + * double x, y, fdtrc(); + * + * y = fdtrc( df1, df2, x ); + * + * DESCRIPTION: + * + * Returns the area from x to infinity under the F density + * function (also known as Snedcor's density or the + * variance ratio density). + * + * + * inf. + * - + * 1 | | a-1 b-1 + * 1-P(x) = ------ | t (1-t) dt + * B(a,b) | | + * - + * x + * + * + * The incomplete beta integral is used, according to the + * formula + * + * P(x) = incbet( df2/2, df1/2, (df2/(df2 + df1*x) ). + * + * + * ACCURACY: + * + * Tested at random points (a,b,x) in the indicated intervals. + * x a,b Relative error: + * arithmetic domain domain # trials peak rms + * IEEE 0,1 1,100 100000 3.7e-14 5.9e-16 + * IEEE 1,5 1,100 100000 8.0e-15 1.6e-15 + * IEEE 0,1 1,10000 100000 1.8e-11 3.5e-13 + * IEEE 1,5 1,10000 100000 2.0e-11 3.0e-12 + * See also incbet.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * fdtrc domain a<0, b<0, x<0 0.0 + * + */ + +/* fdtri() + * + * Inverse of F distribution + * + * + * + * SYNOPSIS: + * + * double df1, df2; + * double x, p, fdtri(); + * + * x = fdtri( df1, df2, p ); + * + * DESCRIPTION: + * + * Finds the F density argument x such that the integral + * from -infinity to x of the F density is equal to the + * given probability p. + * + * This is accomplished using the inverse beta integral + * function and the relations + * + * z = incbi( df2/2, df1/2, p ) + * x = df2 (1-z) / (df1 z). + * + * Note: the following relations hold for the inverse of + * the uncomplemented F distribution: + * + * z = incbi( df1/2, df2/2, p ) + * x = df2 z / (df1 (1-z)). + * + * ACCURACY: + * + * Tested at random points (a,b,p). + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * For p between .001 and 1: + * IEEE 1,100 100000 8.3e-15 4.7e-16 + * IEEE 1,10000 100000 2.1e-11 1.4e-13 + * For p between 10^-6 and 10^-3: + * IEEE 1,100 50000 1.3e-12 8.4e-15 + * IEEE 1,10000 50000 3.0e-12 4.8e-14 + * See also fdtrc.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * fdtri domain p <= 0 or p > 1 NaN + * v < 1 + * + */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + + +#include "mconf.h" + + +double fdtrc(double a, double b, double x) +{ + double w; + + if ((a <= 0.0) || (b <= 0.0) || (x < 0.0)) { + sf_error("fdtrc", SF_ERROR_DOMAIN, NULL); + return NAN; + } + w = b / (b + a * x); + return incbet(0.5 * b, 0.5 * a, w); +} + + +double fdtr(double a, double b, double x) +{ + double w; + + if ((a <= 0.0) || (b <= 0.0) || (x < 0.0)) { + sf_error("fdtr", SF_ERROR_DOMAIN, NULL); + return NAN; + } + w = a * x; + w = w / (b + w); + return incbet(0.5 * a, 0.5 * b, w); +} + + +double fdtri(double a, double b, double y) +{ + double w, x; + + if ((a <= 0.0) || (b <= 0.0) || (y <= 0.0) || (y > 1.0)) { + sf_error("fdtri", SF_ERROR_DOMAIN, NULL); + return NAN; + } + y = 1.0 - y; + /* Compute probability for x = 0.5. */ + w = incbet(0.5 * b, 0.5 * a, 0.5); + /* If that is greater than y, then the solution w < .5. + * Otherwise, solve at 1-y to remove cancellation in (b - b*w). */ + if (w > y || y < 0.001) { + w = incbi(0.5 * b, 0.5 * a, y); + x = (b - b * w) / (a * w); + } + else { + w = incbi(0.5 * a, 0.5 * b, 1.0 - y); + x = b * w / (a * (1.0 - w)); + } + return x; +} diff --git a/gtsam/3rdparty/cephes/cephes/fresnl.c b/gtsam/3rdparty/cephes/cephes/fresnl.c new file mode 100644 index 0000000000..50620fa2e1 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/fresnl.c @@ -0,0 +1,219 @@ +/* fresnl.c + * + * Fresnel integral + * + * + * + * SYNOPSIS: + * + * double x, S, C; + * void fresnl(); + * + * fresnl( x, _&S, _&C ); + * + * + * DESCRIPTION: + * + * Evaluates the Fresnel integrals + * + * x + * - + * | | + * C(x) = | cos(pi/2 t**2) dt, + * | | + * - + * 0 + * + * x + * - + * | | + * S(x) = | sin(pi/2 t**2) dt. + * | | + * - + * 0 + * + * + * The integrals are evaluated by a power series for x < 1. + * For x >= 1 auxiliary functions f(x) and g(x) are employed + * such that + * + * C(x) = 0.5 + f(x) sin( pi/2 x**2 ) - g(x) cos( pi/2 x**2 ) + * S(x) = 0.5 - f(x) cos( pi/2 x**2 ) - g(x) sin( pi/2 x**2 ) + * + * + * + * ACCURACY: + * + * Relative error. + * + * Arithmetic function domain # trials peak rms + * IEEE S(x) 0, 10 10000 2.0e-15 3.2e-16 + * IEEE C(x) 0, 10 10000 1.8e-15 3.3e-16 + */ + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +/* S(x) for small x */ +static double sn[6] = { + -2.99181919401019853726E3, + 7.08840045257738576863E5, + -6.29741486205862506537E7, + 2.54890880573376359104E9, + -4.42979518059697779103E10, + 3.18016297876567817986E11, +}; + +static double sd[6] = { + /* 1.00000000000000000000E0, */ + 2.81376268889994315696E2, + 4.55847810806532581675E4, + 5.17343888770096400730E6, + 4.19320245898111231129E8, + 2.24411795645340920940E10, + 6.07366389490084639049E11, +}; + +/* C(x) for small x */ +static double cn[6] = { + -4.98843114573573548651E-8, + 9.50428062829859605134E-6, + -6.45191435683965050962E-4, + 1.88843319396703850064E-2, + -2.05525900955013891793E-1, + 9.99999999999999998822E-1, +}; + +static double cd[7] = { + 3.99982968972495980367E-12, + 9.15439215774657478799E-10, + 1.25001862479598821474E-7, + 1.22262789024179030997E-5, + 8.68029542941784300606E-4, + 4.12142090722199792936E-2, + 1.00000000000000000118E0, +}; + +/* Auxiliary function f(x) */ +static double fn[10] = { + 4.21543555043677546506E-1, + 1.43407919780758885261E-1, + 1.15220955073585758835E-2, + 3.45017939782574027900E-4, + 4.63613749287867322088E-6, + 3.05568983790257605827E-8, + 1.02304514164907233465E-10, + 1.72010743268161828879E-13, + 1.34283276233062758925E-16, + 3.76329711269987889006E-20, +}; + +static double fd[10] = { + /* 1.00000000000000000000E0, */ + 7.51586398353378947175E-1, + 1.16888925859191382142E-1, + 6.44051526508858611005E-3, + 1.55934409164153020873E-4, + 1.84627567348930545870E-6, + 1.12699224763999035261E-8, + 3.60140029589371370404E-11, + 5.88754533621578410010E-14, + 4.52001434074129701496E-17, + 1.25443237090011264384E-20, +}; + +/* Auxiliary function g(x) */ +static double gn[11] = { + 5.04442073643383265887E-1, + 1.97102833525523411709E-1, + 1.87648584092575249293E-2, + 6.84079380915393090172E-4, + 1.15138826111884280931E-5, + 9.82852443688422223854E-8, + 4.45344415861750144738E-10, + 1.08268041139020870318E-12, + 1.37555460633261799868E-15, + 8.36354435630677421531E-19, + 1.86958710162783235106E-22, +}; + +static double gd[11] = { + /* 1.00000000000000000000E0, */ + 1.47495759925128324529E0, + 3.37748989120019970451E-1, + 2.53603741420338795122E-2, + 8.14679107184306179049E-4, + 1.27545075667729118702E-5, + 1.04314589657571990585E-7, + 4.60680728146520428211E-10, + 1.10273215066240270757E-12, + 1.38796531259578871258E-15, + 8.39158816283118707363E-19, + 1.86958710162783236342E-22, +}; + +extern double MACHEP; + +int fresnl(double xxa, double *ssa, double *cca) +{ + double f, g, cc, ss, c, s, t, u; + double x, x2; + + if (cephes_isinf(xxa)) { + cc = 0.5; + ss = 0.5; + goto done; + } + + x = fabs(xxa); + x2 = x * x; + if (x2 < 2.5625) { + t = x2 * x2; + ss = x * x2 * polevl(t, sn, 5) / p1evl(t, sd, 6); + cc = x * polevl(t, cn, 5) / polevl(t, cd, 6); + goto done; + } + + if (x > 36974.0) { + /* + * http://functions.wolfram.com/GammaBetaErf/FresnelC/06/02/ + * http://functions.wolfram.com/GammaBetaErf/FresnelS/06/02/ + */ + cc = 0.5 + 1/(M_PI*x) * sin(M_PI*x*x/2); + ss = 0.5 - 1/(M_PI*x) * cos(M_PI*x*x/2); + goto done; + } + + + /* Asymptotic power series auxiliary functions + * for large argument + */ + x2 = x * x; + t = M_PI * x2; + u = 1.0 / (t * t); + t = 1.0 / t; + f = 1.0 - u * polevl(u, fn, 9) / p1evl(u, fd, 10); + g = t * polevl(u, gn, 10) / p1evl(u, gd, 11); + + t = M_PI_2 * x2; + c = cos(t); + s = sin(t); + t = M_PI * x; + cc = 0.5 + (f * s - g * c) / t; + ss = 0.5 - (f * c + g * s) / t; + + done: + if (xxa < 0.0) { + cc = -cc; + ss = -ss; + } + + *cca = cc; + *ssa = ss; + return (0); +} diff --git a/gtsam/3rdparty/cephes/cephes/gamma.c b/gtsam/3rdparty/cephes/cephes/gamma.c new file mode 100644 index 0000000000..2a61defedb --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/gamma.c @@ -0,0 +1,364 @@ +/* + * Gamma function + * + * + * + * SYNOPSIS: + * + * double x, y, Gamma(); + * + * y = Gamma( x ); + * + * + * + * DESCRIPTION: + * + * Returns Gamma function of the argument. The result is + * correctly signed. + * + * Arguments |x| <= 34 are reduced by recurrence and the function + * approximated by a rational function of degree 6/7 in the + * interval (2,3). Large arguments are handled by Stirling's + * formula. Large negative arguments are made positive using + * a reflection formula. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -170,-33 20000 2.3e-15 3.3e-16 + * IEEE -33, 33 20000 9.4e-16 2.2e-16 + * IEEE 33, 171.6 20000 2.3e-15 3.2e-16 + * + * Error for arguments outside the test range will be larger + * owing to error amplification by the exponential function. + * + */ + +/* lgam() + * + * Natural logarithm of Gamma function + * + * + * + * SYNOPSIS: + * + * double x, y, lgam(); + * + * y = lgam( x ); + * + * + * + * DESCRIPTION: + * + * Returns the base e (2.718...) logarithm of the absolute + * value of the Gamma function of the argument. + * + * For arguments greater than 13, the logarithm of the Gamma + * function is approximated by the logarithmic version of + * Stirling's formula using a polynomial approximation of + * degree 4. Arguments between -33 and +33 are reduced by + * recurrence to the interval [2,3] of a rational approximation. + * The cosecant reflection formula is employed for arguments + * less than -33. + * + * Arguments greater than MAXLGM return INFINITY and an error + * message. MAXLGM = 2.556348e305 for IEEE arithmetic. + * + * + * + * ACCURACY: + * + * + * arithmetic domain # trials peak rms + * IEEE 0, 3 28000 5.4e-16 1.1e-16 + * IEEE 2.718, 2.556e305 40000 3.5e-16 8.3e-17 + * The error criterion was relative when the function magnitude + * was greater than one but absolute when it was less than one. + * + * The following test used the relative error criterion, though + * at certain points the relative error could be much higher than + * indicated. + * IEEE -200, -4 10000 4.8e-16 1.3e-16 + * + */ + +/* + * Cephes Math Library Release 2.2: July, 1992 + * Copyright 1984, 1987, 1989, 1992 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + + +#include "mconf.h" + +static double P[] = { + 1.60119522476751861407E-4, + 1.19135147006586384913E-3, + 1.04213797561761569935E-2, + 4.76367800457137231464E-2, + 2.07448227648435975150E-1, + 4.94214826801497100753E-1, + 9.99999999999999996796E-1 +}; + +static double Q[] = { + -2.31581873324120129819E-5, + 5.39605580493303397842E-4, + -4.45641913851797240494E-3, + 1.18139785222060435552E-2, + 3.58236398605498653373E-2, + -2.34591795718243348568E-1, + 7.14304917030273074085E-2, + 1.00000000000000000320E0 +}; + +#define MAXGAM 171.624376956302725 +static double LOGPI = 1.14472988584940017414; + +/* Stirling's formula for the Gamma function */ +static double STIR[5] = { + 7.87311395793093628397E-4, + -2.29549961613378126380E-4, + -2.68132617805781232825E-3, + 3.47222221605458667310E-3, + 8.33333333333482257126E-2, +}; + +#define MAXSTIR 143.01608 +static double SQTPI = 2.50662827463100050242E0; + +extern double MAXLOG; +static double stirf(double); + +/* Gamma function computed by Stirling's formula. + * The polynomial STIR is valid for 33 <= x <= 172. + */ +static double stirf(double x) +{ + double y, w, v; + + if (x >= MAXGAM) { + return (INFINITY); + } + w = 1.0 / x; + w = 1.0 + w * polevl(w, STIR, 4); + y = exp(x); + if (x > MAXSTIR) { /* Avoid overflow in pow() */ + v = pow(x, 0.5 * x - 0.25); + y = v * (v / y); + } + else { + y = pow(x, x - 0.5) / y; + } + y = SQTPI * y * w; + return (y); +} + + +double Gamma(double x) +{ + double p, q, z; + int i; + int sgngam = 1; + + if (!cephes_isfinite(x)) { + return x; + } + q = fabs(x); + + if (q > 33.0) { + if (x < 0.0) { + p = floor(q); + if (p == q) { + gamnan: + sf_error("Gamma", SF_ERROR_OVERFLOW, NULL); + return (INFINITY); + } + i = p; + if ((i & 1) == 0) + sgngam = -1; + z = q - p; + if (z > 0.5) { + p += 1.0; + z = q - p; + } + z = q * sin(M_PI * z); + if (z == 0.0) { + return (sgngam * INFINITY); + } + z = fabs(z); + z = M_PI / (z * stirf(q)); + } + else { + z = stirf(x); + } + return (sgngam * z); + } + + z = 1.0; + while (x >= 3.0) { + x -= 1.0; + z *= x; + } + + while (x < 0.0) { + if (x > -1.E-9) + goto small; + z /= x; + x += 1.0; + } + + while (x < 2.0) { + if (x < 1.e-9) + goto small; + z /= x; + x += 1.0; + } + + if (x == 2.0) + return (z); + + x -= 2.0; + p = polevl(x, P, 6); + q = polevl(x, Q, 7); + return (z * p / q); + + small: + if (x == 0.0) { + goto gamnan; + } + else + return (z / ((1.0 + 0.5772156649015329 * x) * x)); +} + + + +/* A[]: Stirling's formula expansion of log Gamma + * B[], C[]: log Gamma function between 2 and 3 + */ +static double A[] = { + 8.11614167470508450300E-4, + -5.95061904284301438324E-4, + 7.93650340457716943945E-4, + -2.77777777730099687205E-3, + 8.33333333333331927722E-2 +}; + +static double B[] = { + -1.37825152569120859100E3, + -3.88016315134637840924E4, + -3.31612992738871184744E5, + -1.16237097492762307383E6, + -1.72173700820839662146E6, + -8.53555664245765465627E5 +}; + +static double C[] = { + /* 1.00000000000000000000E0, */ + -3.51815701436523470549E2, + -1.70642106651881159223E4, + -2.20528590553854454839E5, + -1.13933444367982507207E6, + -2.53252307177582951285E6, + -2.01889141433532773231E6 +}; + +/* log( sqrt( 2*pi ) ) */ +static double LS2PI = 0.91893853320467274178; + +#define MAXLGM 2.556348e305 + + +/* Logarithm of Gamma function */ +double lgam(double x) +{ + int sign; + return lgam_sgn(x, &sign); +} + +double lgam_sgn(double x, int *sign) +{ + double p, q, u, w, z; + int i; + + *sign = 1; + + if (!cephes_isfinite(x)) + return x; + + if (x < -34.0) { + q = -x; + w = lgam_sgn(q, sign); + p = floor(q); + if (p == q) { + lgsing: + sf_error("lgam", SF_ERROR_SINGULAR, NULL); + return (INFINITY); + } + i = p; + if ((i & 1) == 0) + *sign = -1; + else + *sign = 1; + z = q - p; + if (z > 0.5) { + p += 1.0; + z = p - q; + } + z = q * sin(M_PI * z); + if (z == 0.0) + goto lgsing; + /* z = log(M_PI) - log( z ) - w; */ + z = LOGPI - log(z) - w; + return (z); + } + + if (x < 13.0) { + z = 1.0; + p = 0.0; + u = x; + while (u >= 3.0) { + p -= 1.0; + u = x + p; + z *= u; + } + while (u < 2.0) { + if (u == 0.0) + goto lgsing; + z /= u; + p += 1.0; + u = x + p; + } + if (z < 0.0) { + *sign = -1; + z = -z; + } + else + *sign = 1; + if (u == 2.0) + return (log(z)); + p -= 2.0; + x = x + p; + p = x * polevl(x, B, 5) / p1evl(x, C, 6); + return (log(z) + p); + } + + if (x > MAXLGM) { + return (*sign * INFINITY); + } + + q = (x - 0.5) * log(x) - x + LS2PI; + if (x > 1.0e8) + return (q); + + p = 1.0 / (x * x); + if (x >= 1000.0) + q += ((7.9365079365079365079365e-4 * p + - 2.7777777777777777777778e-3) * p + + 0.0833333333333333333333) / x; + else + q += polevl(p, A, 4) / x; + return (q); +} diff --git a/gtsam/3rdparty/cephes/cephes/gammasgn.c b/gtsam/3rdparty/cephes/cephes/gammasgn.c new file mode 100644 index 0000000000..9d74318ff2 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/gammasgn.c @@ -0,0 +1,25 @@ +#include "mconf.h" + +double gammasgn(double x) +{ + double fx; + + if (isnan(x)) { + return x; + } + if (x > 0) { + return 1.0; + } + else { + fx = floor(x); + if (x - fx == 0.0) { + return 0.0; + } + else if ((int)fx % 2) { + return -1.0; + } + else { + return 1.0; + } + } +} diff --git a/gtsam/3rdparty/cephes/cephes/gdtr.c b/gtsam/3rdparty/cephes/cephes/gdtr.c new file mode 100644 index 0000000000..597c8d4d93 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/gdtr.c @@ -0,0 +1,132 @@ +/* gdtr.c + * + * Gamma distribution function + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, gdtr(); + * + * y = gdtr( a, b, x ); + * + * + * + * DESCRIPTION: + * + * Returns the integral from zero to x of the Gamma probability + * density function: + * + * + * x + * b - + * a | | b-1 -at + * y = ----- | t e dt + * - | | + * | (b) - + * 0 + * + * The incomplete Gamma integral is used, according to the + * relation + * + * y = igam( b, ax ). + * + * + * ACCURACY: + * + * See igam(). + * + * ERROR MESSAGES: + * + * message condition value returned + * gdtr domain x < 0 0.0 + * + */ + /* gdtrc.c + * + * Complemented Gamma distribution function + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, gdtrc(); + * + * y = gdtrc( a, b, x ); + * + * + * + * DESCRIPTION: + * + * Returns the integral from x to infinity of the Gamma + * probability density function: + * + * + * inf. + * b - + * a | | b-1 -at + * y = ----- | t e dt + * - | | + * | (b) - + * x + * + * The incomplete Gamma integral is used, according to the + * relation + * + * y = igamc( b, ax ). + * + * + * ACCURACY: + * + * See igamc(). + * + * ERROR MESSAGES: + * + * message condition value returned + * gdtrc domain x < 0 0.0 + * + */ + +/* gdtr() */ + + +/* + * Cephes Math Library Release 2.3: March,1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + + +double gdtr(double a, double b, double x) +{ + + if (x < 0.0) { + sf_error("gdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + return (igam(b, a * x)); +} + + +double gdtrc(double a, double b, double x) +{ + + if (x < 0.0) { + sf_error("gdtrc", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + return (igamc(b, a * x)); +} + + +double gdtri(double a, double b, double y) +{ + + if ((y < 0.0) || (y > 1.0) || (a <= 0.0) || (b < 0.0)) { + sf_error("gdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + return (igamci(b, 1.0 - y) / a); +} diff --git a/gtsam/3rdparty/cephes/cephes/hyp2f1.c b/gtsam/3rdparty/cephes/cephes/hyp2f1.c new file mode 100644 index 0000000000..7f0a84d02a --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/hyp2f1.c @@ -0,0 +1,569 @@ +/* hyp2f1.c + * + * Gauss hypergeometric function F + * 2 1 + * + * + * SYNOPSIS: + * + * double a, b, c, x, y, hyp2f1(); + * + * y = hyp2f1( a, b, c, x ); + * + * + * DESCRIPTION: + * + * + * hyp2f1( a, b, c, x ) = F ( a, b; c; x ) + * 2 1 + * + * inf. + * - a(a+1)...(a+k) b(b+1)...(b+k) k+1 + * = 1 + > ----------------------------- x . + * - c(c+1)...(c+k) (k+1)! + * k = 0 + * + * Cases addressed are + * Tests and escapes for negative integer a, b, or c + * Linear transformation if c - a or c - b negative integer + * Special case c = a or c = b + * Linear transformation for x near +1 + * Transformation for x < -0.5 + * Psi function expansion if x > 0.5 and c - a - b integer + * Conditionally, a recurrence on c to make c-a-b > 0 + * + * x < -1 AMS 15.3.7 transformation applied (Travis Oliphant) + * valid for b,a,c,(b-a) != integer and (c-a),(c-b) != negative integer + * + * x >= 1 is rejected (unless special cases are present) + * + * The parameters a, b, c are considered to be integer + * valued if they are within 1.0e-14 of the nearest integer + * (1.0e-13 for IEEE arithmetic). + * + * ACCURACY: + * + * + * Relative error (-1 < x < 1): + * arithmetic domain # trials peak rms + * IEEE -1,7 230000 1.2e-11 5.2e-14 + * + * Several special cases also tested with a, b, c in + * the range -7 to 7. + * + * ERROR MESSAGES: + * + * A "partial loss of precision" message is printed if + * the internally estimated relative error exceeds 1^-12. + * A "singularity" message is printed on overflow or + * in cases not addressed (such as x < -1). + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1992, 2000 by Stephen L. Moshier + */ + +#include +#include +#include + +#include "mconf.h" + +#define EPS 1.0e-13 +#define EPS2 1.0e-10 + +#define ETHRESH 1.0e-12 + +#define MAX_ITERATIONS 10000 + +extern double MACHEP; + +/* hys2f1 and hyp2f1ra depend on each other, so we need this prototype */ +static double hyp2f1ra(double a, double b, double c, double x, double *loss); + +/* Defining power series expansion of Gauss hypergeometric function */ +/* The `loss` parameter estimates loss of significance */ +static double hys2f1(double a, double b, double c, double x, double *loss) { + double f, g, h, k, m, s, u, umax; + int i; + int ib, intflag = 0; + + if (fabs(b) > fabs(a)) { + /* Ensure that |a| > |b| ... */ + f = b; + b = a; + a = f; + } + + ib = round(b); + + if (fabs(b - ib) < EPS && ib <= 0 && fabs(b) < fabs(a)) { + /* .. except when `b` is a smaller negative integer */ + f = b; + b = a; + a = f; + intflag = 1; + } + + if ((fabs(a) > fabs(c) + 1 || intflag) && fabs(c - a) > 2 && fabs(a) > 2) { + /* |a| >> |c| implies that large cancellation error is to be expected. + * + * We try to reduce it with the recurrence relations + */ + return hyp2f1ra(a, b, c, x, loss); + } + + i = 0; + umax = 0.0; + f = a; + g = b; + h = c; + s = 1.0; + u = 1.0; + k = 0.0; + do { + if (fabs(h) < EPS) { + *loss = 1.0; + return INFINITY; + } + m = k + 1.0; + u = u * ((f + k) * (g + k) * x / ((h + k) * m)); + s += u; + k = fabs(u); /* remember largest term summed */ + if (k > umax) umax = k; + k = m; + if (++i > MAX_ITERATIONS) { /* should never happen */ + *loss = 1.0; + return (s); + } + } while (s == 0 || fabs(u / s) > MACHEP); + + /* return estimated relative error */ + *loss = (MACHEP * umax) / fabs(s) + (MACHEP * i); + + return (s); +} + +/* Apply transformations for |x| near 1 then call the power series */ +static double hyt2f1(double a, double b, double c, double x, double *loss) { + double p, q, r, s, t, y, w, d, err, err1; + double ax, id, d1, d2, e, y1; + int i, aid, sign; + + int ia, ib, neg_int_a = 0, neg_int_b = 0; + + ia = round(a); + ib = round(b); + + if (a <= 0 && fabs(a - ia) < EPS) { /* a is a negative integer */ + neg_int_a = 1; + } + + if (b <= 0 && fabs(b - ib) < EPS) { /* b is a negative integer */ + neg_int_b = 1; + } + + err = 0.0; + s = 1.0 - x; + if (x < -0.5 && !(neg_int_a || neg_int_b)) { + if (b > a) + y = pow(s, -a) * hys2f1(a, c - b, c, -x / s, &err); + + else + y = pow(s, -b) * hys2f1(c - a, b, c, -x / s, &err); + + goto done; + } + + d = c - a - b; + id = round(d); /* nearest integer to d */ + + if (x > 0.9 && !(neg_int_a || neg_int_b)) { + if (fabs(d - id) > EPS) { + int sgngam; + + /* test for integer c-a-b */ + /* Try the power series first */ + y = hys2f1(a, b, c, x, &err); + if (err < ETHRESH) goto done; + /* If power series fails, then apply AMS55 #15.3.6 */ + q = hys2f1(a, b, 1.0 - d, s, &err); + sign = 1; + w = lgam_sgn(d, &sgngam); + sign *= sgngam; + w -= lgam_sgn(c - a, &sgngam); + sign *= sgngam; + w -= lgam_sgn(c - b, &sgngam); + sign *= sgngam; + q *= sign * exp(w); + r = pow(s, d) * hys2f1(c - a, c - b, d + 1.0, s, &err1); + sign = 1; + w = lgam_sgn(-d, &sgngam); + sign *= sgngam; + w -= lgam_sgn(a, &sgngam); + sign *= sgngam; + w -= lgam_sgn(b, &sgngam); + sign *= sgngam; + r *= sign * exp(w); + y = q + r; + + q = fabs(q); /* estimate cancellation error */ + r = fabs(r); + if (q > r) r = q; + err += err1 + (MACHEP * r) / y; + + y *= gamma(c); + goto done; + } else { + /* Psi function expansion, AMS55 #15.3.10, #15.3.11, #15.3.12 + * + * Although AMS55 does not explicitly state it, this expansion fails + * for negative integer a or b, since the psi and Gamma functions + * involved have poles. + */ + + if (id >= 0.0) { + e = d; + d1 = d; + d2 = 0.0; + aid = id; + } else { + e = -d; + d1 = 0.0; + d2 = d; + aid = -id; + } + + ax = log(s); + + /* sum for t = 0 */ + y = psi(1.0) + psi(1.0 + e) - psi(a + d1) - psi(b + d1) - ax; + y /= gamma(e + 1.0); + + p = (a + d1) * (b + d1) * s / gamma(e + 2.0); /* Poch for t=1 */ + t = 1.0; + do { + r = psi(1.0 + t) + psi(1.0 + t + e) - psi(a + t + d1) - + psi(b + t + d1) - ax; + q = p * r; + y += q; + p *= s * (a + t + d1) / (t + 1.0); + p *= (b + t + d1) / (t + 1.0 + e); + t += 1.0; + if (t > MAX_ITERATIONS) { /* should never happen */ + sf_error("hyp2f1", SF_ERROR_SLOW, NULL); + *loss = 1.0; + return NAN; + } + } while (y == 0 || fabs(q / y) > EPS); + + if (id == 0.0) { + y *= gamma(c) / (gamma(a) * gamma(b)); + goto psidon; + } + + y1 = 1.0; + + if (aid == 1) goto nosum; + + t = 0.0; + p = 1.0; + for (i = 1; i < aid; i++) { + r = 1.0 - e + t; + p *= s * (a + t + d2) * (b + t + d2) / r; + t += 1.0; + p /= t; + y1 += p; + } + nosum: + p = gamma(c); + y1 *= gamma(e) * p / (gamma(a + d1) * gamma(b + d1)); + + y *= p / (gamma(a + d2) * gamma(b + d2)); + if ((aid & 1) != 0) y = -y; + + q = pow(s, id); /* s to the id power */ + if (id > 0.0) + y *= q; + else + y1 *= q; + + y += y1; + psidon: + goto done; + } + } + + /* Use defining power series if no special cases */ + y = hys2f1(a, b, c, x, &err); + +done: + *loss = err; + return (y); +} + +/* + 15.4.2 Abramowitz & Stegun. +*/ +static double hyp2f1_neg_c_equal_bc(double a, double b, double x) { + double k; + double collector = 1; + double sum = 1; + double collector_max = 1; + + if (!(fabs(b) < 1e5)) { + return NAN; + } + + for (k = 1; k <= -b; k++) { + collector *= (a + k - 1) * x / k; + collector_max = fmax(fabs(collector), collector_max); + sum += collector; + } + + if (1e-16 * (1 + collector_max / fabs(sum)) > 1e-7) { + return NAN; + } + + return sum; +} + +double hyp2f1(double a, double b, double c, double x) { + double d, d1, d2, e; + double p, q, r, s, y, ax; + double ia, ib, ic, id, err; + double t1; + int i, aid; + int neg_int_a = 0, neg_int_b = 0; + int neg_int_ca_or_cb = 0; + + err = 0.0; + ax = fabs(x); + s = 1.0 - x; + ia = round(a); /* nearest integer to a */ + ib = round(b); + + if (x == 0.0) { + return 1.0; + } + + d = c - a - b; + id = round(d); + + if ((a == 0 || b == 0) && c != 0) { + return 1.0; + } + + if (a <= 0 && fabs(a - ia) < EPS) { /* a is a negative integer */ + neg_int_a = 1; + } + + if (b <= 0 && fabs(b - ib) < EPS) { /* b is a negative integer */ + neg_int_b = 1; + } + + if (d <= -1 && !(fabs(d - id) > EPS && s < 0) && !(neg_int_a || neg_int_b)) { + return pow(s, d) * hyp2f1(c - a, c - b, c, x); + } + if (d <= 0 && x == 1 && !(neg_int_a || neg_int_b)) goto hypdiv; + + if (ax < 1.0 || x == -1.0) { + /* 2F1(a,b;b;x) = (1-x)**(-a) */ + if (fabs(b - c) < EPS) { /* b = c */ + if (neg_int_b) { + y = hyp2f1_neg_c_equal_bc(a, b, x); + } else { + y = pow(s, -a); /* s to the -a power */ + } + goto hypdon; + } + if (fabs(a - c) < EPS) { /* a = c */ + y = pow(s, -b); /* s to the -b power */ + goto hypdon; + } + } + + if (c <= 0.0) { + ic = round(c); /* nearest integer to c */ + if (fabs(c - ic) < EPS) { /* c is a negative integer */ + /* check if termination before explosion */ + if (neg_int_a && (ia > ic)) goto hypok; + if (neg_int_b && (ib > ic)) goto hypok; + goto hypdiv; + } + } + + if (neg_int_a || neg_int_b) /* function is a polynomial */ + goto hypok; + + t1 = fabs(b - a); + if (x < -2.0 && fabs(t1 - round(t1)) > EPS) { + /* This transform has a pole for b-a integer, and + * may produce large cancellation errors for |1/x| close 1 + */ + p = hyp2f1(a, 1 - c + a, 1 - b + a, 1.0 / x); + q = hyp2f1(b, 1 - c + b, 1 - a + b, 1.0 / x); + p *= pow(-x, -a); + q *= pow(-x, -b); + t1 = gamma(c); + s = t1 * gamma(b - a) / (gamma(b) * gamma(c - a)); + y = t1 * gamma(a - b) / (gamma(a) * gamma(c - b)); + return s * p + y * q; + } else if (x < -1.0) { + if (fabs(a) < fabs(b)) { + return pow(s, -a) * hyp2f1(a, c - b, c, x / (x - 1)); + } else { + return pow(s, -b) * hyp2f1(b, c - a, c, x / (x - 1)); + } + } + + if (ax > 1.0) /* series diverges */ + goto hypdiv; + + p = c - a; + ia = round(p); /* nearest integer to c-a */ + if ((ia <= 0.0) && (fabs(p - ia) < EPS)) /* negative int c - a */ + neg_int_ca_or_cb = 1; + + r = c - b; + ib = round(r); /* nearest integer to c-b */ + if ((ib <= 0.0) && (fabs(r - ib) < EPS)) /* negative int c - b */ + neg_int_ca_or_cb = 1; + + id = round(d); /* nearest integer to d */ + q = fabs(d - id); + + /* Thanks to Christian Burger + * for reporting a bug here. */ + if (fabs(ax - 1.0) < EPS) { /* |x| == 1.0 */ + if (x > 0.0) { + if (neg_int_ca_or_cb) { + if (d >= 0.0) + goto hypf; + else + goto hypdiv; + } + if (d <= 0.0) goto hypdiv; + y = gamma(c) * gamma(d) / (gamma(p) * gamma(r)); + goto hypdon; + } + if (d <= -1.0) goto hypdiv; + } + + /* Conditionally make d > 0 by recurrence on c + * AMS55 #15.2.27 + */ + if (d < 0.0) { + /* Try the power series first */ + y = hyt2f1(a, b, c, x, &err); + if (err < ETHRESH) goto hypdon; + /* Apply the recurrence if power series fails */ + err = 0.0; + aid = 2 - id; + e = c + aid; + d2 = hyp2f1(a, b, e, x); + d1 = hyp2f1(a, b, e + 1.0, x); + q = a + b + 1.0; + for (i = 0; i < aid; i++) { + r = e - 1.0; + y = (e * (r - (2.0 * e - q) * x) * d2 + (e - a) * (e - b) * x * d1) / + (e * r * s); + e = r; + d1 = d2; + d2 = y; + } + goto hypdon; + } + + if (neg_int_ca_or_cb) goto hypf; /* negative integer c-a or c-b */ + +hypok: + y = hyt2f1(a, b, c, x, &err); + +hypdon: + if (err > ETHRESH) { + sf_error("hyp2f1", SF_ERROR_LOSS, NULL); + /* printf( "Estimated err = %.2e\n", err ); */ + } + return (y); + + /* The transformation for c-a or c-b negative integer + * AMS55 #15.3.3 + */ +hypf: + y = pow(s, d) * hys2f1(c - a, c - b, c, x, &err); + goto hypdon; + + /* The alarm exit */ +hypdiv: + sf_error("hyp2f1", SF_ERROR_OVERFLOW, NULL); + return INFINITY; +} + +/* + * Evaluate hypergeometric function by two-term recurrence in `a`. + * + * This avoids some of the loss of precision in the strongly alternating + * hypergeometric series, and can be used to reduce the `a` and `b` parameters + * to smaller values. + * + * AMS55 #15.2.10 + */ +static double hyp2f1ra(double a, double b, double c, double x, double *loss) { + double f2, f1, f0; + int n; + double t, err, da; + + /* Don't cross c or zero */ + if ((c < 0 && a <= c) || (c >= 0 && a >= c)) { + da = round(a - c); + } else { + da = round(a); + } + t = a - da; + + *loss = 0; + + assert(da != 0); + + if (fabs(da) > MAX_ITERATIONS) { + /* Too expensive to compute this value, so give up */ + sf_error("hyp2f1", SF_ERROR_NO_RESULT, NULL); + *loss = 1.0; + return NAN; + } + + if (da < 0) { + /* Recurse down */ + f2 = 0; + f1 = hys2f1(t, b, c, x, &err); + *loss += err; + f0 = hys2f1(t - 1, b, c, x, &err); + *loss += err; + t -= 1; + for (n = 1; n < -da; ++n) { + f2 = f1; + f1 = f0; + f0 = -(2 * t - c - t * x + b * x) / (c - t) * f1 - + t * (x - 1) / (c - t) * f2; + t -= 1; + } + } else { + /* Recurse up */ + f2 = 0; + f1 = hys2f1(t, b, c, x, &err); + *loss += err; + f0 = hys2f1(t + 1, b, c, x, &err); + *loss += err; + t += 1; + for (n = 1; n < da; ++n) { + f2 = f1; + f1 = f0; + f0 = -((2 * t - c - t * x + b * x) * f1 + (c - t) * f2) / (t * (x - 1)); + t += 1; + } + } + + return f0; +} diff --git a/gtsam/3rdparty/cephes/cephes/hyperg.c b/gtsam/3rdparty/cephes/cephes/hyperg.c new file mode 100644 index 0000000000..ac23e71339 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/hyperg.c @@ -0,0 +1,362 @@ +/* hyperg.c + * + * Confluent hypergeometric function + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, hyperg(); + * + * y = hyperg( a, b, x ); + * + * + * + * DESCRIPTION: + * + * Computes the confluent hypergeometric function + * + * 1 2 + * a x a(a+1) x + * F ( a,b;x ) = 1 + ---- + --------- + ... + * 1 1 b 1! b(b+1) 2! + * + * Many higher transcendental functions are special cases of + * this power series. + * + * As is evident from the formula, b must not be a negative + * integer or zero unless a is an integer with 0 >= a > b. + * + * The routine attempts both a direct summation of the series + * and an asymptotic expansion. In each case error due to + * roundoff, cancellation, and nonconvergence is estimated. + * The result with smaller estimated error is returned. + * + * + * + * ACCURACY: + * + * Tested at random points (a, b, x), all three variables + * ranging from 0 to 30. + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 1.8e-14 1.1e-15 + * + * Larger errors can be observed when b is near a negative + * integer or zero. Certain combinations of arguments yield + * serious cancellation error in the power series summation + * and also are not in the region of near convergence of the + * asymptotic series. An error message is printed if the + * self-estimated relative error is greater than 1.0e-12. + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1988, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" +#include + +extern double MACHEP; + + +/* the `type` parameter determines what converging factor to use */ +static double hyp2f0(double a, double b, double x, int type, double *err) +{ + double a0, alast, t, tlast, maxt; + double n, an, bn, u, sum, temp; + + an = a; + bn = b; + a0 = 1.0e0; + alast = 1.0e0; + sum = 0.0; + n = 1.0e0; + t = 1.0e0; + tlast = 1.0e9; + maxt = 0.0; + + do { + if (an == 0) + goto pdone; + if (bn == 0) + goto pdone; + + u = an * (bn * x / n); + + /* check for blowup */ + temp = fabs(u); + if ((temp > 1.0) && (maxt > (DBL_MAX / temp))) + goto error; + + a0 *= u; + t = fabs(a0); + + /* terminating condition for asymptotic series: + * the series is divergent (if a or b is not a negative integer), + * but its leading part can be used as an asymptotic expansion + */ + if (t > tlast) + goto ndone; + + tlast = t; + sum += alast; /* the sum is one term behind */ + alast = a0; + + if (n > 200) + goto ndone; + + an += 1.0e0; + bn += 1.0e0; + n += 1.0e0; + if (t > maxt) + maxt = t; + } + while (t > MACHEP); + + + pdone: /* series converged! */ + + /* estimate error due to roundoff and cancellation */ + *err = fabs(MACHEP * (n + maxt)); + + alast = a0; + goto done; + + ndone: /* series did not converge */ + + /* The following "Converging factors" are supposed to improve accuracy, + * but do not actually seem to accomplish very much. */ + + n -= 1.0; + x = 1.0 / x; + + switch (type) { /* "type" given as subroutine argument */ + case 1: + alast *= + (0.5 + (0.125 + 0.25 * b - 0.5 * a + 0.25 * x - 0.25 * n) / x); + break; + + case 2: + alast *= 2.0 / 3.0 - b + 2.0 * a + x - n; + break; + + default: + ; + } + + /* estimate error due to roundoff, cancellation, and nonconvergence */ + *err = MACHEP * (n + maxt) + fabs(a0); + + done: + sum += alast; + return (sum); + + /* series blew up: */ + error: + *err = INFINITY; + sf_error("hyperg", SF_ERROR_NO_RESULT, NULL); + return (sum); +} + + +/* asymptotic formula for hypergeometric function: + * + * ( -a + * -- ( |z| + * | (b) ( -------- 2f0( a, 1+a-b, -1/x ) + * ( -- + * ( | (b-a) + * + * + * x a-b ) + * e |x| ) + * + -------- 2f0( b-a, 1-a, 1/x ) ) + * -- ) + * | (a) ) + */ + +static double hy1f1a(double a, double b, double x, double *err) +{ + double h1, h2, t, u, temp, acanc, asum, err1, err2; + + if (x == 0) { + acanc = 1.0; + asum = INFINITY; + goto adone; + } + temp = log(fabs(x)); + t = x + temp * (a - b); + u = -temp * a; + + if (b > 0) { + temp = lgam(b); + t += temp; + u += temp; + } + + h1 = hyp2f0(a, a - b + 1, -1.0 / x, 1, &err1); + + temp = exp(u) / gamma(b - a); + h1 *= temp; + err1 *= temp; + + h2 = hyp2f0(b - a, 1.0 - a, 1.0 / x, 2, &err2); + + if (a < 0) + temp = exp(t) / gamma(a); + else + temp = exp(t - lgam(a)); + + h2 *= temp; + err2 *= temp; + + if (x < 0.0) + asum = h1; + else + asum = h2; + + acanc = fabs(err1) + fabs(err2); + + if (b < 0) { + temp = gamma(b); + asum *= temp; + acanc *= fabs(temp); + } + + + if (asum != 0.0) + acanc /= fabs(asum); + + if (acanc != acanc) + /* nan */ + acanc = 1.0; + + if (asum == INFINITY || asum == -INFINITY) + /* infinity */ + acanc = 0; + + acanc *= 30.0; /* fudge factor, since error of asymptotic formula + * often seems this much larger than advertised */ + + adone: + *err = acanc; + return (asum); +} + + +/* Power series summation for confluent hypergeometric function */ +static double hy1f1p(double a, double b, double x, double *err) +{ + double n, a0, sum, t, u, temp, maxn; + double an, bn, maxt; + double y, c, sumc; + + + /* set up for power series summation */ + an = a; + bn = b; + a0 = 1.0; + sum = 1.0; + c = 0.0; + n = 1.0; + t = 1.0; + maxt = 0.0; + *err = 1.0; + + maxn = 200.0 + 2 * fabs(a) + 2 * fabs(b); + + while (t > MACHEP) { + if (bn == 0) { /* check bn first since if both */ + sf_error("hyperg", SF_ERROR_SINGULAR, NULL); + return (INFINITY); /* an and bn are zero it is */ + } + if (an == 0) /* a singularity */ + return (sum); + if (n > maxn) { + /* too many terms; take the last one as error estimate */ + c = fabs(c) + fabs(t) * 50.0; + goto pdone; + } + u = x * (an / (bn * n)); + + /* check for blowup */ + temp = fabs(u); + if ((temp > 1.0) && (maxt > (DBL_MAX / temp))) { + *err = 1.0; /* blowup: estimate 100% error */ + return sum; + } + + a0 *= u; + + y = a0 - c; + sumc = sum + y; + c = (sumc - sum) - y; + sum = sumc; + + t = fabs(a0); + + an += 1.0; + bn += 1.0; + n += 1.0; + } + + pdone: + + /* estimate error due to roundoff and cancellation */ + if (sum != 0.0) { + *err = fabs(c / sum); + } + else { + *err = fabs(c); + } + + if (*err != *err) { + /* nan */ + *err = 1.0; + } + + return (sum); +} + + + +double hyperg(double a, double b, double x) +{ + double asum, psum, acanc, pcanc, temp; + + /* See if a Kummer transformation will help */ + temp = b - a; + if (fabs(temp) < 0.001 * fabs(a)) + return (exp(x) * hyperg(temp, b, -x)); + + + /* Try power & asymptotic series, starting from the one that is likely OK */ + if (fabs(x) < 10 + fabs(a) + fabs(b)) { + psum = hy1f1p(a, b, x, &pcanc); + if (pcanc < 1.0e-15) + goto done; + asum = hy1f1a(a, b, x, &acanc); + } + else { + psum = hy1f1a(a, b, x, &pcanc); + if (pcanc < 1.0e-15) + goto done; + asum = hy1f1p(a, b, x, &acanc); + } + + /* Pick the result with less estimated error */ + + if (acanc < pcanc) { + pcanc = acanc; + psum = asum; + } + + done: + if (pcanc > 1.0e-12) + sf_error("hyperg", SF_ERROR_LOSS, NULL); + + return (psum); +} diff --git a/gtsam/3rdparty/cephes/cephes/i0.c b/gtsam/3rdparty/cephes/cephes/i0.c new file mode 100644 index 0000000000..4e85d556ef --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/i0.c @@ -0,0 +1,180 @@ +/* i0.c + * + * Modified Bessel function of order zero + * + * + * + * SYNOPSIS: + * + * double x, y, i0(); + * + * y = i0( x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of order zero of the + * argument. + * + * The function is defined as i0(x) = j0( ix ). + * + * The range is partitioned into the two intervals [0,8] and + * (8, infinity). Chebyshev polynomial expansions are employed + * in each interval. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 5.8e-16 1.4e-16 + * + */ + /* i0e.c + * + * Modified Bessel function of order zero, + * exponentially scaled + * + * + * + * SYNOPSIS: + * + * double x, y, i0e(); + * + * y = i0e( x ); + * + * + * + * DESCRIPTION: + * + * Returns exponentially scaled modified Bessel function + * of order zero of the argument. + * + * The function is defined as i0e(x) = exp(-|x|) j0( ix ). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 5.4e-16 1.2e-16 + * See i0(). + * + */ + +/* i0.c */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +/* Chebyshev coefficients for exp(-x) I0(x) + * in the interval [0,8]. + * + * lim(x->0){ exp(-x) I0(x) } = 1. + */ +static double A[] = { + -4.41534164647933937950E-18, + 3.33079451882223809783E-17, + -2.43127984654795469359E-16, + 1.71539128555513303061E-15, + -1.16853328779934516808E-14, + 7.67618549860493561688E-14, + -4.85644678311192946090E-13, + 2.95505266312963983461E-12, + -1.72682629144155570723E-11, + 9.67580903537323691224E-11, + -5.18979560163526290666E-10, + 2.65982372468238665035E-9, + -1.30002500998624804212E-8, + 6.04699502254191894932E-8, + -2.67079385394061173391E-7, + 1.11738753912010371815E-6, + -4.41673835845875056359E-6, + 1.64484480707288970893E-5, + -5.75419501008210370398E-5, + 1.88502885095841655729E-4, + -5.76375574538582365885E-4, + 1.63947561694133579842E-3, + -4.32430999505057594430E-3, + 1.05464603945949983183E-2, + -2.37374148058994688156E-2, + 4.93052842396707084878E-2, + -9.49010970480476444210E-2, + 1.71620901522208775349E-1, + -3.04682672343198398683E-1, + 6.76795274409476084995E-1 +}; + +/* Chebyshev coefficients for exp(-x) sqrt(x) I0(x) + * in the inverted interval [8,infinity]. + * + * lim(x->inf){ exp(-x) sqrt(x) I0(x) } = 1/sqrt(2pi). + */ +static double B[] = { + -7.23318048787475395456E-18, + -4.83050448594418207126E-18, + 4.46562142029675999901E-17, + 3.46122286769746109310E-17, + -2.82762398051658348494E-16, + -3.42548561967721913462E-16, + 1.77256013305652638360E-15, + 3.81168066935262242075E-15, + -9.55484669882830764870E-15, + -4.15056934728722208663E-14, + 1.54008621752140982691E-14, + 3.85277838274214270114E-13, + 7.18012445138366623367E-13, + -1.79417853150680611778E-12, + -1.32158118404477131188E-11, + -3.14991652796324136454E-11, + 1.18891471078464383424E-11, + 4.94060238822496958910E-10, + 3.39623202570838634515E-9, + 2.26666899049817806459E-8, + 2.04891858946906374183E-7, + 2.89137052083475648297E-6, + 6.88975834691682398426E-5, + 3.36911647825569408990E-3, + 8.04490411014108831608E-1 +}; + +double i0(double x) +{ + double y; + + if (x < 0) + x = -x; + if (x <= 8.0) { + y = (x / 2.0) - 2.0; + return (exp(x) * chbevl(y, A, 30)); + } + + return (exp(x) * chbevl(32.0 / x - 2.0, B, 25) / sqrt(x)); + +} + + + + +double i0e(double x) +{ + double y; + + if (x < 0) + x = -x; + if (x <= 8.0) { + y = (x / 2.0) - 2.0; + return (chbevl(y, A, 30)); + } + + return (chbevl(32.0 / x - 2.0, B, 25) / sqrt(x)); + +} diff --git a/gtsam/3rdparty/cephes/cephes/i1.c b/gtsam/3rdparty/cephes/cephes/i1.c new file mode 100644 index 0000000000..4553873f2c --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/i1.c @@ -0,0 +1,184 @@ +/* i1.c + * + * Modified Bessel function of order one + * + * + * + * SYNOPSIS: + * + * double x, y, i1(); + * + * y = i1( x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of order one of the + * argument. + * + * The function is defined as i1(x) = -i j1( ix ). + * + * The range is partitioned into the two intervals [0,8] and + * (8, infinity). Chebyshev polynomial expansions are employed + * in each interval. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.9e-15 2.1e-16 + * + * + */ + /* i1e.c + * + * Modified Bessel function of order one, + * exponentially scaled + * + * + * + * SYNOPSIS: + * + * double x, y, i1e(); + * + * y = i1e( x ); + * + * + * + * DESCRIPTION: + * + * Returns exponentially scaled modified Bessel function + * of order one of the argument. + * + * The function is defined as i1(x) = -i exp(-|x|) j1( ix ). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 2.0e-15 2.0e-16 + * See i1(). + * + */ + +/* i1.c 2 */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1985, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +/* Chebyshev coefficients for exp(-x) I1(x) / x + * in the interval [0,8]. + * + * lim(x->0){ exp(-x) I1(x) / x } = 1/2. + */ + +static double A[] = { + 2.77791411276104639959E-18, + -2.11142121435816608115E-17, + 1.55363195773620046921E-16, + -1.10559694773538630805E-15, + 7.60068429473540693410E-15, + -5.04218550472791168711E-14, + 3.22379336594557470981E-13, + -1.98397439776494371520E-12, + 1.17361862988909016308E-11, + -6.66348972350202774223E-11, + 3.62559028155211703701E-10, + -1.88724975172282928790E-9, + 9.38153738649577178388E-9, + -4.44505912879632808065E-8, + 2.00329475355213526229E-7, + -8.56872026469545474066E-7, + 3.47025130813767847674E-6, + -1.32731636560394358279E-5, + 4.78156510755005422638E-5, + -1.61760815825896745588E-4, + 5.12285956168575772895E-4, + -1.51357245063125314899E-3, + 4.15642294431288815669E-3, + -1.05640848946261981558E-2, + 2.47264490306265168283E-2, + -5.29459812080949914269E-2, + 1.02643658689847095384E-1, + -1.76416518357834055153E-1, + 2.52587186443633654823E-1 +}; + +/* Chebyshev coefficients for exp(-x) sqrt(x) I1(x) + * in the inverted interval [8,infinity]. + * + * lim(x->inf){ exp(-x) sqrt(x) I1(x) } = 1/sqrt(2pi). + */ +static double B[] = { + 7.51729631084210481353E-18, + 4.41434832307170791151E-18, + -4.65030536848935832153E-17, + -3.20952592199342395980E-17, + 2.96262899764595013876E-16, + 3.30820231092092828324E-16, + -1.88035477551078244854E-15, + -3.81440307243700780478E-15, + 1.04202769841288027642E-14, + 4.27244001671195135429E-14, + -2.10154184277266431302E-14, + -4.08355111109219731823E-13, + -7.19855177624590851209E-13, + 2.03562854414708950722E-12, + 1.41258074366137813316E-11, + 3.25260358301548823856E-11, + -1.89749581235054123450E-11, + -5.58974346219658380687E-10, + -3.83538038596423702205E-9, + -2.63146884688951950684E-8, + -2.51223623787020892529E-7, + -3.88256480887769039346E-6, + -1.10588938762623716291E-4, + -9.76109749136146840777E-3, + 7.78576235018280120474E-1 +}; + +double i1(double x) +{ + double y, z; + + z = fabs(x); + if (z <= 8.0) { + y = (z / 2.0) - 2.0; + z = chbevl(y, A, 29) * z * exp(z); + } + else { + z = exp(z) * chbevl(32.0 / z - 2.0, B, 25) / sqrt(z); + } + if (x < 0.0) + z = -z; + return (z); +} + +/* i1e() */ + +double i1e(double x) +{ + double y, z; + + z = fabs(x); + if (z <= 8.0) { + y = (z / 2.0) - 2.0; + z = chbevl(y, A, 29) * z; + } + else { + z = chbevl(32.0 / z - 2.0, B, 25) / sqrt(z); + } + if (x < 0.0) + z = -z; + return (z); +} diff --git a/gtsam/3rdparty/cephes/cephes/igam.c b/gtsam/3rdparty/cephes/cephes/igam.c new file mode 100644 index 0000000000..75f871ec51 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/igam.c @@ -0,0 +1,423 @@ +/* igam.c + * + * Incomplete Gamma integral + * + * + * + * SYNOPSIS: + * + * double a, x, y, igam(); + * + * y = igam( a, x ); + * + * DESCRIPTION: + * + * The function is defined by + * + * x + * - + * 1 | | -t a-1 + * igam(a,x) = ----- | e t dt. + * - | | + * | (a) - + * 0 + * + * + * In this implementation both arguments must be positive. + * The integral is evaluated by either a power series or + * continued fraction expansion, depending on the relative + * values of a and x. + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 200000 3.6e-14 2.9e-15 + * IEEE 0,100 300000 9.9e-14 1.5e-14 + */ + /* igamc() + * + * Complemented incomplete Gamma integral + * + * + * + * SYNOPSIS: + * + * double a, x, y, igamc(); + * + * y = igamc( a, x ); + * + * DESCRIPTION: + * + * The function is defined by + * + * + * igamc(a,x) = 1 - igam(a,x) + * + * inf. + * - + * 1 | | -t a-1 + * = ----- | e t dt. + * - | | + * | (a) - + * x + * + * + * In this implementation both arguments must be positive. + * The integral is evaluated by either a power series or + * continued fraction expansion, depending on the relative + * values of a and x. + * + * ACCURACY: + * + * Tested at random a, x. + * a x Relative error: + * arithmetic domain domain # trials peak rms + * IEEE 0.5,100 0,100 200000 1.9e-14 1.7e-15 + * IEEE 0.01,0.5 0,100 200000 1.4e-13 1.6e-15 + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1985, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +/* Sources + * [1] "The Digital Library of Mathematical Functions", dlmf.nist.gov + * [2] Maddock et. al., "Incomplete Gamma Functions", + * https://www.boost.org/doc/libs/1_61_0/libs/math/doc/html/math_toolkit/sf_gamma/igamma.html + */ + +/* Scipy changes: + * - 05-01-2016: added asymptotic expansion for igam to improve the + * a ~ x regime. + * - 06-19-2016: additional series expansion added for igamc to + * improve accuracy at small arguments. + * - 06-24-2016: better choice of domain for the asymptotic series; + * improvements in accuracy for the asymptotic series when a and x + * are very close. + */ + +#include "mconf.h" +#include "lanczos.h" +#include "igam.h" + +#ifdef MAXITER +#undef MAXITER +#endif + +#define MAXITER 2000 +#define IGAM 1 +#define IGAMC 0 +#define SMALL 20 +#define LARGE 200 +#define SMALLRATIO 0.3 +#define LARGERATIO 4.5 + +extern double MACHEP, MAXLOG; +static double big = 4.503599627370496e15; +static double biginv = 2.22044604925031308085e-16; + +static double igamc_continued_fraction(double, double); +static double igam_series(double, double); +static double igamc_series(double, double); +static double asymptotic_series(double, double, int); + + +double igam(double a, double x) +{ + double absxma_a; + + if (x < 0 || a < 0) { + sf_error("gammainc", SF_ERROR_DOMAIN, NULL); + return NAN; + } else if (a == 0) { + if (x > 0) { + return 1; + } else { + return NAN; + } + } else if (x == 0) { + /* Zero integration limit */ + return 0; + } else if (isinf(a)) { + if (isinf(x)) { + return NAN; + } + return 0; + } else if (isinf(x)) { + return 1; + } + + /* Asymptotic regime where a ~ x; see [2]. */ + absxma_a = fabs(x - a) / a; + if ((a > SMALL) && (a < LARGE) && (absxma_a < SMALLRATIO)) { + return asymptotic_series(a, x, IGAM); + } else if ((a > LARGE) && (absxma_a < LARGERATIO / sqrt(a))) { + return asymptotic_series(a, x, IGAM); + } + + if ((x > 1.0) && (x > a)) { + return (1.0 - igamc(a, x)); + } + + return igam_series(a, x); +} + + +double igamc(double a, double x) +{ + double absxma_a; + + if (x < 0 || a < 0) { + sf_error("gammaincc", SF_ERROR_DOMAIN, NULL); + return NAN; + } else if (a == 0) { + if (x > 0) { + return 0; + } else { + return NAN; + } + } else if (x == 0) { + return 1; + } else if (isinf(a)) { + if (isinf(x)) { + return NAN; + } + return 1; + } else if (isinf(x)) { + return 0; + } + + /* Asymptotic regime where a ~ x; see [2]. */ + absxma_a = fabs(x - a) / a; + if ((a > SMALL) && (a < LARGE) && (absxma_a < SMALLRATIO)) { + return asymptotic_series(a, x, IGAMC); + } else if ((a > LARGE) && (absxma_a < LARGERATIO / sqrt(a))) { + return asymptotic_series(a, x, IGAMC); + } + + /* Everywhere else; see [2]. */ + if (x > 1.1) { + if (x < a) { + return 1.0 - igam_series(a, x); + } else { + return igamc_continued_fraction(a, x); + } + } else if (x <= 0.5) { + if (-0.4 / log(x) < a) { + return 1.0 - igam_series(a, x); + } else { + return igamc_series(a, x); + } + } else { + if (x * 1.1 < a) { + return 1.0 - igam_series(a, x); + } else { + return igamc_series(a, x); + } + } +} + + +/* Compute + * + * x^a * exp(-x) / gamma(a) + * + * corrected from (15) and (16) in [2] by replacing exp(x - a) with + * exp(a - x). + */ +double igam_fac(double a, double x) +{ + double ax, fac, res, num; + + if (fabs(a - x) > 0.4 * fabs(a)) { + ax = a * log(x) - x - lgam(a); + if (ax < -MAXLOG) { + sf_error("igam", SF_ERROR_UNDERFLOW, NULL); + return 0.0; + } + return exp(ax); + } + + fac = a + lanczos_g - 0.5; + res = sqrt(fac / exp(1)) / lanczos_sum_expg_scaled(a); + + if ((a < 200) && (x < 200)) { + res *= exp(a - x) * pow(x / fac, a); + } else { + num = x - a - lanczos_g + 0.5; + res *= exp(a * log1pmx(num / fac) + x * (0.5 - lanczos_g) / fac); + } + + return res; +} + + +/* Compute igamc using DLMF 8.9.2. */ +static double igamc_continued_fraction(double a, double x) +{ + int i; + double ans, ax, c, yc, r, t, y, z; + double pk, pkm1, pkm2, qk, qkm1, qkm2; + + ax = igam_fac(a, x); + if (ax == 0.0) { + return 0.0; + } + + /* continued fraction */ + y = 1.0 - a; + z = x + y + 1.0; + c = 0.0; + pkm2 = 1.0; + qkm2 = x; + pkm1 = x + 1.0; + qkm1 = z * x; + ans = pkm1 / qkm1; + + for (i = 0; i < MAXITER; i++) { + c += 1.0; + y += 1.0; + z += 2.0; + yc = y * c; + pk = pkm1 * z - pkm2 * yc; + qk = qkm1 * z - qkm2 * yc; + if (qk != 0) { + r = pk / qk; + t = fabs((ans - r) / r); + ans = r; + } + else + t = 1.0; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + if (fabs(pk) > big) { + pkm2 *= biginv; + pkm1 *= biginv; + qkm2 *= biginv; + qkm1 *= biginv; + } + if (t <= MACHEP) { + break; + } + } + + return (ans * ax); +} + + +/* Compute igam using DLMF 8.11.4. */ +static double igam_series(double a, double x) +{ + int i; + double ans, ax, c, r; + + ax = igam_fac(a, x); + if (ax == 0.0) { + return 0.0; + } + + /* power series */ + r = a; + c = 1.0; + ans = 1.0; + + for (i = 0; i < MAXITER; i++) { + r += 1.0; + c *= x / r; + ans += c; + if (c <= MACHEP * ans) { + break; + } + } + + return (ans * ax / a); +} + + +/* Compute igamc using DLMF 8.7.3. This is related to the series in + * igam_series but extra care is taken to avoid cancellation. + */ +static double igamc_series(double a, double x) +{ + int n; + double fac = 1; + double sum = 0; + double term, logx; + + for (n = 1; n < MAXITER; n++) { + fac *= -x / n; + term = fac / (a + n); + sum += term; + if (fabs(term) <= MACHEP * fabs(sum)) { + break; + } + } + + logx = log(x); + term = -expm1(a * logx - lgam1p(a)); + return term - exp(a * logx - lgam(a)) * sum; +} + + +/* Compute igam/igamc using DLMF 8.12.3/8.12.4. */ +static double asymptotic_series(double a, double x, int func) +{ + int k, n, sgn; + int maxpow = 0; + double lambda = x / a; + double sigma = (x - a) / a; + double eta, res, ck, ckterm, term, absterm; + double absoldterm = INFINITY; + double etapow[N] = {1}; + double sum = 0; + double afac = 1; + + if (func == IGAM) { + sgn = -1; + } else { + sgn = 1; + } + + if (lambda > 1) { + eta = sqrt(-2 * log1pmx(sigma)); + } else if (lambda < 1) { + eta = -sqrt(-2 * log1pmx(sigma)); + } else { + eta = 0; + } + res = 0.5 * erfc(sgn * eta * sqrt(a / 2)); + + for (k = 0; k < K; k++) { + ck = d[k][0]; + for (n = 1; n < N; n++) { + if (n > maxpow) { + etapow[n] = eta * etapow[n-1]; + maxpow += 1; + } + ckterm = d[k][n]*etapow[n]; + ck += ckterm; + if (fabs(ckterm) < MACHEP * fabs(ck)) { + break; + } + } + term = ck * afac; + absterm = fabs(term); + if (absterm > absoldterm) { + break; + } + sum += term; + if (absterm < MACHEP * fabs(sum)) { + break; + } + absoldterm = absterm; + afac /= a; + } + res += sgn * exp(-0.5 * a * eta * eta) * sum / sqrt(2 * M_PI * a); + + return res; +} diff --git a/gtsam/3rdparty/cephes/cephes/igam.h b/gtsam/3rdparty/cephes/cephes/igam.h new file mode 100644 index 0000000000..0bc310633c --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/igam.h @@ -0,0 +1,38 @@ +/* This file was automatically generated by _precomp/gammainc.py. + * Do not edit it manually! + */ + +#ifndef IGAM_H +#define IGAM_H + +#define K 25 +#define N 25 + +static const double d[K][N] = +{{-3.3333333333333333e-1, 8.3333333333333333e-2, -1.4814814814814815e-2, 1.1574074074074074e-3, 3.527336860670194e-4, -1.7875514403292181e-4, 3.9192631785224378e-5, -2.1854485106799922e-6, -1.85406221071516e-6, 8.296711340953086e-7, -1.7665952736826079e-7, 6.7078535434014986e-9, 1.0261809784240308e-8, -4.3820360184533532e-9, 9.1476995822367902e-10, -2.551419399494625e-11, -5.8307721325504251e-11, 2.4361948020667416e-11, -5.0276692801141756e-12, 1.1004392031956135e-13, 3.3717632624009854e-13, -1.3923887224181621e-13, 2.8534893807047443e-14, -5.1391118342425726e-16, -1.9752288294349443e-15}, +{-1.8518518518518519e-3, -3.4722222222222222e-3, 2.6455026455026455e-3, -9.9022633744855967e-4, 2.0576131687242798e-4, -4.0187757201646091e-7, -1.8098550334489978e-5, 7.6491609160811101e-6, -1.6120900894563446e-6, 4.6471278028074343e-9, 1.378633446915721e-7, -5.752545603517705e-8, 1.1951628599778147e-8, -1.7543241719747648e-11, -1.0091543710600413e-9, 4.1627929918425826e-10, -8.5639070264929806e-11, 6.0672151016047586e-14, 7.1624989648114854e-12, -2.9331866437714371e-12, 5.9966963656836887e-13, -2.1671786527323314e-16, -4.9783399723692616e-14, 2.0291628823713425e-14, -4.13125571381061e-15}, +{4.1335978835978836e-3, -2.6813271604938272e-3, 7.7160493827160494e-4, 2.0093878600823045e-6, -1.0736653226365161e-4, 5.2923448829120125e-5, -1.2760635188618728e-5, 3.4235787340961381e-8, 1.3721957309062933e-6, -6.298992138380055e-7, 1.4280614206064242e-7, -2.0477098421990866e-10, -1.4092529910867521e-8, 6.228974084922022e-9, -1.3670488396617113e-9, 9.4283561590146782e-13, 1.2872252400089318e-10, -5.5645956134363321e-11, 1.1975935546366981e-11, -4.1689782251838635e-15, -1.0940640427884594e-12, 4.6622399463901357e-13, -9.905105763906906e-14, 1.8931876768373515e-17, 8.8592218725911273e-15}, +{6.4943415637860082e-4, 2.2947209362139918e-4, -4.6918949439525571e-4, 2.6772063206283885e-4, -7.5618016718839764e-5, -2.3965051138672967e-7, 1.1082654115347302e-5, -5.6749528269915966e-6, 1.4230900732435884e-6, -2.7861080291528142e-11, -1.6958404091930277e-7, 8.0994649053880824e-8, -1.9111168485973654e-8, 2.3928620439808118e-12, 2.0620131815488798e-9, -9.4604966618551322e-10, 2.1541049775774908e-10, -1.388823336813903e-14, -2.1894761681963939e-11, 9.7909989511716851e-12, -2.1782191880180962e-12, 6.2088195734079014e-17, 2.126978363279737e-13, -9.3446887915174333e-14, 2.0453671226782849e-14}, +{-8.618882909167117e-4, 7.8403922172006663e-4, -2.9907248030319018e-4, -1.4638452578843418e-6, 6.6414982154651222e-5, -3.9683650471794347e-5, 1.1375726970678419e-5, 2.5074972262375328e-10, -1.6954149536558306e-6, 8.9075075322053097e-7, -2.2929348340008049e-7, 2.956794137544049e-11, 2.8865829742708784e-8, -1.4189739437803219e-8, 3.4463580499464897e-9, -2.3024517174528067e-13, -3.9409233028046405e-10, 1.8602338968504502e-10, -4.356323005056618e-11, 1.2786001016296231e-15, 4.6792750266579195e-12, -2.1492464706134829e-12, 4.9088156148096522e-13, -6.3385914848915603e-18, -5.0453320690800944e-14}, +{-3.3679855336635815e-4, -6.9728137583658578e-5, 2.7727532449593921e-4, -1.9932570516188848e-4, 6.7977804779372078e-5, 1.419062920643967e-7, -1.3594048189768693e-5, 8.0184702563342015e-6, -2.2914811765080952e-6, -3.252473551298454e-10, 3.4652846491085265e-7, -1.8447187191171343e-7, 4.8240967037894181e-8, -1.7989466721743515e-14, -6.3061945000135234e-9, 3.1624176287745679e-9, -7.8409242536974293e-10, 5.1926791652540407e-15, 9.3589442423067836e-11, -4.5134262161632782e-11, 1.0799129993116827e-11, -3.661886712685252e-17, -1.210902069055155e-12, 5.6807435849905643e-13, -1.3249659916340829e-13}, +{5.3130793646399222e-4, -5.9216643735369388e-4, 2.7087820967180448e-4, 7.9023532326603279e-7, -8.1539693675619688e-5, 5.6116827531062497e-5, -1.8329116582843376e-5, -3.0796134506033048e-9, 3.4651553688036091e-6, -2.0291327396058604e-6, 5.7887928631490037e-7, 2.338630673826657e-13, -8.8286007463304835e-8, 4.7435958880408128e-8, -1.2545415020710382e-8, 8.6496488580102925e-14, 1.6846058979264063e-9, -8.5754928235775947e-10, 2.1598224929232125e-10, -7.6132305204761539e-16, -2.6639822008536144e-11, 1.3065700536611057e-11, -3.1799163902367977e-12, 4.7109761213674315e-18, 3.6902800842763467e-13}, +{3.4436760689237767e-4, 5.1717909082605922e-5, -3.3493161081142236e-4, 2.812695154763237e-4, -1.0976582244684731e-4, -1.2741009095484485e-7, 2.7744451511563644e-5, -1.8263488805711333e-5, 5.7876949497350524e-6, 4.9387589339362704e-10, -1.0595367014026043e-6, 6.1667143761104075e-7, -1.7562973359060462e-7, -1.2974473287015439e-12, 2.695423606288966e-8, -1.4578352908731271e-8, 3.887645959386175e-9, -3.8810022510194121e-17, -5.3279941738772867e-10, 2.7437977643314845e-10, -6.9957960920705679e-11, 2.5899863874868481e-17, 8.8566890996696381e-12, -4.403168815871311e-12, 1.0865561947091654e-12}, +{-6.5262391859530942e-4, 8.3949872067208728e-4, -4.3829709854172101e-4, -6.969091458420552e-7, 1.6644846642067548e-4, -1.2783517679769219e-4, 4.6299532636913043e-5, 4.5579098679227077e-9, -1.0595271125805195e-5, 6.7833429048651666e-6, -2.1075476666258804e-6, -1.7213731432817145e-11, 3.7735877416110979e-7, -2.1867506700122867e-7, 6.2202288040189269e-8, 6.5977038267330006e-16, -9.5903864974256858e-9, 5.2132144922808078e-9, -1.3991589583935709e-9, 5.382058999060575e-16, 1.9484714275467745e-10, -1.0127287556389682e-10, 2.6077347197254926e-11, -5.0904186999932993e-18, -3.3721464474854592e-12}, +{-5.9676129019274625e-4, -7.2048954160200106e-5, 6.7823088376673284e-4, -6.4014752602627585e-4, 2.7750107634328704e-4, 1.8197008380465151e-7, -8.4795071170685032e-5, 6.105192082501531e-5, -2.1073920183404862e-5, -8.8585890141255994e-10, 4.5284535953805377e-6, -2.8427815022504408e-6, 8.7082341778646412e-7, 3.6886101871706965e-12, -1.5344695190702061e-7, 8.862466778790695e-8, -2.5184812301826817e-8, -1.0225912098215092e-14, 3.8969470758154777e-9, -2.1267304792235635e-9, 5.7370135528051385e-10, -1.887749850169741e-19, -8.0931538694657866e-11, 4.2382723283449199e-11, -1.1002224534207726e-11}, +{1.3324454494800656e-3, -1.9144384985654775e-3, 1.1089369134596637e-3, 9.932404122642299e-7, -5.0874501293093199e-4, 4.2735056665392884e-4, -1.6858853767910799e-4, -8.1301893922784998e-9, 4.5284402370562147e-5, -3.127053674781734e-5, 1.044986828530338e-5, 4.8435226265680926e-11, -2.1482565873456258e-6, 1.329369701097492e-6, -4.0295693092101029e-7, -1.7567877666323291e-13, 7.0145043163668257e-8, -4.040787734999483e-8, 1.1474026743371963e-8, 3.9642746853563325e-18, -1.7804938269892714e-9, 9.7480262548731646e-10, -2.6405338676507616e-10, 5.794875163403742e-18, 3.7647749553543836e-11}, +{1.579727660730835e-3, 1.6251626278391582e-4, -2.0633421035543276e-3, 2.1389686185689098e-3, -1.0108559391263003e-3, -3.9912705529919201e-7, 3.6235025084764691e-4, -2.8143901463712154e-4, 1.0449513336495887e-4, 2.1211418491830297e-9, -2.5779417251947842e-5, 1.7281818956040463e-5, -5.6413773872904282e-6, -1.1024320105776174e-11, 1.1223224418895175e-6, -6.8693396379526735e-7, 2.0653236975414887e-7, 4.6714772409838506e-14, -3.5609886164949055e-8, 2.0470855345905963e-8, -5.8091738633283358e-9, -1.332821287582869e-16, 9.0354604391335133e-10, -4.9598782517330834e-10, 1.3481607129399749e-10}, +{-4.0725121195140166e-3, 6.4033628338080698e-3, -4.0410161081676618e-3, -2.183732802866233e-6, 2.1740441801254639e-3, -1.9700440518418892e-3, 8.3595469747962458e-4, 1.9445447567109655e-8, -2.5779387120421696e-4, 1.9009987368139304e-4, -6.7696499937438965e-5, -1.4440629666426572e-10, 1.5712512518742269e-5, -1.0304008744776893e-5, 3.304517767401387e-6, 7.9829760242325709e-13, -6.4097794149313004e-7, 3.8894624761300056e-7, -1.1618347644948869e-7, -2.816808630596451e-15, 1.9878012911297093e-8, -1.1407719956357511e-8, 3.2355857064185555e-9, 4.1759468293455945e-20, -5.0423112718105824e-10}, +{-5.9475779383993003e-3, -5.4016476789260452e-4, 8.7910413550767898e-3, -9.8576315587856125e-3, 5.0134695031021538e-3, 1.2807521786221875e-6, -2.0626019342754683e-3, 1.7109128573523058e-3, -6.7695312714133799e-4, -6.9011545676562133e-9, 1.8855128143995902e-4, -1.3395215663491969e-4, 4.6263183033528039e-5, 4.0034230613321351e-11, -1.0255652921494033e-5, 6.612086372797651e-6, -2.0913022027253008e-6, -2.0951775649603837e-13, 3.9756029041993247e-7, -2.3956211978815887e-7, 7.1182883382145864e-8, 8.925574873053455e-16, -1.2101547235064676e-8, 6.9350618248334386e-9, -1.9661464453856102e-9}, +{1.7402027787522711e-2, -2.9527880945699121e-2, 2.0045875571402799e-2, 7.0289515966903407e-6, -1.2375421071343148e-2, 1.1976293444235254e-2, -5.4156038466518525e-3, -6.3290893396418616e-8, 1.8855118129005065e-3, -1.473473274825001e-3, 5.5515810097708387e-4, 5.2406834412550662e-10, -1.4357913535784836e-4, 9.9181293224943297e-5, -3.3460834749478311e-5, -3.5755837291098993e-12, 7.1560851960630076e-6, -4.5516802628155526e-6, 1.4236576649271475e-6, 1.8803149082089664e-14, -2.6623403898929211e-7, 1.5950642189595716e-7, -4.7187514673841102e-8, -6.5107872958755177e-17, 7.9795091026746235e-9}, +{3.0249124160905891e-2, 2.4817436002649977e-3, -4.9939134373457022e-2, 5.9915643009307869e-2, -3.2483207601623391e-2, -5.7212968652103441e-6, 1.5085251778569354e-2, -1.3261324005088445e-2, 5.5515262632426148e-3, 3.0263182257030016e-8, -1.7229548406756723e-3, 1.2893570099929637e-3, -4.6845138348319876e-4, -1.830259937893045e-10, 1.1449739014822654e-4, -7.7378565221244477e-5, 2.5625836246985201e-5, 1.0766165333192814e-12, -5.3246809282422621e-6, 3.349634863064464e-6, -1.0381253128684018e-6, -5.608909920621128e-15, 1.9150821930676591e-7, -1.1418365800203486e-7, 3.3654425209171788e-8}, +{-9.9051020880159045e-2, 1.7954011706123486e-1, -1.2989606383463778e-1, -3.1478872752284357e-5, 9.0510635276848131e-2, -9.2828824411184397e-2, 4.4412112839877808e-2, 2.7779236316835888e-7, -1.7229543805449697e-2, 1.4182925050891573e-2, -5.6214161633747336e-3, -2.39598509186381e-9, 1.6029634366079908e-3, -1.1606784674435773e-3, 4.1001337768153873e-4, 1.8365800754090661e-11, -9.5844256563655903e-5, 6.3643062337764708e-5, -2.076250624489065e-5, -1.1806020912804483e-13, 4.2131808239120649e-6, -2.6262241337012467e-6, 8.0770620494930662e-7, 6.0125912123632725e-16, -1.4729737374018841e-7}, +{-1.9994542198219728e-1, -1.5056113040026424e-2, 3.6470239469348489e-1, -4.6435192311733545e-1, 2.6640934719197893e-1, 3.4038266027147191e-5, -1.3784338709329624e-1, 1.276467178337056e-1, -5.6213828755200985e-2, -1.753150885483011e-7, 1.9235592956768113e-2, -1.5088821281095315e-2, 5.7401854451350123e-3, 1.0622382710310225e-9, -1.5335082692563998e-3, 1.0819320643228214e-3, -3.7372510193945659e-4, -6.6170909729031985e-12, 8.4263617380909628e-5, -5.5150706827483479e-5, 1.7769536448348069e-5, 3.8827923210205533e-14, -3.53513697488768e-6, 2.1865832130045269e-6, -6.6812849447625594e-7}, +{7.2438608504029431e-1, -1.3918010932653375, 1.0654143352413968, 1.876173868950258e-4, -8.2705501176152696e-1, 8.9352433347828414e-1, -4.4971003995291339e-1, -1.6107401567546652e-6, 1.9235590165271091e-1, -1.6597702160042609e-1, 6.8882222681814333e-2, 1.3910091724608687e-8, -2.146911561508663e-2, 1.6228980898865892e-2, -5.9796016172584256e-3, -1.1287469112826745e-10, 1.5167451119784857e-3, -1.0478634293553899e-3, 3.5539072889126421e-4, 8.1704322111801517e-13, -7.7773013442452395e-5, 5.0291413897007722e-5, -1.6035083867000518e-5, 1.2469354315487605e-14, 3.1369106244517615e-6}, +{1.6668949727276811, 1.165462765994632e-1, -3.3288393225018906, 4.4692325482864037, -2.6977693045875807, -2.600667859891061e-4, 1.5389017615694539, -1.4937962361134612, 6.8881964633233148e-1, 1.3077482004552385e-6, -2.5762963325596288e-1, 2.1097676102125449e-1, -8.3714408359219882e-2, -7.7920428881354753e-9, 2.4267923064833599e-2, -1.7813678334552311e-2, 6.3970330388900056e-3, 4.9430807090480523e-11, -1.5554602758465635e-3, 1.0561196919903214e-3, -3.5277184460472902e-4, 9.3002334645022459e-14, 7.5285855026557172e-5, -4.8186515569156351e-5, 1.5227271505597605e-5}, +{-6.6188298861372935, 1.3397985455142589e+1, -1.0789350606845146e+1, -1.4352254537875018e-3, 9.2333694596189809, -1.0456552819547769e+1, 5.5105526029033471, 1.2024439690716742e-5, -2.5762961164755816, 2.3207442745387179, -1.0045728797216284, -1.0207833290021914e-7, 3.3975092171169466e-1, -2.6720517450757468e-1, 1.0235252851562706e-1, 8.4329730484871625e-10, -2.7998284958442595e-2, 2.0066274144976813e-2, -7.0554368915086242e-3, 1.9402238183698188e-12, 1.6562888105449611e-3, -1.1082898580743683e-3, 3.654545161310169e-4, -5.1290032026971794e-11, -7.6340103696869031e-5}, +{-1.7112706061976095e+1, -1.1208044642899116, 3.7131966511885444e+1, -5.2298271025348962e+1, 3.3058589696624618e+1, 2.4791298976200222e-3, -2.061089403411526e+1, 2.088672775145582e+1, -1.0045703956517752e+1, -1.2238783449063012e-5, 4.0770134274221141, -3.473667358470195, 1.4329352617312006, 7.1359914411879712e-8, -4.4797257159115612e-1, 3.4112666080644461e-1, -1.2699786326594923e-1, -2.8953677269081528e-10, 3.3125776278259863e-2, -2.3274087021036101e-2, 8.0399993503648882e-3, -1.177805216235265e-9, -1.8321624891071668e-3, 1.2108282933588665e-3, -3.9479941246822517e-4}, +{7.389033153567425e+1, -1.5680141270402273e+2, 1.322177542759164e+2, 1.3692876877324546e-2, -1.2366496885920151e+2, 1.4620689391062729e+2, -8.0365587724865346e+1, -1.1259851148881298e-4, 4.0770132196179938e+1, -3.8210340013273034e+1, 1.719522294277362e+1, 9.3519707955168356e-7, -6.2716159907747034, 5.1168999071852637, -2.0319658112299095, -4.9507215582761543e-9, 5.9626397294332597e-1, -4.4220765337238094e-1, 1.6079998700166273e-1, -2.4733786203223402e-8, -4.0307574759979762e-2, 2.7849050747097869e-2, -9.4751858992054221e-3, 6.419922235909132e-6, 2.1250180774699461e-3}, +{2.1216837098382522e+2, 1.3107863022633868e+1, -4.9698285932871748e+2, 7.3121595266969204e+2, -4.8213821720890847e+2, -2.8817248692894889e-2, 3.2616720302947102e+2, -3.4389340280087117e+2, 1.7195193870816232e+2, 1.4038077378096158e-4, -7.52594195897599e+1, 6.651969984520934e+1, -2.8447519748152462e+1, -7.613702615875391e-7, 9.5402237105304373, -7.5175301113311376, 2.8943997568871961, -4.6612194999538201e-7, -8.0615149598794088e-1, 5.8483006570631029e-1, -2.0845408972964956e-1, 1.4765818959305817e-4, 5.1000433863753019e-2, -3.3066252141883665e-2, 1.5109265210467774e-2}, +{-9.8959643098322368e+2, 2.1925555360905233e+3, -1.9283586782723356e+3, -1.5925738122215253e-1, 1.9569985945919857e+3, -2.4072514765081556e+3, 1.3756149959336496e+3, 1.2920735237496668e-3, -7.525941715948055e+2, 7.3171668742208716e+2, -3.4137023466220065e+2, -9.9857390260608043e-6, 1.3356313181291573e+2, -1.1276295161252794e+2, 4.6310396098204458e+1, -7.9237387133614756e-6, -1.4510726927018646e+1, 1.1111771248100563e+1, -4.1690817945270892, 3.1008219800117808e-3, 1.1220095449981468, -7.6052379926149916e-1, 3.6262236505085254e-1, 2.216867741940747e-1, 4.8683443692930507e-1}}; + +#endif diff --git a/gtsam/3rdparty/cephes/cephes/igami.c b/gtsam/3rdparty/cephes/cephes/igami.c new file mode 100644 index 0000000000..97fc93ff4d --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/igami.c @@ -0,0 +1,339 @@ +/* + * (C) Copyright John Maddock 2006. + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ +#include "mconf.h" + +static double find_inverse_s(double, double); +static double didonato_SN(double, double, unsigned, double); +static double find_inverse_gamma(double, double, double); + + +static double find_inverse_s(double p, double q) +{ + /* + * Computation of the Incomplete Gamma Function Ratios and their Inverse + * ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR. + * ACM Transactions on Mathematical Software, Vol. 12, No. 4, + * December 1986, Pages 377-393. + * + * See equation 32. + */ + double s, t; + double a[4] = {0.213623493715853, 4.28342155967104, + 11.6616720288968, 3.31125922108741}; + double b[5] = {0.3611708101884203e-1, 1.27364489782223, + 6.40691597760039, 6.61053765625462, 1}; + + if (p < 0.5) { + t = sqrt(-2 * log(p)); + } + else { + t = sqrt(-2 * log(q)); + } + s = t - polevl(t, a, 3) / polevl(t, b, 4); + if(p < 0.5) + s = -s; + return s; +} + + +static double didonato_SN(double a, double x, unsigned N, double tolerance) +{ + /* + * Computation of the Incomplete Gamma Function Ratios and their Inverse + * ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR. + * ACM Transactions on Mathematical Software, Vol. 12, No. 4, + * December 1986, Pages 377-393. + * + * See equation 34. + */ + double sum = 1.0; + + if (N >= 1) { + unsigned i; + double partial = x / (a + 1); + + sum += partial; + for(i = 2; i <= N; ++i) { + partial *= x / (a + i); + sum += partial; + if(partial < tolerance) { + break; + } + } + } + return sum; +} + + +static double find_inverse_gamma(double a, double p, double q) +{ + /* + * In order to understand what's going on here, you will + * need to refer to: + * + * Computation of the Incomplete Gamma Function Ratios and their Inverse + * ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR. + * ACM Transactions on Mathematical Software, Vol. 12, No. 4, + * December 1986, Pages 377-393. + */ + double result; + + if (a == 1) { + if (q > 0.9) { + result = -log1p(-p); + } + else { + result = -log(q); + } + } + else if (a < 1) { + double g = Gamma(a); + double b = q * g; + + if ((b > 0.6) || ((b >= 0.45) && (a >= 0.3))) { + /* DiDonato & Morris Eq 21: + * + * There is a slight variation from DiDonato and Morris here: + * the first form given here is unstable when p is close to 1, + * making it impossible to compute the inverse of Q(a,x) for small + * q. Fortunately the second form works perfectly well in this case. + */ + double u; + if((b * q > 1e-8) && (q > 1e-5)) { + u = pow(p * g * a, 1 / a); + } + else { + u = exp((-q / a) - SCIPY_EULER); + } + result = u / (1 - (u / (a + 1))); + } + else if ((a < 0.3) && (b >= 0.35)) { + /* DiDonato & Morris Eq 22: */ + double t = exp(-SCIPY_EULER - b); + double u = t * exp(t); + result = t * exp(u); + } + else if ((b > 0.15) || (a >= 0.3)) { + /* DiDonato & Morris Eq 23: */ + double y = -log(b); + double u = y - (1 - a) * log(y); + result = y - (1 - a) * log(u) - log(1 + (1 - a) / (1 + u)); + } + else if (b > 0.1) { + /* DiDonato & Morris Eq 24: */ + double y = -log(b); + double u = y - (1 - a) * log(y); + result = y - (1 - a) * log(u) + - log((u * u + 2 * (3 - a) * u + (2 - a) * (3 - a)) + / (u * u + (5 - a) * u + 2)); + } + else { + /* DiDonato & Morris Eq 25: */ + double y = -log(b); + double c1 = (a - 1) * log(y); + double c1_2 = c1 * c1; + double c1_3 = c1_2 * c1; + double c1_4 = c1_2 * c1_2; + double a_2 = a * a; + double a_3 = a_2 * a; + + double c2 = (a - 1) * (1 + c1); + double c3 = (a - 1) * (-(c1_2 / 2) + + (a - 2) * c1 + + (3 * a - 5) / 2); + double c4 = (a - 1) * ((c1_3 / 3) - (3 * a - 5) * c1_2 / 2 + + (a_2 - 6 * a + 7) * c1 + + (11 * a_2 - 46 * a + 47) / 6); + double c5 = (a - 1) * (-(c1_4 / 4) + + (11 * a - 17) * c1_3 / 6 + + (-3 * a_2 + 13 * a -13) * c1_2 + + (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2 + + (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12); + + double y_2 = y * y; + double y_3 = y_2 * y; + double y_4 = y_2 * y_2; + result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4); + } + } + else { + /* DiDonato and Morris Eq 31: */ + double s = find_inverse_s(p, q); + + double s_2 = s * s; + double s_3 = s_2 * s; + double s_4 = s_2 * s_2; + double s_5 = s_4 * s; + double ra = sqrt(a); + + double w = a + s * ra + (s_2 - 1) / 3; + w += (s_3 - 7 * s) / (36 * ra); + w -= (3 * s_4 + 7 * s_2 - 16) / (810 * a); + w += (9 * s_5 + 256 * s_3 - 433 * s) / (38880 * a * ra); + + if ((a >= 500) && (fabs(1 - w / a) < 1e-6)) { + result = w; + } + else if (p > 0.5) { + if (w < 3 * a) { + result = w; + } + else { + double D = fmax(2, a * (a - 1)); + double lg = lgam(a); + double lb = log(q) + lg; + if (lb < -D * 2.3) { + /* DiDonato and Morris Eq 25: */ + double y = -lb; + double c1 = (a - 1) * log(y); + double c1_2 = c1 * c1; + double c1_3 = c1_2 * c1; + double c1_4 = c1_2 * c1_2; + double a_2 = a * a; + double a_3 = a_2 * a; + + double c2 = (a - 1) * (1 + c1); + double c3 = (a - 1) * (-(c1_2 / 2) + + (a - 2) * c1 + + (3 * a - 5) / 2); + double c4 = (a - 1) * ((c1_3 / 3) + - (3 * a - 5) * c1_2 / 2 + + (a_2 - 6 * a + 7) * c1 + + (11 * a_2 - 46 * a + 47) / 6); + double c5 = (a - 1) * (-(c1_4 / 4) + + (11 * a - 17) * c1_3 / 6 + + (-3 * a_2 + 13 * a -13) * c1_2 + + (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2 + + (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12); + + double y_2 = y * y; + double y_3 = y_2 * y; + double y_4 = y_2 * y_2; + result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4); + } + else { + /* DiDonato and Morris Eq 33: */ + double u = -lb + (a - 1) * log(w) - log(1 + (1 - a) / (1 + w)); + result = -lb + (a - 1) * log(u) - log(1 + (1 - a) / (1 + u)); + } + } + } + else { + double z = w; + double ap1 = a + 1; + double ap2 = a + 2; + if (w < 0.15 * ap1) { + /* DiDonato and Morris Eq 35: */ + double v = log(p) + lgam(ap1); + z = exp((v + w) / a); + s = log1p(z / ap1 * (1 + z / ap2)); + z = exp((v + z - s) / a); + s = log1p(z / ap1 * (1 + z / ap2)); + z = exp((v + z - s) / a); + s = log1p(z / ap1 * (1 + z / ap2 * (1 + z / (a + 3)))); + z = exp((v + z - s) / a); + } + + if ((z <= 0.01 * ap1) || (z > 0.7 * ap1)) { + result = z; + } + else { + /* DiDonato and Morris Eq 36: */ + double ls = log(didonato_SN(a, z, 100, 1e-4)); + double v = log(p) + lgam(ap1); + z = exp((v + z - ls) / a); + result = z * (1 - (a * log(z) - z - v + ls) / (a - z)); + } + } + } + return result; +} + + +double igami(double a, double p) +{ + int i; + double x, fac, f_fp, fpp_fp; + + if (isnan(a) || isnan(p)) { + return NAN; + } + else if ((a < 0) || (p < 0) || (p > 1)) { + sf_error("gammaincinv", SF_ERROR_DOMAIN, NULL); + } + else if (p == 0.0) { + return 0.0; + } + else if (p == 1.0) { + return INFINITY; + } + else if (p > 0.9) { + return igamci(a, 1 - p); + } + + x = find_inverse_gamma(a, p, 1 - p); + /* Halley's method */ + for (i = 0; i < 3; i++) { + fac = igam_fac(a, x); + if (fac == 0.0) { + return x; + } + f_fp = (igam(a, x) - p) * x / fac; + /* The ratio of the first and second derivatives simplifies */ + fpp_fp = -1.0 + (a - 1) / x; + if (isinf(fpp_fp)) { + /* Resort to Newton's method in the case of overflow */ + x = x - f_fp; + } + else { + x = x - f_fp / (1.0 - 0.5 * f_fp * fpp_fp); + } + } + + return x; +} + + +double igamci(double a, double q) +{ + int i; + double x, fac, f_fp, fpp_fp; + + if (isnan(a) || isnan(q)) { + return NAN; + } + else if ((a < 0.0) || (q < 0.0) || (q > 1.0)) { + sf_error("gammainccinv", SF_ERROR_DOMAIN, NULL); + } + else if (q == 0.0) { + return INFINITY; + } + else if (q == 1.0) { + return 0.0; + } + else if (q > 0.9) { + return igami(a, 1 - q); + } + + x = find_inverse_gamma(a, 1 - q, q); + for (i = 0; i < 3; i++) { + fac = igam_fac(a, x); + if (fac == 0.0) { + return x; + } + f_fp = (igamc(a, x) - q) * x / (-fac); + fpp_fp = -1.0 + (a - 1) / x; + if (isinf(fpp_fp)) { + x = x - f_fp; + } + else { + x = x - f_fp / (1.0 - 0.5 * f_fp * fpp_fp); + } + } + + return x; +} diff --git a/gtsam/3rdparty/cephes/cephes/incbet.c b/gtsam/3rdparty/cephes/cephes/incbet.c new file mode 100644 index 0000000000..b03427f4f7 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/incbet.c @@ -0,0 +1,369 @@ +/* incbet.c + * + * Incomplete beta integral + * + * + * SYNOPSIS: + * + * double a, b, x, y, incbet(); + * + * y = incbet( a, b, x ); + * + * + * DESCRIPTION: + * + * Returns incomplete beta integral of the arguments, evaluated + * from zero to x. The function is defined as + * + * x + * - - + * | (a+b) | | a-1 b-1 + * ----------- | t (1-t) dt. + * - - | | + * | (a) | (b) - + * 0 + * + * The domain of definition is 0 <= x <= 1. In this + * implementation a and b are restricted to positive values. + * The integral from x to 1 may be obtained by the symmetry + * relation + * + * 1 - incbet( a, b, x ) = incbet( b, a, 1-x ). + * + * The integral is evaluated by a continued fraction expansion + * or, when b*x is small, by a power series. + * + * ACCURACY: + * + * Tested at uniformly distributed random points (a,b,x) with a and b + * in "domain" and x between 0 and 1. + * Relative error + * arithmetic domain # trials peak rms + * IEEE 0,5 10000 6.9e-15 4.5e-16 + * IEEE 0,85 250000 2.2e-13 1.7e-14 + * IEEE 0,1000 30000 5.3e-12 6.3e-13 + * IEEE 0,10000 250000 9.3e-11 7.1e-12 + * IEEE 0,100000 10000 8.7e-10 4.8e-11 + * Outputs smaller than the IEEE gradual underflow threshold + * were excluded from these statistics. + * + * ERROR MESSAGES: + * message condition value returned + * incbet domain x<0, x>1 0.0 + * incbet underflow 0.0 + */ + + +/* + * Cephes Math Library, Release 2.3: March, 1995 + * Copyright 1984, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +#define MAXGAM 171.624376956302725 + +extern double MACHEP, MINLOG, MAXLOG; + +static double big = 4.503599627370496e15; +static double biginv = 2.22044604925031308085e-16; + + +/* Power series for incomplete beta integral. + * Use when b*x is small and x not too close to 1. */ + +static double pseries(double a, double b, double x) +{ + double s, t, u, v, n, t1, z, ai; + + ai = 1.0 / a; + u = (1.0 - b) * x; + v = u / (a + 1.0); + t1 = v; + t = u; + n = 2.0; + s = 0.0; + z = MACHEP * ai; + while (fabs(v) > z) { + u = (n - b) * x / n; + t *= u; + v = t / (a + n); + s += v; + n += 1.0; + } + s += t1; + s += ai; + + u = a * log(x); + if ((a + b) < MAXGAM && fabs(u) < MAXLOG) { + t = 1.0 / beta(a, b); + s = s * t * pow(x, a); + } + else { + t = -lbeta(a,b) + u + log(s); + if (t < MINLOG) + s = 0.0; + else + s = exp(t); + } + return (s); +} + + +/* Continued fraction expansion #1 for incomplete beta integral */ + +static double incbcf(double a, double b, double x) +{ + double xk, pk, pkm1, pkm2, qk, qkm1, qkm2; + double k1, k2, k3, k4, k5, k6, k7, k8; + double r, t, ans, thresh; + int n; + + k1 = a; + k2 = a + b; + k3 = a; + k4 = a + 1.0; + k5 = 1.0; + k6 = b - 1.0; + k7 = k4; + k8 = a + 2.0; + + pkm2 = 0.0; + qkm2 = 1.0; + pkm1 = 1.0; + qkm1 = 1.0; + ans = 1.0; + r = 1.0; + n = 0; + thresh = 3.0 * MACHEP; + do { + + xk = -(x * k1 * k2) / (k3 * k4); + pk = pkm1 + pkm2 * xk; + qk = qkm1 + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + xk = (x * k5 * k6) / (k7 * k8); + pk = pkm1 + pkm2 * xk; + qk = qkm1 + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + if (qk != 0) + r = pk / qk; + if (r != 0) { + t = fabs((ans - r) / r); + ans = r; + } + else + t = 1.0; + + if (t < thresh) + goto cdone; + + k1 += 1.0; + k2 += 1.0; + k3 += 2.0; + k4 += 2.0; + k5 += 1.0; + k6 -= 1.0; + k7 += 2.0; + k8 += 2.0; + + if ((fabs(qk) + fabs(pk)) > big) { + pkm2 *= biginv; + pkm1 *= biginv; + qkm2 *= biginv; + qkm1 *= biginv; + } + if ((fabs(qk) < biginv) || (fabs(pk) < biginv)) { + pkm2 *= big; + pkm1 *= big; + qkm2 *= big; + qkm1 *= big; + } + } + while (++n < 300); + + cdone: + return (ans); +} + + +/* Continued fraction expansion #2 for incomplete beta integral */ + +static double incbd(double a, double b, double x) +{ + double xk, pk, pkm1, pkm2, qk, qkm1, qkm2; + double k1, k2, k3, k4, k5, k6, k7, k8; + double r, t, ans, z, thresh; + int n; + + k1 = a; + k2 = b - 1.0; + k3 = a; + k4 = a + 1.0; + k5 = 1.0; + k6 = a + b; + k7 = a + 1.0;; + k8 = a + 2.0; + + pkm2 = 0.0; + qkm2 = 1.0; + pkm1 = 1.0; + qkm1 = 1.0; + z = x / (1.0 - x); + ans = 1.0; + r = 1.0; + n = 0; + thresh = 3.0 * MACHEP; + do { + + xk = -(z * k1 * k2) / (k3 * k4); + pk = pkm1 + pkm2 * xk; + qk = qkm1 + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + xk = (z * k5 * k6) / (k7 * k8); + pk = pkm1 + pkm2 * xk; + qk = qkm1 + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + if (qk != 0) + r = pk / qk; + if (r != 0) { + t = fabs((ans - r) / r); + ans = r; + } + else + t = 1.0; + + if (t < thresh) + goto cdone; + + k1 += 1.0; + k2 -= 1.0; + k3 += 2.0; + k4 += 2.0; + k5 += 1.0; + k6 += 1.0; + k7 += 2.0; + k8 += 2.0; + + if ((fabs(qk) + fabs(pk)) > big) { + pkm2 *= biginv; + pkm1 *= biginv; + qkm2 *= biginv; + qkm1 *= biginv; + } + if ((fabs(qk) < biginv) || (fabs(pk) < biginv)) { + pkm2 *= big; + pkm1 *= big; + qkm2 *= big; + qkm1 *= big; + } + } + while (++n < 300); + cdone: + return (ans); +} + + +double incbet(double aa, double bb, double xx) +{ + double a, b, t, x, xc, w, y; + int flag; + + if (aa <= 0.0 || bb <= 0.0) + goto domerr; + + if ((xx <= 0.0) || (xx >= 1.0)) { + if (xx == 0.0) + return (0.0); + if (xx == 1.0) + return (1.0); + domerr: + sf_error("incbet", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + flag = 0; + if ((bb * xx) <= 1.0 && xx <= 0.95) { + t = pseries(aa, bb, xx); + goto done; + } + + w = 1.0 - xx; + + /* Reverse a and b if x is greater than the mean. */ + if (xx > (aa / (aa + bb))) { + flag = 1; + a = bb; + b = aa; + xc = xx; + x = w; + } + else { + a = aa; + b = bb; + xc = w; + x = xx; + } + + if (flag == 1 && (b * x) <= 1.0 && x <= 0.95) { + t = pseries(a, b, x); + goto done; + } + + /* Choose expansion for better convergence. */ + y = x * (a + b - 2.0) - (a - 1.0); + if (y < 0.0) + w = incbcf(a, b, x); + else + w = incbd(a, b, x) / xc; + + /* Multiply w by the factor + * a b _ _ _ + * x (1-x) | (a+b) / ( a | (a) | (b) ) . */ + + y = a * log(x); + t = b * log(xc); + if ((a + b) < MAXGAM && fabs(y) < MAXLOG && fabs(t) < MAXLOG) { + t = pow(xc, b); + t *= pow(x, a); + t /= a; + t *= w; + t *= 1.0 / beta(a, b); + goto done; + } + /* Resort to logarithms. */ + y += t - lbeta(a,b); + y += log(w / a); + if (y < MINLOG) + t = 0.0; + else + t = exp(y); + + done: + + if (flag == 1) { + if (t <= MACHEP) + t = 1.0 - MACHEP; + else + t = 1.0 - t; + } + return (t); +} + + diff --git a/gtsam/3rdparty/cephes/cephes/incbi.c b/gtsam/3rdparty/cephes/cephes/incbi.c new file mode 100644 index 0000000000..747c43f538 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/incbi.c @@ -0,0 +1,275 @@ +/* incbi() + * + * Inverse of incomplete beta integral + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, incbi(); + * + * x = incbi( a, b, y ); + * + * + * + * DESCRIPTION: + * + * Given y, the function finds x such that + * + * incbet( a, b, x ) = y . + * + * The routine performs interval halving or Newton iterations to find the + * root of incbet(a,b,x) - y = 0. + * + * + * ACCURACY: + * + * Relative error: + * x a,b + * arithmetic domain domain # trials peak rms + * IEEE 0,1 .5,10000 50000 5.8e-12 1.3e-13 + * IEEE 0,1 .25,100 100000 1.8e-13 3.9e-15 + * IEEE 0,1 0,5 50000 1.1e-12 5.5e-15 + * VAX 0,1 .5,100 25000 3.5e-14 1.1e-15 + * With a and b constrained to half-integer or integer values: + * IEEE 0,1 .5,10000 50000 5.8e-12 1.1e-13 + * IEEE 0,1 .5,100 100000 1.7e-14 7.9e-16 + * With a = .5, b constrained to half-integer or integer values: + * IEEE 0,1 .5,10000 10000 8.3e-11 1.0e-11 + */ + + +/* + * Cephes Math Library Release 2.4: March,1996 + * Copyright 1984, 1996 by Stephen L. Moshier + */ + +#include "mconf.h" + +extern double MACHEP, MAXLOG, MINLOG; + +double incbi(double aa, double bb, double yy0) +{ + double a, b, y0, d, y, x, x0, x1, lgm, yp, di, dithresh, yl, yh, xt; + int i, rflg, dir, nflg; + + + i = 0; + if (yy0 <= 0) + return (0.0); + if (yy0 >= 1.0) + return (1.0); + x0 = 0.0; + yl = 0.0; + x1 = 1.0; + yh = 1.0; + nflg = 0; + + if (aa <= 1.0 || bb <= 1.0) { + dithresh = 1.0e-6; + rflg = 0; + a = aa; + b = bb; + y0 = yy0; + x = a / (a + b); + y = incbet(a, b, x); + goto ihalve; + } + else { + dithresh = 1.0e-4; + } + /* approximation to inverse function */ + + yp = -ndtri(yy0); + + if (yy0 > 0.5) { + rflg = 1; + a = bb; + b = aa; + y0 = 1.0 - yy0; + yp = -yp; + } + else { + rflg = 0; + a = aa; + b = bb; + y0 = yy0; + } + + lgm = (yp * yp - 3.0) / 6.0; + x = 2.0 / (1.0 / (2.0 * a - 1.0) + 1.0 / (2.0 * b - 1.0)); + d = yp * sqrt(x + lgm) / x + - (1.0 / (2.0 * b - 1.0) - 1.0 / (2.0 * a - 1.0)) + * (lgm + 5.0 / 6.0 - 2.0 / (3.0 * x)); + d = 2.0 * d; + if (d < MINLOG) { + x = 1.0; + goto under; + } + x = a / (a + b * exp(d)); + y = incbet(a, b, x); + yp = (y - y0) / y0; + if (fabs(yp) < 0.2) + goto newt; + + /* Resort to interval halving if not close enough. */ + ihalve: + + dir = 0; + di = 0.5; + for (i = 0; i < 100; i++) { + if (i != 0) { + x = x0 + di * (x1 - x0); + if (x == 1.0) + x = 1.0 - MACHEP; + if (x == 0.0) { + di = 0.5; + x = x0 + di * (x1 - x0); + if (x == 0.0) + goto under; + } + y = incbet(a, b, x); + yp = (x1 - x0) / (x1 + x0); + if (fabs(yp) < dithresh) + goto newt; + yp = (y - y0) / y0; + if (fabs(yp) < dithresh) + goto newt; + } + if (y < y0) { + x0 = x; + yl = y; + if (dir < 0) { + dir = 0; + di = 0.5; + } + else if (dir > 3) + di = 1.0 - (1.0 - di) * (1.0 - di); + else if (dir > 1) + di = 0.5 * di + 0.5; + else + di = (y0 - y) / (yh - yl); + dir += 1; + if (x0 > 0.75) { + if (rflg == 1) { + rflg = 0; + a = aa; + b = bb; + y0 = yy0; + } + else { + rflg = 1; + a = bb; + b = aa; + y0 = 1.0 - yy0; + } + x = 1.0 - x; + y = incbet(a, b, x); + x0 = 0.0; + yl = 0.0; + x1 = 1.0; + yh = 1.0; + goto ihalve; + } + } + else { + x1 = x; + if (rflg == 1 && x1 < MACHEP) { + x = 0.0; + goto done; + } + yh = y; + if (dir > 0) { + dir = 0; + di = 0.5; + } + else if (dir < -3) + di = di * di; + else if (dir < -1) + di = 0.5 * di; + else + di = (y - y0) / (yh - yl); + dir -= 1; + } + } + sf_error("incbi", SF_ERROR_LOSS, NULL); + if (x0 >= 1.0) { + x = 1.0 - MACHEP; + goto done; + } + if (x <= 0.0) { + under: + sf_error("incbi", SF_ERROR_UNDERFLOW, NULL); + x = 0.0; + goto done; + } + + newt: + + if (nflg) + goto done; + nflg = 1; + lgm = lgam(a + b) - lgam(a) - lgam(b); + + for (i = 0; i < 8; i++) { + /* Compute the function at this point. */ + if (i != 0) + y = incbet(a, b, x); + if (y < yl) { + x = x0; + y = yl; + } + else if (y > yh) { + x = x1; + y = yh; + } + else if (y < y0) { + x0 = x; + yl = y; + } + else { + x1 = x; + yh = y; + } + if (x == 1.0 || x == 0.0) + break; + /* Compute the derivative of the function at this point. */ + d = (a - 1.0) * log(x) + (b - 1.0) * log(1.0 - x) + lgm; + if (d < MINLOG) + goto done; + if (d > MAXLOG) + break; + d = exp(d); + /* Compute the step to the next approximation of x. */ + d = (y - y0) / d; + xt = x - d; + if (xt <= x0) { + y = (x - x0) / (x1 - x0); + xt = x0 + 0.5 * y * (x - x0); + if (xt <= 0.0) + break; + } + if (xt >= x1) { + y = (x1 - x) / (x1 - x0); + xt = x1 - 0.5 * y * (x1 - x); + if (xt >= 1.0) + break; + } + x = xt; + if (fabs(d / x) < 128.0 * MACHEP) + goto done; + } + /* Did not converge. */ + dithresh = 256.0 * MACHEP; + goto ihalve; + + done: + + if (rflg) { + if (x <= MACHEP) + x = 1.0 - MACHEP; + else + x = 1.0 - x; + } + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/j0.c b/gtsam/3rdparty/cephes/cephes/j0.c new file mode 100644 index 0000000000..094ef6cef1 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/j0.c @@ -0,0 +1,246 @@ +/* j0.c + * + * Bessel function of order zero + * + * + * + * SYNOPSIS: + * + * double x, y, j0(); + * + * y = j0( x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of order zero of the argument. + * + * The domain is divided into the intervals [0, 5] and + * (5, infinity). In the first interval the following rational + * approximation is used: + * + * + * 2 2 + * (w - r ) (w - r ) P (w) / Q (w) + * 1 2 3 8 + * + * 2 + * where w = x and the two r's are zeros of the function. + * + * In the second interval, the Hankel asymptotic expansion + * is employed with two rational functions of degree 6/6 + * and 7/7. + * + * + * + * ACCURACY: + * + * Absolute error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 60000 4.2e-16 1.1e-16 + * + */ + /* y0.c + * + * Bessel function of the second kind, order zero + * + * + * + * SYNOPSIS: + * + * double x, y, y0(); + * + * y = y0( x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of the second kind, of order + * zero, of the argument. + * + * The domain is divided into the intervals [0, 5] and + * (5, infinity). In the first interval a rational approximation + * R(x) is employed to compute + * y0(x) = R(x) + 2 * log(x) * j0(x) / M_PI. + * Thus a call to j0() is required. + * + * In the second interval, the Hankel asymptotic expansion + * is employed with two rational functions of degree 6/6 + * and 7/7. + * + * + * + * ACCURACY: + * + * Absolute error, when y0(x) < 1; else relative error: + * + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.3e-15 1.6e-16 + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier + */ + +/* Note: all coefficients satisfy the relative error criterion + * except YP, YQ which are designed for absolute error. */ + +#include "mconf.h" + +static double PP[7] = { + 7.96936729297347051624E-4, + 8.28352392107440799803E-2, + 1.23953371646414299388E0, + 5.44725003058768775090E0, + 8.74716500199817011941E0, + 5.30324038235394892183E0, + 9.99999999999999997821E-1, +}; + +static double PQ[7] = { + 9.24408810558863637013E-4, + 8.56288474354474431428E-2, + 1.25352743901058953537E0, + 5.47097740330417105182E0, + 8.76190883237069594232E0, + 5.30605288235394617618E0, + 1.00000000000000000218E0, +}; + +static double QP[8] = { + -1.13663838898469149931E-2, + -1.28252718670509318512E0, + -1.95539544257735972385E1, + -9.32060152123768231369E1, + -1.77681167980488050595E2, + -1.47077505154951170175E2, + -5.14105326766599330220E1, + -6.05014350600728481186E0, +}; + +static double QQ[7] = { + /* 1.00000000000000000000E0, */ + 6.43178256118178023184E1, + 8.56430025976980587198E2, + 3.88240183605401609683E3, + 7.24046774195652478189E3, + 5.93072701187316984827E3, + 2.06209331660327847417E3, + 2.42005740240291393179E2, +}; + +static double YP[8] = { + 1.55924367855235737965E4, + -1.46639295903971606143E7, + 5.43526477051876500413E9, + -9.82136065717911466409E11, + 8.75906394395366999549E13, + -3.46628303384729719441E15, + 4.42733268572569800351E16, + -1.84950800436986690637E16, +}; + +static double YQ[7] = { + /* 1.00000000000000000000E0, */ + 1.04128353664259848412E3, + 6.26107330137134956842E5, + 2.68919633393814121987E8, + 8.64002487103935000337E10, + 2.02979612750105546709E13, + 3.17157752842975028269E15, + 2.50596256172653059228E17, +}; + +/* 5.783185962946784521175995758455807035071 */ +static double DR1 = 5.78318596294678452118E0; + +/* 30.47126234366208639907816317502275584842 */ +static double DR2 = 3.04712623436620863991E1; + +static double RP[4] = { + -4.79443220978201773821E9, + 1.95617491946556577543E12, + -2.49248344360967716204E14, + 9.70862251047306323952E15, +}; + +static double RQ[8] = { + /* 1.00000000000000000000E0, */ + 4.99563147152651017219E2, + 1.73785401676374683123E5, + 4.84409658339962045305E7, + 1.11855537045356834862E10, + 2.11277520115489217587E12, + 3.10518229857422583814E14, + 3.18121955943204943306E16, + 1.71086294081043136091E18, +}; + +extern double SQ2OPI; + +double j0(double x) +{ + double w, z, p, q, xn; + + if (x < 0) + x = -x; + + if (x <= 5.0) { + z = x * x; + if (x < 1.0e-5) + return (1.0 - z / 4.0); + + p = (z - DR1) * (z - DR2); + p = p * polevl(z, RP, 3) / p1evl(z, RQ, 8); + return (p); + } + + w = 5.0 / x; + q = 25.0 / (x * x); + p = polevl(q, PP, 6) / polevl(q, PQ, 6); + q = polevl(q, QP, 7) / p1evl(q, QQ, 7); + xn = x - M_PI_4; + p = p * cos(xn) - w * q * sin(xn); + return (p * SQ2OPI / sqrt(x)); +} + +/* y0() 2 */ +/* Bessel function of second kind, order zero */ + +/* Rational approximation coefficients YP[], YQ[] are used here. + * The function computed is y0(x) - 2 * log(x) * j0(x) / M_PI, + * whose value at x = 0 is 2 * ( log(0.5) + EUL ) / M_PI + * = 0.073804295108687225. + */ + +double y0(double x) +{ + double w, z, p, q, xn; + + if (x <= 5.0) { + if (x == 0.0) { + sf_error("y0", SF_ERROR_SINGULAR, NULL); + return -INFINITY; + } + else if (x < 0.0) { + sf_error("y0", SF_ERROR_DOMAIN, NULL); + return NAN; + } + z = x * x; + w = polevl(z, YP, 7) / p1evl(z, YQ, 7); + w += M_2_PI * log(x) * j0(x); + return (w); + } + + w = 5.0 / x; + z = 25.0 / (x * x); + p = polevl(z, PP, 6) / polevl(z, PQ, 6); + q = polevl(z, QP, 7) / p1evl(z, QQ, 7); + xn = x - M_PI_4; + p = p * sin(xn) + w * q * cos(xn); + return (p * SQ2OPI / sqrt(x)); +} diff --git a/gtsam/3rdparty/cephes/cephes/j1.c b/gtsam/3rdparty/cephes/cephes/j1.c new file mode 100644 index 0000000000..123194de84 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/j1.c @@ -0,0 +1,225 @@ +/* j1.c + * + * Bessel function of order one + * + * + * + * SYNOPSIS: + * + * double x, y, j1(); + * + * y = j1( x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of order one of the argument. + * + * The domain is divided into the intervals [0, 8] and + * (8, infinity). In the first interval a 24 term Chebyshev + * expansion is used. In the second, the asymptotic + * trigonometric representation is employed using two + * rational functions of degree 5/5. + * + * + * + * ACCURACY: + * + * Absolute error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 2.6e-16 1.1e-16 + * + * + */ + /* y1.c + * + * Bessel function of second kind of order one + * + * + * + * SYNOPSIS: + * + * double x, y, y1(); + * + * y = y1( x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of the second kind of order one + * of the argument. + * + * The domain is divided into the intervals [0, 8] and + * (8, infinity). In the first interval a 25 term Chebyshev + * expansion is used, and a call to j1() is required. + * In the second, the asymptotic trigonometric representation + * is employed using two rational functions of degree 5/5. + * + * + * + * ACCURACY: + * + * Absolute error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.0e-15 1.3e-16 + * + * (error criterion relative when |y1| > 1). + * + */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier + */ + +/* + * #define PIO4 .78539816339744830962 + * #define THPIO4 2.35619449019234492885 + * #define SQ2OPI .79788456080286535588 + */ + +#include "mconf.h" + +static double RP[4] = { + -8.99971225705559398224E8, + 4.52228297998194034323E11, + -7.27494245221818276015E13, + 3.68295732863852883286E15, +}; + +static double RQ[8] = { + /* 1.00000000000000000000E0, */ + 6.20836478118054335476E2, + 2.56987256757748830383E5, + 8.35146791431949253037E7, + 2.21511595479792499675E10, + 4.74914122079991414898E12, + 7.84369607876235854894E14, + 8.95222336184627338078E16, + 5.32278620332680085395E18, +}; + +static double PP[7] = { + 7.62125616208173112003E-4, + 7.31397056940917570436E-2, + 1.12719608129684925192E0, + 5.11207951146807644818E0, + 8.42404590141772420927E0, + 5.21451598682361504063E0, + 1.00000000000000000254E0, +}; + +static double PQ[7] = { + 5.71323128072548699714E-4, + 6.88455908754495404082E-2, + 1.10514232634061696926E0, + 5.07386386128601488557E0, + 8.39985554327604159757E0, + 5.20982848682361821619E0, + 9.99999999999999997461E-1, +}; + +static double QP[8] = { + 5.10862594750176621635E-2, + 4.98213872951233449420E0, + 7.58238284132545283818E1, + 3.66779609360150777800E2, + 7.10856304998926107277E2, + 5.97489612400613639965E2, + 2.11688757100572135698E2, + 2.52070205858023719784E1, +}; + +static double QQ[7] = { + /* 1.00000000000000000000E0, */ + 7.42373277035675149943E1, + 1.05644886038262816351E3, + 4.98641058337653607651E3, + 9.56231892404756170795E3, + 7.99704160447350683650E3, + 2.82619278517639096600E3, + 3.36093607810698293419E2, +}; + +static double YP[6] = { + 1.26320474790178026440E9, + -6.47355876379160291031E11, + 1.14509511541823727583E14, + -8.12770255501325109621E15, + 2.02439475713594898196E17, + -7.78877196265950026825E17, +}; + +static double YQ[8] = { + /* 1.00000000000000000000E0, */ + 5.94301592346128195359E2, + 2.35564092943068577943E5, + 7.34811944459721705660E7, + 1.87601316108706159478E10, + 3.88231277496238566008E12, + 6.20557727146953693363E14, + 6.87141087355300489866E16, + 3.97270608116560655612E18, +}; + + +static double Z1 = 1.46819706421238932572E1; +static double Z2 = 4.92184563216946036703E1; + +extern double THPIO4, SQ2OPI; + +double j1(double x) +{ + double w, z, p, q, xn; + + w = x; + if (x < 0) + return -j1(-x); + + if (w <= 5.0) { + z = x * x; + w = polevl(z, RP, 3) / p1evl(z, RQ, 8); + w = w * x * (z - Z1) * (z - Z2); + return (w); + } + + w = 5.0 / x; + z = w * w; + p = polevl(z, PP, 6) / polevl(z, PQ, 6); + q = polevl(z, QP, 7) / p1evl(z, QQ, 7); + xn = x - THPIO4; + p = p * cos(xn) - w * q * sin(xn); + return (p * SQ2OPI / sqrt(x)); +} + + +double y1(double x) +{ + double w, z, p, q, xn; + + if (x <= 5.0) { + if (x == 0.0) { + sf_error("y1", SF_ERROR_SINGULAR, NULL); + return -INFINITY; + } + else if (x <= 0.0) { + sf_error("y1", SF_ERROR_DOMAIN, NULL); + return NAN; + } + z = x * x; + w = x * (polevl(z, YP, 5) / p1evl(z, YQ, 8)); + w += M_2_PI * (j1(x) * log(x) - 1.0 / x); + return (w); + } + + w = 5.0 / x; + z = w * w; + p = polevl(z, PP, 6) / polevl(z, PQ, 6); + q = polevl(z, QP, 7) / p1evl(z, QQ, 7); + xn = x - THPIO4; + p = p * sin(xn) + w * q * cos(xn); + return (p * SQ2OPI / sqrt(x)); +} diff --git a/gtsam/3rdparty/cephes/cephes/jv.c b/gtsam/3rdparty/cephes/cephes/jv.c new file mode 100644 index 0000000000..3434c18f31 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/jv.c @@ -0,0 +1,841 @@ +/* jv.c + * + * Bessel function of noninteger order + * + * + * + * SYNOPSIS: + * + * double v, x, y, jv(); + * + * y = jv( v, x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of order v of the argument, + * where v is real. Negative x is allowed if v is an integer. + * + * Several expansions are included: the ascending power + * series, the Hankel expansion, and two transitional + * expansions for large v. If v is not too large, it + * is reduced by recurrence to a region of best accuracy. + * The transitional expansions give 12D accuracy for v > 500. + * + * + * + * ACCURACY: + * Results for integer v are indicated by *, where x and v + * both vary from -125 to +125. Otherwise, + * x ranges from 0 to 125, v ranges as indicated by "domain." + * Error criterion is absolute, except relative when |jv()| > 1. + * + * arithmetic v domain x domain # trials peak rms + * IEEE 0,125 0,125 100000 4.6e-15 2.2e-16 + * IEEE -125,0 0,125 40000 5.4e-11 3.7e-13 + * IEEE 0,500 0,500 20000 4.4e-15 4.0e-16 + * Integer v: + * IEEE -125,125 -125,125 50000 3.5e-15* 1.9e-16* + * + */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1989, 1992, 2000 by Stephen L. Moshier + */ + + +#include "mconf.h" +#define CEPHES_DEBUG 0 + +#if CEPHES_DEBUG +#include +#endif + +#define MAXGAM 171.624376956302725 + +extern double MACHEP, MINLOG, MAXLOG; + +#define BIG 1.44115188075855872E+17 + +static double jvs(double n, double x); +static double hankel(double n, double x); +static double recur(double *n, double x, double *newn, int cancel); +static double jnx(double n, double x); +static double jnt(double n, double x); + +double jv(double n, double x) +{ + double k, q, t, y, an; + int i, sign, nint; + + nint = 0; /* Flag for integer n */ + sign = 1; /* Flag for sign inversion */ + an = fabs(n); + y = floor(an); + if (y == an) { + nint = 1; + i = an - 16384.0 * floor(an / 16384.0); + if (n < 0.0) { + if (i & 1) + sign = -sign; + n = an; + } + if (x < 0.0) { + if (i & 1) + sign = -sign; + x = -x; + } + if (n == 0.0) + return (j0(x)); + if (n == 1.0) + return (sign * j1(x)); + } + + if ((x < 0.0) && (y != an)) { + sf_error("Jv", SF_ERROR_DOMAIN, NULL); + y = NAN; + goto done; + } + + if (x == 0 && n < 0 && !nint) { + sf_error("Jv", SF_ERROR_OVERFLOW, NULL); + return INFINITY / gamma(n + 1); + } + + y = fabs(x); + + if (y * y < fabs(n + 1) * MACHEP) { + return pow(0.5 * x, n) / gamma(n + 1); + } + + k = 3.6 * sqrt(y); + t = 3.6 * sqrt(an); + if ((y < t) && (an > 21.0)) + return (sign * jvs(n, x)); + if ((an < k) && (y > 21.0)) + return (sign * hankel(n, x)); + + if (an < 500.0) { + /* Note: if x is too large, the continued fraction will fail; but then the + * Hankel expansion can be used. */ + if (nint != 0) { + k = 0.0; + q = recur(&n, x, &k, 1); + if (k == 0.0) { + y = j0(x) / q; + goto done; + } + if (k == 1.0) { + y = j1(x) / q; + goto done; + } + } + + if (an > 2.0 * y) + goto rlarger; + + if ((n >= 0.0) && (n < 20.0) + && (y > 6.0) && (y < 20.0)) { + /* Recur backwards from a larger value of n */ + rlarger: + k = n; + + y = y + an + 1.0; + if (y < 30.0) + y = 30.0; + y = n + floor(y - n); + q = recur(&y, x, &k, 0); + y = jvs(y, x) * q; + goto done; + } + + if (k <= 30.0) { + k = 2.0; + } + else if (k < 90.0) { + k = (3 * k) / 4; + } + if (an > (k + 3.0)) { + if (n < 0.0) + k = -k; + q = n - floor(n); + k = floor(k) + q; + if (n > 0.0) + q = recur(&n, x, &k, 1); + else { + t = k; + k = n; + q = recur(&t, x, &k, 1); + k = t; + } + if (q == 0.0) { + y = 0.0; + goto done; + } + } + else { + k = n; + q = 1.0; + } + + /* boundary between convergence of + * power series and Hankel expansion + */ + y = fabs(k); + if (y < 26.0) + t = (0.0083 * y + 0.09) * y + 12.9; + else + t = 0.9 * y; + + if (x > t) + y = hankel(k, x); + else + y = jvs(k, x); +#if CEPHES_DEBUG + printf("y = %.16e, recur q = %.16e\n", y, q); +#endif + if (n > 0.0) + y /= q; + else + y *= q; + } + + else { + /* For large n, use the uniform expansion or the transitional expansion. + * But if x is of the order of n**2, these may blow up, whereas the + * Hankel expansion will then work. + */ + if (n < 0.0) { + sf_error("Jv", SF_ERROR_LOSS, NULL); + y = NAN; + goto done; + } + t = x / n; + t /= n; + if (t > 0.3) + y = hankel(n, x); + else + y = jnx(n, x); + } + + done:return (sign * y); +} + +/* Reduce the order by backward recurrence. + * AMS55 #9.1.27 and 9.1.73. + */ + +static double recur(double *n, double x, double *newn, int cancel) +{ + double pkm2, pkm1, pk, qkm2, qkm1; + + /* double pkp1; */ + double k, ans, qk, xk, yk, r, t, kf; + static double big = BIG; + int nflag, ctr; + int miniter, maxiter; + + /* Continued fraction for Jn(x)/Jn-1(x) + * AMS 9.1.73 + * + * x -x^2 -x^2 + * ------ --------- --------- ... + * 2 n + 2(n+1) + 2(n+2) + + * + * Compute it with the simplest possible algorithm. + * + * This continued fraction starts to converge when (|n| + m) > |x|. + * Hence, at least |x|-|n| iterations are necessary before convergence is + * achieved. There is a hard limit set below, m <= 30000, which is chosen + * so that no branch in `jv` requires more iterations to converge. + * The exact maximum number is (500/3.6)^2 - 500 ~ 19000 + */ + + maxiter = 22000; + miniter = fabs(x) - fabs(*n); + if (miniter < 1) + miniter = 1; + + if (*n < 0.0) + nflag = 1; + else + nflag = 0; + + fstart: + +#if CEPHES_DEBUG + printf("recur: n = %.6e, newn = %.6e, cfrac = ", *n, *newn); +#endif + + pkm2 = 0.0; + qkm2 = 1.0; + pkm1 = x; + qkm1 = *n + *n; + xk = -x * x; + yk = qkm1; + ans = 0.0; /* ans=0.0 ensures that t=1.0 in the first iteration */ + ctr = 0; + do { + yk += 2.0; + pk = pkm1 * yk + pkm2 * xk; + qk = qkm1 * yk + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + /* check convergence */ + if (qk != 0 && ctr > miniter) + r = pk / qk; + else + r = 0.0; + + if (r != 0) { + t = fabs((ans - r) / r); + ans = r; + } + else { + t = 1.0; + } + + if (++ctr > maxiter) { + sf_error("jv", SF_ERROR_UNDERFLOW, NULL); + goto done; + } + if (t < MACHEP) + goto done; + + /* renormalize coefficients */ + if (fabs(pk) > big) { + pkm2 /= big; + pkm1 /= big; + qkm2 /= big; + qkm1 /= big; + } + } + while (t > MACHEP); + + done: + if (ans == 0) + ans = 1.0; + +#if CEPHES_DEBUG + printf("%.6e\n", ans); +#endif + + /* Change n to n-1 if n < 0 and the continued fraction is small */ + if (nflag > 0) { + if (fabs(ans) < 0.125) { + nflag = -1; + *n = *n - 1.0; + goto fstart; + } + } + + + kf = *newn; + + /* backward recurrence + * 2k + * J (x) = --- J (x) - J (x) + * k-1 x k k+1 + */ + + pk = 1.0; + pkm1 = 1.0 / ans; + k = *n - 1.0; + r = 2 * k; + do { + pkm2 = (pkm1 * r - pk * x) / x; + /* pkp1 = pk; */ + pk = pkm1; + pkm1 = pkm2; + r -= 2.0; + /* + * t = fabs(pkp1) + fabs(pk); + * if( (k > (kf + 2.5)) && (fabs(pkm1) < 0.25*t) ) + * { + * k -= 1.0; + * t = x*x; + * pkm2 = ( (r*(r+2.0)-t)*pk - r*x*pkp1 )/t; + * pkp1 = pk; + * pk = pkm1; + * pkm1 = pkm2; + * r -= 2.0; + * } + */ + k -= 1.0; + } + while (k > (kf + 0.5)); + + /* Take the larger of the last two iterates + * on the theory that it may have less cancellation error. + */ + + if (cancel) { + if ((kf >= 0.0) && (fabs(pk) > fabs(pkm1))) { + k += 1.0; + pkm2 = pk; + } + } + *newn = k; +#if CEPHES_DEBUG + printf("newn %.6e rans %.6e\n", k, pkm2); +#endif + return (pkm2); +} + + + +/* Ascending power series for Jv(x). + * AMS55 #9.1.10. + */ + +static double jvs(double n, double x) +{ + double t, u, y, z, k; + int ex, sgngam; + + z = -x * x / 4.0; + u = 1.0; + y = u; + k = 1.0; + t = 1.0; + + while (t > MACHEP) { + u *= z / (k * (n + k)); + y += u; + k += 1.0; + if (y != 0) + t = fabs(u / y); + } +#if CEPHES_DEBUG + printf("power series=%.5e ", y); +#endif + t = frexp(0.5 * x, &ex); + ex = ex * n; + if ((ex > -1023) + && (ex < 1023) + && (n > 0.0) + && (n < (MAXGAM - 1.0))) { + t = pow(0.5 * x, n) / gamma(n + 1.0); +#if CEPHES_DEBUG + printf("pow(.5*x, %.4e)/gamma(n+1)=%.5e\n", n, t); +#endif + y *= t; + } + else { +#if CEPHES_DEBUG + z = n * log(0.5 * x); + k = lgam(n + 1.0); + t = z - k; + printf("log pow=%.5e, lgam(%.4e)=%.5e\n", z, n + 1.0, k); +#else + t = n * log(0.5 * x) - lgam_sgn(n + 1.0, &sgngam); +#endif + if (y < 0) { + sgngam = -sgngam; + y = -y; + } + t += log(y); +#if CEPHES_DEBUG + printf("log y=%.5e\n", log(y)); +#endif + if (t < -MAXLOG) { + return (0.0); + } + if (t > MAXLOG) { + sf_error("Jv", SF_ERROR_OVERFLOW, NULL); + return (INFINITY); + } + y = sgngam * exp(t); + } + return (y); +} + +/* Hankel's asymptotic expansion + * for large x. + * AMS55 #9.2.5. + */ + +static double hankel(double n, double x) +{ + double t, u, z, k, sign, conv; + double p, q, j, m, pp, qq; + int flag; + + m = 4.0 * n * n; + j = 1.0; + z = 8.0 * x; + k = 1.0; + p = 1.0; + u = (m - 1.0) / z; + q = u; + sign = 1.0; + conv = 1.0; + flag = 0; + t = 1.0; + pp = 1.0e38; + qq = 1.0e38; + + while (t > MACHEP) { + k += 2.0; + j += 1.0; + sign = -sign; + u *= (m - k * k) / (j * z); + p += sign * u; + k += 2.0; + j += 1.0; + u *= (m - k * k) / (j * z); + q += sign * u; + t = fabs(u / p); + if (t < conv) { + conv = t; + qq = q; + pp = p; + flag = 1; + } + /* stop if the terms start getting larger */ + if ((flag != 0) && (t > conv)) { +#if CEPHES_DEBUG + printf("Hankel: convergence to %.4E\n", conv); +#endif + goto hank1; + } + } + + hank1: + u = x - (0.5 * n + 0.25) * M_PI; + t = sqrt(2.0 / (M_PI * x)) * (pp * cos(u) - qq * sin(u)); +#if CEPHES_DEBUG + printf("hank: %.6e\n", t); +#endif + return (t); +} + + +/* Asymptotic expansion for large n. + * AMS55 #9.3.35. + */ + +static double lambda[] = { + 1.0, + 1.041666666666666666666667E-1, + 8.355034722222222222222222E-2, + 1.282265745563271604938272E-1, + 2.918490264641404642489712E-1, + 8.816272674437576524187671E-1, + 3.321408281862767544702647E+0, + 1.499576298686255465867237E+1, + 7.892301301158651813848139E+1, + 4.744515388682643231611949E+2, + 3.207490090890661934704328E+3 +}; + +static double mu[] = { + 1.0, + -1.458333333333333333333333E-1, + -9.874131944444444444444444E-2, + -1.433120539158950617283951E-1, + -3.172272026784135480967078E-1, + -9.424291479571202491373028E-1, + -3.511203040826354261542798E+0, + -1.572726362036804512982712E+1, + -8.228143909718594444224656E+1, + -4.923553705236705240352022E+2, + -3.316218568547972508762102E+3 +}; + +static double P1[] = { + -2.083333333333333333333333E-1, + 1.250000000000000000000000E-1 +}; + +static double P2[] = { + 3.342013888888888888888889E-1, + -4.010416666666666666666667E-1, + 7.031250000000000000000000E-2 +}; + +static double P3[] = { + -1.025812596450617283950617E+0, + 1.846462673611111111111111E+0, + -8.912109375000000000000000E-1, + 7.324218750000000000000000E-2 +}; + +static double P4[] = { + 4.669584423426247427983539E+0, + -1.120700261622299382716049E+1, + 8.789123535156250000000000E+0, + -2.364086914062500000000000E+0, + 1.121520996093750000000000E-1 +}; + +static double P5[] = { + -2.8212072558200244877E1, + 8.4636217674600734632E1, + -9.1818241543240017361E1, + 4.2534998745388454861E1, + -7.3687943594796316964E0, + 2.27108001708984375E-1 +}; + +static double P6[] = { + 2.1257013003921712286E2, + -7.6525246814118164230E2, + 1.0599904525279998779E3, + -6.9957962737613254123E2, + 2.1819051174421159048E2, + -2.6491430486951555525E1, + 5.7250142097473144531E-1 +}; + +static double P7[] = { + -1.9194576623184069963E3, + 8.0617221817373093845E3, + -1.3586550006434137439E4, + 1.1655393336864533248E4, + -5.3056469786134031084E3, + 1.2009029132163524628E3, + -1.0809091978839465550E2, + 1.7277275025844573975E0 +}; + + +static double jnx(double n, double x) +{ + double zeta, sqz, zz, zp, np; + double cbn, n23, t, z, sz; + double pp, qq, z32i, zzi; + double ak, bk, akl, bkl; + int sign, doa, dob, nflg, k, s, tk, tkp1, m; + static double u[8]; + static double ai, aip, bi, bip; + + /* Test for x very close to n. Use expansion for transition region if so. */ + cbn = cbrt(n); + z = (x - n) / cbn; + if (fabs(z) <= 0.7) + return (jnt(n, x)); + + z = x / n; + zz = 1.0 - z * z; + if (zz == 0.0) + return (0.0); + + if (zz > 0.0) { + sz = sqrt(zz); + t = 1.5 * (log((1.0 + sz) / z) - sz); /* zeta ** 3/2 */ + zeta = cbrt(t * t); + nflg = 1; + } + else { + sz = sqrt(-zz); + t = 1.5 * (sz - acos(1.0 / z)); + zeta = -cbrt(t * t); + nflg = -1; + } + z32i = fabs(1.0 / t); + sqz = cbrt(t); + + /* Airy function */ + n23 = cbrt(n * n); + t = n23 * zeta; + +#if CEPHES_DEBUG + printf("zeta %.5E, Airy(%.5E)\n", zeta, t); +#endif + airy(t, &ai, &aip, &bi, &bip); + + /* polynomials in expansion */ + u[0] = 1.0; + zzi = 1.0 / zz; + u[1] = polevl(zzi, P1, 1) / sz; + u[2] = polevl(zzi, P2, 2) / zz; + u[3] = polevl(zzi, P3, 3) / (sz * zz); + pp = zz * zz; + u[4] = polevl(zzi, P4, 4) / pp; + u[5] = polevl(zzi, P5, 5) / (pp * sz); + pp *= zz; + u[6] = polevl(zzi, P6, 6) / pp; + u[7] = polevl(zzi, P7, 7) / (pp * sz); + +#if CEPHES_DEBUG + for (k = 0; k <= 7; k++) + printf("u[%d] = %.5E\n", k, u[k]); +#endif + + pp = 0.0; + qq = 0.0; + np = 1.0; + /* flags to stop when terms get larger */ + doa = 1; + dob = 1; + akl = INFINITY; + bkl = INFINITY; + + for (k = 0; k <= 3; k++) { + tk = 2 * k; + tkp1 = tk + 1; + zp = 1.0; + ak = 0.0; + bk = 0.0; + for (s = 0; s <= tk; s++) { + if (doa) { + if ((s & 3) > 1) + sign = nflg; + else + sign = 1; + ak += sign * mu[s] * zp * u[tk - s]; + } + + if (dob) { + m = tkp1 - s; + if (((m + 1) & 3) > 1) + sign = nflg; + else + sign = 1; + bk += sign * lambda[s] * zp * u[m]; + } + zp *= z32i; + } + + if (doa) { + ak *= np; + t = fabs(ak); + if (t < akl) { + akl = t; + pp += ak; + } + else + doa = 0; + } + + if (dob) { + bk += lambda[tkp1] * zp * u[0]; + bk *= -np / sqz; + t = fabs(bk); + if (t < bkl) { + bkl = t; + qq += bk; + } + else + dob = 0; + } +#if CEPHES_DEBUG + printf("a[%d] %.5E, b[%d] %.5E\n", k, ak, k, bk); +#endif + if (np < MACHEP) + break; + np /= n * n; + } + + /* normalizing factor ( 4*zeta/(1 - z**2) )**1/4 */ + t = 4.0 * zeta / zz; + t = sqrt(sqrt(t)); + + t *= ai * pp / cbrt(n) + aip * qq / (n23 * n); + return (t); +} + +/* Asymptotic expansion for transition region, + * n large and x close to n. + * AMS55 #9.3.23. + */ + +static double PF2[] = { + -9.0000000000000000000e-2, + 8.5714285714285714286e-2 +}; + +static double PF3[] = { + 1.3671428571428571429e-1, + -5.4920634920634920635e-2, + -4.4444444444444444444e-3 +}; + +static double PF4[] = { + 1.3500000000000000000e-3, + -1.6036054421768707483e-1, + 4.2590187590187590188e-2, + 2.7330447330447330447e-3 +}; + +static double PG1[] = { + -2.4285714285714285714e-1, + 1.4285714285714285714e-2 +}; + +static double PG2[] = { + -9.0000000000000000000e-3, + 1.9396825396825396825e-1, + -1.1746031746031746032e-2 +}; + +static double PG3[] = { + 1.9607142857142857143e-2, + -1.5983694083694083694e-1, + 6.3838383838383838384e-3 +}; + + +static double jnt(double n, double x) +{ + double z, zz, z3; + double cbn, n23, cbtwo; + double ai, aip, bi, bip; /* Airy functions */ + double nk, fk, gk, pp, qq; + double F[5], G[4]; + int k; + + cbn = cbrt(n); + z = (x - n) / cbn; + cbtwo = cbrt(2.0); + + /* Airy function */ + zz = -cbtwo * z; + airy(zz, &ai, &aip, &bi, &bip); + + /* polynomials in expansion */ + zz = z * z; + z3 = zz * z; + F[0] = 1.0; + F[1] = -z / 5.0; + F[2] = polevl(z3, PF2, 1) * zz; + F[3] = polevl(z3, PF3, 2); + F[4] = polevl(z3, PF4, 3) * z; + G[0] = 0.3 * zz; + G[1] = polevl(z3, PG1, 1); + G[2] = polevl(z3, PG2, 2) * z; + G[3] = polevl(z3, PG3, 2) * zz; +#if CEPHES_DEBUG + for (k = 0; k <= 4; k++) + printf("F[%d] = %.5E\n", k, F[k]); + for (k = 0; k <= 3; k++) + printf("G[%d] = %.5E\n", k, G[k]); +#endif + pp = 0.0; + qq = 0.0; + nk = 1.0; + n23 = cbrt(n * n); + + for (k = 0; k <= 4; k++) { + fk = F[k] * nk; + pp += fk; + if (k != 4) { + gk = G[k] * nk; + qq += gk; + } +#if CEPHES_DEBUG + printf("fk[%d] %.5E, gk[%d] %.5E\n", k, fk, k, gk); +#endif + nk /= n23; + } + + fk = cbtwo * ai * pp / cbn + cbrt(4.0) * aip * qq / n; + return (fk); +} diff --git a/gtsam/3rdparty/cephes/cephes/k0.c b/gtsam/3rdparty/cephes/cephes/k0.c new file mode 100644 index 0000000000..c5b31a1bf1 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/k0.c @@ -0,0 +1,178 @@ +/* k0.c + * + * Modified Bessel function, third kind, order zero + * + * + * + * SYNOPSIS: + * + * double x, y, k0(); + * + * y = k0( x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of the third kind + * of order zero of the argument. + * + * The range is partitioned into the two intervals [0,8] and + * (8, infinity). Chebyshev polynomial expansions are employed + * in each interval. + * + * + * + * ACCURACY: + * + * Tested at 2000 random points between 0 and 8. Peak absolute + * error (relative when K0 > 1) was 1.46e-14; rms, 4.26e-15. + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.2e-15 1.6e-16 + * + * ERROR MESSAGES: + * + * message condition value returned + * K0 domain x <= 0 INFINITY + * + */ + /* k0e() + * + * Modified Bessel function, third kind, order zero, + * exponentially scaled + * + * + * + * SYNOPSIS: + * + * double x, y, k0e(); + * + * y = k0e( x ); + * + * + * + * DESCRIPTION: + * + * Returns exponentially scaled modified Bessel function + * of the third kind of order zero of the argument. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.4e-15 1.4e-16 + * See k0(). + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +/* Chebyshev coefficients for K0(x) + log(x/2) I0(x) + * in the interval [0,2]. The odd order coefficients are all + * zero; only the even order coefficients are listed. + * + * lim(x->0){ K0(x) + log(x/2) I0(x) } = -EUL. + */ + +static double A[] = { + 1.37446543561352307156E-16, + 4.25981614279661018399E-14, + 1.03496952576338420167E-11, + 1.90451637722020886025E-9, + 2.53479107902614945675E-7, + 2.28621210311945178607E-5, + 1.26461541144692592338E-3, + 3.59799365153615016266E-2, + 3.44289899924628486886E-1, + -5.35327393233902768720E-1 +}; + +/* Chebyshev coefficients for exp(x) sqrt(x) K0(x) + * in the inverted interval [2,infinity]. + * + * lim(x->inf){ exp(x) sqrt(x) K0(x) } = sqrt(pi/2). + */ +static double B[] = { + 5.30043377268626276149E-18, + -1.64758043015242134646E-17, + 5.21039150503902756861E-17, + -1.67823109680541210385E-16, + 5.51205597852431940784E-16, + -1.84859337734377901440E-15, + 6.34007647740507060557E-15, + -2.22751332699166985548E-14, + 8.03289077536357521100E-14, + -2.98009692317273043925E-13, + 1.14034058820847496303E-12, + -4.51459788337394416547E-12, + 1.85594911495471785253E-11, + -7.95748924447710747776E-11, + 3.57739728140030116597E-10, + -1.69753450938905987466E-9, + 8.57403401741422608519E-9, + -4.66048989768794782956E-8, + 2.76681363944501510342E-7, + -1.83175552271911948767E-6, + 1.39498137188764993662E-5, + -1.28495495816278026384E-4, + 1.56988388573005337491E-3, + -3.14481013119645005427E-2, + 2.44030308206595545468E0 +}; + +double k0(double x) +{ + double y, z; + + if (x == 0.0) { + sf_error("k0", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + else if (x < 0.0) { + sf_error("k0", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x <= 2.0) { + y = x * x - 2.0; + y = chbevl(y, A, 10) - log(0.5 * x) * i0(x); + return (y); + } + z = 8.0 / x - 2.0; + y = exp(-x) * chbevl(z, B, 25) / sqrt(x); + return (y); +} + + + + +double k0e(double x) +{ + double y; + + if (x == 0.0) { + sf_error("k0e", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + else if (x < 0.0) { + sf_error("k0e", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x <= 2.0) { + y = x * x - 2.0; + y = chbevl(y, A, 10) - log(0.5 * x) * i0(x); + return (y * exp(x)); + } + + y = chbevl(8.0 / x - 2.0, B, 25) / sqrt(x); + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/k1.c b/gtsam/3rdparty/cephes/cephes/k1.c new file mode 100644 index 0000000000..fc33e5c0ee --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/k1.c @@ -0,0 +1,179 @@ +/* k1.c + * + * Modified Bessel function, third kind, order one + * + * + * + * SYNOPSIS: + * + * double x, y, k1(); + * + * y = k1( x ); + * + * + * + * DESCRIPTION: + * + * Computes the modified Bessel function of the third kind + * of order one of the argument. + * + * The range is partitioned into the two intervals [0,2] and + * (2, infinity). Chebyshev polynomial expansions are employed + * in each interval. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.2e-15 1.6e-16 + * + * ERROR MESSAGES: + * + * message condition value returned + * k1 domain x <= 0 INFINITY + * + */ + /* k1e.c + * + * Modified Bessel function, third kind, order one, + * exponentially scaled + * + * + * + * SYNOPSIS: + * + * double x, y, k1e(); + * + * y = k1e( x ); + * + * + * + * DESCRIPTION: + * + * Returns exponentially scaled modified Bessel function + * of the third kind of order one of the argument: + * + * k1e(x) = exp(x) * k1(x). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 7.8e-16 1.2e-16 + * See k1(). + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +/* Chebyshev coefficients for x(K1(x) - log(x/2) I1(x)) + * in the interval [0,2]. + * + * lim(x->0){ x(K1(x) - log(x/2) I1(x)) } = 1. + */ + +static double A[] = { + -7.02386347938628759343E-18, + -2.42744985051936593393E-15, + -6.66690169419932900609E-13, + -1.41148839263352776110E-10, + -2.21338763073472585583E-8, + -2.43340614156596823496E-6, + -1.73028895751305206302E-4, + -6.97572385963986435018E-3, + -1.22611180822657148235E-1, + -3.53155960776544875667E-1, + 1.52530022733894777053E0 +}; + +/* Chebyshev coefficients for exp(x) sqrt(x) K1(x) + * in the interval [2,infinity]. + * + * lim(x->inf){ exp(x) sqrt(x) K1(x) } = sqrt(pi/2). + */ +static double B[] = { + -5.75674448366501715755E-18, + 1.79405087314755922667E-17, + -5.68946255844285935196E-17, + 1.83809354436663880070E-16, + -6.05704724837331885336E-16, + 2.03870316562433424052E-15, + -7.01983709041831346144E-15, + 2.47715442448130437068E-14, + -8.97670518232499435011E-14, + 3.34841966607842919884E-13, + -1.28917396095102890680E-12, + 5.13963967348173025100E-12, + -2.12996783842756842877E-11, + 9.21831518760500529508E-11, + -4.19035475934189648750E-10, + 2.01504975519703286596E-9, + -1.03457624656780970260E-8, + 5.74108412545004946722E-8, + -3.50196060308781257119E-7, + 2.40648494783721712015E-6, + -1.93619797416608296024E-5, + 1.95215518471351631108E-4, + -2.85781685962277938680E-3, + 1.03923736576817238437E-1, + 2.72062619048444266945E0 +}; + +extern double MINLOG; + +double k1(double x) +{ + double y, z; + + if (x == 0.0) { + sf_error("k1", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + else if (x < 0.0) { + sf_error("k1", SF_ERROR_DOMAIN, NULL); + return NAN; + } + z = 0.5 * x; + + if (x <= 2.0) { + y = x * x - 2.0; + y = log(z) * i1(x) + chbevl(y, A, 11) / x; + return (y); + } + + return (exp(-x) * chbevl(8.0 / x - 2.0, B, 25) / sqrt(x)); +} + + + + +double k1e(double x) +{ + double y; + + if (x == 0.0) { + sf_error("k1e", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + else if (x < 0.0) { + sf_error("k1e", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x <= 2.0) { + y = x * x - 2.0; + y = log(0.5 * x) * i1(x) + chbevl(y, A, 11) / x; + return (y * exp(x)); + } + + return (chbevl(8.0 / x - 2.0, B, 25) / sqrt(x)); +} diff --git a/gtsam/3rdparty/cephes/cephes/kn.c b/gtsam/3rdparty/cephes/cephes/kn.c new file mode 100644 index 0000000000..ff7584a154 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/kn.c @@ -0,0 +1,235 @@ +/* kn.c + * + * Modified Bessel function, third kind, integer order + * + * + * + * SYNOPSIS: + * + * double x, y, kn(); + * int n; + * + * y = kn( n, x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of the third kind + * of order n of the argument. + * + * The range is partitioned into the two intervals [0,9.55] and + * (9.55, infinity). An ascending power series is used in the + * low range, and an asymptotic expansion in the high range. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 90000 1.8e-8 3.0e-10 + * + * Error is high only near the crossover point x = 9.55 + * between the two expansions used. + */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1988, 2000 by Stephen L. Moshier + */ + + +/* + * Algorithm for Kn. + * n-1 + * -n - (n-k-1)! 2 k + * K (x) = 0.5 (x/2) > -------- (-x /4) + * n - k! + * k=0 + * + * inf. 2 k + * n n - (x /4) + * + (-1) 0.5(x/2) > {p(k+1) + p(n+k+1) - 2log(x/2)} --------- + * - k! (n+k)! + * k=0 + * + * where p(m) is the psi function: p(1) = -EUL and + * + * m-1 + * - + * p(m) = -EUL + > 1/k + * - + * k=1 + * + * For large x, + * 2 2 2 + * u-1 (u-1 )(u-3 ) + * K (z) = sqrt(pi/2z) exp(-z) { 1 + ------- + ------------ + ...} + * v 1 2 + * 1! (8z) 2! (8z) + * asymptotically, where + * + * 2 + * u = 4 v . + * + */ + +#include "mconf.h" +#include + +#define EUL 5.772156649015328606065e-1 +#define MAXFAC 31 +extern double MACHEP, MAXLOG; + +double kn(int nn, double x) +{ + double k, kf, nk1f, nkf, zn, t, s, z0, z; + double ans, fn, pn, pk, zmn, tlg, tox; + int i, n; + + if (nn < 0) + n = -nn; + else + n = nn; + + if (n > MAXFAC) { + overf: + sf_error("kn", SF_ERROR_OVERFLOW, NULL); + return (INFINITY); + } + + if (x <= 0.0) { + if (x < 0.0) { + sf_error("kn", SF_ERROR_DOMAIN, NULL); + return NAN; + } + else { + sf_error("kn", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + } + + + if (x > 9.55) + goto asymp; + + ans = 0.0; + z0 = 0.25 * x * x; + fn = 1.0; + pn = 0.0; + zmn = 1.0; + tox = 2.0 / x; + + if (n > 0) { + /* compute factorial of n and psi(n) */ + pn = -EUL; + k = 1.0; + for (i = 1; i < n; i++) { + pn += 1.0 / k; + k += 1.0; + fn *= k; + } + + zmn = tox; + + if (n == 1) { + ans = 1.0 / x; + } + else { + nk1f = fn / n; + kf = 1.0; + s = nk1f; + z = -z0; + zn = 1.0; + for (i = 1; i < n; i++) { + nk1f = nk1f / (n - i); + kf = kf * i; + zn *= z; + t = nk1f * zn / kf; + s += t; + if ((DBL_MAX - fabs(t)) < fabs(s)) + goto overf; + if ((tox > 1.0) && ((DBL_MAX / tox) < zmn)) + goto overf; + zmn *= tox; + } + s *= 0.5; + t = fabs(s); + if ((zmn > 1.0) && ((DBL_MAX / zmn) < t)) + goto overf; + if ((t > 1.0) && ((DBL_MAX / t) < zmn)) + goto overf; + ans = s * zmn; + } + } + + + tlg = 2.0 * log(0.5 * x); + pk = -EUL; + if (n == 0) { + pn = pk; + t = 1.0; + } + else { + pn = pn + 1.0 / n; + t = 1.0 / fn; + } + s = (pk + pn - tlg) * t; + k = 1.0; + do { + t *= z0 / (k * (k + n)); + pk += 1.0 / k; + pn += 1.0 / (k + n); + s += (pk + pn - tlg) * t; + k += 1.0; + } + while (fabs(t / s) > MACHEP); + + s = 0.5 * s / zmn; + if (n & 1) + s = -s; + ans += s; + + return (ans); + + + + /* Asymptotic expansion for Kn(x) */ + /* Converges to 1.4e-17 for x > 18.4 */ + + asymp: + + if (x > MAXLOG) { + sf_error("kn", SF_ERROR_UNDERFLOW, NULL); + return (0.0); + } + k = n; + pn = 4.0 * k * k; + pk = 1.0; + z0 = 8.0 * x; + fn = 1.0; + t = 1.0; + s = t; + nkf = INFINITY; + i = 0; + do { + z = pn - pk * pk; + t = t * z / (fn * z0); + nk1f = fabs(t); + if ((i >= n) && (nk1f > nkf)) { + goto adone; + } + nkf = nk1f; + s += t; + fn += 1.0; + pk += 2.0; + i += 1; + } + while (fabs(t / s) > MACHEP); + + adone: + ans = exp(-x) * sqrt(M_PI / (2.0 * x)) * s; + return (ans); +} diff --git a/gtsam/3rdparty/cephes/cephes/kolmogorov.c b/gtsam/3rdparty/cephes/cephes/kolmogorov.c new file mode 100644 index 0000000000..2135e0ebbd --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/kolmogorov.c @@ -0,0 +1,1147 @@ +/* File altered for inclusion in cephes module for Python: + * Main loop commented out.... */ +/* Travis Oliphant Nov. 1998 */ + +/* Re Kolmogorov statistics, here is Birnbaum and Tingey's (actually it was already present + * in Smirnov's paper) formula for the + * distribution of D+, the maximum of all positive deviations between a + * theoretical distribution function P(x) and an empirical one Sn(x) + * from n samples. + * + * + + * D = sup [P(x) - S (x)] + * n -inf < x < inf n + * + * + * [n(1-d)] + * + - v-1 n-v + * Pr{D > d} = > C d (d + v/n) (1 - d - v/n) + * n - n v + * v=0 + * + * (also equals the following sum, but note the terms may be large and alternating in sign) + * See Smirnov 1944, Dwass 1959 + * n + * - v-1 n-v + * = 1 - > C d (d + v/n) (1 - d - v/n) + * - n v + * v=[n(1-d)]+1 + * + * [n(1-d)] is the largest integer not exceeding n(1-d). + * nCv is the number of combinations of n things taken v at a time. + + * Sources: + * [1] Smirnov, N.V. "Approximate laws of distribution of random variables from empirical data" + * Usp. Mat. Nauk, 1944. http://mi.mathnet.ru/umn8798 + * [2] Birnbaum, Z. W. and Tingey, Fred H. + * "One-Sided Confidence Contours for Probability Distribution Functions", + * Ann. Math. Statist. 1951. https://doi.org/10.1214/aoms/1177729550 + * [3] Dwass, Meyer, "The Distribution of a Generalized $\mathrm{D}^+_n$ Statistic", + * Ann. Math. Statist., 1959. https://doi.org/10.1214/aoms/1177706085 + * [4] van Mulbregt, Paul, "Computing the Cumulative Distribution Function and Quantiles of the One-sided Kolmogorov-Smirnov Statistic" + * http://arxiv.org/abs/1802.06966 + * [5] van Mulbregt, Paul, "Computing the Cumulative Distribution Function and Quantiles of the limit of the Two-sided Kolmogorov-Smirnov Statistic" + * https://arxiv.org/abs/1803.00426 + * + */ + +#include "mconf.h" +#include +#include +#include + + +/* ************************************************************************ */ +/* Algorithm Configuration */ + +/* + * Kolmogorov Two-sided: + * Switchover between the two series to compute K(x) + * 0 <= x <= KOLMOG_CUTOVER and + * KOLMOG_CUTOVER < x < infty + */ +#define KOLMOG_CUTOVER 0.82 + + +/* + * Smirnov One-sided: + * n larger than SMIRNOV_MAX_COMPUTE_N will result in an approximation + */ +const int SMIRNOV_MAX_COMPUTE_N = 1000000; + +/* + * Use the upper sum formula, if the number of terms is at most SM_UPPER_MAX_TERMS, + * and n is at least SM_UPPERSUM_MIN_N + * Don't use the upper sum if lots of terms are involved as the series alternates + * sign and the terms get much bigger than 1. + */ +#define SM_UPPER_MAX_TERMS 3 +#define SM_UPPERSUM_MIN_N 10 + +/* ************************************************************************ */ +/* ************************************************************************ */ + +/* Assuming LOW and HIGH are constants. */ +#define CLIP(X, LOW, HIGH) ((X) < LOW ? LOW : MIN(X, HIGH)) +#ifndef MIN +#define MIN(a,b) (((a) < (b)) ? (a) : (b)) +#endif +#ifndef MAX +#define MAX(a,b) (((a) < (b)) ? (b) : (a)) +#endif + +/* from cephes constants */ +extern double MINLOG; + +/* exp() of anything below this returns 0 */ +static const int MIN_EXPABLE = (-708 - 38); + +#ifndef LOGSQRT2PI +#define LOGSQRT2PI 0.91893853320467274178032973640561764 +#endif + +/* Struct to hold the CDF, SF and PDF, which are computed simultaneously */ +typedef struct ThreeProbs { + double sf; + double cdf; + double pdf; +} ThreeProbs; + +#define RETURN_3PROBS(PSF, PCDF, PDF) \ + ret.cdf = (PCDF); \ + ret.sf = (PSF); \ + ret.pdf = (PDF); \ + return ret; + +static const double _xtol = DBL_EPSILON; +static const double _rtol = 2*DBL_EPSILON; + +static int +_within_tol(double x, double y, double atol, double rtol) +{ + double diff = fabs(x-y); + int result = (diff <= (atol + rtol * fabs(y))); + return result; +} + +#include "dd_real.h" + +/* Shorten some of the double-double names for readibility */ +#define valueD dd_to_double +#define add_dd dd_add_d_d +#define sub_dd dd_sub_d_d +#define mul_dd dd_mul_d_d +#define neg_D dd_neg +#define div_dd dd_div_d_d +#define add_DD dd_add +#define sub_DD dd_sub +#define mul_DD dd_mul +#define div_DD dd_div +#define add_Dd dd_add_dd_d +#define add_dD dd_add_d_dd +#define sub_Dd dd_sub_dd_d +#define sub_dD dd_sub_d_dd +#define mul_Dd dd_mul_dd_d +#define mul_dD dd_mul_d_dd +#define div_Dd dd_div_dd_d +#define div_dD dd_div_d_dd +#define frexpD dd_frexp +#define ldexpD dd_ldexp +#define logD dd_log +#define log1pD dd_log1p + + +/* ************************************************************************ */ +/* Kolmogorov : Two-sided **************************** */ +/* ************************************************************************ */ + +static ThreeProbs +_kolmogorov(double x) +{ + double P = 1.0; + double D = 0; + double sf, cdf, pdf; + ThreeProbs ret; + + if (isnan(x)) { + RETURN_3PROBS(NAN, NAN, NAN); + } + if (x <= 0) { + RETURN_3PROBS(1.0, 0.0, 0); + } + /* x <= 0.040611972203751713 */ + if (x <= (double)M_PI/sqrt(-MIN_EXPABLE * 8)) { + RETURN_3PROBS(1.0, 0.0, 0); + } + + P = 1.0; + if (x <= KOLMOG_CUTOVER) { + /* + * u = e^(-pi^2/(8x^2)) + * w = sqrt(2pi)/x + * P = w*u * (1 + u^8 + u^24 + u^48 + ...) + */ + double w = sqrt(2 * M_PI)/x; + double logu8 = -M_PI * M_PI/(x * x); /* log(u^8) */ + double u = exp(logu8/8); + if (u == 0) { + /* + * P = w*u, but u < 1e-308, and w > 1, + * so compute as logs, then exponentiate + */ + double logP = logu8/8 + log(w); + P = exp(logP); + } else { + /* Just unroll the loop, 3 iterations */ + double u8 = exp(logu8); + double u8cub = pow(u8, 3); + P = 1 + u8cub * P; + D = 5*5 + u8cub * D; + P = 1 + u8*u8 * P; + D = 3*3 + u8*u8 * D; + P = 1 + u8 * P; + D = 1*1 + u8 * D; + + D = M_PI * M_PI/4/(x*x) * D - P; + D *= w * u/x; + P = w * u * P; + } + cdf = P; + sf = 1-P; + pdf = D; + } + else { + /* + * v = e^(-2x^2) + * P = 2 (v - v^4 + v^9 - v^16 + ...) + * = 2v(1 - v^3*(1 - v^5*(1 - v^7*(1 - ...))) + */ + double logv = -2*x*x; + double v = exp(logv); + /* + * Want q^((2k-1)^2)(1-q^(4k-1)) / q(1-q^3) < epsilon to break out of loop. + * With KOLMOG_CUTOVER ~ 0.82, k <= 4. Just unroll the loop, 4 iterations + */ + double vsq = v*v; + double v3 = pow(v, 3); + double vpwr; + + vpwr = v3*v3*v; /* v**7 */ + P = 1 - vpwr * P; /* P <- 1 - (1-v**(2k-1)) * P */ + D = 3*3 - vpwr * D; + + vpwr = v3*vsq; + P = 1 - vpwr * P; + D = 2*2 - vpwr * D; + + vpwr = v3; + P = 1 - vpwr * P; + D = 1*1 - vpwr * D; + + P = 2 * v * P; + D = 8 * v * x * D; + sf = P; + cdf = 1 - sf; + pdf = D; + } + pdf = MAX(0, pdf); + cdf = CLIP(cdf, 0, 1); + sf = CLIP(sf, 0, 1); + RETURN_3PROBS(sf, cdf, pdf); +} + + +/* Find x such kolmogorov(x)=psf, kolmogc(x)=pcdf */ +static double +_kolmogi(double psf, double pcdf) +{ + double x, t; + double xmin = 0; + double xmax = INFINITY; + int iterations; + double a = xmin, b = xmax; + + if (!(psf >= 0.0 && pcdf >= 0.0 && pcdf <= 1.0 && psf <= 1.0)) { + sf_error("kolmogi", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (fabs(1.0 - pcdf - psf) > 4* DBL_EPSILON) { + sf_error("kolmogi", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (pcdf == 0.0) { + return 0.0; + } + if (psf == 0.0) { + return INFINITY; + } + + if (pcdf <= 0.5) { + /* p ~ (sqrt(2pi)/x) *exp(-pi^2/8x^2). Generate lower and upper bounds */ + double logpcdf = log(pcdf); + const double SQRT2 = M_SQRT2; + /* Now that 1 >= x >= sqrt(p) */ + /* Iterate twice: x <- pi/(sqrt(8) sqrt(log(sqrt(2pi)) - log(x) - log(pdf))) */ + a = M_PI / (2 * SQRT2 * sqrt(-(logpcdf + logpcdf/2 - LOGSQRT2PI))); + b = M_PI / (2 * SQRT2 * sqrt(-(logpcdf + 0 - LOGSQRT2PI))); + a = M_PI / (2 * SQRT2 * sqrt(-(logpcdf + log(a) - LOGSQRT2PI))); + b = M_PI / (2 * SQRT2 * sqrt(-(logpcdf + log(b) - LOGSQRT2PI))); + x = (a + b) / 2.0; + } + else { + /* + * Based on the approximation p ~ 2 exp(-2x^2) + * Found that needed to replace psf with a slightly smaller number in the second element + * as otherwise _kolmogorov(b) came back as a very small number but with + * the same sign as _kolmogorov(a) + * kolmogi(0.5) = 0.82757355518990772 + * so (1-q^(-(4-1)*2*x^2)) = (1-exp(-6*0.8275^2) ~ (1-exp(-4.1) + */ + const double jiggerb = 256 * DBL_EPSILON; + double pba = psf/(1.0 - exp(-4))/2, pbb = psf * (1 - jiggerb)/2; + double q0; + a = sqrt(-0.5 * log(pba)); + b = sqrt(-0.5 * log(pbb)); + /* + * Use inversion of + * p = q - q^4 + q^9 - q^16 + ...: + * q = p + p^4 + 4p^7 - p^9 + 22p^10 - 13p^12 + 140*p^13 ... + */ + { + double p = psf/2.0; + double p2 = p*p; + double p3 = p*p*p; + q0 = 1 + p3 * (1 + p3 * (4 + p2 *(-1 + p*(22 + p2* (-13 + 140 * p))))); + q0 *= p; + } + x = sqrt(-log(q0) / 2); + if (x < a || x > b) { + x = (a+b)/2; + } + } + assert(a <= b); + + iterations = 0; + do { + double x0 = x; + ThreeProbs probs = _kolmogorov(x0); + double df = ((pcdf < 0.5) ? (pcdf - probs.cdf) : (probs.sf - psf)); + double dfdx; + + if (fabs(df) == 0) { + break; + } + /* Update the bracketing interval */ + if (df > 0 && x > a) { + a = x; + } else if (df < 0 && x < b) { + b = x; + } + + dfdx = -probs.pdf; + if (fabs(dfdx) <= 0.0) { + x = (a+b)/2; + t = x0 - x; + } else { + t = df/dfdx; + x = x0 - t; + } + + /* + * Check out-of-bounds. + * Not expecting this to happen often --- kolmogorov is convex near x=infinity and + * concave near x=0, and we should be approaching from the correct side. + * If out-of-bounds, replace x with a midpoint of the bracket. + */ + if (x >= a && x <= b) { + if (_within_tol(x, x0, _xtol, _rtol)) { + break; + } + if ((x == a) || (x == b)) { + x = (a + b) / 2.0; + /* If the bracket is already so small ... */ + if (x == a || x == b) { + break; + } + } + } else { + x = (a + b) / 2.0; + if (_within_tol(x, x0, _xtol, _rtol)) { + break; + } + } + + if (++iterations > MAXITER) { + sf_error("kolmogi", SF_ERROR_SLOW, NULL); + break; + } + } while(1); + return (x); +} + + +double +kolmogorov(double x) +{ + if (isnan(x)) { + return NAN; + } + return _kolmogorov(x).sf; +} + +double +kolmogc(double x) +{ + if (isnan(x)) { + return NAN; + } + return _kolmogorov(x).cdf; +} + +double +kolmogp(double x) +{ + if (isnan(x)) { + return NAN; + } + if (x <= 0) { + return -0.0; + } + return -_kolmogorov(x).pdf; +} + +/* Functional inverse of Kolmogorov survival statistic for two-sided test. + * Finds x such that kolmogorov(x) = p. + */ +double +kolmogi(double p) +{ + if (isnan(p)) { + return NAN; + } + return _kolmogi(p, 1-p); +} + +/* Functional inverse of Kolmogorov cumulative statistic for two-sided test. + * Finds x such that kolmogc(x) = p = (or kolmogorov(x) = 1-p). + */ +double +kolmogci(double p) +{ + if (isnan(p)) { + return NAN; + } + return _kolmogi(1-p, p); +} + + + +/* ************************************************************************ */ +/* ********** Smirnov : One-sided ***************************************** */ +/* ************************************************************************ */ + +static double +nextPowerOf2(double x) +{ + double q = ldexp(x, 1-DBL_MANT_DIG); + double L = fabs(q+x); + if (L == 0) { + L = fabs(x); + } else { + int Lint = (int)(L); + if (Lint == L) { + L = Lint; + } + } + return L; +} + +static double +modNX(int n, double x, int *pNXFloor, double *pNX) +{ + /* + * Compute floor(n*x) and remainder *exactly*. + * If remainder is too close to 1 (E.g. (1, -DBL_EPSILON/2)) + * round up and adjust */ + double2 alphaD, nxD, nxfloorD; + int nxfloor; + double alpha; + + nxD = mul_dd(n, x); + nxfloorD = dd_floor(nxD); + alphaD = sub_DD(nxD, nxfloorD); + alpha = dd_hi(alphaD); + nxfloor = dd_to_int(nxfloorD); + assert(alpha >= 0); + assert(alpha <= 1); + if (alpha == 1) { + nxfloor += 1; + alpha = 0; + } + assert(alpha < 1.0); + *pNX = dd_to_double(nxD); + *pNXFloor = nxfloor; + return alpha; +} + +/* + * The binomial coefficient C overflows a 64 bit double, as the 11-bit + * exponent is too small. + * Store C as (Cman:double2, Cexpt:int). + * I.e a Mantissa/significand, and an exponent. + * Cman lies between 0.5 and 1, and the exponent has >=32-bit. + */ +static void +updateBinomial(double2 *Cman, int *Cexpt, int n, int j) +{ + int expt; + double2 rat = div_dd(n - j, j + 1.0); + double2 man2 = mul_DD(*Cman, rat); + man2 = frexpD(man2, &expt); + assert (!dd_is_zero(man2)); + *Cexpt += expt; + *Cman = man2; +} + + +static double2 +pow_D(double2 a, int m) +{ + /* + * Using dd_npwr() here would be quite time-consuming. + * Tradeoff accuracy-time by using pow(). + */ + double ans, r, adj; + if (m <= 0) { + if (m == 0) { + return DD_C_ONE; + } + return dd_inv(pow_D(a, -m)); + } + if (dd_is_zero(a)) { + return DD_C_ZERO; + } + ans = pow(a.x[0], m); + r = a.x[1]/a.x[0]; + adj = m*r; + if (fabs(adj) > 1e-8) { + if (fabs(adj) < 1e-4) { + /* Take 1st two terms of Taylor Series for (1+r)^m */ + adj += (m*r) * ((m-1)/2.0 * r); + } else { + /* Take exp of scaled log */ + adj = expm1(m*log1p(r)); + } + } + return dd_add_d_d(ans, ans*adj); +} + +static double +pow2(double a, double b, int m) +{ + return dd_to_double(pow_D(add_dd(a, b), m)); +} + +/* + * Not 1024 as too big. Want _MAX_EXPONENT < 1023-52 so as to keep both + * elements of the double2 normalized + */ +#define _MAX_EXPONENT 960 + +#define RETURN_M_E(MAND, EXPT) \ + *pExponent = EXPT;\ + return MAND; + + +static double2 +pow2Scaled_D(double2 a, int m, int *pExponent) +{ + /* Compute a^m = significand*2^expt and return as (significand, expt) */ + double2 ans, y; + int ansE, yE; + int maxExpt = _MAX_EXPONENT; + int q, r, y2mE, y2rE, y2mqE; + double2 y2r, y2m, y2mq; + + if (m <= 0) + { + int aE1, aE2; + if (m == 0) { + RETURN_M_E(DD_C_ONE, 0); + } + ans = pow2Scaled_D(a, -m, &aE1); + ans = frexpD(dd_inv(ans), &aE2); + ansE = -aE1 + aE2; + RETURN_M_E(ans, ansE); + } + y = frexpD(a, &yE); + if (m == 1) { + RETURN_M_E(y, yE); + } + /* + * y ^ maxExpt >= 2^{-960} + * => maxExpt = 960 / log2(y.x[0]) = 708 / log(y.x[0]) + * = 665/((1-y.x[0] + y.x[0]^2/2 - ...) + * <= 665/(1-y.x[0]) + * Quick check to see if we might need to break up the exponentiation + */ + if (m*(y.x[0]-1) / y.x[0] < -_MAX_EXPONENT * M_LN2) { + /* Now do it carefully, calling log() */ + double lg2y = log(y.x[0]) / M_LN2; + double lgAns = m * lg2y; + if (lgAns <= -_MAX_EXPONENT) { + maxExpt = (int)(nextPowerOf2(-_MAX_EXPONENT / lg2y + 1)/2); + } + } + if (m <= maxExpt) + { + double2 ans1 = pow_D(y, m); + ans = frexpD(ans1, &ansE); + ansE += m * yE; + RETURN_M_E(ans, ansE); + } + + q = m / maxExpt; + r = m % maxExpt; + /* y^m = (y^maxExpt)^q * y^r */ + y2r = pow2Scaled_D(y, r, &y2rE); + y2m = pow2Scaled_D(y, maxExpt, &y2mE); + y2mq = pow2Scaled_D(y2m, q, &y2mqE); + ans = frexpD(mul_DD(y2r, y2mq), &ansE); + y2mqE += y2mE * q; + ansE += y2mqE + y2rE; + ansE += m * yE; + RETURN_M_E(ans, ansE); +} + + +static double2 +pow4_D(double a, double b, double c, double d, int m) +{ + /* Compute ((a+b)/(c+d)) ^ m */ + double2 A, C, X; + if (m <= 0){ + if (m == 0) { + return DD_C_ONE; + } + return pow4_D(c, d, a, b, -m); + } + A = add_dd(a, b); + C = add_dd(c, d); + if (dd_is_zero(A)) { + return (dd_is_zero(C) ? DD_C_NAN : DD_C_ZERO); + } + if (dd_is_zero(C)) { + return (dd_is_negative(A) ? DD_C_NEGINF : DD_C_INF); + } + X = div_DD(A, C); + return pow_D(X, m); +} + +static double +pow4(double a, double b, double c, double d, int m) +{ + double2 ret = pow4_D(a, b, c, d, m); + return dd_to_double(ret); +} + + +static double2 +logpow4_D(double a, double b, double c, double d, int m) +{ + /* + * Compute log(((a+b)/(c+d)) ^ m) + * == m * log((a+b)/(c+d)) + * == m * log( 1 + (a+b-c-d)/(c+d)) + */ + double2 ans; + double2 A, C, X; + if (m == 0) { + return DD_C_ZERO; + } + A = add_dd(a, b); + C = add_dd(c, d); + if (dd_is_zero(A)) { + return (dd_is_zero(C) ? DD_C_ZERO : DD_C_NEGINF); + } + if (dd_is_zero(C)) { + return DD_C_INF; + } + X = div_DD(A, C); + assert(X.x[0] >= 0); + if (0.5 <= X.x[0] && X.x[0] <= 1.5) { + double2 A1 = sub_DD(A, C); + double2 X1 = div_DD(A1, C); + ans = log1pD(X1); + } else { + ans = logD(X); + } + ans = mul_dD(m, ans); + return ans; +} + +static double +logpow4(double a, double b, double c, double d, int m) +{ + double2 ans = logpow4_D(a, b, c, d, m); + return dd_to_double(ans); +} + +/* + * Compute a single term in the summation, A_v(n, x): + * A_v(n, x) = Binomial(n,v) * (1-x-v/n)^(n-v) * (x+v/n)^(v-1) + */ +static void +computeAv(int n, double x, int v, double2 Cman, int Cexpt, + double2 *pt1, double2 *pt2, double2 *pAv) +{ + int t1E, t2E, ansE; + double2 Av; + double2 t2x = sub_Dd(div_dd(n - v, n), x); /* 1 - x - v/n */ + double2 t2 = pow2Scaled_D(t2x, n-v, &t2E); + double2 t1x = add_Dd(div_dd(v, n), x); /* x + v/n */ + double2 t1 = pow2Scaled_D(t1x, v-1, &t1E); + double2 ans = mul_DD(t1, t2); + ans = mul_DD(ans, Cman); + ansE = Cexpt + t1E + t2E; + Av = ldexpD(ans, ansE); + *pAv = Av; + *pt1 = t1; + *pt2 = t2; +} + + +static ThreeProbs +_smirnov(int n, double x) +{ + double nx, alpha; + double2 AjSum = DD_C_ZERO; + double2 dAjSum = DD_C_ZERO; + double cdf, sf, pdf; + + int bUseUpperSum; + int nxfl, n1mxfl, n1mxceil; + ThreeProbs ret; + + if (!(n > 0 && x >= 0.0 && x <= 1.0)) { + RETURN_3PROBS(NAN, NAN, NAN); + } + if (n == 1) { + RETURN_3PROBS(1-x, x, 1.0); + } + if (x == 0.0) { + RETURN_3PROBS(1.0, 0.0, 1.0); + } + if (x == 1.0) { + RETURN_3PROBS(0.0, 1.0, 0.0); + } + + alpha = modNX(n, x, &nxfl, &nx); + n1mxfl = n - nxfl - (alpha == 0 ? 0 : 1); + n1mxceil = n - nxfl; + /* + * If alpha is 0, don't actually want to include the last term + * in either the lower or upper summations. + */ + if (alpha == 0) { + n1mxfl -= 1; + n1mxceil += 1; + } + + /* Special case: x <= 1/n */ + if (nxfl == 0 || (nxfl == 1 && alpha == 0)) { + double t = pow2(1, x, n-1); + pdf = (nx + 1) * t / (1+x); + cdf = x * t; + sf = 1 - cdf; + /* Adjust if x=1/n *exactly* */ + if (nxfl == 1) { + assert(alpha == 0); + pdf -= 0.5; + } + RETURN_3PROBS(sf, cdf, pdf); + } + /* Special case: x is so big, the sf underflows double64 */ + if (-2 * n * x*x < MINLOG) { + RETURN_3PROBS(0, 1, 0); + } + /* Special case: x >= 1 - 1/n */ + if (nxfl >= n-1) { + sf = pow2(1, -x, n); + cdf = 1 - sf; + pdf = n * sf/(1-x); + RETURN_3PROBS(sf, cdf, pdf); + } + /* Special case: n is so big, take too long to compute */ + if (n > SMIRNOV_MAX_COMPUTE_N) { + /* p ~ e^(-(6nx+1)^2 / 18n) */ + double logp = -pow(6.0*n*x+1, 2)/18.0/n; + /* Maximise precision for small p-value. */ + if (logp < -M_LN2) { + sf = exp(logp); + cdf = 1 - sf; + } else { + cdf = -expm1(logp); + sf = 1 - cdf; + } + pdf = (6.0*n*x+1) * 2 * sf/3; + RETURN_3PROBS(sf, cdf, pdf); + } + { + /* + * Use the upper sum if n is large enough, and x is small enough and + * the number of terms is going to be small enough. + * Otherwise it just drops accuracy, about 1.6bits * nUpperTerms + */ + int nUpperTerms = n - n1mxceil + 1; + bUseUpperSum = (nUpperTerms <= 1 && x < 0.5); + bUseUpperSum = (bUseUpperSum || + ((n >= SM_UPPERSUM_MIN_N) + && (nUpperTerms <= SM_UPPER_MAX_TERMS) + && (x <= 0.5 / sqrt(n)))); + } + + { + int start=0, step=1, nTerms=n1mxfl+1; + int j, firstJ = 0; + int vmid = n/2; + double2 Cman = DD_C_ONE; + int Cexpt = 0; + double2 Aj, dAj, t1, t2, dAjCoeff; + double2 oneOverX = div_dd(1, x); + + if (bUseUpperSum) { + start = n; + step = -1; + nTerms = n - n1mxceil + 1; + + t1 = pow4_D(1, x, 1, 0, n - 1); + t2 = DD_C_ONE; + Aj = t1; + + dAjCoeff = div_dD(n - 1, add_dd(1, x)); + dAjCoeff = add_DD(dAjCoeff, oneOverX); + } else { + t1 = oneOverX; + t2 = pow4_D(1, -x, 1, 0, n); + Aj = div_Dd(t2, x); + + dAjCoeff = div_DD(sub_dD(-1, mul_dd(n - 1, x)), sub_dd(1, x)); + dAjCoeff = div_Dd(dAjCoeff, x); + dAjCoeff = add_DD(dAjCoeff, oneOverX); + } + + dAj = mul_DD(Aj, dAjCoeff); + AjSum = add_DD(AjSum, Aj); + dAjSum = add_DD(dAjSum, dAj); + + updateBinomial(&Cman, &Cexpt, n, 0); + firstJ ++; + + for (j = firstJ; j < nTerms; j += 1) { + int v = start + j * step; + + computeAv(n, x, v, Cman, Cexpt, &t1, &t2, &Aj); + + if (dd_isfinite(Aj) && !dd_is_zero(Aj)) { + /* coeff = 1/x + (j-1)/(x+j/n) - (n-j)/(1-x-j/n) */ + dAjCoeff = sub_DD(div_dD((n * (v - 1)), add_dd(nxfl + v, alpha)), + div_dD(((n - v) * n), sub_dd(n - nxfl - v, alpha))); + dAjCoeff = add_DD(dAjCoeff, oneOverX); + dAj = mul_DD(Aj, dAjCoeff); + + assert(dd_isfinite(Aj)); + AjSum = add_DD(AjSum, Aj); + dAjSum = add_DD(dAjSum, dAj); + } + /* Safe to terminate early? */ + if (!dd_is_zero(Aj)) { + if ((4*(nTerms-j) * fabs(dd_to_double(Aj)) < DBL_EPSILON * dd_to_double(AjSum)) + && (j != nTerms - 1)) { + break; + } + } + else if (j > vmid) { + assert(dd_is_zero(Aj)); + break; + } + + updateBinomial(&Cman, &Cexpt, n, j); + } + assert(dd_isfinite(AjSum)); + assert(dd_isfinite(dAjSum)); + { + double2 derivD = mul_dD(x, dAjSum); + double2 probD = mul_dD(x, AjSum); + double deriv = dd_to_double(derivD); + double prob = dd_to_double(probD); + + assert (nx != 1 || alpha > 0); + if (step < 0) { + cdf = prob; + sf = 1-prob; + pdf = deriv; + } else { + cdf = 1-prob; + sf = prob; + pdf = -deriv; + } + } + } + + pdf = MAX(0, pdf); + cdf = CLIP(cdf, 0, 1); + sf = CLIP(sf, 0, 1); + RETURN_3PROBS(sf, cdf, pdf); +} + +/* + * Functional inverse of Smirnov distribution + * finds x such that smirnov(n, x) = psf; smirnovc(n, x) = pcdf). + */ +static double +_smirnovi(int n, double psf, double pcdf) +{ + /* + * Need to use a bracketing NR algorithm here and be very careful + * about the starting point. + */ + double x, logpcdf; + int iterations = 0; + int function_calls = 0; + double a=0, b=1; + double maxlogpcdf, psfrootn; + double dx, dxold; + + if (!(n > 0 && psf >= 0.0 && pcdf >= 0.0 && pcdf <= 1.0 && psf <= 1.0)) { + sf_error("smirnovi", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (fabs(1.0 - pcdf - psf) > 4* DBL_EPSILON) { + sf_error("smirnovi", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + /* STEP 1: Handle psf==0, or pcdf == 0 */ + if (pcdf == 0.0) { + return 0.0; + } + if (psf == 0.0) { + return 1.0; + } + /* STEP 2: Handle n=1 */ + if (n == 1) { + return pcdf; + } + + /* STEP 3 Handle psf *very* close to 0. Correspond to (n-1)/n < x < 1 */ + psfrootn = pow(psf, 1.0 / n); + /* xmin > 1 - 1.0 / n */ + if (n < 150 && n*psfrootn <= 1) { + /* Solve exactly. */ + x = 1 - psfrootn; + return x; + } + + logpcdf = (pcdf < 0.5 ? log(pcdf) : log1p(-psf)); + + /* + * STEP 4 Find bracket and initial estimate for use in N-R + * 4(a) Handle 0 < x <= 1/n: pcdf = x * (1+x)^*(n-1) + */ + maxlogpcdf = logpow4(1, 0.0, n, 0, 1) + logpow4(n, 1, n, 0, n - 1); + if (logpcdf <= maxlogpcdf) { + double xmin = pcdf / SCIPY_El; + double xmax = pcdf; + double P1 = pow4(n, 1, n, 0, n - 1) / n; + double R = pcdf/P1; + double z0 = R; + /* + * Do one iteration of N-R solving: z*e^(z-1) = R, with z0=pcdf/P1 + * z <- z - (z exp(z-1) - pcdf)/((z+1)exp(z-1)) + * If z_0 = R, z_1 = R(1-exp(1-R))/(R+1) + */ + if (R >= 1) { + /* + * R=1 is OK; + * R>1 can happen due to truncation error for x = (1-1/n)+-eps + */ + R = 1; + x = R/n; + return x; + } + z0 = (z0*z0 + R * exp(1-z0))/(1+z0); + x = z0/n; + a = xmin*(1 - 4 * DBL_EPSILON); + a = MAX(a, 0); + b = xmax * (1 + 4 * DBL_EPSILON); + b = MIN(b, 1.0/n); + x = CLIP(x, a, b); + } + else + { + /* 4(b) : 1/n < x < (n-1)/n */ + double xmin = 1 - psfrootn; + double logpsf = (psf < 0.5 ? log(psf) : log1p(-pcdf)); + double xmax = sqrt(-logpsf / (2.0L * n)); + double xmax6 = xmax - 1.0L / (6 * n); + a = xmin; + b = xmax; + /* Allow for a little rounding error */ + a *= 1 - 4 * DBL_EPSILON; + b *= 1 + 4 * DBL_EPSILON; + a = MAX(xmin, 1.0/n); + b = MIN(xmax, 1-1.0/n); + x = xmax6; + } + if (x < a || x > b) { + x = (a + b)/2; + } + assert (x < 1); + + /* + * Skip computing fa, fb as that takes cycles and the exact values + * are not needed. + */ + + /* STEP 5 Run N-R. + * smirnov should be well-enough behaved for NR starting at this location. + * Use smirnov(n, x)-psf, or pcdf - smirnovc(n, x), whichever has smaller p. + */ + dxold = b - a; + dx = dxold; + do { + double dfdx, x0 = x, deltax, df; + assert(x < 1); + assert(x > 0); + { + ThreeProbs probs = _smirnov(n, x0); + ++function_calls; + df = ((pcdf < 0.5) ? (pcdf - probs.cdf) : (probs.sf - psf)); + dfdx = -probs.pdf; + } + if (df == 0) { + return x; + } + /* Update the bracketing interval */ + if (df > 0 && x > a) { + a = x; + } else if (df < 0 && x < b) { + b = x; + } + + if (dfdx == 0) { + /* + * x was not within tolerance, but now we hit a 0 derivative. + * This implies that x >> 1/sqrt(n), and even then |smirnovp| >= |smirnov| + * so this condition is unexpected. Do a bisection step. + */ + x = (a+b)/2; + deltax = x0 - x; + } else { + deltax = df / dfdx; + x = x0 - deltax; + } + /* + * Check out-of-bounds. + * Not expecting this to happen ofen --- smirnov is convex near x=1 and + * concave near x=0, and we should be approaching from the correct side. + * If out-of-bounds, replace x with a midpoint of the bracket. + * Also check fast enough convergence. + */ + if ((a <= x) && (x <= b) && (fabs(2 * deltax) <= fabs(dxold) || fabs(dxold) < 256 * DBL_EPSILON)) { + dxold = dx; + dx = deltax; + } else { + dxold = dx; + dx = dx / 2; + x = (a + b) / 2; + deltax = x0 - x; + } + /* + * Note that if psf is close to 1, f(x) -> 1, f'(x) -> -1. + * => abs difference |x-x0| is approx |f(x)-p| >= DBL_EPSILON, + * => |x-x0|/x >= DBL_EPSILON/x. + * => cannot use a purely relative criteria as it will fail for x close to 0. + */ + if (_within_tol(x, x0, (psf < 0.5 ? 0 : _xtol), _rtol)) { + break; + } + if (++iterations > MAXITER) { + sf_error("smirnovi", SF_ERROR_SLOW, NULL); + return (x); + } + } while (1); + return x; +} + + +double +smirnov(int n, double d) +{ + ThreeProbs probs; + if (isnan(d)) { + return NAN; + } + probs = _smirnov(n, d); + return probs.sf; +} + +double +smirnovc(int n, double d) +{ + ThreeProbs probs; + if (isnan(d)) { + return NAN; + } + probs = _smirnov(n, d); + return probs.cdf; +} + + +/* + * Derivative of smirnov(n, d) + * One interior point of discontinuity at d=1/n. +*/ +double +smirnovp(int n, double d) +{ + ThreeProbs probs; + if (!(n > 0 && d >= 0.0 && d <= 1.0)) { + return (NAN); + } + if (n == 1) { + /* Slope is always -1 for n=1, even at d = 1.0 */ + return -1.0; + } + if (d == 1.0) { + return -0.0; + } + /* + * If d is 0, the derivative is discontinuous, but approaching + * from the right the limit is -1 + */ + if (d == 0.0) { + return -1.0; + } + probs = _smirnov(n, d); + return -probs.pdf; +} + + +double +smirnovi(int n, double p) +{ + if (isnan(p)) { + return NAN; + } + return _smirnovi(n, p, 1-p); +} + +double +smirnovci(int n, double p) +{ + if (isnan(p)) { + return NAN; + } + return _smirnovi(n, 1-p, p); +} diff --git a/gtsam/3rdparty/cephes/cephes/lanczos.c b/gtsam/3rdparty/cephes/cephes/lanczos.c new file mode 100644 index 0000000000..f92a8d2088 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/lanczos.c @@ -0,0 +1,56 @@ +/* (C) Copyright John Maddock 2006. + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ + +/* Scipy changes: + * - 06-22-2016: Removed all code not related to double precision and + * ported to c for use in Cephes + */ + +#include "mconf.h" +#include "lanczos.h" + + +static double lanczos_sum(double x) +{ + return ratevl(x, lanczos_num, + sizeof(lanczos_num) / sizeof(lanczos_num[0]) - 1, + lanczos_denom, + sizeof(lanczos_denom) / sizeof(lanczos_denom[0]) - 1); +} + + +double lanczos_sum_expg_scaled(double x) +{ + return ratevl(x, lanczos_sum_expg_scaled_num, + sizeof(lanczos_sum_expg_scaled_num) / sizeof(lanczos_sum_expg_scaled_num[0]) - 1, + lanczos_sum_expg_scaled_denom, + sizeof(lanczos_sum_expg_scaled_denom) / sizeof(lanczos_sum_expg_scaled_denom[0]) - 1); +} + + +static double lanczos_sum_near_1(double dx) +{ + double result = 0; + unsigned k; + + for (k = 1; k <= sizeof(lanczos_sum_near_1_d)/sizeof(lanczos_sum_near_1_d[0]); ++k) { + result += (-lanczos_sum_near_1_d[k-1]*dx)/(k*dx + k*k); + } + return result; +} + + +static double lanczos_sum_near_2(double dx) +{ + double result = 0; + double x = dx + 2; + unsigned k; + + for(k = 1; k <= sizeof(lanczos_sum_near_2_d)/sizeof(lanczos_sum_near_2_d[0]); ++k) { + result += (-lanczos_sum_near_2_d[k-1]*dx)/(x + k*x + k*k - 1); + } + return result; +} diff --git a/gtsam/3rdparty/cephes/cephes/lanczos.h b/gtsam/3rdparty/cephes/cephes/lanczos.h new file mode 100644 index 0000000000..92ab8c1b26 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/lanczos.h @@ -0,0 +1,133 @@ +/* (C) Copyright John Maddock 2006. + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ + +/* Both lanczos.h and lanczos.c were formed from Boost's lanczos.hpp + * + * Scipy changes: + * - 06-22-2016: Removed all code not related to double precision and + * ported to c for use in Cephes. Note that the order of the + * coefficients is reversed to match the behavior of polevl. + */ + +/* + * Optimal values for G for each N are taken from + * https://web.viu.ca/pughg/phdThesis/phdThesis.pdf, + * as are the theoretical error bounds. + * + * Constants calculated using the method described by Godfrey + * https://my.fit.edu/~gabdo/gamma.txt and elaborated by Toth at + * https://www.rskey.org/gamma.htm using NTL::RR at 1000 bit precision. + */ + +/* + * Lanczos Coefficients for N=13 G=6.024680040776729583740234375 + * Max experimental error (with arbitrary precision arithmetic) 1.196214e-17 + * Generated with compiler: Microsoft Visual C++ version 8.0 on Win32 at Mar 23 2006 + * + * Use for double precision. + */ + +#ifndef LANCZOS_H +#define LANCZOS_H + + +static const double lanczos_num[13] = { + 2.506628274631000270164908177133837338626, + 210.8242777515793458725097339207133627117, + 8071.672002365816210638002902272250613822, + 186056.2653952234950402949897160456992822, + 2876370.628935372441225409051620849613599, + 31426415.58540019438061423162831820536287, + 248874557.8620541565114603864132294232163, + 1439720407.311721673663223072794912393972, + 6039542586.35202800506429164430729792107, + 17921034426.03720969991975575445893111267, + 35711959237.35566804944018545154716670596, + 42919803642.64909876895789904700198885093, + 23531376880.41075968857200767445163675473 +}; + +static const double lanczos_denom[13] = { + 1, + 66, + 1925, + 32670, + 357423, + 2637558, + 13339535, + 45995730, + 105258076, + 150917976, + 120543840, + 39916800, + 0 +}; + +static const double lanczos_sum_expg_scaled_num[13] = { + 0.006061842346248906525783753964555936883222, + 0.5098416655656676188125178644804694509993, + 19.51992788247617482847860966235652136208, + 449.9445569063168119446858607650988409623, + 6955.999602515376140356310115515198987526, + 75999.29304014542649875303443598909137092, + 601859.6171681098786670226533699352302507, + 3481712.15498064590882071018964774556468, + 14605578.08768506808414169982791359218571, + 43338889.32467613834773723740590533316085, + 86363131.28813859145546927288977868422342, + 103794043.1163445451906271053616070238554, + 56906521.91347156388090791033559122686859 +}; + +static const double lanczos_sum_expg_scaled_denom[13] = { + 1, + 66, + 1925, + 32670, + 357423, + 2637558, + 13339535, + 45995730, + 105258076, + 150917976, + 120543840, + 39916800, + 0 +}; + +static const double lanczos_sum_near_1_d[12] = { + 0.3394643171893132535170101292240837927725e-9, + -0.2499505151487868335680273909354071938387e-8, + 0.8690926181038057039526127422002498960172e-8, + -0.1933117898880828348692541394841204288047e-7, + 0.3075580174791348492737947340039992829546e-7, + -0.2752907702903126466004207345038327818713e-7, + -0.1515973019871092388943437623825208095123e-5, + 0.004785200610085071473880915854204301886437, + -0.1993758927614728757314233026257810172008, + 1.483082862367253753040442933770164111678, + -3.327150580651624233553677113928873034916, + 2.208709979316623790862569924861841433016 +}; + +static const double lanczos_sum_near_2_d[12] = { + 0.1009141566987569892221439918230042368112e-8, + -0.7430396708998719707642735577238449585822e-8, + 0.2583592566524439230844378948704262291927e-7, + -0.5746670642147041587497159649318454348117e-7, + 0.9142922068165324132060550591210267992072e-7, + -0.8183698410724358930823737982119474130069e-7, + -0.4506604409707170077136555010018549819192e-5, + 0.01422519127192419234315002746252160965831, + -0.5926941084905061794445733628891024027949, + 4.408830289125943377923077727900630927902, + -9.8907772644920670589288081640128194231, + 6.565936202082889535528455955485877361223 +}; + +static const double lanczos_g = 6.024680040776729583740234375; + +#endif diff --git a/gtsam/3rdparty/cephes/cephes/mconf.h b/gtsam/3rdparty/cephes/cephes/mconf.h new file mode 100644 index 0000000000..9f3deb628f --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/mconf.h @@ -0,0 +1,109 @@ +/* mconf.h + * + * Common include file for math routines + * + * + * + * SYNOPSIS: + * + * #include "mconf.h" + * + * + * + * DESCRIPTION: + * + * The file includes a conditional assembly definition for the type of + * computer arithmetic (IEEE, Motorola IEEE, or UNKnown). + * + * For little-endian computers, such as IBM PC, that follow the + * IEEE Standard for Binary Floating Point Arithmetic (ANSI/IEEE + * Std 754-1985), the symbol IBMPC should be defined. These + * numbers have 53-bit significands. In this mode, constants + * are provided as arrays of hexadecimal 16 bit integers. + * + * Big-endian IEEE format is denoted MIEEE. On some RISC + * systems such as Sun SPARC, double precision constants + * must be stored on 8-byte address boundaries. Since integer + * arrays may be aligned differently, the MIEEE configuration + * may fail on such machines. + * + * To accommodate other types of computer arithmetic, all + * constants are also provided in a normal decimal radix + * which one can hope are correctly converted to a suitable + * format by the available C language compiler. To invoke + * this mode, define the symbol UNK. + * + * An important difference among these modes is a predefined + * set of machine arithmetic constants for each. The numbers + * MACHEP (the machine roundoff error), MAXNUM (largest number + * represented), and several other parameters are preset by + * the configuration symbol. Check the file const.c to + * ensure that these values are correct for your computer. + * + * Configurations NANS, INFINITIES, MINUSZERO, and DENORMAL + * may fail on many systems. Verify that they are supposed + * to work on your computer. + */ + +/* + * Cephes Math Library Release 2.3: June, 1995 + * Copyright 1984, 1987, 1989, 1995 by Stephen L. Moshier + */ + +#ifndef CEPHES_MCONF_H +#define CEPHES_MCONF_H + +#include +#include + +#include "cephes_names.h" +#include "cephes.h" +#include "polevl.h" +#include "sf_error.h" + +#define MAXITER 500 +#define EDOM 33 +#define ERANGE 34 + +/* Type of computer arithmetic */ + +/* UNKnown arithmetic, invokes coefficients given in + * normal decimal format. Beware of range boundary + * problems (MACHEP, MAXLOG, etc. in const.c) and + * roundoff problems in pow.c: + * (Sun SPARCstation) + */ + +/* SciPy note: by defining UNK, we prevent the compiler from + * casting integers to floating point numbers. If the Endianness + * is detected incorrectly, this causes problems on some platforms. + */ +#define UNK 1 + +/* Define to support tiny denormal numbers, else undefine. */ +#define DENORMAL 1 + +#define gamma Gamma + +/* + * Enable loop unrolling on GCC and use faster isnan et al. + */ +#if !defined(__clang__) && defined(__GNUC__) && defined(__GNUC_MINOR__) +#if __GNUC__ >= 5 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 4) +#pragma GCC optimize("unroll-loops") +#define cephes_isnan(x) __builtin_isnan(x) +#define cephes_isinf(x) __builtin_isinf(x) +#define cephes_isfinite(x) __builtin_isfinite(x) +#endif +#endif +#ifndef cephes_isnan +#define cephes_isnan(x) isnan(x) +#define cephes_isinf(x) isinf(x) +#define cephes_isfinite(x) isfinite(x) +#endif + +/* Constants needed that are not available in the C standard library */ +#define SCIPY_EULER 0.577215664901532860606512090082402431 /* Euler constant */ +#define SCIPY_El 2.718281828459045235360287471352662498L /* e as long double */ + +#endif /* CEPHES_MCONF_H */ diff --git a/gtsam/3rdparty/cephes/cephes/nbdtr.c b/gtsam/3rdparty/cephes/cephes/nbdtr.c new file mode 100644 index 0000000000..7697f257ee --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/nbdtr.c @@ -0,0 +1,207 @@ +/* nbdtr.c + * + * Negative binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, nbdtr(); + * + * y = nbdtr( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms 0 through k of the negative + * binomial distribution: + * + * k + * -- ( n+j-1 ) n j + * > ( ) p (1-p) + * -- ( j ) + * j=0 + * + * In a sequence of Bernoulli trials, this is the probability + * that k or fewer failures precede the nth success. + * + * The terms are not computed individually; instead the incomplete + * beta integral is employed, according to the formula + * + * y = nbdtr( k, n, p ) = incbet( n, k+1, p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * Tested at random points (a,b,p), with p between 0 and 1. + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,100 100000 1.7e-13 8.8e-15 + * See also incbet.c. + * + */ + /* nbdtrc.c + * + * Complemented negative binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, nbdtrc(); + * + * y = nbdtrc( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms k+1 to infinity of the negative + * binomial distribution: + * + * inf + * -- ( n+j-1 ) n j + * > ( ) p (1-p) + * -- ( j ) + * j=k+1 + * + * The terms are not computed individually; instead the incomplete + * beta integral is employed, according to the formula + * + * y = nbdtrc( k, n, p ) = incbet( k+1, n, 1-p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * Tested at random points (a,b,p), with p between 0 and 1. + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,100 100000 1.7e-13 8.8e-15 + * See also incbet.c. + */ + +/* nbdtrc + * + * Complemented negative binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, nbdtrc(); + * + * y = nbdtrc( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms k+1 to infinity of the negative + * binomial distribution: + * + * inf + * -- ( n+j-1 ) n j + * > ( ) p (1-p) + * -- ( j ) + * j=k+1 + * + * The terms are not computed individually; instead the incomplete + * beta integral is employed, according to the formula + * + * y = nbdtrc( k, n, p ) = incbet( k+1, n, 1-p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * See incbet.c. + */ + /* nbdtri + * + * Functional inverse of negative binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, nbdtri(); + * + * p = nbdtri( k, n, y ); + * + * DESCRIPTION: + * + * Finds the argument p such that nbdtr(k,n,p) is equal to y. + * + * ACCURACY: + * + * Tested at random points (a,b,y), with y between 0 and 1. + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,100 100000 1.5e-14 8.5e-16 + * See also incbi.c. + */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +double nbdtrc(int k, int n, double p) +{ + double dk, dn; + + if ((p < 0.0) || (p > 1.0)) + goto domerr; + if (k < 0) { + domerr: + sf_error("nbdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + dk = k + 1; + dn = n; + return (incbet(dk, dn, 1.0 - p)); +} + + + +double nbdtr(int k, int n, double p) +{ + double dk, dn; + + if ((p < 0.0) || (p > 1.0)) + goto domerr; + if (k < 0) { + domerr: + sf_error("nbdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + dk = k + 1; + dn = n; + return (incbet(dn, dk, p)); +} + + + +double nbdtri(int k, int n, double p) +{ + double dk, dn, w; + + if ((p < 0.0) || (p > 1.0)) + goto domerr; + if (k < 0) { + domerr: + sf_error("nbdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + dk = k + 1; + dn = n; + w = incbi(dn, dk, p); + return (w); +} diff --git a/gtsam/3rdparty/cephes/cephes/ndtr.c b/gtsam/3rdparty/cephes/cephes/ndtr.c new file mode 100644 index 0000000000..168e98b5ab --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ndtr.c @@ -0,0 +1,305 @@ +/* ndtr.c + * + * Normal distribution function + * + * + * + * SYNOPSIS: + * + * double x, y, ndtr(); + * + * y = ndtr( x ); + * + * + * + * DESCRIPTION: + * + * Returns the area under the Gaussian probability density + * function, integrated from minus infinity to x: + * + * x + * - + * 1 | | 2 + * ndtr(x) = --------- | exp( - t /2 ) dt + * sqrt(2pi) | | + * - + * -inf. + * + * = ( 1 + erf(z) ) / 2 + * = erfc(z) / 2 + * + * where z = x/sqrt(2). Computation is via the functions + * erf and erfc. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -13,0 30000 3.4e-14 6.7e-15 + * + * + * ERROR MESSAGES: + * + * message condition value returned + * erfc underflow x > 37.519379347 0.0 + * + */ +/* erf.c + * + * Error function + * + * + * + * SYNOPSIS: + * + * double x, y, erf(); + * + * y = erf( x ); + * + * + * + * DESCRIPTION: + * + * The integral is + * + * x + * - + * 2 | | 2 + * erf(x) = -------- | exp( - t ) dt. + * sqrt(pi) | | + * - + * 0 + * + * For 0 <= |x| < 1, erf(x) = x * P4(x**2)/Q5(x**2); otherwise + * erf(x) = 1 - erfc(x). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,1 30000 3.7e-16 1.0e-16 + * + */ +/* erfc.c + * + * Complementary error function + * + * + * + * SYNOPSIS: + * + * double x, y, erfc(); + * + * y = erfc( x ); + * + * + * + * DESCRIPTION: + * + * + * 1 - erf(x) = + * + * inf. + * - + * 2 | | 2 + * erfc(x) = -------- | exp( - t ) dt + * sqrt(pi) | | + * - + * x + * + * + * For small x, erfc(x) = 1 - erf(x); otherwise rational + * approximations are computed. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,26.6417 30000 5.7e-14 1.5e-14 + */ + + +/* + * Cephes Math Library Release 2.2: June, 1992 + * Copyright 1984, 1987, 1988, 1992 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include /* DBL_EPSILON */ +#include "mconf.h" + +extern double MAXLOG; + +static double P[] = { + 2.46196981473530512524E-10, + 5.64189564831068821977E-1, + 7.46321056442269912687E0, + 4.86371970985681366614E1, + 1.96520832956077098242E2, + 5.26445194995477358631E2, + 9.34528527171957607540E2, + 1.02755188689515710272E3, + 5.57535335369399327526E2 +}; + +static double Q[] = { + /* 1.00000000000000000000E0, */ + 1.32281951154744992508E1, + 8.67072140885989742329E1, + 3.54937778887819891062E2, + 9.75708501743205489753E2, + 1.82390916687909736289E3, + 2.24633760818710981792E3, + 1.65666309194161350182E3, + 5.57535340817727675546E2 +}; + +static double R[] = { + 5.64189583547755073984E-1, + 1.27536670759978104416E0, + 5.01905042251180477414E0, + 6.16021097993053585195E0, + 7.40974269950448939160E0, + 2.97886665372100240670E0 +}; + +static double S[] = { + /* 1.00000000000000000000E0, */ + 2.26052863220117276590E0, + 9.39603524938001434673E0, + 1.20489539808096656605E1, + 1.70814450747565897222E1, + 9.60896809063285878198E0, + 3.36907645100081516050E0 +}; + +static double T[] = { + 9.60497373987051638749E0, + 9.00260197203842689217E1, + 2.23200534594684319226E3, + 7.00332514112805075473E3, + 5.55923013010394962768E4 +}; + +static double U[] = { + /* 1.00000000000000000000E0, */ + 3.35617141647503099647E1, + 5.21357949780152679795E2, + 4.59432382970980127987E3, + 2.26290000613890934246E4, + 4.92673942608635921086E4 +}; + +#define UTHRESH 37.519379347 + + +double ndtr(double a) +{ + double x, y, z; + + if (cephes_isnan(a)) { + sf_error("ndtr", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + x = a * M_SQRT1_2; + z = fabs(x); + + if (z < M_SQRT1_2) { + y = 0.5 + 0.5 * erf(x); + } + else { + y = 0.5 * erfc(z); + if (x > 0) { + y = 1.0 - y; + } + } + + return y; +} + + +double erfc(double a) +{ + double p, q, x, y, z; + + if (cephes_isnan(a)) { + sf_error("erfc", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (a < 0.0) { + x = -a; + } + else { + x = a; + } + + if (x < 1.0) { + return 1.0 - erf(a); + } + + z = -a * a; + + if (z < -MAXLOG) { + goto under; + } + + z = exp(z); + + if (x < 8.0) { + p = polevl(x, P, 8); + q = p1evl(x, Q, 8); + } + else { + p = polevl(x, R, 5); + q = p1evl(x, S, 6); + } + y = (z * p) / q; + + if (a < 0) { + y = 2.0 - y; + } + + if (y != 0.0) { + return y; + } + +under: + sf_error("erfc", SF_ERROR_UNDERFLOW, NULL); + if (a < 0) { + return 2.0; + } + else { + return 0.0; + } +} + + + +double erf(double x) +{ + double y, z; + + if (cephes_isnan(x)) { + sf_error("erf", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x < 0.0) { + return -erf(-x); + } + + if (fabs(x) > 1.0) { + return (1.0 - erfc(x)); + } + z = x * x; + + y = x * polevl(z, T, 4) / p1evl(z, U, 5); + return y; +} diff --git a/gtsam/3rdparty/cephes/cephes/ndtri.c b/gtsam/3rdparty/cephes/cephes/ndtri.c new file mode 100644 index 0000000000..e7fe5cce04 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ndtri.c @@ -0,0 +1,176 @@ +/* ndtri.c + * + * Inverse of Normal distribution function + * + * + * + * SYNOPSIS: + * + * double x, y, ndtri(); + * + * x = ndtri( y ); + * + * + * + * DESCRIPTION: + * + * Returns the argument, x, for which the area under the + * Gaussian probability density function (integrated from + * minus infinity to x) is equal to y. + * + * + * For small arguments 0 < y < exp(-2), the program computes + * z = sqrt( -2.0 * log(y) ); then the approximation is + * x = z - log(z)/z - (1/z) P(1/z) / Q(1/z). + * There are two rational functions P/Q, one for 0 < y < exp(-32) + * and the other for y up to exp(-2). For larger arguments, + * w = y - 0.5, and x/sqrt(2pi) = w + w**3 R(w**2)/S(w**2)). + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0.125, 1 20000 7.2e-16 1.3e-16 + * IEEE 3e-308, 0.135 50000 4.6e-16 9.8e-17 + * + * + * ERROR MESSAGES: + * + * message condition value returned + * ndtri domain x < 0 NAN + * ndtri domain x > 1 NAN + * + */ + + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +/* sqrt(2pi) */ +static double s2pi = 2.50662827463100050242E0; + +/* approximation for 0 <= |y - 0.5| <= 3/8 */ +static double P0[5] = { + -5.99633501014107895267E1, + 9.80010754185999661536E1, + -5.66762857469070293439E1, + 1.39312609387279679503E1, + -1.23916583867381258016E0, +}; + +static double Q0[8] = { + /* 1.00000000000000000000E0, */ + 1.95448858338141759834E0, + 4.67627912898881538453E0, + 8.63602421390890590575E1, + -2.25462687854119370527E2, + 2.00260212380060660359E2, + -8.20372256168333339912E1, + 1.59056225126211695515E1, + -1.18331621121330003142E0, +}; + +/* Approximation for interval z = sqrt(-2 log y ) between 2 and 8 + * i.e., y between exp(-2) = .135 and exp(-32) = 1.27e-14. + */ +static double P1[9] = { + 4.05544892305962419923E0, + 3.15251094599893866154E1, + 5.71628192246421288162E1, + 4.40805073893200834700E1, + 1.46849561928858024014E1, + 2.18663306850790267539E0, + -1.40256079171354495875E-1, + -3.50424626827848203418E-2, + -8.57456785154685413611E-4, +}; + +static double Q1[8] = { + /* 1.00000000000000000000E0, */ + 1.57799883256466749731E1, + 4.53907635128879210584E1, + 4.13172038254672030440E1, + 1.50425385692907503408E1, + 2.50464946208309415979E0, + -1.42182922854787788574E-1, + -3.80806407691578277194E-2, + -9.33259480895457427372E-4, +}; + +/* Approximation for interval z = sqrt(-2 log y ) between 8 and 64 + * i.e., y between exp(-32) = 1.27e-14 and exp(-2048) = 3.67e-890. + */ + +static double P2[9] = { + 3.23774891776946035970E0, + 6.91522889068984211695E0, + 3.93881025292474443415E0, + 1.33303460815807542389E0, + 2.01485389549179081538E-1, + 1.23716634817820021358E-2, + 3.01581553508235416007E-4, + 2.65806974686737550832E-6, + 6.23974539184983293730E-9, +}; + +static double Q2[8] = { + /* 1.00000000000000000000E0, */ + 6.02427039364742014255E0, + 3.67983563856160859403E0, + 1.37702099489081330271E0, + 2.16236993594496635890E-1, + 1.34204006088543189037E-2, + 3.28014464682127739104E-4, + 2.89247864745380683936E-6, + 6.79019408009981274425E-9, +}; + +double ndtri(double y0) +{ + double x, y, z, y2, x0, x1; + int code; + + if (y0 == 0.0) { + return -INFINITY; + } + if (y0 == 1.0) { + return INFINITY; + } + if (y0 < 0.0 || y0 > 1.0) { + sf_error("ndtri", SF_ERROR_DOMAIN, NULL); + return NAN; + } + code = 1; + y = y0; + if (y > (1.0 - 0.13533528323661269189)) { /* 0.135... = exp(-2) */ + y = 1.0 - y; + code = 0; + } + + if (y > 0.13533528323661269189) { + y = y - 0.5; + y2 = y * y; + x = y + y * (y2 * polevl(y2, P0, 4) / p1evl(y2, Q0, 8)); + x = x * s2pi; + return (x); + } + + x = sqrt(-2.0 * log(y)); + x0 = x - log(x) / x; + + z = 1.0 / x; + if (x < 8.0) /* y > exp(-32) = 1.2664165549e-14 */ + x1 = z * polevl(z, P1, 8) / p1evl(z, Q1, 8); + else + x1 = z * polevl(z, P2, 8) / p1evl(z, Q2, 8); + x = x0 - x1; + if (code != 0) + x = -x; + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/owens_t.c b/gtsam/3rdparty/cephes/cephes/owens_t.c new file mode 100644 index 0000000000..6eb063510e --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/owens_t.c @@ -0,0 +1,364 @@ +/* Copyright Benjamin Sobotta 2012 + * + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ + +/* + * Reference: + * Mike Patefield, David Tandy + * FAST AND ACCURATE CALCULATION OF OWEN'S T-FUNCTION + * Journal of Statistical Software, 5 (5), 1-25 + */ +#include "mconf.h" + +static const int SELECT_METHOD[] = { + 0, 0, 1, 12, 12, 12, 12, 12, 12, 12, 12, 15, 15, 15, 8, + 0, 1, 1, 2, 2, 4, 4, 13, 13, 14, 14, 15, 15, 15, 8, + 1, 1, 2, 2, 2, 4, 4, 14, 14, 14, 14, 15, 15, 15, 9, + 1, 1, 2, 4, 4, 4, 4, 6, 6, 15, 15, 15, 15, 15, 9, + 1, 2 , 2, 4, 4, 5 , 5, 7, 7, 16 ,16, 16, 11, 11, 10, + 1, 2 , 4, 4 , 4, 5 , 5, 7, 7, 16, 16, 16, 11, 11, 11, + 1, 2 , 3, 3, 5, 5 , 7, 7, 16, 16, 16, 16, 16, 11, 11, + 1, 2 , 3 , 3 , 5, 5, 17, 17, 17, 17, 16, 16, 16, 11, 11 +}; + +static const double HRANGE[] = {0.02, 0.06, 0.09, 0.125, 0.26, 0.4, 0.6, 1.6, + 1.7, 2.33, 2.4, 3.36, 3.4, 4.8}; + +static const double ARANGE[] = {0.025, 0.09, 0.15, 0.36, 0.5, 0.9, 0.99999}; + +static const double ORD[] = {2, 3, 4, 5, 7, 10, 12, 18, 10, 20, 30, 0, 4, 7, + 8, 20, 0, 0}; + +static const int METHODS[] = {1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 3, 4, 4, 4, 4, + 5, 6}; + +static const double C[] = { + 0.99999999999999999999999729978162447266851932041876728736094298092917625009873, + -0.99999999999999999999467056379678391810626533251885323416799874878563998732905968, + 0.99999999999999999824849349313270659391127814689133077036298754586814091034842536, + -0.9999999999999997703859616213643405880166422891953033591551179153879839440241685, + 0.99999999999998394883415238173334565554173013941245103172035286759201504179038147, + -0.9999999999993063616095509371081203145247992197457263066869044528823599399470977, + 0.9999999999797336340409464429599229870590160411238245275855903767652432017766116267, + -0.999999999574958412069046680119051639753412378037565521359444170241346845522403274, + 0.9999999933226234193375324943920160947158239076786103108097456617750134812033362048, + -0.9999999188923242461073033481053037468263536806742737922476636768006622772762168467, + 0.9999992195143483674402853783549420883055129680082932629160081128947764415749728967, + -0.999993935137206712830997921913316971472227199741857386575097250553105958772041501, + 0.99996135597690552745362392866517133091672395614263398912807169603795088421057688716, + -0.99979556366513946026406788969630293820987757758641211293079784585126692672425362469, + 0.999092789629617100153486251423850590051366661947344315423226082520411961968929483, + -0.996593837411918202119308620432614600338157335862888580671450938858935084316004769854, + 0.98910017138386127038463510314625339359073956513420458166238478926511821146316469589567, + -0.970078558040693314521331982203762771512160168582494513347846407314584943870399016019, + 0.92911438683263187495758525500033707204091967947532160289872782771388170647150321633673, + -0.8542058695956156057286980736842905011429254735181323743367879525470479126968822863, + 0.73796526033030091233118357742803709382964420335559408722681794195743240930748630755, + -0.58523469882837394570128599003785154144164680587615878645171632791404210655891158, + 0.415997776145676306165661663581868460503874205343014196580122174949645271353372263, + -0.2588210875241943574388730510317252236407805082485246378222935376279663808416534365, + 0.1375535825163892648504646951500265585055789019410617565727090346559210218472356689, + -0.0607952766325955730493900985022020434830339794955745989150270485056436844239206648, + 0.0216337683299871528059836483840390514275488679530797294557060229266785853764115, + -0.00593405693455186729876995814181203900550014220428843483927218267309209471516256, + 0.0011743414818332946510474576182739210553333860106811865963485870668929503649964142, + -1.489155613350368934073453260689881330166342484405529981510694514036264969925132E-4, + 9.072354320794357587710929507988814669454281514268844884841547607134260303118208E-6 +}; + +static const double PTS[] = { + 0.35082039676451715489E-02, 0.31279042338030753740E-01, + 0.85266826283219451090E-01, 0.16245071730812277011E+00, + 0.25851196049125434828E+00, 0.36807553840697533536E+00, + 0.48501092905604697475E+00, 0.60277514152618576821E+00, + 0.71477884217753226516E+00, 0.81475510988760098605E+00, + 0.89711029755948965867E+00, 0.95723808085944261843E+00, + 0.99178832974629703586E+00 +}; + +static const double WTS[] = { + 0.18831438115323502887E-01, 0.18567086243977649478E-01, + 0.18042093461223385584E-01, 0.17263829606398753364E-01, + 0.16243219975989856730E-01, 0.14994592034116704829E-01, + 0.13535474469662088392E-01, 0.11886351605820165233E-01, + 0.10070377242777431897E-01, 0.81130545742299586629E-02, + 0.60419009528470238773E-02, 0.38862217010742057883E-02, + 0.16793031084546090448E-02 +}; + + +static int get_method(double h, double a) { + int ihint, iaint, i; + + ihint = 14; + iaint = 7; + + for (i = 0; i < 14; i++) { + if (h <= HRANGE[i]) { + ihint = i; + break; + } + } + + for (i = 0; i < 7; i++) { + if (a <= ARANGE[i]) { + iaint = i; + break; + } + } + return SELECT_METHOD[iaint * 15 + ihint]; +} + + +static double owens_t_norm1(double x) { + return erf(x / sqrt(2)) / 2; +} + + +static double owens_t_norm2(double x) { + return erfc(x / sqrt(2)) / 2; +} + + +static double owensT1(double h, double a, double m) { + int j = 1; + int jj = 1; + + double hs = -0.5 * h * h; + double dhs = exp(hs); + double as = a * a; + double aj = a / (2 * M_PI); + double dj = expm1(hs); + double gj = hs * dhs; + + double val = atan(a) / (2 * M_PI); + + while (1) { + val += dj*aj / jj; + + if (m <= j) { + break; + } + j++; + jj += 2; + aj *= as; + dj = gj - dj; + gj *= hs / j; + } + + return val; +} + + +static double owensT2(double h, double a, double ah, double m) { + int i = 1; + int maxi = 2 * m + 1; + double hs = h * h; + double as = -a * a; + double y = 1.0 / hs; + double val = 0.0; + double vi = a*exp(-0.5 * ah * ah) / sqrt(2 * M_PI); + double z = (ndtr(ah) - 0.5) / h; + + while (1) { + val += z; + if (maxi <= i) { + break; + } + z = y * (vi - i * z); + vi *= as; + i += 2; + } + val *= exp(-0.5 * hs) / sqrt(2 * M_PI); + + return val; +} + + +static double owensT3(double h, double a, double ah) { + double aa, hh, y, vi, zi, result; + int i; + + aa = a * a; + hh = h * h; + y = 1 / hh; + + vi = a * exp(-ah * ah/ 2) / sqrt(2 * M_PI); + zi = owens_t_norm1(ah) / h; + result = 0; + + for(i = 0; i<= 30; i++) { + result += zi * C[i]; + zi = y * ((2 * i + 1) * zi - vi); + vi *= aa; + } + + result *= exp(-hh / 2) / sqrt(2 * M_PI); + + return result; +} + + +static double owensT4(double h, double a, double m) { + double maxi, hh, naa, ai, yi, result; + int i; + + maxi = 2 * m + 1; + hh = h * h; + naa = -a * a; + + i = 1; + ai = a * exp(-hh * (1 - naa) / 2) / (2 * M_PI); + yi = 1; + result = 0; + + while (1) { + result += ai * yi; + + if (maxi <= i) { + break; + } + + i += 2; + yi = (1 - hh * yi) / i; + ai *= naa; + } + + return result; +} + + +static double owensT5(double h, double a) { + double result, r, aa, nhh; + int i; + + result = 0; + r = 0; + aa = a * a; + nhh = -0.5 * h * h; + + for (i = 1; i < 14; i++) { + r = 1 + aa * PTS[i - 1]; + result += WTS[i - 1] * exp(nhh * r) / r; + } + + result *= a; + + return result; +} + + +static double owensT6(double h, double a) { + double normh, y, r, result; + + normh = owens_t_norm2(h); + y = 1 - a; + r = atan2(y, (1 + a)); + result = normh * (1 - normh) / 2; + + if (r != 0) { + result -= r * exp(-y * h * h / (2 * r)) / (2 * M_PI); + } + + return result; +} + + +static double owens_t_dispatch(double h, double a, double ah) { + int index, meth_code; + double m, result; + + if (h == 0) { + return atan(a) / (2 * M_PI); + } + if (a == 0) { + return 0; + } + if (a == 1) { + return owens_t_norm2(-h) * owens_t_norm2(h) / 2; + } + + index = get_method(h, a); + m = ORD[index]; + meth_code = METHODS[index]; + + switch(meth_code) { + case 1: + result = owensT1(h, a, m); + break; + case 2: + result = owensT2(h, a, ah, m); + break; + case 3: + result = owensT3(h, a, ah); + break; + case 4: + result = owensT4(h, a, m); + break; + case 5: + result = owensT5(h, a); + break; + case 6: + result = owensT6(h, a); + break; + default: + result = NAN; + } + + return result; +} + + +double owens_t(double h, double a) { + double result, fabs_a, fabs_ah, normh, normah; + + if (cephes_isnan(h) || cephes_isnan(a)) { + return NAN; + } + + /* exploit that T(-h,a) == T(h,a) */ + h = fabs(h); + + /* + * Use equation (2) in the paper to remap the arguments such that + * h >= 0 and 0 <= a <= 1 for the call of the actual computation + * routine. + */ + fabs_a = fabs(a); + fabs_ah = fabs_a * h; + + if (fabs_a == INFINITY) { + /* See page 13 in the paper */ + result = 0.5 * owens_t_norm2(h); + } + else if (h == INFINITY) { + result = 0; + } + else if (fabs_a <= 1) { + result = owens_t_dispatch(h, fabs_a, fabs_ah); + } + else { + if (fabs_ah <= 0.67) { + normh = owens_t_norm1(h); + normah = owens_t_norm1(fabs_ah); + result = 0.25 - normh * normah - + owens_t_dispatch(fabs_ah, (1 / fabs_a), h); + } + else { + normh = owens_t_norm2(h); + normah = owens_t_norm2(fabs_ah); + result = (normh + normah) / 2 - normh * normah - + owens_t_dispatch(fabs_ah, (1 / fabs_a), h); + } + } + + if (a < 0) { + /* exploit that T(h,-a) == -T(h,a) */ + return -result; + } + + return result; +} diff --git a/gtsam/3rdparty/cephes/cephes/pdtr.c b/gtsam/3rdparty/cephes/cephes/pdtr.c new file mode 100644 index 0000000000..0249074d98 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/pdtr.c @@ -0,0 +1,173 @@ +/* pdtr.c + * + * Poisson distribution + * + * + * + * SYNOPSIS: + * + * int k; + * double m, y, pdtr(); + * + * y = pdtr( k, m ); + * + * + * + * DESCRIPTION: + * + * Returns the sum of the first k terms of the Poisson + * distribution: + * + * k j + * -- -m m + * > e -- + * -- j! + * j=0 + * + * The terms are not summed directly; instead the incomplete + * Gamma integral is employed, according to the relation + * + * y = pdtr( k, m ) = igamc( k+1, m ). + * + * The arguments must both be nonnegative. + * + * + * + * ACCURACY: + * + * See igamc(). + * + */ +/* pdtrc() + * + * Complemented poisson distribution + * + * + * + * SYNOPSIS: + * + * int k; + * double m, y, pdtrc(); + * + * y = pdtrc( k, m ); + * + * + * + * DESCRIPTION: + * + * Returns the sum of the terms k+1 to infinity of the Poisson + * distribution: + * + * inf. j + * -- -m m + * > e -- + * -- j! + * j=k+1 + * + * The terms are not summed directly; instead the incomplete + * Gamma integral is employed, according to the formula + * + * y = pdtrc( k, m ) = igam( k+1, m ). + * + * The arguments must both be nonnegative. + * + * + * + * ACCURACY: + * + * See igam.c. + * + */ +/* pdtri() + * + * Inverse Poisson distribution + * + * + * + * SYNOPSIS: + * + * int k; + * double m, y, pdtr(); + * + * m = pdtri( k, y ); + * + * + * + * + * DESCRIPTION: + * + * Finds the Poisson variable x such that the integral + * from 0 to x of the Poisson density is equal to the + * given probability y. + * + * This is accomplished using the inverse Gamma integral + * function and the relation + * + * m = igamci( k+1, y ). + * + * + * + * + * ACCURACY: + * + * See igami.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * pdtri domain y < 0 or y >= 1 0.0 + * k < 0 + * + */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +double pdtrc(double k, double m) +{ + double v; + + if (k < 0.0 || m < 0.0) { + sf_error("pdtrc", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (m == 0.0) { + return 0.0; + } + v = floor(k) + 1; + return (igam(v, m)); +} + + +double pdtr(double k, double m) +{ + double v; + + if (k < 0 || m < 0) { + sf_error("pdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (m == 0.0) { + return 1.0; + } + v = floor(k) + 1; + return (igamc(v, m)); +} + + +double pdtri(int k, double y) +{ + double v; + + if ((k < 0) || (y < 0.0) || (y >= 1.0)) { + sf_error("pdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + v = k + 1; + v = igamci(v, y); + return (v); +} diff --git a/gtsam/3rdparty/cephes/cephes/poch.c b/gtsam/3rdparty/cephes/cephes/poch.c new file mode 100644 index 0000000000..4c04fa14eb --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/poch.c @@ -0,0 +1,81 @@ +/* + * Pochhammer symbol (a)_m = gamma(a + m) / gamma(a) + */ +#include "mconf.h" + +static double is_nonpos_int(double x) +{ + return x <= 0 && x == ceil(x) && fabs(x) < 1e13; +} + +double poch(double a, double m) +{ + double r; + + r = 1.0; + + /* + * 1. Reduce magnitude of `m` to |m| < 1 by using recurrence relations. + * + * This may end up in over/underflow, but then the function itself either + * diverges or goes to zero. In case the remainder goes to the opposite + * direction, we end up returning 0*INF = NAN, which is OK. + */ + + /* Recurse down */ + while (m >= 1.0) { + if (a + m == 1) { + break; + } + m -= 1.0; + r *= (a + m); + if (!isfinite(r) || r == 0) { + break; + } + } + + /* Recurse up */ + while (m <= -1.0) { + if (a + m == 0) { + break; + } + r /= (a + m); + m += 1.0; + if (!isfinite(r) || r == 0) { + break; + } + } + + /* + * 2. Evaluate function with reduced `m` + * + * Now either `m` is not big, or the `r` product has over/underflown. + * If so, the function itself does similarly. + */ + + if (m == 0) { + /* Easy case */ + return r; + } + else if (a > 1e4 && fabs(m) <= 1) { + /* Avoid loss of precision */ + return r * pow(a, m) * ( + 1 + + m*(m-1)/(2*a) + + m*(m-1)*(m-2)*(3*m-1)/(24*a*a) + + m*m*(m-1)*(m-1)*(m-2)*(m-3)/(48*a*a*a) + ); + } + + /* Check for infinity */ + if (is_nonpos_int(a + m) && !is_nonpos_int(a) && a + m != m) { + return INFINITY; + } + + /* Check for zero */ + if (!is_nonpos_int(a + m) && is_nonpos_int(a)) { + return 0; + } + + return r * exp(lgam(a + m) - lgam(a)) * gammasgn(a + m) * gammasgn(a); +} diff --git a/gtsam/3rdparty/cephes/cephes/polevl.h b/gtsam/3rdparty/cephes/cephes/polevl.h new file mode 100644 index 0000000000..eb23ddf88a --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/polevl.h @@ -0,0 +1,165 @@ +/* polevl.c + * p1evl.c + * + * Evaluate polynomial + * + * + * + * SYNOPSIS: + * + * int N; + * double x, y, coef[N+1], polevl[]; + * + * y = polevl( x, coef, N ); + * + * + * + * DESCRIPTION: + * + * Evaluates polynomial of degree N: + * + * 2 N + * y = C + C x + C x +...+ C x + * 0 1 2 N + * + * Coefficients are stored in reverse order: + * + * coef[0] = C , ..., coef[N] = C . + * N 0 + * + * The function p1evl() assumes that c_N = 1.0 so that coefficent + * is omitted from the array. Its calling arguments are + * otherwise the same as polevl(). + * + * + * SPEED: + * + * In the interest of speed, there are no checks for out + * of bounds arithmetic. This routine is used by most of + * the functions in the library. Depending on available + * equipment features, the user may wish to rewrite the + * program in microcode or assembly language. + * + */ + +/* + * Cephes Math Library Release 2.1: December, 1988 + * Copyright 1984, 1987, 1988 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +/* Sources: + * [1] Holin et. al., "Polynomial and Rational Function Evaluation", + * https://www.boost.org/doc/libs/1_61_0/libs/math/doc/html/math_toolkit/roots/rational.html + */ + +/* Scipy changes: + * - 06-23-2016: add code for evaluating rational functions + */ + +#ifndef CEPHES_POLEV +#define CEPHES_POLEV + +#include + +static inline double polevl(double x, const double coef[], int N) +{ + double ans; + int i; + const double *p; + + p = coef; + ans = *p++; + i = N; + + do + ans = ans * x + *p++; + while (--i); + + return (ans); +} + +/* p1evl() */ +/* N + * Evaluate polynomial when coefficient of x is 1.0. + * That is, C_{N} is assumed to be 1, and that coefficient + * is not included in the input array coef. + * coef must have length N and contain the polynomial coefficients + * stored as + * coef[0] = C_{N-1} + * coef[1] = C_{N-2} + * ... + * coef[N-2] = C_1 + * coef[N-1] = C_0 + * Otherwise same as polevl. + */ + +static inline double p1evl(double x, const double coef[], int N) +{ + double ans; + const double *p; + int i; + + p = coef; + ans = x + *p++; + i = N - 1; + + do + ans = ans * x + *p++; + while (--i); + + return (ans); +} + +/* Evaluate a rational function. See [1]. */ + +static inline double ratevl(double x, const double num[], int M, + const double denom[], int N) +{ + int i, dir; + double y, num_ans, denom_ans; + double absx = fabs(x); + const double *p; + + if (absx > 1) { + /* Evaluate as a polynomial in 1/x. */ + dir = -1; + p = num + M; + y = 1 / x; + } else { + dir = 1; + p = num; + y = x; + } + + /* Evaluate the numerator */ + num_ans = *p; + p += dir; + for (i = 1; i <= M; i++) { + num_ans = num_ans * y + *p; + p += dir; + } + + /* Evaluate the denominator */ + if (absx > 1) { + p = denom + N; + } else { + p = denom; + } + + denom_ans = *p; + p += dir; + for (i = 1; i <= N; i++) { + denom_ans = denom_ans * y + *p; + p += dir; + } + + if (absx > 1) { + i = N - M; + return pow(x, i) * num_ans / denom_ans; + } else { + return num_ans / denom_ans; + } +} + +#endif diff --git a/gtsam/3rdparty/cephes/cephes/psi.c b/gtsam/3rdparty/cephes/cephes/psi.c new file mode 100644 index 0000000000..190c6d1628 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/psi.c @@ -0,0 +1,205 @@ +/* psi.c + * + * Psi (digamma) function + * + * + * SYNOPSIS: + * + * double x, y, psi(); + * + * y = psi( x ); + * + * + * DESCRIPTION: + * + * d - + * psi(x) = -- ln | (x) + * dx + * + * is the logarithmic derivative of the gamma function. + * For integer x, + * n-1 + * - + * psi(n) = -EUL + > 1/k. + * - + * k=1 + * + * This formula is used for 0 < n <= 10. If x is negative, it + * is transformed to a positive argument by the reflection + * formula psi(1-x) = psi(x) + pi cot(pi x). + * For general positive x, the argument is made greater than 10 + * using the recurrence psi(x+1) = psi(x) + 1/x. + * Then the following asymptotic expansion is applied: + * + * inf. B + * - 2k + * psi(x) = log(x) - 1/2x - > ------- + * - 2k + * k=1 2k x + * + * where the B2k are Bernoulli numbers. + * + * ACCURACY: + * Relative error (except absolute when |psi| < 1): + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 1.3e-15 1.4e-16 + * IEEE -30,0 40000 1.5e-15 2.2e-16 + * + * ERROR MESSAGES: + * message condition value returned + * psi singularity x integer <=0 INFINITY + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1992, 2000 by Stephen L. Moshier + */ + +/* + * Code for the rational approximation on [1, 2] is: + * + * (C) Copyright John Maddock 2006. + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ + +#include "mconf.h" + +static double A[] = { + 8.33333333333333333333E-2, + -2.10927960927960927961E-2, + 7.57575757575757575758E-3, + -4.16666666666666666667E-3, + 3.96825396825396825397E-3, + -8.33333333333333333333E-3, + 8.33333333333333333333E-2 +}; + + +static double digamma_imp_1_2(double x) +{ + /* + * Rational approximation on [1, 2] taken from Boost. + * + * Now for the approximation, we use the form: + * + * digamma(x) = (x - root) * (Y + R(x-1)) + * + * Where root is the location of the positive root of digamma, + * Y is a constant, and R is optimised for low absolute error + * compared to Y. + * + * Maximum Deviation Found: 1.466e-18 + * At double precision, max error found: 2.452e-17 + */ + double r, g; + + static const float Y = 0.99558162689208984f; + + static const double root1 = 1569415565.0 / 1073741824.0; + static const double root2 = (381566830.0 / 1073741824.0) / 1073741824.0; + static const double root3 = 0.9016312093258695918615325266959189453125e-19; + + static double P[] = { + -0.0020713321167745952, + -0.045251321448739056, + -0.28919126444774784, + -0.65031853770896507, + -0.32555031186804491, + 0.25479851061131551 + }; + static double Q[] = { + -0.55789841321675513e-6, + 0.0021284987017821144, + 0.054151797245674225, + 0.43593529692665969, + 1.4606242909763515, + 2.0767117023730469, + 1.0 + }; + g = x - root1; + g -= root2; + g -= root3; + r = polevl(x - 1.0, P, 5) / polevl(x - 1.0, Q, 6); + + return g * Y + g * r; +} + + +static double psi_asy(double x) +{ + double y, z; + + if (x < 1.0e17) { + z = 1.0 / (x * x); + y = z * polevl(z, A, 6); + } + else { + y = 0.0; + } + + return log(x) - (0.5 / x) - y; +} + + +double psi(double x) +{ + double y = 0.0; + double q, r; + int i, n; + + if (isnan(x)) { + return x; + } + else if (x == INFINITY) { + return x; + } + else if (x == -INFINITY) { + return NAN; + } + else if (x == 0) { + sf_error("psi", SF_ERROR_SINGULAR, NULL); + return copysign(INFINITY, -x); + } + else if (x < 0.0) { + /* argument reduction before evaluating tan(pi * x) */ + r = modf(x, &q); + if (r == 0.0) { + sf_error("psi", SF_ERROR_SINGULAR, NULL); + return NAN; + } + y = -M_PI / tan(M_PI * r); + x = 1.0 - x; + } + + /* check for positive integer up to 10 */ + if ((x <= 10.0) && (x == floor(x))) { + n = (int)x; + for (i = 1; i < n; i++) { + y += 1.0 / i; + } + y -= SCIPY_EULER; + return y; + } + + /* use the recurrence relation to move x into [1, 2] */ + if (x < 1.0) { + y -= 1.0 / x; + x += 1.0; + } + else if (x < 10.0) { + while (x > 2.0) { + x -= 1.0; + y += 1.0 / x; + } + } + if ((1.0 <= x) && (x <= 2.0)) { + y += digamma_imp_1_2(x); + return y; + } + + /* x is large, use the asymptotic series */ + y += psi_asy(x); + return y; +} diff --git a/gtsam/3rdparty/cephes/cephes/rgamma.c b/gtsam/3rdparty/cephes/cephes/rgamma.c new file mode 100644 index 0000000000..6420ccaa94 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/rgamma.c @@ -0,0 +1,128 @@ +/* rgamma.c + * + * Reciprocal Gamma function + * + * + * + * SYNOPSIS: + * + * double x, y, rgamma(); + * + * y = rgamma( x ); + * + * + * + * DESCRIPTION: + * + * Returns one divided by the Gamma function of the argument. + * + * The function is approximated by a Chebyshev expansion in + * the interval [0,1]. Range reduction is by recurrence + * for arguments between -34.034 and +34.84425627277176174. + * 0 is returned for positive arguments outside this + * range. For arguments less than -34.034 the cosecant + * reflection formula is applied; lograrithms are employed + * to avoid unnecessary overflow. + * + * The reciprocal Gamma function has no singularities, + * but overflow and underflow may occur for large arguments. + * These conditions return either INFINITY or 0 with + * appropriate sign. + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -30,+30 30000 1.1e-15 2.0e-16 + * For arguments less than -34.034 the peak error is on the + * order of 5e-15 (DEC), excepting overflow or underflow. + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1985, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +/* Chebyshev coefficients for reciprocal Gamma function + * in interval 0 to 1. Function is 1/(x Gamma(x)) - 1 + */ + +static double R[] = { + 3.13173458231230000000E-17, + -6.70718606477908000000E-16, + 2.20039078172259550000E-15, + 2.47691630348254132600E-13, + -6.60074100411295197440E-12, + 5.13850186324226978840E-11, + 1.08965386454418662084E-9, + -3.33964630686836942556E-8, + 2.68975996440595483619E-7, + 2.96001177518801696639E-6, + -8.04814124978471142852E-5, + 4.16609138709688864714E-4, + 5.06579864028608725080E-3, + -6.41925436109158228810E-2, + -4.98558728684003594785E-3, + 1.27546015610523951063E-1 +}; + +static char name[] = "rgamma"; + +extern double MAXLOG; + + +double rgamma(double x) +{ + double w, y, z; + int sign; + + if (x > 34.84425627277176174) { + return exp(-lgam(x)); + } + if (x < -34.034) { + w = -x; + z = sinpi(w); + if (z == 0.0) { + return 0.0; + } + if (z < 0.0) { + sign = 1; + z = -z; + } + else { + sign = -1; + } + + y = log(w * z) - log(M_PI) + lgam(w); + if (y < -MAXLOG) { + sf_error(name, SF_ERROR_UNDERFLOW, NULL); + return (sign * 0.0); + } + if (y > MAXLOG) { + sf_error(name, SF_ERROR_OVERFLOW, NULL); + return (sign * INFINITY); + } + return (sign * exp(y)); + } + z = 1.0; + w = x; + + while (w > 1.0) { /* Downward recurrence */ + w -= 1.0; + z *= w; + } + while (w < 0.0) { /* Upward recurrence */ + z /= w; + w += 1.0; + } + if (w == 0.0) /* Nonpositive integer */ + return (0.0); + if (w == 1.0) /* Other integer */ + return (1.0 / z); + + y = w * (1.0 + chbevl(4.0 * w - 2.0, R, 16)) / z; + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/round.c b/gtsam/3rdparty/cephes/cephes/round.c new file mode 100644 index 0000000000..0ed1f1415b --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/round.c @@ -0,0 +1,63 @@ +/* round.c + * + * Round double to nearest or even integer valued double + * + * + * + * SYNOPSIS: + * + * double x, y, round(); + * + * y = round(x); + * + * + * + * DESCRIPTION: + * + * Returns the nearest integer to x as a double precision + * floating point result. If x ends in 0.5 exactly, the + * nearest even integer is chosen. + * + * + * + * ACCURACY: + * + * If x is greater than 1/(2*MACHEP), its closest machine + * representation is already an integer, so rounding does + * not change it. + */ + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +double round(double x) +{ + double y, r; + + /* Largest integer <= x */ + y = floor(x); + + /* Fractional part */ + r = x - y; + + /* Round up to nearest. */ + if (r > 0.5) + goto rndup; + + /* Round to even */ + if (r == 0.5) { + r = y - 2.0 * floor(0.5 * y); + if (r == 1.0) { + rndup: + y += 1.0; + } + } + + /* Else round down. */ + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/scipy_iv.c b/gtsam/3rdparty/cephes/cephes/scipy_iv.c new file mode 100644 index 0000000000..e7bb220119 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/scipy_iv.c @@ -0,0 +1,654 @@ +/* iv.c + * + * Modified Bessel function of noninteger order + * + * + * + * SYNOPSIS: + * + * double v, x, y, iv(); + * + * y = iv( v, x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of order v of the + * argument. If x is negative, v must be integer valued. + * + */ +/* iv.c */ +/* Modified Bessel function of noninteger order */ +/* If x < 0, then v must be an integer. */ + + +/* + * Parts of the code are copyright: + * + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1988, 2000 by Stephen L. Moshier + * + * And other parts: + * + * Copyright (c) 2006 Xiaogang Zhang + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. + * + * Boost Software License - Version 1.0 - August 17th, 2003 + * + * Permission is hereby granted, free of charge, to any person or + * organization obtaining a copy of the software and accompanying + * documentation covered by this license (the "Software") to use, reproduce, + * display, distribute, execute, and transmit the Software, and to prepare + * derivative works of the Software, and to permit third-parties to whom the + * Software is furnished to do so, all subject to the following: + * + * The copyright notices in the Software and this entire statement, + * including the above license grant, this restriction and the following + * disclaimer, must be included in all copies of the Software, in whole or + * in part, and all derivative works of the Software, unless such copies or + * derivative works are solely in the form of machine-executable object code + * generated by a source language processor. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE AND + * NON-INFRINGEMENT. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR ANYONE + * DISTRIBUTING THE SOFTWARE BE LIABLE FOR ANY DAMAGES OR OTHER LIABILITY, + * WHETHER IN CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * And the rest are: + * + * Copyright (C) 2009 Pauli Virtanen + * Distributed under the same license as Scipy. + * + */ + +#include "mconf.h" +#include +#include + +extern double MACHEP; + +static double iv_asymptotic(double v, double x); +static void ikv_asymptotic_uniform(double v, double x, double *Iv, double *Kv); +static void ikv_temme(double v, double x, double *Iv, double *Kv); + +double iv(double v, double x) +{ + int sign; + double t, ax, res; + + if (isnan(v) || isnan(x)) { + return NAN; + } + + /* If v is a negative integer, invoke symmetry */ + t = floor(v); + if (v < 0.0) { + if (t == v) { + v = -v; /* symmetry */ + t = -t; + } + } + /* If x is negative, require v to be an integer */ + sign = 1; + if (x < 0.0) { + if (t != v) { + sf_error("iv", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (v != 2.0 * floor(v / 2.0)) { + sign = -1; + } + } + + /* Avoid logarithm singularity */ + if (x == 0.0) { + if (v == 0.0) { + return 1.0; + } + if (v < 0.0) { + sf_error("iv", SF_ERROR_OVERFLOW, NULL); + return INFINITY; + } + else + return 0.0; + } + + ax = fabs(x); + if (fabs(v) > 50) { + /* + * Uniform asymptotic expansion for large orders. + * + * This appears to overflow slightly later than the Boost + * implementation of Temme's method. + */ + ikv_asymptotic_uniform(v, ax, &res, NULL); + } + else { + /* Otherwise: Temme's method */ + ikv_temme(v, ax, &res, NULL); + } + res *= sign; + return res; +} + + +/* + * Compute Iv from (AMS5 9.7.1), asymptotic expansion for large |z| + * Iv ~ exp(x)/sqrt(2 pi x) ( 1 + (4*v*v-1)/8x + (4*v*v-1)(4*v*v-9)/8x/2! + ...) + */ +static double iv_asymptotic(double v, double x) +{ + double mu; + double sum, term, prefactor, factor; + int k; + + prefactor = exp(x) / sqrt(2 * M_PI * x); + + if (prefactor == INFINITY) { + return prefactor; + } + + mu = 4 * v * v; + sum = 1.0; + term = 1.0; + k = 1; + + do { + factor = (mu - (2 * k - 1) * (2 * k - 1)) / (8 * x) / k; + if (k > 100) { + /* didn't converge */ + sf_error("iv(iv_asymptotic)", SF_ERROR_NO_RESULT, NULL); + break; + } + term *= -factor; + sum += term; + ++k; + } while (fabs(term) > MACHEP * fabs(sum)); + return sum * prefactor; +} + + +/* + * Uniform asymptotic expansion factors, (AMS5 9.3.9; AMS5 9.3.10) + * + * Computed with: + * -------------------- + import numpy as np + t = np.poly1d([1,0]) + def up1(p): + return .5*t*t*(1-t*t)*p.deriv() + 1/8. * ((1-5*t*t)*p).integ() + us = [np.poly1d([1])] + for k in range(10): + us.append(up1(us[-1])) + n = us[-1].order + for p in us: + print "{" + ", ".join(["0"]*(n-p.order) + map(repr, p)) + "}," + print "N_UFACTORS", len(us) + print "N_UFACTOR_TERMS", us[-1].order + 1 + * -------------------- + */ +#define N_UFACTORS 11 +#define N_UFACTOR_TERMS 31 +static const double asymptotic_ufactors[N_UFACTORS][N_UFACTOR_TERMS] = { + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, -0.20833333333333334, 0.0, 0.125, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.3342013888888889, 0.0, -0.40104166666666669, 0.0, 0.0703125, 0.0, + 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + -1.0258125964506173, 0.0, 1.8464626736111112, 0.0, + -0.89121093750000002, 0.0, 0.0732421875, 0.0, 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 4.6695844234262474, 0.0, -11.207002616222995, 0.0, 8.78912353515625, + 0.0, -2.3640869140624998, 0.0, 0.112152099609375, 0.0, 0.0, 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -28.212072558200244, 0.0, + 84.636217674600744, 0.0, -91.818241543240035, 0.0, 42.534998745388457, + 0.0, -7.3687943594796312, 0.0, 0.22710800170898438, 0.0, 0.0, 0.0, + 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 212.5701300392171, 0.0, + -765.25246814118157, 0.0, 1059.9904525279999, 0.0, + -699.57962737613275, 0.0, 218.19051174421159, 0.0, + -26.491430486951554, 0.0, 0.57250142097473145, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, -1919.4576623184068, 0.0, + 8061.7221817373083, 0.0, -13586.550006434136, 0.0, 11655.393336864536, + 0.0, -5305.6469786134048, 0.0, 1200.9029132163525, 0.0, + -108.09091978839464, 0.0, 1.7277275025844574, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 20204.291330966149, 0.0, -96980.598388637503, 0.0, + 192547.0012325315, 0.0, -203400.17728041555, 0.0, 122200.46498301747, + 0.0, -41192.654968897557, 0.0, 7109.5143024893641, 0.0, + -493.915304773088, 0.0, 6.074042001273483, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0}, + {0, 0, 0, -242919.18790055133, 0.0, 1311763.6146629769, 0.0, + -2998015.9185381061, 0.0, 3763271.2976564039, 0.0, + -2813563.2265865342, 0.0, 1268365.2733216248, 0.0, + -331645.17248456361, 0.0, 45218.768981362737, 0.0, + -2499.8304818112092, 0.0, 24.380529699556064, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0}, + {3284469.8530720375, 0.0, -19706819.11843222, 0.0, 50952602.492664628, + 0.0, -74105148.211532637, 0.0, 66344512.274729028, 0.0, + -37567176.660763353, 0.0, 13288767.166421819, 0.0, + -2785618.1280864552, 0.0, 308186.40461266245, 0.0, + -13886.089753717039, 0.0, 110.01714026924674, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0} +}; + + +/* + * Compute Iv, Kv from (AMS5 9.7.7 + 9.7.8), asymptotic expansion for large v + */ +static void ikv_asymptotic_uniform(double v, double x, + double *i_value, double *k_value) +{ + double i_prefactor, k_prefactor; + double t, t2, eta, z; + double i_sum, k_sum, term, divisor; + int k, n; + int sign = 1; + + if (v < 0) { + /* Negative v; compute I_{-v} and K_{-v} and use (AMS 9.6.2) */ + sign = -1; + v = -v; + } + + z = x / v; + t = 1 / sqrt(1 + z * z); + t2 = t * t; + eta = sqrt(1 + z * z) + log(z / (1 + 1 / t)); + + i_prefactor = sqrt(t / (2 * M_PI * v)) * exp(v * eta); + i_sum = 1.0; + + k_prefactor = sqrt(M_PI * t / (2 * v)) * exp(-v * eta); + k_sum = 1.0; + + divisor = v; + for (n = 1; n < N_UFACTORS; ++n) { + /* + * Evaluate u_k(t) with Horner's scheme; + * (using the knowledge about which coefficients are zero) + */ + term = 0; + for (k = N_UFACTOR_TERMS - 1 - 3 * n; + k < N_UFACTOR_TERMS - n; k += 2) { + term *= t2; + term += asymptotic_ufactors[n][k]; + } + for (k = 1; k < n; k += 2) { + term *= t2; + } + if (n % 2 == 1) { + term *= t; + } + + /* Sum terms */ + term /= divisor; + i_sum += term; + k_sum += (n % 2 == 0) ? term : -term; + + /* Check convergence */ + if (fabs(term) < MACHEP) { + break; + } + + divisor *= v; + } + + if (fabs(term) > 1e-3 * fabs(i_sum)) { + /* Didn't converge */ + sf_error("ikv_asymptotic_uniform", SF_ERROR_NO_RESULT, NULL); + } + if (fabs(term) > MACHEP * fabs(i_sum)) { + /* Some precision lost */ + sf_error("ikv_asymptotic_uniform", SF_ERROR_LOSS, NULL); + } + + if (k_value != NULL) { + /* symmetric in v */ + *k_value = k_prefactor * k_sum; + } + + if (i_value != NULL) { + if (sign == 1) { + *i_value = i_prefactor * i_sum; + } + else { + /* (AMS 9.6.2) */ + *i_value = (i_prefactor * i_sum + + (2 / M_PI) * sin(M_PI * v) * k_prefactor * k_sum); + } + } +} + + +/* + * The following code originates from the Boost C++ library, + * from file `boost/math/special_functions/detail/bessel_ik.hpp`, + * converted from C++ to C. + */ + +#ifdef DEBUG +#define BOOST_ASSERT(a) assert(a) +#else +#define BOOST_ASSERT(a) +#endif + +/* + * Modified Bessel functions of the first and second kind of fractional order + * + * Calculate K(v, x) and K(v+1, x) by method analogous to + * Temme, Journal of Computational Physics, vol 21, 343 (1976) + */ +static int temme_ik_series(double v, double x, double *K, double *K1) +{ + double f, h, p, q, coef, sum, sum1, tolerance; + double a, b, c, d, sigma, gamma1, gamma2; + unsigned long k; + double gp; + double gm; + + + /* + * |x| <= 2, Temme series converge rapidly + * |x| > 2, the larger the |x|, the slower the convergence + */ + BOOST_ASSERT(fabs(x) <= 2); + BOOST_ASSERT(fabs(v) <= 0.5f); + + gp = gamma(v + 1) - 1; + gm = gamma(-v + 1) - 1; + + a = log(x / 2); + b = exp(v * a); + sigma = -a * v; + c = fabs(v) < MACHEP ? 1 : sin(M_PI * v) / (v * M_PI); + d = fabs(sigma) < MACHEP ? 1 : sinh(sigma) / sigma; + gamma1 = fabs(v) < MACHEP ? -SCIPY_EULER : (0.5f / v) * (gp - gm) * c; + gamma2 = (2 + gp + gm) * c / 2; + + /* initial values */ + p = (gp + 1) / (2 * b); + q = (1 + gm) * b / 2; + f = (cosh(sigma) * gamma1 + d * (-a) * gamma2) / c; + h = p; + coef = 1; + sum = coef * f; + sum1 = coef * h; + + /* series summation */ + tolerance = MACHEP; + for (k = 1; k < MAXITER; k++) { + f = (k * f + p + q) / (k * k - v * v); + p /= k - v; + q /= k + v; + h = p - k * f; + coef *= x * x / (4 * k); + sum += coef * f; + sum1 += coef * h; + if (fabs(coef * f) < fabs(sum) * tolerance) { + break; + } + } + if (k == MAXITER) { + sf_error("ikv_temme(temme_ik_series)", SF_ERROR_NO_RESULT, NULL); + } + + *K = sum; + *K1 = 2 * sum1 / x; + + return 0; +} + +/* Evaluate continued fraction fv = I_(v+1) / I_v, derived from + * Abramowitz and Stegun, Handbook of Mathematical Functions, 1972, 9.1.73 */ +static int CF1_ik(double v, double x, double *fv) +{ + double C, D, f, a, b, delta, tiny, tolerance; + unsigned long k; + + + /* + * |x| <= |v|, CF1_ik converges rapidly + * |x| > |v|, CF1_ik needs O(|x|) iterations to converge + */ + + /* + * modified Lentz's method, see + * Lentz, Applied Optics, vol 15, 668 (1976) + */ + tolerance = 2 * MACHEP; + tiny = 1 / sqrt(DBL_MAX); + C = f = tiny; /* b0 = 0, replace with tiny */ + D = 0; + for (k = 1; k < MAXITER; k++) { + a = 1; + b = 2 * (v + k) / x; + C = b + a / C; + D = b + a * D; + if (C == 0) { + C = tiny; + } + if (D == 0) { + D = tiny; + } + D = 1 / D; + delta = C * D; + f *= delta; + if (fabs(delta - 1) <= tolerance) { + break; + } + } + if (k == MAXITER) { + sf_error("ikv_temme(CF1_ik)", SF_ERROR_NO_RESULT, NULL); + } + + *fv = f; + + return 0; +} + +/* + * Calculate K(v, x) and K(v+1, x) by evaluating continued fraction + * z1 / z0 = U(v+1.5, 2v+1, 2x) / U(v+0.5, 2v+1, 2x), see + * Thompson and Barnett, Computer Physics Communications, vol 47, 245 (1987) + */ +static int CF2_ik(double v, double x, double *Kv, double *Kv1) +{ + + double S, C, Q, D, f, a, b, q, delta, tolerance, current, prev; + unsigned long k; + + /* + * |x| >= |v|, CF2_ik converges rapidly + * |x| -> 0, CF2_ik fails to converge + */ + + BOOST_ASSERT(fabs(x) > 1); + + /* + * Steed's algorithm, see Thompson and Barnett, + * Journal of Computational Physics, vol 64, 490 (1986) + */ + tolerance = MACHEP; + a = v * v - 0.25f; + b = 2 * (x + 1); /* b1 */ + D = 1 / b; /* D1 = 1 / b1 */ + f = delta = D; /* f1 = delta1 = D1, coincidence */ + prev = 0; /* q0 */ + current = 1; /* q1 */ + Q = C = -a; /* Q1 = C1 because q1 = 1 */ + S = 1 + Q * delta; /* S1 */ + for (k = 2; k < MAXITER; k++) { /* starting from 2 */ + /* continued fraction f = z1 / z0 */ + a -= 2 * (k - 1); + b += 2; + D = 1 / (b + a * D); + delta *= b * D - 1; + f += delta; + + /* series summation S = 1 + \sum_{n=1}^{\infty} C_n * z_n / z_0 */ + q = (prev - (b - 2) * current) / a; + prev = current; + current = q; /* forward recurrence for q */ + C *= -a / k; + Q += C * q; + S += Q * delta; + + /* S converges slower than f */ + if (fabs(Q * delta) < fabs(S) * tolerance) { + break; + } + } + if (k == MAXITER) { + sf_error("ikv_temme(CF2_ik)", SF_ERROR_NO_RESULT, NULL); + } + + *Kv = sqrt(M_PI / (2 * x)) * exp(-x) / S; + *Kv1 = *Kv * (0.5f + v + x + (v * v - 0.25f) * f) / x; + + return 0; +} + +/* Flags for what to compute */ +enum { + need_i = 0x1, + need_k = 0x2 +}; + +/* + * Compute I(v, x) and K(v, x) simultaneously by Temme's method, see + * Temme, Journal of Computational Physics, vol 19, 324 (1975) + */ +static void ikv_temme(double v, double x, double *Iv_p, double *Kv_p) +{ + /* Kv1 = K_(v+1), fv = I_(v+1) / I_v */ + /* Ku1 = K_(u+1), fu = I_(u+1) / I_u */ + double u, Iv, Kv, Kv1, Ku, Ku1, fv; + double W, current, prev, next; + int reflect = 0; + unsigned n, k; + int kind; + + kind = 0; + if (Iv_p != NULL) { + kind |= need_i; + } + if (Kv_p != NULL) { + kind |= need_k; + } + + if (v < 0) { + reflect = 1; + v = -v; /* v is non-negative from here */ + kind |= need_k; + } + n = round(v); + u = v - n; /* -1/2 <= u < 1/2 */ + + if (x < 0) { + if (Iv_p != NULL) + *Iv_p = NAN; + if (Kv_p != NULL) + *Kv_p = NAN; + sf_error("ikv_temme", SF_ERROR_DOMAIN, NULL); + return; + } + if (x == 0) { + Iv = (v == 0) ? 1 : 0; + if (kind & need_k) { + sf_error("ikv_temme", SF_ERROR_OVERFLOW, NULL); + Kv = INFINITY; + } + else { + Kv = NAN; /* any value will do */ + } + + if (reflect && (kind & need_i)) { + double z = (u + n % 2); + + Iv = sin((double)M_PI * z) == 0 ? Iv : INFINITY; + if (Iv == INFINITY || Iv == -INFINITY) { + sf_error("ikv_temme", SF_ERROR_OVERFLOW, NULL); + } + } + + if (Iv_p != NULL) { + *Iv_p = Iv; + } + if (Kv_p != NULL) { + *Kv_p = Kv; + } + return; + } + /* x is positive until reflection */ + W = 1 / x; /* Wronskian */ + if (x <= 2) { /* x in (0, 2] */ + temme_ik_series(u, x, &Ku, &Ku1); /* Temme series */ + } + else { /* x in (2, \infty) */ + CF2_ik(u, x, &Ku, &Ku1); /* continued fraction CF2_ik */ + } + prev = Ku; + current = Ku1; + for (k = 1; k <= n; k++) { /* forward recurrence for K */ + next = 2 * (u + k) * current / x + prev; + prev = current; + current = next; + } + Kv = prev; + Kv1 = current; + if (kind & need_i) { + double lim = (4 * v * v + 10) / (8 * x); + + lim *= lim; + lim *= lim; + lim /= 24; + if ((lim < MACHEP * 10) && (x > 100)) { + /* + * x is huge compared to v, CF1 may be very slow + * to converge so use asymptotic expansion for large + * x case instead. Note that the asymptotic expansion + * isn't very accurate - so it's deliberately very hard + * to get here - probably we're going to overflow: + */ + Iv = iv_asymptotic(v, x); + } + else { + CF1_ik(v, x, &fv); /* continued fraction CF1_ik */ + Iv = W / (Kv * fv + Kv1); /* Wronskian relation */ + } + } + else { + Iv = NAN; /* any value will do */ + } + + if (reflect) { + double z = (u + n % 2); + + if (Iv_p != NULL) { + *Iv_p = Iv + (2 / M_PI) * sin(M_PI * z) * Kv; /* reflection formula */ + } + if (Kv_p != NULL) { + *Kv_p = Kv; + } + } + else { + if (Iv_p != NULL) { + *Iv_p = Iv; + } + if (Kv_p != NULL) { + *Kv_p = Kv; + } + } + return; +} diff --git a/gtsam/3rdparty/cephes/cephes/sf_error.c b/gtsam/3rdparty/cephes/cephes/sf_error.c new file mode 100644 index 0000000000..95a47c797b --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sf_error.c @@ -0,0 +1,45 @@ +#include "sf_error.h" + +#include +#include + +const char *sf_error_messages[] = {"no error", + "singularity", + "underflow", + "overflow", + "too slow convergence", + "loss of precision", + "no result obtained", + "domain error", + "invalid input argument", + "other error", + NULL}; + +/* If this isn't volatile clang tries to optimize it away */ +static volatile sf_action_t sf_error_actions[] = { + SF_ERROR_IGNORE, /* SF_ERROR_OK */ + SF_ERROR_IGNORE, /* SF_ERROR_SINGULAR */ + SF_ERROR_IGNORE, /* SF_ERROR_UNDERFLOW */ + SF_ERROR_IGNORE, /* SF_ERROR_OVERFLOW */ + SF_ERROR_IGNORE, /* SF_ERROR_SLOW */ + SF_ERROR_IGNORE, /* SF_ERROR_LOSS */ + SF_ERROR_IGNORE, /* SF_ERROR_NO_RESULT */ + SF_ERROR_IGNORE, /* SF_ERROR_DOMAIN */ + SF_ERROR_IGNORE, /* SF_ERROR_ARG */ + SF_ERROR_IGNORE, /* SF_ERROR_OTHER */ + SF_ERROR_IGNORE /* SF_ERROR__LAST */ +}; + +void sf_error_set_action(sf_error_t code, sf_action_t action) { + sf_error_actions[(int)code] = action; +} + +sf_action_t sf_error_get_action(sf_error_t code) { + return sf_error_actions[(int)code]; +} + +void sf_error(const char *func_name, sf_error_t code, const char *fmt, ...) { + va_list ap; + va_start(ap, fmt); + va_end(ap); +} diff --git a/gtsam/3rdparty/cephes/cephes/sf_error.h b/gtsam/3rdparty/cephes/cephes/sf_error.h new file mode 100644 index 0000000000..43986df812 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sf_error.h @@ -0,0 +1,38 @@ +#ifndef SF_ERROR_H_ +#define SF_ERROR_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { + SF_ERROR_OK = 0, /* no error */ + SF_ERROR_SINGULAR, /* singularity encountered */ + SF_ERROR_UNDERFLOW, /* floating point underflow */ + SF_ERROR_OVERFLOW, /* floating point overflow */ + SF_ERROR_SLOW, /* too many iterations required */ + SF_ERROR_LOSS, /* loss of precision */ + SF_ERROR_NO_RESULT, /* no result obtained */ + SF_ERROR_DOMAIN, /* out of domain */ + SF_ERROR_ARG, /* invalid input parameter */ + SF_ERROR_OTHER, /* unclassified error */ + SF_ERROR__LAST +} sf_error_t; + +typedef enum { + SF_ERROR_IGNORE = 0, /* Ignore errors */ + SF_ERROR_WARN, /* Warn on errors */ + SF_ERROR_RAISE /* Raise on errors */ +} sf_action_t; + +extern const char *sf_error_messages[]; +void sf_error(const char *func_name, sf_error_t code, const char *fmt, ...); +void sf_error_check_fpe(const char *func_name); +void sf_error_set_action(sf_error_t code, sf_action_t action); +sf_action_t sf_error_get_action(sf_error_t code); + +#ifdef __cplusplus +} +#endif + +#endif /* SF_ERROR_H_ */ diff --git a/gtsam/3rdparty/cephes/cephes/shichi.c b/gtsam/3rdparty/cephes/cephes/shichi.c new file mode 100644 index 0000000000..75104e7247 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/shichi.c @@ -0,0 +1,305 @@ +/* shichi.c + * + * Hyperbolic sine and cosine integrals + * + * + * + * SYNOPSIS: + * + * double x, Chi, Shi, shichi(); + * + * shichi( x, &Chi, &Shi ); + * + * + * DESCRIPTION: + * + * Approximates the integrals + * + * x + * - + * | | cosh t - 1 + * Chi(x) = eul + ln x + | ----------- dt, + * | | t + * - + * 0 + * + * x + * - + * | | sinh t + * Shi(x) = | ------ dt + * | | t + * - + * 0 + * + * where eul = 0.57721566490153286061 is Euler's constant. + * The integrals are evaluated by power series for x < 8 + * and by Chebyshev expansions for x between 8 and 88. + * For large x, both functions approach exp(x)/2x. + * Arguments greater than 88 in magnitude return INFINITY. + * + * + * ACCURACY: + * + * Test interval 0 to 88. + * Relative error: + * arithmetic function # trials peak rms + * IEEE Shi 30000 6.9e-16 1.6e-16 + * Absolute error, except relative when |Chi| > 1: + * IEEE Chi 30000 8.4e-16 1.4e-16 + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + + +#include "mconf.h" + +/* x exp(-x) shi(x), inverted interval 8 to 18 */ +static double S1[] = { + 1.83889230173399459482E-17, + -9.55485532279655569575E-17, + 2.04326105980879882648E-16, + 1.09896949074905343022E-15, + -1.31313534344092599234E-14, + 5.93976226264314278932E-14, + -3.47197010497749154755E-14, + -1.40059764613117131000E-12, + 9.49044626224223543299E-12, + -1.61596181145435454033E-11, + -1.77899784436430310321E-10, + 1.35455469767246947469E-9, + -1.03257121792819495123E-9, + -3.56699611114982536845E-8, + 1.44818877384267342057E-7, + 7.82018215184051295296E-7, + -5.39919118403805073710E-6, + -3.12458202168959833422E-5, + 8.90136741950727517826E-5, + 2.02558474743846862168E-3, + 2.96064440855633256972E-2, + 1.11847751047257036625E0 +}; + +/* x exp(-x) shi(x), inverted interval 18 to 88 */ +static double S2[] = { + -1.05311574154850938805E-17, + 2.62446095596355225821E-17, + 8.82090135625368160657E-17, + -3.38459811878103047136E-16, + -8.30608026366935789136E-16, + 3.93397875437050071776E-15, + 1.01765565969729044505E-14, + -4.21128170307640802703E-14, + -1.60818204519802480035E-13, + 3.34714954175994481761E-13, + 2.72600352129153073807E-12, + 1.66894954752839083608E-12, + -3.49278141024730899554E-11, + -1.58580661666482709598E-10, + -1.79289437183355633342E-10, + 1.76281629144264523277E-9, + 1.69050228879421288846E-8, + 1.25391771228487041649E-7, + 1.16229947068677338732E-6, + 1.61038260117376323993E-5, + 3.49810375601053973070E-4, + 1.28478065259647610779E-2, + 1.03665722588798326712E0 +}; + +/* x exp(-x) chin(x), inverted interval 8 to 18 */ +static double C1[] = { + -8.12435385225864036372E-18, + 2.17586413290339214377E-17, + 5.22624394924072204667E-17, + -9.48812110591690559363E-16, + 5.35546311647465209166E-15, + -1.21009970113732918701E-14, + -6.00865178553447437951E-14, + 7.16339649156028587775E-13, + -2.93496072607599856104E-12, + -1.40359438136491256904E-12, + 8.76302288609054966081E-11, + -4.40092476213282340617E-10, + -1.87992075640569295479E-10, + 1.31458150989474594064E-8, + -4.75513930924765465590E-8, + -2.21775018801848880741E-7, + 1.94635531373272490962E-6, + 4.33505889257316408893E-6, + -6.13387001076494349496E-5, + -3.13085477492997465138E-4, + 4.97164789823116062801E-4, + 2.64347496031374526641E-2, + 1.11446150876699213025E0 +}; + +/* x exp(-x) chin(x), inverted interval 18 to 88 */ +static double C2[] = { + 8.06913408255155572081E-18, + -2.08074168180148170312E-17, + -5.98111329658272336816E-17, + 2.68533951085945765591E-16, + 4.52313941698904694774E-16, + -3.10734917335299464535E-15, + -4.42823207332531972288E-15, + 3.49639695410806959872E-14, + 6.63406731718911586609E-14, + -3.71902448093119218395E-13, + -1.27135418132338309016E-12, + 2.74851141935315395333E-12, + 2.33781843985453438400E-11, + 2.71436006377612442764E-11, + -2.56600180000355990529E-10, + -1.61021375163803438552E-9, + -4.72543064876271773512E-9, + -3.00095178028681682282E-9, + 7.79387474390914922337E-8, + 1.06942765566401507066E-6, + 1.59503164802313196374E-5, + 3.49592575153777996871E-4, + 1.28475387530065247392E-2, + 1.03665693917934275131E0 +}; + +static double hyp3f0(double a1, double a2, double a3, double z); + +/* Sine and cosine integrals */ + +extern double MACHEP; + +int shichi(double x, double *si, double *ci) +{ + double k, z, c, s, a, b; + short sign; + + if (x < 0.0) { + sign = -1; + x = -x; + } + else + sign = 0; + + + if (x == 0.0) { + *si = 0.0; + *ci = -INFINITY; + return (0); + } + + if (x >= 8.0) + goto chb; + + if (x >= 88.0) + goto asymp; + + z = x * x; + + /* Direct power series expansion */ + a = 1.0; + s = 1.0; + c = 0.0; + k = 2.0; + + do { + a *= z / k; + c += a / k; + k += 1.0; + a /= k; + s += a / k; + k += 1.0; + } + while (fabs(a / s) > MACHEP); + + s *= x; + goto done; + + +chb: + /* Chebyshev series expansions */ + if (x < 18.0) { + a = (576.0 / x - 52.0) / 10.0; + k = exp(x) / x; + s = k * chbevl(a, S1, 22); + c = k * chbevl(a, C1, 23); + goto done; + } + + if (x <= 88.0) { + a = (6336.0 / x - 212.0) / 70.0; + k = exp(x) / x; + s = k * chbevl(a, S2, 23); + c = k * chbevl(a, C2, 24); + goto done; + } + +asymp: + if (x > 1000) { + *si = INFINITY; + *ci = INFINITY; + } + else { + /* Asymptotic expansions + * http://functions.wolfram.com/GammaBetaErf/CoshIntegral/06/02/ + * http://functions.wolfram.com/GammaBetaErf/SinhIntegral/06/02/0001/ + */ + a = hyp3f0(0.5, 1, 1, 4.0/(x*x)); + b = hyp3f0(1, 1, 1.5, 4.0/(x*x)); + *si = cosh(x)/x * a + sinh(x)/(x*x) * b; + *ci = sinh(x)/x * a + cosh(x)/(x*x) * b; + } + if (sign) { + *si = -*si; + } + return 0; + +done: + if (sign) + s = -s; + + *si = s; + + *ci = SCIPY_EULER + log(x) + c; + return (0); +} + + +/* + * Evaluate 3F0(a1, a2, a3; z) + * + * The series is only asymptotic, so this requires z large enough. + */ +static double hyp3f0(double a1, double a2, double a3, double z) +{ + int n, maxiter; + double err, sum, term, m; + + m = pow(z, -1.0/3); + if (m < 50) { + maxiter = m; + } + else { + maxiter = 50; + } + + term = 1.0; + sum = term; + for (n = 0; n < maxiter; ++n) { + term *= (a1 + n) * (a2 + n) * (a3 + n) * z / (n + 1); + sum += term; + if (fabs(term) < 1e-13 * fabs(sum) || term == 0) { + break; + } + } + + err = fabs(term); + + if (err > 1e-13 * fabs(sum)) { + return NAN; + } + + return sum; +} diff --git a/gtsam/3rdparty/cephes/cephes/sici.c b/gtsam/3rdparty/cephes/cephes/sici.c new file mode 100644 index 0000000000..7bb79bc25f --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sici.c @@ -0,0 +1,276 @@ +/* sici.c + * + * Sine and cosine integrals + * + * + * + * SYNOPSIS: + * + * double x, Ci, Si, sici(); + * + * sici( x, &Si, &Ci ); + * + * + * DESCRIPTION: + * + * Evaluates the integrals + * + * x + * - + * | cos t - 1 + * Ci(x) = eul + ln x + | --------- dt, + * | t + * - + * 0 + * x + * - + * | sin t + * Si(x) = | ----- dt + * | t + * - + * 0 + * + * where eul = 0.57721566490153286061 is Euler's constant. + * The integrals are approximated by rational functions. + * For x > 8 auxiliary functions f(x) and g(x) are employed + * such that + * + * Ci(x) = f(x) sin(x) - g(x) cos(x) + * Si(x) = pi/2 - f(x) cos(x) - g(x) sin(x) + * + * + * ACCURACY: + * Test interval = [0,50]. + * Absolute error, except relative when > 1: + * arithmetic function # trials peak rms + * IEEE Si 30000 4.4e-16 7.3e-17 + * IEEE Ci 30000 6.9e-16 5.1e-17 + */ + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +static double SN[] = { + -8.39167827910303881427E-11, + 4.62591714427012837309E-8, + -9.75759303843632795789E-6, + 9.76945438170435310816E-4, + -4.13470316229406538752E-2, + 1.00000000000000000302E0, +}; + +static double SD[] = { + 2.03269266195951942049E-12, + 1.27997891179943299903E-9, + 4.41827842801218905784E-7, + 9.96412122043875552487E-5, + 1.42085239326149893930E-2, + 9.99999999999999996984E-1, +}; + +static double CN[] = { + 2.02524002389102268789E-11, + -1.35249504915790756375E-8, + 3.59325051419993077021E-6, + -4.74007206873407909465E-4, + 2.89159652607555242092E-2, + -1.00000000000000000080E0, +}; + +static double CD[] = { + 4.07746040061880559506E-12, + 3.06780997581887812692E-9, + 1.23210355685883423679E-6, + 3.17442024775032769882E-4, + 5.10028056236446052392E-2, + 4.00000000000000000080E0, +}; + +static double FN4[] = { + 4.23612862892216586994E0, + 5.45937717161812843388E0, + 1.62083287701538329132E0, + 1.67006611831323023771E-1, + 6.81020132472518137426E-3, + 1.08936580650328664411E-4, + 5.48900223421373614008E-7, +}; + +static double FD4[] = { + /* 1.00000000000000000000E0, */ + 8.16496634205391016773E0, + 7.30828822505564552187E0, + 1.86792257950184183883E0, + 1.78792052963149907262E-1, + 7.01710668322789753610E-3, + 1.10034357153915731354E-4, + 5.48900252756255700982E-7, +}; + +static double FN8[] = { + 4.55880873470465315206E-1, + 7.13715274100146711374E-1, + 1.60300158222319456320E-1, + 1.16064229408124407915E-2, + 3.49556442447859055605E-4, + 4.86215430826454749482E-6, + 3.20092790091004902806E-8, + 9.41779576128512936592E-11, + 9.70507110881952024631E-14, +}; + +static double FD8[] = { + /* 1.00000000000000000000E0, */ + 9.17463611873684053703E-1, + 1.78685545332074536321E-1, + 1.22253594771971293032E-2, + 3.58696481881851580297E-4, + 4.92435064317881464393E-6, + 3.21956939101046018377E-8, + 9.43720590350276732376E-11, + 9.70507110881952025725E-14, +}; + +static double GN4[] = { + 8.71001698973114191777E-2, + 6.11379109952219284151E-1, + 3.97180296392337498885E-1, + 7.48527737628469092119E-2, + 5.38868681462177273157E-3, + 1.61999794598934024525E-4, + 1.97963874140963632189E-6, + 7.82579040744090311069E-9, +}; + +static double GD4[] = { + /* 1.00000000000000000000E0, */ + 1.64402202413355338886E0, + 6.66296701268987968381E-1, + 9.88771761277688796203E-2, + 6.22396345441768420760E-3, + 1.73221081474177119497E-4, + 2.02659182086343991969E-6, + 7.82579218933534490868E-9, +}; + +static double GN8[] = { + 6.97359953443276214934E-1, + 3.30410979305632063225E-1, + 3.84878767649974295920E-2, + 1.71718239052347903558E-3, + 3.48941165502279436777E-5, + 3.47131167084116673800E-7, + 1.70404452782044526189E-9, + 3.85945925430276600453E-12, + 3.14040098946363334640E-15, +}; + +static double GD8[] = { + /* 1.00000000000000000000E0, */ + 1.68548898811011640017E0, + 4.87852258695304967486E-1, + 4.67913194259625806320E-2, + 1.90284426674399523638E-3, + 3.68475504442561108162E-5, + 3.57043223443740838771E-7, + 1.72693748966316146736E-9, + 3.87830166023954706752E-12, + 3.14040098946363335242E-15, +}; + +extern double MACHEP; + + +int sici(double x, double *si, double *ci) +{ + double z, c, s, f, g; + short sign; + + if (x < 0.0) { + sign = -1; + x = -x; + } + else + sign = 0; + + + if (x == 0.0) { + *si = 0.0; + *ci = -INFINITY; + return (0); + } + + + if (x > 1.0e9) { + if (cephes_isinf(x)) { + if (sign == -1) { + *si = -M_PI_2; + *ci = NAN; + } + else { + *si = M_PI_2; + *ci = 0; + } + return 0; + } + *si = M_PI_2 - cos(x) / x; + *ci = sin(x) / x; + } + + + + if (x > 4.0) + goto asympt; + + z = x * x; + s = x * polevl(z, SN, 5) / polevl(z, SD, 5); + c = z * polevl(z, CN, 5) / polevl(z, CD, 5); + + if (sign) + s = -s; + *si = s; + *ci = SCIPY_EULER + log(x) + c; /* real part if x < 0 */ + return (0); + + + + /* The auxiliary functions are: + * + * + * *si = *si - M_PI_2; + * c = cos(x); + * s = sin(x); + * + * t = *ci * s - *si * c; + * a = *ci * c + *si * s; + * + * *si = t; + * *ci = -a; + */ + + + asympt: + + s = sin(x); + c = cos(x); + z = 1.0 / (x * x); + if (x < 8.0) { + f = polevl(z, FN4, 6) / (x * p1evl(z, FD4, 7)); + g = z * polevl(z, GN4, 7) / p1evl(z, GD4, 7); + } + else { + f = polevl(z, FN8, 8) / (x * p1evl(z, FD8, 8)); + g = z * polevl(z, GN8, 8) / p1evl(z, GD8, 9); + } + *si = M_PI_2 - f * c - g * s; + if (sign) + *si = -(*si); + *ci = f * s - g * c; + + return (0); +} diff --git a/gtsam/3rdparty/cephes/cephes/sindg.c b/gtsam/3rdparty/cephes/cephes/sindg.c new file mode 100644 index 0000000000..d9c37ebdbf --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sindg.c @@ -0,0 +1,219 @@ +/* sindg.c + * + * Circular sine of angle in degrees + * + * + * + * SYNOPSIS: + * + * double x, y, sindg(); + * + * y = sindg( x ); + * + * + * + * DESCRIPTION: + * + * Range reduction is into intervals of 45 degrees. + * + * Two polynomial approximating functions are employed. + * Between 0 and pi/4 the sine is approximated by + * x + x**3 P(x**2). + * Between pi/4 and pi/2 the cosine is represented as + * 1 - x**2 P(x**2). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE +-1000 30000 2.3e-16 5.6e-17 + * + * ERROR MESSAGES: + * + * message condition value returned + * sindg total loss x > 1.0e14 (IEEE) 0.0 + * + */ + /* cosdg.c + * + * Circular cosine of angle in degrees + * + * + * + * SYNOPSIS: + * + * double x, y, cosdg(); + * + * y = cosdg( x ); + * + * + * + * DESCRIPTION: + * + * Range reduction is into intervals of 45 degrees. + * + * Two polynomial approximating functions are employed. + * Between 0 and pi/4 the cosine is approximated by + * 1 - x**2 P(x**2). + * Between pi/4 and pi/2 the sine is represented as + * x + x**3 P(x**2). + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE +-1000 30000 2.1e-16 5.7e-17 + * See also sin(). + * + */ + +/* Cephes Math Library Release 2.0: April, 1987 + * Copyright 1985, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 */ + +#include "mconf.h" + +static double sincof[] = { + 1.58962301572218447952E-10, + -2.50507477628503540135E-8, + 2.75573136213856773549E-6, + -1.98412698295895384658E-4, + 8.33333333332211858862E-3, + -1.66666666666666307295E-1 +}; + +static double coscof[] = { + 1.13678171382044553091E-11, + -2.08758833757683644217E-9, + 2.75573155429816611547E-7, + -2.48015872936186303776E-5, + 1.38888888888806666760E-3, + -4.16666666666666348141E-2, + 4.99999999999999999798E-1 +}; + +static double PI180 = 1.74532925199432957692E-2; /* pi/180 */ +static double lossth = 1.0e14; + +double sindg(double x) +{ + double y, z, zz; + int j, sign; + + /* make argument positive but save the sign */ + sign = 1; + if (x < 0) { + x = -x; + sign = -1; + } + + if (x > lossth) { + sf_error("sindg", SF_ERROR_NO_RESULT, NULL); + return (0.0); + } + + y = floor(x / 45.0); /* integer part of x/M_PI_4 */ + + /* strip high bits of integer part to prevent integer overflow */ + z = ldexp(y, -4); + z = floor(z); /* integer part of y/8 */ + z = y - ldexp(z, 4); /* y - 16 * (y/16) */ + + j = z; /* convert to integer for tests on the phase angle */ + /* map zeros to origin */ + if (j & 1) { + j += 1; + y += 1.0; + } + j = j & 07; /* octant modulo 360 degrees */ + /* reflect in x axis */ + if (j > 3) { + sign = -sign; + j -= 4; + } + + z = x - y * 45.0; /* x mod 45 degrees */ + z *= PI180; /* multiply by pi/180 to convert to radians */ + zz = z * z; + + if ((j == 1) || (j == 2)) { + y = 1.0 - zz * polevl(zz, coscof, 6); + } + else { + y = z + z * (zz * polevl(zz, sincof, 5)); + } + + if (sign < 0) + y = -y; + + return (y); +} + + +double cosdg(double x) +{ + double y, z, zz; + int j, sign; + + /* make argument positive */ + sign = 1; + if (x < 0) + x = -x; + + if (x > lossth) { + sf_error("cosdg", SF_ERROR_NO_RESULT, NULL); + return (0.0); + } + + y = floor(x / 45.0); + z = ldexp(y, -4); + z = floor(z); /* integer part of y/8 */ + z = y - ldexp(z, 4); /* y - 16 * (y/16) */ + + /* integer and fractional part modulo one octant */ + j = z; + if (j & 1) { /* map zeros to origin */ + j += 1; + y += 1.0; + } + j = j & 07; + if (j > 3) { + j -= 4; + sign = -sign; + } + + if (j > 1) + sign = -sign; + + z = x - y * 45.0; /* x mod 45 degrees */ + z *= PI180; /* multiply by pi/180 to convert to radians */ + + zz = z * z; + + if ((j == 1) || (j == 2)) { + y = z + z * (zz * polevl(zz, sincof, 5)); + } + else { + y = 1.0 - zz * polevl(zz, coscof, 6); + } + + if (sign < 0) + y = -y; + + return (y); +} + + +/* Degrees, minutes, seconds to radians: */ + +/* 1 arc second, in radians = 4.848136811095359935899141023579479759563533023727e-6 */ +static double P64800 = + 4.848136811095359935899141023579479759563533023727e-6; + +double radian(double d, double m, double s) +{ + return (((d * 60.0 + m) * 60.0 + s) * P64800); +} diff --git a/gtsam/3rdparty/cephes/cephes/sinpi.c b/gtsam/3rdparty/cephes/cephes/sinpi.c new file mode 100644 index 0000000000..f0e52f9904 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sinpi.c @@ -0,0 +1,54 @@ +/* + * Implement sin(pi * x) and cos(pi * x) for real x. Since the periods + * of these functions are integral (and thus representable in double + * precision), it's possible to compute them with greater accuracy + * than sin(x) and cos(x). + */ +#include "mconf.h" + + +/* Compute sin(pi * x). */ +double sinpi(double x) +{ + double s = 1.0; + double r; + + if (x < 0.0) { + x = -x; + s = -1.0; + } + + r = fmod(x, 2.0); + if (r < 0.5) { + return s*sin(M_PI*r); + } + else if (r > 1.5) { + return s*sin(M_PI*(r - 2.0)); + } + else { + return -s*sin(M_PI*(r - 1.0)); + } +} + + +/* Compute cos(pi * x) */ +double cospi(double x) +{ + double r; + + if (x < 0.0) { + x = -x; + } + + r = fmod(x, 2.0); + if (r == 0.5) { + // We don't want to return -0.0 + return 0.0; + } + if (r < 1.0) { + return -sin(M_PI*(r - 0.5)); + } + else { + return sin(M_PI*(r - 1.5)); + } +} diff --git a/gtsam/3rdparty/cephes/cephes/spence.c b/gtsam/3rdparty/cephes/cephes/spence.c new file mode 100644 index 0000000000..48e1c40878 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/spence.c @@ -0,0 +1,125 @@ +/* spence.c + * + * Dilogarithm + * + * + * + * SYNOPSIS: + * + * double x, y, spence(); + * + * y = spence( x ); + * + * + * + * DESCRIPTION: + * + * Computes the integral + * + * x + * - + * | | log t + * spence(x) = - | ----- dt + * | | t - 1 + * - + * 1 + * + * for x >= 0. A rational approximation gives the integral in + * the interval (0.5, 1.5). Transformation formulas for 1/x + * and 1-x are employed outside the basic expansion range. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,4 30000 3.9e-15 5.4e-16 + * + * + */ + +/* spence.c */ + + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1985, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +static double A[8] = { + 4.65128586073990045278E-5, + 7.31589045238094711071E-3, + 1.33847639578309018650E-1, + 8.79691311754530315341E-1, + 2.71149851196553469920E0, + 4.25697156008121755724E0, + 3.29771340985225106936E0, + 1.00000000000000000126E0, +}; + +static double B[8] = { + 6.90990488912553276999E-4, + 2.54043763932544379113E-2, + 2.82974860602568089943E-1, + 1.41172597751831069617E0, + 3.63800533345137075418E0, + 5.03278880143316990390E0, + 3.54771340985225096217E0, + 9.99999999999999998740E-1, +}; + +extern double MACHEP; + +double spence(double x) +{ + double w, y, z; + int flag; + + if (x < 0.0) { + sf_error("spence", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + if (x == 1.0) + return (0.0); + + if (x == 0.0) + return (M_PI * M_PI / 6.0); + + flag = 0; + + if (x > 2.0) { + x = 1.0 / x; + flag |= 2; + } + + if (x > 1.5) { + w = (1.0 / x) - 1.0; + flag |= 2; + } + + else if (x < 0.5) { + w = -x; + flag |= 1; + } + + else + w = x - 1.0; + + + y = -w * polevl(w, A, 7) / polevl(w, B, 7); + + if (flag & 1) + y = (M_PI * M_PI) / 6.0 - log(x) * log(1.0 - x) - y; + + if (flag & 2) { + z = log(x); + y = -0.5 * z * z - y; + } + + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/stdtr.c b/gtsam/3rdparty/cephes/cephes/stdtr.c new file mode 100644 index 0000000000..5a37536bed --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/stdtr.c @@ -0,0 +1,203 @@ +/* stdtr.c + * + * Student's t distribution + * + * + * + * SYNOPSIS: + * + * double t, stdtr(); + * short k; + * + * y = stdtr( k, t ); + * + * + * DESCRIPTION: + * + * Computes the integral from minus infinity to t of the Student + * t distribution with integer k > 0 degrees of freedom: + * + * t + * - + * | | + * - | 2 -(k+1)/2 + * | ( (k+1)/2 ) | ( x ) + * ---------------------- | ( 1 + --- ) dx + * - | ( k ) + * sqrt( k pi ) | ( k/2 ) | + * | | + * - + * -inf. + * + * Relation to incomplete beta integral: + * + * 1 - stdtr(k,t) = 0.5 * incbet( k/2, 1/2, z ) + * where + * z = k/(k + t**2). + * + * For t < -2, this is the method of computation. For higher t, + * a direct method is derived from integration by parts. + * Since the function is symmetric about t=0, the area under the + * right tail of the density is found by calling the function + * with -t instead of t. + * + * ACCURACY: + * + * Tested at random 1 <= k <= 25. The "domain" refers to t. + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -100,-2 50000 5.9e-15 1.4e-15 + * IEEE -2,100 500000 2.7e-15 4.9e-17 + */ + +/* stdtri.c + * + * Functional inverse of Student's t distribution + * + * + * + * SYNOPSIS: + * + * double p, t, stdtri(); + * int k; + * + * t = stdtri( k, p ); + * + * + * DESCRIPTION: + * + * Given probability p, finds the argument t such that stdtr(k,t) + * is equal to p. + * + * ACCURACY: + * + * Tested at random 1 <= k <= 100. The "domain" refers to p: + * Relative error: + * arithmetic domain # trials peak rms + * IEEE .001,.999 25000 5.7e-15 8.0e-16 + * IEEE 10^-6,.001 25000 2.0e-12 2.9e-14 + */ + + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" +#include + +extern double MACHEP; + +double stdtr(int k, double t) +{ + double x, rk, z, f, tz, p, xsqk; + int j; + + if (k <= 0) { + sf_error("stdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + if (t == 0) + return (0.5); + + if (t < -2.0) { + rk = k; + z = rk / (rk + t * t); + p = 0.5 * incbet(0.5 * rk, 0.5, z); + return (p); + } + + /* compute integral from -t to + t */ + + if (t < 0) + x = -t; + else + x = t; + + rk = k; /* degrees of freedom */ + z = 1.0 + (x * x) / rk; + + /* test if k is odd or even */ + if ((k & 1) != 0) { + + /* computation for odd k */ + + xsqk = x / sqrt(rk); + p = atan(xsqk); + if (k > 1) { + f = 1.0; + tz = 1.0; + j = 3; + while ((j <= (k - 2)) && ((tz / f) > MACHEP)) { + tz *= (j - 1) / (z * j); + f += tz; + j += 2; + } + p += f * xsqk / z; + } + p *= 2.0 / M_PI; + } + + + else { + + /* computation for even k */ + + f = 1.0; + tz = 1.0; + j = 2; + + while ((j <= (k - 2)) && ((tz / f) > MACHEP)) { + tz *= (j - 1) / (z * j); + f += tz; + j += 2; + } + p = f * x / sqrt(z * rk); + } + + /* common exit */ + + + if (t < 0) + p = -p; /* note destruction of relative accuracy */ + + p = 0.5 + 0.5 * p; + return (p); +} + +double stdtri(int k, double p) +{ + double t, rk, z; + int rflg; + + if (k <= 0 || p <= 0.0 || p >= 1.0) { + sf_error("stdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + rk = k; + + if (p > 0.25 && p < 0.75) { + if (p == 0.5) + return (0.0); + z = 1.0 - 2.0 * p; + z = incbi(0.5, 0.5 * rk, fabs(z)); + t = sqrt(rk * z / (1.0 - z)); + if (p < 0.5) + t = -t; + return (t); + } + rflg = -1; + if (p >= 0.5) { + p = 1.0 - p; + rflg = 1; + } + z = incbi(0.5 * rk, 0.5, 2.0 * p); + + if (DBL_MAX * z < rk) + return (rflg * INFINITY); + t = sqrt(rk / z - rk); + return (rflg * t); +} diff --git a/gtsam/3rdparty/cephes/cephes/struve.c b/gtsam/3rdparty/cephes/cephes/struve.c new file mode 100644 index 0000000000..26c86fa2d7 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/struve.c @@ -0,0 +1,408 @@ +/* + * Compute the Struve function. + * + * Notes + * ----- + * + * We use three expansions for the Struve function discussed in [1]: + * + * - power series + * - expansion in Bessel functions + * - asymptotic large-z expansion + * + * Rounding errors are estimated based on the largest terms in the sums. + * + * ``struve_convergence.py`` plots the convergence regions of the different + * expansions. + * + * (i) + * + * Looking at the error in the asymptotic expansion, one finds that + * it's not worth trying if z ~> 0.7 * v + 12 for v > 0. + * + * (ii) + * + * The Bessel function expansion tends to fail for |z| >~ |v| and is not tried + * there. + * + * For Struve H it covers the quadrant v > z where the power series may fail to + * produce reasonable results. + * + * (iii) + * + * The three expansions together cover for Struve H the region z > 0, v real. + * + * They also cover Struve L, except that some loss of precision may occur around + * the transition region z ~ 0.7 |v|, v < 0, |v| >> 1 where the function changes + * rapidly. + * + * (iv) + * + * The power series is evaluated in double-double precision. This fixes accuracy + * issues in Struve H for |v| << |z| before the asymptotic expansion kicks in. + * Moreover, it improves the Struve L behavior for negative v. + * + * + * References + * ---------- + * [1] NIST Digital Library of Mathematical Functions + * https://dlmf.nist.gov/11 + */ + +/* + * Copyright (C) 2013 Pauli Virtanen + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * a. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * b. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * c. Neither the name of Enthought nor the names of the SciPy Developers + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS + * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, + * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF + * THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include "mconf.h" +#include "dd_real.h" + +// #include "amos_wrappers.h" + +#define STRUVE_MAXITER 10000 +#define SUM_EPS 1e-16 /* be sure we are in the tail of the sum */ +#define SUM_TINY 1e-100 +#define GOOD_EPS 1e-12 +#define ACCEPTABLE_EPS 1e-7 +#define ACCEPTABLE_ATOL 1e-300 + +#define MIN(a, b) ((a) < (b) ? (a) : (b)) + +double struve_power_series(double v, double x, int is_h, double *err); +double struve_asymp_large_z(double v, double z, int is_h, double *err); +double struve_bessel_series(double v, double z, int is_h, double *err); + +static double bessel_y(double v, double x); +static double bessel_j(double v, double x); +static double struve_hl(double v, double x, int is_h); + +double struve_h(double v, double z) +{ + return struve_hl(v, z, 1); +} + +double struve_l(double v, double z) +{ + return struve_hl(v, z, 0); +} + +static double struve_hl(double v, double z, int is_h) +{ + double value[4], err[4], tmp; + int n; + + if (z < 0) { + n = v; + if (v == n) { + tmp = (n % 2 == 0) ? -1 : 1; + return tmp * struve_hl(v, -z, is_h); + } + else { + return NAN; + } + } + else if (z == 0) { + if (v < -1) { + return gammasgn(v + 1.5) * INFINITY; + } + else if (v == -1) { + return 2 / sqrt(M_PI) / Gamma(0.5); + } + else { + return 0; + } + } + + n = -v - 0.5; + if (n == -v - 0.5 && n > 0) { + if (is_h) { + return (n % 2 == 0 ? 1 : -1) * bessel_j(n + 0.5, z); + } + else { + return iv(n + 0.5, z); + } + } + + /* Try the asymptotic expansion */ + if (z >= 0.7*v + 12) { + value[0] = struve_asymp_large_z(v, z, is_h, &err[0]); + if (err[0] < GOOD_EPS * fabs(value[0])) { + return value[0]; + } + } + else { + err[0] = INFINITY; + } + + /* Try power series */ + value[1] = struve_power_series(v, z, is_h, &err[1]); + if (err[1] < GOOD_EPS * fabs(value[1])) { + return value[1]; + } + + /* Try bessel series */ + if (fabs(z) < fabs(v) + 20) { + value[2] = struve_bessel_series(v, z, is_h, &err[2]); + if (err[2] < GOOD_EPS * fabs(value[2])) { + return value[2]; + } + } + else { + err[2] = INFINITY; + } + + /* Return the best of the three, if it is acceptable */ + n = 0; + if (err[1] < err[n]) n = 1; + if (err[2] < err[n]) n = 2; + if (err[n] < ACCEPTABLE_EPS * fabs(value[n]) || err[n] < ACCEPTABLE_ATOL) { + return value[n]; + } + + /* Maybe it really is an overflow? */ + tmp = -lgam(v + 1.5) + (v + 1)*log(z/2); + if (!is_h) { + tmp = fabs(tmp); + } + if (tmp > 700) { + sf_error("struve", SF_ERROR_OVERFLOW, NULL); + return INFINITY * gammasgn(v + 1.5); + } + + /* Failure */ + sf_error("struve", SF_ERROR_NO_RESULT, NULL); + return NAN; +} + + +/* + * Power series for Struve H and L + * https://dlmf.nist.gov/11.2.1 + * + * Starts to converge roughly at |n| > |z| + */ +double struve_power_series(double v, double z, int is_h, double *err) +{ + int n, sgn; + double term, sum, maxterm, scaleexp, tmp; + double2 cterm, csum, cdiv, z2, c2v, ctmp; + + if (is_h) { + sgn = -1; + } + else { + sgn = 1; + } + + tmp = -lgam(v + 1.5) + (v + 1)*log(z/2); + if (tmp < -600 || tmp > 600) { + /* Scale exponent to postpone underflow/overflow */ + scaleexp = tmp/2; + tmp -= scaleexp; + } + else { + scaleexp = 0; + } + + term = 2 / sqrt(M_PI) * exp(tmp) * gammasgn(v + 1.5); + sum = term; + maxterm = 0; + + cterm = dd_create_d(term); + csum = dd_create_d(sum); + z2 = dd_create_d(sgn*z*z); + c2v = dd_create_d(2*v); + + for (n = 0; n < STRUVE_MAXITER; ++n) { + /* cdiv = (3 + 2*n) * (3 + 2*n + 2*v)) */ + cdiv = dd_create_d(3 + 2*n); + ctmp = dd_create_d(3 + 2*n); + ctmp = dd_add(ctmp, c2v); + cdiv = dd_mul(cdiv, ctmp); + + /* cterm *= z2 / cdiv */ + cterm = dd_mul(cterm, z2); + cterm = dd_div(cterm, cdiv); + + csum = dd_add(csum, cterm); + + term = dd_to_double(cterm); + sum = dd_to_double(csum); + + if (fabs(term) > maxterm) { + maxterm = fabs(term); + } + if (fabs(term) < SUM_TINY * fabs(sum) || term == 0 || !isfinite(sum)) { + break; + } + } + + *err = fabs(term) + fabs(maxterm) * 1e-22; + + if (scaleexp != 0) { + sum *= exp(scaleexp); + *err *= exp(scaleexp); + } + + if (sum == 0 && term == 0 && v < 0 && !is_h) { + /* Spurious underflow */ + *err = INFINITY; + return NAN; + } + + return sum; +} + + +/* + * Bessel series + * https://dlmf.nist.gov/11.4.19 + */ +double struve_bessel_series(double v, double z, int is_h, double *err) +{ + int n; + double term, cterm, sum, maxterm; + + if (is_h && v < 0) { + /* Works less reliably in this region */ + *err = INFINITY; + return NAN; + } + + sum = 0; + maxterm = 0; + + cterm = sqrt(z / (2*M_PI)); + + for (n = 0; n < STRUVE_MAXITER; ++n) { + if (is_h) { + term = cterm * bessel_j(n + v + 0.5, z) / (n + 0.5); + cterm *= z/2 / (n + 1); + } + else { + term = cterm * iv(n + v + 0.5, z) / (n + 0.5); + cterm *= -z/2 / (n + 1); + } + sum += term; + if (fabs(term) > maxterm) { + maxterm = fabs(term); + } + if (fabs(term) < SUM_EPS * fabs(sum) || term == 0 || !isfinite(sum)) { + break; + } + } + + *err = fabs(term) + fabs(maxterm) * 1e-16; + + /* Account for potential underflow of the Bessel functions */ + *err += 1e-300 * fabs(cterm); + + return sum; +} + + +/* + * Large-z expansion for Struve H and L + * https://dlmf.nist.gov/11.6.1 + */ +double struve_asymp_large_z(double v, double z, int is_h, double *err) +{ + int n, sgn, maxiter; + double term, sum, maxterm; + double m; + + if (is_h) { + sgn = -1; + } + else { + sgn = 1; + } + + /* Asymptotic expansion divergenge point */ + m = z/2; + if (m <= 0) { + maxiter = 0; + } + else if (m > STRUVE_MAXITER) { + maxiter = STRUVE_MAXITER; + } + else { + maxiter = (int)m; + } + if (maxiter == 0) { + *err = INFINITY; + return NAN; + } + + if (z < v) { + /* Exclude regions where our error estimation fails */ + *err = INFINITY; + return NAN; + } + + /* Evaluate sum */ + term = -sgn / sqrt(M_PI) * exp(-lgam(v + 0.5) + (v - 1) * log(z/2)) * gammasgn(v + 0.5); + sum = term; + maxterm = 0; + + for (n = 0; n < maxiter; ++n) { + term *= sgn * (1 + 2*n) * (1 + 2*n - 2*v) / (z*z); + sum += term; + if (fabs(term) > maxterm) { + maxterm = fabs(term); + } + if (fabs(term) < SUM_EPS * fabs(sum) || term == 0 || !isfinite(sum)) { + break; + } + } + + if (is_h) { + sum += bessel_y(v, z); + } + else { + sum += iv(v, z); + } + + /* + * This error estimate is strictly speaking valid only for + * n > v - 0.5, but numerical results indicate that it works + * reasonably. + */ + *err = fabs(term) + fabs(maxterm) * 1e-16; + + return sum; +} + + +static double bessel_y(double v, double x) +{ + return cbesy_wrap_real(v, x); +} + +static double bessel_j(double v, double x) +{ + return cbesj_wrap_real(v, x); +} diff --git a/gtsam/3rdparty/cephes/cephes/tandg.c b/gtsam/3rdparty/cephes/cephes/tandg.c new file mode 100644 index 0000000000..1ea86329be --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/tandg.c @@ -0,0 +1,141 @@ +/* tandg.c + * + * Circular tangent of argument in degrees + * + * + * + * SYNOPSIS: + * + * double x, y, tandg(); + * + * y = tandg( x ); + * + * + * + * DESCRIPTION: + * + * Returns the circular tangent of the argument x in degrees. + * + * Range reduction is modulo pi/4. A rational function + * x + x**3 P(x**2)/Q(x**2) + * is employed in the basic interval [0, pi/4]. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,10 30000 3.2e-16 8.4e-17 + * + * ERROR MESSAGES: + * + * message condition value returned + * tandg total loss x > 1.0e14 (IEEE) 0.0 + * tandg singularity x = 180 k + 90 INFINITY + */ + /* cotdg.c + * + * Circular cotangent of argument in degrees + * + * + * + * SYNOPSIS: + * + * double x, y, cotdg(); + * + * y = cotdg( x ); + * + * + * + * DESCRIPTION: + * + * Returns the circular cotangent of the argument x in degrees. + * + * Range reduction is modulo pi/4. A rational function + * x + x**3 P(x**2)/Q(x**2) + * is employed in the basic interval [0, pi/4]. + * + * + * ERROR MESSAGES: + * + * message condition value returned + * cotdg total loss x > 1.0e14 (IEEE) 0.0 + * cotdg singularity x = 180 k INFINITY + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +static double PI180 = 1.74532925199432957692E-2; +static double lossth = 1.0e14; + +static double tancot(double, int); + +double tandg(double x) +{ + return (tancot(x, 0)); +} + + +double cotdg(double x) +{ + return (tancot(x, 1)); +} + + +static double tancot(double xx, int cotflg) +{ + double x; + int sign; + + /* make argument positive but save the sign */ + if (xx < 0) { + x = -xx; + sign = -1; + } + else { + x = xx; + sign = 1; + } + + if (x > lossth) { + sf_error("tandg", SF_ERROR_NO_RESULT, NULL); + return 0.0; + } + + /* modulo 180 */ + x = x - 180.0 * floor(x / 180.0); + if (cotflg) { + if (x <= 90.0) { + x = 90.0 - x; + } + else { + x = x - 90.0; + sign *= -1; + } + } + else { + if (x > 90.0) { + x = 180.0 - x; + sign *= -1; + } + } + if (x == 0.0) { + return 0.0; + } + else if (x == 45.0) { + return sign * 1.0; + } + else if (x == 90.0) { + sf_error((cotflg ? "cotdg" : "tandg"), SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + /* x is now transformed into [0, 90) */ + return sign * tan(x * PI180); +} diff --git a/gtsam/3rdparty/cephes/cephes/tukey.c b/gtsam/3rdparty/cephes/cephes/tukey.c new file mode 100644 index 0000000000..751314a875 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/tukey.c @@ -0,0 +1,68 @@ + +/* Compute the CDF of the Tukey-Lambda distribution + * using a bracketing search with special checks + * + * The PPF of the Tukey-lambda distribution is + * G(p) = (p**lam + (1-p)**lam) / lam + * + * Author: Travis Oliphant + */ + +#include + +#define SMALLVAL 1e-4 +#define EPS 1.0e-14 +#define MAXCOUNT 60 + +double tukeylambdacdf(double x, double lmbda) +{ + double pmin, pmid, pmax, plow, phigh, xeval; + int count; + + if (isnan(x) || isnan(lmbda)) { + return NAN; + } + + xeval = 1.0 / lmbda; + if (lmbda > 0.0) { + if (x <= (-xeval)) { + return 0.0; + } + if (x >= xeval) { + return 1.0; + } + } + + if ((-SMALLVAL < lmbda) && (lmbda < SMALLVAL)) { + if (x >= 0) { + return 1.0 / (1.0 + exp(-x)); + } + else { + return exp(x) / (1.0 + exp(x)); + } + } + + pmin = 0.0; + pmid = 0.5; + pmax = 1.0; + plow = pmin; + phigh = pmax; + count = 0; + + while ((count < MAXCOUNT) && (fabs(pmid - plow) > EPS)) { + xeval = (pow(pmid, lmbda) - pow(1.0 - pmid, lmbda)) / lmbda; + if (xeval == x) { + return pmid; + } + if (xeval > x) { + phigh = pmid; + pmid = (pmid + plow) / 2.0; + } + else { + plow = pmid; + pmid = (pmid + phigh) / 2.0; + } + count++; + } + return pmid; +} diff --git a/gtsam/3rdparty/cephes/cephes/unity.c b/gtsam/3rdparty/cephes/cephes/unity.c new file mode 100644 index 0000000000..76bc7f08df --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/unity.c @@ -0,0 +1,190 @@ +/* unity.c + * + * Relative error approximations for function arguments near + * unity. + * + * log1p(x) = log(1+x) + * expm1(x) = exp(x) - 1 + * cosm1(x) = cos(x) - 1 + * lgam1p(x) = lgam(1+x) + * + */ + +/* Scipy changes: + * - 06-10-2016: added lgam1p + */ + +#include "mconf.h" + +extern double MACHEP; + + + +/* log1p(x) = log(1 + x) */ + +/* Coefficients for log(1+x) = x - x**2/2 + x**3 P(x)/Q(x) + * 1/sqrt(2) <= x < sqrt(2) + * Theoretical peak relative error = 2.32e-20 + */ +static const double LP[] = { + 4.5270000862445199635215E-5, + 4.9854102823193375972212E-1, + 6.5787325942061044846969E0, + 2.9911919328553073277375E1, + 6.0949667980987787057556E1, + 5.7112963590585538103336E1, + 2.0039553499201281259648E1, +}; + +static const double LQ[] = { + /* 1.0000000000000000000000E0, */ + 1.5062909083469192043167E1, + 8.3047565967967209469434E1, + 2.2176239823732856465394E2, + 3.0909872225312059774938E2, + 2.1642788614495947685003E2, + 6.0118660497603843919306E1, +}; + +double log1p(double x) +{ + double z; + + z = 1.0 + x; + if ((z < M_SQRT1_2) || (z > M_SQRT2)) + return (log(z)); + z = x * x; + z = -0.5 * z + x * (z * polevl(x, LP, 6) / p1evl(x, LQ, 6)); + return (x + z); +} + + +/* log(1 + x) - x */ +double log1pmx(double x) +{ + if (fabs(x) < 0.5) { + int n; + double xfac = x; + double term; + double res = 0; + + for(n = 2; n < MAXITER; n++) { + xfac *= -x; + term = xfac / n; + res += term; + if (fabs(term) < MACHEP * fabs(res)) { + break; + } + } + return res; + } + else { + return log1p(x) - x; + } +} + + +/* expm1(x) = exp(x) - 1 */ + +/* e^x = 1 + 2x P(x^2)/( Q(x^2) - P(x^2) ) + * -0.5 <= x <= 0.5 + */ + +static double EP[3] = { + 1.2617719307481059087798E-4, + 3.0299440770744196129956E-2, + 9.9999999999999999991025E-1, +}; + +static double EQ[4] = { + 3.0019850513866445504159E-6, + 2.5244834034968410419224E-3, + 2.2726554820815502876593E-1, + 2.0000000000000000000897E0, +}; + +double expm1(double x) +{ + double r, xx; + + if (!cephes_isfinite(x)) { + if (cephes_isnan(x)) { + return x; + } + else if (x > 0) { + return x; + } + else { + return -1.0; + } + + } + if ((x < -0.5) || (x > 0.5)) + return (exp(x) - 1.0); + xx = x * x; + r = x * polevl(xx, EP, 2); + r = r / (polevl(xx, EQ, 3) - r); + return (r + r); +} + + + +/* cosm1(x) = cos(x) - 1 */ + +static double coscof[7] = { + 4.7377507964246204691685E-14, + -1.1470284843425359765671E-11, + 2.0876754287081521758361E-9, + -2.7557319214999787979814E-7, + 2.4801587301570552304991E-5, + -1.3888888888888872993737E-3, + 4.1666666666666666609054E-2, +}; + +double cosm1(double x) +{ + double xx; + + if ((x < -M_PI_4) || (x > M_PI_4)) + return (cos(x) - 1.0); + xx = x * x; + xx = -0.5 * xx + xx * xx * polevl(xx, coscof, 6); + return xx; +} + + +/* Compute lgam(x + 1) around x = 0 using its Taylor series. */ +static double lgam1p_taylor(double x) +{ + int n; + double xfac, coeff, res; + + if (x == 0) { + return 0; + } + res = -SCIPY_EULER * x; + xfac = -x; + for (n = 2; n < 42; n++) { + xfac *= -x; + coeff = zeta(n, 1) * xfac / n; + res += coeff; + if (fabs(coeff) < MACHEP * fabs(res)) { + break; + } + } + + return res; +} + + +/* Compute lgam(x + 1). */ +double lgam1p(double x) +{ + if (fabs(x) <= 0.5) { + return lgam1p_taylor(x); + } else if (fabs(x - 1) < 0.5) { + return log(x) + lgam1p_taylor(x - 1); + } else { + return lgam(x + 1); + } +} diff --git a/gtsam/3rdparty/cephes/cephes/yn.c b/gtsam/3rdparty/cephes/cephes/yn.c new file mode 100644 index 0000000000..c02ff0acd8 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/yn.c @@ -0,0 +1,105 @@ +/* yn.c + * + * Bessel function of second kind of integer order + * + * + * + * SYNOPSIS: + * + * double x, y, yn(); + * int n; + * + * y = yn( n, x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of order n, where n is a + * (possibly negative) integer. + * + * The function is evaluated by forward recurrence on + * n, starting with values computed by the routines + * y0() and y1(). + * + * If n = 0 or 1 the routine for y0 or y1 is called + * directly. + * + * + * + * ACCURACY: + * + * + * Absolute error, except relative + * when y > 1: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 3.4e-15 4.3e-16 + * + * + * ERROR MESSAGES: + * + * message condition value returned + * yn singularity x = 0 INFINITY + * yn overflow INFINITY + * + * Spot checked against tables for x, n between 0 and 100. + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" +extern double MAXLOG; + +double yn(int n, double x) +{ + double an, anm1, anm2, r; + int k, sign; + + if (n < 0) { + n = -n; + if ((n & 1) == 0) /* -1**n */ + sign = 1; + else + sign = -1; + } + else + sign = 1; + + + if (n == 0) + return (sign * y0(x)); + if (n == 1) + return (sign * y1(x)); + + /* test for overflow */ + if (x == 0.0) { + sf_error("yn", SF_ERROR_SINGULAR, NULL); + return -INFINITY * sign; + } + else if (x < 0.0) { + sf_error("yn", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + /* forward recurrence on n */ + + anm2 = y0(x); + anm1 = y1(x); + k = 1; + r = 2 * k; + do { + an = r * anm1 / x - anm2; + anm2 = anm1; + anm1 = an; + r += 2.0; + ++k; + } + while (k < n); + + + return (sign * an); +} diff --git a/gtsam/3rdparty/cephes/cephes/yv.c b/gtsam/3rdparty/cephes/cephes/yv.c new file mode 100644 index 0000000000..e61a155214 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/yv.c @@ -0,0 +1,46 @@ +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +extern double MACHEP; + + +/* + * Bessel function of noninteger order + */ +double yv(double v, double x) +{ + double y, t; + int n; + + n = v; + if (n == v) { + y = yn(n, x); + return (y); + } + else if (v == floor(v)) { + /* Zero in denominator. */ + sf_error("yv", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + t = M_PI * v; + y = (cos(t) * jv(v, x) - jv(-v, x)) / sin(t); + + if (cephes_isinf(y)) { + if (v > 0) { + sf_error("yv", SF_ERROR_OVERFLOW, NULL); + return -INFINITY; + } + else if (v < -1e10) { + /* Whether it's +inf or -inf is numerically ill-defined. */ + sf_error("yv", SF_ERROR_DOMAIN, NULL); + return NAN; + } + } + + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/zeta.c b/gtsam/3rdparty/cephes/cephes/zeta.c new file mode 100644 index 0000000000..554933a24c --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/zeta.c @@ -0,0 +1,160 @@ +/* zeta.c + * + * Riemann zeta function of two arguments + * + * + * + * SYNOPSIS: + * + * double x, q, y, zeta(); + * + * y = zeta( x, q ); + * + * + * + * DESCRIPTION: + * + * + * + * inf. + * - -x + * zeta(x,q) = > (k+q) + * - + * k=0 + * + * where x > 1 and q is not a negative integer or zero. + * The Euler-Maclaurin summation formula is used to obtain + * the expansion + * + * n + * - -x + * zeta(x,q) = > (k+q) + * - + * k=1 + * + * 1-x inf. B x(x+1)...(x+2j) + * (n+q) 1 - 2j + * + --------- - ------- + > -------------------- + * x-1 x - x+2j+1 + * 2(n+q) j=1 (2j)! (n+q) + * + * where the B2j are Bernoulli numbers. Note that (see zetac.c) + * zeta(x,1) = zetac(x) + 1. + * + * + * + * ACCURACY: + * + * + * + * REFERENCE: + * + * Gradshteyn, I. S., and I. M. Ryzhik, Tables of Integrals, + * Series, and Products, p. 1073; Academic Press, 1980. + * + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" +extern double MACHEP; + +/* Expansion coefficients + * for Euler-Maclaurin summation formula + * (2k)! / B2k + * where B2k are Bernoulli numbers + */ +static double A[] = { + 12.0, + -720.0, + 30240.0, + -1209600.0, + 47900160.0, + -1.8924375803183791606e9, /*1.307674368e12/691 */ + 7.47242496e10, + -2.950130727918164224e12, /*1.067062284288e16/3617 */ + 1.1646782814350067249e14, /*5.109094217170944e18/43867 */ + -4.5979787224074726105e15, /*8.028576626982912e20/174611 */ + 1.8152105401943546773e17, /*1.5511210043330985984e23/854513 */ + -7.1661652561756670113e18 /*1.6938241367317436694528e27/236364091 */ +}; + +/* 30 Nov 86 -- error in third coefficient fixed */ + + +double zeta(double x, double q) +{ + int i; + double a, b, k, s, t, w; + + if (x == 1.0) + goto retinf; + + if (x < 1.0) { + domerr: + sf_error("zeta", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + if (q <= 0.0) { + if (q == floor(q)) { + sf_error("zeta", SF_ERROR_SINGULAR, NULL); + retinf: + return (INFINITY); + } + if (x != floor(x)) + goto domerr; /* because q^-x not defined */ + } + + /* Asymptotic expansion + * https://dlmf.nist.gov/25.11#E43 + */ + if (q > 1e8) { + return (1/(x - 1) + 1/(2*q)) * pow(q, 1 - x); + } + + /* Euler-Maclaurin summation formula */ + + /* Permit negative q but continue sum until n+q > +9 . + * This case should be handled by a reflection formula. + * If q<0 and x is an integer, there is a relation to + * the polyGamma function. + */ + s = pow(q, -x); + a = q; + i = 0; + b = 0.0; + while ((i < 9) || (a <= 9.0)) { + i += 1; + a += 1.0; + b = pow(a, -x); + s += b; + if (fabs(b / s) < MACHEP) + goto done; + } + + w = a; + s += b * w / (x - 1.0); + s -= 0.5 * b; + a = 1.0; + k = 0.0; + for (i = 0; i < 12; i++) { + a *= x + k; + b /= w; + t = a * b / A[i]; + s = s + t; + t = fabs(t / s); + if (t < MACHEP) + goto done; + k += 1.0; + a *= x + k; + b /= w; + k += 1.0; + } +done: + return (s); +} diff --git a/gtsam/3rdparty/cephes/cephes/zetac.c b/gtsam/3rdparty/cephes/cephes/zetac.c new file mode 100644 index 0000000000..8414331832 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/zetac.c @@ -0,0 +1,345 @@ +/* zetac.c + * + * Riemann zeta function + * + * + * + * SYNOPSIS: + * + * double x, y, zetac(); + * + * y = zetac( x ); + * + * + * + * DESCRIPTION: + * + * + * + * inf. + * - -x + * zetac(x) = > k , x > 1, + * - + * k=2 + * + * is related to the Riemann zeta function by + * + * Riemann zeta(x) = zetac(x) + 1. + * + * Extension of the function definition for x < 1 is implemented. + * Zero is returned for x > log2(INFINITY). + * + * ACCURACY: + * + * Tabulated values have full machine accuracy. + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 1,50 10000 9.8e-16 1.3e-16 + * + * + */ + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" +#include "lanczos.h" + +/* Riemann zeta(x) - 1 + * for integer arguments between 0 and 30. + */ +static const double azetac[] = { + -1.50000000000000000000E0, + 0.0, /* Not used; zetac(1.0) is infinity. */ + 6.44934066848226436472E-1, + 2.02056903159594285400E-1, + 8.23232337111381915160E-2, + 3.69277551433699263314E-2, + 1.73430619844491397145E-2, + 8.34927738192282683980E-3, + 4.07735619794433937869E-3, + 2.00839282608221441785E-3, + 9.94575127818085337146E-4, + 4.94188604119464558702E-4, + 2.46086553308048298638E-4, + 1.22713347578489146752E-4, + 6.12481350587048292585E-5, + 3.05882363070204935517E-5, + 1.52822594086518717326E-5, + 7.63719763789976227360E-6, + 3.81729326499983985646E-6, + 1.90821271655393892566E-6, + 9.53962033872796113152E-7, + 4.76932986787806463117E-7, + 2.38450502727732990004E-7, + 1.19219925965311073068E-7, + 5.96081890512594796124E-8, + 2.98035035146522801861E-8, + 1.49015548283650412347E-8, + 7.45071178983542949198E-9, + 3.72533402478845705482E-9, + 1.86265972351304900640E-9, + 9.31327432419668182872E-10 +}; + +/* 2**x (1 - 1/x) (zeta(x) - 1) = P(1/x)/Q(1/x), 1 <= x <= 10 */ +static double P[9] = { + 5.85746514569725319540E11, + 2.57534127756102572888E11, + 4.87781159567948256438E10, + 5.15399538023885770696E9, + 3.41646073514754094281E8, + 1.60837006880656492731E7, + 5.92785467342109522998E5, + 1.51129169964938823117E4, + 2.01822444485997955865E2, +}; + +static double Q[8] = { + /* 1.00000000000000000000E0, */ + 3.90497676373371157516E11, + 5.22858235368272161797E10, + 5.64451517271280543351E9, + 3.39006746015350418834E8, + 1.79410371500126453702E7, + 5.66666825131384797029E5, + 1.60382976810944131506E4, + 1.96436237223387314144E2, +}; + +/* log(zeta(x) - 1 - 2**-x), 10 <= x <= 50 */ +static double A[11] = { + 8.70728567484590192539E6, + 1.76506865670346462757E8, + 2.60889506707483264896E10, + 5.29806374009894791647E11, + 2.26888156119238241487E13, + 3.31884402932705083599E14, + 5.13778997975868230192E15, + -1.98123688133907171455E15, + -9.92763810039983572356E16, + 7.82905376180870586444E16, + 9.26786275768927717187E16, +}; + +static double B[10] = { + /* 1.00000000000000000000E0, */ + -7.92625410563741062861E6, + -1.60529969932920229676E8, + -2.37669260975543221788E10, + -4.80319584350455169857E11, + -2.07820961754173320170E13, + -2.96075404507272223680E14, + -4.86299103694609136686E15, + 5.34589509675789930199E15, + 5.71464111092297631292E16, + -1.79915597658676556828E16, +}; + +/* (1-x) (zeta(x) - 1), 0 <= x <= 1 */ +static double R[6] = { + -3.28717474506562731748E-1, + 1.55162528742623950834E1, + -2.48762831680821954401E2, + 1.01050368053237678329E3, + 1.26726061410235149405E4, + -1.11578094770515181334E5, +}; + +static double S[5] = { + /* 1.00000000000000000000E0, */ + 1.95107674914060531512E1, + 3.17710311750646984099E2, + 3.03835500874445748734E3, + 2.03665876435770579345E4, + 7.43853965136767874343E4, +}; + +static double TAYLOR0[10] = { + -1.0000000009110164892, + -1.0000000057646759799, + -9.9999983138417361078e-1, + -1.0000013011460139596, + -1.000001940896320456, + -9.9987929950057116496e-1, + -1.000785194477042408, + -1.0031782279542924256, + -9.1893853320467274178e-1, + -1.5, +}; + +#define MAXL2 127 +#define SQRT_2_PI 0.79788456080286535587989 + +extern double MACHEP; + +static double zeta_reflection(double); +static double zetac_smallneg(double); +static double zetac_positive(double); + + +/* + * Riemann zeta function, minus one + */ +double zetac(double x) +{ + if (isnan(x)) { + return x; + } + else if (x == -INFINITY) { + return NAN; + } + else if (x < 0.0 && x > -0.01) { + return zetac_smallneg(x); + } + else if (x < 0.0) { + return zeta_reflection(-x) - 1; + } + else { + return zetac_positive(x); + } +} + + +/* + * Riemann zeta function + */ +double riemann_zeta(double x) +{ + if (isnan(x)) { + return x; + } + else if (x == -INFINITY) { + return NAN; + } + else if (x < 0.0 && x > -0.01) { + return 1 + zetac_smallneg(x); + } + else if (x < 0.0) { + return zeta_reflection(-x); + } + else { + return 1 + zetac_positive(x); + } +} + + +/* + * Compute zetac for positive arguments + */ +static inline double zetac_positive(double x) +{ + int i; + double a, b, s, w; + + if (x == 1.0) { + return INFINITY; + } + + if (x >= MAXL2) { + /* because first term is 2**-x */ + return 0.0; + } + + /* Tabulated values for integer argument */ + w = floor(x); + if (w == x) { + i = x; + if (i < 31) { +#ifdef UNK + return (azetac[i]); +#else + return (*(double *) &azetac[4 * i]); +#endif + } + } + + if (x < 1.0) { + w = 1.0 - x; + a = polevl(x, R, 5) / (w * p1evl(x, S, 5)); + return a; + } + + if (x <= 10.0) { + b = pow(2.0, x) * (x - 1.0); + w = 1.0 / x; + s = (x * polevl(w, P, 8)) / (b * p1evl(w, Q, 8)); + return s; + } + + if (x <= 50.0) { + b = pow(2.0, -x); + w = polevl(x, A, 10) / p1evl(x, B, 10); + w = exp(w) + b; + return w; + } + + /* Basic sum of inverse powers */ + s = 0.0; + a = 1.0; + do { + a += 2.0; + b = pow(a, -x); + s += b; + } + while (b / s > MACHEP); + + b = pow(2.0, -x); + s = (s + b) / (1.0 - b); + return s; +} + + +/* + * Compute zetac for small negative x. We can't use the reflection + * formula because to double precision 1 - x = 1 and zetac(1) = inf. + */ +static inline double zetac_smallneg(double x) +{ + return polevl(x, TAYLOR0, 9); +} + + +/* + * Compute zetac using the reflection formula (see DLMF 25.4.2) plus + * the Lanczos approximation for Gamma to avoid overflow. + */ +static inline double zeta_reflection(double x) +{ + double base, large_term, small_term, hx, x_shift; + + hx = x / 2; + if (hx == floor(hx)) { + /* Hit a zero of the sine factor */ + return 0; + } + + /* Reduce the argument to sine */ + x_shift = fmod(x, 4); + small_term = -SQRT_2_PI * sin(0.5 * M_PI * x_shift); + small_term *= lanczos_sum_expg_scaled(x + 1) * zeta(x + 1, 1); + + /* Group large terms together to prevent overflow */ + base = (x + lanczos_g + 0.5) / (2 * M_PI * M_E); + large_term = pow(base, x + 0.5); + if (isfinite(large_term)) { + return large_term * small_term; + } + /* + * We overflowed, but we might be able to stave off overflow by + * factoring in the small term earlier. To do this we compute + * + * (sqrt(large_term) * small_term) * sqrt(large_term) + * + * Since we only call this method for negative x bounded away from + * zero, the small term can only be as small sine on that region; + * i.e. about machine epsilon. This means that if the above still + * overflows, then there was truly no avoiding it. + */ + large_term = pow(base, 0.5 * x + 0.25); + return (large_term * small_term) * large_term; +} From 3538488b28cd18fb9b87dc4ce104800005473ee6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 09:30:26 -0500 Subject: [PATCH 22/33] refactor Cephes CMakeLists.txt to allow building from parent directory --- gtsam/3rdparty/cephes/CMakeLists.txt | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt index fdc17ea614..8ee91569bb 100644 --- a/gtsam/3rdparty/cephes/CMakeLists.txt +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -19,6 +19,9 @@ set(CEPHES_HEADER_FILES cephes/polevl.h cephes/sf_error.h) +# Add header files +install(FILES ${CEPHES_HEADER_FILES} DESTINATION include/gtsam/3rdparty/cephes) + set(CEPHES_SOURCES cephes/airy.c cephes/bdtr.c @@ -70,7 +73,6 @@ set(CEPHES_SOURCES cephes/psi.c cephes/rgamma.c cephes/round.c - # cephes/scipy_iv.c cephes/sf_error.c cephes/shichi.c cephes/sici.c @@ -87,16 +89,23 @@ set(CEPHES_SOURCES cephes/zetac.c) # Add library source files -add_library(${PROJECT_NAME} SHARED ${CEPHES_SOURCES}) +add_library(cephes-gtsam SHARED ${CEPHES_SOURCES}) # Add include directory (aka headers) -target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_include_directories( + cephes-gtsam BEFORE PUBLIC $ + $) set_target_properties( - ${PROJECT_NAME} + cephes-gtsam PROPERTIES VERSION ${PROJECT_VERSION} SOVERSION ${PROJECT_VERSION_MAJOR} - PUBLIC_HEADER ${CEPHES_HEADER_FILES} + # PUBLIC_HEADER ${CEPHES_HEADER_FILES} C_STANDARD 99) -install(FILES ${CEPHES_HEADER_FILES} DESTINATION include/gtsam/3rdparty/cephes) +install( + TARGETS cephes-gtsam + EXPORT GTSAM-exports + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) From 5481159f95296193b1accd525bcafaa0617c03ce Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 09:31:08 -0500 Subject: [PATCH 23/33] Link to cephes from gtsam --- CMakeLists.txt | 1 + cmake/HandleCephes.cmake | 47 ++++++++++++++++++++++++++++++++++++++++ gtsam/CMakeLists.txt | 3 +++ 3 files changed, 51 insertions(+) create mode 100644 cmake/HandleCephes.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 16848a7219..8e0497f8df 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,6 +72,7 @@ include(cmake/HandleCCache.cmake) # ccache include(cmake/HandleCPack.cmake) # CPack include(cmake/HandleEigen.cmake) # Eigen3 include(cmake/HandleMetis.cmake) # metis +include(cmake/HandleCephes.cmake) # cephes include(cmake/HandleMKL.cmake) # MKL include(cmake/HandleOpenMP.cmake) # OpenMP include(cmake/HandlePerfTools.cmake) # Google perftools diff --git a/cmake/HandleCephes.cmake b/cmake/HandleCephes.cmake new file mode 100644 index 0000000000..6ef406987f --- /dev/null +++ b/cmake/HandleCephes.cmake @@ -0,0 +1,47 @@ +# ############################################################################## +# Cephes library + +# For both system or bundle version, a cmake target "cephes-gtsam-if" is defined +# (interface library) + +option( + GTSAM_USE_SYSTEM_CEPHES + "Find and use system-installed cephes. If 'off', use the one bundled with GTSAM" + OFF) + +if(GTSAM_USE_SYSTEM_CEPHES) + # # Debian package: libmetis-dev + + # find_path(METIS_INCLUDE_DIR metis.h REQUIRED) find_library(METIS_LIBRARY + # metis REQUIRED) + + # if(METIS_INCLUDE_DIR AND METIS_LIBRARY) mark_as_advanced(METIS_INCLUDE_DIR) + # mark_as_advanced(METIS_LIBRARY) + + # add_library(cephes-gtsam-if INTERFACE) + # target_include_directories(cephes-gtsam-if BEFORE INTERFACE + # ${METIS_INCLUDE_DIR} # gtsam_unstable/partition/FindSeparator-inl.h uses + # internal metislib.h API # via extern "C" + # $ + # $ ) + # target_link_libraries(cephes-gtsam-if INTERFACE ${METIS_LIBRARY}) endif() + +else() + # Bundled version: + add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/cephes) + + list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam) + set(GTSAM_EXPORTED_TARGETS + "${GTSAM_EXPORTED_TARGETS}" + PARENT_SCOPE) + + add_library(cephes-gtsam-if INTERFACE) + target_link_libraries(cephes-gtsam-if INTERFACE cephes-gtsam) + +endif() + +list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam-if) +install( + TARGETS cephes-gtsam-if + EXPORT GTSAM-exports + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index e35f5aadaa..1fc8e45707 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -110,6 +110,9 @@ if(GTSAM_SUPPORT_NESTED_DISSECTION) list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam-if) endif() +# Link to cephes library +list(APPEND GTSAM_ADDITIONAL_LIBRARIES cephes-gtsam-if) + # Versions set(gtsam_version ${GTSAM_VERSION_STRING}) set(gtsam_soversion ${GTSAM_VERSION_MAJOR}) From ea81675393e3bc13b30f3519c88453363cf2a93b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 09:36:04 -0500 Subject: [PATCH 24/33] minor refactor to be consistent --- gtsam/nonlinear/GncOptimizer.h | 2 +- gtsam/nonlinear/internal/ChiSquaredInverse.h | 45 ++ gtsam/nonlinear/internal/Gamma.h | 537 ------------------ gtsam/nonlinear/internal/Utils.h | 49 -- gtsam/nonlinear/internal/chiSquaredInverse.h | 387 ------------- .../nonlinear/tests/testChiSquaredInverse.cpp | 2 +- 6 files changed, 47 insertions(+), 975 deletions(-) create mode 100644 gtsam/nonlinear/internal/ChiSquaredInverse.h delete mode 100644 gtsam/nonlinear/internal/Gamma.h delete mode 100644 gtsam/nonlinear/internal/Utils.h delete mode 100644 gtsam/nonlinear/internal/chiSquaredInverse.h diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index edcc6f0bb6..0fe576159a 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -28,7 +28,7 @@ #include #include -#include +#include namespace gtsam { /* diff --git a/gtsam/nonlinear/internal/ChiSquaredInverse.h b/gtsam/nonlinear/internal/ChiSquaredInverse.h new file mode 100644 index 0000000000..78eb73f673 --- /dev/null +++ b/gtsam/nonlinear/internal/ChiSquaredInverse.h @@ -0,0 +1,45 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file ChiSquaredInverse.h + * @brief Implementation of the Chi Squared inverse function. + * + * Uses the cephes 3rd party library to help with gamma inverse functions. + * + * @author Varun Agrawal + */ + +#pragma once + +#include + +namespace gtsam { + +namespace internal { + +/** + * @brief Compute the quantile function of the Chi-Squared distribution. + * + * @param dofs Degrees of freedom + * @param alpha Quantile value + * @return double + */ +double chi_squared_quantile(const double dofs, const double alpha) { + // The quantile function of the Chi-squared distribution is the quantile of + // the specific (inverse) incomplete Gamma distribution + // return 2 * internal::igami(dofs / 2, alpha); + return 2 * cephes_igami(dofs / 2, alpha); +} + +} // namespace internal + +} // namespace gtsam diff --git a/gtsam/nonlinear/internal/Gamma.h b/gtsam/nonlinear/internal/Gamma.h deleted file mode 100644 index 6c63ff7cbf..0000000000 --- a/gtsam/nonlinear/internal/Gamma.h +++ /dev/null @@ -1,537 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Gamma.h - * @brief Gamma and Gamma Inverse functions - * - * A lot of this code has been picked up from - * https://www.boost.org/doc/libs/1_83_0/boost/math/special_functions/detail/igamma_inverse.hpp - * - * @author Varun Agrawal - */ - -#pragma once - -#include - -#include -#include - -namespace gtsam { - -namespace internal { - -template -inline constexpr T log_max_value() { - return log(LIM::max()); -} - -/** - * @brief Upper gamma fraction for integer a - * - * @param a - * @param x - * @param pol - * @param pderivative - * @return template - */ -template -inline T finite_gamma_q(T a, T x, Policy const& pol, T* pderivative = 0) { - // Calculates normalised Q when a is an integer: - T e = exp(-x); - T sum = e; - if (sum != 0) { - T term = sum; - for (unsigned n = 1; n < a; ++n) { - term /= n; - term *= x; - sum += term; - } - } - if (pderivative) { - *pderivative = e * pow(x, a) / - boost::math::unchecked_factorial(std::trunc(T(a - 1))); - } - return sum; -} - -/** - * @brief Upper gamma fraction for half integer a - * - * @tparam T - * @tparam Policy - * @param a - * @param x - * @param p_derivative - * @param pol - * @return T - */ -template -T finite_half_gamma_q(T a, T x, T* p_derivative, const Policy& pol) { - // Calculates normalised Q when a is a half-integer: - T e = boost::math::erfc(sqrt(x), pol); - if ((e != 0) && (a > 1)) { - T term = exp(-x) / sqrt(M_PI * x); - term *= x; - static const T half = T(1) / 2; - term /= half; - T sum = term; - for (unsigned n = 2; n < a; ++n) { - term /= n - half; - term *= x; - sum += term; - } - e += sum; - if (p_derivative) { - *p_derivative = 0; - } - } else if (p_derivative) { - // We'll be dividing by x later, so calculate derivative * x: - *p_derivative = sqrt(x) * exp(-x) / sqrt(M_PI); - } - return e; -} - -/** - * @brief Incomplete gamma functions follow - * - * @tparam T - */ -template -struct upper_incomplete_gamma_fract { - private: - T z, a; - int k; - - public: - typedef std::pair result_type; - - upper_incomplete_gamma_fract(T a1, T z1) : z(z1 - a1 + 1), a(a1), k(0) {} - - result_type operator()() { - ++k; - z += 2; - return result_type(k * (a - k), z); - } -}; - -template -inline T upper_gamma_fraction(T a, T z, T eps) { - // Multiply result by z^a * e^-z to get the full - // upper incomplete integral. Divide by tgamma(z) - // to normalise. - upper_incomplete_gamma_fract f(a, z); - return 1 / (z - a + 1 + boost::math::tools::continued_fraction_a(f, eps)); -} - -/** - * @brief Main incomplete gamma entry point, handles all four incomplete - * gamma's: - * - * @tparam T - * @tparam Policy - * @param a - * @param x - * @param normalised - * @param invert - * @param pol - * @param p_derivative - * @return T - */ -template -T gamma_incomplete_imp(T a, T x, bool normalised, bool invert, - const Policy& pol, T* p_derivative) { - if (a <= 0) { - throw std::runtime_error( - "Argument a to the incomplete gamma function must be greater than " - "zero"); - } - if (x < 0) { - throw std::runtime_error( - "Argument x to the incomplete gamma function must be >= 0"); - } - - typedef typename boost::math::lanczos::lanczos::type lanczos_type; - - T result = 0; // Just to avoid warning C4701: potentially uninitialized local - // variable 'result' used - - // max_factorial value for long double is 170 in Boost - if (a >= 170 && !normalised) { - // - // When we're computing the non-normalized incomplete gamma - // and a is large the result is rather hard to compute unless - // we use logs. There are really two options - if x is a long - // way from a in value then we can reliably use methods 2 and 4 - // below in logarithmic form and go straight to the result. - // Otherwise we let the regularized gamma take the strain - // (the result is unlikely to underflow in the central region anyway) - // and combine with lgamma in the hopes that we get a finite result. - // - if (invert && (a * 4 < x)) { - // This is method 4 below, done in logs: - result = a * log(x) - x; - if (p_derivative) *p_derivative = exp(result); - result += log(upper_gamma_fraction( - a, x, boost::math::policies::get_epsilon())); - } else if (!invert && (a > 4 * x)) { - // This is method 2 below, done in logs: - result = a * log(x) - x; - if (p_derivative) *p_derivative = exp(result); - T init_value = 0; - result += log( - boost::math::detail::lower_gamma_series(a, x, pol, init_value) / a); - } else { - result = gamma_incomplete_imp(a, x, true, invert, pol, p_derivative); - if (result == 0) { - if (invert) { - // Try http://functions.wolfram.com/06.06.06.0039.01 - result = 1 + 1 / (12 * a) + 1 / (288 * a * a); - result = log(result) - a + (a - 0.5f) * log(a) + log(sqrt(2 * M_PI)); - if (p_derivative) *p_derivative = exp(a * log(x) - x); - } else { - // This is method 2 below, done in logs, we're really outside the - // range of this method, but since the result is almost certainly - // infinite, we should probably be OK: - result = a * log(x) - x; - if (p_derivative) *p_derivative = exp(result); - T init_value = 0; - result += log( - boost::math::detail::lower_gamma_series(a, x, pol, init_value) / - a); - } - } else { - result = log(result) + boost::math::lgamma(a, pol); - } - } - if (result > log_max_value()) { - throw std::overflow_error( - "gamma_incomplete_imp: result is larger than log of max value"); - } - - return exp(result); - } - - assert((p_derivative == nullptr) || normalised); - - bool is_int, is_half_int; - bool is_small_a = (a < 30) && (a <= x + 1) && (x < log_max_value()); - if (is_small_a) { - T fa = floor(a); - is_int = (fa == a); - is_half_int = is_int ? false : (fabs(fa - a) == 0.5f); - } else { - is_int = is_half_int = false; - } - - int eval_method; - - if (is_int && (x > 0.6)) { - // calculate Q via finite sum: - invert = !invert; - eval_method = 0; - } else if (is_half_int && (x > 0.2)) { - // calculate Q via finite sum for half integer a: - invert = !invert; - eval_method = 1; - } else if ((x < boost::math::tools::root_epsilon()) && (a > 1)) { - eval_method = 6; - } else if ((x > 1000) && ((a < x) || (fabs(a - 50) / x < 1))) { - // calculate Q via asymptotic approximation: - invert = !invert; - eval_method = 7; - } else if (x < T(0.5)) { - // - // Changeover criterion chosen to give a changeover at Q ~ 0.33 - // - if (T(-0.4) / log(x) < a) { - eval_method = 2; - } else { - eval_method = 3; - } - } else if (x < T(1.1)) { - // - // Changeover here occurs when P ~ 0.75 or Q ~ 0.25: - // - if (x * 0.75f < a) { - eval_method = 2; - } else { - eval_method = 3; - } - } else { - // - // Begin by testing whether we're in the "bad" zone - // where the result will be near 0.5 and the usual - // series and continued fractions are slow to converge: - // - bool use_temme = false; - if (normalised && std::numeric_limits::is_specialized && (a > 20)) { - T sigma = fabs((x - a) / a); - if ((a > 200) && (boost::math::policies::digits() <= 113)) { - // - // This limit is chosen so that we use Temme's expansion - // only if the result would be larger than about 10^-6. - // Below that the regular series and continued fractions - // converge OK, and if we use Temme's method we get increasing - // errors from the dominant erfc term as it's (inexact) argument - // increases in magnitude. - // - if (20 / a > sigma * sigma) use_temme = true; - } else if (boost::math::policies::digits() <= 64) { - // Note in this zone we can't use Temme's expansion for - // types longer than an 80-bit real: - // it would require too many terms in the polynomials. - if (sigma < 0.4) use_temme = true; - } - } - if (use_temme) { - eval_method = 5; - } else { - // - // Regular case where the result will not be too close to 0.5. - // - // Changeover here occurs at P ~ Q ~ 0.5 - // Note that series computation of P is about x2 faster than continued - // fraction calculation of Q, so try and use the CF only when really - // necessary, especially for small x. - // - if (x - (1 / (3 * x)) < a) { - eval_method = 2; - } else { - eval_method = 4; - invert = !invert; - } - } - } - - switch (eval_method) { - case 0: { - result = finite_gamma_q(a, x, pol, p_derivative); - if (!normalised) result *= boost::math::tgamma(a, pol); - break; - } - case 1: { - result = - boost::math::detail::finite_half_gamma_q(a, x, p_derivative, pol); - if (!normalised) result *= boost::math::tgamma(a, pol); - if (p_derivative && (*p_derivative == 0)) - *p_derivative = boost::math::detail::regularised_gamma_prefix( - a, x, pol, lanczos_type()); - break; - } - case 2: { - // Compute P: - result = normalised ? boost::math::detail::regularised_gamma_prefix( - a, x, pol, lanczos_type()) - : boost::math::detail::full_igamma_prefix(a, x, pol); - if (p_derivative) *p_derivative = result; - if (result != 0) { - // - // If we're going to be inverting the result then we can - // reduce the number of series evaluations by quite - // a few iterations if we set an initial value for the - // series sum based on what we'll end up subtracting it from - // at the end. - // Have to be careful though that this optimization doesn't - // lead to spurious numeric overflow. Note that the - // scary/expensive overflow checks below are more often - // than not bypassed in practice for "sensible" input - // values: - // - T init_value = 0; - bool optimised_invert = false; - if (invert) { - init_value = (normalised ? 1 : boost::math::tgamma(a, pol)); - if (normalised || (result >= 1) || - (LIM::max() * result > init_value)) { - init_value /= result; - if (normalised || (a < 1) || (LIM::max() / a > init_value)) { - init_value *= -a; - optimised_invert = true; - } else - init_value = 0; - } else - init_value = 0; - } - result *= - boost::math::detail::lower_gamma_series(a, x, pol, init_value) / a; - if (optimised_invert) { - invert = false; - result = -result; - } - } - break; - } - case 3: { - // Compute Q: - invert = !invert; - T g; - result = boost::math::detail::tgamma_small_upper_part( - a, x, pol, &g, invert, p_derivative); - invert = false; - if (normalised) result /= g; - break; - } - case 4: { - // Compute Q: - result = normalised ? boost::math::detail::regularised_gamma_prefix( - a, x, pol, lanczos_type()) - : boost::math::detail::full_igamma_prefix(a, x, pol); - if (p_derivative) *p_derivative = result; - if (result != 0) - result *= upper_gamma_fraction( - a, x, boost::math::policies::get_epsilon()); - break; - } - case 5: { - // - // Use compile time dispatch to the appropriate - // Temme asymptotic expansion. This may be dead code - // if T does not have numeric limits support, or has - // too many digits for the most precise version of - // these expansions, in that case we'll be calling - // an empty function. - // - typedef typename boost::math::policies::precision::type - precision_type; - - typedef std::integral_constant - tag_type; - - result = boost::math::detail::igamma_temme_large( - a, x, pol, static_cast(nullptr)); - if (x >= a) invert = !invert; - if (p_derivative) - *p_derivative = boost::math::detail::regularised_gamma_prefix( - a, x, pol, lanczos_type()); - break; - } - case 6: { - // x is so small that P is necessarily very small too, - // use - // http://functions.wolfram.com/GammaBetaErf/GammaRegularized/06/01/05/01/01/ - if (!normalised) - result = pow(x, a) / (a); - else { - try { - result = pow(x, a) / boost::math::tgamma(a + 1, pol); - } catch (const std::overflow_error&) { - result = 0; - } - } - result *= 1 - a * x / (a + 1); - if (p_derivative) - *p_derivative = boost::math::detail::regularised_gamma_prefix( - a, x, pol, lanczos_type()); - break; - } - case 7: { - // x is large, - // Compute Q: - result = normalised ? boost::math::detail::regularised_gamma_prefix( - a, x, pol, lanczos_type()) - : boost::math::detail::full_igamma_prefix(a, x, pol); - if (p_derivative) *p_derivative = result; - result /= x; - if (result != 0) - result *= boost::math::detail::incomplete_tgamma_large_x(a, x, pol); - break; - } - } - - if (normalised && (result > 1)) result = 1; - if (invert) { - T gam = normalised ? 1 : boost::math::tgamma(a, pol); - result = gam - result; - } - if (p_derivative) { - // - // Need to convert prefix term to derivative: - // - if ((x < 1) && (LIM::max() * x < *p_derivative)) { - // overflow, just return an arbitrarily large value: - *p_derivative = LIM::max() / 2; - } - - *p_derivative /= x; - } - - return result; -} - -/** - * @brief Functional to compute the gamma inverse. - * Mainly used with Halley iteration. - * - * @tparam T - */ -template -struct gamma_p_inverse_func { - gamma_p_inverse_func(T a_, T p_, bool inv) : a(a_), p(p_), invert(inv) { - /* - If p is too near 1 then P(x) - p suffers from cancellation - errors causing our root-finding algorithms to "thrash", better - to invert in this case and calculate Q(x) - (1-p) instead. - - Of course if p is *very* close to 1, then the answer we get will - be inaccurate anyway (because there's not enough information in p) - but at least we will converge on the (inaccurate) answer quickly. - */ - if (p > T(0.9)) { - p = 1 - p; - invert = !invert; - } - } - - std::tuple operator()(const T& x) const { - // Calculate P(x) - p and the first two derivates, or if the invert - // flag is set, then Q(x) - q and it's derivatives. - T f, f1; - T ft; - boost::math::policies::policy<> pol; - f = static_cast( - internal::gamma_incomplete_imp(a, x, true, invert, pol, &ft)); - f1 = ft; - T f2; - T div = (a - x - 1) / x; - f2 = f1; - - if (fabs(div) > 1) { - if (internal::LIM::max() / fabs(div) < f2) { - // overflow: - f2 = -internal::LIM::max() / 2; - } else { - f2 *= div; - } - } else { - f2 *= div; - } - - if (invert) { - f1 = -f1; - f2 = -f2; - } - - return std::make_tuple(static_cast(f - p), f1, f2); - } - - private: - T a, p; - bool invert; -}; - -} // namespace internal -} // namespace gtsam diff --git a/gtsam/nonlinear/internal/Utils.h b/gtsam/nonlinear/internal/Utils.h deleted file mode 100644 index 23573346cb..0000000000 --- a/gtsam/nonlinear/internal/Utils.h +++ /dev/null @@ -1,49 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file Utils.h - * @brief Utilities for the Chi Squared inverse and related operations. - * @author Varun Agrawal - */ - -#pragma once - -namespace gtsam { -namespace internal { - -/// Template type for numeric limits -template -using LIM = std::numeric_limits; - -template -using return_t = - typename std::conditional::value, double, T>::type; - -/// Get common type amongst all arguments -template -using common_t = typename std::common_type::type; - -/// Helper template for finding common return type -template -using common_return_t = return_t>; - -/// Check if integer is odd -constexpr bool is_odd(const long long int x) noexcept { return (x & 1U) != 0; } - -/// Templated check for NaN -template -constexpr bool is_nan(const T x) noexcept { - return x != x; -} - -} // namespace internal -} // namespace gtsam diff --git a/gtsam/nonlinear/internal/chiSquaredInverse.h b/gtsam/nonlinear/internal/chiSquaredInverse.h deleted file mode 100644 index d4f79147af..0000000000 --- a/gtsam/nonlinear/internal/chiSquaredInverse.h +++ /dev/null @@ -1,387 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/** - * @file chiSquaredInverse.h - * @brief This file contains an implementation of the Chi Squared inverse - * function, which is implemented similar to Boost with additional template - * parameter helpers. - * - * A lot of this code has been picked up from - * https://www.boost.org/doc/libs/1_83_0/boost/math/special_functions/detail/igamma_inverse.hpp - * https://www.boost.org/doc/libs/1_83_0/boost/math/tools/roots.hpp - * - * @author Varun Agrawal - */ - -#pragma once - -#include -#include - -#include - -// TODO(Varun) remove -#include - -namespace gtsam { - -namespace internal { - -/** - * @brief Polynomial evaluation with runtime size. - * - * @tparam T - * @tparam U - */ -template -inline U evaluate_polynomial(const T* poly, U const& z, std::size_t count) { - assert(count > 0); - U sum = static_cast(poly[count - 1]); - for (int i = static_cast(count) - 2; i >= 0; --i) { - sum *= z; - sum += static_cast(poly[i]); - } - return sum; -} - -/** - * @brief Computation of the Incomplete Gamma Function Ratios and their Inverse. - * - * Reference: - * ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR. - * ACM Transactions on Mathematical Software, Vol. 12, No. 4, - * December 1986, Pages 377-393. - * - * See equation 32. - * - * @tparam T - * @param p - * @param q - * @return T - */ -template -T find_inverse_s(T p, T q) { - T t; - if (p < T(0.5)) { - t = sqrt(-2 * log(p)); - } else { - t = sqrt(-2 * log(q)); - } - static const double a[4] = {3.31125922108741, 11.6616720288968, - 4.28342155967104, 0.213623493715853}; - static const double b[5] = {1, 6.61053765625462, 6.40691597760039, - 1.27364489782223, 0.3611708101884203e-1}; - T s = t - internal::evaluate_polynomial(a, t, 4) / - internal::evaluate_polynomial(b, t, 5); - if (p < T(0.5)) s = -s; - return s; -} - -/** - * @brief Computation of the Incomplete Gamma Function Ratios and their Inverse. - * - * Reference: - * ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR. - * ACM Transactions on Mathematical Software, Vol. 12, No. 4, - * December 1986, Pages 377-393. - * - * See equation 34. - * - * @tparam T - * @param a - * @param x - * @param N - * @param tolerance - * @return T - */ -template -T didonato_SN(T a, T x, unsigned N, T tolerance = 0) { - T sum = 1; - if (N >= 1) { - T partial = x / (a + 1); - sum += partial; - for (unsigned i = 2; i <= N; ++i) { - partial *= x / (a + i); - sum += partial; - if (partial < tolerance) break; - } - } - return sum; -} - -/** - * @brief Compute the initial inverse gamma value guess. - * - * We use the implementation in this paper: - * Computation of the Incomplete Gamma Function Ratios and their Inverse - * ARMIDO R. DIDONATO and ALFRED H. MORRIS, JR. - * ACM Transactions on Mathematical Software, Vol. 12, No. 4, - * December 1986, Pages 377-393. - * - * @tparam T - * @param a - * @param p - * @param q - * @param p_has_10_digits - * @return T - */ -template -T find_inverse_gamma(T a, T p, T q, bool* p_has_10_digits) { - T result; - *p_has_10_digits = false; - - // TODO(Varun) replace with egamma_v in C++20 - // Euler-Mascheroni constant - double euler = 0.577215664901532860606512090082402431042159335939923598805; - - if (a == 1) { - result = -log(q); - } else if (a < 1) { - T g = std::tgamma(a); - T b = q * g; - - if ((b > T(0.6)) || ((b >= T(0.45)) && (a >= T(0.3)))) { - // DiDonato & Morris Eq 21: - // - // There is a slight variation from DiDonato and Morris here: - // the first form given here is unstable when p is close to 1, - // making it impossible to compute the inverse of Q(a,x) for small - // q. Fortunately the second form works perfectly well in this case. - T u; - if ((b * q > T(1e-8)) && (q > T(1e-5))) { - u = pow(p * g * a, 1 / a); - } else { - u = exp((-q / a) - euler); - } - result = u / (1 - (u / (a + 1))); - - } else if ((a < 0.3) && (b >= 0.35)) { - // DiDonato & Morris Eq 22: - T t = exp(-euler - b); - T u = t * exp(t); - result = t * exp(u); - - } else if ((b > 0.15) || (a >= 0.3)) { - // DiDonato & Morris Eq 23: - T y = -log(b); - T u = y - (1 - a) * log(y); - result = y - (1 - a) * log(u) - log(1 + (1 - a) / (1 + u)); - - } else if (b > 0.1) { - // DiDonato & Morris Eq 24: - T y = -log(b); - T u = y - (1 - a) * log(y); - result = y - (1 - a) * log(u) - - log((u * u + 2 * (3 - a) * u + (2 - a) * (3 - a)) / - (u * u + (5 - a) * u + 2)); - - } else { - // DiDonato & Morris Eq 25: - T y = -log(b); - T c1 = (a - 1) * log(y); - T c1_2 = c1 * c1; - T c1_3 = c1_2 * c1; - T c1_4 = c1_2 * c1_2; - T a_2 = a * a; - T a_3 = a_2 * a; - - T c2 = (a - 1) * (1 + c1); - T c3 = (a - 1) * (-(c1_2 / 2) + (a - 2) * c1 + (3 * a - 5) / 2); - T c4 = (a - 1) * ((c1_3 / 3) - (3 * a - 5) * c1_2 / 2 + - (a_2 - 6 * a + 7) * c1 + (11 * a_2 - 46 * a + 47) / 6); - T c5 = (a - 1) * (-(c1_4 / 4) + (11 * a - 17) * c1_3 / 6 + - (-3 * a_2 + 13 * a - 13) * c1_2 + - (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2 + - (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12); - - T y_2 = y * y; - T y_3 = y_2 * y; - T y_4 = y_2 * y_2; - result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4); - - if (b < 1e-28f) *p_has_10_digits = true; - } - } else { - // DiDonato and Morris Eq 31: - T s = find_inverse_s(p, q); - - T s_2 = s * s; - T s_3 = s_2 * s; - T s_4 = s_2 * s_2; - T s_5 = s_4 * s; - T ra = sqrt(a); - - T w = a + s * ra + (s * s - 1) / 3; - w += (s_3 - 7 * s) / (36 * ra); - w -= (3 * s_4 + 7 * s_2 - 16) / (810 * a); - w += (9 * s_5 + 256 * s_3 - 433 * s) / (38880 * a * ra); - - if ((a >= 500) && (fabs(1 - w / a) < 1e-6)) { - result = w; - *p_has_10_digits = true; - - } else if (p > 0.5) { - if (w < 3 * a) { - result = w; - - } else { - T D = (std::max)(T(2), T(a * (a - 1))); - T lg = std::lgamma(a); - T lb = log(q) + lg; - if (lb < -D * T(2.3)) { - // DiDonato and Morris Eq 25: - T y = -lb; - T c1 = (a - 1) * log(y); - T c1_2 = c1 * c1; - T c1_3 = c1_2 * c1; - T c1_4 = c1_2 * c1_2; - T a_2 = a * a; - T a_3 = a_2 * a; - - T c2 = (a - 1) * (1 + c1); - T c3 = (a - 1) * (-(c1_2 / 2) + (a - 2) * c1 + (3 * a - 5) / 2); - T c4 = - (a - 1) * ((c1_3 / 3) - (3 * a - 5) * c1_2 / 2 + - (a_2 - 6 * a + 7) * c1 + (11 * a_2 - 46 * a + 47) / 6); - T c5 = (a - 1) * (-(c1_4 / 4) + (11 * a - 17) * c1_3 / 6 + - (-3 * a_2 + 13 * a - 13) * c1_2 + - (2 * a_3 - 25 * a_2 + 72 * a - 61) * c1 / 2 + - (25 * a_3 - 195 * a_2 + 477 * a - 379) / 12); - - T y_2 = y * y; - T y_3 = y_2 * y; - T y_4 = y_2 * y_2; - result = y + c1 + (c2 / y) + (c3 / y_2) + (c4 / y_3) + (c5 / y_4); - - } else { - // DiDonato and Morris Eq 33: - T u = -lb + (a - 1) * log(w) - log(1 + (1 - a) / (1 + w)); - result = -lb + (a - 1) * log(u) - log(1 + (1 - a) / (1 + u)); - } - } - } else { - T z = w; - T ap1 = a + 1; - T ap2 = a + 2; - if (w < 0.15f * ap1) { - // DiDonato and Morris Eq 35: - T v = log(p) + std::lgamma(ap1); - z = exp((v + w) / a); - s = std::log1p(z / ap1 * (1 + z / ap2)); - z = exp((v + z - s) / a); - s = std::log1p(z / ap1 * (1 + z / ap2)); - z = exp((v + z - s) / a); - s = std::log1p(z / ap1 * (1 + z / ap2 * (1 + z / (a + 3)))); - z = exp((v + z - s) / a); - } - - if ((z <= 0.01 * ap1) || (z > 0.7 * ap1)) { - result = z; - if (z <= T(0.002) * ap1) *p_has_10_digits = true; - - } else { - // DiDonato and Morris Eq 36: - T ls = log(didonato_SN(a, z, 100, T(1e-4))); - T v = log(p) + std::lgamma(ap1); - z = exp((v + z - ls) / a); - result = z * (1 - (a * log(z) - z - v + ls) / (a - z)); - } - } - } - return result; -} - -template -T gamma_p_inv_imp(const T a, const T p) { - if (is_nan(a) || is_nan(p)) { - return LIM::quiet_NaN(); - if (a <= T(0)) { - throw std::runtime_error( - "Argument a in the incomplete gamma function inverse must be >= 0."); - } - } else if (p < T(0) || p > T(1)) { - throw std::runtime_error( - "Probability must be in the range [0,1] in the incomplete gamma " - "function inverse."); - } else if (p == T(0)) { - return 0; - } - - // Get an initial guess (https://dl.acm.org/doi/abs/10.1145/22721.23109) - bool has_10_digits = false; - T guess = find_inverse_gamma(a, p, 1 - p, &has_10_digits); - if (has_10_digits) { - return guess; - } - - T lower = LIM::min(); - if (guess <= lower) { - guess = LIM::min(); - } - - // The number of digits to converge to. - // This is an arbitrary but reasonable number, - // though Boost does more sophisticated things - // using the first derivative. - unsigned digits = 25; - - // Number of Halley iterations - uintmax_t max_iter = 200; - - // TODO - // Perform Halley iteration for root-finding to get a more refined answer - // guess = halley_iterate(gamma_p_inverse_func(a, p, false), guess, lower, - // LIM::max(), digits, max_iter); - - // Go ahead and iterate: - guess = boost::math::tools::halley_iterate( - internal::gamma_p_inverse_func(a, p, false), guess, lower, - LIM::max(), digits, max_iter); - - if (guess == lower) { - throw std::runtime_error( - "Expected result known to be non-zero, but is smaller than the " - "smallest available number."); - } - - return guess; -} - -/** - * Compile-time check for inverse incomplete gamma function - * - * @param a a real-valued, non-negative input. - * @param p a real-valued input with values in the unit-interval. - */ -template -constexpr common_return_t incomplete_gamma_inv(const T1 a, - const T2 p) noexcept { - return internal::gamma_p_inv_imp(static_cast>(a), - static_cast>(p)); -} - -/** - * @brief Compute the quantile function of the Chi-Squared distribution. - * - * @param dofs Degrees of freedom - * @param alpha Quantile value - * @return double - */ -double chi_squared_quantile(const double dofs, const double alpha) { - // The quantile function of the Chi-squared distribution is the quantile of - // the specific (inverse) incomplete Gamma distribution - return 2 * incomplete_gamma_inv(dofs / 2, alpha); -} - -} // namespace internal - -} // namespace gtsam diff --git a/gtsam/nonlinear/tests/testChiSquaredInverse.cpp b/gtsam/nonlinear/tests/testChiSquaredInverse.cpp index 8da4df5c2e..b9ff64fdb2 100644 --- a/gtsam/nonlinear/tests/testChiSquaredInverse.cpp +++ b/gtsam/nonlinear/tests/testChiSquaredInverse.cpp @@ -18,7 +18,7 @@ #include #include -#include +#include using namespace gtsam; From 3cde40ddc8c2c0dd9f4ded69c5a2ae65723c791c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 10:06:44 -0500 Subject: [PATCH 25/33] OS-based improved support --- cmake/HandleCephes.cmake | 3 --- gtsam/3rdparty/cephes/CMakeLists.txt | 14 +++++++++++++- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/cmake/HandleCephes.cmake b/cmake/HandleCephes.cmake index 6ef406987f..837f7ad222 100644 --- a/cmake/HandleCephes.cmake +++ b/cmake/HandleCephes.cmake @@ -31,9 +31,6 @@ else() add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/cephes) list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam) - set(GTSAM_EXPORTED_TARGETS - "${GTSAM_EXPORTED_TARGETS}" - PARENT_SCOPE) add_library(cephes-gtsam-if INTERFACE) target_link_libraries(cephes-gtsam-if INTERFACE cephes-gtsam) diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt index 8ee91569bb..e840e9e493 100644 --- a/gtsam/3rdparty/cephes/CMakeLists.txt +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -100,9 +100,21 @@ set_target_properties( cephes-gtsam PROPERTIES VERSION ${PROJECT_VERSION} SOVERSION ${PROJECT_VERSION_MAJOR} - # PUBLIC_HEADER ${CEPHES_HEADER_FILES} C_STANDARD 99) +if(WIN32) + set_target_properties( + cephes-gtsam + PROPERTIES PREFIX "" + COMPILE_FLAGS /w + RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin") +endif() + +if(APPLE) + set_target_properties(cephes-gtsam PROPERTIES INSTALL_NAME_DIR + "${CMAKE_INSTALL_PREFIX}/lib") +endif() + install( TARGETS cephes-gtsam EXPORT GTSAM-exports From 5806f5f98cd5e74aa3ffc50573b7101a8de5caaa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 10:35:44 -0500 Subject: [PATCH 26/33] add M_PI definition if unavailable (e.g. in Windows) --- gtsam/3rdparty/cephes/cephes/mconf.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/3rdparty/cephes/cephes/mconf.h b/gtsam/3rdparty/cephes/cephes/mconf.h index 9f3deb628f..08119ef857 100644 --- a/gtsam/3rdparty/cephes/cephes/mconf.h +++ b/gtsam/3rdparty/cephes/cephes/mconf.h @@ -102,6 +102,10 @@ #define cephes_isfinite(x) isfinite(x) #endif +#if !defined(M_PI) +#define M_PI 3.14159265358979323846 +#endif + /* Constants needed that are not available in the C standard library */ #define SCIPY_EULER 0.577215664901532860606512090082402431 /* Euler constant */ #define SCIPY_El 2.718281828459045235360287471352662498L /* e as long double */ From 8db9e0146a195e9b30a5a5df61e55dc66ef69686 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 12:00:47 -0500 Subject: [PATCH 27/33] additional M_PI definitions --- gtsam/3rdparty/cephes/cephes/mconf.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/3rdparty/cephes/cephes/mconf.h b/gtsam/3rdparty/cephes/cephes/mconf.h index 08119ef857..e200b9b036 100644 --- a/gtsam/3rdparty/cephes/cephes/mconf.h +++ b/gtsam/3rdparty/cephes/cephes/mconf.h @@ -102,9 +102,15 @@ #define cephes_isfinite(x) isfinite(x) #endif +/* M_PI et al. are not defined in math.h in C99, even with _USE_MATH_DEFINES */ #if !defined(M_PI) #define M_PI 3.14159265358979323846 #endif +#ifndef M_PI_2 +#define M_PI_2 1.57079632679489661923 /* pi/2 */ +#define M_1_PI 0.31830988618379067154 /* 1/pi */ +#define M_2_PI 0.63661977236758134308 /* 2/pi */ +#endif /* Constants needed that are not available in the C standard library */ #define SCIPY_EULER 0.577215664901532860606512090082402431 /* Euler constant */ From b1ce501afe385196a0efdd25d0913ead4c82da16 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 12:12:38 -0500 Subject: [PATCH 28/33] hopefully last of M_ definitions --- gtsam/3rdparty/cephes/cephes/mconf.h | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/gtsam/3rdparty/cephes/cephes/mconf.h b/gtsam/3rdparty/cephes/cephes/mconf.h index e200b9b036..c59d17a470 100644 --- a/gtsam/3rdparty/cephes/cephes/mconf.h +++ b/gtsam/3rdparty/cephes/cephes/mconf.h @@ -110,6 +110,19 @@ #define M_PI_2 1.57079632679489661923 /* pi/2 */ #define M_1_PI 0.31830988618379067154 /* 1/pi */ #define M_2_PI 0.63661977236758134308 /* 2/pi */ +#define M_E 2.71828182845904523536 +#define M_LOG2E 1.44269504088896340736 +#define M_LOG10E 0.434294481903251827651 +#define M_LN2 0.693147180559945309417 +#define M_LN10 2.30258509299404568402 +#define M_PI 3.14159265358979323846 +#define M_PI_2 1.57079632679489661923 +#define M_PI_4 0.785398163397448309616 +#define M_1_PI 0.318309886183790671538 +#define M_2_PI 0.636619772367581343076 +#define M_2_SQRTPI 1.12837916709551257390 +#define M_SQRT2 1.41421356237309504880 +#define M_SQRT1_2 0.707106781186547524401 #endif /* Constants needed that are not available in the C standard library */ From 70f1d4a80457b22dea93979e676e173869f3c6b4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 12:51:09 -0500 Subject: [PATCH 29/33] mark GTSAM_EXPORT and update docstring --- gtsam/nonlinear/internal/ChiSquaredInverse.h | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/internal/ChiSquaredInverse.h b/gtsam/nonlinear/internal/ChiSquaredInverse.h index 78eb73f673..5a25a030e0 100644 --- a/gtsam/nonlinear/internal/ChiSquaredInverse.h +++ b/gtsam/nonlinear/internal/ChiSquaredInverse.h @@ -21,6 +21,7 @@ #pragma once #include +#include namespace gtsam { @@ -29,14 +30,19 @@ namespace internal { /** * @brief Compute the quantile function of the Chi-Squared distribution. * + * The quantile function of the Chi-squared distribution is the quantile of + * the specific (inverse) incomplete Gamma distribution. + * + * We have a dedicated function so we can unit test any issues easily while also + * allowing it to be updated in the future without any backwards-compatibility + * issues. + * * @param dofs Degrees of freedom * @param alpha Quantile value * @return double */ -double chi_squared_quantile(const double dofs, const double alpha) { - // The quantile function of the Chi-squared distribution is the quantile of - // the specific (inverse) incomplete Gamma distribution - // return 2 * internal::igami(dofs / 2, alpha); +double GTSAM_EXPORT chi_squared_quantile(const double dofs, + const double alpha) { return 2 * cephes_igami(dofs / 2, alpha); } From 98444aba3e1b0e6ecbfcdced442d9c16ffe14aa7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 13:29:28 -0500 Subject: [PATCH 30/33] another windows fix --- gtsam/nonlinear/internal/ChiSquaredInverse.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/internal/ChiSquaredInverse.h b/gtsam/nonlinear/internal/ChiSquaredInverse.h index 5a25a030e0..e0284c3132 100644 --- a/gtsam/nonlinear/internal/ChiSquaredInverse.h +++ b/gtsam/nonlinear/internal/ChiSquaredInverse.h @@ -13,7 +13,8 @@ * @file ChiSquaredInverse.h * @brief Implementation of the Chi Squared inverse function. * - * Uses the cephes 3rd party library to help with gamma inverse functions. + * Uses the cephes 3rd party library to help with + * incomplete gamma inverse functions. * * @author Varun Agrawal */ @@ -41,9 +42,8 @@ namespace internal { * @param alpha Quantile value * @return double */ -double GTSAM_EXPORT chi_squared_quantile(const double dofs, - const double alpha) { - return 2 * cephes_igami(dofs / 2, alpha); +double chi_squared_quantile(const double dofs, const double alpha) { + return 2 * igami(dofs / 2, alpha); } } // namespace internal From e0b8c5292ade142b25dd521777b2eff02eea210e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 14:05:39 -0500 Subject: [PATCH 31/33] kill testChiSquaredInverse --- gtsam/nonlinear/internal/ChiSquaredInverse.h | 7 ---- .../nonlinear/tests/testChiSquaredInverse.cpp | 37 ------------------- 2 files changed, 44 deletions(-) delete mode 100644 gtsam/nonlinear/tests/testChiSquaredInverse.cpp diff --git a/gtsam/nonlinear/internal/ChiSquaredInverse.h b/gtsam/nonlinear/internal/ChiSquaredInverse.h index e0284c3132..dbf83f92b4 100644 --- a/gtsam/nonlinear/internal/ChiSquaredInverse.h +++ b/gtsam/nonlinear/internal/ChiSquaredInverse.h @@ -22,10 +22,8 @@ #pragma once #include -#include namespace gtsam { - namespace internal { /** @@ -34,10 +32,6 @@ namespace internal { * The quantile function of the Chi-squared distribution is the quantile of * the specific (inverse) incomplete Gamma distribution. * - * We have a dedicated function so we can unit test any issues easily while also - * allowing it to be updated in the future without any backwards-compatibility - * issues. - * * @param dofs Degrees of freedom * @param alpha Quantile value * @return double @@ -47,5 +41,4 @@ double chi_squared_quantile(const double dofs, const double alpha) { } } // namespace internal - } // namespace gtsam diff --git a/gtsam/nonlinear/tests/testChiSquaredInverse.cpp b/gtsam/nonlinear/tests/testChiSquaredInverse.cpp deleted file mode 100644 index b9ff64fdb2..0000000000 --- a/gtsam/nonlinear/tests/testChiSquaredInverse.cpp +++ /dev/null @@ -1,37 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * GTSAM Copyright 2010, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * Authors: Frank Dellaert, et al. (see THANKS for the full author list) - - * See LICENSE for the license information - - * -------------------------------------------------------------------------- */ - -/* - * @file testChiSquaredInverse.cpp - * @date July 10, 2023 - * @author Varun Agrawal - * @brief Tests for Chi-squared distribution. - */ - -#include -#include -#include - -using namespace gtsam; - -/* ************************************************************************* */ -TEST(ChiSquaredInverse, ChiSqInv) { - double barcSq = internal::chi_squared_quantile(2, 0.99); - EXPECT_DOUBLES_EQUAL(9.21034, barcSq, 1e-5); -} - -/* ************************************************************************* */ -int main() { - srand(time(nullptr)); - TestResult tr; - return TestRegistry::runAllTests(tr); -} -/* ************************************************************************* */ From ba93dec850f0ce45b93271158ab9f778cfe7cc53 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 29 Dec 2023 11:18:58 -0500 Subject: [PATCH 32/33] only used built in version of Cephes since there doesn't seem to be an easy packaged version --- cmake/HandleCephes.cmake | 33 ++++----------------------------- 1 file changed, 4 insertions(+), 29 deletions(-) diff --git a/cmake/HandleCephes.cmake b/cmake/HandleCephes.cmake index 837f7ad222..9addddd60f 100644 --- a/cmake/HandleCephes.cmake +++ b/cmake/HandleCephes.cmake @@ -4,38 +4,13 @@ # For both system or bundle version, a cmake target "cephes-gtsam-if" is defined # (interface library) -option( - GTSAM_USE_SYSTEM_CEPHES - "Find and use system-installed cephes. If 'off', use the one bundled with GTSAM" - OFF) -if(GTSAM_USE_SYSTEM_CEPHES) - # # Debian package: libmetis-dev +add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/cephes) - # find_path(METIS_INCLUDE_DIR metis.h REQUIRED) find_library(METIS_LIBRARY - # metis REQUIRED) +list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam) - # if(METIS_INCLUDE_DIR AND METIS_LIBRARY) mark_as_advanced(METIS_INCLUDE_DIR) - # mark_as_advanced(METIS_LIBRARY) - - # add_library(cephes-gtsam-if INTERFACE) - # target_include_directories(cephes-gtsam-if BEFORE INTERFACE - # ${METIS_INCLUDE_DIR} # gtsam_unstable/partition/FindSeparator-inl.h uses - # internal metislib.h API # via extern "C" - # $ - # $ ) - # target_link_libraries(cephes-gtsam-if INTERFACE ${METIS_LIBRARY}) endif() - -else() - # Bundled version: - add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/cephes) - - list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam) - - add_library(cephes-gtsam-if INTERFACE) - target_link_libraries(cephes-gtsam-if INTERFACE cephes-gtsam) - -endif() +add_library(cephes-gtsam-if INTERFACE) +target_link_libraries(cephes-gtsam-if INTERFACE cephes-gtsam) list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam-if) install( From 687667add2da920f0cc3121bf460437e85afa576 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 29 Dec 2023 13:44:52 -0500 Subject: [PATCH 33/33] minor formatting --- CMakeLists.txt | 2 +- gtsam/3rdparty/cephes/cephes/cephes_names.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e0497f8df..66b1804aff 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,7 +72,7 @@ include(cmake/HandleCCache.cmake) # ccache include(cmake/HandleCPack.cmake) # CPack include(cmake/HandleEigen.cmake) # Eigen3 include(cmake/HandleMetis.cmake) # metis -include(cmake/HandleCephes.cmake) # cephes +include(cmake/HandleCephes.cmake) # cephes include(cmake/HandleMKL.cmake) # MKL include(cmake/HandleOpenMP.cmake) # OpenMP include(cmake/HandlePerfTools.cmake) # Google perftools diff --git a/gtsam/3rdparty/cephes/cephes/cephes_names.h b/gtsam/3rdparty/cephes/cephes/cephes_names.h index 5322feb384..94be8c880a 100644 --- a/gtsam/3rdparty/cephes/cephes/cephes_names.h +++ b/gtsam/3rdparty/cephes/cephes/cephes_names.h @@ -73,7 +73,7 @@ #define psi cephes_psi #define rgamma cephes_rgamma #define riemann_zeta cephes_riemann_zeta -// #define round cephes_round +// #define round cephes_round // Commented out since it clashes with std::round #define shichi cephes_shichi #define sici cephes_sici #define radian cephes_radian