From ea81675393e3bc13b30f3519c88453363cf2a93b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 09:36:04 -0500 Subject: [PATCH] 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;