From d0b3f1dd25a7e7b7e4e49bbde024e9fa00170ac9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 May 2023 14:52:13 -0400 Subject: [PATCH 01/73] 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 000000000..399da2c99 --- /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/73] 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 399da2c99..38185249c 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); -} +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_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_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); -} + 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); + } -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)); -} + 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_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))); -} + 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_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_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); -} + 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); + } -// reverse cf expansion -// see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ + // 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_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)); -} + /** + * @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_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)); -} + // 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); + } -// cf expansion -// see: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ + 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_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 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 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)); -} + 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/73] 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 38185249c..92e2599a5 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/73] remove recursion for Halley update --- gtsam/nonlinear/GncHelpers.h | 87 +++++++++++++----------------------- 1 file changed, 32 insertions(+), 55 deletions(-) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 92e2599a5..1340958a3 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 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 a Degrees of freedom * @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); + 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 + * @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/73] 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 efd0e9dc2..40028ad00 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 d59eb4371..414bee5eb 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 d7b68c4ec..66812d6bb 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/73] 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 1340958a3..6852e8131 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/73] try older version --- gtsam/nonlinear/GncHelpers.h | 618 +++++++++++++++++------------------ 1 file changed, 307 insertions(+), 311 deletions(-) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 6852e8131..2dac9ac5e 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); - } +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); +} - /// 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); - } +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); +} - 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); - } +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); +} - 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_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)); +} - 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)); - } +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))); +} - 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; - } +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))); +} - /** - * @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; - } - } +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); +} - /** - * @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); - } +// reverse cf expansion +// see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ - /** - * @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); - } - } +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)); +} - /** - * @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); - } +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)); +} - 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); - } - } -}; +// 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; + T(0.001308L) * t_val * t_val * t_val)); +} - 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)); - } - } - } +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)); +} - /** - * @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); - } +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); +} - /** - * @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)); - } +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))); +} - /** - * @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))); - } +// initial value - /** - * @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); - } +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))); +} - /** - * @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); - } +// +// Halley recursion - /** - * @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))); - } +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); +} - /** - * @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)); - } +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)); +} - 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)); - } - } -}; +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/73] 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 10ac80663..b792fa058 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 5424a5744..28261683b 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/73] 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 2dac9ac5e..7a27a3530 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 000000000..6e47f97cc --- /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/73] 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 b792fa058..10ac80663 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 28261683b..5424a5744 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/73] 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 10ac80663..b1237b790 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 5424a5744..4e0ebf516 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/73] 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/73] 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/73] 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 21d4e826b..45c674401 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 7a27a3530..3729bf754 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 6e47f97cc..65c3b417d 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/73] much improved chi_squared_quantile, incremental update --- gtsam/nonlinear/GncOptimizer.h | 2 +- gtsam/nonlinear/internal/chiSquaredInverse.h | 531 +++--------------- .../nonlinear/tests/testChiSquaredInverse.cpp | 2 +- 3 files changed, 94 insertions(+), 441 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 45c674401..edcc6f0bb 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 3729bf754..528694284 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 +#include +#include + #include +// TODO(Varun) remove +#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>; - -/// 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 { - 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)) { - 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 > -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)); - } -} - -// 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)); - } else { - return 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 { - 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 -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 { +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() > 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)); + 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_inv_type_check(const T1 a, const T2 p) noexcept { - return incomplete_gamma_inv_check(static_cast(a), static_cast(p)); -} + // 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); -} // namespace internal + T lower = LIM::min(); + if (guess <= lower) { + guess = LIM::min(); + } + + // 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 { + 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( + 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."); + } + + return guess; +} /** - * 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 65c3b417d..8da4df5c2 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/73] 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 528694284..dc8595846 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/73] 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 000000000..325fb7eee --- /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 000000000..23573346c --- /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 dc8595846..b7744ffa2 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/73] 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 b7744ffa2..7577721dc 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 4711f5807dbe9afe78f5ee9b82b0d3ada650432a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 11 Nov 2023 10:31:42 -0500 Subject: [PATCH 19/73] discrete error method that returns an ADT --- gtsam/discrete/DecisionTreeFactor.cpp | 16 ++++++++++++++++ gtsam/discrete/DecisionTreeFactor.h | 3 +++ gtsam/discrete/DiscreteFactor.h | 15 ++++++++++----- gtsam/discrete/TableFactor.cpp | 5 +++++ gtsam/discrete/TableFactor.h | 3 +++ .../discrete/tests/testDecisionTreeFactor.cpp | 18 ++++++++++++++++++ gtsam_unstable/discrete/AllDiff.h | 5 +++++ gtsam_unstable/discrete/BinaryAllDiff.h | 5 +++++ gtsam_unstable/discrete/Domain.h | 5 +++++ gtsam_unstable/discrete/SingleValue.h | 5 +++++ 10 files changed, 75 insertions(+), 5 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 7202e6853..cbb26016c 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -62,6 +62,22 @@ namespace gtsam { return error(values.discrete()); } + /* ************************************************************************ */ + AlgebraicDecisionTree DecisionTreeFactor::error() const { + // Get all possible assignments + DiscreteKeys dkeys = discreteKeys(); + // Reverse to make cartesian product output a more natural ordering. + DiscreteKeys rdkeys(dkeys.rbegin(), dkeys.rend()); + const auto assignments = DiscreteValues::CartesianProduct(rdkeys); + + // Construct vector with error values + std::vector errors; + for (const auto& assignment : assignments) { + errors.push_back(error(assignment)); + } + return AlgebraicDecisionTree(dkeys, errors); + } + /* ************************************************************************ */ double DecisionTreeFactor::safe_div(const double& a, const double& b) { // The use for safe_div is when we divide the product factor by the sum diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index a9a7e5ce0..5e0acc056 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -292,6 +292,9 @@ namespace gtsam { */ double error(const HybridValues& values) const override; + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree error() const override; + /// @} private: diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 24b2b55e4..e84533655 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -18,9 +18,10 @@ #pragma once +#include +#include #include #include -#include #include namespace gtsam { @@ -35,7 +36,7 @@ class HybridValues; * * @ingroup discrete */ -class GTSAM_EXPORT DiscreteFactor: public Factor { +class GTSAM_EXPORT DiscreteFactor : public Factor { public: // typedefs needed to play nice with gtsam typedef DiscreteFactor This; ///< This class @@ -103,7 +104,11 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { */ double error(const HybridValues& c) const override; - /// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor + /// Compute error for each assignment and return as a tree + virtual AlgebraicDecisionTree error() const = 0; + + /// Multiply in a DecisionTreeFactor and return the result as + /// DecisionTreeFactor virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0; virtual DecisionTreeFactor toDecisionTreeFactor() const = 0; @@ -111,7 +116,7 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { /// @} /// @name Wrapper support /// @{ - + /// Translation table from values to strings. using Names = DiscreteValues::Names; @@ -175,4 +180,4 @@ template<> struct traits : public Testable {}; std::vector expNormalize(const std::vector &logProbs); -}// namespace gtsam +} // namespace gtsam diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index f4e023a4d..be5f2af5b 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -168,6 +168,11 @@ double TableFactor::error(const HybridValues& values) const { return error(values.discrete()); } +/* ************************************************************************ */ +AlgebraicDecisionTree TableFactor::error() const { + return toDecisionTreeFactor().error(); +} + /* ************************************************************************ */ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const { return toDecisionTreeFactor() * f; diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 828e794e6..40ed231fd 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -358,6 +358,9 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { */ double error(const HybridValues& values) const override; + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree error() const override; + /// @} }; diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index 9d73475a3..69ee52662 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -67,6 +67,24 @@ TEST( DecisionTreeFactor, constructors) EXPECT_DOUBLES_EQUAL(0.8, f4(x121), 1e-9); } +/* ************************************************************************* */ +TEST(DecisionTreeFactor, Error) { + // Declare a bunch of keys + DiscreteKey X(0,2), Y(1,3), Z(2,2); + + // Create factors + DecisionTreeFactor f(X & Y & Z, "2 5 3 6 4 7 25 55 35 65 45 75"); + + auto errors = f.error(); + // regression + AlgebraicDecisionTree expected( + {X, Y, Z}, + vector{-0.69314718, -1.6094379, -1.0986123, -1.7917595, + -1.3862944, -1.9459101, -3.2188758, -4.0073332, -3.5553481, + -4.1743873, -3.8066625, -4.3174881}); + EXPECT(assert_equal(expected, errors, 1e-6)); +} + /* ************************************************************************* */ TEST(DecisionTreeFactor, multiplication) { DiscreteKey v0(0, 2), v1(1, 2), v2(2, 2); diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 9496fc1a6..9c8e62ecd 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -53,6 +53,11 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { /// Multiply into a decisiontree DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree error() const override { + throw std::runtime_error("AllDiff::error not implemented"); + } + /* * Ensure Arc-consistency by checking every possible value of domain j. * @param j domain to be checked diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index b207acb9d..33f6562b4 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -91,6 +91,11 @@ class BinaryAllDiff : public Constraint { const Domains&) const override { throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); } + + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree error() const override { + throw std::runtime_error("BinaryAllDiff::error not implemented"); + } }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 0f5b5fdf9..ca7340a9f 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -69,6 +69,11 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { } } + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree error() const override { + throw std::runtime_error("Domain::error not implemented"); + } + // Return concise string representation, mostly to debug arc consistency. // Converts from base 0 to base1. std::string base1Str() const; diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 1c726d4d0..f57f24b42 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -49,6 +49,11 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { } } + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree error() const override { + throw std::runtime_error("SingleValue::error not implemented"); + } + /// Calculate value double operator()(const DiscreteValues& values) const override; From 114a0b220b430e96b9d61962a53a6a48aa36cc86 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 12 Nov 2023 22:32:58 -0500 Subject: [PATCH 20/73] printErrors method for HybridGaussianFactorGraph --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 81 +++++++++++++++++++++- gtsam/hybrid/HybridGaussianFactorGraph.h | 16 ++++- 2 files changed, 93 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 7a7ca0cbf..6dd7186e2 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -74,6 +74,86 @@ const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph) { index, KeyVector(discrete_keys.begin(), discrete_keys.end()), true); } +/* ************************************************************************ */ +void HybridGaussianFactorGraph::printErrors( + const HybridValues &values, const std::string &str, + const KeyFormatter &keyFormatter, + const std::function + &printCondition) const { + std::cout << str << "size: " << size() << std::endl << std::endl; + + std::stringstream ss; + + for (size_t i = 0; i < factors_.size(); i++) { + auto &&factor = factors_[i]; + std::cout << "Factor " << i << ": "; + + // Clear the stringstream + ss.str(std::string()); + + if (auto gmf = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + gmf->error(values.continuous()).print("", DefaultKeyFormatter); + std::cout << std::endl; + } + } else if (auto hc = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + + if (hc->isContinuous()) { + std::cout << "error = " << hc->asGaussian()->error(values) << "\n"; + } else if (hc->isDiscrete()) { + std::cout << "error = "; + hc->asDiscrete()->error().print("", DefaultKeyFormatter); + std::cout << "\n"; + } else { + // Is hybrid + std::cout << "error = "; + hc->asMixture()->error(values.continuous()).print(); + std::cout << "\n"; + } + } + } else if (auto gf = std::dynamic_pointer_cast(factor)) { + const double errorValue = (factor != nullptr ? gf->error(values) : .0); + if (!printCondition(factor.get(), errorValue, i)) + continue; // User-provided filter did not pass + + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = " << errorValue << "\n"; + } + } else if (auto df = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + df->error().print("", DefaultKeyFormatter); + std::cout << std::endl; + } + + } else { + continue; + } + + std::cout << "\n"; + } + std::cout.flush(); +} + /* ************************************************************************ */ static GaussianFactorGraphTree addGaussian( const GaussianFactorGraphTree &gfgTree, @@ -96,7 +176,6 @@ static GaussianFactorGraphTree addGaussian( // TODO(dellaert): it's probably more efficient to first collect the discrete // keys, and then loop over all assignments to populate a vector. GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { - GaussianFactorGraphTree result; for (auto &f : factors_) { diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index b3f159150..bfbd7ab3c 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -140,9 +140,19 @@ class GTSAM_EXPORT HybridGaussianFactorGraph /// @{ // TODO(dellaert): customize print and equals. - // void print(const std::string& s = "HybridGaussianFactorGraph", - // const KeyFormatter& keyFormatter = DefaultKeyFormatter) const - // override; + // void print( + // const std::string& s = "HybridGaussianFactorGraph", + // const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + + void printErrors( + const HybridValues& values, + const std::string& str = "HybridGaussianFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const std::function& + printCondition = + [](const Factor*, double, size_t) { return true; }) const; + // bool equals(const This& fg, double tol = 1e-9) const override; /// @} From b2ab23375087ddc45209d6d22c6614f8e8f21709 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 12 Nov 2023 22:33:16 -0500 Subject: [PATCH 21/73] printErrors method for HybridNonlinearFactorGraph --- gtsam/hybrid/HybridNonlinearFactorGraph.cpp | 92 +++++++++++++++++++ gtsam/hybrid/HybridNonlinearFactorGraph.h | 12 ++- .../tests/testHybridNonlinearFactorGraph.cpp | 31 +++++-- 3 files changed, 128 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index e51adb9cd..c2a8f81a4 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -42,6 +42,98 @@ void HybridNonlinearFactorGraph::print(const std::string& s, } } +/* ************************************************************************* */ +void HybridNonlinearFactorGraph::printErrors( + const HybridValues& values, const std::string& str, + const KeyFormatter& keyFormatter, + const std::function& printCondition) const { + std::cout << str << "size: " << size() << std::endl << std::endl; + + std::stringstream ss; + + for (size_t i = 0; i < factors_.size(); i++) { + auto&& factor = factors_[i]; + std::cout << "Factor " << i << ": "; + + // Clear the stringstream + ss.str(std::string()); + + if (auto mf = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + mf->error(values.nonlinear()).print("", DefaultKeyFormatter); + std::cout << std::endl; + } + } else if (auto gmf = + std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + gmf->error(values.continuous()).print("", DefaultKeyFormatter); + std::cout << std::endl; + } + } else if (auto gm = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + gm->error(values.continuous()).print("", DefaultKeyFormatter); + std::cout << std::endl; + } + } else if (auto nf = std::dynamic_pointer_cast(factor)) { + const double errorValue = (factor != nullptr ? nf->error(values) : .0); + if (!printCondition(factor.get(), errorValue, i)) + continue; // User-provided filter did not pass + + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = " << errorValue << "\n"; + } + } else if (auto gf = std::dynamic_pointer_cast(factor)) { + const double errorValue = (factor != nullptr ? gf->error(values) : .0); + if (!printCondition(factor.get(), errorValue, i)) + continue; // User-provided filter did not pass + + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = " << errorValue << "\n"; + } + } else if (auto df = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + df->error().print("", DefaultKeyFormatter); + std::cout << std::endl; + } + + } else { + continue; + } + + std::cout << "\n"; + } + std::cout.flush(); +} + /* ************************************************************************* */ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize( const Values& continuousValues) const { diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index 89b4fb391..54dc9e93f 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -34,7 +34,7 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { protected: public: using Base = HybridFactorGraph; - using This = HybridNonlinearFactorGraph; ///< this class + using This = HybridNonlinearFactorGraph; ///< this class using shared_ptr = std::shared_ptr; ///< shared_ptr to This using Values = gtsam::Values; ///< backwards compatibility @@ -63,6 +63,16 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { const std::string& s = "HybridNonlinearFactorGraph", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + /** print errors along with factors*/ + void printErrors( + const HybridValues& values, + const std::string& str = "HybridNonlinearFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const std::function& + printCondition = + [](const Factor*, double, size_t) { return true; }) const; + /// @} /// @name Standard Interface /// @{ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index a493de5c5..c801f0bd7 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -327,8 +327,8 @@ GaussianFactorGraph::shared_ptr batchGFG(double between, NonlinearFactorGraph graph; graph.addPrior(X(0), 0, Isotropic::Sigma(1, 0.1)); - auto between_x0_x1 = std::make_shared( - X(0), X(1), between, Isotropic::Sigma(1, 1.0)); + auto between_x0_x1 = std::make_shared(X(0), X(1), between, + Isotropic::Sigma(1, 1.0)); graph.push_back(between_x0_x1); @@ -397,6 +397,25 @@ TEST(HybridFactorGraph, Partial_Elimination) { EXPECT(remainingFactorGraph->at(2)->keys() == KeyVector({M(0), M(1)})); } +TEST(HybridFactorGraph, PrintErrors) { + Switching self(3); + + // Get nonlinear factor graph and add linear factors to be holistic + HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph; + fg.add(self.linearizedFactorGraph); + + // Optimize to get HybridValues + HybridBayesNet::shared_ptr bn = + self.linearizedFactorGraph.eliminateSequential(); + HybridValues hv = bn->optimize(); + + // Print and verify + fg.print(); + std::cout << "\n\n\n" << std::endl; + fg.printErrors( + HybridValues(hv.continuous(), DiscreteValues(), self.linearizationPoint)); +} + /**************************************************************************** * Test full elimination */ @@ -564,7 +583,7 @@ factor 6: P( m1 | m0 ): )"; #else -string expected_hybridFactorGraph = R"( + string expected_hybridFactorGraph = R"( size: 7 factor 0: A[x0] = [ @@ -759,9 +778,9 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { KeyVector contKeys = {X(0), X(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); auto still = std::make_shared(X(0), X(1), Pose2(0, 0, 0), - noise_model), + noise_model), moving = std::make_shared(X(0), X(1), odometry, - noise_model); + noise_model); std::vector motion_models = {still, moving}; fg.emplace_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models); @@ -788,7 +807,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { initialEstimate.insert(L(1), Point2(4.1, 1.8)); // We want to eliminate variables not connected to DCFactors first. - const Ordering ordering {L(0), L(1), X(0), X(1)}; + const Ordering ordering{L(0), L(1), X(0), X(1)}; HybridGaussianFactorGraph linearized = *fg.linearize(initialEstimate); From 5387299b8b3a0e5ee3865a66183b67cf01207ed6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 14 Nov 2023 04:48:23 -0500 Subject: [PATCH 22/73] use correct key formatter --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 6 +++--- gtsam/hybrid/HybridNonlinearFactorGraph.cpp | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 6dd7186e2..3b2d4a024 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -99,7 +99,7 @@ void HybridGaussianFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - gmf->error(values.continuous()).print("", DefaultKeyFormatter); + gmf->error(values.continuous()).print("", keyFormatter); std::cout << std::endl; } } else if (auto hc = std::dynamic_pointer_cast(factor)) { @@ -113,7 +113,7 @@ void HybridGaussianFactorGraph::printErrors( std::cout << "error = " << hc->asGaussian()->error(values) << "\n"; } else if (hc->isDiscrete()) { std::cout << "error = "; - hc->asDiscrete()->error().print("", DefaultKeyFormatter); + hc->asDiscrete()->error().print("", keyFormatter); std::cout << "\n"; } else { // Is hybrid @@ -141,7 +141,7 @@ void HybridGaussianFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - df->error().print("", DefaultKeyFormatter); + df->error().print("", keyFormatter); std::cout << std::endl; } diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index c2a8f81a4..e0dfd413c 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -66,7 +66,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - mf->error(values.nonlinear()).print("", DefaultKeyFormatter); + mf->error(values.nonlinear()).print("", keyFormatter); std::cout << std::endl; } } else if (auto gmf = @@ -77,7 +77,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - gmf->error(values.continuous()).print("", DefaultKeyFormatter); + gmf->error(values.continuous()).print("", keyFormatter); std::cout << std::endl; } } else if (auto gm = std::dynamic_pointer_cast(factor)) { @@ -87,7 +87,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - gm->error(values.continuous()).print("", DefaultKeyFormatter); + gm->error(values.continuous()).print("", keyFormatter); std::cout << std::endl; } } else if (auto nf = std::dynamic_pointer_cast(factor)) { @@ -121,7 +121,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - df->error().print("", DefaultKeyFormatter); + df->error().print("", keyFormatter); std::cout << std::endl; } From 95a534e7c13d77ad0b670affe93b185e16ee24ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 16 Nov 2023 17:16:05 -0500 Subject: [PATCH 23/73] error method for HybridBayesNet --- gtsam/hybrid/HybridBayesNet.cpp | 30 +++++++++++++++++ gtsam/hybrid/HybridBayesNet.h | 10 ++++++ gtsam/hybrid/tests/testHybridBayesNet.cpp | 39 +++++++++++++++++++++++ 3 files changed, 79 insertions(+) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 3bafe5a9c..31177ddb7 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -281,6 +281,36 @@ HybridValues HybridBayesNet::sample() const { return sample(&kRandomNumberGenerator); } +/* ************************************************************************* */ +AlgebraicDecisionTree HybridBayesNet::error( + const VectorValues &continuousValues) const { + AlgebraicDecisionTree result(0.0); + + // Iterate over each conditional. + for (auto &&conditional : *this) { + if (auto gm = conditional->asMixture()) { + // If conditional is hybrid, compute error for all assignments. + result = result + gm->error(continuousValues); + + } else if (auto gc = conditional->asGaussian()) { + // If continuous, get the error and add it to the result + double error = gc->error(continuousValues); + // Add the computed error to every leaf of the result tree. + result = result.apply( + [error](double leaf_value) { return leaf_value + error; }); + + } else if (auto dc = conditional->asDiscrete()) { + // If discrete, add the discrete error in the right branch + result = result.apply( + [dc](const Assignment &assignment, double leaf_value) { + return leaf_value + dc->error(DiscreteValues(assignment)); + }); + } + } + + return result; +} + /* ************************************************************************* */ AlgebraicDecisionTree HybridBayesNet::logProbability( const VectorValues &continuousValues) const { diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index e71cfe9b4..2934ef176 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -187,6 +187,16 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * @param continuousValues Continuous values at which to compute the error. * @return AlgebraicDecisionTree */ + AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + + /** + * @brief Compute log probability for each discrete assignment, + * and return as a tree. + * + * @param continuousValues Continuous values at which + * to compute the log probability. + * @return AlgebraicDecisionTree + */ AlgebraicDecisionTree logProbability( const VectorValues &continuousValues) const; diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 5248fce01..66985cc78 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -153,6 +153,45 @@ TEST(HybridBayesNet, Choose) { *gbn.at(3))); } +/* ****************************************************************************/ +// Test error for a hybrid Bayes net P(X0|X1) P(X1|Asia) P(Asia). +TEST(HybridBayesNet, Error) { + const auto continuousConditional = GaussianConditional::sharedMeanAndStddev( + X(0), 2 * I_1x1, X(1), Vector1(-4.0), 5.0); + + const SharedDiagonal model0 = noiseModel::Diagonal::Sigmas(Vector1(2.0)), + model1 = noiseModel::Diagonal::Sigmas(Vector1(3.0)); + + const auto conditional0 = std::make_shared( + X(1), Vector1::Constant(5), I_1x1, model0), + conditional1 = std::make_shared( + X(1), Vector1::Constant(2), I_1x1, model1); + + auto gm = + new GaussianMixture({X(1)}, {}, {Asia}, {conditional0, conditional1}); + // Create hybrid Bayes net. + HybridBayesNet bayesNet; + bayesNet.push_back(continuousConditional); + bayesNet.emplace_back(gm); + bayesNet.emplace_back(new DiscreteConditional(Asia, "99/1")); + + // Create values at which to evaluate. + HybridValues values; + values.insert(asiaKey, 0); + values.insert(X(0), Vector1(-6)); + values.insert(X(1), Vector1(1)); + + AlgebraicDecisionTree actual_errors = + bayesNet.error(values.continuous()); + + // Regression. + // Manually added all the error values from the 3 conditional types. + AlgebraicDecisionTree expected_errors( + {Asia}, std::vector{2.33005033585, 5.38619084965}); + + EXPECT(assert_equal(expected_errors, actual_errors)); +} + /* ****************************************************************************/ // Test Bayes net optimize TEST(HybridBayesNet, OptimizeAssignment) { From a2ba56de27d39d4ea9527fd8be8d382af104ee35 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 16 Nov 2023 17:36:41 -0500 Subject: [PATCH 24/73] include BayesNet::error in HybridBayesNet --- gtsam/hybrid/HybridBayesNet.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 2934ef176..f84eaa70d 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -189,6 +189,9 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { */ AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + /// Error method using HybridValues which returns specific error. + using Base::error; + /** * @brief Compute log probability for each discrete assignment, * and return as a tree. From eabd11df6fdf339cfbc87b36d4064ae4177b5d09 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Nov 2023 10:34:11 -0500 Subject: [PATCH 25/73] better docstring for BayesNet::error --- gtsam/hybrid/HybridBayesNet.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index f84eaa70d..22e03bba9 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -189,7 +189,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { */ AlgebraicDecisionTree error(const VectorValues &continuousValues) const; - /// Error method using HybridValues which returns specific error. + /** + * @brief Error method using HybridValues which returns specific error for + * assignment. + */ using Base::error; /** From 7d7e83348dd469761aff0d84ea0d7ae44df786cf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Nov 2023 13:56:19 -0500 Subject: [PATCH 26/73] add test case for HybridGaussianFactorGraph::printErrors --- .../tests/testHybridGaussianFactorGraph.cpp | 98 ++++++++++++++++++- 1 file changed, 97 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index b240e1626..daee44ab6 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -658,7 +658,7 @@ bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements, } /* ****************************************************************************/ -// Check that the factor graph unnormalized probability is proportional to the +// Check that the bayes net unnormalized probability is proportional to the // Bayes net probability for the given measurements. bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements, const HybridBayesNet &posterior, size_t num_samples = 100) { @@ -907,6 +907,102 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { EXPECT(ratioTest(bn, measurements, *posterior)); } +/* ****************************************************************************/ +// Test printErrors with multivariate example. +TEST(HybridGaussianFactorGraph, PrintErrors) { + HybridGaussianFactorGraph hfg; + HybridBayesNet bayesNet; + + size_t num_measurements = 1; + // Create Gaussian mixture z_i = x0 + noise for each measurement. + for (size_t i = 0; i < num_measurements; i++) { + const DiscreteKey mode_i{M(i), 2}; + bayesNet.emplace_back(new GaussianMixture( + {Z(i)}, {X(0)}, {mode_i}, + {GaussianConditional::sharedMeanAndStddev(Z(i), I_3x3, X(0), Z_3x1, 10), + GaussianConditional::sharedMeanAndStddev(Z(i), I_3x3, X(0), Z_3x1, + 0.1)})); + } + + // Create prior on X(0). + bayesNet.push_back(GaussianConditional::sharedMeanAndStddev( + X(0), Vector3(1.0, 2.0, 5.0), 0.5)); + + // Add prior on mode. + const size_t nrModes = 1; + for (size_t i = 0; i < nrModes; i++) { + bayesNet.emplace_back(new DiscreteConditional({M(i), 2}, "4/6")); + } + + VectorValues measurements{{Z(0), Vector3(1.0, 2.0, 5.0)}}; + HybridGaussianFactorGraph measurement_fg = + bayesNet.toFactorGraph(measurements); + HybridValues values = bayesNet.optimize(); + + std::stringstream buffer; + // Save the original output stream so we can reset later + std::streambuf *old = std::cout.rdbuf(buffer.rdbuf()); + + // We test against actual std::cout for faithful reproduction + measurement_fg.printErrors(values); + + // Get output string and reset stdout + std::string actual = buffer.str(); + std::cout.rdbuf(old); + + std::string expected = R"(HybridGaussianFactorGraph: size: 3 + +Factor 0: Hybrid [x0; m0]{ + Choice(m0) + 0 Leaf : + A[x0] = [ + -1, -0, -0; + -0, -1, -0; + -0, -0, -1; + 0, 0, 0 +] + b = [ -1 -2 -5 5.25652 ] + Noise model: diagonal sigmas [10; 10; 10; 1]; + + 1 Leaf : + A[x0] = [ + -1, -0, -0; + -0, -1, -0; + -0, -0, -1 +] + b = [ -1 -2 -5 ] +isotropic dim=3 sigma=0.1 + +} +error = Choice(m0) + 0 Leaf 13.815511 + 1 Leaf 0 + + +Factor 1: p(x0) + R = [ 1 0 0 ] + [ 0 1 0 ] + [ 0 0 1 ] + d = [ 1 2 5 ] + mean: 1 elements + x0: 1 2 5 +isotropic dim=3 sigma=0.5 +error = 0 + +Factor 2: P( m0 ): + Choice(m0) + 0 Leaf 0.4 + 1 Leaf 0.6 + +error = Choice(m0) + 0 Leaf 0.91629073 + 1 Leaf 0.51082562 + + +)"; + EXPECT(expected == actual); +} + /* ************************************************************************* */ int main() { TestResult tr; From cd5c13065b77c8c90a0f4ca2fecb79a06b9df637 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Nov 2023 13:58:52 -0500 Subject: [PATCH 27/73] remove extra newline --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 3b2d4a024..467cff710 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -142,7 +142,6 @@ void HybridGaussianFactorGraph::printErrors( factor->print(ss.str(), keyFormatter); std::cout << "error = "; df->error().print("", keyFormatter); - std::cout << std::endl; } } else { From 9dad12eabf8dbf81c2b28a9a16f4d1b5f09a1d7d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 21 Nov 2023 16:19:19 -0500 Subject: [PATCH 28/73] comment out print --- gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index c801f0bd7..93081d309 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -410,10 +410,10 @@ TEST(HybridFactorGraph, PrintErrors) { HybridValues hv = bn->optimize(); // Print and verify - fg.print(); - std::cout << "\n\n\n" << std::endl; - fg.printErrors( - HybridValues(hv.continuous(), DiscreteValues(), self.linearizationPoint)); + // fg.print(); + // std::cout << "\n\n\n" << std::endl; + // fg.printErrors( + // HybridValues(hv.continuous(), DiscreteValues(), self.linearizationPoint)); } /**************************************************************************** From cf42a0819d6b03b1cf86eb48dbdd196663c71d73 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 25 Nov 2023 15:13:18 -0500 Subject: [PATCH 29/73] remove test case --- .../tests/testHybridGaussianFactorGraph.cpp | 96 ------------------- 1 file changed, 96 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index daee44ab6..99c2299c0 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -907,102 +907,6 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { EXPECT(ratioTest(bn, measurements, *posterior)); } -/* ****************************************************************************/ -// Test printErrors with multivariate example. -TEST(HybridGaussianFactorGraph, PrintErrors) { - HybridGaussianFactorGraph hfg; - HybridBayesNet bayesNet; - - size_t num_measurements = 1; - // Create Gaussian mixture z_i = x0 + noise for each measurement. - for (size_t i = 0; i < num_measurements; i++) { - const DiscreteKey mode_i{M(i), 2}; - bayesNet.emplace_back(new GaussianMixture( - {Z(i)}, {X(0)}, {mode_i}, - {GaussianConditional::sharedMeanAndStddev(Z(i), I_3x3, X(0), Z_3x1, 10), - GaussianConditional::sharedMeanAndStddev(Z(i), I_3x3, X(0), Z_3x1, - 0.1)})); - } - - // Create prior on X(0). - bayesNet.push_back(GaussianConditional::sharedMeanAndStddev( - X(0), Vector3(1.0, 2.0, 5.0), 0.5)); - - // Add prior on mode. - const size_t nrModes = 1; - for (size_t i = 0; i < nrModes; i++) { - bayesNet.emplace_back(new DiscreteConditional({M(i), 2}, "4/6")); - } - - VectorValues measurements{{Z(0), Vector3(1.0, 2.0, 5.0)}}; - HybridGaussianFactorGraph measurement_fg = - bayesNet.toFactorGraph(measurements); - HybridValues values = bayesNet.optimize(); - - std::stringstream buffer; - // Save the original output stream so we can reset later - std::streambuf *old = std::cout.rdbuf(buffer.rdbuf()); - - // We test against actual std::cout for faithful reproduction - measurement_fg.printErrors(values); - - // Get output string and reset stdout - std::string actual = buffer.str(); - std::cout.rdbuf(old); - - std::string expected = R"(HybridGaussianFactorGraph: size: 3 - -Factor 0: Hybrid [x0; m0]{ - Choice(m0) - 0 Leaf : - A[x0] = [ - -1, -0, -0; - -0, -1, -0; - -0, -0, -1; - 0, 0, 0 -] - b = [ -1 -2 -5 5.25652 ] - Noise model: diagonal sigmas [10; 10; 10; 1]; - - 1 Leaf : - A[x0] = [ - -1, -0, -0; - -0, -1, -0; - -0, -0, -1 -] - b = [ -1 -2 -5 ] -isotropic dim=3 sigma=0.1 - -} -error = Choice(m0) - 0 Leaf 13.815511 - 1 Leaf 0 - - -Factor 1: p(x0) - R = [ 1 0 0 ] - [ 0 1 0 ] - [ 0 0 1 ] - d = [ 1 2 5 ] - mean: 1 elements - x0: 1 2 5 -isotropic dim=3 sigma=0.5 -error = 0 - -Factor 2: P( m0 ): - Choice(m0) - 0 Leaf 0.4 - 1 Leaf 0.6 - -error = Choice(m0) - 0 Leaf 0.91629073 - 1 Leaf 0.51082562 - - -)"; - EXPECT(expected == actual); -} - /* ************************************************************************* */ int main() { TestResult tr; From fcda1536c6c97bac6c514717342ce0c7fbef3b98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 22 Dec 2023 14:53:45 -0800 Subject: [PATCH 30/73] Cleaner version of eliminate --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 102 +++++++++++---------- 1 file changed, 56 insertions(+), 46 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 2029b48e0..7eaefbf85 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -96,7 +96,6 @@ static GaussianFactorGraphTree addGaussian( // TODO(dellaert): it's probably more efficient to first collect the discrete // keys, and then loop over all assignments to populate a vector. GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { - GaussianFactorGraphTree result; for (auto &f : factors_) { @@ -198,6 +197,51 @@ GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) { } /* ************************************************************************ */ +using Result = std::pair, + GaussianMixtureFactor::sharedFactor>; + +// Integrate the probability mass in the last continuous conditional using +// the unnormalized probability q(μ;m) = exp(-error(μ;m)) at the mean. +// discrete_probability = exp(-error(μ;m)) * sqrt(det(2π Σ_m)) +static std::shared_ptr createDiscreteFactor( + const DecisionTree &eliminationResults, + const DiscreteKeys &discreteSeparator) { + auto probability = [&](const Result &pair) -> double { + const auto &[conditional, factor] = pair; + static const VectorValues kEmpty; + // If the factor is not null, it has no keys, just contains the residual. + if (!factor) return 1.0; // TODO(dellaert): not loving this. + return exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); + }; + + DecisionTree probabilities(eliminationResults, probability); + + return std::make_shared(discreteSeparator, probabilities); +} + +// Create GaussianMixtureFactor on the separator, taking care to correct +// for conditional constants. +static std::shared_ptr createGaussianMixtureFactor( + const DecisionTree &eliminationResults, + const KeyVector &continuousSeparator, + const DiscreteKeys &discreteSeparator) { + // Correct for the normalization constant used up by the conditional + auto correct = [&](const Result &pair) -> GaussianFactor::shared_ptr { + const auto &[conditional, factor] = pair; + if (factor) { + auto hf = std::dynamic_pointer_cast(factor); + if (!hf) throw std::runtime_error("Expected HessianFactor!"); + hf->constantTerm() += 2.0 * conditional->logNormalizationConstant(); + } + return factor; + }; + DecisionTree newFactors(eliminationResults, + correct); + + return std::make_shared(continuousSeparator, + discreteSeparator, newFactors); +} + static std::pair> hybridElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys, @@ -217,9 +261,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // FG has a nullptr as we're looping over the factors. factorGraphTree = removeEmpty(factorGraphTree); - using Result = std::pair, - GaussianMixtureFactor::sharedFactor>; - // This is the elimination method on the leaf nodes auto eliminate = [&](const GaussianFactorGraph &graph) -> Result { if (graph.empty()) { @@ -234,53 +275,22 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // Perform elimination! DecisionTree eliminationResults(factorGraphTree, eliminate); - // Separate out decision tree into conditionals and remaining factors. - const auto [conditionals, newFactors] = unzip(eliminationResults); + // If there are no more continuous parents we create a DiscreteFactor with the + // error for each discrete choice. Otherwise, create a GaussianMixtureFactor + // on the separator, taking care to correct for conditional constants. + auto newFactor = + continuousSeparator.empty() + ? createDiscreteFactor(eliminationResults, discreteSeparator) + : createGaussianMixtureFactor(eliminationResults, continuousSeparator, + discreteSeparator); // Create the GaussianMixture from the conditionals + GaussianMixture::Conditionals conditionals( + eliminationResults, [](const Result &pair) { return pair.first; }); auto gaussianMixture = std::make_shared( frontalKeys, continuousSeparator, discreteSeparator, conditionals); - if (continuousSeparator.empty()) { - // If there are no more continuous parents, then we create a - // DiscreteFactor here, with the error for each discrete choice. - - // Integrate the probability mass in the last continuous conditional using - // the unnormalized probability q(μ;m) = exp(-error(μ;m)) at the mean. - // discrete_probability = exp(-error(μ;m)) * sqrt(det(2π Σ_m)) - auto probability = [&](const Result &pair) -> double { - static const VectorValues kEmpty; - // If the factor is not null, it has no keys, just contains the residual. - const auto &factor = pair.second; - if (!factor) return 1.0; // TODO(dellaert): not loving this. - return exp(-factor->error(kEmpty)) / pair.first->normalizationConstant(); - }; - - DecisionTree probabilities(eliminationResults, probability); - - return { - std::make_shared(gaussianMixture), - std::make_shared(discreteSeparator, probabilities)}; - } else { - // Otherwise, we create a resulting GaussianMixtureFactor on the separator, - // taking care to correct for conditional constant. - - // Correct for the normalization constant used up by the conditional - auto correct = [&](const Result &pair) { - const auto &factor = pair.second; - if (!factor) return; - auto hf = std::dynamic_pointer_cast(factor); - if (!hf) throw std::runtime_error("Expected HessianFactor!"); - hf->constantTerm() += 2.0 * pair.first->logNormalizationConstant(); - }; - eliminationResults.visit(correct); - - const auto mixtureFactor = std::make_shared( - continuousSeparator, discreteSeparator, newFactors); - - return {std::make_shared(gaussianMixture), - mixtureFactor}; - } + return {std::make_shared(gaussianMixture), newFactor}; } /* ************************************************************************ From 203a84dc0ec32526f9e98af68c10a4191a57e5f9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Dec 2023 17:12:51 -0500 Subject: [PATCH 31/73] 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 325fb7eee..edd2b2071 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 7577721dc..d4f79147a 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 32/73] 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 edd2b2071..6c63ff7cb 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 33/73] 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 000000000..fdc17ea61 --- /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 000000000..629733eef --- /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 000000000..95e16a55f --- /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 000000000..29fcdf1af --- /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 000000000..a58fe2037 --- /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 000000000..c0389deea --- /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 000000000..fa115c7b7 --- /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 000000000..a83c07834 --- /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 000000000..5322feb38 --- /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 000000000..a0e9c5c52 --- /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 000000000..d576e7a8d --- /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 000000000..8631554cc --- /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 000000000..7049f191e --- /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 000000000..fec97c478 --- /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 000000000..c37f57a7b --- /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 000000000..4e09da143 --- /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 000000000..d2b9ac1d6 --- /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 000000000..8a2823f3a --- /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 000000000..ee73e062a --- /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 000000000..1ef8e0c12 --- /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 000000000..6891a8244 --- /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 000000000..3842a7403 --- /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 000000000..f7f49284c --- /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 000000000..0a71d3c52 --- /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 000000000..14911f59c --- /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 000000000..2a6ee14c0 --- /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 000000000..8ced02687 --- /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 000000000..9c119ed8f --- /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 000000000..50620fa2e --- /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 000000000..2a61defed --- /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 000000000..9d74318ff --- /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 000000000..597c8d4d9 --- /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 000000000..7f0a84d02 --- /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 000000000..ac23e7133 --- /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 000000000..4e85d556e --- /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 000000000..4553873f2 --- /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 000000000..75f871ec5 --- /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 000000000..0bc310633 --- /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 000000000..97fc93ff4 --- /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 000000000..b03427f4f --- /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 000000000..747c43f53 --- /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 000000000..094ef6cef --- /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 000000000..123194de8 --- /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 000000000..3434c18f3 --- /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 000000000..c5b31a1bf --- /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 000000000..fc33e5c0e --- /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 000000000..ff7584a15 --- /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 000000000..2135e0ebb --- /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 000000000..f92a8d208 --- /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 000000000..92ab8c1b2 --- /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 000000000..9f3deb628 --- /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 000000000..7697f257e --- /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 000000000..168e98b5a --- /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 000000000..e7fe5cce0 --- /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 000000000..6eb063510 --- /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 000000000..0249074d9 --- /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 000000000..4c04fa14e --- /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 000000000..eb23ddf88 --- /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 000000000..190c6d162 --- /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 000000000..6420ccaa9 --- /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 000000000..0ed1f1415 --- /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 000000000..e7bb22011 --- /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 000000000..95a47c797 --- /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 000000000..43986df81 --- /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 000000000..75104e724 --- /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 000000000..7bb79bc25 --- /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 000000000..d9c37ebdb --- /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 000000000..f0e52f990 --- /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 000000000..48e1c4087 --- /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 000000000..5a37536be --- /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 000000000..26c86fa2d --- /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 000000000..1ea86329b --- /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 000000000..751314a87 --- /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 000000000..76bc7f08d --- /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 000000000..c02ff0acd --- /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 000000000..e61a15521 --- /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 000000000..554933a24 --- /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 000000000..841433183 --- /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 34/73] 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 fdc17ea61..8ee91569b 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 35/73] 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 16848a721..8e0497f8d 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 000000000..6ef406987 --- /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 e35f5aada..1fc8e4570 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 36/73] 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 edcc6f0bb..0fe576159 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 000000000..78eb73f67 --- /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 6c63ff7cb..000000000 --- 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 23573346c..000000000 --- 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 d4f79147a..000000000 --- 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 8da4df5c2..b9ff64fdb 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 37/73] 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 6ef406987..837f7ad22 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 8ee91569b..e840e9e49 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 38/73] 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 9f3deb628..08119ef85 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 39/73] 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 08119ef85..e200b9b03 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 40/73] 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 e200b9b03..c59d17a47 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 41/73] 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 78eb73f67..5a25a030e 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 42/73] 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 5a25a030e..e0284c313 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 43/73] 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 e0284c313..dbf83f92b 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 b9ff64fdb..000000000 --- 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 44/73] 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 837f7ad22..9addddd60 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 45/73] 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 8e0497f8d..66b1804af 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 5322feb38..94be8c880 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 From b15bcb166e4efb633714236a51877fab2eef4447 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 29 Dec 2023 14:43:39 -0500 Subject: [PATCH 46/73] replace structured binding with exclusive access --- gtsam/navigation/NavState.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 4410d629c..3e2817752 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -106,7 +106,8 @@ bool NavState::equals(const NavState& other, double tol) const { //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { - auto [nRb, n_t, n_v] = (*this); + Rot3 nRb = R_; + Point3 n_t = t_, n_v = v_; Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0); const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0); From ddd38abe6b591baf265d99c03909f1cd8a3c5064 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 12:19:45 -0500 Subject: [PATCH 47/73] add Cephes README and License --- gtsam/3rdparty/cephes/LICENSE.txt | 7 +++++++ gtsam/3rdparty/cephes/README.md | 22 ++++++++++++++++++++++ 2 files changed, 29 insertions(+) create mode 100644 gtsam/3rdparty/cephes/LICENSE.txt create mode 100644 gtsam/3rdparty/cephes/README.md diff --git a/gtsam/3rdparty/cephes/LICENSE.txt b/gtsam/3rdparty/cephes/LICENSE.txt new file mode 100644 index 000000000..6c2842a96 --- /dev/null +++ b/gtsam/3rdparty/cephes/LICENSE.txt @@ -0,0 +1,7 @@ +MIT License + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +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 AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. \ No newline at end of file diff --git a/gtsam/3rdparty/cephes/README.md b/gtsam/3rdparty/cephes/README.md new file mode 100644 index 000000000..63e3a6c8c --- /dev/null +++ b/gtsam/3rdparty/cephes/README.md @@ -0,0 +1,22 @@ +# README + +This is a vendored version of the Cephes Mathematical Library. The source code can be found on [netlib.org](https://www.netlib.org/cephes/). + +The software is provided with an [MIT License](https://smath.com/en-US/view/CephesMathLibrary/license). + +## Original Readme + +Some software in this archive may be from the book _Methods and +Programs for Mathematical Functions_ (Prentice-Hall or Simon & Schuster +International, 1989) or from the Cephes Mathematical Library, a +commercial product. In either event, it is copyrighted by the author. +What you see here may be used freely but it comes with no support or +guarantee. + +The two known misprints in the book are repaired here in the +source listings for the gamma function and the incomplete beta +integral. + + +Stephen L. Moshier +moshier@na-net.ornl.gov From 04bee2b8738ea193a453e37f7ed47cd48f94bd49 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 12:37:50 -0500 Subject: [PATCH 48/73] add cross platform extern marking to cephes.h --- gtsam/3rdparty/cephes/cephes.h | 235 +++++++++++++++--------------- gtsam/3rdparty/cephes/dllexport.h | 25 ++++ 2 files changed, 146 insertions(+), 114 deletions(-) create mode 100644 gtsam/3rdparty/cephes/dllexport.h diff --git a/gtsam/3rdparty/cephes/cephes.h b/gtsam/3rdparty/cephes/cephes.h index 629733eef..d5b59d895 100644 --- a/gtsam/3rdparty/cephes/cephes.h +++ b/gtsam/3rdparty/cephes/cephes.h @@ -2,159 +2,166 @@ #define CEPHES_H #include "cephes/cephes_names.h" +#include "dllexport.h" #ifdef __cplusplus extern "C" { #endif -extern int airy(double x, double *ai, double *aip, double *bi, double *bip); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double bdtrc(double k, int n, double p); +CEPHES_EXTERN_EXPORT double bdtr(double k, int n, double p); +CEPHES_EXTERN_EXPORT double bdtri(double k, int n, double y); -extern double besselpoly(double a, double lambda, double nu); +CEPHES_EXTERN_EXPORT double besselpoly(double a, double lambda, double nu); -extern double beta(double a, double b); -extern double lbeta(double a, double b); +CEPHES_EXTERN_EXPORT double beta(double a, double b); +CEPHES_EXTERN_EXPORT double lbeta(double a, double b); -extern double btdtr(double a, double b, double x); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double cbrt(double x); +CEPHES_EXTERN_EXPORT double chbevl(double x, double array[], int n); +CEPHES_EXTERN_EXPORT double chdtrc(double df, double x); +CEPHES_EXTERN_EXPORT double chdtr(double df, double x); +CEPHES_EXTERN_EXPORT double chdtri(double df, double y); +CEPHES_EXTERN_EXPORT double dawsn(double xx); -extern double ellie(double phi, double m); -extern double ellik(double phi, double m); -extern double ellpe(double x); +CEPHES_EXTERN_EXPORT double ellie(double phi, double m); +CEPHES_EXTERN_EXPORT double ellik(double phi, double m); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT int ellpj(double u, double m, double *sn, double *cn, + double *dn, double *ph); +CEPHES_EXTERN_EXPORT double ellpk(double x); +CEPHES_EXTERN_EXPORT double exp10(double x); +CEPHES_EXTERN_EXPORT double exp2(double x); -extern double expn(int n, double x); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double fdtrc(double a, double b, double x); +CEPHES_EXTERN_EXPORT double fdtr(double a, double b, double x); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT int fresnl(double xxa, double *ssa, double *cca); +CEPHES_EXTERN_EXPORT double Gamma(double x); +CEPHES_EXTERN_EXPORT double lgam(double x); +CEPHES_EXTERN_EXPORT double lgam_sgn(double x, int *sign); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double gdtr(double a, double b, double x); +CEPHES_EXTERN_EXPORT double gdtrc(double a, double b, double x); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double hyp2f1(double a, double b, double c, double x); +CEPHES_EXTERN_EXPORT double hyperg(double a, double b, double x); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double i0(double x); +CEPHES_EXTERN_EXPORT double i0e(double x); +CEPHES_EXTERN_EXPORT double i1(double x); +CEPHES_EXTERN_EXPORT double i1e(double x); +CEPHES_EXTERN_EXPORT double igamc(double a, double x); +CEPHES_EXTERN_EXPORT double igam(double a, double x); +CEPHES_EXTERN_EXPORT double igam_fac(double a, double x); +CEPHES_EXTERN_EXPORT double igamci(double a, double q); +CEPHES_EXTERN_EXPORT double igami(double a, double p); -extern double incbet(double aa, double bb, double xx); -extern double incbi(double aa, double bb, double yy0); +CEPHES_EXTERN_EXPORT double incbet(double aa, double bb, double xx); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double iv(double v, double x); +CEPHES_EXTERN_EXPORT double j0(double x); +CEPHES_EXTERN_EXPORT double y0(double x); +CEPHES_EXTERN_EXPORT double j1(double x); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double jn(int n, double x); +CEPHES_EXTERN_EXPORT double jv(double n, double x); +CEPHES_EXTERN_EXPORT double k0(double x); +CEPHES_EXTERN_EXPORT double k0e(double x); +CEPHES_EXTERN_EXPORT double k1(double x); +CEPHES_EXTERN_EXPORT double k1e(double x); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double nbdtrc(int k, int n, double p); +CEPHES_EXTERN_EXPORT double nbdtr(int k, int n, double p); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double ndtr(double a); +CEPHES_EXTERN_EXPORT double log_ndtr(double a); +CEPHES_EXTERN_EXPORT double erfc(double a); +CEPHES_EXTERN_EXPORT double erf(double x); +CEPHES_EXTERN_EXPORT double erfinv(double y); +CEPHES_EXTERN_EXPORT double erfcinv(double y); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double pdtrc(double k, double m); +CEPHES_EXTERN_EXPORT double pdtr(double k, double m); +CEPHES_EXTERN_EXPORT double pdtri(int k, double y); -extern double poch(double x, double m); +CEPHES_EXTERN_EXPORT double poch(double x, double m); -extern double psi(double x); +CEPHES_EXTERN_EXPORT double psi(double x); -extern double rgamma(double x); -extern double round(double x); +CEPHES_EXTERN_EXPORT double rgamma(double x); +CEPHES_EXTERN_EXPORT double round(double x); -extern int shichi(double x, double *si, double *ci); -extern int sici(double x, double *si, double *ci); +CEPHES_EXTERN_EXPORT int shichi(double x, double *si, double *ci); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double radian(double d, double m, double s); +CEPHES_EXTERN_EXPORT double sindg(double x); +CEPHES_EXTERN_EXPORT double sinpi(double x); +CEPHES_EXTERN_EXPORT double cosdg(double x); +CEPHES_EXTERN_EXPORT double cospi(double x); -extern double spence(double x); +CEPHES_EXTERN_EXPORT double spence(double x); -extern double stdtr(int k, double t); -extern double stdtri(int k, double p); +CEPHES_EXTERN_EXPORT double stdtr(int k, double t); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double struve_h(double v, double x); +CEPHES_EXTERN_EXPORT double struve_l(double v, double x); +CEPHES_EXTERN_EXPORT double struve_power_series(double v, double x, int is_h, + double *err); +CEPHES_EXTERN_EXPORT double struve_asymp_large_z(double v, double z, int is_h, + double *err); +CEPHES_EXTERN_EXPORT double struve_bessel_series(double v, double z, int is_h, + double *err); -extern double yv(double v, double x); +CEPHES_EXTERN_EXPORT double yv(double v, double x); -extern double tandg(double x); -extern double cotdg(double x); +CEPHES_EXTERN_EXPORT double tandg(double x); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double log1p(double x); +CEPHES_EXTERN_EXPORT double log1pmx(double x); +CEPHES_EXTERN_EXPORT double expm1(double x); +CEPHES_EXTERN_EXPORT double cosm1(double x); +CEPHES_EXTERN_EXPORT double lgam1p(double x); -extern double yn(int n, double x); -extern double zeta(double x, double q); -extern double zetac(double x); +CEPHES_EXTERN_EXPORT double yn(int n, double x); +CEPHES_EXTERN_EXPORT double zeta(double x, double q); +CEPHES_EXTERN_EXPORT 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); +CEPHES_EXTERN_EXPORT double smirnov(int n, double d); +CEPHES_EXTERN_EXPORT double smirnovi(int n, double p); +CEPHES_EXTERN_EXPORT double smirnovp(int n, double d); +CEPHES_EXTERN_EXPORT double smirnovc(int n, double d); +CEPHES_EXTERN_EXPORT double smirnovci(int n, double p); +CEPHES_EXTERN_EXPORT double kolmogorov(double x); +CEPHES_EXTERN_EXPORT double kolmogi(double p); +CEPHES_EXTERN_EXPORT double kolmogp(double x); +CEPHES_EXTERN_EXPORT double kolmogc(double x); +CEPHES_EXTERN_EXPORT double kolmogci(double p); -extern double lanczos_sum_expg_scaled(double x); +CEPHES_EXTERN_EXPORT double lanczos_sum_expg_scaled(double x); -extern double owens_t(double h, double a); +CEPHES_EXTERN_EXPORT double owens_t(double h, double a); #ifdef __cplusplus } diff --git a/gtsam/3rdparty/cephes/dllexport.h b/gtsam/3rdparty/cephes/dllexport.h new file mode 100644 index 000000000..525587164 --- /dev/null +++ b/gtsam/3rdparty/cephes/dllexport.h @@ -0,0 +1,25 @@ +// Macros for exporting DLL symbols on Windows +// Usage example: +// In header file: +// class CEPHES_EXPORT MyClass { ... }; +// +// Results in the following declarations: +// When included while compiling the library itself: +// class __declspec(dllexport) MyClass { ... }; +// When included while compiling other code against the library: +// class __declspec(dllimport) MyClass { ... }; + +#pragma once + +#ifdef _WIN32 +# define CEPHES_EXPORT __declspec(dllimport) +# define CEPHES_EXTERN_EXPORT __declspec(dllimport) +#else +#ifdef __APPLE__ +# define CEPHES_EXPORT __attribute__((visibility("default"))) +# define CEPHES_EXTERN_EXPORT extern +#else +# define CEPHES_EXPORT +# define CEPHES_EXTERN_EXPORT extern +#endif +#endif From bc3b96a6e81e67f04445398d6abf511c8e10f070 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 5 Jan 2024 03:24:50 -0500 Subject: [PATCH 49/73] rename error to errorTree when it returns an AlgebraicDecisionTree --- gtsam/discrete/DecisionTreeFactor.cpp | 2 +- gtsam/discrete/DecisionTreeFactor.h | 2 +- gtsam/discrete/DiscreteFactor.h | 2 +- gtsam/discrete/TableFactor.cpp | 4 ++-- gtsam/discrete/TableFactor.h | 2 +- gtsam/discrete/tests/testDecisionTreeFactor.cpp | 2 +- gtsam/hybrid/GaussianMixture.cpp | 6 +++--- gtsam/hybrid/GaussianMixture.h | 2 +- gtsam/hybrid/GaussianMixtureFactor.cpp | 6 +++--- gtsam/hybrid/GaussianMixtureFactor.h | 2 +- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 6 +++--- gtsam/hybrid/HybridGaussianFactorGraph.h | 3 ++- gtsam/hybrid/MixtureFactor.h | 6 +++--- gtsam/hybrid/tests/testGaussianMixture.cpp | 4 ++-- gtsam/hybrid/tests/testGaussianMixtureFactor.cpp | 2 +- gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp | 2 +- gtsam/hybrid/tests/testMixtureFactor.cpp | 3 ++- gtsam_unstable/discrete/AllDiff.h | 2 +- gtsam_unstable/discrete/BinaryAllDiff.h | 2 +- gtsam_unstable/discrete/Domain.h | 2 +- gtsam_unstable/discrete/SingleValue.h | 2 +- 21 files changed, 33 insertions(+), 31 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index cbb26016c..c56818448 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -63,7 +63,7 @@ namespace gtsam { } /* ************************************************************************ */ - AlgebraicDecisionTree DecisionTreeFactor::error() const { + AlgebraicDecisionTree DecisionTreeFactor::errorTree() const { // Get all possible assignments DiscreteKeys dkeys = discreteKeys(); // Reverse to make cartesian product output a more natural ordering. diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 5e0acc056..784b11e51 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -293,7 +293,7 @@ namespace gtsam { double error(const HybridValues& values) const override; /// Compute error for each assignment and return as a tree - AlgebraicDecisionTree error() const override; + AlgebraicDecisionTree errorTree() const override; /// @} diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index e84533655..771efbe5b 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -105,7 +105,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { double error(const HybridValues& c) const override; /// Compute error for each assignment and return as a tree - virtual AlgebraicDecisionTree error() const = 0; + virtual AlgebraicDecisionTree errorTree() const = 0; /// Multiply in a DecisionTreeFactor and return the result as /// DecisionTreeFactor diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index be5f2af5b..b360617f5 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -169,8 +169,8 @@ double TableFactor::error(const HybridValues& values) const { } /* ************************************************************************ */ -AlgebraicDecisionTree TableFactor::error() const { - return toDecisionTreeFactor().error(); +AlgebraicDecisionTree TableFactor::errorTree() const { + return toDecisionTreeFactor().errorTree(); } /* ************************************************************************ */ diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 40ed231fd..228b36337 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -359,7 +359,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { double error(const HybridValues& values) const override; /// Compute error for each assignment and return as a tree - AlgebraicDecisionTree error() const override; + AlgebraicDecisionTree errorTree() const override; /// @} }; diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index 69ee52662..d764da7bf 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -75,7 +75,7 @@ TEST(DecisionTreeFactor, Error) { // Create factors DecisionTreeFactor f(X & Y & Z, "2 5 3 6 4 7 25 55 35 65 45 75"); - auto errors = f.error(); + auto errors = f.errorTree(); // regression AlgebraicDecisionTree expected( {X, Y, Z}, diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 753e35bf0..c105a329e 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -313,14 +313,14 @@ AlgebraicDecisionTree GaussianMixture::logProbability( } /* *******************************************************************************/ -AlgebraicDecisionTree GaussianMixture::error( +AlgebraicDecisionTree GaussianMixture::errorTree( const VectorValues &continuousValues) const { auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) { return conditional->error(continuousValues) + // logConstant_ - conditional->logNormalizationConstant(); }; - DecisionTree errorTree(conditionals_, errorFunc); - return errorTree; + DecisionTree error_tree(conditionals_, errorFunc); + return error_tree; } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 0b68fcfd0..521a4ca7a 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -214,7 +214,7 @@ class GTSAM_EXPORT GaussianMixture * @return AlgebraicDecisionTree A decision tree on the discrete keys * only, with the leaf values as the error for each assignment. */ - AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + AlgebraicDecisionTree errorTree(const VectorValues &continuousValues) const; /** * @brief Compute the logProbability of this Gaussian Mixture. diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 0c7ff0e87..a3db16d04 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -102,14 +102,14 @@ GaussianFactorGraphTree GaussianMixtureFactor::asGaussianFactorGraphTree() } /* *******************************************************************************/ -AlgebraicDecisionTree GaussianMixtureFactor::error( +AlgebraicDecisionTree GaussianMixtureFactor::errorTree( const VectorValues &continuousValues) const { // functor to convert from sharedFactor to double error value. auto errorFunc = [&continuousValues](const sharedFactor &gf) { return gf->error(continuousValues); }; - DecisionTree errorTree(factors_, errorFunc); - return errorTree; + DecisionTree error_tree(factors_, errorFunc); + return error_tree; } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 1325cfe93..63ca9e923 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -135,7 +135,7 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * @return AlgebraicDecisionTree A decision tree with the same keys * as the factors involved, and leaf values as the error. */ - AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + AlgebraicDecisionTree errorTree(const VectorValues &continuousValues) const; /** * @brief Compute the log-likelihood, including the log-normalizing constant. diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 7eaefbf85..bdfac8468 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -420,7 +420,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, } /* ************************************************************************ */ -AlgebraicDecisionTree HybridGaussianFactorGraph::error( +AlgebraicDecisionTree HybridGaussianFactorGraph::errorTree( const VectorValues &continuousValues) const { AlgebraicDecisionTree error_tree(0.0); @@ -431,7 +431,7 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::error( if (auto gaussianMixture = dynamic_pointer_cast(f)) { // Compute factor error and add it. - error_tree = error_tree + gaussianMixture->error(continuousValues); + error_tree = error_tree + gaussianMixture->errorTree(continuousValues); } else if (auto gaussian = dynamic_pointer_cast(f)) { // If continuous only, get the (double) error // and add it to the error_tree @@ -460,7 +460,7 @@ double HybridGaussianFactorGraph::probPrime(const HybridValues &values) const { /* ************************************************************************ */ AlgebraicDecisionTree HybridGaussianFactorGraph::probPrime( const VectorValues &continuousValues) const { - AlgebraicDecisionTree error_tree = this->error(continuousValues); + AlgebraicDecisionTree error_tree = this->errorTree(continuousValues); AlgebraicDecisionTree prob_tree = error_tree.apply([](double error) { // NOTE: The 0.5 term is handled by each factor return exp(-error); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index b3f159150..f924b7a1c 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -161,7 +161,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph * @param continuousValues Continuous values at which to compute the error. * @return AlgebraicDecisionTree */ - AlgebraicDecisionTree error(const VectorValues& continuousValues) const; + AlgebraicDecisionTree errorTree( + const VectorValues& continuousValues) const; /** * @brief Compute unnormalized probability \f$ P(X | M, Z) \f$ diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index df8e0193a..09a641b48 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -131,13 +131,13 @@ class MixtureFactor : public HybridFactor { * @return AlgebraicDecisionTree A decision tree with the same keys * as the factor, and leaf values as the error. */ - AlgebraicDecisionTree error(const Values& continuousValues) const { + AlgebraicDecisionTree errorTree(const Values& continuousValues) const { // functor to convert from sharedFactor to double error value. auto errorFunc = [continuousValues](const sharedFactor& factor) { return factor->error(continuousValues); }; - DecisionTree errorTree(factors_, errorFunc); - return errorTree; + DecisionTree result(factors_, errorFunc); + return result; } /** diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index f15c06165..4da61912e 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -97,7 +97,7 @@ TEST(GaussianMixture, LogProbability) { /// Check error. TEST(GaussianMixture, Error) { using namespace equal_constants; - auto actual = mixture.error(vv); + auto actual = mixture.errorTree(vv); // Check result. std::vector discrete_keys = {mode}; @@ -134,7 +134,7 @@ TEST(GaussianMixture, Likelihood) { std::vector leaves = {conditionals[0]->likelihood(vv)->error(vv), conditionals[1]->likelihood(vv)->error(vv)}; AlgebraicDecisionTree expected(discrete_keys, leaves); - EXPECT(assert_equal(expected, likelihood->error(vv), 1e-6)); + EXPECT(assert_equal(expected, likelihood->errorTree(vv), 1e-6)); // Check that the ratio of probPrime to evaluate is the same for all modes. std::vector ratio(2); diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 75ba5a059..9cc7e6bfd 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -178,7 +178,7 @@ TEST(GaussianMixtureFactor, Error) { continuousValues.insert(X(2), Vector2(1, 1)); // error should return a tree of errors, with nodes for each discrete value. - AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); + AlgebraicDecisionTree error_tree = mixtureFactor.errorTree(continuousValues); std::vector discrete_keys = {m1}; // Error values for regression test diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index b240e1626..98a8a794f 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -580,7 +580,7 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) { HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential(); HybridValues delta = hybridBayesNet->optimize(); - auto error_tree = graph.error(delta.continuous()); + auto error_tree = graph.errorTree(delta.continuous()); std::vector discrete_keys = {{M(0), 2}, {M(1), 2}}; std::vector leaves = {0.9998558, 0.4902432, 0.5193694, 0.0097568}; diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 67a7fd8ae..0b2564403 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -97,7 +97,8 @@ TEST(MixtureFactor, Error) { continuousValues.insert(X(1), 0); continuousValues.insert(X(2), 1); - AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); + AlgebraicDecisionTree error_tree = + mixtureFactor.errorTree(continuousValues); DiscreteKey m1(1, 2); std::vector discrete_keys = {m1}; diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 9c8e62ecd..d7a63eae0 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -54,7 +54,7 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; /// Compute error for each assignment and return as a tree - AlgebraicDecisionTree error() const override { + AlgebraicDecisionTree errorTree() const override { throw std::runtime_error("AllDiff::error not implemented"); } diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index 33f6562b4..18b335092 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -93,7 +93,7 @@ class BinaryAllDiff : public Constraint { } /// Compute error for each assignment and return as a tree - AlgebraicDecisionTree error() const override { + AlgebraicDecisionTree errorTree() const override { throw std::runtime_error("BinaryAllDiff::error not implemented"); } }; diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index ca7340a9f..7f7b717c2 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -70,7 +70,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { } /// Compute error for each assignment and return as a tree - AlgebraicDecisionTree error() const override { + AlgebraicDecisionTree errorTree() const override { throw std::runtime_error("Domain::error not implemented"); } diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index f57f24b42..3f7f22d6a 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -50,7 +50,7 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { } /// Compute error for each assignment and return as a tree - AlgebraicDecisionTree error() const override { + AlgebraicDecisionTree errorTree() const override { throw std::runtime_error("SingleValue::error not implemented"); } From 7ea1bbcfc3f051a1ee938137790021f3fb4e5c0d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 5 Jan 2024 15:04:15 -0500 Subject: [PATCH 50/73] replace error with errorTree --- gtsam/hybrid/HybridBayesNet.cpp | 4 ++-- gtsam/hybrid/HybridBayesNet.h | 3 ++- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 8 ++++---- gtsam/hybrid/HybridNonlinearFactorGraph.cpp | 8 ++++---- gtsam/hybrid/tests/testHybridBayesNet.cpp | 2 +- 5 files changed, 13 insertions(+), 12 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 31177ddb7..b02967555 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -282,7 +282,7 @@ HybridValues HybridBayesNet::sample() const { } /* ************************************************************************* */ -AlgebraicDecisionTree HybridBayesNet::error( +AlgebraicDecisionTree HybridBayesNet::errorTree( const VectorValues &continuousValues) const { AlgebraicDecisionTree result(0.0); @@ -290,7 +290,7 @@ AlgebraicDecisionTree HybridBayesNet::error( for (auto &&conditional : *this) { if (auto gm = conditional->asMixture()) { // If conditional is hybrid, compute error for all assignments. - result = result + gm->error(continuousValues); + result = result + gm->errorTree(continuousValues); } else if (auto gc = conditional->asGaussian()) { // If continuous, get the error and add it to the result diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 22e03bba9..032cd55b9 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -187,7 +187,8 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * @param continuousValues Continuous values at which to compute the error. * @return AlgebraicDecisionTree */ - AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + AlgebraicDecisionTree errorTree( + const VectorValues &continuousValues) const; /** * @brief Error method using HybridValues which returns specific error for diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index e372d0361..b764dc9e0 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -99,7 +99,7 @@ void HybridGaussianFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - gmf->error(values.continuous()).print("", keyFormatter); + gmf->errorTree(values.continuous()).print("", keyFormatter); std::cout << std::endl; } } else if (auto hc = std::dynamic_pointer_cast(factor)) { @@ -113,12 +113,12 @@ void HybridGaussianFactorGraph::printErrors( std::cout << "error = " << hc->asGaussian()->error(values) << "\n"; } else if (hc->isDiscrete()) { std::cout << "error = "; - hc->asDiscrete()->error().print("", keyFormatter); + hc->asDiscrete()->errorTree().print("", keyFormatter); std::cout << "\n"; } else { // Is hybrid std::cout << "error = "; - hc->asMixture()->error(values.continuous()).print(); + hc->asMixture()->errorTree(values.continuous()).print(); std::cout << "\n"; } } @@ -141,7 +141,7 @@ void HybridGaussianFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - df->error().print("", keyFormatter); + df->errorTree().print("", keyFormatter); } } else { diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index e0dfd413c..cdd448412 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -66,7 +66,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - mf->error(values.nonlinear()).print("", keyFormatter); + mf->errorTree(values.nonlinear()).print("", keyFormatter); std::cout << std::endl; } } else if (auto gmf = @@ -77,7 +77,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - gmf->error(values.continuous()).print("", keyFormatter); + gmf->errorTree(values.continuous()).print("", keyFormatter); std::cout << std::endl; } } else if (auto gm = std::dynamic_pointer_cast(factor)) { @@ -87,7 +87,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - gm->error(values.continuous()).print("", keyFormatter); + gm->errorTree(values.continuous()).print("", keyFormatter); std::cout << std::endl; } } else if (auto nf = std::dynamic_pointer_cast(factor)) { @@ -121,7 +121,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - df->error().print("", keyFormatter); + df->errorTree().print("", keyFormatter); std::cout << std::endl; } diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 66985cc78..00dc36cd0 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -182,7 +182,7 @@ TEST(HybridBayesNet, Error) { values.insert(X(1), Vector1(1)); AlgebraicDecisionTree actual_errors = - bayesNet.error(values.continuous()); + bayesNet.errorTree(values.continuous()); // Regression. // Manually added all the error values from the 3 conditional types. From a4a0fefd84418fedbe89834b8f78541523661032 Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Fri, 12 Jan 2024 17:28:12 -0500 Subject: [PATCH 51/73] libcephes only needs 3.10, allowing build on ubuntu 18.04 --- gtsam/3rdparty/cephes/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt index e840e9e49..946448160 100644 --- a/gtsam/3rdparty/cephes/CMakeLists.txt +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.12) +cmake_minimum_required(VERSION 3.10) enable_testing() project( cephes From 2971d9e74ebac099e7741b78612fd5a69ece4035 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 8 Dec 2023 01:25:41 +0200 Subject: [PATCH 52/73] Compile gtsam python for windows --- .github/scripts/python.sh | 7 ++-- .github/workflows/build-python.yml | 55 +++++++++++++++++++++++++-- gtsam/base/utilities.h | 4 +- gtsam/discrete/DiscreteValues.h | 4 +- gtsam/geometry/Rot3.h | 2 +- gtsam/nonlinear/CustomFactor.h | 2 +- gtsam/sfm/DsfTrackGenerator.h | 2 +- python/CMakeLists.txt | 60 ++++++++++++++++++++++++++++-- python/setup.py.in | 3 +- 9 files changed, 121 insertions(+), 18 deletions(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 08b8084a0..c851c7e36 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -65,14 +65,13 @@ function build() # Set to 2 cores so that Actions does not error out during resource provisioning. cmake --build build -j2 - $PYTHON -m pip install --user build/python + cmake --build build --target python-install } function test() { - cd $GITHUB_WORKSPACE/python/gtsam/tests - $PYTHON -m unittest discover -v - cd $GITHUB_WORKSPACE + cmake --build build --target python-test + cmake --build build --target python-test-unstable } # select between build or test diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 91bc4e80a..676fe8ea0 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -18,9 +18,11 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} PYTHON_VERSION: ${{ matrix.python_version }} + BOOST_VERSION: 1.72.0 + BOOST_EXE: boost_1_72_0-msvc-14.2 strategy: - fail-fast: true + fail-fast: false matrix: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. @@ -30,6 +32,7 @@ jobs: ubuntu-20.04-gcc-9-tbb, ubuntu-20.04-clang-9, macOS-11-xcode-13.4.1, + windows-2019-msbuild, ] build_type: [Release] @@ -56,6 +59,10 @@ jobs: compiler: xcode version: "13.4.1" + - name: windows-2019-msbuild + os: windows-2019 + platform: 64 + steps: - name: Checkout uses: actions/checkout@v3 @@ -97,29 +104,71 @@ jobs: echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV + - name: Setup msbuild (Windows) + if: runner.os == 'Windows' + uses: ilammy/msvc-dev-cmd@v1 + with: + arch: x${{matrix.platform}} + + - name: Setup python (Windows) + uses: actions/setup-python@v4 + if: runner.os == 'Windows' + with: + python-version: ${{ matrix.python_version }} + + - name: Install ninja (Windows) + if: runner.os == 'Windows' + shell: bash + run: | + choco install ninja + ninja --version + where ninja + + - name: Install Boost (Windows) + if: runner.os == 'Windows' + shell: powershell + run: | + # Snippet from: https://github.com/actions/virtual-environments/issues/2667 + $BOOST_PATH = "C:\hostedtoolcache\windows\Boost\$env:BOOST_VERSION\x86_64" + + # Use the prebuilt binary for Windows + $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE-${{matrix.platform}}.exe" + (New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe") + Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=$BOOST_PATH" + + # Set the BOOST_ROOT variable + echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV + - name: Set GTSAM_WITH_TBB Flag if: matrix.flag == 'tbb' run: | echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" - - name: Set Swap Space + - name: Set Swap Space (Linux) if: runner.os == 'Linux' uses: pierotofy/set-swap-space@master with: swap-size-gb: 6 - - name: Install System Dependencies + - name: Install System Dependencies (Linux, macOS) + if: runner.os != 'Windows' run: | bash .github/scripts/python.sh -d - name: Install Python Dependencies + shell: bash run: python$PYTHON_VERSION -m pip install -r python/dev_requirements.txt - name: Build + shell: bash run: | bash .github/scripts/python.sh -b - name: Test + # Disable running tests for windows because some of them are failing. + # Remove this condition when you want to run tests on windows CI. + if: runner.os != 'Windows' + shell: bash run: | bash .github/scripts/python.sh -t diff --git a/gtsam/base/utilities.h b/gtsam/base/utilities.h index 03e9636da..a67c5d1b6 100644 --- a/gtsam/base/utilities.h +++ b/gtsam/base/utilities.h @@ -4,6 +4,8 @@ #include #include +#include + namespace gtsam { /** * For Python __str__(). @@ -11,7 +13,7 @@ namespace gtsam { * of an object when it prints to cout. * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string */ -struct RedirectCout { +struct GTSAM_EXPORT RedirectCout { /// constructor -- redirect stdout buffer to a stringstream buffer RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {} diff --git a/gtsam/discrete/DiscreteValues.h b/gtsam/discrete/DiscreteValues.h index 9ec08302b..9fdff014c 100644 --- a/gtsam/discrete/DiscreteValues.h +++ b/gtsam/discrete/DiscreteValues.h @@ -126,12 +126,12 @@ inline std::vector cartesianProduct(const DiscreteKeys& keys) { } /// Free version of markdown. -std::string markdown(const DiscreteValues& values, +std::string GTSAM_EXPORT markdown(const DiscreteValues& values, const KeyFormatter& keyFormatter = DefaultKeyFormatter, const DiscreteValues::Names& names = {}); /// Free version of html. -std::string html(const DiscreteValues& values, +std::string GTSAM_EXPORT html(const DiscreteValues& values, const KeyFormatter& keyFormatter = DefaultKeyFormatter, const DiscreteValues::Names& names = {}); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 2b9c5a45a..7e05ee4da 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -396,7 +396,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { Matrix3 AdjointMap() const { return matrix(); } // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE - struct ChartAtOrigin { + struct GTSAM_EXPORT ChartAtOrigin { static Rot3 Retract(const Vector3& v, ChartJacobian H = {}); static Vector3 Local(const Rot3& r, ChartJacobian H = {}); }; diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index ac2942032..c4015db37 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -42,7 +42,7 @@ using CustomErrorFunction = std::function; * correspondence indices, from each image. * @param Length-N list of keypoints, for N images/cameras. */ -std::vector tracksFromPairwiseMatches( +std::vector GTSAM_EXPORT tracksFromPairwiseMatches( const MatchIndicesMap& matches, const KeypointsVector& keypoints, bool verbose = false); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index f874c2f21..ba55ac2af 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -94,6 +94,14 @@ set(interface_headers set(GTSAM_PYTHON_TARGET gtsam_py) set(GTSAM_PYTHON_UNSTABLE_TARGET gtsam_unstable_py) +set(GTSAM_OUTPUT_NAME "gtsam") +set(GTSAM_UNSTABLE_OUTPUT_NAME "gtsam_unstable") + +if(MSVC) + set(GTSAM_OUTPUT_NAME "gtsam_py") + set(GTSAM_UNSTABLE_OUTPUT_NAME "gtsam_unstable_py") +endif() + pybind_wrap(${GTSAM_PYTHON_TARGET} # target "${interface_headers}" # interface_headers "gtsam.cpp" # generated_cpp @@ -109,12 +117,30 @@ pybind_wrap(${GTSAM_PYTHON_TARGET} # target set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE - OUTPUT_NAME "gtsam" + OUTPUT_NAME "${GTSAM_OUTPUT_NAME}" LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam" DEBUG_POSTFIX "" # Otherwise you will have a wrong name RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name ) +if(WIN32) + set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES + SUFFIX ".pyd" + ) + ADD_CUSTOM_COMMAND(TARGET ${GTSAM_PYTHON_TARGET} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/${GTSAM_OUTPUT_NAME}.pyd" + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/gtsam.pyd" + ) + ADD_CUSTOM_COMMAND(TARGET ${GTSAM_PYTHON_TARGET} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "$;$" + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/" + COMMAND_EXPAND_LISTS + VERBATIM + ) +endif() + # Set the path for the GTSAM python module set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam) @@ -188,7 +214,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) set_target_properties(${GTSAM_PYTHON_UNSTABLE_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE - OUTPUT_NAME "gtsam_unstable" + OUTPUT_NAME "${GTSAM_UNSTABLE_OUTPUT_NAME}" LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable" DEBUG_POSTFIX "" # Otherwise you will have a wrong name RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name @@ -208,13 +234,39 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) # Add gtsam_unstable to the install target list(APPEND GTSAM_PYTHON_DEPENDENCIES ${GTSAM_PYTHON_UNSTABLE_TARGET}) - + if(WIN32) + set_target_properties(${GTSAM_PYTHON_UNSTABLE_TARGET} PROPERTIES + SUFFIX ".pyd" + ) + ADD_CUSTOM_COMMAND(TARGET ${GTSAM_PYTHON_UNSTABLE_TARGET} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/${GTSAM_UNSTABLE_OUTPUT_NAME}.pyd" + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/gtsam_unstable.pyd" + ) + ADD_CUSTOM_COMMAND(TARGET ${GTSAM_PYTHON_UNSTABLE_TARGET} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "$;$" + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/" + COMMAND_EXPAND_LISTS + VERBATIM + ) + endif() + # Custom make command to run all GTSAM_UNSTABLE Python tests + add_custom_target( + python-test-unstable + COMMAND + ${CMAKE_COMMAND} -E env # add package to python path so no need to install + "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} -m unittest discover -v -s . + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} + WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/tests" + ) endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} -m pip install . + COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) diff --git a/python/setup.py.in b/python/setup.py.in index e15e39075..824a6656e 100644 --- a/python/setup.py.in +++ b/python/setup.py.in @@ -11,7 +11,8 @@ print("PACKAGES: ", packages) package_data = { '': [ "./*.so", - "./*.dll" + "./*.dll", + "./*.pyd", ] } From 8023df456d9a4c59ce23979a2ab32953dfc520e3 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Sun, 31 Dec 2023 23:19:29 +0200 Subject: [PATCH 53/73] add windows template specialization --- .github/workflows/build-python.yml | 3 -- gtsam/nonlinear/Values-inl.h | 53 ++++++++++++++++++++++++++++++ 2 files changed, 53 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 676fe8ea0..bb10b27c3 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -166,9 +166,6 @@ jobs: bash .github/scripts/python.sh -b - name: Test - # Disable running tests for windows because some of them are failing. - # Remove this condition when you want to run tests on windows CI. - if: runner.os != 'Windows' shell: bash run: | bash .github/scripts/python.sh -t diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index a93f9570e..6da87d71c 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -197,6 +197,59 @@ namespace gtsam { } }; +#ifdef _WIN32 + // Handle dynamic matrices + template + struct handle_matrix, true> { + inline Eigen::Matrix operator()(Key j, const Value* const pointer) { + auto ptr = dynamic_cast>*>(pointer); + if (ptr) { + // value returns a const Matrix&, and the return makes a copy !!!!! + return ptr->value(); + } else { + // If a fixed matrix was stored, we end up here as well. + throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix)); + } + } + }; + + // Handle fixed matrices + template + struct handle_matrix, false> { + inline Eigen::Matrix operator()(Key j, const Value* const pointer) { + auto ptr = dynamic_cast>*>(pointer); + if (ptr) { + // value returns a const MatrixMN&, and the return makes a copy !!!!! + return ptr->value(); + } else { + Matrix A; + // Check if a dynamic matrix was stored + auto ptr = dynamic_cast*>(pointer); + if (ptr) { + A = ptr->value(); + } else { + // Or a dynamic vector + A = handle_matrix()(j, pointer); // will throw if not.... + } + // Yes: check size, and throw if not a match + if (A.rows() != M || A.cols() != N) + throw NoMatchFoundForFixed(M, N, A.rows(), A.cols()); + else + return A; // copy but not malloc + } + } + }; + + // Handle matrices + template + struct handle> { + Eigen::Matrix operator()(Key j, const Value* const pointer) { + return handle_matrix, + (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer); + } + }; +#endif // #ifdef _WIN32 + } // internal /* ************************************************************************* */ From 85cae70cefe58d5ffb336e8fb77f73fc83c8b753 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Thu, 4 Jan 2024 08:51:19 +0200 Subject: [PATCH 54/73] revert lines according to review comments. --- .github/scripts/python.sh | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index c851c7e36..0c04eeec3 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -70,8 +70,16 @@ function build() function test() { - cmake --build build --target python-test - cmake --build build --target python-test-unstable + cd $GITHUB_WORKSPACE/python/gtsam/tests + $PYTHON -m unittest discover -v + cd $GITHUB_WORKSPACE + + cd $GITHUB_WORKSPACE/python/gtsam_unstable/tests + $PYTHON -m unittest discover -v + cd $GITHUB_WORKSPACE + + # cmake --build build --target python-test + # cmake --build build --target python-test-unstable } # select between build or test From d1ab94f51c979652b924b2323dd3e3e4a2f58655 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 5 Jan 2024 11:05:03 +0200 Subject: [PATCH 55/73] Add comments explain the windows workaround. --- gtsam/nonlinear/Values-inl.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 6da87d71c..1fe909a11 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -197,6 +197,8 @@ namespace gtsam { } }; +// Added this section for compile gtsam python on windows. +// msvc don't deduct the template arguments correctly, due possible bug in msvc. #ifdef _WIN32 // Handle dynamic matrices template From 3b969bf94c2e562c07e3e7c118d43dd82627cf4c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jan 2024 15:04:09 -0500 Subject: [PATCH 56/73] Squashed 'wrap/' changes from bd57210d9..cdcf23207 cdcf23207 Merge pull request #163 from borglab/dunder-methods 22d429cc2 use STL functions instead of container methods abe8818ab update pyparsing to 3.1.1 a9e896d0c add unit tests 535fe4f14 wrap supported dunder methods 079687eac parse dunder methods in interface file git-subtree-dir: wrap git-subtree-split: cdcf23207fbb03457c5d9dbfc2b0b57e515b5f3d --- gtwrap/interface_parser/classes.py | 48 +++++++++++--- gtwrap/interface_parser/function.py | 2 +- gtwrap/interface_parser/tokens.py | 1 + gtwrap/pybind_wrapper.py | 64 +++++++++++++++++++ gtwrap/template_instantiator/classes.py | 3 + requirements.txt | 2 +- tests/expected/matlab/FastSet.m | 36 +++++++++++ tests/expected/matlab/MyFactorPosePoint2.m | 12 ++-- tests/expected/matlab/class_wrapper.cpp | 74 ++++++++++++++++++---- tests/expected/python/class_pybind.cpp | 6 ++ tests/fixtures/class.i | 9 +++ tests/test_interface_parser.py | 22 +++++-- 12 files changed, 245 insertions(+), 34 deletions(-) create mode 100644 tests/expected/matlab/FastSet.m diff --git a/gtwrap/interface_parser/classes.py b/gtwrap/interface_parser/classes.py index b63a0b5eb..8967bea93 100644 --- a/gtwrap/interface_parser/classes.py +++ b/gtwrap/interface_parser/classes.py @@ -12,13 +12,14 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae from typing import Any, Iterable, List, Union -from pyparsing import Literal, Optional, ZeroOrMore # type: ignore +from pyparsing import ZeroOrMore # type: ignore +from pyparsing import Literal, Optional, Word, alphas from .enum import Enum from .function import ArgumentList, ReturnType from .template import Template -from .tokens import (CLASS, COLON, CONST, IDENT, LBRACE, LPAREN, OPERATOR, - RBRACE, RPAREN, SEMI_COLON, STATIC, VIRTUAL) +from .tokens import (CLASS, COLON, CONST, DUNDER, IDENT, LBRACE, LPAREN, + OPERATOR, RBRACE, RPAREN, SEMI_COLON, STATIC, VIRTUAL) from .type import TemplatedType, Typename from .utils import collect_namespaces from .variable import Variable @@ -212,6 +213,26 @@ class Operator: ) +class DunderMethod: + """Special Python double-underscore (dunder) methods, e.g. __iter__, __contains__""" + rule = ( + DUNDER # + + (Word(alphas))("name") # + + DUNDER # + + LPAREN # + + ArgumentList.rule("args_list") # + + RPAREN # + + SEMI_COLON # BR + ).setParseAction(lambda t: DunderMethod(t.name, t.args_list)) + + def __init__(self, name: str, args: ArgumentList): + self.name = name + self.args = args + + def __repr__(self) -> str: + return f"DunderMethod: __{self.name}__({self.args})" + + class Class: """ Rule to parse a class defined in the interface file. @@ -223,11 +244,13 @@ class Class: }; ``` """ + class Members: """ Rule for all the members within a class. """ - rule = ZeroOrMore(Constructor.rule # + rule = ZeroOrMore(DunderMethod.rule # + ^ Constructor.rule # ^ Method.rule # ^ StaticMethod.rule # ^ Variable.rule # @@ -235,11 +258,12 @@ class Class: ^ Enum.rule # ).setParseAction(lambda t: Class.Members(t.asList())) - def __init__(self, - members: List[Union[Constructor, Method, StaticMethod, - Variable, Operator]]): + def __init__(self, members: List[Union[Constructor, Method, + StaticMethod, Variable, + Operator, Enum, DunderMethod]]): self.ctors = [] self.methods = [] + self.dunder_methods = [] self.static_methods = [] self.properties = [] self.operators = [] @@ -251,6 +275,8 @@ class Class: self.methods.append(m) elif isinstance(m, StaticMethod): self.static_methods.append(m) + elif isinstance(m, DunderMethod): + self.dunder_methods.append(m) elif isinstance(m, Variable): self.properties.append(m) elif isinstance(m, Operator): @@ -271,8 +297,8 @@ class Class: + SEMI_COLON # BR ).setParseAction(lambda t: Class( t.template, t.is_virtual, t.name, t.parent_class, t.members.ctors, t. - members.methods, t.members.static_methods, t.members.properties, t. - members.operators, t.members.enums)) + members.methods, t.members.static_methods, t.members.dunder_methods, t. + members.properties, t.members.operators, t.members.enums)) def __init__( self, @@ -283,6 +309,7 @@ class Class: ctors: List[Constructor], methods: List[Method], static_methods: List[StaticMethod], + dunder_methods: List[DunderMethod], properties: List[Variable], operators: List[Operator], enums: List[Enum], @@ -308,6 +335,7 @@ class Class: self.ctors = ctors self.methods = methods self.static_methods = static_methods + self.dunder_methods = dunder_methods self.properties = properties self.operators = operators self.enums = enums @@ -326,6 +354,8 @@ class Class: method.parent = self for static_method in self.static_methods: static_method.parent = self + for dunder_method in self.dunder_methods: + dunder_method.parent = self for _property in self.properties: _property.parent = self diff --git a/gtwrap/interface_parser/function.py b/gtwrap/interface_parser/function.py index b40884488..5385c744f 100644 --- a/gtwrap/interface_parser/function.py +++ b/gtwrap/interface_parser/function.py @@ -82,7 +82,7 @@ class ArgumentList: return ArgumentList([]) def __repr__(self) -> str: - return repr(tuple(self.args_list)) + return ",".join([repr(x) for x in self.args_list]) def __len__(self) -> int: return len(self.args_list) diff --git a/gtwrap/interface_parser/tokens.py b/gtwrap/interface_parser/tokens.py index 02e6d82f8..11c99d19c 100644 --- a/gtwrap/interface_parser/tokens.py +++ b/gtwrap/interface_parser/tokens.py @@ -22,6 +22,7 @@ RAW_POINTER, SHARED_POINTER, REF = map(Literal, "@*&") LPAREN, RPAREN, LBRACE, RBRACE, COLON, SEMI_COLON = map(Suppress, "(){}:;") LOPBRACK, ROPBRACK, COMMA, EQUAL = map(Suppress, "<>,=") +DUNDER = Suppress(Literal("__")) # Default argument passed to functions/methods. # Allow anything up to ',' or ';' except when they diff --git a/gtwrap/pybind_wrapper.py b/gtwrap/pybind_wrapper.py index 78730a909..479c2d67d 100755 --- a/gtwrap/pybind_wrapper.py +++ b/gtwrap/pybind_wrapper.py @@ -45,6 +45,8 @@ class PybindWrapper: 'continue', 'global', 'pass' ] + self.dunder_methods = ('len', 'contains', 'iter') + # amount of indentation to add before each function/method declaration. self.method_indent = '\n' + (' ' * 8) @@ -153,6 +155,51 @@ class PybindWrapper: suffix=suffix) return ret + def _wrap_dunder(self, + method, + cpp_class, + prefix, + suffix, + method_suffix=""): + """ + Wrap a Python double-underscore (dunder) method. + + E.g. __len__() gets wrapped as `.def("__len__", [](gtsam::KeySet* self) {return self->size();})` + + Supported methods are: + - __contains__(T x) + - __len__() + - __iter__() + """ + py_method = method.name + method_suffix + args_names = method.args.names() + py_args_names = self._py_args_names(method.args) + args_signature_with_names = self._method_args_signature(method.args) + + if method.name == 'len': + function_call = "return std::distance(self->begin(), self->end());" + elif method.name == 'contains': + function_call = f"return std::find(self->begin(), self->end(), {method.args.args_list[0].name}) != self->end();" + elif method.name == 'iter': + function_call = "return py::make_iterator(self->begin(), self->end());" + + ret = ('{prefix}.def("__{py_method}__",' + '[]({self}{opt_comma}{args_signature_with_names}){{' + '{function_call}' + '}}' + '{py_args_names}){suffix}'.format( + prefix=prefix, + py_method=py_method, + self=f"{cpp_class}* self", + opt_comma=', ' if args_names else '', + args_signature_with_names=args_signature_with_names, + function_call=function_call, + py_args_names=py_args_names, + suffix=suffix, + )) + + return ret + def _wrap_method(self, method, cpp_class, @@ -235,6 +282,20 @@ class PybindWrapper: return ret + def wrap_dunder_methods(self, + methods, + cpp_class, + prefix='\n' + ' ' * 8, + suffix=''): + res = "" + for method in methods: + res += self._wrap_dunder(method=method, + cpp_class=cpp_class, + prefix=prefix, + suffix=suffix) + + return res + def wrap_methods(self, methods, cpp_class, @@ -398,6 +459,7 @@ class PybindWrapper: '{wrapped_ctors}' '{wrapped_methods}' '{wrapped_static_methods}' + '{wrapped_dunder_methods}' '{wrapped_properties}' '{wrapped_operators};\n'.format( class_declaration=class_declaration, @@ -406,6 +468,8 @@ class PybindWrapper: instantiated_class.methods, cpp_class), wrapped_static_methods=self.wrap_methods( instantiated_class.static_methods, cpp_class), + wrapped_dunder_methods=self.wrap_dunder_methods( + instantiated_class.dunder_methods, cpp_class), wrapped_properties=self.wrap_properties( instantiated_class.properties, cpp_class), wrapped_operators=self.wrap_operators( diff --git a/gtwrap/template_instantiator/classes.py b/gtwrap/template_instantiator/classes.py index ce51d5b96..702654678 100644 --- a/gtwrap/template_instantiator/classes.py +++ b/gtwrap/template_instantiator/classes.py @@ -57,6 +57,8 @@ class InstantiatedClass(parser.Class): # Instantiate all instance methods self.methods = self.instantiate_methods(typenames) + + self.dunder_methods = original.dunder_methods super().__init__( self.template, @@ -66,6 +68,7 @@ class InstantiatedClass(parser.Class): self.ctors, self.methods, self.static_methods, + self.dunder_methods, self.properties, self.operators, self.enums, diff --git a/requirements.txt b/requirements.txt index 0aac9302e..f43fdda61 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,2 +1,2 @@ -pyparsing==2.4.7 +pyparsing==3.1.1 pytest>=6.2.4 diff --git a/tests/expected/matlab/FastSet.m b/tests/expected/matlab/FastSet.m new file mode 100644 index 000000000..4d2a1813e --- /dev/null +++ b/tests/expected/matlab/FastSet.m @@ -0,0 +1,36 @@ +%class FastSet, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%FastSet() +% +classdef FastSet < handle + properties + ptr_FastSet = 0 + end + methods + function obj = FastSet(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + class_wrapper(73, my_ptr); + elseif nargin == 0 + my_ptr = class_wrapper(74); + else + error('Arguments do not match any overload of FastSet constructor'); + end + obj.ptr_FastSet = my_ptr; + end + + function delete(obj) + class_wrapper(75, obj.ptr_FastSet); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/tests/expected/matlab/MyFactorPosePoint2.m b/tests/expected/matlab/MyFactorPosePoint2.m index 4a30bd489..ac5b134f9 100644 --- a/tests/expected/matlab/MyFactorPosePoint2.m +++ b/tests/expected/matlab/MyFactorPosePoint2.m @@ -15,9 +15,9 @@ classdef MyFactorPosePoint2 < handle function obj = MyFactorPosePoint2(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(73, my_ptr); + class_wrapper(76, my_ptr); elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base') - my_ptr = class_wrapper(74, varargin{1}, varargin{2}, varargin{3}, varargin{4}); + my_ptr = class_wrapper(77, varargin{1}, varargin{2}, varargin{3}, varargin{4}); else error('Arguments do not match any overload of MyFactorPosePoint2 constructor'); end @@ -25,7 +25,7 @@ classdef MyFactorPosePoint2 < handle end function delete(obj) - class_wrapper(75, obj.ptr_MyFactorPosePoint2); + class_wrapper(78, obj.ptr_MyFactorPosePoint2); end function display(obj), obj.print(''); end @@ -36,19 +36,19 @@ classdef MyFactorPosePoint2 < handle % PRINT usage: print(string s, KeyFormatter keyFormatter) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.KeyFormatter') - class_wrapper(76, this, varargin{:}); + class_wrapper(79, this, varargin{:}); return end % PRINT usage: print(string s) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'char') - class_wrapper(77, this, varargin{:}); + class_wrapper(80, this, varargin{:}); return end % PRINT usage: print() : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - class_wrapper(78, this, varargin{:}); + class_wrapper(81, this, varargin{:}); return end error('Arguments do not match any overload of function MyFactorPosePoint2.print'); diff --git a/tests/expected/matlab/class_wrapper.cpp b/tests/expected/matlab/class_wrapper.cpp index c4be52018..e33f14238 100644 --- a/tests/expected/matlab/class_wrapper.cpp +++ b/tests/expected/matlab/class_wrapper.cpp @@ -31,6 +31,8 @@ typedef std::set*> Collector_ForwardKinematic static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_TemplatedConstructor; static Collector_TemplatedConstructor collector_TemplatedConstructor; +typedef std::set*> Collector_FastSet; +static Collector_FastSet collector_FastSet; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; @@ -101,6 +103,12 @@ void _deleteAllObjects() collector_TemplatedConstructor.erase(iter++); anyDeleted = true; } } + { for(Collector_FastSet::iterator iter = collector_FastSet.begin(); + iter != collector_FastSet.end(); ) { + delete *iter; + collector_FastSet.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -844,7 +852,40 @@ void TemplatedConstructor_deconstructor_72(int nargout, mxArray *out[], int narg delete self; } -void MyFactorPosePoint2_collectorInsertAndMakeBase_73(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void FastSet_collectorInsertAndMakeBase_73(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef std::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_FastSet.insert(self); +} + +void FastSet_constructor_74(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef std::shared_ptr Shared; + + Shared *self = new Shared(new FastSet()); + collector_FastSet.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void FastSet_deconstructor_75(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef std::shared_ptr Shared; + checkArguments("delete_FastSet",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_FastSet::iterator item; + item = collector_FastSet.find(self); + if(item != collector_FastSet.end()) { + collector_FastSet.erase(item); + } + delete self; +} + +void MyFactorPosePoint2_collectorInsertAndMakeBase_76(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef std::shared_ptr> Shared; @@ -853,7 +894,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_73(int nargout, mxArray *out[ collector_MyFactorPosePoint2.insert(self); } -void MyFactorPosePoint2_constructor_74(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_constructor_77(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef std::shared_ptr> Shared; @@ -868,7 +909,7 @@ void MyFactorPosePoint2_constructor_74(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void MyFactorPosePoint2_deconstructor_75(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_deconstructor_78(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef std::shared_ptr> Shared; checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); @@ -881,7 +922,7 @@ void MyFactorPosePoint2_deconstructor_75(int nargout, mxArray *out[], int nargin delete self; } -void MyFactorPosePoint2_print_76(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_79(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,2); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -890,7 +931,7 @@ void MyFactorPosePoint2_print_76(int nargout, mxArray *out[], int nargin, const obj->print(s,keyFormatter); } -void MyFactorPosePoint2_print_77(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_80(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,1); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -898,7 +939,7 @@ void MyFactorPosePoint2_print_77(int nargout, mxArray *out[], int nargin, const obj->print(s,gtsam::DefaultKeyFormatter); } -void MyFactorPosePoint2_print_78(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_81(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -1137,22 +1178,31 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) TemplatedConstructor_deconstructor_72(nargout, out, nargin-1, in+1); break; case 73: - MyFactorPosePoint2_collectorInsertAndMakeBase_73(nargout, out, nargin-1, in+1); + FastSet_collectorInsertAndMakeBase_73(nargout, out, nargin-1, in+1); break; case 74: - MyFactorPosePoint2_constructor_74(nargout, out, nargin-1, in+1); + FastSet_constructor_74(nargout, out, nargin-1, in+1); break; case 75: - MyFactorPosePoint2_deconstructor_75(nargout, out, nargin-1, in+1); + FastSet_deconstructor_75(nargout, out, nargin-1, in+1); break; case 76: - MyFactorPosePoint2_print_76(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_collectorInsertAndMakeBase_76(nargout, out, nargin-1, in+1); break; case 77: - MyFactorPosePoint2_print_77(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_constructor_77(nargout, out, nargin-1, in+1); break; case 78: - MyFactorPosePoint2_print_78(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_deconstructor_78(nargout, out, nargin-1, in+1); + break; + case 79: + MyFactorPosePoint2_print_79(nargout, out, nargin-1, in+1); + break; + case 80: + MyFactorPosePoint2_print_80(nargout, out, nargin-1, in+1); + break; + case 81: + MyFactorPosePoint2_print_81(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/tests/expected/python/class_pybind.cpp b/tests/expected/python/class_pybind.cpp index 86d69c2e0..2292f46be 100644 --- a/tests/expected/python/class_pybind.cpp +++ b/tests/expected/python/class_pybind.cpp @@ -91,6 +91,12 @@ PYBIND11_MODULE(class_py, m_) { .def(py::init(), py::arg("arg")) .def(py::init(), py::arg("arg")); + py::class_>(m_, "FastSet") + .def(py::init<>()) + .def("__len__",[](FastSet* self){return std::distance(self->begin(), self->end());}) + .def("__contains__",[](FastSet* self, size_t key){return std::find(self->begin(), self->end(), key) != self->end();}, py::arg("key")) + .def("__iter__",[](FastSet* self){return py::make_iterator(self->begin(), self->end());}); + py::class_, std::shared_ptr>>(m_, "MyFactorPosePoint2") .def(py::init>(), py::arg("key1"), py::arg("key2"), py::arg("measured"), py::arg("noiseModel")) .def("print",[](MyFactor* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter) diff --git a/tests/fixtures/class.i b/tests/fixtures/class.i index 766f55329..775bbc737 100644 --- a/tests/fixtures/class.i +++ b/tests/fixtures/class.i @@ -145,3 +145,12 @@ class TemplatedConstructor { class SuperCoolFactor; typedef SuperCoolFactor SuperCoolFactorPose3; + +/// @brief class with dunder methods for container behavior +class FastSet { + FastSet(); + + __len__(); + __contains__(size_t key); + __iter__(); +}; \ No newline at end of file diff --git a/tests/test_interface_parser.py b/tests/test_interface_parser.py index 45415995f..2a923b3c5 100644 --- a/tests/test_interface_parser.py +++ b/tests/test_interface_parser.py @@ -18,11 +18,12 @@ import unittest sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -from gtwrap.interface_parser import (ArgumentList, Class, Constructor, Enum, - Enumerator, ForwardDeclaration, - GlobalFunction, Include, Method, Module, - Namespace, Operator, ReturnType, - StaticMethod, TemplatedType, Type, +from gtwrap.interface_parser import (ArgumentList, Class, Constructor, + DunderMethod, Enum, Enumerator, + ForwardDeclaration, GlobalFunction, + Include, Method, Module, Namespace, + Operator, ReturnType, StaticMethod, + TemplatedType, Type, TypedefTemplateInstantiation, Typename, Variable) from gtwrap.template_instantiator.classes import InstantiatedClass @@ -344,6 +345,17 @@ class TestInterfaceParser(unittest.TestCase): self.assertEqual(1, len(ret.args)) self.assertEqual("const T & name", ret.args.args_list[0].to_cpp()) + def test_dunder_method(self): + """Test for special python dunder methods.""" + iter_string = "__iter__();" + ret = DunderMethod.rule.parse_string(iter_string)[0] + self.assertEqual("iter", ret.name) + + contains_string = "__contains__(size_t key);" + ret = DunderMethod.rule.parse_string(contains_string)[0] + self.assertEqual("contains", ret.name) + self.assertTrue(len(ret.args) == 1) + def test_operator_overload(self): """Test for operator overloading.""" # Unary operator From f4ea5511535b21b203d9c540bceb4d2c0c80145c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jan 2024 15:17:16 -0500 Subject: [PATCH 57/73] add dunder methods to interface file --- gtsam/gtsam.i | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 834d5a147..9cead2a1c 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -39,6 +39,11 @@ class KeyList { void remove(size_t key); void serialize() const; + + // Specual dunder methods for Python wrapping + __len__(); + __contains__(size_t key); + __iter__(); }; // Actually a FastSet @@ -64,6 +69,11 @@ class KeySet { bool count(size_t key) const; // returns true if value exists void serialize() const; + + // Specual dunder methods for Python wrapping + __len__(); + __contains__(size_t key); + __iter__(); }; // Actually a vector, needed for Matlab @@ -85,6 +95,11 @@ class KeyVector { void push_back(size_t key) const; void serialize() const; + + // Specual dunder methods for Python wrapping + __len__(); + __contains__(size_t key); + __iter__(); }; // Actually a FastMap From b2e4fa2112397f5120e113e90670dea6c84d6d07 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jan 2024 15:17:26 -0500 Subject: [PATCH 58/73] tests for dunder methods --- python/gtsam/tests/test_Utilities.py | 36 +++++++++++++++++++++++++++- 1 file changed, 35 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Utilities.py b/python/gtsam/tests/test_Utilities.py index 851684f12..3dd472c75 100644 --- a/python/gtsam/tests/test_Utilities.py +++ b/python/gtsam/tests/test_Utilities.py @@ -12,13 +12,14 @@ Author: Varun Agrawal import unittest import numpy as np +from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam.utils.test_case import GtsamTestCase class TestUtilites(GtsamTestCase): """Test various GTSAM utilities.""" + def test_createKeyList(self): """Test createKeyList.""" I = [0, 1, 2] @@ -28,6 +29,17 @@ class TestUtilites(GtsamTestCase): kl = gtsam.utilities.createKeyList("s", I) self.assertEqual(kl.size(), 3) + def test_KeyList_iteration(self): + """Tests for KeyList iteration""" + I = [0, 1, 2] + kl = gtsam.utilities.createKeyList(I) + + self.assertEqual(len(kl), len(I)) + + for i, key in enumerate(kl): + self.assertTrue(key in kl) + self.assertEqual(I[i], key) + def test_createKeyVector(self): """Test createKeyVector.""" I = [0, 1, 2] @@ -37,6 +49,17 @@ class TestUtilites(GtsamTestCase): kl = gtsam.utilities.createKeyVector("s", I) self.assertEqual(len(kl), 3) + def test_KeyVector_iteration(self): + """Tests for KeyVector iteration""" + I = [0, 1, 2] + kv = gtsam.utilities.createKeyVector(I) + + self.assertEqual(len(kv), len(I)) + + for i, key in enumerate(kv): + self.assertTrue(key in kv) + self.assertEqual(I[i], key) + def test_createKeySet(self): """Test createKeySet.""" I = [0, 1, 2] @@ -46,6 +69,17 @@ class TestUtilites(GtsamTestCase): kl = gtsam.utilities.createKeySet("s", I) self.assertEqual(kl.size(), 3) + def test_KeySet_iteration(self): + """Tests for KeySet iteration""" + I = [0, 1, 2] + ks = gtsam.utilities.createKeySet(I) + + self.assertEqual(len(ks), len(I)) + + for i, key in enumerate(ks): + self.assertTrue(key in ks) + self.assertEqual(I[i], key) + def test_extractPoint2(self): """Test extractPoint2.""" initial = gtsam.Values() From b104fd66901d63ce27949a311bf2b720d78e9d75 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Tue, 16 Jan 2024 22:36:08 +0200 Subject: [PATCH 59/73] fail-fast: true --- .github/workflows/build-python.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index bb10b27c3..520e94c09 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -22,7 +22,7 @@ jobs: BOOST_EXE: boost_1_72_0-msvc-14.2 strategy: - fail-fast: false + fail-fast: true matrix: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. From 1979f027d5994893e952906e8e2d80428cac90a1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jan 2024 15:46:01 -0500 Subject: [PATCH 60/73] fix comment typo --- gtsam/gtsam.i | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 9cead2a1c..6d77e8eda 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -40,7 +40,7 @@ class KeyList { void serialize() const; - // Specual dunder methods for Python wrapping + // Special dunder methods for Python wrapping __len__(); __contains__(size_t key); __iter__(); @@ -70,7 +70,7 @@ class KeySet { void serialize() const; - // Specual dunder methods for Python wrapping + // Special dunder methods for Python wrapping __len__(); __contains__(size_t key); __iter__(); @@ -96,7 +96,7 @@ class KeyVector { void serialize() const; - // Specual dunder methods for Python wrapping + // Special dunder methods for Python wrapping __len__(); __contains__(size_t key); __iter__(); From 6cc16a3943f48992c0090e9ebe88f8c13ac7407c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jan 2024 19:32:20 -0500 Subject: [PATCH 61/73] delete cephes_names.h to remove collisions --- gtsam/3rdparty/cephes/CMakeLists.txt | 1 - gtsam/3rdparty/cephes/cephes.h | 1 - gtsam/3rdparty/cephes/cephes/cephes_names.h | 114 -------------------- gtsam/3rdparty/cephes/cephes/mconf.h | 1 - 4 files changed, 117 deletions(-) delete mode 100644 gtsam/3rdparty/cephes/cephes/cephes_names.h diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt index 946448160..5940d39d2 100644 --- a/gtsam/3rdparty/cephes/CMakeLists.txt +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -8,7 +8,6 @@ project( set(CEPHES_HEADER_FILES cephes.h - cephes/cephes_names.h cephes/dd_idefs.h cephes/dd_real.h cephes/dd_real_idefs.h diff --git a/gtsam/3rdparty/cephes/cephes.h b/gtsam/3rdparty/cephes/cephes.h index d5b59d895..ed53e521b 100644 --- a/gtsam/3rdparty/cephes/cephes.h +++ b/gtsam/3rdparty/cephes/cephes.h @@ -1,7 +1,6 @@ #ifndef CEPHES_H #define CEPHES_H -#include "cephes/cephes_names.h" #include "dllexport.h" #ifdef __cplusplus diff --git a/gtsam/3rdparty/cephes/cephes/cephes_names.h b/gtsam/3rdparty/cephes/cephes/cephes_names.h deleted file mode 100644 index 94be8c880..000000000 --- a/gtsam/3rdparty/cephes/cephes/cephes_names.h +++ /dev/null @@ -1,114 +0,0 @@ -#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 // Commented out since it clashes with std::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/mconf.h b/gtsam/3rdparty/cephes/cephes/mconf.h index c59d17a47..5e971afad 100644 --- a/gtsam/3rdparty/cephes/cephes/mconf.h +++ b/gtsam/3rdparty/cephes/cephes/mconf.h @@ -56,7 +56,6 @@ #include #include -#include "cephes_names.h" #include "cephes.h" #include "polevl.h" #include "sf_error.h" From 6680d7de4cdc8dd5ed3222cec4aad19d26da893d Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Wed, 17 Jan 2024 08:03:39 +0200 Subject: [PATCH 62/73] remove swap from python ci --- .github/workflows/build-python.yml | 6 ------ 1 file changed, 6 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 520e94c09..de9d755ba 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -145,12 +145,6 @@ jobs: echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" - - name: Set Swap Space (Linux) - if: runner.os == 'Linux' - uses: pierotofy/set-swap-space@master - with: - swap-size-gb: 6 - - name: Install System Dependencies (Linux, macOS) if: runner.os != 'Windows' run: | From 046a9f5b436a206fecc356fb36456fe1bb4c3df8 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 26 Jan 2024 11:58:14 +0200 Subject: [PATCH 63/73] Ubuntu 22.04 for python tbb. hopefully will solve the hang problem. --- .github/workflows/build-python.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index de9d755ba..d8dfce0ee 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -29,7 +29,7 @@ jobs: name: [ ubuntu-20.04-gcc-9, - ubuntu-20.04-gcc-9-tbb, + ubuntu-22.04-gcc-9-tbb, ubuntu-20.04-clang-9, macOS-11-xcode-13.4.1, windows-2019-msbuild, @@ -43,8 +43,8 @@ jobs: compiler: gcc version: "9" - - name: ubuntu-20.04-gcc-9-tbb - os: ubuntu-20.04 + - name: ubuntu-22.04-gcc-9-tbb + os: ubuntu-22.04 compiler: gcc version: "9" flag: tbb From 17a4e4c17479ea6ec05494b9cd9780a3927f6d44 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Sat, 10 Feb 2024 12:52:24 +0200 Subject: [PATCH 64/73] #if BOOST_VERSION < 108000 --- gtsam/base/std_optional_serialization.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 5c250eab4..0ab778761 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -11,6 +11,8 @@ // Defined only if boost serialization is enabled #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +// Only for old boost +#if BOOST_VERSION < 108000 #pragma once #include #include @@ -99,3 +101,4 @@ void serialize(Archive& ar, std::optional& t, const unsigned int version) { } // namespace serialization } // namespace boost #endif +#endif From 26f57ce3b2b84a068761adb4dc5c740861a419ac Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 22 Feb 2024 15:07:24 -0500 Subject: [PATCH 65/73] wrap barometric factor --- gtsam/base/std_optional_serialization.h | 3 +++ gtsam/navigation/BarometricFactor.h | 2 +- gtsam/navigation/navigation.i | 21 +++++++++++++++++++-- 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 5c250eab4..ac0c16c87 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -56,6 +56,8 @@ namespace std { template<> struct is_trivially_move_constructible& t, const unsigned int version) { } // namespace serialization } // namespace boost #endif +#endif diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h index 38677ed58..70cae8d36 100644 --- a/gtsam/navigation/BarometricFactor.h +++ b/gtsam/navigation/BarometricFactor.h @@ -91,7 +91,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN { -0.00649; } - inline double baroOut(const double& meters) { + inline double baroOut(const double& meters) const { double temp = 15.04 - 0.00649 * meters; return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256); } diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 8e6090e06..92864c18a 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -294,7 +294,7 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GPSFactor& expected, double tol); + bool equals(const gtsam::NonlinearFactor& expected, double tol); // Standard Interface gtsam::Point3 measurementIn() const; @@ -307,12 +307,29 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GPSFactor2& expected, double tol); + bool equals(const gtsam::NonlinearFactor& expected, double tol); // Standard Interface gtsam::Point3 measurementIn() const; }; +#include +virtual class BarometricFactor : gtsam::NonlinearFactor { + BarometricFactor(); + BarometricFactor(size_t key, size_t baroKey, const double& baroIn, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol); + + // Standard Interface + const double& measurementIn() const; + double heightOut(double n) const; + double baroOut(const double& meters) const; +}; + #include virtual class Scenario { gtsam::Pose3 pose(double t) const; From a47539506b3a6893de31bc95b1b9a9b9a5149096 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Feb 2024 08:39:47 -0500 Subject: [PATCH 66/73] undo serialization header change --- gtsam/base/std_optional_serialization.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index ac0c16c87..5c250eab4 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -56,8 +56,6 @@ namespace std { template<> struct is_trivially_move_constructible& t, const unsigned int version) { } // namespace serialization } // namespace boost #endif -#endif From 9b2c4787ac7132cd04f44139da7b0286ac491434 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Feb 2024 08:44:36 -0500 Subject: [PATCH 67/73] Revert "Merge pull request #1719 from talregev/TalR/python_tbb_ubuntu_22_04" This reverts commit f724f303889cbde32b097289d64cc771a9ee4369, reversing changes made to 448132af2746ff7164db88ac2cadd93cedbd0813. --- .github/workflows/build-python.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index d8dfce0ee..de9d755ba 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -29,7 +29,7 @@ jobs: name: [ ubuntu-20.04-gcc-9, - ubuntu-22.04-gcc-9-tbb, + ubuntu-20.04-gcc-9-tbb, ubuntu-20.04-clang-9, macOS-11-xcode-13.4.1, windows-2019-msbuild, @@ -43,8 +43,8 @@ jobs: compiler: gcc version: "9" - - name: ubuntu-22.04-gcc-9-tbb - os: ubuntu-22.04 + - name: ubuntu-20.04-gcc-9-tbb + os: ubuntu-20.04 compiler: gcc version: "9" flag: tbb From 67cf8706d837126c96586b16e9cbbb147169a84b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Feb 2024 08:50:51 -0500 Subject: [PATCH 68/73] Revert "Merge pull request #1714 from talregev/TalR/remove_swap" This reverts commit 448132af2746ff7164db88ac2cadd93cedbd0813, reversing changes made to 2dfd15e16ce072931ee58e33866383919b38d8c0. --- .github/workflows/build-python.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index de9d755ba..520e94c09 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -145,6 +145,12 @@ jobs: echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" + - name: Set Swap Space (Linux) + if: runner.os == 'Linux' + uses: pierotofy/set-swap-space@master + with: + swap-size-gb: 6 + - name: Install System Dependencies (Linux, macOS) if: runner.os != 'Windows' run: | From 0d7d159203ad5fa436b755ddd0133afd5668e0a1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Feb 2024 09:14:39 -0500 Subject: [PATCH 69/73] fix for boost serialization collision --- gtsam/base/std_optional_serialization.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 5c250eab4..079cb7745 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -56,6 +56,7 @@ namespace std { template<> struct is_trivially_move_constructible& t, const unsigned int version) { } // namespace serialization } // namespace boost #endif +#endif From 2379cfab9a366fac203a2ef15329194cefd127d8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Feb 2024 09:35:46 -0500 Subject: [PATCH 70/73] change C++ version to check against --- gtsam/base/std_optional_serialization.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 079cb7745..34556ada4 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -56,7 +56,7 @@ namespace std { template<> struct is_trivially_move_constructible Date: Mon, 26 Feb 2024 17:04:15 -0500 Subject: [PATCH 71/73] correctly fix name collisions due to newly merged Boost PR --- gtsam/base/std_optional_serialization.h | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 34556ada4..2fc829a85 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -55,8 +55,14 @@ namespace std { template<> struct is_trivially_move_constructible Date: Thu, 29 Feb 2024 15:12:57 -0800 Subject: [PATCH 72/73] Close unmatched endif --- gtsam/base/std_optional_serialization.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 444c31703..7e30ae4d3 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -8,12 +8,12 @@ * Functionality to serialize std::optional to boost::archive * Inspired from this PR: https://github.com/boostorg/serialization/pull/163 * ---------------------------------------------------------------------------- */ +#pragma once // Defined only if boost serialization is enabled #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION // Only for old boost #if BOOST_VERSION < 108000 -#pragma once #include #include @@ -107,5 +107,6 @@ void serialize(Archive& ar, std::optional& t, const unsigned int version) { } // namespace serialization } // namespace boost -#endif -#endif +#endif // BOOST_VERSION < 108400 +#endif // BOOST_VERSION < 108000 +#endif // GTSAM_ENABLE_BOOST_SERIALIZATION From 4947a101329772fbe530eb773d232ce3e5d5c744 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 3 Mar 2024 10:20:29 -0500 Subject: [PATCH 73/73] remove macro block for boost version 108000 --- gtsam/base/std_optional_serialization.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 7e30ae4d3..7e0f2e844 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -12,8 +12,6 @@ // Defined only if boost serialization is enabled #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION -// Only for old boost -#if BOOST_VERSION < 108000 #include #include @@ -108,5 +106,4 @@ void serialize(Archive& ar, std::optional& t, const unsigned int version) { } // namespace serialization } // namespace boost #endif // BOOST_VERSION < 108400 -#endif // BOOST_VERSION < 108000 #endif // GTSAM_ENABLE_BOOST_SERIALIZATION