12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

minor refactor to be consistent

release/4.3a0
Varun Agrawal 2023-12-28 09:36:04 -05:00
parent 5481159f95
commit ea81675393
6 changed files with 47 additions and 975 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/nonlinear/GncOptimizer.h
View File

@ -28,7 +28,7 @@
#include <gtsam/nonlinear/GncParams.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/internal/chiSquaredInverse.h>
#include <gtsam/nonlinear/internal/ChiSquaredInverse.h>
namespace gtsam {
/*

45
gtsam/nonlinear/internal/ChiSquaredInverse.h Normal file
View File

@ -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 <gtsam/3rdparty/cephes/cephes.h>
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

537
gtsam/nonlinear/internal/Gamma.h
View File

@ -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 <gtsam/nonlinear/internal/Utils.h>
#include <boost/math/special_functions/gamma.hpp>
#include <boost/math/special_functions/trunc.hpp>
namespace gtsam {
namespace internal {
template <class T>
inline constexpr T log_max_value() {
return log(LIM<T>::max());
}
/**
* @brief Upper gamma fraction for integer a
*
* @param a
* @param x
* @param pol
* @param pderivative
* @return template <class T, class Policy>
*/
template <class T, class Policy>
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<T>(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 <class T, class Policy>
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 <class T>
struct upper_incomplete_gamma_fract {
private:
T z, a;
int k;
public:
typedef std::pair<T, T> 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 <class T>
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<T> 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 <class T, class Policy>
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<T, Policy>::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<T, Policy>()));
} 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<T>()) {
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<T>());
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<T>()) && (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<T>::is_specialized && (a > 20)) {
T sigma = fabs((x - a) / a);
if ((a > 200) && (boost::math::policies::digits<T, Policy>() <= 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<T, Policy>() <= 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<T>::max() * result > init_value)) {
init_value /= result;
if (normalised || (a < 1) || (LIM<T>::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<T, Policy>());
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<T, Policy>::type
precision_type;
typedef std::integral_constant<int, precision_type::value <= 0 ? 0
: precision_type::value <= 53 ? 53
: precision_type::value <= 64 ? 64
: precision_type::value <= 113 ? 113
: 0>
tag_type;
result = boost::math::detail::igamma_temme_large(
a, x, pol, static_cast<tag_type const*>(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<T>::max() * x < *p_derivative)) {
// overflow, just return an arbitrarily large value:
*p_derivative = LIM<T>::max() / 2;
}
*p_derivative /= x;
}
return result;
}
/**
* @brief Functional to compute the gamma inverse.
* Mainly used with Halley iteration.
*
* @tparam T
*/
template <class T>
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<T, T, T> 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<T>(
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<T>::max() / fabs(div) < f2) {
// overflow:
f2 = -internal::LIM<T>::max() / 2;
} else {
f2 *= div;
}
} else {
f2 *= div;
}
if (invert) {
f1 = -f1;
f2 = -f2;
}
return std::make_tuple(static_cast<T>(f - p), f1, f2);
}
private:
T a, p;
bool invert;
};
} // namespace internal
} // namespace gtsam

49
gtsam/nonlinear/internal/Utils.h
View File

@ -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 <class T>
using LIM = std::numeric_limits<T>;
template <typename T>
using return_t =
typename std::conditional<std::is_integral<T>::value, double, T>::type;
/// Get common type amongst all arguments
template <typename... T>
using common_t = typename std::common_type<T...>::type;
/// Helper template for finding common return type
template <typename... T>
using common_return_t = return_t<common_t<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 <typename T>
constexpr bool is_nan(const T x) noexcept {
return x != x;
}
} // namespace internal
} // namespace gtsam

387
gtsam/nonlinear/internal/chiSquaredInverse.h
View File

@ -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 <gtsam/nonlinear/internal/Gamma.h>
#include <gtsam/nonlinear/internal/Utils.h>
#include <algorithm>
// TODO(Varun) remove
#include <boost/math/tools/roots.hpp>
namespace gtsam {
namespace internal {
/**
* @brief Polynomial evaluation with runtime size.
*
* @tparam T
* @tparam U
*/
template <class T, class U>
inline U evaluate_polynomial(const T* poly, U const& z, std::size_t count) {
assert(count > 0);
U sum = static_cast<U>(poly[count - 1]);
for (int i = static_cast<int>(count) - 2; i >= 0; --i) {
sum *= z;
sum += static_cast<U>(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 <class T>
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 <class T>
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 <class T>
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<double> 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 <typename T>
T gamma_p_inv_imp(const T a, const T p) {
if (is_nan(a) || is_nan(p)) {
return LIM<T>::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<T>(a, p, 1 - p, &has_10_digits);
if (has_10_digits) {
return guess;
}
T lower = LIM<T>::min();
if (guess <= lower) {
guess = LIM<T>::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<T>(a, p, false), guess, lower,
// LIM<T>::max(), digits, max_iter);
// Go ahead and iterate:
guess = boost::math::tools::halley_iterate(
internal::gamma_p_inverse_func<T>(a, p, false), guess, lower,
LIM<T>::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 <typename T1, typename T2>
constexpr common_return_t<T1, T2> incomplete_gamma_inv(const T1 a,
const T2 p) noexcept {
return internal::gamma_p_inv_imp(static_cast<common_return_t<T1, T2>>(a),
static_cast<common_return_t<T1, T2>>(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

2
gtsam/nonlinear/tests/testChiSquaredInverse.cpp
View File

@ -18,7 +18,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/internal/chiSquaredInverse.h>
#include <gtsam/nonlinear/internal/ChiSquaredInverse.h>
using namespace gtsam;