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(