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); }