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

fix chi_squared_quantile

release/4.3a0
Varun Agrawal 2023-07-10 12:54:32 -04:00
parent 64c28504ad
commit a5fd9c120b
2 changed files with 121 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

168
gtsam/nonlinear/GncHelpers.h
View File

@ -29,9 +29,11 @@ template <typename T>
using return_t = using return_t =
typename std::conditional<std::is_integral<T>::value, double, T>::type; typename std::conditional<std::is_integral<T>::value, double, T>::type;
/// Get common type amongst all arguments
template <typename... T> template <typename... T>
using common_t = typename std::common_type<T...>::type; using common_t = typename std::common_type<T...>::type;
/// Helper template for finding common return type
template <typename... T> template <typename... T>
using common_return_t = return_t<common_t<T...>>; using common_return_t = return_t<common_t<T...>>;
@ -126,16 +128,14 @@ template <typename T>
constexpr T incomplete_gamma_quad_recur(const T lb, const T ub, const T a, constexpr T incomplete_gamma_quad_recur(const T lb, const T ub, const T a,
const T lg_term, const T lg_term,
const int counter) noexcept { const int counter) noexcept {
return (counter < 49 ? // if T val = incomplete_gamma_quad_fn(
incomplete_gamma_quad_fn( incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) *
incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * incomplete_gamma_quad_weight_vals(lb, ub, counter);
incomplete_gamma_quad_weight_vals(lb, ub, counter) + if (counter < 49) {
incomplete_gamma_quad_recur(lb, ub, a, lg_term, counter + 1) return val + incomplete_gamma_quad_recur(lb, ub, a, lg_term, counter + 1);
: } else {
// else return val;
incomplete_gamma_quad_fn( }
incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) *
incomplete_gamma_quad_weight_vals(lb, ub, counter));
} }
template <typename T> template <typename T>
@ -180,10 +180,13 @@ constexpr T incomplete_gamma_quad(const T a, const T z) noexcept {
template <typename T> template <typename T>
constexpr T incomplete_gamma_cf_2_recur(const T a, const T z, constexpr T incomplete_gamma_cf_2_recur(const T a, const T z,
const int depth) noexcept { const int depth) noexcept {
return (depth < 100 ? (1 + (depth - 1) * 2 - a + z) + T val = 1 + (depth - 1) * 2 - a + z;
depth * (a - depth) / if (depth < 100) {
incomplete_gamma_cf_2_recur(a, z, depth + 1) return val +
: (1 + (depth - 1) * 2 - a + z)); depth * (a - depth) / incomplete_gamma_cf_2_recur(a, z, depth + 1);
} else {
return val;
}
} }
template <typename T> template <typename T>
@ -200,50 +203,49 @@ constexpr T incomplete_gamma_cf_2(
template <typename T> template <typename T>
constexpr T incomplete_gamma_cf_1_coef(const T a, const T z, constexpr T incomplete_gamma_cf_1_coef(const T a, const T z,
const int depth) noexcept { const int depth) noexcept {
return (is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z if (is_odd(depth)) {
: T(depth) / T(2) * z); return -(a - 1 + T(depth + 1) / T(2)) * z;
} else {
return T(depth) / T(2) * z;
}
} }
template <typename T> template <typename T>
constexpr T incomplete_gamma_cf_1_recur(const T a, const T z, constexpr T incomplete_gamma_cf_1_recur(const T a, const T z,
const int depth) noexcept { const int depth) noexcept {
return (depth < 55 ? // if T val = a + depth - 1;
(a + depth - 1) + incomplete_gamma_cf_1_coef(a, z, depth) / if (depth < 55) {
incomplete_gamma_cf_1_recur(a, z, depth + 1) return val + incomplete_gamma_cf_1_coef(a, z, depth) /
: incomplete_gamma_cf_1_recur(a, z, depth + 1);
// else } else {
(a + depth - 1)); return val;
}
} }
/// lower (regularized) incomplete gamma function
template <typename T> template <typename T>
constexpr T incomplete_gamma_cf_1( constexpr T incomplete_gamma_cf_1(const T a, const T z) noexcept {
const T a, return exp(a * log(z) - z - lgamma(a)) / incomplete_gamma_cf_1_recur(a, z, 1);
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));
} }
// /// Perform NaN check
template <typename T> template <typename T>
constexpr T incomplete_gamma_check(const T a, const T z) noexcept { constexpr T incomplete_gamma_check(const T a, const T z) noexcept {
return ( // NaN check if (is_nan(a) || is_nan(z)) {
(is_nan(a) || is_nan(z)) ? LIM<T>::quiet_NaN() : return LIM<T>::quiet_NaN();
// } else if (a < T(0)) {
a < T(0) ? LIM<T>::quiet_NaN() return LIM<T>::quiet_NaN();
: } else if (LIM<T>::min() > z) {
// return T(0);
LIM<T>::min() > z ? T(0) } else if (LIM<T>::min() > a) {
: return T(1);
// } else if (a < T(10) && z - a < T(10)) { // cf or quadrature
LIM<T>::min() > a ? T(1) return incomplete_gamma_cf_1(a, z);
: } else if (a < T(10) || z / a > T(3)) {
// cf or quadrature return incomplete_gamma_cf_2(a, z);
(a < T(10)) && (z - a < T(10)) ? incomplete_gamma_cf_1(a, z) } else {
: (a < T(10)) || (z / a > T(3)) ? incomplete_gamma_cf_2(a, z) return incomplete_gamma_quad(a, z);
: }
// else
incomplete_gamma_quad(a, z));
} }
template <typename T1, typename T2, typename TC = common_return_t<T1, T2>> template <typename T1, typename T2, typename TC = common_return_t<T1, T2>>
@ -323,24 +325,24 @@ constexpr T incomplete_gamma_inv_initial_val_1(
template <typename T> template <typename T>
constexpr T incomplete_gamma_inv_initial_val_2( constexpr T incomplete_gamma_inv_initial_val_2(
const T a, const T p, const T t_val) noexcept { // a <= 1.0 const T a, const T p, const T t_val) noexcept { // a <= 1.0
return (p < t_val ? // if if (p < t_val) {
pow(p / t_val, T(1) / a) return pow(p / t_val, T(1) / a);
: } else {
// else return T(1) - log(T(1) - (p - t_val) / (T(1) - t_val));
T(1) - log(T(1) - (p - t_val) / (T(1) - t_val))); }
} }
// initial value // Initial value
template <typename T> template <typename T>
constexpr T incomplete_gamma_inv_initial_val(const T a, const T p) noexcept { constexpr T incomplete_gamma_inv_initial_val(const T a, const T p) noexcept {
return (a > T(1) ? // if if (a > T(1)) {
incomplete_gamma_inv_initial_val_1( return incomplete_gamma_inv_initial_val_1(
a, incomplete_gamma_inv_t_val_1(p), p > T(0.5) ? T(-1) : T(1)) a, incomplete_gamma_inv_t_val_1(p), p > T(0.5) ? T(-1) : T(1));
: } else {
// else return incomplete_gamma_inv_initial_val_2(a, p,
incomplete_gamma_inv_initial_val_2( incomplete_gamma_inv_t_val_2(a));
a, p, incomplete_gamma_inv_t_val_2(a))); }
} }
// //
@ -405,18 +407,15 @@ template <typename T>
constexpr T incomplete_gamma_inv_decision(const T value, const T a, const T p, constexpr T incomplete_gamma_inv_decision(const T value, const T a, const T p,
const T direc, const T lg_val, const T direc, const T lg_val,
const int iter_count) noexcept { const int iter_count) noexcept {
// return( abs(direc) > GCEM_INCML_GAMMA_INV_TOL ? constexpr int INCML_GAMMA_INV_MAX_ITER = 35;
// incomplete_gamma_inv_recur(value - direc, a, p,
// incomplete_gamma_inv_deriv_1(value,a,lg_val), lg_val) : value - direc ); if (iter_count <= INCML_GAMMA_INV_MAX_ITER) {
#define INCML_GAMMA_INV_MAX_ITER 35 return incomplete_gamma_inv_recur(
return (iter_count <= INCML_GAMMA_INV_MAX_ITER ? // if value - direc, a, p, incomplete_gamma_inv_deriv_1(value, a, lg_val),
incomplete_gamma_inv_recur( lg_val, iter_count + 1);
value - direc, a, p, } else {
incomplete_gamma_inv_deriv_1(value, a, lg_val), lg_val, return value - direc;
iter_count + 1) }
:
// else
value - direc);
} }
template <typename T> template <typename T>
@ -429,19 +428,20 @@ constexpr T incomplete_gamma_inv_begin(const T initial_val, const T a,
template <typename T> template <typename T>
constexpr T incomplete_gamma_inv_check(const T a, const T p) noexcept { constexpr T incomplete_gamma_inv_check(const T a, const T p) noexcept {
return ( // NaN check if (is_nan(a) || is_nan(p)) {
(is_nan(a) || is_nan(p)) ? LIM<T>::quiet_NaN() : return LIM<T>::quiet_NaN();
// } else if (LIM<T>::min() > p) {
LIM<T>::min() > p ? T(0) return T(0);
: p > T(1) ? LIM<T>::quiet_NaN() } else if (p > T(1)) {
: LIM<T>::min() > abs(T(1) - p) ? LIM<T>::infinity() return LIM<T>::quiet_NaN();
: } else if (LIM<T>::min() > fabs(T(1) - p)) {
// return LIM<T>::infinity();
LIM<T>::min() > a ? T(0) } else if (LIM<T>::min() > a) {
: return T(0);
// else } else {
incomplete_gamma_inv_begin(incomplete_gamma_inv_initial_val(a, p), a, return incomplete_gamma_inv_begin(incomplete_gamma_inv_initial_val(a, p), a,
p, lgamma(a))); p, lgamma(a));
}
} }
template <typename T1, typename T2, typename TC = common_return_t<T1, T2>> template <typename T1, typename T2, typename TC = common_return_t<T1, T2>>

37
gtsam/nonlinear/tests/testGncHelpers.cpp Normal file
View File

@ -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 <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/nonlinear/GncHelpers.h>
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);
}
/* ************************************************************************* */