From d472362614f63f286135d3dc0ec6b910360e076e Mon Sep 17 00:00:00 2001 From: klaas kelchtermans Date: Thu, 4 May 2023 11:11:45 +0200 Subject: [PATCH 001/208] add ProjectionFactorPPPCal3Fisheye --- gtsam_unstable/gtsam_unstable.i | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index f36118873..eea4d861a 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -699,6 +699,7 @@ virtual class OdometryFactorBase : gtsam::NoiseModelFactor { #include #include +#include template virtual class ProjectionFactorPPP : gtsam::NoiseModelFactor { ProjectionFactorPPP(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel, @@ -717,7 +718,7 @@ virtual class ProjectionFactorPPP : gtsam::NoiseModelFactor { }; typedef gtsam::ProjectionFactorPPP ProjectionFactorPPPCal3_S2; typedef gtsam::ProjectionFactorPPP ProjectionFactorPPPCal3DS2; - +typedef gtsam::ProjectionFactorPPP ProjectionFactorPPPCal3Fisheye; #include template @@ -737,6 +738,7 @@ virtual class ProjectionFactorPPPC : gtsam::NoiseModelFactor { }; typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCCal3_S2; typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCCal3DS2; +typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCal3Fisheye; #include virtual class ProjectionFactorRollingShutter : gtsam::NoiseModelFactor { From 8aff7b52f0132b6930cb593893f0189747e55bad Mon Sep 17 00:00:00 2001 From: klaas kelchtermans Date: Thu, 4 May 2023 13:46:35 +0200 Subject: [PATCH 002/208] typo --- gtsam_unstable/gtsam_unstable.i | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam_unstable/gtsam_unstable.i b/gtsam_unstable/gtsam_unstable.i index eea4d861a..9323255ad 100644 --- a/gtsam_unstable/gtsam_unstable.i +++ b/gtsam_unstable/gtsam_unstable.i @@ -738,7 +738,7 @@ virtual class ProjectionFactorPPPC : gtsam::NoiseModelFactor { }; typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCCal3_S2; typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCCal3DS2; -typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCal3Fisheye; +typedef gtsam::ProjectionFactorPPPC ProjectionFactorPPPCCal3Fisheye; #include virtual class ProjectionFactorRollingShutter : gtsam::NoiseModelFactor { From d0b3f1dd25a7e7b7e4e49bbde024e9fa00170ac9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 May 2023 14:52:13 -0400 Subject: [PATCH 003/208] code for computing quantile of chi-squared distribution --- gtsam/nonlinear/GncHelpers.h | 516 +++++++++++++++++++++++++++++++++++ 1 file changed, 516 insertions(+) create mode 100644 gtsam/nonlinear/GncHelpers.h diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h new file mode 100644 index 000000000..399da2c99 --- /dev/null +++ b/gtsam/nonlinear/GncHelpers.h @@ -0,0 +1,516 @@ +/* ---------------------------------------------------------------------------- + + * 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 GncHelpers.h + * @brief Helper functions for use with the GncOptimizer + * @author Varun Agrawal + */ + +#pragma once + +#include +#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; + +template +using common_t = typename std::common_type::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 { + return (counter < 49 ? // if + incomplete_gamma_quad_fn( + incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * + incomplete_gamma_quad_weight_vals(lb, ub, counter) + + incomplete_gamma_quad_recur(lb, ub, a, lg_term, counter + 1) + : + // else + incomplete_gamma_quad_fn( + incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * + incomplete_gamma_quad_weight_vals(lb, ub, counter)); +} + +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 { + return (depth < 100 ? (1 + (depth - 1) * 2 - a + z) + + depth * (a - depth) / + incomplete_gamma_cf_2_recur(a, z, depth + 1) + : (1 + (depth - 1) * 2 - a + z)); +} + +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 { + return (is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z + : T(depth) / T(2) * z); +} + +template +constexpr T incomplete_gamma_cf_1_recur(const T a, const T z, + const int depth) noexcept { + return (depth < 55 ? // if + (a + depth - 1) + incomplete_gamma_cf_1_coef(a, z, depth) / + incomplete_gamma_cf_1_recur(a, z, depth + 1) + : + // else + (a + depth - 1)); +} + +template +constexpr T incomplete_gamma_cf_1( + const T a, + 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)); +} + +// + +template +constexpr T incomplete_gamma_check(const T a, const T z) noexcept { + return ( // NaN check + (is_nan(a) || is_nan(z)) ? LIM::quiet_NaN() : + // + a < T(0) ? LIM::quiet_NaN() + : + // + LIM::min() > z ? T(0) + : + // + LIM::min() > a ? T(1) + : + // cf or quadrature + (a < T(10)) && (z - a < T(10)) ? incomplete_gamma_cf_1(a, z) + : (a < T(10)) || (z / a > T(3)) ? incomplete_gamma_cf_2(a, z) + : + // else + 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 +class IncompleteGammaInverse { + /** + * @brief Compute an initial value for the inverse gamma function which is + * then iteratively updated. + * + * @param a + * @param p + * @return constexpr T + */ + static constexpr T initial_val(const T a, const T p) noexcept { + if (a > T(1)) { + // Inverse gamma function initial value when a > 1.0 + const T t_val = p > T(0.5) ? sqrt(-2 * log(T(1) - p)) : sqrt(-2 * log(p)); + const T sgn_term = p > T(0.5) ? T(-1) : T(1); + const T initial_val_1 = + 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); + const T signed_initial_val_1 = sgn_term * initial_val_1; + + return std::max( + T(1e-04), + a * pow(T(1) - T(1) / (9 * a) - signed_initial_val_1 / (3 * sqrt(a)), + 3)); + } else { + // Inverse gamma function initial value when a <= 1.0 + T t_val = T(1) - T(0.253) * a - T(0.12) * a * a; + 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)); + } + } + } + + /** + * @brief Compute the error value `f(x)` which we can use for root-finding. + * + * @param value + * @param a + * @param p + * @return constexpr T + */ + static constexpr T err_val(const T value, const T a, const T p) noexcept { + return (incomplete_gamma(a, value) - p); + } + + /** + * @brief Derivative of the incomplete gamma function w.r.t. value + * + * @param value + * @param a + * @param log_val + * @return constexpr T + */ + static constexpr T derivative(const T value, const T a, + const T lg_val) noexcept { + return (exp(-value + (a - T(1)) * log(value) - lg_val)); + } + + /** + * @brief Second derivative of the incomplete gamma function w.r.t. value + * + * @param value + * @param a + * @param derivative + * @return constexpr T + */ + static constexpr T second_derivative(const T value, const T a, + const T derivative) noexcept { + return (derivative * ((a - T(1)) / value - T(1))); + } + + /** + * @brief Compute \f[ \frac{f(x_n)}{f'(x_n)} \f] as part + * of the update denominator. + * + * @param value + * @param a + * @param p + * @param derivative + * @return constexpr T + */ + static constexpr T ratio_val_1(const T value, const T a, const T p, + const T derivative) noexcept { + return (err_val(value, a, p) / derivative); + } + + /** + * @brief Compute \f[ \frac{f''(x_n)}{f'(x_n)} \f] as part + * of the update denominator. + * + * @param value + * @param a + * @param derivative + * @return constexpr T + */ + static constexpr T ratio_val_2(const T value, const T a, + const T derivative) noexcept { + return (second_derivative(value, a, derivative) / derivative); + } + + /** + * @brief Halley's method update step + * + * @param ratio_val_1 + * @param ratio_val_2 + * @return constexpr T + */ + static constexpr T 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))); + } + /** + * @brief Recursive method for computing the iterative solution for the + * incomplete inverse gamma function. + * + * @param value + * @param a + * @param p + * @param derivative + * @param lg_val + * @param iter_count + * @return constexpr T + */ + static constexpr T recurse(const T value, const T a, const T p, + const T derivative, const T lg_val, + const int iter_count) noexcept { + return decision(value, a, p, + halley(ratio_val_1(value, a, p, derivative), + ratio_val_2(value, a, derivative)), + lg_val, iter_count); + } + + static constexpr T decision(const T value, const T a, const T p, + const T direc, const T lg_val, + const int iter_count) noexcept { + const int GAMMA_INV_MAX_ITER = 35; + if (iter_count <= GAMMA_INV_MAX_ITER) { + return recurse(value - direc, a, p, derivative(value, a, lg_val), lg_val, + iter_count + 1); + } else { + return value - direc; + } + } + + /** + * @brief Start point for numerical computation of the incomplete gamma + * inverse funtion. + * + * @param initial_val Initial value guess + * @param a + * @param p + * @param lg_val + * @return constexpr T + */ + static constexpr T begin(const T initial_val, const T a, const T p, + const T lg_val) noexcept { + return recurse(initial_val, a, p, derivative(initial_val, a, lg_val), + lg_val, 1); + } + + public: + /** + * @brief Compute the percent point function for the Gamma distribution. + * + * @param a + * @param p + * @return constexpr T + */ + static constexpr T compute(const T a, const T p) noexcept { + // Perform checks on the input and return the corresponding best answer + if (isnan(a) || isnan(p)) { // NaN check + return LIM::quiet_NaN(); + } else if (LIM::min() > p) { // Check lower bound + return T(0); + } else if (p > T(1)) { // Check upper bound + return LIM::quiet_NaN(); + } else if (LIM::min() > abs(T(1) - p)) { + return LIM::infinity(); + } else if (LIM::min() > a) { // Check lower bound for degrees of freedom + return T(0); + } else { + return begin(initial_val(a, p), a, p, lgamma(a)); + } + } +}; + +} // namespace internal + +/** + * Compile-time inverse incomplete gamma function + * + * Compute the 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. + * + * We find 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] + * + * @param a The degrees of freedom for the gamma distribution. + * @param p The quantile value for computing the percent point function. + * + * @return Computes the inverse incomplete gamma function. + */ +template +constexpr common_return_t incomplete_gamma_inv(const T1 a, + const T2 p) noexcept { + using TC = common_return_t; + return internal::IncompleteGammaInverse::compute(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 constexpr double + */ +constexpr double chi_squared_quantile(const size_t 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); +} + +} // namespace gtsam From 8201c77b30b3d69d0a9b775b8eb164be9650ae3c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 May 2023 15:37:46 -0400 Subject: [PATCH 004/208] refactor IncompleteGamma class --- gtsam/nonlinear/GncHelpers.h | 267 +++++++++++++++++------------------ 1 file changed, 131 insertions(+), 136 deletions(-) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 399da2c99..38185249c 100644 --- a/gtsam/nonlinear/GncHelpers.h +++ b/gtsam/nonlinear/GncHelpers.h @@ -103,154 +103,147 @@ static const long double gauss_legendre_50_weights[50] = { 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); -} +class IncompleteGamma { + /// 50 point Gauss-Legendre quadrature + static constexpr T quadrature_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); -} + static constexpr T quadrature_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); -} + static constexpr T quadrature_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 { - return (counter < 49 ? // if - incomplete_gamma_quad_fn( - incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * - incomplete_gamma_quad_weight_vals(lb, ub, counter) + - incomplete_gamma_quad_recur(lb, ub, a, lg_term, counter + 1) - : - // else - incomplete_gamma_quad_fn( - incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * - incomplete_gamma_quad_weight_vals(lb, ub, counter)); -} + static constexpr T quadrature_recur(const T lb, const T ub, const T a, + const T lg_term, + const int counter) noexcept { + if (counter < 49) { + return quadrature_fn(quadrature_inp_vals(lb, ub, counter), a, lg_term) * + quadrature_weight_vals(lb, ub, counter) + + quadrature_recur(lb, ub, a, lg_term, counter + 1); + } else { + return quadrature_fn(quadrature_inp_vals(lb, ub, counter), a, lg_term) * + quadrature_weight_vals(lb, ub, counter); + } + } -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))); -} + static constexpr T quadrature_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))); -} + static constexpr T quadrature_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); -} + static constexpr T quadrature(const T a, const T z) noexcept { + return quadrature_recur(quadrature_lb(a, z), quadrature_ub(a, z), a, + lgamma(a), 0); + } -// reverse cf expansion -// see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ + // reverse cf expansion + // see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ + static constexpr T cf_2_recur(const T a, const T z, + const int depth) noexcept { + if (depth < 100) { + return (1 + (depth - 1) * 2 - a + z) + + depth * (a - depth) / cf_2_recur(a, z, depth + 1); + } else { + return 1 + (depth - 1) * 2 - a + z; + } + } -template -constexpr T incomplete_gamma_cf_2_recur(const T a, const T z, - const int depth) noexcept { - return (depth < 100 ? (1 + (depth - 1) * 2 - a + z) + - depth * (a - depth) / - incomplete_gamma_cf_2_recur(a, z, depth + 1) - : (1 + (depth - 1) * 2 - a + z)); -} + /** + * @brief Lower (regularized) incomplete gamma function + * + * @param a + * @param z + * @return constexpr T + */ + static constexpr T cf_2(const T a, const T z) noexcept { + return T(1.0) - exp(a * log(z) - z - lgamma(a)) / cf_2_recur(a, z, 1); + } -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)); -} + // continued fraction expansion + // see: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ + static constexpr T cf_1_coef(const T a, const T z, const int depth) noexcept { + return (is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z + : T(depth) / T(2) * z); + } -// cf expansion -// see: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ + static constexpr T cf_1_recur(const T a, const T z, + const int depth) noexcept { + if (depth < 55) { + return (a + depth - 1) + + cf_1_coef(a, z, depth) / cf_1_recur(a, z, depth + 1); + } else { + return (a + depth - 1); + } + } -template -constexpr T incomplete_gamma_cf_1_coef(const T a, const T z, - const int depth) noexcept { - return (is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z - : T(depth) / T(2) * z); -} + /** + * @brief Lower (regularized) incomplete gamma function + * + * @param a + * @param z + * @return constexpr T + */ + static constexpr T cf_1(const T a, const T z) noexcept { + return exp(a * log(z) - z - lgamma(a)) / cf_1_recur(a, z, 1); + } -template -constexpr T incomplete_gamma_cf_1_recur(const T a, const T z, - const int depth) noexcept { - return (depth < 55 ? // if - (a + depth - 1) + incomplete_gamma_cf_1_coef(a, z, depth) / - incomplete_gamma_cf_1_recur(a, z, depth + 1) - : - // else - (a + depth - 1)); -} - -template -constexpr T incomplete_gamma_cf_1( - const T a, - 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)); -} - -// - -template -constexpr T incomplete_gamma_check(const T a, const T z) noexcept { - return ( // NaN check - (is_nan(a) || is_nan(z)) ? LIM::quiet_NaN() : - // - a < T(0) ? LIM::quiet_NaN() - : - // - LIM::min() > z ? T(0) - : - // - LIM::min() > a ? T(1) - : - // cf or quadrature - (a < T(10)) && (z - a < T(10)) ? incomplete_gamma_cf_1(a, z) - : (a < T(10)) || (z / a > T(3)) ? incomplete_gamma_cf_2(a, z) - : - // else - 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)); -} + public: + /** + * @brief Compute the CDF for the Gamma distribution. + * + * @param a + * @param z + * @return constexpr T + */ + static constexpr T compute(const T a, const T z) noexcept { + if (is_nan(a) || is_nan(z)) { // NaN check + 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)) { + return cf_1(a, z); + } else if (a < T(10) || z / a > T(3)) { + return cf_2(a, z); + } else { + return quadrature(a, z); + } + } +}; } // namespace internal @@ -274,7 +267,9 @@ constexpr TC incomplete_gamma_type_check(const T1 a, const T2 p) noexcept { template constexpr common_return_t incomplete_gamma(const T1 a, const T2 x) noexcept { - return internal::incomplete_gamma_type_check(a, x); + using TC = common_return_t; + return internal::IncompleteGamma::compute(static_cast(a), + static_cast(x)); } namespace internal { From d5771609a4475b7af937b8d2c7140653d93554fe Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 May 2023 15:54:42 -0400 Subject: [PATCH 005/208] Simplified IncompleteGamma --- gtsam/nonlinear/GncHelpers.h | 69 ++++++++++++++++++++---------------- 1 file changed, 38 insertions(+), 31 deletions(-) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 38185249c..92e2599a5 100644 --- a/gtsam/nonlinear/GncHelpers.h +++ b/gtsam/nonlinear/GncHelpers.h @@ -19,6 +19,7 @@ #include #include +#include namespace gtsam { @@ -105,13 +106,14 @@ namespace internal { template class IncompleteGamma { - /// 50 point Gauss-Legendre quadrature + /// 50 point Gauss-Legendre quadrature values static constexpr T quadrature_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); } + /// 50 point Gauss-Legendre quadrature weights static constexpr T quadrature_weight_vals(const T lb, const T ub, const int counter) noexcept { return (ub - lb) * gauss_legendre_50_weights[counter] / T(2); @@ -122,19 +124,6 @@ class IncompleteGamma { return exp(-x + (a - T(1)) * log(x) - lg_term); } - static constexpr T quadrature_recur(const T lb, const T ub, const T a, - const T lg_term, - const int counter) noexcept { - if (counter < 49) { - return quadrature_fn(quadrature_inp_vals(lb, ub, counter), a, lg_term) * - quadrature_weight_vals(lb, ub, counter) + - quadrature_recur(lb, ub, a, lg_term, counter + 1); - } else { - return quadrature_fn(quadrature_inp_vals(lb, ub, counter), a, lg_term) * - quadrature_weight_vals(lb, ub, counter); - } - } - static constexpr T quadrature_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)) @@ -163,12 +152,27 @@ class IncompleteGamma { } static constexpr T quadrature(const T a, const T z) noexcept { - return quadrature_recur(quadrature_lb(a, z), quadrature_ub(a, z), a, - lgamma(a), 0); + T lb = quadrature_lb(a, z); + T ub = quadrature_ub(a, z); + T lg_term = lgamma(a); + T value = quadrature_fn(quadrature_inp_vals(lb, ub, 49), a, lg_term) * + quadrature_weight_vals(lb, ub, 49); + for (size_t counter = 48; counter >= 0; counter--) { + value += quadrature_fn(quadrature_inp_vals(lb, ub, counter), a, lg_term) * + quadrature_weight_vals(lb, ub, counter); + } + return value; } - // reverse cf expansion - // see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ + /** + * @brief Reverse continued fraction expansion + * See: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ + * + * @param a + * @param z + * @param depth + * @return constexpr T + */ static constexpr T cf_2_recur(const T a, const T z, const int depth) noexcept { if (depth < 100) { @@ -186,22 +190,25 @@ class IncompleteGamma { * @param z * @return constexpr T */ - static constexpr T cf_2(const T a, const T z) noexcept { + static constexpr T continued_fraction_2(const T a, const T z) noexcept { return T(1.0) - exp(a * log(z) - z - lgamma(a)) / cf_2_recur(a, z, 1); } - // continued fraction expansion - // see: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ - static constexpr T cf_1_coef(const T a, const T z, const int depth) noexcept { - return (is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z - : T(depth) / T(2) * z); - } - + /** + * @brief Continued fraction expansion of Gamma function + * See: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ + * + * @param a + * @param z + * @param depth + * @return constexpr T + */ static constexpr T cf_1_recur(const T a, const T z, const int depth) noexcept { if (depth < 55) { - return (a + depth - 1) + - cf_1_coef(a, z, depth) / cf_1_recur(a, z, depth + 1); + T cf_coef = is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z + : T(depth) / T(2) * z; + return (a + depth - 1) + cf_coef / cf_1_recur(a, z, depth + 1); } else { return (a + depth - 1); } @@ -214,7 +221,7 @@ class IncompleteGamma { * @param z * @return constexpr T */ - static constexpr T cf_1(const T a, const T z) noexcept { + static constexpr T continued_fraction_1(const T a, const T z) noexcept { return exp(a * log(z) - z - lgamma(a)) / cf_1_recur(a, z, 1); } @@ -236,9 +243,9 @@ class IncompleteGamma { } else if (LIM::min() > a) { return T(1); } else if (a < T(10) && z - a < T(10)) { - return cf_1(a, z); + return continued_fraction_1(a, z); } else if (a < T(10) || z / a > T(3)) { - return cf_2(a, z); + return continued_fraction_2(a, z); } else { return quadrature(a, z); } From 7ce5684e058cf533d1712205d92e778bfa426644 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 10 May 2023 16:16:20 -0400 Subject: [PATCH 006/208] remove recursion for Halley update --- gtsam/nonlinear/GncHelpers.h | 87 +++++++++++++----------------------- 1 file changed, 32 insertions(+), 55 deletions(-) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 92e2599a5..1340958a3 100644 --- a/gtsam/nonlinear/GncHelpers.h +++ b/gtsam/nonlinear/GncHelpers.h @@ -168,7 +168,7 @@ class IncompleteGamma { * @brief Reverse continued fraction expansion * See: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ * - * @param a + * @param a Degrees of freedom * @param z * @param depth * @return constexpr T @@ -186,7 +186,7 @@ class IncompleteGamma { /** * @brief Lower (regularized) incomplete gamma function * - * @param a + * @param a Degrees of freedom * @param z * @return constexpr T */ @@ -198,7 +198,7 @@ class IncompleteGamma { * @brief Continued fraction expansion of Gamma function * See: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ * - * @param a + * @param a Degrees of freedom * @param z * @param depth * @return constexpr T @@ -217,7 +217,7 @@ class IncompleteGamma { /** * @brief Lower (regularized) incomplete gamma function * - * @param a + * @param a Degrees of freedom * @param z * @return constexpr T */ @@ -229,7 +229,7 @@ class IncompleteGamma { /** * @brief Compute the CDF for the Gamma distribution. * - * @param a + * @param a Degrees of freedom * @param z * @return constexpr T */ @@ -287,7 +287,7 @@ class IncompleteGammaInverse { * @brief Compute an initial value for the inverse gamma function which is * then iteratively updated. * - * @param a + * @param a Degrees of freedom * @param p * @return constexpr T */ @@ -322,7 +322,7 @@ class IncompleteGammaInverse { * @brief Compute the error value `f(x)` which we can use for root-finding. * * @param value - * @param a + * @param a Degrees of freedom * @param p * @return constexpr T */ @@ -334,12 +334,12 @@ class IncompleteGammaInverse { * @brief Derivative of the incomplete gamma function w.r.t. value * * @param value - * @param a + * @param a Degrees of freedom * @param log_val * @return constexpr T */ - static constexpr T derivative(const T value, const T a, - const T lg_val) noexcept { + static constexpr T first_derivative(const T value, const T a, + const T lg_val) noexcept { return (exp(-value + (a - T(1)) * log(value) - lg_val)); } @@ -347,7 +347,7 @@ class IncompleteGammaInverse { * @brief Second derivative of the incomplete gamma function w.r.t. value * * @param value - * @param a + * @param a Degrees of freedom * @param derivative * @return constexpr T */ @@ -361,7 +361,7 @@ class IncompleteGammaInverse { * of the update denominator. * * @param value - * @param a + * @param a Degrees of freedom * @param p * @param derivative * @return constexpr T @@ -376,7 +376,7 @@ class IncompleteGammaInverse { * of the update denominator. * * @param value - * @param a + * @param a Degrees of freedom * @param derivative * @return constexpr T */ @@ -386,7 +386,7 @@ class IncompleteGammaInverse { } /** - * @brief Halley's method update step + * @brief Halley's method update delta * * @param ratio_val_1 * @param ratio_val_2 @@ -397,60 +397,37 @@ class IncompleteGammaInverse { std::max(T(0.8), std::min(T(1.2), T(1) - T(0.5) * ratio_val_1 * ratio_val_2))); } + /** - * @brief Recursive method for computing the iterative solution for the + * @brief Compute the iterative solution for the * incomplete inverse gamma function. * - * @param value - * @param a - * @param p - * @param derivative - * @param lg_val - * @param iter_count - * @return constexpr T - */ - static constexpr T recurse(const T value, const T a, const T p, - const T derivative, const T lg_val, - const int iter_count) noexcept { - return decision(value, a, p, - halley(ratio_val_1(value, a, p, derivative), - ratio_val_2(value, a, derivative)), - lg_val, iter_count); - } - - static constexpr T decision(const T value, const T a, const T p, - const T direc, const T lg_val, - const int iter_count) noexcept { - const int GAMMA_INV_MAX_ITER = 35; - if (iter_count <= GAMMA_INV_MAX_ITER) { - return recurse(value - direc, a, p, derivative(value, a, lg_val), lg_val, - iter_count + 1); - } else { - return value - direc; - } - } - - /** - * @brief Start point for numerical computation of the incomplete gamma - * inverse funtion. - * * @param initial_val Initial value guess - * @param a + * @param a Degrees of freedom * @param p * @param lg_val * @return constexpr T */ - static constexpr T begin(const T initial_val, const T a, const T p, - const T lg_val) noexcept { - return recurse(initial_val, a, p, derivative(initial_val, a, lg_val), - lg_val, 1); + static constexpr T find_root(const T initial_val, const T a, const T p, + const T lg_val) noexcept { + const int GAMMA_INV_MAX_ITER = 35; + T x = initial_val; + T derivative = first_derivative(initial_val, a, lg_val); + for (size_t counter = 1; counter <= GAMMA_INV_MAX_ITER; counter++) { + T direc = halley(ratio_val_1(x, a, p, derivative), + ratio_val_2(x, a, derivative)); + derivative = first_derivative(x, a, lg_val); + x = x - direc; + } + return x - halley(ratio_val_1(x, a, p, derivative), + ratio_val_2(x, a, derivative)); } public: /** * @brief Compute the percent point function for the Gamma distribution. * - * @param a + * @param a Degrees of freedom * @param p * @return constexpr T */ @@ -467,7 +444,7 @@ class IncompleteGammaInverse { } else if (LIM::min() > a) { // Check lower bound for degrees of freedom return T(0); } else { - return begin(initial_val(a, p), a, p, lgamma(a)); + return find_root(initial_val(a, p), a, p, lgamma(a)); } } }; From a807127b512e41704712bdca53922a3bf2224309 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 11 May 2023 13:17:17 -0400 Subject: [PATCH 007/208] update GncOptimizer and make it available --- gtsam/CMakeLists.txt | 1 - gtsam/nonlinear/GncOptimizer.h | 5 ++--- tests/CMakeLists.txt | 1 - 3 files changed, 2 insertions(+), 5 deletions(-) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index efd0e9dc2..40028ad00 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -59,7 +59,6 @@ endif() # if GTSAM_USE_BOOST_FEATURES is not set, then we need to exclude the following: if(NOT GTSAM_USE_BOOST_FEATURES) list (APPEND excluded_sources - "${CMAKE_CURRENT_SOURCE_DIR}/nonlinear/GncOptimizer.h" "${CMAKE_CURRENT_SOURCE_DIR}/inference/graph.h" "${CMAKE_CURRENT_SOURCE_DIR}/inference/graph-inl.h" ) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index d59eb4371..414bee5eb 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -26,9 +26,9 @@ #pragma once +#include #include #include -#include namespace gtsam { /* @@ -36,8 +36,7 @@ namespace gtsam { * Equivalent to chi2inv in Matlab. */ static double Chi2inv(const double alpha, const size_t dofs) { - boost::math::chi_squared_distribution chi2(dofs); - return boost::math::quantile(chi2, alpha); + return chi_squared_quantile(dofs, alpha); } /* ************************************************************************* */ diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index d7b68c4ec..66812d6bb 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -8,7 +8,6 @@ if (${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang") # might not be best test - Richar endif() if (NOT GTSAM_USE_BOOST_FEATURES) - list(APPEND excluded_tests "testGncOptimizer.cpp") list(APPEND excluded_tests "testGraph.cpp") endif() From 6fb3f0f209dcfa52245469814f6820c29248cd07 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 11 May 2023 13:58:55 -0400 Subject: [PATCH 008/208] use templated is_nan check --- gtsam/nonlinear/GncHelpers.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 1340958a3..6852e8131 100644 --- a/gtsam/nonlinear/GncHelpers.h +++ b/gtsam/nonlinear/GncHelpers.h @@ -18,7 +18,6 @@ #pragma once #include -#include #include namespace gtsam { @@ -433,7 +432,7 @@ class IncompleteGammaInverse { */ static constexpr T compute(const T a, const T p) noexcept { // Perform checks on the input and return the corresponding best answer - if (isnan(a) || isnan(p)) { // NaN check + if (is_nan(a) || is_nan(p)) { // NaN check return LIM::quiet_NaN(); } else if (LIM::min() > p) { // Check lower bound return T(0); From d614fda81f229b3b7404f5c163e4f23f86bbba60 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 11 May 2023 18:41:33 -0400 Subject: [PATCH 009/208] try older version --- gtsam/nonlinear/GncHelpers.h | 618 +++++++++++++++++------------------ 1 file changed, 307 insertions(+), 311 deletions(-) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 6852e8131..2dac9ac5e 100644 --- a/gtsam/nonlinear/GncHelpers.h +++ b/gtsam/nonlinear/GncHelpers.h @@ -18,7 +18,6 @@ #pragma once #include -#include namespace gtsam { @@ -103,153 +102,154 @@ static const long double gauss_legendre_50_weights[50] = { namespace internal { +/// 50 point Gauss-Legendre quadrature template -class IncompleteGamma { - /// 50 point Gauss-Legendre quadrature values - static constexpr T quadrature_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); - } +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); +} - /// 50 point Gauss-Legendre quadrature weights - static constexpr T quadrature_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_weight_vals(const T lb, const T ub, + const int counter) noexcept { + return (ub - lb) * gauss_legendre_50_weights[counter] / T(2); +} - static constexpr T quadrature_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_fn(const T x, const T a, + const T lg_term) noexcept { + return exp(-x + (a - T(1)) * log(x) - lg_term); +} - static constexpr T quadrature_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_recur(const T lb, const T ub, const T a, + const T lg_term, + const int counter) noexcept { + return (counter < 49 ? // if + incomplete_gamma_quad_fn( + incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * + incomplete_gamma_quad_weight_vals(lb, ub, counter) + + incomplete_gamma_quad_recur(lb, ub, a, lg_term, counter + 1) + : + // else + incomplete_gamma_quad_fn( + incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * + incomplete_gamma_quad_weight_vals(lb, ub, counter)); +} - static constexpr T quadrature_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_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))); +} - static constexpr T quadrature(const T a, const T z) noexcept { - T lb = quadrature_lb(a, z); - T ub = quadrature_ub(a, z); - T lg_term = lgamma(a); - T value = quadrature_fn(quadrature_inp_vals(lb, ub, 49), a, lg_term) * - quadrature_weight_vals(lb, ub, 49); - for (size_t counter = 48; counter >= 0; counter--) { - value += quadrature_fn(quadrature_inp_vals(lb, ub, counter), a, lg_term) * - quadrature_weight_vals(lb, ub, counter); - } - return value; - } +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))); +} - /** - * @brief Reverse continued fraction expansion - * See: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ - * - * @param a Degrees of freedom - * @param z - * @param depth - * @return constexpr T - */ - static constexpr T cf_2_recur(const T a, const T z, - const int depth) noexcept { - if (depth < 100) { - return (1 + (depth - 1) * 2 - a + z) + - depth * (a - depth) / cf_2_recur(a, z, depth + 1); - } else { - return 1 + (depth - 1) * 2 - a + z; - } - } +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); +} - /** - * @brief Lower (regularized) incomplete gamma function - * - * @param a Degrees of freedom - * @param z - * @return constexpr T - */ - static constexpr T continued_fraction_2(const T a, const T z) noexcept { - return T(1.0) - exp(a * log(z) - z - lgamma(a)) / cf_2_recur(a, z, 1); - } +// reverse cf expansion +// see: https://functions.wolfram.com/GammaBetaErf/Gamma2/10/0003/ - /** - * @brief Continued fraction expansion of Gamma function - * See: http://functions.wolfram.com/GammaBetaErf/Gamma2/10/0009/ - * - * @param a Degrees of freedom - * @param z - * @param depth - * @return constexpr T - */ - static constexpr T cf_1_recur(const T a, const T z, - const int depth) noexcept { - if (depth < 55) { - T cf_coef = is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z - : T(depth) / T(2) * z; - return (a + depth - 1) + cf_coef / cf_1_recur(a, z, depth + 1); - } else { - return (a + depth - 1); - } - } +template +constexpr T incomplete_gamma_cf_2_recur(const T a, const T z, + const int depth) noexcept { + return (depth < 100 ? (1 + (depth - 1) * 2 - a + z) + + depth * (a - depth) / + incomplete_gamma_cf_2_recur(a, z, depth + 1) + : (1 + (depth - 1) * 2 - a + z)); +} - /** - * @brief Lower (regularized) incomplete gamma function - * - * @param a Degrees of freedom - * @param z - * @return constexpr T - */ - static constexpr T continued_fraction_1(const T a, const T z) noexcept { - return exp(a * log(z) - z - lgamma(a)) / cf_1_recur(a, z, 1); - } +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)); +} - public: - /** - * @brief Compute the CDF for the Gamma distribution. - * - * @param a Degrees of freedom - * @param z - * @return constexpr T - */ - static constexpr T compute(const T a, const T z) noexcept { - if (is_nan(a) || is_nan(z)) { // NaN check - 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)) { - return continued_fraction_1(a, z); - } else if (a < T(10) || z / a > T(3)) { - return continued_fraction_2(a, z); - } else { - return quadrature(a, z); - } - } -}; +// 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 { + return (is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z + : T(depth) / T(2) * z); +} + +template +constexpr T incomplete_gamma_cf_1_recur(const T a, const T z, + const int depth) noexcept { + return (depth < 55 ? // if + (a + depth - 1) + incomplete_gamma_cf_1_coef(a, z, depth) / + incomplete_gamma_cf_1_recur(a, z, depth + 1) + : + // else + (a + depth - 1)); +} + +template +constexpr T incomplete_gamma_cf_1( + const T a, + 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)); +} + +// + +template +constexpr T incomplete_gamma_check(const T a, const T z) noexcept { + return ( // NaN check + (is_nan(a) || is_nan(z)) ? LIM::quiet_NaN() : + // + a < T(0) ? LIM::quiet_NaN() + : + // + LIM::min() > z ? T(0) + : + // + LIM::min() > a ? T(1) + : + // cf or quadrature + (a < T(10)) && (z - a < T(10)) ? incomplete_gamma_cf_1(a, z) + : (a < T(10)) || (z / a > T(3)) ? incomplete_gamma_cf_2(a, z) + : + // else + 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 @@ -270,212 +270,208 @@ class IncompleteGamma { * \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 { - using TC = common_return_t; - return internal::IncompleteGamma::compute(static_cast(a), - static_cast(x)); + return internal::incomplete_gamma_type_check(a, x); } namespace internal { template -class IncompleteGammaInverse { - /** - * @brief Compute an initial value for the inverse gamma function which is - * then iteratively updated. - * - * @param a Degrees of freedom - * @param p - * @return constexpr T - */ - static constexpr T initial_val(const T a, const T p) noexcept { - if (a > T(1)) { - // Inverse gamma function initial value when a > 1.0 - const T t_val = p > T(0.5) ? sqrt(-2 * log(T(1) - p)) : sqrt(-2 * log(p)); - const T sgn_term = p > T(0.5) ? T(-1) : T(1); - const T initial_val_1 = - t_val - +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); - const T signed_initial_val_1 = sgn_term * initial_val_1; + T(0.001308L) * t_val * t_val * t_val)); +} - return std::max( - T(1e-04), - a * pow(T(1) - T(1) / (9 * a) - signed_initial_val_1 / (3 * sqrt(a)), - 3)); - } else { - // Inverse gamma function initial value when a <= 1.0 - T t_val = T(1) - T(0.253) * a - T(0.12) * a * a; - 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)); - } - } - } +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)); +} - /** - * @brief Compute the error value `f(x)` which we can use for root-finding. - * - * @param value - * @param a Degrees of freedom - * @param p - * @return constexpr T - */ - static constexpr T err_val(const T value, const T a, const T p) noexcept { - return (incomplete_gamma(a, value) - p); - } +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); +} - /** - * @brief Derivative of the incomplete gamma function w.r.t. value - * - * @param value - * @param a Degrees of freedom - * @param log_val - * @return constexpr T - */ - static constexpr T first_derivative(const T value, const T a, - const T lg_val) noexcept { - return (exp(-value + (a - T(1)) * log(value) - lg_val)); - } +template +constexpr T incomplete_gamma_inv_initial_val_2( + const T a, const T p, const T t_val) noexcept { // a <= 1.0 + return (p < t_val ? // if + pow(p / t_val, T(1) / a) + : + // else + T(1) - log(T(1) - (p - t_val) / (T(1) - t_val))); +} - /** - * @brief Second derivative of the incomplete gamma function w.r.t. value - * - * @param value - * @param a Degrees of freedom - * @param derivative - * @return constexpr T - */ - static constexpr T second_derivative(const T value, const T a, - const T derivative) noexcept { - return (derivative * ((a - T(1)) / value - T(1))); - } +// initial value - /** - * @brief Compute \f[ \frac{f(x_n)}{f'(x_n)} \f] as part - * of the update denominator. - * - * @param value - * @param a Degrees of freedom - * @param p - * @param derivative - * @return constexpr T - */ - static constexpr T ratio_val_1(const T value, const T a, const T p, - const T derivative) noexcept { - return (err_val(value, a, p) / derivative); - } +template +constexpr T incomplete_gamma_inv_initial_val(const T a, const T p) noexcept { + return (a > T(1) ? // if + incomplete_gamma_inv_initial_val_1( + a, incomplete_gamma_inv_t_val_1(p), p > T(0.5) ? T(-1) : T(1)) + : + // else + incomplete_gamma_inv_initial_val_2( + a, p, incomplete_gamma_inv_t_val_2(a))); +} - /** - * @brief Compute \f[ \frac{f''(x_n)}{f'(x_n)} \f] as part - * of the update denominator. - * - * @param value - * @param a Degrees of freedom - * @param derivative - * @return constexpr T - */ - static constexpr T ratio_val_2(const T value, const T a, - const T derivative) noexcept { - return (second_derivative(value, a, derivative) / derivative); - } +// +// Halley recursion - /** - * @brief Halley's method update delta - * - * @param ratio_val_1 - * @param ratio_val_2 - * @return constexpr T - */ - static constexpr T 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_err_val( + const T value, const T a, const T p) noexcept { // err_val = f(x) + return (incomplete_gamma(a, value) - p); +} - /** - * @brief Compute the iterative solution for the - * incomplete inverse gamma function. - * - * @param initial_val Initial value guess - * @param a Degrees of freedom - * @param p - * @param lg_val - * @return constexpr T - */ - static constexpr T find_root(const T initial_val, const T a, const T p, - const T lg_val) noexcept { - const int GAMMA_INV_MAX_ITER = 35; - T x = initial_val; - T derivative = first_derivative(initial_val, a, lg_val); - for (size_t counter = 1; counter <= GAMMA_INV_MAX_ITER; counter++) { - T direc = halley(ratio_val_1(x, a, p, derivative), - ratio_val_2(x, a, derivative)); - derivative = first_derivative(x, a, lg_val); - x = x - direc; - } - return x - halley(ratio_val_1(x, a, p, derivative), - ratio_val_2(x, a, derivative)); - } +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)); +} - public: - /** - * @brief Compute the percent point function for the Gamma distribution. - * - * @param a Degrees of freedom - * @param p - * @return constexpr T - */ - static constexpr T compute(const T a, const T p) noexcept { - // Perform checks on the input and return the corresponding best answer - if (is_nan(a) || is_nan(p)) { // NaN check - return LIM::quiet_NaN(); - } else if (LIM::min() > p) { // Check lower bound - return T(0); - } else if (p > T(1)) { // Check upper bound - return LIM::quiet_NaN(); - } else if (LIM::min() > abs(T(1) - p)) { - return LIM::infinity(); - } else if (LIM::min() > a) { // Check lower bound for degrees of freedom - return T(0); - } else { - return find_root(initial_val(a, p), a, p, lgamma(a)); - } - } -}; +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 { +// return( abs(direc) > GCEM_INCML_GAMMA_INV_TOL ? +// incomplete_gamma_inv_recur(value - direc, a, p, +// incomplete_gamma_inv_deriv_1(value,a,lg_val), lg_val) : value - direc ); +#define INCML_GAMMA_INV_MAX_ITER 35 + return (iter_count <= INCML_GAMMA_INV_MAX_ITER ? // if + incomplete_gamma_inv_recur( + value - direc, a, p, + incomplete_gamma_inv_deriv_1(value, a, lg_val), lg_val, + iter_count + 1) + : + // else + 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 { + return ( // NaN check + (is_nan(a) || is_nan(p)) ? LIM::quiet_NaN() : + // + LIM::min() > p ? T(0) + : p > T(1) ? LIM::quiet_NaN() + : LIM::min() > abs(T(1) - p) ? LIM::infinity() + : + // + LIM::min() > a ? T(0) + : + // else + incomplete_gamma_inv_begin(incomplete_gamma_inv_initial_val(a, p), a, + p, lgamma(a))); +} + +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)); +} } // namespace internal /** * Compile-time inverse incomplete gamma function * - * Compute the 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. + * @param a a real-valued, non-negative input. + * @param p a real-valued input with values in the unit-interval. * - * We find 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} + * @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] - * - * @param a The degrees of freedom for the gamma distribution. - * @param p The quantile value for computing the percent point function. - * - * @return Computes the inverse incomplete gamma function. */ + template constexpr common_return_t incomplete_gamma_inv(const T1 a, const T2 p) noexcept { - using TC = common_return_t; - return internal::IncompleteGammaInverse::compute(static_cast(a), - static_cast(p)); + return internal::incomplete_gamma_inv_type_check(a, p); } /** From 64c28504adf2a6abb272141a616728be0e4e76ce Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Jul 2023 12:54:03 -0400 Subject: [PATCH 010/208] switch from IndexVector to FastVector now that pybind/stl.h is enabled --- gtsam/nonlinear/GncParams.h | 11 ++++------- tests/testGncOptimizer.cpp | 22 +++++++++++----------- 2 files changed, 15 insertions(+), 18 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 10ac80663..b792fa058 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -73,13 +73,10 @@ class GncParams { double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level - //TODO(Varun) replace IndexVector with vector once pybind11/stl.h is globally enabled. - /// Use IndexVector for inliers and outliers since it is fast + wrapping - using IndexVector = FastVector; ///< Slots in the factor graph corresponding to measurements that we know are inliers - IndexVector knownInliers = IndexVector(); + FastVector knownInliers; ///< Slots in the factor graph corresponding to measurements that we know are outliers - IndexVector knownOutliers = IndexVector(); + FastVector knownOutliers; /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { @@ -120,7 +117,7 @@ class GncParams { * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and * only apply GNC to prune outliers from the loop closures. * */ - void setKnownInliers(const IndexVector& knownIn) { + void setKnownInliers(const FastVector& knownIn) { for (size_t i = 0; i < knownIn.size(); i++){ knownInliers.push_back(knownIn[i]); } @@ -131,7 +128,7 @@ class GncParams { * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. * */ - void setKnownOutliers(const IndexVector& knownOut) { + void setKnownOutliers(const FastVector& knownOut) { for (size_t i = 0; i < knownOut.size(); i++){ knownOutliers.push_back(knownOut[i]); } diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 5424a5744..28261683b 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { Values initial; initial.insert(X(1), p0); - GncParams::IndexVector knownInliers; + FastVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) { Values initial; initial.insert(X(1), p0); - GncParams::IndexVector knownInliers; + FastVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) { Values initial; initial.insert(X(1), p0); - GncParams::IndexVector knownInliers; + FastVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -761,7 +761,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // GNC // Note: in difficult instances, we set the odometry measurements to be // inliers, but this problem is simple enought to succeed even without that - // assumption GncParams::IndexVector knownInliers; + // assumption; GncParams gncParams; auto gnc = GncOptimizer>(*graph, *initial, gncParams); @@ -782,12 +782,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers (check early stopping // when all measurements are known to be inliers or outliers) { - GncParams::IndexVector knownInliers; + FastVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); - GncParams::IndexVector knownOutliers; + FastVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -811,11 +811,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers { - GncParams::IndexVector knownInliers; + FastVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - GncParams::IndexVector knownOutliers; + FastVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -839,7 +839,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // only known outliers { - GncParams::IndexVector knownOutliers; + FastVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -914,11 +914,11 @@ TEST(GncOptimizer, setWeights) { // initialize weights and also set known inliers/outliers { GncParams gncParams; - GncParams::IndexVector knownInliers; + FastVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - GncParams::IndexVector knownOutliers; + FastVector knownOutliers; knownOutliers.push_back(3); gncParams.setKnownInliers(knownInliers); gncParams.setKnownOutliers(knownOutliers); From a5fd9c120b3159812c196836f82e537cf6b43c07 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Jul 2023 12:54:32 -0400 Subject: [PATCH 011/208] fix chi_squared_quantile --- gtsam/nonlinear/GncHelpers.h | 168 +++++++++++------------ gtsam/nonlinear/tests/testGncHelpers.cpp | 37 +++++ 2 files changed, 121 insertions(+), 84 deletions(-) create mode 100644 gtsam/nonlinear/tests/testGncHelpers.cpp diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/GncHelpers.h index 2dac9ac5e..7a27a3530 100644 --- a/gtsam/nonlinear/GncHelpers.h +++ b/gtsam/nonlinear/GncHelpers.h @@ -29,9 +29,11 @@ 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>; @@ -126,16 +128,14 @@ template constexpr T incomplete_gamma_quad_recur(const T lb, const T ub, const T a, const T lg_term, const int counter) noexcept { - return (counter < 49 ? // if - incomplete_gamma_quad_fn( - incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * - incomplete_gamma_quad_weight_vals(lb, ub, counter) + - incomplete_gamma_quad_recur(lb, ub, a, lg_term, counter + 1) - : - // else - incomplete_gamma_quad_fn( - incomplete_gamma_quad_inp_vals(lb, ub, counter), a, lg_term) * - incomplete_gamma_quad_weight_vals(lb, ub, counter)); + 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 @@ -180,10 +180,13 @@ constexpr T incomplete_gamma_quad(const T a, const T z) noexcept { template constexpr T incomplete_gamma_cf_2_recur(const T a, const T z, const int depth) noexcept { - return (depth < 100 ? (1 + (depth - 1) * 2 - a + z) + - depth * (a - depth) / - incomplete_gamma_cf_2_recur(a, z, depth + 1) - : (1 + (depth - 1) * 2 - a + z)); + 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 @@ -200,50 +203,49 @@ constexpr T incomplete_gamma_cf_2( template constexpr T incomplete_gamma_cf_1_coef(const T a, const T z, const int depth) noexcept { - return (is_odd(depth) ? -(a - 1 + T(depth + 1) / T(2)) * z - : T(depth) / T(2) * z); + 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 { - return (depth < 55 ? // if - (a + depth - 1) + incomplete_gamma_cf_1_coef(a, z, depth) / - incomplete_gamma_cf_1_recur(a, z, depth + 1) - : - // else - (a + depth - 1)); + 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 { // lower (regularized) incomplete gamma function - return (exp(a * log(z) - z - lgamma(a)) / - incomplete_gamma_cf_1_recur(a, z, 1)); +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 { - return ( // NaN check - (is_nan(a) || is_nan(z)) ? LIM::quiet_NaN() : - // - a < T(0) ? LIM::quiet_NaN() - : - // - LIM::min() > z ? T(0) - : - // - LIM::min() > a ? T(1) - : - // cf or quadrature - (a < T(10)) && (z - a < T(10)) ? incomplete_gamma_cf_1(a, z) - : (a < T(10)) || (z / a > T(3)) ? incomplete_gamma_cf_2(a, z) - : - // else - incomplete_gamma_quad(a, z)); + 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 > @@ -323,24 +325,24 @@ constexpr T incomplete_gamma_inv_initial_val_1( template constexpr T incomplete_gamma_inv_initial_val_2( const T a, const T p, const T t_val) noexcept { // a <= 1.0 - return (p < t_val ? // if - pow(p / t_val, T(1) / a) - : - // else - T(1) - log(T(1) - (p - t_val) / (T(1) - t_val))); + 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 +// Initial value template constexpr T incomplete_gamma_inv_initial_val(const T a, const T p) noexcept { - return (a > T(1) ? // if - incomplete_gamma_inv_initial_val_1( - a, incomplete_gamma_inv_t_val_1(p), p > T(0.5) ? T(-1) : T(1)) - : - // else - incomplete_gamma_inv_initial_val_2( - a, p, incomplete_gamma_inv_t_val_2(a))); + 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)); + } } // @@ -405,18 +407,15 @@ 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 { -// return( abs(direc) > GCEM_INCML_GAMMA_INV_TOL ? -// incomplete_gamma_inv_recur(value - direc, a, p, -// incomplete_gamma_inv_deriv_1(value,a,lg_val), lg_val) : value - direc ); -#define INCML_GAMMA_INV_MAX_ITER 35 - return (iter_count <= INCML_GAMMA_INV_MAX_ITER ? // if - incomplete_gamma_inv_recur( - value - direc, a, p, - incomplete_gamma_inv_deriv_1(value, a, lg_val), lg_val, - iter_count + 1) - : - // else - value - direc); + 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 @@ -429,19 +428,20 @@ constexpr T incomplete_gamma_inv_begin(const T initial_val, const T a, template constexpr T incomplete_gamma_inv_check(const T a, const T p) noexcept { - return ( // NaN check - (is_nan(a) || is_nan(p)) ? LIM::quiet_NaN() : - // - LIM::min() > p ? T(0) - : p > T(1) ? LIM::quiet_NaN() - : LIM::min() > abs(T(1) - p) ? LIM::infinity() - : - // - LIM::min() > a ? T(0) - : - // else - incomplete_gamma_inv_begin(incomplete_gamma_inv_initial_val(a, p), a, - p, lgamma(a))); + 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)); + } } template > diff --git a/gtsam/nonlinear/tests/testGncHelpers.cpp b/gtsam/nonlinear/tests/testGncHelpers.cpp new file mode 100644 index 000000000..6e47f97cc --- /dev/null +++ b/gtsam/nonlinear/tests/testGncHelpers.cpp @@ -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 +#include +#include + +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); +} +/* ************************************************************************* */ From 00f5596e704dcf676cb0f317b81c77ed65ca1dd8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Jul 2023 13:40:14 -0400 Subject: [PATCH 012/208] Revert "switch from IndexVector to FastVector now that pybind/stl.h is enabled" This reverts commit 64c28504adf2a6abb272141a616728be0e4e76ce. --- gtsam/nonlinear/GncParams.h | 11 +++++++---- tests/testGncOptimizer.cpp | 22 +++++++++++----------- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index b792fa058..10ac80663 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -73,10 +73,13 @@ class GncParams { double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level + //TODO(Varun) replace IndexVector with vector once pybind11/stl.h is globally enabled. + /// Use IndexVector for inliers and outliers since it is fast + wrapping + using IndexVector = FastVector; ///< Slots in the factor graph corresponding to measurements that we know are inliers - FastVector knownInliers; + IndexVector knownInliers = IndexVector(); ///< Slots in the factor graph corresponding to measurements that we know are outliers - FastVector knownOutliers; + IndexVector knownOutliers = IndexVector(); /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { @@ -117,7 +120,7 @@ class GncParams { * This functionality is commonly used in SLAM when one may assume the odometry is outlier free, and * only apply GNC to prune outliers from the loop closures. * */ - void setKnownInliers(const FastVector& knownIn) { + void setKnownInliers(const IndexVector& knownIn) { for (size_t i = 0; i < knownIn.size(); i++){ knownInliers.push_back(knownIn[i]); } @@ -128,7 +131,7 @@ class GncParams { * corresponds to the slots in the factor graph. For instance, if you have a nonlinear factor graph nfg, * and you provide knownOut = {0, 2, 15}, GNC will not apply outlier rejection to nfg[0], nfg[2], and nfg[15]. * */ - void setKnownOutliers(const FastVector& knownOut) { + void setKnownOutliers(const IndexVector& knownOut) { for (size_t i = 0; i < knownOut.size(); i++){ knownOutliers.push_back(knownOut[i]); } diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 28261683b..5424a5744 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -567,7 +567,7 @@ TEST(GncOptimizer, optimizeWithKnownInliers) { Values initial; initial.insert(X(1), p0); - FastVector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -644,7 +644,7 @@ TEST(GncOptimizer, barcsq) { Values initial; initial.insert(X(1), p0); - FastVector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -691,7 +691,7 @@ TEST(GncOptimizer, setInlierCostThresholds) { Values initial; initial.insert(X(1), p0); - FastVector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); @@ -761,7 +761,7 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // GNC // Note: in difficult instances, we set the odometry measurements to be // inliers, but this problem is simple enought to succeed even without that - // assumption; + // assumption GncParams::IndexVector knownInliers; GncParams gncParams; auto gnc = GncOptimizer>(*graph, *initial, gncParams); @@ -782,12 +782,12 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers (check early stopping // when all measurements are known to be inliers or outliers) { - FastVector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(0); knownInliers.push_back(1); knownInliers.push_back(2); - FastVector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -811,11 +811,11 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // nonconvexity with known inliers and known outliers { - FastVector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - FastVector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -839,7 +839,7 @@ TEST(GncOptimizer, knownInliersAndOutliers) { // only known outliers { - FastVector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); GncParams gncParams; @@ -914,11 +914,11 @@ TEST(GncOptimizer, setWeights) { // initialize weights and also set known inliers/outliers { GncParams gncParams; - FastVector knownInliers; + GncParams::IndexVector knownInliers; knownInliers.push_back(2); knownInliers.push_back(0); - FastVector knownOutliers; + GncParams::IndexVector knownOutliers; knownOutliers.push_back(3); gncParams.setKnownInliers(knownInliers); gncParams.setKnownOutliers(knownOutliers); From 7c935d9e434ce0cd504296611d40fff30d4253ff Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 10 Jul 2023 13:43:44 -0400 Subject: [PATCH 013/208] small update to GNC IndexVector --- gtsam/nonlinear/GncParams.h | 7 +++---- tests/testGncOptimizer.cpp | 9 +++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/gtsam/nonlinear/GncParams.h b/gtsam/nonlinear/GncParams.h index 10ac80663..b1237b790 100644 --- a/gtsam/nonlinear/GncParams.h +++ b/gtsam/nonlinear/GncParams.h @@ -73,13 +73,12 @@ class GncParams { double weightsTol = 1e-4; ///< If the weights are within weightsTol from being binary, stop iterating (only for TLS) Verbosity verbosity = SILENT; ///< Verbosity level - //TODO(Varun) replace IndexVector with vector once pybind11/stl.h is globally enabled. - /// Use IndexVector for inliers and outliers since it is fast + wrapping + /// Use IndexVector for inliers and outliers since it is fast using IndexVector = FastVector; ///< Slots in the factor graph corresponding to measurements that we know are inliers - IndexVector knownInliers = IndexVector(); + IndexVector knownInliers; ///< Slots in the factor graph corresponding to measurements that we know are outliers - IndexVector knownOutliers = IndexVector(); + IndexVector knownOutliers; /// Set the robust loss function to be used in GNC (chosen among the ones in GncLossType). void setLossType(const GncLossType type) { diff --git a/tests/testGncOptimizer.cpp b/tests/testGncOptimizer.cpp index 5424a5744..4e0ebf516 100644 --- a/tests/testGncOptimizer.cpp +++ b/tests/testGncOptimizer.cpp @@ -750,7 +750,8 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // add a few outliers SharedDiagonal betweenNoise = noiseModel::Diagonal::Sigmas( Vector3(0.1, 0.1, 0.01)); - graph->push_back(BetweenFactor(90, 50, Pose2(), betweenNoise)); // some arbitrary and incorrect between factor + // some arbitrary and incorrect between factor + graph->push_back(BetweenFactor(90, 50, Pose2(), betweenNoise)); /// get expected values by optimizing outlier-free graph Values expectedWithOutliers = LevenbergMarquardtOptimizer(*graph, *initial) @@ -759,9 +760,9 @@ TEST(GncOptimizer, optimizeSmallPoseGraph) { // CHECK(assert_equal(expected, expectedWithOutliers, 1e-3)); // GNC - // Note: in difficult instances, we set the odometry measurements to be - // inliers, but this problem is simple enought to succeed even without that - // assumption GncParams::IndexVector knownInliers; + // NOTE: in difficult instances, we set the odometry measurements to be + // inliers, but this problem is simple enough to succeed even without that + // assumption. GncParams gncParams; auto gnc = GncOptimizer>(*graph, *initial, gncParams); From dcb42998e8b7d5f1e490ea85426de8f442a05300 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Oct 2023 05:47:43 -0400 Subject: [PATCH 014/208] rename GncHelpers to chiSquaredInverse and move to internal directory --- gtsam/nonlinear/{GncHelpers.h => internal/chiSquaredInverse.hpp} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename gtsam/nonlinear/{GncHelpers.h => internal/chiSquaredInverse.hpp} (100%) diff --git a/gtsam/nonlinear/GncHelpers.h b/gtsam/nonlinear/internal/chiSquaredInverse.hpp similarity index 100% rename from gtsam/nonlinear/GncHelpers.h rename to gtsam/nonlinear/internal/chiSquaredInverse.hpp From d65e664a7c2762b0dd93a9e671bfe974ada841ed Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Sep 2023 15:55:58 -0400 Subject: [PATCH 015/208] re-enable debug CI for linux --- .github/workflows/build-linux.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-linux.yml b/.github/workflows/build-linux.yml index 5de09c63f..4502dbe0e 100644 --- a/.github/workflows/build-linux.yml +++ b/.github/workflows/build-linux.yml @@ -31,7 +31,7 @@ jobs: ubuntu-22.04-clang-14, ] - build_type: [Release] + build_type: [Debug, Release] build_unstable: [ON] include: - name: ubuntu-20.04-gcc-9 From e8817ae3ea1c748abfe9a45aa4e0f67a85adcc94 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Oct 2023 08:25:56 -0400 Subject: [PATCH 016/208] rename other files accordingly --- .../internal/{chiSquaredInverse.hpp => chiSquaredInverse.h} | 0 .../tests/{testGncHelpers.cpp => testChiSquaredInverse.cpp} | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename gtsam/nonlinear/internal/{chiSquaredInverse.hpp => chiSquaredInverse.h} (100%) rename gtsam/nonlinear/tests/{testGncHelpers.cpp => testChiSquaredInverse.cpp} (100%) diff --git a/gtsam/nonlinear/internal/chiSquaredInverse.hpp b/gtsam/nonlinear/internal/chiSquaredInverse.h similarity index 100% rename from gtsam/nonlinear/internal/chiSquaredInverse.hpp rename to gtsam/nonlinear/internal/chiSquaredInverse.h diff --git a/gtsam/nonlinear/tests/testGncHelpers.cpp b/gtsam/nonlinear/tests/testChiSquaredInverse.cpp similarity index 100% rename from gtsam/nonlinear/tests/testGncHelpers.cpp rename to gtsam/nonlinear/tests/testChiSquaredInverse.cpp From a46a78d413206cc6f712524dcc13bcdeb263cbe8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 19 Oct 2023 08:27:53 -0400 Subject: [PATCH 017/208] update paths --- gtsam/nonlinear/GncOptimizer.h | 2 +- gtsam/nonlinear/internal/chiSquaredInverse.h | 2 +- gtsam/nonlinear/tests/testChiSquaredInverse.cpp | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/GncOptimizer.h b/gtsam/nonlinear/GncOptimizer.h index 21d4e826b..45c674401 100644 --- a/gtsam/nonlinear/GncOptimizer.h +++ b/gtsam/nonlinear/GncOptimizer.h @@ -26,9 +26,9 @@ #pragma once -#include #include #include +#include namespace gtsam { /* diff --git a/gtsam/nonlinear/internal/chiSquaredInverse.h b/gtsam/nonlinear/internal/chiSquaredInverse.h index 7a27a3530..3729bf754 100644 --- a/gtsam/nonlinear/internal/chiSquaredInverse.h +++ b/gtsam/nonlinear/internal/chiSquaredInverse.h @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file GncHelpers.h + * @file chiSquaredInverse.h * @brief Helper functions for use with the GncOptimizer * @author Varun Agrawal */ diff --git a/gtsam/nonlinear/tests/testChiSquaredInverse.cpp b/gtsam/nonlinear/tests/testChiSquaredInverse.cpp index 6e47f97cc..65c3b417d 100644 --- a/gtsam/nonlinear/tests/testChiSquaredInverse.cpp +++ b/gtsam/nonlinear/tests/testChiSquaredInverse.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /* - * @file testGncHelpers.cpp + * @file testChiSquaredInverse.cpp * @date July 10, 2023 * @author Varun Agrawal * @brief Tests for Chi-squared distribution. @@ -18,12 +18,12 @@ #include #include -#include +#include using namespace gtsam; /* ************************************************************************* */ -TEST(GncHelpers, ChiSqInv) { +TEST(ChiSquaredInverse, ChiSqInv) { double barcSq = chi_squared_quantile(2, 0.99); EXPECT_DOUBLES_EQUAL(9.21034, barcSq, 1e-5); } From c8a0cdc543587799292302d804afde01ddd9a4d2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Oct 2023 07:00:34 -0400 Subject: [PATCH 018/208] much improved chi_squared_quantile, incremental update --- gtsam/nonlinear/GncOptimizer.h | 2 +- gtsam/nonlinear/internal/chiSquaredInverse.h | 531 +++--------------- .../nonlinear/tests/testChiSquaredInverse.cpp | 2 +- 3 files changed, 94 insertions(+), 441 deletions(-) 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); } From bebb275489ae6030353148eef881fba9c24ca9b7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Oct 2023 10:21:49 -0400 Subject: [PATCH 019/208] compute initial guess for inverse gamma value --- gtsam/nonlinear/internal/chiSquaredInverse.h | 304 +++++++++++++++++-- 1 file changed, 275 insertions(+), 29 deletions(-) diff --git a/gtsam/nonlinear/internal/chiSquaredInverse.h b/gtsam/nonlinear/internal/chiSquaredInverse.h index 528694284..dc8595846 100644 --- a/gtsam/nonlinear/internal/chiSquaredInverse.h +++ b/gtsam/nonlinear/internal/chiSquaredInverse.h @@ -24,9 +24,9 @@ #pragma once -#include -#include -#include +#include +#include +#include #include @@ -37,6 +37,270 @@ 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)) { @@ -53,13 +317,9 @@ T gamma_p_inv_imp(const T a, const T p) { return 0; } - // 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); + T guess = find_inverse_gamma(a, p, 1 - p, &has_10_digits); T lower = LIM::min(); if (guess <= lower) { @@ -67,35 +327,21 @@ T gamma_p_inv_imp(const T a, const T p) { } // TODO + // 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 // 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( From 6f386168a459db94f6cdda9e15ef3da499dc873d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 20 Oct 2023 11:09:35 -0400 Subject: [PATCH 020/208] gamma inverse functional --- gtsam/nonlinear/internal/Gamma.h | 94 ++++++++++++++++++++ gtsam/nonlinear/internal/Utils.h | 49 ++++++++++ gtsam/nonlinear/internal/chiSquaredInverse.h | 19 ++-- 3 files changed, 152 insertions(+), 10 deletions(-) create mode 100644 gtsam/nonlinear/internal/Gamma.h create mode 100644 gtsam/nonlinear/internal/Utils.h 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( From 25ebdd54fc23ca0624a42540e941cd04abc7eb9e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 22 Oct 2023 15:15:02 -0400 Subject: [PATCH 021/208] add gamma_p_inverse_func --- gtsam/nonlinear/internal/chiSquaredInverse.h | 72 ++++++++++++++++++-- 1 file changed, 67 insertions(+), 5 deletions(-) diff --git a/gtsam/nonlinear/internal/chiSquaredInverse.h b/gtsam/nonlinear/internal/chiSquaredInverse.h index b7744ffa2..7577721dc 100644 --- a/gtsam/nonlinear/internal/chiSquaredInverse.h +++ b/gtsam/nonlinear/internal/chiSquaredInverse.h @@ -24,13 +24,12 @@ #pragma once -#include #include #include // TODO(Varun) remove -// #include +#include #include namespace gtsam { @@ -301,6 +300,65 @@ T find_inverse_gamma(T a, T p, T q, bool* p_has_10_digits) { 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; + f = static_cast(gamma_incomplete_imp(a, x, true, invert, &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; +}; + template T gamma_p_inv_imp(const T a, const T p) { if (is_nan(a) || is_nan(p)) { @@ -339,14 +397,18 @@ T gamma_p_inv_imp(const T a, const T p) { uintmax_t max_iter = 200; // TODO - // // Perform Halley iteration for root-finding to get a more refined answer + // 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); guess = boost::math::tools::halley_iterate( - internal::gamma_p_inverse_func(a, p, false), guess, lower, - LIM::max(), digits, max_iter); + 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( From 90a9e6f74695612e98fcfaf276daade00ca3cd05 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 9 Nov 2023 15:37:15 -0500 Subject: [PATCH 022/208] report correct error for discreteElimination --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 7a7ca0cbf..2029b48e0 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -169,10 +169,10 @@ discreteElimination(const HybridGaussianFactorGraph &factors, // TODO(dellaert): is this correct? If so explain here. } else if (auto hc = dynamic_pointer_cast(f)) { auto dc = hc->asDiscrete(); - if (!dc) throwRuntimeError("continuousElimination", dc); + if (!dc) throwRuntimeError("discreteElimination", dc); dfg.push_back(hc->asDiscrete()); } else { - throwRuntimeError("continuousElimination", f); + throwRuntimeError("discreteElimination", f); } } From 66a3c084f19cb7583630ac2459979949a77e68e0 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 9 Nov 2023 15:37:34 -0500 Subject: [PATCH 023/208] improved GaussianConditional docstring --- gtsam/linear/GaussianConditional.h | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/GaussianConditional.h b/gtsam/linear/GaussianConditional.h index 43d349b67..420efabca 100644 --- a/gtsam/linear/GaussianConditional.h +++ b/gtsam/linear/GaussianConditional.h @@ -31,8 +31,10 @@ namespace gtsam { /** * A GaussianConditional functions as the node in a Bayes network. - * It has a set of parents y,z, etc. and implements a probability density on x. + * It has a set of parents y,z, etc. and implements a Gaussian probability density p(x | y, z) on x. * The negative log-density is given by \f$ \frac{1}{2} |Rx - (d - Sy - Tz - ...)|^2 \f$ + * The mean of the conditional density is \f$ R^{-1}(d - Sy - Tz - ...) \f$. + * The covariance of the conditional density is given by the noise model and is constrained to be diagonal. * @ingroup linear */ class GTSAM_EXPORT GaussianConditional : From 4eef9023e9898b2acef928eac45ebeeba09b036f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 9 Nov 2023 15:37:43 -0500 Subject: [PATCH 024/208] small CMake formatting --- gtsam/nonlinear/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/nonlinear/CMakeLists.txt b/gtsam/nonlinear/CMakeLists.txt index ad4824817..29ff2efa5 100644 --- a/gtsam/nonlinear/CMakeLists.txt +++ b/gtsam/nonlinear/CMakeLists.txt @@ -1,9 +1,9 @@ # Install headers file(GLOB nonlinear_headers "*.h") -install(FILES ${nonlinear_headers} DESTINATION include/gtsam/nonlinear) +install(FILES ${nonlinear_headers} DESTINATION "include/gtsam/nonlinear") file(GLOB nonlinear_headers_internal "internal/*.h") -install(FILES ${nonlinear_headers_internal} DESTINATION include/gtsam/nonlinear/internal) +install(FILES ${nonlinear_headers_internal} DESTINATION "include/gtsam/nonlinear/internal") # Build tests add_subdirectory(tests) From 81b3aebc427fb629acfe9ac6c9fbf19aa9bd3f6c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 9 Nov 2023 15:39:14 -0500 Subject: [PATCH 025/208] fix NavState.cpp filename in docstring --- gtsam/navigation/NavState.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 6a17e9ea0..4410d629c 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file NavState.h + * @file NavState.cpp * @brief Navigation state composing of attitude, position, and velocity * @author Frank Dellaert * @date July 2015 From 4711f5807dbe9afe78f5ee9b82b0d3ada650432a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 11 Nov 2023 10:31:42 -0500 Subject: [PATCH 026/208] discrete error method that returns an ADT --- gtsam/discrete/DecisionTreeFactor.cpp | 16 ++++++++++++++++ gtsam/discrete/DecisionTreeFactor.h | 3 +++ gtsam/discrete/DiscreteFactor.h | 15 ++++++++++----- gtsam/discrete/TableFactor.cpp | 5 +++++ gtsam/discrete/TableFactor.h | 3 +++ .../discrete/tests/testDecisionTreeFactor.cpp | 18 ++++++++++++++++++ gtsam_unstable/discrete/AllDiff.h | 5 +++++ gtsam_unstable/discrete/BinaryAllDiff.h | 5 +++++ gtsam_unstable/discrete/Domain.h | 5 +++++ gtsam_unstable/discrete/SingleValue.h | 5 +++++ 10 files changed, 75 insertions(+), 5 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index 7202e6853..cbb26016c 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -62,6 +62,22 @@ namespace gtsam { return error(values.discrete()); } + /* ************************************************************************ */ + AlgebraicDecisionTree DecisionTreeFactor::error() const { + // Get all possible assignments + DiscreteKeys dkeys = discreteKeys(); + // Reverse to make cartesian product output a more natural ordering. + DiscreteKeys rdkeys(dkeys.rbegin(), dkeys.rend()); + const auto assignments = DiscreteValues::CartesianProduct(rdkeys); + + // Construct vector with error values + std::vector errors; + for (const auto& assignment : assignments) { + errors.push_back(error(assignment)); + } + return AlgebraicDecisionTree(dkeys, errors); + } + /* ************************************************************************ */ double DecisionTreeFactor::safe_div(const double& a, const double& b) { // The use for safe_div is when we divide the product factor by the sum diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index a9a7e5ce0..5e0acc056 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -292,6 +292,9 @@ namespace gtsam { */ double error(const HybridValues& values) const override; + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree error() const override; + /// @} private: diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index 24b2b55e4..e84533655 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -18,9 +18,10 @@ #pragma once +#include +#include #include #include -#include #include namespace gtsam { @@ -35,7 +36,7 @@ class HybridValues; * * @ingroup discrete */ -class GTSAM_EXPORT DiscreteFactor: public Factor { +class GTSAM_EXPORT DiscreteFactor : public Factor { public: // typedefs needed to play nice with gtsam typedef DiscreteFactor This; ///< This class @@ -103,7 +104,11 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { */ double error(const HybridValues& c) const override; - /// Multiply in a DecisionTreeFactor and return the result as DecisionTreeFactor + /// Compute error for each assignment and return as a tree + virtual AlgebraicDecisionTree error() const = 0; + + /// Multiply in a DecisionTreeFactor and return the result as + /// DecisionTreeFactor virtual DecisionTreeFactor operator*(const DecisionTreeFactor&) const = 0; virtual DecisionTreeFactor toDecisionTreeFactor() const = 0; @@ -111,7 +116,7 @@ class GTSAM_EXPORT DiscreteFactor: public Factor { /// @} /// @name Wrapper support /// @{ - + /// Translation table from values to strings. using Names = DiscreteValues::Names; @@ -175,4 +180,4 @@ template<> struct traits : public Testable {}; std::vector expNormalize(const std::vector &logProbs); -}// namespace gtsam +} // namespace gtsam diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index f4e023a4d..be5f2af5b 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -168,6 +168,11 @@ double TableFactor::error(const HybridValues& values) const { return error(values.discrete()); } +/* ************************************************************************ */ +AlgebraicDecisionTree TableFactor::error() const { + return toDecisionTreeFactor().error(); +} + /* ************************************************************************ */ DecisionTreeFactor TableFactor::operator*(const DecisionTreeFactor& f) const { return toDecisionTreeFactor() * f; diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 828e794e6..40ed231fd 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -358,6 +358,9 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { */ double error(const HybridValues& values) const override; + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree error() const override; + /// @} }; diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index 9d73475a3..69ee52662 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -67,6 +67,24 @@ TEST( DecisionTreeFactor, constructors) EXPECT_DOUBLES_EQUAL(0.8, f4(x121), 1e-9); } +/* ************************************************************************* */ +TEST(DecisionTreeFactor, Error) { + // Declare a bunch of keys + DiscreteKey X(0,2), Y(1,3), Z(2,2); + + // Create factors + DecisionTreeFactor f(X & Y & Z, "2 5 3 6 4 7 25 55 35 65 45 75"); + + auto errors = f.error(); + // regression + AlgebraicDecisionTree expected( + {X, Y, Z}, + vector{-0.69314718, -1.6094379, -1.0986123, -1.7917595, + -1.3862944, -1.9459101, -3.2188758, -4.0073332, -3.5553481, + -4.1743873, -3.8066625, -4.3174881}); + EXPECT(assert_equal(expected, errors, 1e-6)); +} + /* ************************************************************************* */ TEST(DecisionTreeFactor, multiplication) { DiscreteKey v0(0, 2), v1(1, 2), v2(2, 2); diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 9496fc1a6..9c8e62ecd 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -53,6 +53,11 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { /// Multiply into a decisiontree DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree error() const override { + throw std::runtime_error("AllDiff::error not implemented"); + } + /* * Ensure Arc-consistency by checking every possible value of domain j. * @param j domain to be checked diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index b207acb9d..33f6562b4 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -91,6 +91,11 @@ class BinaryAllDiff : public Constraint { const Domains&) const override { throw std::runtime_error("BinaryAllDiff::partiallyApply not implemented"); } + + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree error() const override { + throw std::runtime_error("BinaryAllDiff::error not implemented"); + } }; } // namespace gtsam diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index 0f5b5fdf9..ca7340a9f 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -69,6 +69,11 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { } } + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree error() const override { + throw std::runtime_error("Domain::error not implemented"); + } + // Return concise string representation, mostly to debug arc consistency. // Converts from base 0 to base1. std::string base1Str() const; diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index 1c726d4d0..f57f24b42 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -49,6 +49,11 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { } } + /// Compute error for each assignment and return as a tree + AlgebraicDecisionTree error() const override { + throw std::runtime_error("SingleValue::error not implemented"); + } + /// Calculate value double operator()(const DiscreteValues& values) const override; From 114a0b220b430e96b9d61962a53a6a48aa36cc86 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 12 Nov 2023 22:32:58 -0500 Subject: [PATCH 027/208] printErrors method for HybridGaussianFactorGraph --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 81 +++++++++++++++++++++- gtsam/hybrid/HybridGaussianFactorGraph.h | 16 ++++- 2 files changed, 93 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 7a7ca0cbf..6dd7186e2 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -74,6 +74,86 @@ const Ordering HybridOrdering(const HybridGaussianFactorGraph &graph) { index, KeyVector(discrete_keys.begin(), discrete_keys.end()), true); } +/* ************************************************************************ */ +void HybridGaussianFactorGraph::printErrors( + const HybridValues &values, const std::string &str, + const KeyFormatter &keyFormatter, + const std::function + &printCondition) const { + std::cout << str << "size: " << size() << std::endl << std::endl; + + std::stringstream ss; + + for (size_t i = 0; i < factors_.size(); i++) { + auto &&factor = factors_[i]; + std::cout << "Factor " << i << ": "; + + // Clear the stringstream + ss.str(std::string()); + + if (auto gmf = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + gmf->error(values.continuous()).print("", DefaultKeyFormatter); + std::cout << std::endl; + } + } else if (auto hc = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + + if (hc->isContinuous()) { + std::cout << "error = " << hc->asGaussian()->error(values) << "\n"; + } else if (hc->isDiscrete()) { + std::cout << "error = "; + hc->asDiscrete()->error().print("", DefaultKeyFormatter); + std::cout << "\n"; + } else { + // Is hybrid + std::cout << "error = "; + hc->asMixture()->error(values.continuous()).print(); + std::cout << "\n"; + } + } + } else if (auto gf = std::dynamic_pointer_cast(factor)) { + const double errorValue = (factor != nullptr ? gf->error(values) : .0); + if (!printCondition(factor.get(), errorValue, i)) + continue; // User-provided filter did not pass + + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = " << errorValue << "\n"; + } + } else if (auto df = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + df->error().print("", DefaultKeyFormatter); + std::cout << std::endl; + } + + } else { + continue; + } + + std::cout << "\n"; + } + std::cout.flush(); +} + /* ************************************************************************ */ static GaussianFactorGraphTree addGaussian( const GaussianFactorGraphTree &gfgTree, @@ -96,7 +176,6 @@ static GaussianFactorGraphTree addGaussian( // TODO(dellaert): it's probably more efficient to first collect the discrete // keys, and then loop over all assignments to populate a vector. GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { - GaussianFactorGraphTree result; for (auto &f : factors_) { diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index b3f159150..bfbd7ab3c 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -140,9 +140,19 @@ class GTSAM_EXPORT HybridGaussianFactorGraph /// @{ // TODO(dellaert): customize print and equals. - // void print(const std::string& s = "HybridGaussianFactorGraph", - // const KeyFormatter& keyFormatter = DefaultKeyFormatter) const - // override; + // void print( + // const std::string& s = "HybridGaussianFactorGraph", + // const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + + void printErrors( + const HybridValues& values, + const std::string& str = "HybridGaussianFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const std::function& + printCondition = + [](const Factor*, double, size_t) { return true; }) const; + // bool equals(const This& fg, double tol = 1e-9) const override; /// @} From b2ab23375087ddc45209d6d22c6614f8e8f21709 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 12 Nov 2023 22:33:16 -0500 Subject: [PATCH 028/208] printErrors method for HybridNonlinearFactorGraph --- gtsam/hybrid/HybridNonlinearFactorGraph.cpp | 92 +++++++++++++++++++ gtsam/hybrid/HybridNonlinearFactorGraph.h | 12 ++- .../tests/testHybridNonlinearFactorGraph.cpp | 31 +++++-- 3 files changed, 128 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index e51adb9cd..c2a8f81a4 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -42,6 +42,98 @@ void HybridNonlinearFactorGraph::print(const std::string& s, } } +/* ************************************************************************* */ +void HybridNonlinearFactorGraph::printErrors( + const HybridValues& values, const std::string& str, + const KeyFormatter& keyFormatter, + const std::function& printCondition) const { + std::cout << str << "size: " << size() << std::endl << std::endl; + + std::stringstream ss; + + for (size_t i = 0; i < factors_.size(); i++) { + auto&& factor = factors_[i]; + std::cout << "Factor " << i << ": "; + + // Clear the stringstream + ss.str(std::string()); + + if (auto mf = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + mf->error(values.nonlinear()).print("", DefaultKeyFormatter); + std::cout << std::endl; + } + } else if (auto gmf = + std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + gmf->error(values.continuous()).print("", DefaultKeyFormatter); + std::cout << std::endl; + } + } else if (auto gm = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + gm->error(values.continuous()).print("", DefaultKeyFormatter); + std::cout << std::endl; + } + } else if (auto nf = std::dynamic_pointer_cast(factor)) { + const double errorValue = (factor != nullptr ? nf->error(values) : .0); + if (!printCondition(factor.get(), errorValue, i)) + continue; // User-provided filter did not pass + + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = " << errorValue << "\n"; + } + } else if (auto gf = std::dynamic_pointer_cast(factor)) { + const double errorValue = (factor != nullptr ? gf->error(values) : .0); + if (!printCondition(factor.get(), errorValue, i)) + continue; // User-provided filter did not pass + + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = " << errorValue << "\n"; + } + } else if (auto df = std::dynamic_pointer_cast(factor)) { + if (factor == nullptr) { + std::cout << "nullptr" + << "\n"; + } else { + factor->print(ss.str(), keyFormatter); + std::cout << "error = "; + df->error().print("", DefaultKeyFormatter); + std::cout << std::endl; + } + + } else { + continue; + } + + std::cout << "\n"; + } + std::cout.flush(); +} + /* ************************************************************************* */ HybridGaussianFactorGraph::shared_ptr HybridNonlinearFactorGraph::linearize( const Values& continuousValues) const { diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.h b/gtsam/hybrid/HybridNonlinearFactorGraph.h index 89b4fb391..54dc9e93f 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.h +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.h @@ -34,7 +34,7 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { protected: public: using Base = HybridFactorGraph; - using This = HybridNonlinearFactorGraph; ///< this class + using This = HybridNonlinearFactorGraph; ///< this class using shared_ptr = std::shared_ptr; ///< shared_ptr to This using Values = gtsam::Values; ///< backwards compatibility @@ -63,6 +63,16 @@ class GTSAM_EXPORT HybridNonlinearFactorGraph : public HybridFactorGraph { const std::string& s = "HybridNonlinearFactorGraph", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override; + /** print errors along with factors*/ + void printErrors( + const HybridValues& values, + const std::string& str = "HybridNonlinearFactorGraph: ", + const KeyFormatter& keyFormatter = DefaultKeyFormatter, + const std::function& + printCondition = + [](const Factor*, double, size_t) { return true; }) const; + /// @} /// @name Standard Interface /// @{ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index a493de5c5..c801f0bd7 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -327,8 +327,8 @@ GaussianFactorGraph::shared_ptr batchGFG(double between, NonlinearFactorGraph graph; graph.addPrior(X(0), 0, Isotropic::Sigma(1, 0.1)); - auto between_x0_x1 = std::make_shared( - X(0), X(1), between, Isotropic::Sigma(1, 1.0)); + auto between_x0_x1 = std::make_shared(X(0), X(1), between, + Isotropic::Sigma(1, 1.0)); graph.push_back(between_x0_x1); @@ -397,6 +397,25 @@ TEST(HybridFactorGraph, Partial_Elimination) { EXPECT(remainingFactorGraph->at(2)->keys() == KeyVector({M(0), M(1)})); } +TEST(HybridFactorGraph, PrintErrors) { + Switching self(3); + + // Get nonlinear factor graph and add linear factors to be holistic + HybridNonlinearFactorGraph fg = self.nonlinearFactorGraph; + fg.add(self.linearizedFactorGraph); + + // Optimize to get HybridValues + HybridBayesNet::shared_ptr bn = + self.linearizedFactorGraph.eliminateSequential(); + HybridValues hv = bn->optimize(); + + // Print and verify + fg.print(); + std::cout << "\n\n\n" << std::endl; + fg.printErrors( + HybridValues(hv.continuous(), DiscreteValues(), self.linearizationPoint)); +} + /**************************************************************************** * Test full elimination */ @@ -564,7 +583,7 @@ factor 6: P( m1 | m0 ): )"; #else -string expected_hybridFactorGraph = R"( + string expected_hybridFactorGraph = R"( size: 7 factor 0: A[x0] = [ @@ -759,9 +778,9 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { KeyVector contKeys = {X(0), X(1)}; auto noise_model = noiseModel::Isotropic::Sigma(3, 1.0); auto still = std::make_shared(X(0), X(1), Pose2(0, 0, 0), - noise_model), + noise_model), moving = std::make_shared(X(0), X(1), odometry, - noise_model); + noise_model); std::vector motion_models = {still, moving}; fg.emplace_shared( contKeys, DiscreteKeys{gtsam::DiscreteKey(M(1), 2)}, motion_models); @@ -788,7 +807,7 @@ TEST(HybridFactorGraph, DefaultDecisionTree) { initialEstimate.insert(L(1), Point2(4.1, 1.8)); // We want to eliminate variables not connected to DCFactors first. - const Ordering ordering {L(0), L(1), X(0), X(1)}; + const Ordering ordering{L(0), L(1), X(0), X(1)}; HybridGaussianFactorGraph linearized = *fg.linearize(initialEstimate); From 5387299b8b3a0e5ee3865a66183b67cf01207ed6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 14 Nov 2023 04:48:23 -0500 Subject: [PATCH 029/208] use correct key formatter --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 6 +++--- gtsam/hybrid/HybridNonlinearFactorGraph.cpp | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 6dd7186e2..3b2d4a024 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -99,7 +99,7 @@ void HybridGaussianFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - gmf->error(values.continuous()).print("", DefaultKeyFormatter); + gmf->error(values.continuous()).print("", keyFormatter); std::cout << std::endl; } } else if (auto hc = std::dynamic_pointer_cast(factor)) { @@ -113,7 +113,7 @@ void HybridGaussianFactorGraph::printErrors( std::cout << "error = " << hc->asGaussian()->error(values) << "\n"; } else if (hc->isDiscrete()) { std::cout << "error = "; - hc->asDiscrete()->error().print("", DefaultKeyFormatter); + hc->asDiscrete()->error().print("", keyFormatter); std::cout << "\n"; } else { // Is hybrid @@ -141,7 +141,7 @@ void HybridGaussianFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - df->error().print("", DefaultKeyFormatter); + df->error().print("", keyFormatter); std::cout << std::endl; } diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index c2a8f81a4..e0dfd413c 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -66,7 +66,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - mf->error(values.nonlinear()).print("", DefaultKeyFormatter); + mf->error(values.nonlinear()).print("", keyFormatter); std::cout << std::endl; } } else if (auto gmf = @@ -77,7 +77,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - gmf->error(values.continuous()).print("", DefaultKeyFormatter); + gmf->error(values.continuous()).print("", keyFormatter); std::cout << std::endl; } } else if (auto gm = std::dynamic_pointer_cast(factor)) { @@ -87,7 +87,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - gm->error(values.continuous()).print("", DefaultKeyFormatter); + gm->error(values.continuous()).print("", keyFormatter); std::cout << std::endl; } } else if (auto nf = std::dynamic_pointer_cast(factor)) { @@ -121,7 +121,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - df->error().print("", DefaultKeyFormatter); + df->error().print("", keyFormatter); std::cout << std::endl; } From 95a534e7c13d77ad0b670affe93b185e16ee24ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 16 Nov 2023 17:16:05 -0500 Subject: [PATCH 030/208] error method for HybridBayesNet --- gtsam/hybrid/HybridBayesNet.cpp | 30 +++++++++++++++++ gtsam/hybrid/HybridBayesNet.h | 10 ++++++ gtsam/hybrid/tests/testHybridBayesNet.cpp | 39 +++++++++++++++++++++++ 3 files changed, 79 insertions(+) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 3bafe5a9c..31177ddb7 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -281,6 +281,36 @@ HybridValues HybridBayesNet::sample() const { return sample(&kRandomNumberGenerator); } +/* ************************************************************************* */ +AlgebraicDecisionTree HybridBayesNet::error( + const VectorValues &continuousValues) const { + AlgebraicDecisionTree result(0.0); + + // Iterate over each conditional. + for (auto &&conditional : *this) { + if (auto gm = conditional->asMixture()) { + // If conditional is hybrid, compute error for all assignments. + result = result + gm->error(continuousValues); + + } else if (auto gc = conditional->asGaussian()) { + // If continuous, get the error and add it to the result + double error = gc->error(continuousValues); + // Add the computed error to every leaf of the result tree. + result = result.apply( + [error](double leaf_value) { return leaf_value + error; }); + + } else if (auto dc = conditional->asDiscrete()) { + // If discrete, add the discrete error in the right branch + result = result.apply( + [dc](const Assignment &assignment, double leaf_value) { + return leaf_value + dc->error(DiscreteValues(assignment)); + }); + } + } + + return result; +} + /* ************************************************************************* */ AlgebraicDecisionTree HybridBayesNet::logProbability( const VectorValues &continuousValues) const { diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index e71cfe9b4..2934ef176 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -187,6 +187,16 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * @param continuousValues Continuous values at which to compute the error. * @return AlgebraicDecisionTree */ + AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + + /** + * @brief Compute log probability for each discrete assignment, + * and return as a tree. + * + * @param continuousValues Continuous values at which + * to compute the log probability. + * @return AlgebraicDecisionTree + */ AlgebraicDecisionTree logProbability( const VectorValues &continuousValues) const; diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 5248fce01..66985cc78 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -153,6 +153,45 @@ TEST(HybridBayesNet, Choose) { *gbn.at(3))); } +/* ****************************************************************************/ +// Test error for a hybrid Bayes net P(X0|X1) P(X1|Asia) P(Asia). +TEST(HybridBayesNet, Error) { + const auto continuousConditional = GaussianConditional::sharedMeanAndStddev( + X(0), 2 * I_1x1, X(1), Vector1(-4.0), 5.0); + + const SharedDiagonal model0 = noiseModel::Diagonal::Sigmas(Vector1(2.0)), + model1 = noiseModel::Diagonal::Sigmas(Vector1(3.0)); + + const auto conditional0 = std::make_shared( + X(1), Vector1::Constant(5), I_1x1, model0), + conditional1 = std::make_shared( + X(1), Vector1::Constant(2), I_1x1, model1); + + auto gm = + new GaussianMixture({X(1)}, {}, {Asia}, {conditional0, conditional1}); + // Create hybrid Bayes net. + HybridBayesNet bayesNet; + bayesNet.push_back(continuousConditional); + bayesNet.emplace_back(gm); + bayesNet.emplace_back(new DiscreteConditional(Asia, "99/1")); + + // Create values at which to evaluate. + HybridValues values; + values.insert(asiaKey, 0); + values.insert(X(0), Vector1(-6)); + values.insert(X(1), Vector1(1)); + + AlgebraicDecisionTree actual_errors = + bayesNet.error(values.continuous()); + + // Regression. + // Manually added all the error values from the 3 conditional types. + AlgebraicDecisionTree expected_errors( + {Asia}, std::vector{2.33005033585, 5.38619084965}); + + EXPECT(assert_equal(expected_errors, actual_errors)); +} + /* ****************************************************************************/ // Test Bayes net optimize TEST(HybridBayesNet, OptimizeAssignment) { From a2ba56de27d39d4ea9527fd8be8d382af104ee35 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 16 Nov 2023 17:36:41 -0500 Subject: [PATCH 031/208] include BayesNet::error in HybridBayesNet --- gtsam/hybrid/HybridBayesNet.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 2934ef176..f84eaa70d 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -189,6 +189,9 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { */ AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + /// Error method using HybridValues which returns specific error. + using Base::error; + /** * @brief Compute log probability for each discrete assignment, * and return as a tree. From eabd11df6fdf339cfbc87b36d4064ae4177b5d09 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Nov 2023 10:34:11 -0500 Subject: [PATCH 032/208] better docstring for BayesNet::error --- gtsam/hybrid/HybridBayesNet.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index f84eaa70d..22e03bba9 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -189,7 +189,10 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { */ AlgebraicDecisionTree error(const VectorValues &continuousValues) const; - /// Error method using HybridValues which returns specific error. + /** + * @brief Error method using HybridValues which returns specific error for + * assignment. + */ using Base::error; /** From 7d7e83348dd469761aff0d84ea0d7ae44df786cf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Nov 2023 13:56:19 -0500 Subject: [PATCH 033/208] add test case for HybridGaussianFactorGraph::printErrors --- .../tests/testHybridGaussianFactorGraph.cpp | 98 ++++++++++++++++++- 1 file changed, 97 insertions(+), 1 deletion(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index b240e1626..daee44ab6 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -658,7 +658,7 @@ bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements, } /* ****************************************************************************/ -// Check that the factor graph unnormalized probability is proportional to the +// Check that the bayes net unnormalized probability is proportional to the // Bayes net probability for the given measurements. bool ratioTest(const HybridBayesNet &bn, const VectorValues &measurements, const HybridBayesNet &posterior, size_t num_samples = 100) { @@ -907,6 +907,102 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { EXPECT(ratioTest(bn, measurements, *posterior)); } +/* ****************************************************************************/ +// Test printErrors with multivariate example. +TEST(HybridGaussianFactorGraph, PrintErrors) { + HybridGaussianFactorGraph hfg; + HybridBayesNet bayesNet; + + size_t num_measurements = 1; + // Create Gaussian mixture z_i = x0 + noise for each measurement. + for (size_t i = 0; i < num_measurements; i++) { + const DiscreteKey mode_i{M(i), 2}; + bayesNet.emplace_back(new GaussianMixture( + {Z(i)}, {X(0)}, {mode_i}, + {GaussianConditional::sharedMeanAndStddev(Z(i), I_3x3, X(0), Z_3x1, 10), + GaussianConditional::sharedMeanAndStddev(Z(i), I_3x3, X(0), Z_3x1, + 0.1)})); + } + + // Create prior on X(0). + bayesNet.push_back(GaussianConditional::sharedMeanAndStddev( + X(0), Vector3(1.0, 2.0, 5.0), 0.5)); + + // Add prior on mode. + const size_t nrModes = 1; + for (size_t i = 0; i < nrModes; i++) { + bayesNet.emplace_back(new DiscreteConditional({M(i), 2}, "4/6")); + } + + VectorValues measurements{{Z(0), Vector3(1.0, 2.0, 5.0)}}; + HybridGaussianFactorGraph measurement_fg = + bayesNet.toFactorGraph(measurements); + HybridValues values = bayesNet.optimize(); + + std::stringstream buffer; + // Save the original output stream so we can reset later + std::streambuf *old = std::cout.rdbuf(buffer.rdbuf()); + + // We test against actual std::cout for faithful reproduction + measurement_fg.printErrors(values); + + // Get output string and reset stdout + std::string actual = buffer.str(); + std::cout.rdbuf(old); + + std::string expected = R"(HybridGaussianFactorGraph: size: 3 + +Factor 0: Hybrid [x0; m0]{ + Choice(m0) + 0 Leaf : + A[x0] = [ + -1, -0, -0; + -0, -1, -0; + -0, -0, -1; + 0, 0, 0 +] + b = [ -1 -2 -5 5.25652 ] + Noise model: diagonal sigmas [10; 10; 10; 1]; + + 1 Leaf : + A[x0] = [ + -1, -0, -0; + -0, -1, -0; + -0, -0, -1 +] + b = [ -1 -2 -5 ] +isotropic dim=3 sigma=0.1 + +} +error = Choice(m0) + 0 Leaf 13.815511 + 1 Leaf 0 + + +Factor 1: p(x0) + R = [ 1 0 0 ] + [ 0 1 0 ] + [ 0 0 1 ] + d = [ 1 2 5 ] + mean: 1 elements + x0: 1 2 5 +isotropic dim=3 sigma=0.5 +error = 0 + +Factor 2: P( m0 ): + Choice(m0) + 0 Leaf 0.4 + 1 Leaf 0.6 + +error = Choice(m0) + 0 Leaf 0.91629073 + 1 Leaf 0.51082562 + + +)"; + EXPECT(expected == actual); +} + /* ************************************************************************* */ int main() { TestResult tr; From cd5c13065b77c8c90a0f4ca2fecb79a06b9df637 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Nov 2023 13:58:52 -0500 Subject: [PATCH 034/208] remove extra newline --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 3b2d4a024..467cff710 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -142,7 +142,6 @@ void HybridGaussianFactorGraph::printErrors( factor->print(ss.str(), keyFormatter); std::cout << "error = "; df->error().print("", keyFormatter); - std::cout << std::endl; } } else { From 7695fd6de3fc00cf7d348dc2f30cd6e6f4548f1a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Nov 2023 13:24:39 -0500 Subject: [PATCH 035/208] Improved HybridBayesNet::optimize with proper model selection --- gtsam/hybrid/HybridBayesNet.cpp | 60 +++++++++++++++++++++++++++++++-- 1 file changed, 57 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 31177ddb7..ba869a6f5 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -220,15 +220,69 @@ GaussianBayesNet HybridBayesNet::choose( /* ************************************************************************* */ HybridValues HybridBayesNet::optimize() const { // Collect all the discrete factors to compute MPE - DiscreteBayesNet discrete_bn; + DiscreteFactorGraph discrete_fg; + VectorValues continuousValues; + for (auto &&conditional : *this) { if (conditional->isDiscrete()) { - discrete_bn.push_back(conditional->asDiscrete()); + discrete_fg.push_back(conditional->asDiscrete()); + } else { + /* + Perform the integration of L(X;M, Z)P(X|M) which is the model selection + term. + TODO(Varun) Write better comments detailing the whole process. + */ + if (conditional->isContinuous()) { + auto gc = conditional->asGaussian(); + for (GaussianConditional::const_iterator frontal = gc->beginFrontals(); + frontal != gc->endFrontals(); ++frontal) { + continuousValues.insert_or_assign(*frontal, + Vector::Zero(gc->getDim(frontal))); + } + } else if (conditional->isHybrid()) { + auto gm = conditional->asMixture(); + gm->conditionals().apply( + [&continuousValues](const GaussianConditional::shared_ptr &gc) { + if (gc) { + for (GaussianConditional::const_iterator frontal = gc->begin(); + frontal != gc->end(); ++frontal) { + continuousValues.insert_or_assign( + *frontal, Vector::Zero(gc->getDim(frontal))); + } + } + return gc; + }); + + DecisionTree error = gm->error(continuousValues); + + // Functional to take error and compute the probability + auto integrate = [&gm](const double &error) { + // q(mu; M, Z) = exp(-error) + // k = 1.0 / sqrt((2*pi)^n*det(Sigma)) + // thus, q*sqrt(|2*pi*Sigma|) = q/k = exp(log(q) - log(k)) + // = exp(-error - log(k)) + double prob = std::exp(-error - gm->logNormalizationConstant()); + if (prob > 1e-12) { + return prob; + } else { + return 1.0; + } + }; + AlgebraicDecisionTree model_selection = + DecisionTree(error, integrate); + + std::cout << "\n\nmodel selection"; + model_selection.print("", DefaultKeyFormatter); + discrete_fg.push_back( + DecisionTreeFactor(gm->discreteKeys(), model_selection)); + } } } // Solve for the MPE - DiscreteValues mpe = DiscreteFactorGraph(discrete_bn).optimize(); + discrete_fg.print(); + DiscreteValues mpe = discrete_fg.optimize(); + mpe.print("mpe"); // Given the MPE, compute the optimal continuous values. return HybridValues(optimize(mpe), mpe); From 39f7ac20a1d8a0bdaff47d31dd61331188a5a14c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Nov 2023 13:28:05 -0500 Subject: [PATCH 036/208] handle nullptrsin GaussianMixture::error --- gtsam/hybrid/GaussianMixture.cpp | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 753e35bf0..a1ba167f5 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -316,8 +316,15 @@ AlgebraicDecisionTree GaussianMixture::logProbability( AlgebraicDecisionTree GaussianMixture::error( const VectorValues &continuousValues) const { auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) { - return conditional->error(continuousValues) + // - logConstant_ - conditional->logNormalizationConstant(); + // Check if valid pointer + if (conditional) { + return conditional->error(continuousValues) + // + logConstant_ - conditional->logNormalizationConstant(); + } else { + // If not valid, pointer, it means this conditional was pruned, + // so we return maximum error. + return std::numeric_limits::max(); + } }; DecisionTree errorTree(conditionals_, errorFunc); return errorTree; From c374a26b45ed1f25c597ad08d12594ee4414282e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 20 Nov 2023 13:31:07 -0500 Subject: [PATCH 037/208] nicer HybridBayesNet::optimize with normalized errors --- gtsam/hybrid/HybridBayesNet.cpp | 75 ++++++++++++++++++++++----------- 1 file changed, 51 insertions(+), 24 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index ba869a6f5..cd1157576 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -223,22 +223,38 @@ HybridValues HybridBayesNet::optimize() const { DiscreteFactorGraph discrete_fg; VectorValues continuousValues; + // Error values for each hybrid factor + AlgebraicDecisionTree error(0.0); + std::set discreteKeySet; + for (auto &&conditional : *this) { if (conditional->isDiscrete()) { discrete_fg.push_back(conditional->asDiscrete()); } else { /* - Perform the integration of L(X;M, Z)P(X|M) which is the model selection - term. - TODO(Varun) Write better comments detailing the whole process. + Perform the integration of L(X;M,Z)P(X|M) + which is the model selection term. + + By Bayes' rule, P(X|M) ∝ L(X;M,Z)P(X|M), + hence L(X;M,Z)P(X|M) is the unnormalized probabilty of + the joint Gaussian distribution. + + This can be computed by multiplying all the exponentiaed errors + of each of the conditionals, which we do below in hybrid case. */ if (conditional->isContinuous()) { + /* + If we are here, it means there are no discrete variables in + the Bayes net (due to strong elimination ordering). + This is a continuous-only problem hence model selection doesn't matter. + */ auto gc = conditional->asGaussian(); for (GaussianConditional::const_iterator frontal = gc->beginFrontals(); frontal != gc->endFrontals(); ++frontal) { continuousValues.insert_or_assign(*frontal, Vector::Zero(gc->getDim(frontal))); } + } else if (conditional->isHybrid()) { auto gm = conditional->asMixture(); gm->conditionals().apply( @@ -253,36 +269,47 @@ HybridValues HybridBayesNet::optimize() const { return gc; }); - DecisionTree error = gm->error(continuousValues); + /* + To perform model selection, we need: + q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) - // Functional to take error and compute the probability - auto integrate = [&gm](const double &error) { - // q(mu; M, Z) = exp(-error) - // k = 1.0 / sqrt((2*pi)^n*det(Sigma)) - // thus, q*sqrt(|2*pi*Sigma|) = q/k = exp(log(q) - log(k)) - // = exp(-error - log(k)) - double prob = std::exp(-error - gm->logNormalizationConstant()); - if (prob > 1e-12) { - return prob; - } else { - return 1.0; - } + If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) + thus, q*sqrt(|2*pi*Sigma|) = q/k = exp(log(q/k)) + = exp(log(q) - log(k)) = exp(-error - log(k)) + = exp(-(error + log(k))) + + So let's compute (error + log(k)) and exponentiate later + */ + error = error + gm->error(continuousValues); + + // Add the logNormalization constant to the error + // Also compute the mean for normalization (for numerical stability) + double mean = 0.0; + auto addConstant = [&gm, &mean](const double &error) { + double e = error + gm->logNormalizationConstant(); + mean += e; + return e; }; - AlgebraicDecisionTree model_selection = - DecisionTree(error, integrate); + error = error.apply(addConstant); + // Normalize by the mean + error = error.apply([&mean](double x) { return x / mean; }); - std::cout << "\n\nmodel selection"; - model_selection.print("", DefaultKeyFormatter); - discrete_fg.push_back( - DecisionTreeFactor(gm->discreteKeys(), model_selection)); + // Include the discrete keys + std::copy(gm->discreteKeys().begin(), gm->discreteKeys().end(), + std::inserter(discreteKeySet, discreteKeySet.end())); } } } + AlgebraicDecisionTree model_selection = DecisionTree( + error, [](const double &error) { return std::exp(-error); }); + + discrete_fg.push_back(DecisionTreeFactor( + DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()), + model_selection)); + // Solve for the MPE - discrete_fg.print(); DiscreteValues mpe = discrete_fg.optimize(); - mpe.print("mpe"); // Given the MPE, compute the optimal continuous values. return HybridValues(optimize(mpe), mpe); From 9dad12eabf8dbf81c2b28a9a16f4d1b5f09a1d7d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 21 Nov 2023 16:19:19 -0500 Subject: [PATCH 038/208] comment out print --- gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index c801f0bd7..93081d309 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -410,10 +410,10 @@ TEST(HybridFactorGraph, PrintErrors) { HybridValues hv = bn->optimize(); // Print and verify - fg.print(); - std::cout << "\n\n\n" << std::endl; - fg.printErrors( - HybridValues(hv.continuous(), DiscreteValues(), self.linearizationPoint)); + // fg.print(); + // std::cout << "\n\n\n" << std::endl; + // fg.printErrors( + // HybridValues(hv.continuous(), DiscreteValues(), self.linearizationPoint)); } /**************************************************************************** From cf42a0819d6b03b1cf86eb48dbdd196663c71d73 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 25 Nov 2023 15:13:18 -0500 Subject: [PATCH 039/208] remove test case --- .../tests/testHybridGaussianFactorGraph.cpp | 96 ------------------- 1 file changed, 96 deletions(-) diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index daee44ab6..99c2299c0 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -907,102 +907,6 @@ TEST(HybridGaussianFactorGraph, EliminateSwitchingNetwork) { EXPECT(ratioTest(bn, measurements, *posterior)); } -/* ****************************************************************************/ -// Test printErrors with multivariate example. -TEST(HybridGaussianFactorGraph, PrintErrors) { - HybridGaussianFactorGraph hfg; - HybridBayesNet bayesNet; - - size_t num_measurements = 1; - // Create Gaussian mixture z_i = x0 + noise for each measurement. - for (size_t i = 0; i < num_measurements; i++) { - const DiscreteKey mode_i{M(i), 2}; - bayesNet.emplace_back(new GaussianMixture( - {Z(i)}, {X(0)}, {mode_i}, - {GaussianConditional::sharedMeanAndStddev(Z(i), I_3x3, X(0), Z_3x1, 10), - GaussianConditional::sharedMeanAndStddev(Z(i), I_3x3, X(0), Z_3x1, - 0.1)})); - } - - // Create prior on X(0). - bayesNet.push_back(GaussianConditional::sharedMeanAndStddev( - X(0), Vector3(1.0, 2.0, 5.0), 0.5)); - - // Add prior on mode. - const size_t nrModes = 1; - for (size_t i = 0; i < nrModes; i++) { - bayesNet.emplace_back(new DiscreteConditional({M(i), 2}, "4/6")); - } - - VectorValues measurements{{Z(0), Vector3(1.0, 2.0, 5.0)}}; - HybridGaussianFactorGraph measurement_fg = - bayesNet.toFactorGraph(measurements); - HybridValues values = bayesNet.optimize(); - - std::stringstream buffer; - // Save the original output stream so we can reset later - std::streambuf *old = std::cout.rdbuf(buffer.rdbuf()); - - // We test against actual std::cout for faithful reproduction - measurement_fg.printErrors(values); - - // Get output string and reset stdout - std::string actual = buffer.str(); - std::cout.rdbuf(old); - - std::string expected = R"(HybridGaussianFactorGraph: size: 3 - -Factor 0: Hybrid [x0; m0]{ - Choice(m0) - 0 Leaf : - A[x0] = [ - -1, -0, -0; - -0, -1, -0; - -0, -0, -1; - 0, 0, 0 -] - b = [ -1 -2 -5 5.25652 ] - Noise model: diagonal sigmas [10; 10; 10; 1]; - - 1 Leaf : - A[x0] = [ - -1, -0, -0; - -0, -1, -0; - -0, -0, -1 -] - b = [ -1 -2 -5 ] -isotropic dim=3 sigma=0.1 - -} -error = Choice(m0) - 0 Leaf 13.815511 - 1 Leaf 0 - - -Factor 1: p(x0) - R = [ 1 0 0 ] - [ 0 1 0 ] - [ 0 0 1 ] - d = [ 1 2 5 ] - mean: 1 elements - x0: 1 2 5 -isotropic dim=3 sigma=0.5 -error = 0 - -Factor 2: P( m0 ): - Choice(m0) - 0 Leaf 0.4 - 1 Leaf 0.6 - -error = Choice(m0) - 0 Leaf 0.91629073 - 1 Leaf 0.51082562 - - -)"; - EXPECT(expected == actual); -} - /* ************************************************************************* */ int main() { TestResult tr; From bf6680b2d378d9be2eb7d3914b33fcf4425676f8 Mon Sep 17 00:00:00 2001 From: etamarlu <69709091+etamarlu@users.noreply.github.com> Date: Mon, 27 Nov 2023 13:55:05 -0500 Subject: [PATCH 040/208] Update CMakeLists.txt add a missing field to "matlab_wrap" when GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX=ON --- matlab/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index e38d7cb6f..60f3074ce 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -99,7 +99,7 @@ if(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) # Wrap matlab_wrap(${GTSAM_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i "gtsam_unstable" - "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}" "${ignore}") + "${GTSAM_ADDITIONAL_LIBRARIES}" "" "${mexFlags}" "${ignore}" "${GTSAM_ENABLE_BOOST_SERIALIZATION}") endif(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX) # Record the root dir for gtsam - needed during external builds, e.g., ROS From 6f365b330bd8b067c456b9b5defc1cc30cde97ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 4 Dec 2023 05:04:06 -0500 Subject: [PATCH 041/208] fix call to Ellipse, plus some formatting --- python/gtsam/utils/plot.py | 21 +++++++++++---------- 1 file changed, 11 insertions(+), 10 deletions(-) diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index fe067e787..84ca89a0f 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -12,8 +12,8 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import import gtsam from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values -# For translation between a scaling of the uncertainty ellipse and the -# percentage of inliers see discussion in +# For translation between a scaling of the uncertainty ellipse and the +# percentage of inliers see discussion in # [PR 1067](https://github.com/borglab/gtsam/pull/1067) # and the notebook python/gtsam/notebooks/ellipses.ipynb (needs scipy). # @@ -32,6 +32,7 @@ from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values # 2D 39.34693 86.46647 98.88910 99.96645 99.99963 # 3D 19.87480 73.85359 97.07091 99.88660 99.99846 + def set_axes_equal(fignum: int) -> None: """ Make axes of 3D plot have equal scale so that spheres appear as spheres, @@ -127,8 +128,7 @@ def plot_covariance_ellipse_3d(axes, axes.plot_surface(x, y, z, alpha=alpha, cmap='hot') -def plot_covariance_ellipse_2d(axes, - origin: Point2, +def plot_covariance_ellipse_2d(axes, origin: Point2, covariance: np.ndarray) -> None: """ Plots a Gaussian as an uncertainty ellipse @@ -154,7 +154,7 @@ def plot_covariance_ellipse_2d(axes, e1 = patches.Ellipse(origin, np.sqrt(w[0]) * 2 * k, np.sqrt(w[1]) * 2 * k, - np.rad2deg(angle), + angle=np.rad2deg(angle), fill=False) axes.add_patch(e1) @@ -180,12 +180,13 @@ def plot_point2_on_axes(axes, if P is not None: plot_covariance_ellipse_2d(axes, point, P) + def plot_point2( - fignum: int, - point: Point2, - linespec: str, - P: np.ndarray = None, - axis_labels: Iterable[str] = ("X axis", "Y axis"), + fignum: int, + point: Point2, + linespec: str, + P: np.ndarray = None, + axis_labels: Iterable[str] = ("X axis", "Y axis"), ) -> plt.Figure: """ Plot a 2D point on given figure with given `linespec`. From bc1b949f8b616d218b7c121105a33734cd098da3 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 5 Dec 2023 14:10:29 -0500 Subject: [PATCH 042/208] remove ParameterMatrix method --- gtsam/basis/Chebyshev.h | 10 ---------- gtsam/basis/Chebyshev2.h | 5 ----- gtsam/basis/FitBasis.h | 2 +- gtsam/basis/Fourier.h | 5 ----- 4 files changed, 1 insertion(+), 21 deletions(-) diff --git a/gtsam/basis/Chebyshev.h b/gtsam/basis/Chebyshev.h index 25bd69698..1c16c47bf 100644 --- a/gtsam/basis/Chebyshev.h +++ b/gtsam/basis/Chebyshev.h @@ -34,11 +34,6 @@ struct GTSAM_EXPORT Chebyshev1Basis : Basis { Parameters parameters_; - /// Return a zero initialized Parameter matrix. - static Parameters ParameterMatrix(size_t N) { - return Parameters::Zero(N); - } - /** * @brief Evaluate Chebyshev Weights on [-1,1] at x up to order N-1 (N values) * @@ -85,11 +80,6 @@ struct GTSAM_EXPORT Chebyshev1Basis : Basis { struct GTSAM_EXPORT Chebyshev2Basis : Basis { using Parameters = Eigen::Matrix; - /// Return a zero initialized Parameter matrix. - static Parameters ParameterMatrix(size_t N) { - return Parameters::Zero(N); - } - /** * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values). * diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 0c253fb75..6d0bc7f6b 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -86,11 +86,6 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { return points; } - /// Return a zero initialized Parameter matrix. - static Parameters ParameterMatrix(size_t N) { - return Parameters::Zero(N); - } - /** * Evaluate Chebyshev Weights on [-1,1] at any x up to order N-1 (N values) * These weights implement barycentric interpolation at a specific x. diff --git a/gtsam/basis/FitBasis.h b/gtsam/basis/FitBasis.h index 6e7e809c7..f5cb99bd7 100644 --- a/gtsam/basis/FitBasis.h +++ b/gtsam/basis/FitBasis.h @@ -74,7 +74,7 @@ class FitBasis { const Sequence& sequence, const SharedNoiseModel& model, size_t N) { NonlinearFactorGraph graph = NonlinearGraph(sequence, model, N); Values values; - values.insert(0, Basis::ParameterMatrix(N)); + values.insert(0, Parameters::Zero(N)); GaussianFactorGraph::shared_ptr gfg = graph.linearize(values); return gfg; } diff --git a/gtsam/basis/Fourier.h b/gtsam/basis/Fourier.h index 65d141561..eb259bd8a 100644 --- a/gtsam/basis/Fourier.h +++ b/gtsam/basis/Fourier.h @@ -51,11 +51,6 @@ class FourierBasis : public Basis { return b; } - /// Return a zero initialized Parameter matrix. - static Parameters ParameterMatrix(size_t N) { - return Parameters::Zero(N); - } - /** * @brief Evaluate Real Fourier Weights of size N in interval [a, b], * e.g. N=5 yields bases: 1, cos(x), sin(x), cos(2*x), sin(2*x) From 7799fd5a6a8f5059e09cb4173fb542b03c56894e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 5 Dec 2023 14:13:11 -0500 Subject: [PATCH 043/208] remove overridden WeightMatrix method --- gtsam/basis/Chebyshev2.cpp | 9 --------- gtsam/basis/Chebyshev2.h | 10 ---------- 2 files changed, 19 deletions(-) diff --git a/gtsam/basis/Chebyshev2.cpp b/gtsam/basis/Chebyshev2.cpp index c37fa9f6b..63fca64cc 100644 --- a/gtsam/basis/Chebyshev2.cpp +++ b/gtsam/basis/Chebyshev2.cpp @@ -59,15 +59,6 @@ Weights Chebyshev2::CalculateWeights(size_t N, double x, double a, double b) { return weights / d; } -Matrix Chebyshev2::WeightMatrix(size_t N, const Vector& X, double a, double b) { - // Chebyshev points go from 0 to N, hence N+1 points. - Matrix W(X.size(), N); - for (int i = 0; i < X.size(); i++) { - W.row(i) = CalculateWeights(N, X(i), a, b); - } - return W; -} - Weights Chebyshev2::DerivativeWeights(size_t N, double x, double a, double b) { // Allocate space for weights Weights weightDerivatives(N); diff --git a/gtsam/basis/Chebyshev2.h b/gtsam/basis/Chebyshev2.h index 6d0bc7f6b..849a51104 100644 --- a/gtsam/basis/Chebyshev2.h +++ b/gtsam/basis/Chebyshev2.h @@ -97,16 +97,6 @@ class GTSAM_EXPORT Chebyshev2 : public Basis { static Weights CalculateWeights(size_t N, double x, double a = -1, double b = 1); - /** - * Calculate weights for all x in vector X. - * Returns M*N matrix where M is the size of the vector X, - * and N is the number of basis functions. - * - * Overriden for Chebyshev2. - */ - static Matrix WeightMatrix(size_t N, const Vector& X, double a = -1, - double b = 1); - /** * Evaluate derivative of barycentric weights. * This is easy and efficient via the DifferentiationMatrix. From 50670da07ceeb2947ca75d15cd3b52a6fd0adbb9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 12 Dec 2023 05:57:23 -0500 Subject: [PATCH 044/208] HybridValues formatting --- gtsam/hybrid/HybridValues.h | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/gtsam/hybrid/HybridValues.h b/gtsam/hybrid/HybridValues.h index 40d4b5e92..09e7d773c 100644 --- a/gtsam/hybrid/HybridValues.h +++ b/gtsam/hybrid/HybridValues.h @@ -12,6 +12,7 @@ /** * @file HybridValues.h * @date Jul 28, 2022 + * @author Varun Agrawal * @author Shangjie Xue */ @@ -54,13 +55,13 @@ class GTSAM_EXPORT HybridValues { HybridValues() = default; /// Construct from DiscreteValues and VectorValues. - HybridValues(const VectorValues &cv, const DiscreteValues &dv) - : continuous_(cv), discrete_(dv){} + HybridValues(const VectorValues& cv, const DiscreteValues& dv) + : continuous_(cv), discrete_(dv) {} /// Construct from all values types. HybridValues(const VectorValues& cv, const DiscreteValues& dv, const Values& v) - : continuous_(cv), discrete_(dv), nonlinear_(v){} + : continuous_(cv), discrete_(dv), nonlinear_(v) {} /// @} /// @name Testable @@ -101,9 +102,7 @@ class GTSAM_EXPORT HybridValues { bool existsDiscrete(Key j) { return (discrete_.find(j) != discrete_.end()); } /// Check whether a variable with key \c j exists in values. - bool existsNonlinear(Key j) { - return nonlinear_.exists(j); - } + bool existsNonlinear(Key j) { return nonlinear_.exists(j); } /// Check whether a variable with key \c j exists. bool exists(Key j) { @@ -128,9 +127,7 @@ class GTSAM_EXPORT HybridValues { } /// insert_or_assign() , similar to Values.h - void insert_or_assign(Key j, size_t value) { - discrete_[j] = value; - } + void insert_or_assign(Key j, size_t value) { discrete_[j] = value; } /** Insert all continuous values from \c values. Throws an invalid_argument * exception if any keys to be inserted are already used. */ From af490e9ffcbab044637f9a9b1885cfc4e751332c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 12 Dec 2023 06:26:57 -0500 Subject: [PATCH 045/208] sum and normalize helper methods for the AlgebraicDecisionTree --- gtsam/discrete/AlgebraicDecisionTree.h | 19 +++++++++++++++++++ gtsam/hybrid/HybridBayesNet.cpp | 13 +++++++------ 2 files changed, 26 insertions(+), 6 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 9f55f3b63..1e5dc848b 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -196,6 +196,25 @@ namespace gtsam { return this->apply(g, &Ring::div); } + /// Compute sum of all values + double sum() const { + double sum = 0; + auto visitor = [&](int y) { sum += y; }; + this->visit(visitor); + return sum; + } + + /** + * @brief Helper method to perform normalization such that all leaves in the + * tree sum to 1 + * + * @param sum + * @return AlgebraicDecisionTree + */ + AlgebraicDecisionTree normalize(double sum) const { + return this->apply([&sum](const double& x) { return x / sum; }); + } + /** sum out variable */ AlgebraicDecisionTree sum(const L& label, size_t cardinality) const { return this->combine(label, cardinality, &Ring::add); diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index cd1157576..eea51d329 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -283,16 +283,17 @@ HybridValues HybridBayesNet::optimize() const { error = error + gm->error(continuousValues); // Add the logNormalization constant to the error - // Also compute the mean for normalization (for numerical stability) - double mean = 0.0; - auto addConstant = [&gm, &mean](const double &error) { + // Also compute the sum for discrete probability normalization + // (normalization trick for numerical stability) + double sum = 0.0; + auto addConstant = [&gm, &sum](const double &error) { double e = error + gm->logNormalizationConstant(); - mean += e; + sum += e; return e; }; error = error.apply(addConstant); - // Normalize by the mean - error = error.apply([&mean](double x) { return x / mean; }); + // Normalize by the sum + error = error.normalize(sum); // Include the discrete keys std::copy(gm->discreteKeys().begin(), gm->discreteKeys().end(), From c004bd8df0775bdb14f6df46334545cf1ac03b90 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 12 Dec 2023 19:15:24 -0500 Subject: [PATCH 046/208] test for differing covariances --- gtsam/hybrid/tests/testMixtureFactor.cpp | 70 ++++++++++++++++++++++++ 1 file changed, 70 insertions(+) diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 67a7fd8ae..50bf2d4b4 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -18,6 +18,9 @@ #include #include +#include +#include +#include #include #include #include @@ -114,6 +117,73 @@ TEST(MixtureFactor, Dim) { EXPECT_LONGS_EQUAL(1, mixtureFactor.dim()); } +/* ************************************************************************* */ +// Test components with differing covariances +TEST(MixtureFactor, DifferentCovariances) { + DiscreteKey m1(M(1), 2); + + Values values; + double x1 = 1.0, x2 = 1.0; + values.insert(X(1), x1); + values.insert(X(2), x2); + + double between = 0.0; + + auto model0 = noiseModel::Isotropic::Sigma(1, 1e2); + auto model1 = noiseModel::Isotropic::Sigma(1, 1e-2); + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-3); + + auto f0 = + std::make_shared>(X(1), X(2), between, model0); + auto f1 = + std::make_shared>(X(1), X(2), between, model1); + std::vector factors{f0, f1}; + + // Create via toFactorGraph + using symbol_shorthand::Z; + Matrix H0_1, H0_2, H1_1, H1_2; + Vector d0 = f0->evaluateError(x1, x2, &H0_1, &H0_2); + std::vector> terms0 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H0_1 /*Sp1*/}, + {X(2), H0_2 /*Tp2*/}}; + + Vector d1 = f1->evaluateError(x1, x2, &H1_1, &H1_2); + std::vector> terms1 = {{Z(1), gtsam::I_1x1 /*Rx*/}, + // + {X(1), H1_1 /*Sp1*/}, + {X(2), H1_2 /*Tp2*/}}; + auto gm = new gtsam::GaussianMixture( + {Z(1)}, {X(1), X(2)}, {m1}, + {std::make_shared(terms0, 1, -d0, model0), + std::make_shared(terms1, 1, -d1, model1)}); + gtsam::HybridBayesNet bn2; + bn2.emplace_back(gm); + + gtsam::VectorValues measurements; + measurements.insert(Z(1), gtsam::Z_1x1); + // Create FG with single GaussianMixtureFactor + auto mixture_fg = bn2.toFactorGraph(measurements); + + // Linearized prior factor on X1 + auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); + mixture_fg.push_back(prior); + + auto hbn = mixture_fg.eliminateSequential(); + // hbn->print("\n\nfinal bayes net"); + + HybridValues actual_values = hbn->optimize(); + + VectorValues cv; + cv.insert(X(1), Vector1(0.0)); + cv.insert(X(2), Vector1(0.0)); + DiscreteValues dv; + dv.insert({M(1), 1}); + HybridValues expected_values(cv, dv); + + EXPECT(assert_equal(expected_values, actual_values)); +} + /* ************************************************************************* */ int main() { TestResult tr; From 7b56c96b430cf9d828016749c3299366a8a67b0e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 15 Dec 2023 15:19:48 -0500 Subject: [PATCH 047/208] differing means test --- gtsam/hybrid/tests/testMixtureFactor.cpp | 45 ++++++++++++++++++++++++ 1 file changed, 45 insertions(+) diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 50bf2d4b4..4bb716540 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -117,6 +117,51 @@ TEST(MixtureFactor, Dim) { EXPECT_LONGS_EQUAL(1, mixtureFactor.dim()); } +/* ************************************************************************* */ +// Test components with differing means +TEST(MixtureFactor, DifferentMeans) { + DiscreteKey m1(M(1), 2), m2(M(2), 2); + + Values values; + double x1 = 0.0, x2 = 1.75, x3 = 2.60; + values.insert(X(1), x1); + values.insert(X(2), x2); + values.insert(X(3), x3); + + auto model0 = noiseModel::Isotropic::Sigma(1, 1e-0); + auto model1 = noiseModel::Isotropic::Sigma(1, 1e-0); + auto prior_noise = noiseModel::Isotropic::Sigma(1, 1e-0); + + auto f0 = std::make_shared>(X(1), X(2), 0.0, model0); + auto f1 = std::make_shared>(X(1), X(2), 2.0, model1); + std::vector factors{f0, f1}; + + MixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); + HybridNonlinearFactorGraph hnfg; + hnfg.push_back(mixtureFactor); + + f0 = std::make_shared>(X(2), X(3), 0.0, model0); + f1 = std::make_shared>(X(2), X(3), 2.0, model1); + std::vector factors23{f0, f1}; + hnfg.push_back(MixtureFactor({X(2), X(3)}, {m2}, factors23)); + + auto prior = PriorFactor(X(1), x1, prior_noise); + hnfg.push_back(prior); + + hnfg.emplace_shared>(X(2), 2.0, prior_noise); + + auto hgfg = hnfg.linearize(values); + auto bn = hgfg->eliminateSequential(); + HybridValues actual = bn->optimize(); + + HybridValues expected( + VectorValues{ + {X(1), Vector1(0.0)}, {X(2), Vector1(0.25)}, {X(3), Vector1(-0.6)}}, + DiscreteValues{{M(1), 1}, {M(2), 0}}); + + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ // Test components with differing covariances TEST(MixtureFactor, DifferentCovariances) { From e549a9b41fb6980e4fb1fbcf89df40b927797d9c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 15 Dec 2023 15:21:25 -0500 Subject: [PATCH 048/208] normalize model selection term --- gtsam/hybrid/HybridBayesNet.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index eea51d329..f2cac485d 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -239,7 +239,7 @@ HybridValues HybridBayesNet::optimize() const { hence L(X;M,Z)P(X|M) is the unnormalized probabilty of the joint Gaussian distribution. - This can be computed by multiplying all the exponentiaed errors + This can be computed by multiplying all the exponentiated errors of each of the conditionals, which we do below in hybrid case. */ if (conditional->isContinuous()) { @@ -288,7 +288,7 @@ HybridValues HybridBayesNet::optimize() const { double sum = 0.0; auto addConstant = [&gm, &sum](const double &error) { double e = error + gm->logNormalizationConstant(); - sum += e; + sum += std::abs(e); return e; }; error = error.apply(addConstant); @@ -302,12 +302,17 @@ HybridValues HybridBayesNet::optimize() const { } } + double min_log = error.min(); AlgebraicDecisionTree model_selection = DecisionTree( - error, [](const double &error) { return std::exp(-error); }); + error, [&min_log](const double &x) { return std::exp(-(x - min_log)); }); + model_selection = model_selection + exp(-min_log); - discrete_fg.push_back(DecisionTreeFactor( - DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()), - model_selection)); + // Only add model_selection if we have discrete keys + if (discreteKeySet.size() > 0) { + discrete_fg.push_back(DecisionTreeFactor( + DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()), + model_selection)); + } // Solve for the MPE DiscreteValues mpe = discrete_fg.optimize(); From b2638c8698689545c0b0825a0a7fd3d1bd7fc0ba Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 17 Dec 2023 15:14:03 -0500 Subject: [PATCH 049/208] max and min functions for AlgebraicDecisionTree --- gtsam/discrete/AlgebraicDecisionTree.h | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 1e5dc848b..1a6358680 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -199,7 +199,7 @@ namespace gtsam { /// Compute sum of all values double sum() const { double sum = 0; - auto visitor = [&](int y) { sum += y; }; + auto visitor = [&](double y) { sum += y; }; this->visit(visitor); return sum; } @@ -215,6 +215,22 @@ namespace gtsam { return this->apply([&sum](const double& x) { return x / sum; }); } + /// Find the minimum values amongst all leaves + double min() const { + double min = std::numeric_limits::max(); + auto visitor = [&](double x) { min = x < min ? x : min; }; + this->visit(visitor); + return min; + } + + /// Find the maximum values amongst all leaves + double max() const { + double max = std::numeric_limits::min(); + auto visitor = [&](double x) { max = x > max ? x : max; }; + this->visit(visitor); + return max; + } + /** sum out variable */ AlgebraicDecisionTree sum(const L& label, size_t cardinality) const { return this->combine(label, cardinality, &Ring::add); From 6f09be51cbf145590b3ba545f59fdbc5aa9c1718 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 17 Dec 2023 18:55:15 -0500 Subject: [PATCH 050/208] error normalization and log-sum-exp trick --- gtsam/hybrid/HybridBayesNet.cpp | 7 ++++--- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 23 ++++++++++++++++------ 2 files changed, 21 insertions(+), 9 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index f2cac485d..e48b3faf7 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -303,9 +303,10 @@ HybridValues HybridBayesNet::optimize() const { } double min_log = error.min(); - AlgebraicDecisionTree model_selection = DecisionTree( - error, [&min_log](const double &x) { return std::exp(-(x - min_log)); }); - model_selection = model_selection + exp(-min_log); + AlgebraicDecisionTree model_selection = + DecisionTree(error, [&min_log](const double &x) { + return std::exp(-(x - min_log)) * exp(-min_log); + }); // Only add model_selection if we have discrete keys if (discreteKeySet.size() > 0) { diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 467cff710..dfff3d4f3 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -323,18 +323,29 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // If there are no more continuous parents, then we create a // DiscreteFactor here, with the error for each discrete choice. - // Integrate the probability mass in the last continuous conditional using - // the unnormalized probability q(μ;m) = exp(-error(μ;m)) at the mean. - // discrete_probability = exp(-error(μ;m)) * sqrt(det(2π Σ_m)) - auto probability = [&](const Result &pair) -> double { + // Compute the probability q(μ;m) = exp(-error(μ;m)) * sqrt(det(2π Σ_m)) + // from the residual error at the mean μ. + // The residual error contains no keys, and only depends on the discrete + // separator if present. + auto logProbability = [&](const Result &pair) -> double { + // auto probability = [&](const Result &pair) -> double { static const VectorValues kEmpty; // If the factor is not null, it has no keys, just contains the residual. const auto &factor = pair.second; if (!factor) return 1.0; // TODO(dellaert): not loving this. - return exp(-factor->error(kEmpty)) / pair.first->normalizationConstant(); + + // exp(-factor->error(kEmpty)) / pair.first->normalizationConstant(); + return -factor->error(kEmpty) - pair.first->logNormalizationConstant(); }; - DecisionTree probabilities(eliminationResults, probability); + AlgebraicDecisionTree logProbabilities( + DecisionTree(eliminationResults, logProbability)); + + // Perform normalization + double max_log = logProbabilities.max(); + DecisionTree probabilities( + logProbabilities, + [&max_log](const double x) { return exp(x - max_log) * exp(max_log); }); return { std::make_shared(gaussianMixture), From 36604297d7db6f8a0c299f782a157c72f1a70cf9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 18 Dec 2023 14:25:19 -0500 Subject: [PATCH 051/208] handle numerical instability --- gtsam/discrete/AlgebraicDecisionTree.h | 3 ++- gtsam/hybrid/HybridBayesNet.cpp | 28 ++++++++++----------- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 7 +++--- gtsam/hybrid/tests/testHybridEstimation.cpp | 1 - gtsam/linear/GaussianConditional.cpp | 6 +++-- 5 files changed, 24 insertions(+), 21 deletions(-) diff --git a/gtsam/discrete/AlgebraicDecisionTree.h b/gtsam/discrete/AlgebraicDecisionTree.h index 1a6358680..17385a975 100644 --- a/gtsam/discrete/AlgebraicDecisionTree.h +++ b/gtsam/discrete/AlgebraicDecisionTree.h @@ -225,7 +225,8 @@ namespace gtsam { /// Find the maximum values amongst all leaves double max() const { - double max = std::numeric_limits::min(); + // Get the most negative value + double max = -std::numeric_limits::max(); auto visitor = [&](double x) { max = x > max ? x : max; }; this->visit(visitor); return max; diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index e48b3faf7..2b0b11e36 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -274,26 +274,26 @@ HybridValues HybridBayesNet::optimize() const { q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) - thus, q*sqrt(|2*pi*Sigma|) = q/k = exp(log(q/k)) + thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) = exp(log(q) - log(k)) = exp(-error - log(k)) = exp(-(error + log(k))) - So let's compute (error + log(k)) and exponentiate later + So we compute (error + log(k)) and exponentiate later */ - error = error + gm->error(continuousValues); - // Add the logNormalization constant to the error + // Add the error and the logNormalization constant to the error + auto err = gm->error(continuousValues) + gm->logNormalizationConstant(); + // Also compute the sum for discrete probability normalization // (normalization trick for numerical stability) double sum = 0.0; - auto addConstant = [&gm, &sum](const double &error) { - double e = error + gm->logNormalizationConstant(); + auto absSum = [&sum](const double &e) { sum += std::abs(e); return e; }; - error = error.apply(addConstant); - // Normalize by the sum - error = error.normalize(sum); + err.visit(absSum); + // Normalize by the sum to prevent overflow + error = error + err.normalize(sum); // Include the discrete keys std::copy(gm->discreteKeys().begin(), gm->discreteKeys().end(), @@ -302,11 +302,11 @@ HybridValues HybridBayesNet::optimize() const { } } - double min_log = error.min(); - AlgebraicDecisionTree model_selection = - DecisionTree(error, [&min_log](const double &x) { - return std::exp(-(x - min_log)) * exp(-min_log); - }); + error = error * -1; + double max_log = error.max(); + AlgebraicDecisionTree model_selection = DecisionTree( + error, [&max_log](const double &x) { return std::exp(x - max_log); }); + model_selection = model_selection.normalize(model_selection.sum()); // Only add model_selection if we have discrete keys if (discreteKeySet.size() > 0) { diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index dfff3d4f3..260dc6bbe 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -328,7 +328,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // The residual error contains no keys, and only depends on the discrete // separator if present. auto logProbability = [&](const Result &pair) -> double { - // auto probability = [&](const Result &pair) -> double { static const VectorValues kEmpty; // If the factor is not null, it has no keys, just contains the residual. const auto &factor = pair.second; @@ -343,9 +342,11 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // Perform normalization double max_log = logProbabilities.max(); - DecisionTree probabilities( + AlgebraicDecisionTree probabilities = DecisionTree( logProbabilities, - [&max_log](const double x) { return exp(x - max_log) * exp(max_log); }); + [&max_log](const double x) { return exp(x - max_log); }); + // probabilities.print("", DefaultKeyFormatter); + probabilities = probabilities.normalize(probabilities.sum()); return { std::make_shared(gaussianMixture), diff --git a/gtsam/hybrid/tests/testHybridEstimation.cpp b/gtsam/hybrid/tests/testHybridEstimation.cpp index b8edc39d8..1cc28b386 100644 --- a/gtsam/hybrid/tests/testHybridEstimation.cpp +++ b/gtsam/hybrid/tests/testHybridEstimation.cpp @@ -333,7 +333,6 @@ TEST(HybridEstimation, Probability) { for (auto discrete_conditional : *discreteBayesNet) { bayesNet->add(discrete_conditional); } - auto discreteConditional = discreteBayesNet->at(0)->asDiscrete(); HybridValues hybrid_values = bayesNet->optimize(); diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 0112835aa..4ec1d8b95 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -184,8 +184,10 @@ namespace gtsam { double GaussianConditional::logNormalizationConstant() const { constexpr double log2pi = 1.8378770664093454835606594728112; size_t n = d().size(); - // log det(Sigma)) = - 2.0 * logDeterminant() - return - 0.5 * n * log2pi + logDeterminant(); + // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1} + // log det(Sigma) = -log(det(R'R)) = -2*log(det(R)) + // Hence, log det(Sigma)) = - 2.0 * logDeterminant() + return -0.5 * n * log2pi + logDeterminant(); } /* ************************************************************************* */ From 07ddec5f4b4aac614bc1730022b130c99b59a944 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 21 Dec 2023 14:19:21 -0500 Subject: [PATCH 052/208] remove stray comment --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 260dc6bbe..d63f8b5ff 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -345,7 +345,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, AlgebraicDecisionTree probabilities = DecisionTree( logProbabilities, [&max_log](const double x) { return exp(x - max_log); }); - // probabilities.print("", DefaultKeyFormatter); probabilities = probabilities.normalize(probabilities.sum()); return { From c6584f63cea0f2638d2a04a76573cf971ac75c2a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 22 Dec 2023 14:55:31 -0500 Subject: [PATCH 053/208] minor test cleanup --- gtsam/hybrid/tests/testMixtureFactor.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 4bb716540..4819458e8 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -202,20 +202,19 @@ TEST(MixtureFactor, DifferentCovariances) { {Z(1)}, {X(1), X(2)}, {m1}, {std::make_shared(terms0, 1, -d0, model0), std::make_shared(terms1, 1, -d1, model1)}); - gtsam::HybridBayesNet bn2; - bn2.emplace_back(gm); + gtsam::HybridBayesNet bn; + bn.emplace_back(gm); gtsam::VectorValues measurements; measurements.insert(Z(1), gtsam::Z_1x1); // Create FG with single GaussianMixtureFactor - auto mixture_fg = bn2.toFactorGraph(measurements); + HybridGaussianFactorGraph mixture_fg = bn.toFactorGraph(measurements); // Linearized prior factor on X1 auto prior = PriorFactor(X(1), x1, prior_noise).linearize(values); mixture_fg.push_back(prior); auto hbn = mixture_fg.eliminateSequential(); - // hbn->print("\n\nfinal bayes net"); HybridValues actual_values = hbn->optimize(); From fcda1536c6c97bac6c514717342ce0c7fbef3b98 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Fri, 22 Dec 2023 14:53:45 -0800 Subject: [PATCH 054/208] Cleaner version of eliminate --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 102 +++++++++++---------- 1 file changed, 56 insertions(+), 46 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 2029b48e0..7eaefbf85 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -96,7 +96,6 @@ static GaussianFactorGraphTree addGaussian( // TODO(dellaert): it's probably more efficient to first collect the discrete // keys, and then loop over all assignments to populate a vector. GaussianFactorGraphTree HybridGaussianFactorGraph::assembleGraphTree() const { - GaussianFactorGraphTree result; for (auto &f : factors_) { @@ -198,6 +197,51 @@ GaussianFactorGraphTree removeEmpty(const GaussianFactorGraphTree &sum) { } /* ************************************************************************ */ +using Result = std::pair, + GaussianMixtureFactor::sharedFactor>; + +// Integrate the probability mass in the last continuous conditional using +// the unnormalized probability q(μ;m) = exp(-error(μ;m)) at the mean. +// discrete_probability = exp(-error(μ;m)) * sqrt(det(2π Σ_m)) +static std::shared_ptr createDiscreteFactor( + const DecisionTree &eliminationResults, + const DiscreteKeys &discreteSeparator) { + auto probability = [&](const Result &pair) -> double { + const auto &[conditional, factor] = pair; + static const VectorValues kEmpty; + // If the factor is not null, it has no keys, just contains the residual. + if (!factor) return 1.0; // TODO(dellaert): not loving this. + return exp(-factor->error(kEmpty)) / conditional->normalizationConstant(); + }; + + DecisionTree probabilities(eliminationResults, probability); + + return std::make_shared(discreteSeparator, probabilities); +} + +// Create GaussianMixtureFactor on the separator, taking care to correct +// for conditional constants. +static std::shared_ptr createGaussianMixtureFactor( + const DecisionTree &eliminationResults, + const KeyVector &continuousSeparator, + const DiscreteKeys &discreteSeparator) { + // Correct for the normalization constant used up by the conditional + auto correct = [&](const Result &pair) -> GaussianFactor::shared_ptr { + const auto &[conditional, factor] = pair; + if (factor) { + auto hf = std::dynamic_pointer_cast(factor); + if (!hf) throw std::runtime_error("Expected HessianFactor!"); + hf->constantTerm() += 2.0 * conditional->logNormalizationConstant(); + } + return factor; + }; + DecisionTree newFactors(eliminationResults, + correct); + + return std::make_shared(continuousSeparator, + discreteSeparator, newFactors); +} + static std::pair> hybridElimination(const HybridGaussianFactorGraph &factors, const Ordering &frontalKeys, @@ -217,9 +261,6 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // FG has a nullptr as we're looping over the factors. factorGraphTree = removeEmpty(factorGraphTree); - using Result = std::pair, - GaussianMixtureFactor::sharedFactor>; - // This is the elimination method on the leaf nodes auto eliminate = [&](const GaussianFactorGraph &graph) -> Result { if (graph.empty()) { @@ -234,53 +275,22 @@ hybridElimination(const HybridGaussianFactorGraph &factors, // Perform elimination! DecisionTree eliminationResults(factorGraphTree, eliminate); - // Separate out decision tree into conditionals and remaining factors. - const auto [conditionals, newFactors] = unzip(eliminationResults); + // If there are no more continuous parents we create a DiscreteFactor with the + // error for each discrete choice. Otherwise, create a GaussianMixtureFactor + // on the separator, taking care to correct for conditional constants. + auto newFactor = + continuousSeparator.empty() + ? createDiscreteFactor(eliminationResults, discreteSeparator) + : createGaussianMixtureFactor(eliminationResults, continuousSeparator, + discreteSeparator); // Create the GaussianMixture from the conditionals + GaussianMixture::Conditionals conditionals( + eliminationResults, [](const Result &pair) { return pair.first; }); auto gaussianMixture = std::make_shared( frontalKeys, continuousSeparator, discreteSeparator, conditionals); - if (continuousSeparator.empty()) { - // If there are no more continuous parents, then we create a - // DiscreteFactor here, with the error for each discrete choice. - - // Integrate the probability mass in the last continuous conditional using - // the unnormalized probability q(μ;m) = exp(-error(μ;m)) at the mean. - // discrete_probability = exp(-error(μ;m)) * sqrt(det(2π Σ_m)) - auto probability = [&](const Result &pair) -> double { - static const VectorValues kEmpty; - // If the factor is not null, it has no keys, just contains the residual. - const auto &factor = pair.second; - if (!factor) return 1.0; // TODO(dellaert): not loving this. - return exp(-factor->error(kEmpty)) / pair.first->normalizationConstant(); - }; - - DecisionTree probabilities(eliminationResults, probability); - - return { - std::make_shared(gaussianMixture), - std::make_shared(discreteSeparator, probabilities)}; - } else { - // Otherwise, we create a resulting GaussianMixtureFactor on the separator, - // taking care to correct for conditional constant. - - // Correct for the normalization constant used up by the conditional - auto correct = [&](const Result &pair) { - const auto &factor = pair.second; - if (!factor) return; - auto hf = std::dynamic_pointer_cast(factor); - if (!hf) throw std::runtime_error("Expected HessianFactor!"); - hf->constantTerm() += 2.0 * pair.first->logNormalizationConstant(); - }; - eliminationResults.visit(correct); - - const auto mixtureFactor = std::make_shared( - continuousSeparator, discreteSeparator, newFactors); - - return {std::make_shared(gaussianMixture), - mixtureFactor}; - } + return {std::make_shared(gaussianMixture), newFactor}; } /* ************************************************************************ From ebcf958d691f1be867900bb5c0a45b24ad36eff9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Dec 2023 16:04:48 -0500 Subject: [PATCH 055/208] better, more correct version of model selection --- gtsam/hybrid/HybridBayesNet.cpp | 208 ++++++++++++++++++++++---------- 1 file changed, 142 insertions(+), 66 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 2b0b11e36..23a1c7787 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -26,6 +26,18 @@ static std::mt19937_64 kRandomNumberGenerator(42); namespace gtsam { +using std::dynamic_pointer_cast; + +/* ************************************************************************ */ +// Throw a runtime exception for method specified in string s, +// and conditional f: +static void throwRuntimeError(const std::string &s, + const std::shared_ptr &f) { + auto &fr = *f; + throw std::runtime_error(s + " not implemented for conditional type " + + demangle(typeid(fr).name()) + "."); +} + /* ************************************************************************* */ void HybridBayesNet::print(const std::string &s, const KeyFormatter &formatter) const { @@ -217,6 +229,56 @@ GaussianBayesNet HybridBayesNet::choose( return gbn; } +/* ************************************************************************ */ +static GaussianBayesNetTree addGaussian( + const GaussianBayesNetTree &gfgTree, + const GaussianConditional::shared_ptr &factor) { + // If the decision tree is not initialized, then initialize it. + if (gfgTree.empty()) { + GaussianBayesNet result{factor}; + return GaussianBayesNetTree(result); + } else { + auto add = [&factor](const GaussianBayesNet &graph) { + auto result = graph; + result.push_back(factor); + return result; + }; + return gfgTree.apply(add); + } +} + +/* ************************************************************************ */ +GaussianBayesNetTree HybridBayesNet::assembleTree() const { + GaussianBayesNetTree result; + + for (auto &f : factors_) { + // TODO(dellaert): just use a virtual method defined in HybridFactor. + if (auto gm = dynamic_pointer_cast(f)) { + result = gm->add(result); + } else if (auto hc = dynamic_pointer_cast(f)) { + if (auto gm = hc->asMixture()) { + result = gm->add(result); + } else if (auto g = hc->asGaussian()) { + result = addGaussian(result, g); + } else { + // Has to be discrete. + // TODO(dellaert): in C++20, we can use std::visit. + continue; + } + } else if (dynamic_pointer_cast(f)) { + // Don't do anything for discrete-only factors + // since we want to evaluate continuous values only. + continue; + } else { + // We need to handle the case where the object is actually an + // BayesTreeOrphanWrapper! + throwRuntimeError("HybridBayesNet::assembleTree", f); + } + } + + return result; +} + /* ************************************************************************* */ HybridValues HybridBayesNet::optimize() const { // Collect all the discrete factors to compute MPE @@ -227,74 +289,94 @@ HybridValues HybridBayesNet::optimize() const { AlgebraicDecisionTree error(0.0); std::set discreteKeySet; + // this->print(); + GaussianBayesNetTree bnTree = assembleTree(); + // bnTree.print("", DefaultKeyFormatter, [](const GaussianBayesNet &gbn) { + // gbn.print(); + // return ""; + // }); + /* + Perform the integration of L(X;M,Z)P(X|M) + which is the model selection term. + + By Bayes' rule, P(X|M,Z) ∝ L(X;M,Z)P(X|M), + hence L(X;M,Z)P(X|M) is the unnormalized probabilty of + the joint Gaussian distribution. + + This can be computed by multiplying all the exponentiated errors + of each of the conditionals, which we do below in hybrid case. + */ + /* + To perform model selection, we need: + q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) + + If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) + thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) + = exp(log(q) - log(k)) = exp(-error - log(k)) + = exp(-(error + log(k))) + + So we compute (error + log(k)) and exponentiate later + */ + // Compute the X* of each assignment and use that as the MAP. + DecisionTree x_map( + bnTree, [](const GaussianBayesNet &gbn) { return gbn.optimize(); }); + + // Only compute logNormalizationConstant for now + AlgebraicDecisionTree log_norm_constants = + DecisionTree(bnTree, [](const GaussianBayesNet &gbn) { + if (gbn.size() == 0) { + return -std::numeric_limits::max(); + } + return -gbn.logNormalizationConstant(); + }); + // Compute unnormalized error term and compute model selection term + AlgebraicDecisionTree model_selection_term = log_norm_constants.apply( + [this, &x_map](const Assignment &assignment, double x) { + double error = 0.0; + for (auto &&f : *this) { + if (auto gm = dynamic_pointer_cast(f)) { + error += gm->error( + HybridValues(x_map(assignment), DiscreteValues(assignment))); + } else if (auto hc = dynamic_pointer_cast(f)) { + if (auto gm = hc->asMixture()) { + error += gm->error( + HybridValues(x_map(assignment), DiscreteValues(assignment))); + } else if (auto g = hc->asGaussian()) { + error += g->error(x_map(assignment)); + } + } + } + return -(error + x); + }); + // model_selection_term.print("", DefaultKeyFormatter); + + double max_log = model_selection_term.max(); + AlgebraicDecisionTree model_selection = DecisionTree( + model_selection_term, + [&max_log](const double &x) { return std::exp(x - max_log); }); + model_selection = model_selection.normalize(model_selection.sum()); + // std::cout << "normalized model selection" << std::endl; + // model_selection.print("", DefaultKeyFormatter); + for (auto &&conditional : *this) { if (conditional->isDiscrete()) { discrete_fg.push_back(conditional->asDiscrete()); } else { - /* - Perform the integration of L(X;M,Z)P(X|M) - which is the model selection term. - - By Bayes' rule, P(X|M) ∝ L(X;M,Z)P(X|M), - hence L(X;M,Z)P(X|M) is the unnormalized probabilty of - the joint Gaussian distribution. - - This can be computed by multiplying all the exponentiated errors - of each of the conditionals, which we do below in hybrid case. - */ if (conditional->isContinuous()) { - /* - If we are here, it means there are no discrete variables in - the Bayes net (due to strong elimination ordering). - This is a continuous-only problem hence model selection doesn't matter. - */ - auto gc = conditional->asGaussian(); - for (GaussianConditional::const_iterator frontal = gc->beginFrontals(); - frontal != gc->endFrontals(); ++frontal) { - continuousValues.insert_or_assign(*frontal, - Vector::Zero(gc->getDim(frontal))); - } + // /* + // If we are here, it means there are no discrete variables in + // the Bayes net (due to strong elimination ordering). + // This is a continuous-only problem hence model selection doesn't matter. + // */ + // auto gc = conditional->asGaussian(); + // for (GaussianConditional::const_iterator frontal = gc->beginFrontals(); + // frontal != gc->endFrontals(); ++frontal) { + // continuousValues.insert_or_assign(*frontal, + // Vector::Zero(gc->getDim(frontal))); + // } } else if (conditional->isHybrid()) { auto gm = conditional->asMixture(); - gm->conditionals().apply( - [&continuousValues](const GaussianConditional::shared_ptr &gc) { - if (gc) { - for (GaussianConditional::const_iterator frontal = gc->begin(); - frontal != gc->end(); ++frontal) { - continuousValues.insert_or_assign( - *frontal, Vector::Zero(gc->getDim(frontal))); - } - } - return gc; - }); - - /* - To perform model selection, we need: - q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) - - If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) - thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) - = exp(log(q) - log(k)) = exp(-error - log(k)) - = exp(-(error + log(k))) - - So we compute (error + log(k)) and exponentiate later - */ - - // Add the error and the logNormalization constant to the error - auto err = gm->error(continuousValues) + gm->logNormalizationConstant(); - - // Also compute the sum for discrete probability normalization - // (normalization trick for numerical stability) - double sum = 0.0; - auto absSum = [&sum](const double &e) { - sum += std::abs(e); - return e; - }; - err.visit(absSum); - // Normalize by the sum to prevent overflow - error = error + err.normalize(sum); - // Include the discrete keys std::copy(gm->discreteKeys().begin(), gm->discreteKeys().end(), std::inserter(discreteKeySet, discreteKeySet.end())); @@ -302,12 +384,6 @@ HybridValues HybridBayesNet::optimize() const { } } - error = error * -1; - double max_log = error.max(); - AlgebraicDecisionTree model_selection = DecisionTree( - error, [&max_log](const double &x) { return std::exp(x - max_log); }); - model_selection = model_selection.normalize(model_selection.sum()); - // Only add model_selection if we have discrete keys if (discreteKeySet.size() > 0) { discrete_fg.push_back(DecisionTreeFactor( From 1e298be3b3b09dbb996d40c8d2b7d0997f024508 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Dec 2023 18:47:44 -0500 Subject: [PATCH 056/208] Better way of handling assignments --- gtsam/hybrid/HybridBayesNet.cpp | 76 +++++++++++++++++++-------------- 1 file changed, 43 insertions(+), 33 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 23a1c7787..89be86056 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -285,8 +285,6 @@ HybridValues HybridBayesNet::optimize() const { DiscreteFactorGraph discrete_fg; VectorValues continuousValues; - // Error values for each hybrid factor - AlgebraicDecisionTree error(0.0); std::set discreteKeySet; // this->print(); @@ -313,7 +311,8 @@ HybridValues HybridBayesNet::optimize() const { If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) = exp(log(q) - log(k)) = exp(-error - log(k)) - = exp(-(error + log(k))) + = exp(-(error + log(k))), + where error is computed at the corresponding MAP point, gbn.error(mu). So we compute (error + log(k)) and exponentiate later */ @@ -325,29 +324,45 @@ HybridValues HybridBayesNet::optimize() const { AlgebraicDecisionTree log_norm_constants = DecisionTree(bnTree, [](const GaussianBayesNet &gbn) { if (gbn.size() == 0) { - return -std::numeric_limits::max(); + return 0.0; } - return -gbn.logNormalizationConstant(); + return gbn.logNormalizationConstant(); }); - // Compute unnormalized error term and compute model selection term - AlgebraicDecisionTree model_selection_term = log_norm_constants.apply( - [this, &x_map](const Assignment &assignment, double x) { - double error = 0.0; - for (auto &&f : *this) { - if (auto gm = dynamic_pointer_cast(f)) { - error += gm->error( - HybridValues(x_map(assignment), DiscreteValues(assignment))); - } else if (auto hc = dynamic_pointer_cast(f)) { - if (auto gm = hc->asMixture()) { - error += gm->error( - HybridValues(x_map(assignment), DiscreteValues(assignment))); - } else if (auto g = hc->asGaussian()) { - error += g->error(x_map(assignment)); - } - } + + // Compute unnormalized error term + std::vector labels; + for (auto &&key : x_map.labels()) { + labels.push_back(std::make_pair(key, 2)); + } + + std::vector errors; + x_map.visitWith([this, &errors](const Assignment &assignment, + const VectorValues &mu) { + double error = 0.0; + for (auto &&f : *this) { + if (auto gm = dynamic_pointer_cast(f)) { + error += gm->error(HybridValues(mu, DiscreteValues(assignment))); + } else if (auto hc = dynamic_pointer_cast(f)) { + if (auto gm = hc->asMixture()) { + error += gm->error(HybridValues(mu, DiscreteValues(assignment))); + } else if (auto g = hc->asGaussian()) { + error += g->error(mu); } - return -(error + x); + } + } + errors.push_back(error); + }); + + AlgebraicDecisionTree errorTree = + DecisionTree(labels, errors); + + // Compute model selection term + AlgebraicDecisionTree model_selection_term = errorTree.apply( + [&log_norm_constants](const Assignment assignment, double err) { + return -(err + log_norm_constants(assignment)); }); + + // std::cout << "model selection term" << std::endl; // model_selection_term.print("", DefaultKeyFormatter); double max_log = model_selection_term.max(); @@ -355,6 +370,7 @@ HybridValues HybridBayesNet::optimize() const { model_selection_term, [&max_log](const double &x) { return std::exp(x - max_log); }); model_selection = model_selection.normalize(model_selection.sum()); + // std::cout << "normalized model selection" << std::endl; // model_selection.print("", DefaultKeyFormatter); @@ -363,17 +379,11 @@ HybridValues HybridBayesNet::optimize() const { discrete_fg.push_back(conditional->asDiscrete()); } else { if (conditional->isContinuous()) { - // /* - // If we are here, it means there are no discrete variables in - // the Bayes net (due to strong elimination ordering). - // This is a continuous-only problem hence model selection doesn't matter. - // */ - // auto gc = conditional->asGaussian(); - // for (GaussianConditional::const_iterator frontal = gc->beginFrontals(); - // frontal != gc->endFrontals(); ++frontal) { - // continuousValues.insert_or_assign(*frontal, - // Vector::Zero(gc->getDim(frontal))); - // } + /* + If we are here, it means there are no discrete variables in + the Bayes net (due to strong elimination ordering). + This is a continuous-only problem hence model selection doesn't matter. + */ } else if (conditional->isHybrid()) { auto gm = conditional->asMixture(); From b4f07a01629f5ccb8d9413a82c9b9453627be28d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 25 Dec 2023 23:11:49 -0500 Subject: [PATCH 057/208] cleaner model selection computation --- gtsam/hybrid/HybridBayesNet.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 89be86056..90951b074 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -342,9 +342,11 @@ HybridValues HybridBayesNet::optimize() const { for (auto &&f : *this) { if (auto gm = dynamic_pointer_cast(f)) { error += gm->error(HybridValues(mu, DiscreteValues(assignment))); + } else if (auto hc = dynamic_pointer_cast(f)) { if (auto gm = hc->asMixture()) { error += gm->error(HybridValues(mu, DiscreteValues(assignment))); + } else if (auto g = hc->asGaussian()) { error += g->error(mu); } @@ -356,11 +358,9 @@ HybridValues HybridBayesNet::optimize() const { AlgebraicDecisionTree errorTree = DecisionTree(labels, errors); - // Compute model selection term - AlgebraicDecisionTree model_selection_term = errorTree.apply( - [&log_norm_constants](const Assignment assignment, double err) { - return -(err + log_norm_constants(assignment)); - }); + // Compute model selection term (with help from ADT methods) + AlgebraicDecisionTree model_selection_term = + (errorTree + log_norm_constants) * -1; // std::cout << "model selection term" << std::endl; // model_selection_term.print("", DefaultKeyFormatter); From 6f4343ca94aba06aff18fb3c019d18860af8f219 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 26 Dec 2023 00:20:44 -0500 Subject: [PATCH 058/208] almost working --- gtsam/hybrid/HybridBayesNet.cpp | 45 +++++++++++++++------------------ 1 file changed, 20 insertions(+), 25 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 90951b074..0ff4e342b 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -329,34 +329,29 @@ HybridValues HybridBayesNet::optimize() const { return gbn.logNormalizationConstant(); }); - // Compute unnormalized error term - std::vector labels; - for (auto &&key : x_map.labels()) { - labels.push_back(std::make_pair(key, 2)); - } + // Compute errors as VectorValues + DecisionTree errorVectors = x_map.apply( + [this](const Assignment &assignment, const VectorValues &mu) { + double error = 0.0; + for (auto &&f : *this) { + if (auto gm = dynamic_pointer_cast(f)) { + error += gm->error(HybridValues(mu, DiscreteValues(assignment))); - std::vector errors; - x_map.visitWith([this, &errors](const Assignment &assignment, - const VectorValues &mu) { - double error = 0.0; - for (auto &&f : *this) { - if (auto gm = dynamic_pointer_cast(f)) { - error += gm->error(HybridValues(mu, DiscreteValues(assignment))); + } else if (auto hc = dynamic_pointer_cast(f)) { + if (auto gm = hc->asMixture()) { + error += gm->error(HybridValues(mu, DiscreteValues(assignment))); - } else if (auto hc = dynamic_pointer_cast(f)) { - if (auto gm = hc->asMixture()) { - error += gm->error(HybridValues(mu, DiscreteValues(assignment))); - - } else if (auto g = hc->asGaussian()) { - error += g->error(mu); + } else if (auto g = hc->asGaussian()) { + error += g->error(mu); + } + } } - } - } - errors.push_back(error); - }); - - AlgebraicDecisionTree errorTree = - DecisionTree(labels, errors); + VectorValues e; + e.insert(0, Vector1(error)); + return e; + }); + AlgebraicDecisionTree errorTree = DecisionTree( + errorVectors, [](const VectorValues &v) { return v[0](0); }); // Compute model selection term (with help from ADT methods) AlgebraicDecisionTree model_selection_term = From 409938f4b493fde3c2f7fe84c6ba3b914a115d5a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 26 Dec 2023 16:33:43 -0500 Subject: [PATCH 059/208] improved model selection code --- gtsam/hybrid/HybridBayesNet.cpp | 68 +++++++++++++-------------------- gtsam/hybrid/HybridBayesNet.h | 2 + gtsam/hybrid/HybridFactor.h | 8 ++++ 3 files changed, 37 insertions(+), 41 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 0ff4e342b..027bd75d4 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -248,7 +248,7 @@ static GaussianBayesNetTree addGaussian( } /* ************************************************************************ */ -GaussianBayesNetTree HybridBayesNet::assembleTree() const { +GaussianBayesNetValTree HybridBayesNet::assembleTree() const { GaussianBayesNetTree result; for (auto &f : factors_) { @@ -276,23 +276,17 @@ GaussianBayesNetTree HybridBayesNet::assembleTree() const { } } - return result; + GaussianBayesNetValTree resultTree(result, [](const GaussianBayesNet &gbn) { + return std::make_pair(gbn, 0.0); + }); + return resultTree; } /* ************************************************************************* */ HybridValues HybridBayesNet::optimize() const { // Collect all the discrete factors to compute MPE DiscreteFactorGraph discrete_fg; - VectorValues continuousValues; - std::set discreteKeySet; - - // this->print(); - GaussianBayesNetTree bnTree = assembleTree(); - // bnTree.print("", DefaultKeyFormatter, [](const GaussianBayesNet &gbn) { - // gbn.print(); - // return ""; - // }); /* Perform the integration of L(X;M,Z)P(X|M) which is the model selection term. @@ -316,43 +310,35 @@ HybridValues HybridBayesNet::optimize() const { So we compute (error + log(k)) and exponentiate later */ - // Compute the X* of each assignment and use that as the MAP. - DecisionTree x_map( - bnTree, [](const GaussianBayesNet &gbn) { return gbn.optimize(); }); - // Only compute logNormalizationConstant for now - AlgebraicDecisionTree log_norm_constants = - DecisionTree(bnTree, [](const GaussianBayesNet &gbn) { + std::set discreteKeySet; + GaussianBayesNetValTree bnTree = assembleTree(); + + GaussianBayesNetValTree bn_error = bnTree.apply( + [this](const Assignment &assignment, + const std::pair &gbnAndValue) { + // Compute the X* of each assignment + VectorValues mu = gbnAndValue.first.optimize(); + // Compute the error for X* and the assignment + double error = + this->error(HybridValues(mu, DiscreteValues(assignment))); + + return std::make_pair(gbnAndValue.first, error); + }); + + auto trees = unzip(bn_error); + AlgebraicDecisionTree errorTree = trees.second; + + // Only compute logNormalizationConstant + AlgebraicDecisionTree log_norm_constants = DecisionTree( + bnTree, [](const std::pair &gbnAndValue) { + GaussianBayesNet gbn = gbnAndValue.first; if (gbn.size() == 0) { return 0.0; } return gbn.logNormalizationConstant(); }); - // Compute errors as VectorValues - DecisionTree errorVectors = x_map.apply( - [this](const Assignment &assignment, const VectorValues &mu) { - double error = 0.0; - for (auto &&f : *this) { - if (auto gm = dynamic_pointer_cast(f)) { - error += gm->error(HybridValues(mu, DiscreteValues(assignment))); - - } else if (auto hc = dynamic_pointer_cast(f)) { - if (auto gm = hc->asMixture()) { - error += gm->error(HybridValues(mu, DiscreteValues(assignment))); - - } else if (auto g = hc->asGaussian()) { - error += g->error(mu); - } - } - } - VectorValues e; - e.insert(0, Vector1(error)); - return e; - }); - AlgebraicDecisionTree errorTree = DecisionTree( - errorVectors, [](const VectorValues &v) { return v[0](0); }); - // Compute model selection term (with help from ADT methods) AlgebraicDecisionTree model_selection_term = (errorTree + log_norm_constants) * -1; diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 22e03bba9..f0cdbaaf9 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -118,6 +118,8 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { return evaluate(values); } + GaussianBayesNetValTree assembleTree() const; + /** * @brief Solve the HybridBayesNet by first computing the MPE of all the * discrete variables and then optimizing the continuous variables based on diff --git a/gtsam/hybrid/HybridFactor.h b/gtsam/hybrid/HybridFactor.h index afd1c8032..8828a9172 100644 --- a/gtsam/hybrid/HybridFactor.h +++ b/gtsam/hybrid/HybridFactor.h @@ -33,6 +33,14 @@ class HybridValues; /// Alias for DecisionTree of GaussianFactorGraphs using GaussianFactorGraphTree = DecisionTree; +/// Alias for DecisionTree of GaussianBayesNets +using GaussianBayesNetTree = DecisionTree; +/** + * Alias for DecisionTree of (GaussianBayesNet, double) pairs. + * Used for model selection in BayesNet::optimize + */ +using GaussianBayesNetValTree = + DecisionTree>; KeyVector CollectKeys(const KeyVector &continuousKeys, const DiscreteKeys &discreteKeys); From 93c824c482babdb48cb7c856d906757c57841d3f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Dec 2023 15:34:25 -0500 Subject: [PATCH 060/208] overload == operator for GaussianBayesNet and VectorValues --- gtsam/linear/GaussianBayesNet.h | 6 ++++++ gtsam/linear/VectorValues.h | 5 +++++ 2 files changed, 11 insertions(+) diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index 19781d1e6..d4d60845d 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -82,6 +82,12 @@ namespace gtsam { /** Check equality */ bool equals(const This& bn, double tol = 1e-9) const; + /// Check exact equality. + friend bool operator==(const GaussianBayesNet& lhs, + const GaussianBayesNet& rhs) { + return lhs.isEqual(rhs); + } + /// print graph void print( const std::string& s = "", diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 478cfd770..865fa3c8a 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -260,6 +260,11 @@ namespace gtsam { /** equals required by Testable for unit testing */ bool equals(const VectorValues& x, double tol = 1e-9) const; + /// Check exact equality. + friend bool operator==(const VectorValues& lhs, const VectorValues& rhs) { + return lhs.equals(rhs); + } + /// @{ /// @name Advanced Interface /// @{ From b20d33d79e8aaac6c0cbc76dcee4ba35180c93cb Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Dec 2023 15:38:30 -0500 Subject: [PATCH 061/208] logNormalizationConstant() for GaussianBayesNet --- gtsam/linear/GaussianBayesNet.cpp | 20 ++++++++++++++++++++ gtsam/linear/GaussianBayesNet.h | 8 ++++++++ 2 files changed, 28 insertions(+) diff --git a/gtsam/linear/GaussianBayesNet.cpp b/gtsam/linear/GaussianBayesNet.cpp index b04878ac5..861e19cc9 100644 --- a/gtsam/linear/GaussianBayesNet.cpp +++ b/gtsam/linear/GaussianBayesNet.cpp @@ -243,5 +243,25 @@ namespace gtsam { } /* ************************************************************************* */ + double GaussianBayesNet::logNormalizationConstant() const { + /* + normalization constant = 1.0 / sqrt((2*pi)^n*det(Sigma)) + logConstant = -0.5 * n*log(2*pi) - 0.5 * log det(Sigma) + + log det(Sigma)) = -2.0 * logDeterminant() + thus, logConstant = -0.5*n*log(2*pi) + logDeterminant() + + BayesNet logConstant = sum(-0.5*n_i*log(2*pi) + logDeterminant_i()) + = sum(-0.5*n_i*log(2*pi)) + sum(logDeterminant_i()) + = sum(-0.5*n_i*log(2*pi)) + bn->logDeterminant() + */ + double logNormConst = 0.0; + for (const sharedConditional& cg : *this) { + logNormConst += cg->logNormalizationConstant(); + } + return logNormConst; + } + + /* ************************************************************************* */ } // namespace gtsam diff --git a/gtsam/linear/GaussianBayesNet.h b/gtsam/linear/GaussianBayesNet.h index d4d60845d..ea1cb8603 100644 --- a/gtsam/linear/GaussianBayesNet.h +++ b/gtsam/linear/GaussianBayesNet.h @@ -234,6 +234,14 @@ namespace gtsam { * @return The determinant */ double logDeterminant() const; + /** + * @brief Get the log of the normalization constant corresponding to the + * joint Gaussian density represented by this Bayes net. + * + * @return double + */ + double logNormalizationConstant() const; + /** * Backsubstitute with a different RHS vector than the one stored in this BayesNet. * gy=inv(R*inv(Sigma))*gx From 3a89653e91b9c4fd09d047fef860bf5c3f9fb794 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Dec 2023 15:45:35 -0500 Subject: [PATCH 062/208] helper methods in GaussianMixture for model selection --- gtsam/hybrid/GaussianMixture.cpp | 55 ++++++++++++++++++++++++++++++-- gtsam/hybrid/GaussianMixture.h | 15 +++++++++ 2 files changed, 68 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index a1ba167f5..61b40e566 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -24,6 +24,7 @@ #include #include #include +#include #include namespace gtsam { @@ -92,6 +93,34 @@ GaussianFactorGraphTree GaussianMixture::asGaussianFactorGraphTree() const { return {conditionals_, wrap}; } +/* *******************************************************************************/ +GaussianBayesNetTree GaussianMixture::add( + const GaussianBayesNetTree &sum) const { + using Y = GaussianBayesNet; + auto add = [](const Y &graph1, const Y &graph2) { + auto result = graph1; + if (graph2.size() == 0) { + return GaussianBayesNet(); + } + result.push_back(graph2); + return result; + }; + const auto tree = asGaussianBayesNetTree(); + return sum.empty() ? tree : sum.apply(tree, add); +} + +/* *******************************************************************************/ +GaussianBayesNetTree GaussianMixture::asGaussianBayesNetTree() const { + auto wrap = [](const GaussianConditional::shared_ptr &gc) { + if (gc) { + return GaussianBayesNet{gc}; + } else { + return GaussianBayesNet(); + } + }; + return {conditionals_, wrap}; +} + /* *******************************************************************************/ size_t GaussianMixture::nrComponents() const { size_t total = 0; @@ -332,10 +361,32 @@ AlgebraicDecisionTree GaussianMixture::error( /* *******************************************************************************/ double GaussianMixture::error(const HybridValues &values) const { + // Check if discrete keys in discrete assignment are + // present in the GaussianMixture + KeyVector dKeys = this->discreteKeys_.indices(); + bool valid_assignment = false; + for (auto &&kv : values.discrete()) { + if (std::find(dKeys.begin(), dKeys.end(), kv.first) != dKeys.end()) { + valid_assignment = true; + break; + } + } + + // The discrete assignment is not valid so we return 0.0 erorr. + if (!valid_assignment) { + return 0.0; + } + // Directly index to get the conditional, no need to build the whole tree. auto conditional = conditionals_(values.discrete()); - return conditional->error(values.continuous()) + // - logConstant_ - conditional->logNormalizationConstant(); + if (conditional) { + return conditional->error(values.continuous()) + // + logConstant_ - conditional->logNormalizationConstant(); + } else { + // If not valid, pointer, it means this conditional was pruned, + // so we return maximum error. + return std::numeric_limits::max(); + } } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 0b68fcfd0..a52cd6c82 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -71,6 +71,12 @@ class GTSAM_EXPORT GaussianMixture */ GaussianFactorGraphTree asGaussianFactorGraphTree() const; + /** + * @brief Convert a DecisionTree of conditionals into + * a DT of Gaussian Bayes nets. + */ + GaussianBayesNetTree asGaussianBayesNetTree() const; + /** * @brief Helper function to get the pruner functor. * @@ -248,6 +254,15 @@ class GTSAM_EXPORT GaussianMixture * @return GaussianFactorGraphTree */ GaussianFactorGraphTree add(const GaussianFactorGraphTree &sum) const; + + /** + * @brief Merge the Gaussian Bayes Nets in `this` and `sum` while + * maintaining the decision tree structure. + * + * @param sum Decision Tree of Gaussian Bayes Nets + * @return GaussianBayesNetTree + */ + GaussianBayesNetTree add(const GaussianBayesNetTree &sum) const; /// @} private: From 6f66d04f1425219f51b284c4b4094a15dc2a5791 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Dec 2023 15:46:31 -0500 Subject: [PATCH 063/208] handle pruning in model selection --- gtsam/hybrid/HybridBayesNet.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 027bd75d4..0352d7962 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -319,6 +319,13 @@ HybridValues HybridBayesNet::optimize() const { const std::pair &gbnAndValue) { // Compute the X* of each assignment VectorValues mu = gbnAndValue.first.optimize(); + + // mu is empty if gbn had nullptrs + if (mu.size() == 0) { + return std::make_pair(gbnAndValue.first, + std::numeric_limits::max()); + } + // Compute the error for X* and the assignment double error = this->error(HybridValues(mu, DiscreteValues(assignment))); @@ -343,18 +350,12 @@ HybridValues HybridBayesNet::optimize() const { AlgebraicDecisionTree model_selection_term = (errorTree + log_norm_constants) * -1; - // std::cout << "model selection term" << std::endl; - // model_selection_term.print("", DefaultKeyFormatter); - double max_log = model_selection_term.max(); AlgebraicDecisionTree model_selection = DecisionTree( model_selection_term, [&max_log](const double &x) { return std::exp(x - max_log); }); model_selection = model_selection.normalize(model_selection.sum()); - // std::cout << "normalized model selection" << std::endl; - // model_selection.print("", DefaultKeyFormatter); - for (auto &&conditional : *this) { if (conditional->isDiscrete()) { discrete_fg.push_back(conditional->asDiscrete()); From 203a84dc0ec32526f9e98af68c10a4191a57e5f9 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Dec 2023 17:12:51 -0500 Subject: [PATCH 064/208] add more gamma functions --- gtsam/nonlinear/internal/Gamma.h | 383 ++++++++++++++++++- gtsam/nonlinear/internal/chiSquaredInverse.h | 69 +--- 2 files changed, 384 insertions(+), 68 deletions(-) diff --git a/gtsam/nonlinear/internal/Gamma.h b/gtsam/nonlinear/internal/Gamma.h index 325fb7eee..edd2b2071 100644 --- a/gtsam/nonlinear/internal/Gamma.h +++ b/gtsam/nonlinear/internal/Gamma.h @@ -29,6 +29,385 @@ namespace gtsam { namespace internal { +template +inline constexpr T log_max_value() { + return log(LIM::max()); +} + +/** + * @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 + + if (a >= boost::math::max_factorial::value && !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); + } + + BOOST_MATH_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 = boost::math::detail::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 { +#ifndef BOOST_NO_EXCEPTIONS + try { +#endif + result = pow(x, a) / boost::math::tgamma(a + 1, pol); +#ifndef BOOST_NO_EXCEPTIONS + } catch (const std::overflow_error&) { + result = 0; + } +#endif + } + 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. @@ -59,8 +438,8 @@ struct gamma_p_inverse_func { 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)); + f = static_cast( + internal::gamma_incomplete_imp(a, x, true, invert, pol, &ft)); f1 = ft; T f2; T div = (a - x - 1) / x; diff --git a/gtsam/nonlinear/internal/chiSquaredInverse.h b/gtsam/nonlinear/internal/chiSquaredInverse.h index 7577721dc..d4f79147a 100644 --- a/gtsam/nonlinear/internal/chiSquaredInverse.h +++ b/gtsam/nonlinear/internal/chiSquaredInverse.h @@ -24,12 +24,12 @@ #pragma once +#include #include #include // TODO(Varun) remove -#include #include namespace gtsam { @@ -300,65 +300,6 @@ T find_inverse_gamma(T a, T p, T q, bool* p_has_10_digits) { 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; - f = static_cast(gamma_incomplete_imp(a, x, true, invert, &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; -}; - template T gamma_p_inv_imp(const T a, const T p) { if (is_nan(a) || is_nan(p)) { @@ -402,13 +343,9 @@ T gamma_p_inv_imp(const T a, const T p) { // 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); 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( From 87c572912e98b9ee95d8fc854bd490b31b8635ed Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 27 Dec 2023 18:03:02 -0500 Subject: [PATCH 065/208] more code --- gtsam/nonlinear/internal/Gamma.h | 78 +++++++++++++++++++++++++++++--- 1 file changed, 71 insertions(+), 7 deletions(-) diff --git a/gtsam/nonlinear/internal/Gamma.h b/gtsam/nonlinear/internal/Gamma.h index edd2b2071..6c63ff7cb 100644 --- a/gtsam/nonlinear/internal/Gamma.h +++ b/gtsam/nonlinear/internal/Gamma.h @@ -24,6 +24,7 @@ #include #include +#include namespace gtsam { @@ -34,6 +35,72 @@ 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 * @@ -98,7 +165,8 @@ T gamma_incomplete_imp(T a, T x, bool normalised, bool invert, T result = 0; // Just to avoid warning C4701: potentially uninitialized local // variable 'result' used - if (a >= boost::math::max_factorial::value && !normalised) { + // 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 @@ -153,7 +221,7 @@ T gamma_incomplete_imp(T a, T x, bool normalised, bool invert, return exp(result); } - BOOST_MATH_ASSERT((p_derivative == nullptr) || normalised); + assert((p_derivative == nullptr) || normalised); bool is_int, is_half_int; bool is_small_a = (a < 30) && (a <= x + 1) && (x < log_max_value()); @@ -247,7 +315,7 @@ T gamma_incomplete_imp(T a, T x, bool normalised, bool invert, switch (eval_method) { case 0: { - result = boost::math::detail::finite_gamma_q(a, x, pol, p_derivative); + result = finite_gamma_q(a, x, pol, p_derivative); if (!normalised) result *= boost::math::tgamma(a, pol); break; } @@ -358,15 +426,11 @@ T gamma_incomplete_imp(T a, T x, bool normalised, bool invert, if (!normalised) result = pow(x, a) / (a); else { -#ifndef BOOST_NO_EXCEPTIONS try { -#endif result = pow(x, a) / boost::math::tgamma(a + 1, pol); -#ifndef BOOST_NO_EXCEPTIONS } catch (const std::overflow_error&) { result = 0; } -#endif } result *= 1 - a * x / (a + 1); if (p_derivative) From 4326195786676cc344a713bb4eec71a37906a8bf Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 07:25:55 -0500 Subject: [PATCH 066/208] cephes source code --- gtsam/3rdparty/cephes/CMakeLists.txt | 102 ++ gtsam/3rdparty/cephes/cephes.h | 163 +++ gtsam/3rdparty/cephes/cephes/airy.c | 376 ++++++ gtsam/3rdparty/cephes/cephes/bdtr.c | 241 ++++ gtsam/3rdparty/cephes/cephes/besselpoly.c | 34 + gtsam/3rdparty/cephes/cephes/beta.c | 258 ++++ gtsam/3rdparty/cephes/cephes/btdtr.c | 59 + gtsam/3rdparty/cephes/cephes/cbrt.c | 117 ++ gtsam/3rdparty/cephes/cephes/cephes_names.h | 114 ++ gtsam/3rdparty/cephes/cephes/chbevl.c | 81 ++ gtsam/3rdparty/cephes/cephes/chdtr.c | 186 +++ gtsam/3rdparty/cephes/cephes/const.c | 129 ++ gtsam/3rdparty/cephes/cephes/dawsn.c | 160 +++ gtsam/3rdparty/cephes/cephes/dd_idefs.h | 198 +++ gtsam/3rdparty/cephes/cephes/dd_real.c | 587 +++++++++ gtsam/3rdparty/cephes/cephes/dd_real.h | 143 +++ gtsam/3rdparty/cephes/cephes/dd_real_idefs.h | 557 +++++++++ gtsam/3rdparty/cephes/cephes/ellie.c | 282 +++++ gtsam/3rdparty/cephes/cephes/ellik.c | 246 ++++ gtsam/3rdparty/cephes/cephes/ellpe.c | 108 ++ gtsam/3rdparty/cephes/cephes/ellpj.c | 154 +++ gtsam/3rdparty/cephes/cephes/ellpk.c | 124 ++ gtsam/3rdparty/cephes/cephes/erfinv.c | 78 ++ gtsam/3rdparty/cephes/cephes/exp10.c | 115 ++ gtsam/3rdparty/cephes/cephes/exp2.c | 108 ++ gtsam/3rdparty/cephes/cephes/expn.c | 224 ++++ gtsam/3rdparty/cephes/cephes/expn.h | 19 + gtsam/3rdparty/cephes/cephes/fdtr.c | 216 ++++ gtsam/3rdparty/cephes/cephes/fresnl.c | 219 ++++ gtsam/3rdparty/cephes/cephes/gamma.c | 364 ++++++ gtsam/3rdparty/cephes/cephes/gammasgn.c | 25 + gtsam/3rdparty/cephes/cephes/gdtr.c | 132 ++ gtsam/3rdparty/cephes/cephes/hyp2f1.c | 569 +++++++++ gtsam/3rdparty/cephes/cephes/hyperg.c | 362 ++++++ gtsam/3rdparty/cephes/cephes/i0.c | 180 +++ gtsam/3rdparty/cephes/cephes/i1.c | 184 +++ gtsam/3rdparty/cephes/cephes/igam.c | 423 +++++++ gtsam/3rdparty/cephes/cephes/igam.h | 38 + gtsam/3rdparty/cephes/cephes/igami.c | 339 ++++++ gtsam/3rdparty/cephes/cephes/incbet.c | 369 ++++++ gtsam/3rdparty/cephes/cephes/incbi.c | 275 +++++ gtsam/3rdparty/cephes/cephes/j0.c | 246 ++++ gtsam/3rdparty/cephes/cephes/j1.c | 225 ++++ gtsam/3rdparty/cephes/cephes/jv.c | 841 +++++++++++++ gtsam/3rdparty/cephes/cephes/k0.c | 178 +++ gtsam/3rdparty/cephes/cephes/k1.c | 179 +++ gtsam/3rdparty/cephes/cephes/kn.c | 235 ++++ gtsam/3rdparty/cephes/cephes/kolmogorov.c | 1147 ++++++++++++++++++ gtsam/3rdparty/cephes/cephes/lanczos.c | 56 + gtsam/3rdparty/cephes/cephes/lanczos.h | 133 ++ gtsam/3rdparty/cephes/cephes/mconf.h | 109 ++ gtsam/3rdparty/cephes/cephes/nbdtr.c | 207 ++++ gtsam/3rdparty/cephes/cephes/ndtr.c | 305 +++++ gtsam/3rdparty/cephes/cephes/ndtri.c | 176 +++ gtsam/3rdparty/cephes/cephes/owens_t.c | 364 ++++++ gtsam/3rdparty/cephes/cephes/pdtr.c | 173 +++ gtsam/3rdparty/cephes/cephes/poch.c | 81 ++ gtsam/3rdparty/cephes/cephes/polevl.h | 165 +++ gtsam/3rdparty/cephes/cephes/psi.c | 205 ++++ gtsam/3rdparty/cephes/cephes/rgamma.c | 128 ++ gtsam/3rdparty/cephes/cephes/round.c | 63 + gtsam/3rdparty/cephes/cephes/scipy_iv.c | 654 ++++++++++ gtsam/3rdparty/cephes/cephes/sf_error.c | 45 + gtsam/3rdparty/cephes/cephes/sf_error.h | 38 + gtsam/3rdparty/cephes/cephes/shichi.c | 305 +++++ gtsam/3rdparty/cephes/cephes/sici.c | 276 +++++ gtsam/3rdparty/cephes/cephes/sindg.c | 219 ++++ gtsam/3rdparty/cephes/cephes/sinpi.c | 54 + gtsam/3rdparty/cephes/cephes/spence.c | 125 ++ gtsam/3rdparty/cephes/cephes/stdtr.c | 203 ++++ gtsam/3rdparty/cephes/cephes/struve.c | 408 +++++++ gtsam/3rdparty/cephes/cephes/tandg.c | 141 +++ gtsam/3rdparty/cephes/cephes/tukey.c | 68 ++ gtsam/3rdparty/cephes/cephes/unity.c | 190 +++ gtsam/3rdparty/cephes/cephes/yn.c | 105 ++ gtsam/3rdparty/cephes/cephes/yv.c | 46 + gtsam/3rdparty/cephes/cephes/zeta.c | 160 +++ gtsam/3rdparty/cephes/cephes/zetac.c | 345 ++++++ 78 files changed, 17256 insertions(+) create mode 100644 gtsam/3rdparty/cephes/CMakeLists.txt create mode 100644 gtsam/3rdparty/cephes/cephes.h create mode 100644 gtsam/3rdparty/cephes/cephes/airy.c create mode 100644 gtsam/3rdparty/cephes/cephes/bdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/besselpoly.c create mode 100644 gtsam/3rdparty/cephes/cephes/beta.c create mode 100644 gtsam/3rdparty/cephes/cephes/btdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/cbrt.c create mode 100644 gtsam/3rdparty/cephes/cephes/cephes_names.h create mode 100644 gtsam/3rdparty/cephes/cephes/chbevl.c create mode 100644 gtsam/3rdparty/cephes/cephes/chdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/const.c create mode 100644 gtsam/3rdparty/cephes/cephes/dawsn.c create mode 100644 gtsam/3rdparty/cephes/cephes/dd_idefs.h create mode 100644 gtsam/3rdparty/cephes/cephes/dd_real.c create mode 100644 gtsam/3rdparty/cephes/cephes/dd_real.h create mode 100644 gtsam/3rdparty/cephes/cephes/dd_real_idefs.h create mode 100644 gtsam/3rdparty/cephes/cephes/ellie.c create mode 100644 gtsam/3rdparty/cephes/cephes/ellik.c create mode 100644 gtsam/3rdparty/cephes/cephes/ellpe.c create mode 100644 gtsam/3rdparty/cephes/cephes/ellpj.c create mode 100644 gtsam/3rdparty/cephes/cephes/ellpk.c create mode 100644 gtsam/3rdparty/cephes/cephes/erfinv.c create mode 100644 gtsam/3rdparty/cephes/cephes/exp10.c create mode 100644 gtsam/3rdparty/cephes/cephes/exp2.c create mode 100644 gtsam/3rdparty/cephes/cephes/expn.c create mode 100644 gtsam/3rdparty/cephes/cephes/expn.h create mode 100644 gtsam/3rdparty/cephes/cephes/fdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/fresnl.c create mode 100644 gtsam/3rdparty/cephes/cephes/gamma.c create mode 100644 gtsam/3rdparty/cephes/cephes/gammasgn.c create mode 100644 gtsam/3rdparty/cephes/cephes/gdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/hyp2f1.c create mode 100644 gtsam/3rdparty/cephes/cephes/hyperg.c create mode 100644 gtsam/3rdparty/cephes/cephes/i0.c create mode 100644 gtsam/3rdparty/cephes/cephes/i1.c create mode 100644 gtsam/3rdparty/cephes/cephes/igam.c create mode 100644 gtsam/3rdparty/cephes/cephes/igam.h create mode 100644 gtsam/3rdparty/cephes/cephes/igami.c create mode 100644 gtsam/3rdparty/cephes/cephes/incbet.c create mode 100644 gtsam/3rdparty/cephes/cephes/incbi.c create mode 100644 gtsam/3rdparty/cephes/cephes/j0.c create mode 100644 gtsam/3rdparty/cephes/cephes/j1.c create mode 100644 gtsam/3rdparty/cephes/cephes/jv.c create mode 100644 gtsam/3rdparty/cephes/cephes/k0.c create mode 100644 gtsam/3rdparty/cephes/cephes/k1.c create mode 100644 gtsam/3rdparty/cephes/cephes/kn.c create mode 100644 gtsam/3rdparty/cephes/cephes/kolmogorov.c create mode 100644 gtsam/3rdparty/cephes/cephes/lanczos.c create mode 100644 gtsam/3rdparty/cephes/cephes/lanczos.h create mode 100644 gtsam/3rdparty/cephes/cephes/mconf.h create mode 100644 gtsam/3rdparty/cephes/cephes/nbdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/ndtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/ndtri.c create mode 100644 gtsam/3rdparty/cephes/cephes/owens_t.c create mode 100644 gtsam/3rdparty/cephes/cephes/pdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/poch.c create mode 100644 gtsam/3rdparty/cephes/cephes/polevl.h create mode 100644 gtsam/3rdparty/cephes/cephes/psi.c create mode 100644 gtsam/3rdparty/cephes/cephes/rgamma.c create mode 100644 gtsam/3rdparty/cephes/cephes/round.c create mode 100644 gtsam/3rdparty/cephes/cephes/scipy_iv.c create mode 100644 gtsam/3rdparty/cephes/cephes/sf_error.c create mode 100644 gtsam/3rdparty/cephes/cephes/sf_error.h create mode 100644 gtsam/3rdparty/cephes/cephes/shichi.c create mode 100644 gtsam/3rdparty/cephes/cephes/sici.c create mode 100644 gtsam/3rdparty/cephes/cephes/sindg.c create mode 100644 gtsam/3rdparty/cephes/cephes/sinpi.c create mode 100644 gtsam/3rdparty/cephes/cephes/spence.c create mode 100644 gtsam/3rdparty/cephes/cephes/stdtr.c create mode 100644 gtsam/3rdparty/cephes/cephes/struve.c create mode 100644 gtsam/3rdparty/cephes/cephes/tandg.c create mode 100644 gtsam/3rdparty/cephes/cephes/tukey.c create mode 100644 gtsam/3rdparty/cephes/cephes/unity.c create mode 100644 gtsam/3rdparty/cephes/cephes/yn.c create mode 100644 gtsam/3rdparty/cephes/cephes/yv.c create mode 100644 gtsam/3rdparty/cephes/cephes/zeta.c create mode 100644 gtsam/3rdparty/cephes/cephes/zetac.c diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt new file mode 100644 index 000000000..fdc17ea61 --- /dev/null +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -0,0 +1,102 @@ +cmake_minimum_required(VERSION 3.12) +enable_testing() +project( + cephes + DESCRIPTION "Cephes Mathematical Function Library" + VERSION 1.0.0 + LANGUAGES C) + +set(CEPHES_HEADER_FILES + cephes.h + cephes/cephes_names.h + cephes/dd_idefs.h + cephes/dd_real.h + cephes/dd_real_idefs.h + cephes/expn.h + cephes/igam.h + cephes/lanczos.h + cephes/mconf.h + cephes/polevl.h + cephes/sf_error.h) + +set(CEPHES_SOURCES + cephes/airy.c + cephes/bdtr.c + cephes/besselpoly.c + cephes/beta.c + cephes/btdtr.c + cephes/cbrt.c + cephes/chbevl.c + cephes/chdtr.c + cephes/const.c + cephes/dawsn.c + cephes/dd_real.c + cephes/ellie.c + cephes/ellik.c + cephes/ellpe.c + cephes/ellpj.c + cephes/ellpk.c + cephes/erfinv.c + cephes/exp10.c + cephes/exp2.c + cephes/expn.c + cephes/fdtr.c + cephes/fresnl.c + cephes/gamma.c + cephes/gammasgn.c + cephes/gdtr.c + cephes/hyp2f1.c + cephes/hyperg.c + cephes/i0.c + cephes/i1.c + cephes/igam.c + cephes/igami.c + cephes/incbet.c + cephes/incbi.c + cephes/j0.c + cephes/j1.c + cephes/jv.c + cephes/k0.c + cephes/k1.c + cephes/kn.c + cephes/kolmogorov.c + cephes/lanczos.c + cephes/nbdtr.c + cephes/ndtr.c + cephes/ndtri.c + cephes/owens_t.c + cephes/pdtr.c + cephes/poch.c + cephes/psi.c + cephes/rgamma.c + cephes/round.c + # cephes/scipy_iv.c + cephes/sf_error.c + cephes/shichi.c + cephes/sici.c + cephes/sindg.c + cephes/sinpi.c + cephes/spence.c + cephes/stdtr.c + cephes/tandg.c + cephes/tukey.c + cephes/unity.c + cephes/yn.c + cephes/yv.c + cephes/zeta.c + cephes/zetac.c) + +# Add library source files +add_library(${PROJECT_NAME} SHARED ${CEPHES_SOURCES}) + +# Add include directory (aka headers) +target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +set_target_properties( + ${PROJECT_NAME} + PROPERTIES VERSION ${PROJECT_VERSION} + SOVERSION ${PROJECT_VERSION_MAJOR} + PUBLIC_HEADER ${CEPHES_HEADER_FILES} + C_STANDARD 99) + +install(FILES ${CEPHES_HEADER_FILES} DESTINATION include/gtsam/3rdparty/cephes) diff --git a/gtsam/3rdparty/cephes/cephes.h b/gtsam/3rdparty/cephes/cephes.h new file mode 100644 index 000000000..629733eef --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes.h @@ -0,0 +1,163 @@ +#ifndef CEPHES_H +#define CEPHES_H + +#include "cephes/cephes_names.h" + +#ifdef __cplusplus +extern "C" { +#endif + +extern int airy(double x, double *ai, double *aip, double *bi, double *bip); + +extern double bdtrc(double k, int n, double p); +extern double bdtr(double k, int n, double p); +extern double bdtri(double k, int n, double y); + +extern double besselpoly(double a, double lambda, double nu); + +extern double beta(double a, double b); +extern double lbeta(double a, double b); + +extern double btdtr(double a, double b, double x); + +extern double cbrt(double x); +extern double chbevl(double x, double array[], int n); +extern double chdtrc(double df, double x); +extern double chdtr(double df, double x); +extern double chdtri(double df, double y); +extern double dawsn(double xx); + +extern double ellie(double phi, double m); +extern double ellik(double phi, double m); +extern double ellpe(double x); + +extern int ellpj(double u, double m, double *sn, double *cn, double *dn, double *ph); +extern double ellpk(double x); +extern double exp10(double x); +extern double exp2(double x); + +extern double expn(int n, double x); + +extern double fdtrc(double a, double b, double x); +extern double fdtr(double a, double b, double x); +extern double fdtri(double a, double b, double y); + +extern int fresnl(double xxa, double *ssa, double *cca); +extern double Gamma(double x); +extern double lgam(double x); +extern double lgam_sgn(double x, int *sign); +extern double gammasgn(double x); + +extern double gdtr(double a, double b, double x); +extern double gdtrc(double a, double b, double x); +extern double gdtri(double a, double b, double y); + +extern double hyp2f1(double a, double b, double c, double x); +extern double hyperg(double a, double b, double x); +extern double threef0(double a, double b, double c, double x, double *err); + +extern double i0(double x); +extern double i0e(double x); +extern double i1(double x); +extern double i1e(double x); +extern double igamc(double a, double x); +extern double igam(double a, double x); +extern double igam_fac(double a, double x); +extern double igamci(double a, double q); +extern double igami(double a, double p); + +extern double incbet(double aa, double bb, double xx); +extern double incbi(double aa, double bb, double yy0); + +extern double iv(double v, double x); +extern double j0(double x); +extern double y0(double x); +extern double j1(double x); +extern double y1(double x); + +extern double jn(int n, double x); +extern double jv(double n, double x); +extern double k0(double x); +extern double k0e(double x); +extern double k1(double x); +extern double k1e(double x); +extern double kn(int nn, double x); + +extern double nbdtrc(int k, int n, double p); +extern double nbdtr(int k, int n, double p); +extern double nbdtri(int k, int n, double p); + +extern double ndtr(double a); +extern double log_ndtr(double a); +extern double erfc(double a); +extern double erf(double x); +extern double erfinv(double y); +extern double erfcinv(double y); +extern double ndtri(double y0); + +extern double pdtrc(double k, double m); +extern double pdtr(double k, double m); +extern double pdtri(int k, double y); + +extern double poch(double x, double m); + +extern double psi(double x); + +extern double rgamma(double x); +extern double round(double x); + +extern int shichi(double x, double *si, double *ci); +extern int sici(double x, double *si, double *ci); + +extern double radian(double d, double m, double s); +extern double sindg(double x); +extern double sinpi(double x); +extern double cosdg(double x); +extern double cospi(double x); + +extern double spence(double x); + +extern double stdtr(int k, double t); +extern double stdtri(int k, double p); + +extern double struve_h(double v, double x); +extern double struve_l(double v, double x); +extern double struve_power_series(double v, double x, int is_h, double *err); +extern double struve_asymp_large_z(double v, double z, int is_h, double *err); +extern double struve_bessel_series(double v, double z, int is_h, double *err); + +extern double yv(double v, double x); + +extern double tandg(double x); +extern double cotdg(double x); + +extern double log1p(double x); +extern double log1pmx(double x); +extern double expm1(double x); +extern double cosm1(double x); +extern double lgam1p(double x); + +extern double yn(int n, double x); +extern double zeta(double x, double q); +extern double zetac(double x); + +extern double smirnov(int n, double d); +extern double smirnovi(int n, double p); +extern double smirnovp(int n, double d); +extern double smirnovc(int n, double d); +extern double smirnovci(int n, double p); +extern double kolmogorov(double x); +extern double kolmogi(double p); +extern double kolmogp(double x); +extern double kolmogc(double x); +extern double kolmogci(double p); + +extern double lanczos_sum_expg_scaled(double x); + +extern double owens_t(double h, double a); + +#ifdef __cplusplus +} +#endif + +#endif /* CEPHES_H */ diff --git a/gtsam/3rdparty/cephes/cephes/airy.c b/gtsam/3rdparty/cephes/cephes/airy.c new file mode 100644 index 000000000..95e16a55f --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/airy.c @@ -0,0 +1,376 @@ +/* airy.c + * + * Airy function + * + * + * + * SYNOPSIS: + * + * double x, ai, aip, bi, bip; + * int airy(); + * + * airy( x, _&ai, _&aip, _&bi, _&bip ); + * + * + * + * DESCRIPTION: + * + * Solution of the differential equation + * + * y"(x) = xy. + * + * The function returns the two independent solutions Ai, Bi + * and their first derivatives Ai'(x), Bi'(x). + * + * Evaluation is by power series summation for small x, + * by rational minimax approximations for large x. + * + * + * + * ACCURACY: + * Error criterion is absolute when function <= 1, relative + * when function > 1, except * denotes relative error criterion. + * For large negative x, the absolute error increases as x^1.5. + * For large positive x, the relative error increases as x^1.5. + * + * Arithmetic domain function # trials peak rms + * IEEE -10, 0 Ai 10000 1.6e-15 2.7e-16 + * IEEE 0, 10 Ai 10000 2.3e-14* 1.8e-15* + * IEEE -10, 0 Ai' 10000 4.6e-15 7.6e-16 + * IEEE 0, 10 Ai' 10000 1.8e-14* 1.5e-15* + * IEEE -10, 10 Bi 30000 4.2e-15 5.3e-16 + * IEEE -10, 10 Bi' 30000 4.9e-15 7.3e-16 + * + */ + /* airy.c */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +static double c1 = 0.35502805388781723926; +static double c2 = 0.258819403792806798405; +static double sqrt3 = 1.732050807568877293527; +static double sqpii = 5.64189583547756286948E-1; + +extern double MACHEP; + +#ifdef UNK +#define MAXAIRY 25.77 +#endif +#ifdef IBMPC +#define MAXAIRY 103.892 +#endif +#ifdef MIEEE +#define MAXAIRY 103.892 +#endif + + +static double AN[8] = { + 3.46538101525629032477E-1, + 1.20075952739645805542E1, + 7.62796053615234516538E1, + 1.68089224934630576269E2, + 1.59756391350164413639E2, + 7.05360906840444183113E1, + 1.40264691163389668864E1, + 9.99999999999999995305E-1, +}; + +static double AD[8] = { + 5.67594532638770212846E-1, + 1.47562562584847203173E1, + 8.45138970141474626562E1, + 1.77318088145400459522E2, + 1.64234692871529701831E2, + 7.14778400825575695274E1, + 1.40959135607834029598E1, + 1.00000000000000000470E0, +}; + +static double APN[8] = { + 6.13759184814035759225E-1, + 1.47454670787755323881E1, + 8.20584123476060982430E1, + 1.71184781360976385540E2, + 1.59317847137141783523E2, + 6.99778599330103016170E1, + 1.39470856980481566958E1, + 1.00000000000000000550E0, +}; + +static double APD[8] = { + 3.34203677749736953049E-1, + 1.11810297306158156705E1, + 7.11727352147859965283E1, + 1.58778084372838313640E2, + 1.53206427475809220834E2, + 6.86752304592780337944E1, + 1.38498634758259442477E1, + 9.99999999999999994502E-1, +}; + +static double BN16[5] = { + -2.53240795869364152689E-1, + 5.75285167332467384228E-1, + -3.29907036873225371650E-1, + 6.44404068948199951727E-2, + -3.82519546641336734394E-3, +}; + +static double BD16[5] = { + /* 1.00000000000000000000E0, */ + -7.15685095054035237902E0, + 1.06039580715664694291E1, + -5.23246636471251500874E0, + 9.57395864378383833152E-1, + -5.50828147163549611107E-2, +}; + +static double BPPN[5] = { + 4.65461162774651610328E-1, + -1.08992173800493920734E0, + 6.38800117371827987759E-1, + -1.26844349553102907034E-1, + 7.62487844342109852105E-3, +}; + +static double BPPD[5] = { + /* 1.00000000000000000000E0, */ + -8.70622787633159124240E0, + 1.38993162704553213172E1, + -7.14116144616431159572E0, + 1.34008595960680518666E0, + -7.84273211323341930448E-2, +}; + +static double AFN[9] = { + -1.31696323418331795333E-1, + -6.26456544431912369773E-1, + -6.93158036036933542233E-1, + -2.79779981545119124951E-1, + -4.91900132609500318020E-2, + -4.06265923594885404393E-3, + -1.59276496239262096340E-4, + -2.77649108155232920844E-6, + -1.67787698489114633780E-8, +}; + +static double AFD[9] = { + /* 1.00000000000000000000E0, */ + 1.33560420706553243746E1, + 3.26825032795224613948E1, + 2.67367040941499554804E1, + 9.18707402907259625840E0, + 1.47529146771666414581E0, + 1.15687173795188044134E-1, + 4.40291641615211203805E-3, + 7.54720348287414296618E-5, + 4.51850092970580378464E-7, +}; + +static double AGN[11] = { + 1.97339932091685679179E-2, + 3.91103029615688277255E-1, + 1.06579897599595591108E0, + 9.39169229816650230044E-1, + 3.51465656105547619242E-1, + 6.33888919628925490927E-2, + 5.85804113048388458567E-3, + 2.82851600836737019778E-4, + 6.98793669997260967291E-6, + 8.11789239554389293311E-8, + 3.41551784765923618484E-10, +}; + +static double AGD[10] = { + /* 1.00000000000000000000E0, */ + 9.30892908077441974853E0, + 1.98352928718312140417E1, + 1.55646628932864612953E1, + 5.47686069422975497931E0, + 9.54293611618961883998E-1, + 8.64580826352392193095E-2, + 4.12656523824222607191E-3, + 1.01259085116509135510E-4, + 1.17166733214413521882E-6, + 4.91834570062930015649E-9, +}; + +static double APFN[9] = { + 1.85365624022535566142E-1, + 8.86712188052584095637E-1, + 9.87391981747398547272E-1, + 4.01241082318003734092E-1, + 7.10304926289631174579E-2, + 5.90618657995661810071E-3, + 2.33051409401776799569E-4, + 4.08718778289035454598E-6, + 2.48379932900442457853E-8, +}; + +static double APFD[9] = { + /* 1.00000000000000000000E0, */ + 1.47345854687502542552E1, + 3.75423933435489594466E1, + 3.14657751203046424330E1, + 1.09969125207298778536E1, + 1.78885054766999417817E0, + 1.41733275753662636873E-1, + 5.44066067017226003627E-3, + 9.39421290654511171663E-5, + 5.65978713036027009243E-7, +}; + +static double APGN[11] = { + -3.55615429033082288335E-2, + -6.37311518129435504426E-1, + -1.70856738884312371053E0, + -1.50221872117316635393E0, + -5.63606665822102676611E-1, + -1.02101031120216891789E-1, + -9.48396695961445269093E-3, + -4.60325307486780994357E-4, + -1.14300836484517375919E-5, + -1.33415518685547420648E-7, + -5.63803833958893494476E-10, +}; + +static double APGD[11] = { + /* 1.00000000000000000000E0, */ + 9.85865801696130355144E0, + 2.16401867356585941885E1, + 1.73130776389749389525E1, + 6.17872175280828766327E0, + 1.08848694396321495475E0, + 9.95005543440888479402E-2, + 4.78468199683886610842E-3, + 1.18159633322838625562E-4, + 1.37480673554219441465E-6, + 5.79912514929147598821E-9, +}; + +int airy(double x, double *ai, double *aip, double *bi, double *bip) +{ + double z, zz, t, f, g, uf, ug, k, zeta, theta; + int domflg; + + domflg = 0; + if (x > MAXAIRY) { + *ai = 0; + *aip = 0; + *bi = INFINITY; + *bip = INFINITY; + return (-1); + } + + if (x < -2.09) { + domflg = 15; + t = sqrt(-x); + zeta = -2.0 * x * t / 3.0; + t = sqrt(t); + k = sqpii / t; + z = 1.0 / zeta; + zz = z * z; + uf = 1.0 + zz * polevl(zz, AFN, 8) / p1evl(zz, AFD, 9); + ug = z * polevl(zz, AGN, 10) / p1evl(zz, AGD, 10); + theta = zeta + 0.25 * M_PI; + f = sin(theta); + g = cos(theta); + *ai = k * (f * uf - g * ug); + *bi = k * (g * uf + f * ug); + uf = 1.0 + zz * polevl(zz, APFN, 8) / p1evl(zz, APFD, 9); + ug = z * polevl(zz, APGN, 10) / p1evl(zz, APGD, 10); + k = sqpii * t; + *aip = -k * (g * uf + f * ug); + *bip = k * (f * uf - g * ug); + return (0); + } + + if (x >= 2.09) { /* cbrt(9) */ + domflg = 5; + t = sqrt(x); + zeta = 2.0 * x * t / 3.0; + g = exp(zeta); + t = sqrt(t); + k = 2.0 * t * g; + z = 1.0 / zeta; + f = polevl(z, AN, 7) / polevl(z, AD, 7); + *ai = sqpii * f / k; + k = -0.5 * sqpii * t / g; + f = polevl(z, APN, 7) / polevl(z, APD, 7); + *aip = f * k; + + if (x > 8.3203353) { /* zeta > 16 */ + f = z * polevl(z, BN16, 4) / p1evl(z, BD16, 5); + k = sqpii * g; + *bi = k * (1.0 + f) / t; + f = z * polevl(z, BPPN, 4) / p1evl(z, BPPD, 5); + *bip = k * t * (1.0 + f); + return (0); + } + } + + f = 1.0; + g = x; + t = 1.0; + uf = 1.0; + ug = x; + k = 1.0; + z = x * x * x; + while (t > MACHEP) { + uf *= z; + k += 1.0; + uf /= k; + ug *= z; + k += 1.0; + ug /= k; + uf /= k; + f += uf; + k += 1.0; + ug /= k; + g += ug; + t = fabs(uf / f); + } + uf = c1 * f; + ug = c2 * g; + if ((domflg & 1) == 0) + *ai = uf - ug; + if ((domflg & 2) == 0) + *bi = sqrt3 * (uf + ug); + + /* the deriviative of ai */ + k = 4.0; + uf = x * x / 2.0; + ug = z / 3.0; + f = uf; + g = 1.0 + ug; + uf /= 3.0; + t = 1.0; + + while (t > MACHEP) { + uf *= z; + ug /= k; + k += 1.0; + ug *= z; + uf /= k; + f += uf; + k += 1.0; + ug /= k; + uf /= k; + g += ug; + k += 1.0; + t = fabs(ug / g); + } + + uf = c1 * f; + ug = c2 * g; + if ((domflg & 4) == 0) + *aip = uf - ug; + if ((domflg & 8) == 0) + *bip = sqrt3 * (uf + ug); + return (0); +} diff --git a/gtsam/3rdparty/cephes/cephes/bdtr.c b/gtsam/3rdparty/cephes/cephes/bdtr.c new file mode 100644 index 000000000..29fcdf1af --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/bdtr.c @@ -0,0 +1,241 @@ +/* bdtr.c + * + * Binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, bdtr(); + * + * y = bdtr( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms 0 through k of the Binomial + * probability density: + * + * k + * -- ( n ) j n-j + * > ( ) p (1-p) + * -- ( j ) + * j=0 + * + * The terms are not summed directly; instead the incomplete + * beta integral is employed, according to the formula + * + * y = bdtr( k, n, p ) = incbet( n-k, k+1, 1-p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * Tested at random points (a,b,p), with p between 0 and 1. + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * For p between 0.001 and 1: + * IEEE 0,100 100000 4.3e-15 2.6e-16 + * See also incbet.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * bdtr domain k < 0 0.0 + * n < k + * x < 0, x > 1 + */ +/* bdtrc() + * + * Complemented binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, bdtrc(); + * + * y = bdtrc( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms k+1 through n of the Binomial + * probability density: + * + * n + * -- ( n ) j n-j + * > ( ) p (1-p) + * -- ( j ) + * j=k+1 + * + * The terms are not summed directly; instead the incomplete + * beta integral is employed, according to the formula + * + * y = bdtrc( k, n, p ) = incbet( k+1, n-k, p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * Tested at random points (a,b,p). + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * For p between 0.001 and 1: + * IEEE 0,100 100000 6.7e-15 8.2e-16 + * For p between 0 and .001: + * IEEE 0,100 100000 1.5e-13 2.7e-15 + * + * ERROR MESSAGES: + * + * message condition value returned + * bdtrc domain x<0, x>1, n 1 + */ + +/* bdtr() */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +double bdtrc(double k, int n, double p) { + double dk, dn; + double fk = floor(k); + + if (isnan(p) || isnan(k)) { + return NAN; + } + + if (p < 0.0 || p > 1.0 || n < fk) { + sf_error("bdtrc", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (fk < 0) { + return 1.0; + } + + if (fk == n) { + return 0.0; + } + + dn = n - fk; + if (k == 0) { + if (p < .01) + dk = -expm1(dn * log1p(-p)); + else + dk = 1.0 - pow(1.0 - p, dn); + } else { + dk = fk + 1; + dk = incbet(dk, dn, p); + } + return dk; +} + +double bdtr(double k, int n, double p) { + double dk, dn; + double fk = floor(k); + + if (isnan(p) || isnan(k)) { + return NAN; + } + + if (p < 0.0 || p > 1.0 || fk < 0 || n < fk) { + sf_error("bdtr", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (fk == n) return 1.0; + + dn = n - fk; + if (fk == 0) { + dk = pow(1.0 - p, dn); + } else { + dk = fk + 1.; + dk = incbet(dn, dk, 1.0 - p); + } + return dk; +} + +double bdtri(double k, int n, double y) { + double p, dn, dk; + double fk = floor(k); + + if (isnan(k)) { + return NAN; + } + + if (y < 0.0 || y > 1.0 || fk < 0.0 || n <= fk) { + sf_error("bdtri", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + dn = n - fk; + + if (fk == n) return 1.0; + + if (fk == 0) { + if (y > 0.8) { + p = -expm1(log1p(y - 1.0) / dn); + } else { + p = 1.0 - pow(y, 1.0 / dn); + } + } else { + dk = fk + 1; + p = incbet(dn, dk, 0.5); + if (p > 0.5) + p = incbi(dk, dn, 1.0 - y); + else + p = 1.0 - incbi(dn, dk, y); + } + return p; +} diff --git a/gtsam/3rdparty/cephes/cephes/besselpoly.c b/gtsam/3rdparty/cephes/cephes/besselpoly.c new file mode 100644 index 000000000..a58fe2037 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/besselpoly.c @@ -0,0 +1,34 @@ +#include "mconf.h" + +#define EPS 1.0e-17 + +double besselpoly(double a, double lambda, double nu) { + + int m, factor=0; + double Sm, relerr, Sol; + double sum=0.0; + + /* Special handling for a = 0.0 */ + if (a == 0.0) { + if (nu == 0.0) return 1.0/(lambda + 1); + else return 0.0; + } + /* Special handling for negative and integer nu */ + if ((nu < 0) && (floor(nu)==nu)) { + nu = -nu; + factor = ((int) nu) % 2; + } + Sm = exp(nu*log(a))/(Gamma(nu+1)*(lambda+nu+1)); + m = 0; + do { + sum += Sm; + Sol = Sm; + Sm *= -a*a*(lambda+nu+1+2*m)/((nu+m+1)*(m+1)*(lambda+nu+1+2*m+2)); + m++; + relerr = fabs((Sm-Sol)/Sm); + } while (relerr > EPS && m < 1000); + if (!factor) + return sum; + else + return -sum; +} diff --git a/gtsam/3rdparty/cephes/cephes/beta.c b/gtsam/3rdparty/cephes/cephes/beta.c new file mode 100644 index 000000000..c0389deea --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/beta.c @@ -0,0 +1,258 @@ +/* beta.c + * + * Beta function + * + * + * + * SYNOPSIS: + * + * double a, b, y, beta(); + * + * y = beta( a, b ); + * + * + * + * DESCRIPTION: + * + * - - + * | (a) | (b) + * beta( a, b ) = -----------. + * - + * | (a+b) + * + * For large arguments the logarithm of the function is + * evaluated using lgam(), then exponentiated. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 8.1e-14 1.1e-14 + * + * ERROR MESSAGES: + * + * message condition value returned + * beta overflow log(beta) > MAXLOG 0.0 + * a or b <0 integer 0.0 + * + */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +#define MAXGAM 171.624376956302725 + +extern double MAXLOG; + +#define ASYMP_FACTOR 1e6 + +static double lbeta_asymp(double a, double b, int *sgn); +static double lbeta_negint(int a, double b); +static double beta_negint(int a, double b); + +double beta(double a, double b) +{ + double y; + int sign = 1; + + if (a <= 0.0) { + if (a == floor(a)) { + if (a == (int)a) { + return beta_negint((int)a, b); + } + else { + goto overflow; + } + } + } + + if (b <= 0.0) { + if (b == floor(b)) { + if (b == (int)b) { + return beta_negint((int)b, a); + } + else { + goto overflow; + } + } + } + + if (fabs(a) < fabs(b)) { + y = a; a = b; b = y; + } + + if (fabs(a) > ASYMP_FACTOR * fabs(b) && a > ASYMP_FACTOR) { + /* Avoid loss of precision in lgam(a + b) - lgam(a) */ + y = lbeta_asymp(a, b, &sign); + return sign * exp(y); + } + + y = a + b; + if (fabs(y) > MAXGAM || fabs(a) > MAXGAM || fabs(b) > MAXGAM) { + int sgngam; + y = lgam_sgn(y, &sgngam); + sign *= sgngam; /* keep track of the sign */ + y = lgam_sgn(b, &sgngam) - y; + sign *= sgngam; + y = lgam_sgn(a, &sgngam) + y; + sign *= sgngam; + if (y > MAXLOG) { + goto overflow; + } + return (sign * exp(y)); + } + + y = Gamma(y); + a = Gamma(a); + b = Gamma(b); + if (y == 0.0) + goto overflow; + + if (fabs(fabs(a) - fabs(y)) > fabs(fabs(b) - fabs(y))) { + y = b / y; + y *= a; + } + else { + y = a / y; + y *= b; + } + + return (y); + +overflow: + sf_error("beta", SF_ERROR_OVERFLOW, NULL); + return (sign * INFINITY); +} + + +/* Natural log of |beta|. */ + +double lbeta(double a, double b) +{ + double y; + int sign; + + sign = 1; + + if (a <= 0.0) { + if (a == floor(a)) { + if (a == (int)a) { + return lbeta_negint((int)a, b); + } + else { + goto over; + } + } + } + + if (b <= 0.0) { + if (b == floor(b)) { + if (b == (int)b) { + return lbeta_negint((int)b, a); + } + else { + goto over; + } + } + } + + if (fabs(a) < fabs(b)) { + y = a; a = b; b = y; + } + + if (fabs(a) > ASYMP_FACTOR * fabs(b) && a > ASYMP_FACTOR) { + /* Avoid loss of precision in lgam(a + b) - lgam(a) */ + y = lbeta_asymp(a, b, &sign); + return y; + } + + y = a + b; + if (fabs(y) > MAXGAM || fabs(a) > MAXGAM || fabs(b) > MAXGAM) { + int sgngam; + y = lgam_sgn(y, &sgngam); + sign *= sgngam; /* keep track of the sign */ + y = lgam_sgn(b, &sgngam) - y; + sign *= sgngam; + y = lgam_sgn(a, &sgngam) + y; + sign *= sgngam; + return (y); + } + + y = Gamma(y); + a = Gamma(a); + b = Gamma(b); + if (y == 0.0) { + over: + sf_error("lbeta", SF_ERROR_OVERFLOW, NULL); + return (sign * INFINITY); + } + + if (fabs(fabs(a) - fabs(y)) > fabs(fabs(b) - fabs(y))) { + y = b / y; + y *= a; + } + else { + y = a / y; + y *= b; + } + + if (y < 0) { + y = -y; + } + + return (log(y)); +} + +/* + * Asymptotic expansion for ln(|B(a, b)|) for a > ASYMP_FACTOR*max(|b|, 1). + */ +static double lbeta_asymp(double a, double b, int *sgn) +{ + double r = lgam_sgn(b, sgn); + r -= b * log(a); + + r += b*(1-b)/(2*a); + r += b*(1-b)*(1-2*b)/(12*a*a); + r += - b*b*(1-b)*(1-b)/(12*a*a*a); + + return r; +} + + +/* + * Special case for a negative integer argument + */ + +static double beta_negint(int a, double b) +{ + int sgn; + if (b == (int)b && 1 - a - b > 0) { + sgn = ((int)b % 2 == 0) ? 1 : -1; + return sgn * beta(1 - a - b, b); + } + else { + sf_error("lbeta", SF_ERROR_OVERFLOW, NULL); + return INFINITY; + } +} + +static double lbeta_negint(int a, double b) +{ + double r; + if (b == (int)b && 1 - a - b > 0) { + r = lbeta(1 - a - b, b); + return r; + } + else { + sf_error("lbeta", SF_ERROR_OVERFLOW, NULL); + return INFINITY; + } +} diff --git a/gtsam/3rdparty/cephes/cephes/btdtr.c b/gtsam/3rdparty/cephes/cephes/btdtr.c new file mode 100644 index 000000000..fa115c7b7 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/btdtr.c @@ -0,0 +1,59 @@ + +/* btdtr.c + * + * Beta distribution + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, btdtr(); + * + * y = btdtr( a, b, x ); + * + * + * + * DESCRIPTION: + * + * Returns the area from zero to x under the beta density + * function: + * + * + * x + * - - + * | (a+b) | | a-1 b-1 + * P(x) = ---------- | t (1-t) dt + * - - | | + * | (a) | (b) - + * 0 + * + * + * This function is identical to the incomplete beta + * integral function incbet(a, b, x). + * + * The complemented function is + * + * 1 - P(1-x) = incbet( b, a, x ); + * + * + * ACCURACY: + * + * See incbet.c. + * + */ + +/* btdtr() */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +double btdtr(double a, double b, double x) +{ + + return (incbet(a, b, x)); +} diff --git a/gtsam/3rdparty/cephes/cephes/cbrt.c b/gtsam/3rdparty/cephes/cephes/cbrt.c new file mode 100644 index 000000000..a83c07834 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/cbrt.c @@ -0,0 +1,117 @@ +/* cbrt.c + * + * Cube root + * + * + * + * SYNOPSIS: + * + * double x, y, cbrt(); + * + * y = cbrt( x ); + * + * + * + * DESCRIPTION: + * + * Returns the cube root of the argument, which may be negative. + * + * Range reduction involves determining the power of 2 of + * the argument. A polynomial of degree 2 applied to the + * mantissa, and multiplication by the cube root of 1, 2, or 4 + * approximates the root to within about 0.1%. Then Newton's + * iteration is used three times to converge to an accurate + * result. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,1e308 30000 1.5e-16 5.0e-17 + * + */ + /* cbrt.c */ + +/* + * Cephes Math Library Release 2.2: January, 1991 + * Copyright 1984, 1991 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + + +#include "mconf.h" + +static double CBRT2 = 1.2599210498948731647672; +static double CBRT4 = 1.5874010519681994747517; +static double CBRT2I = 0.79370052598409973737585; +static double CBRT4I = 0.62996052494743658238361; + +double cbrt(double x) +{ + int e, rem, sign; + double z; + + if (!cephes_isfinite(x)) + return x; + if (x == 0) + return (x); + if (x > 0) + sign = 1; + else { + sign = -1; + x = -x; + } + + z = x; + /* extract power of 2, leaving + * mantissa between 0.5 and 1 + */ + x = frexp(x, &e); + + /* Approximate cube root of number between .5 and 1, + * peak relative error = 9.2e-6 + */ + x = (((-1.3466110473359520655053e-1 * x + + 5.4664601366395524503440e-1) * x + - 9.5438224771509446525043e-1) * x + + 1.1399983354717293273738e0) * x + 4.0238979564544752126924e-1; + + /* exponent divided by 3 */ + if (e >= 0) { + rem = e; + e /= 3; + rem -= 3 * e; + if (rem == 1) + x *= CBRT2; + else if (rem == 2) + x *= CBRT4; + } + + + /* argument less than 1 */ + + else { + e = -e; + rem = e; + e /= 3; + rem -= 3 * e; + if (rem == 1) + x *= CBRT2I; + else if (rem == 2) + x *= CBRT4I; + e = -e; + } + + /* multiply by power of 2 */ + x = ldexp(x, e); + + /* Newton iteration */ + x -= (x - (z / (x * x))) * 0.33333333333333333333; + x -= (x - (z / (x * x))) * 0.33333333333333333333; + + if (sign < 0) + x = -x; + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/cephes_names.h b/gtsam/3rdparty/cephes/cephes/cephes_names.h new file mode 100644 index 000000000..5322feb38 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/cephes_names.h @@ -0,0 +1,114 @@ +#ifndef CEPHES_NAMES_H +#define CEPHES_NAMES_H + +#define airy cephes_airy +#define bdtrc cephes_bdtrc +#define bdtr cephes_bdtr +#define bdtri cephes_bdtri +#define besselpoly cephes_besselpoly +#define beta cephes_beta +#define lbeta cephes_lbeta +#define btdtr cephes_btdtr +#define cbrt cephes_cbrt +#define chdtrc cephes_chdtrc +#define chbevl cephes_chbevl +#define chdtr cephes_chdtr +#define chdtri cephes_chdtri +#define dawsn cephes_dawsn +#define ellie cephes_ellie +#define ellik cephes_ellik +#define ellpe cephes_ellpe +#define ellpj cephes_ellpj +#define ellpk cephes_ellpk +#define exp10 cephes_exp10 +#define exp2 cephes_exp2 +#define expn cephes_expn +#define fdtrc cephes_fdtrc +#define fdtr cephes_fdtr +#define fdtri cephes_fdtri +#define fresnl cephes_fresnl +#define Gamma cephes_Gamma +#define lgam cephes_lgam +#define lgam_sgn cephes_lgam_sgn +#define gammasgn cephes_gammasgn +#define gdtr cephes_gdtr +#define gdtrc cephes_gdtrc +#define gdtri cephes_gdtri +#define hyp2f1 cephes_hyp2f1 +#define hyperg cephes_hyperg +#define i0 cephes_i0 +#define i0e cephes_i0e +#define i1 cephes_i1 +#define i1e cephes_i1e +#define igamc cephes_igamc +#define igam cephes_igam +#define igami cephes_igami +#define incbet cephes_incbet +#define incbi cephes_incbi +#define iv cephes_iv +#define j0 cephes_j0 +#define y0 cephes_y0 +#define j1 cephes_j1 +#define y1 cephes_y1 +#define jn cephes_jn +#define jv cephes_jv +#define k0 cephes_k0 +#define k0e cephes_k0e +#define k1 cephes_k1 +#define k1e cephes_k1e +#define kn cephes_kn +#define nbdtrc cephes_nbdtrc +#define nbdtr cephes_nbdtr +#define nbdtri cephes_nbdtri +#define ndtr cephes_ndtr +#define erfc cephes_erfc +#define erf cephes_erf +#define erfinv cephes_erfinv +#define erfcinv cephes_erfcinv +#define ndtri cephes_ndtri +#define pdtrc cephes_pdtrc +#define pdtr cephes_pdtr +#define pdtri cephes_pdtri +#define poch cephes_poch +#define psi cephes_psi +#define rgamma cephes_rgamma +#define riemann_zeta cephes_riemann_zeta +// #define round cephes_round +#define shichi cephes_shichi +#define sici cephes_sici +#define radian cephes_radian +#define sindg cephes_sindg +#define sinpi cephes_sinpi +#define cosdg cephes_cosdg +#define cospi cephes_cospi +#define sincos cephes_sincos +#define spence cephes_spence +#define stdtr cephes_stdtr +#define stdtri cephes_stdtri +#define struve_h cephes_struve_h +#define struve_l cephes_struve_l +#define struve_power_series cephes_struve_power_series +#define struve_asymp_large_z cephes_struve_asymp_large_z +#define struve_bessel_series cephes_struve_bessel_series +#define yv cephes_yv +#define tandg cephes_tandg +#define cotdg cephes_cotdg +#define log1p cephes_log1p +#define expm1 cephes_expm1 +#define cosm1 cephes_cosm1 +#define yn cephes_yn +#define zeta cephes_zeta +#define zetac cephes_zetac +#define smirnov cephes_smirnov +#define smirnovc cephes_smirnovc +#define smirnovi cephes_smirnovi +#define smirnovci cephes_smirnovci +#define smirnovp cephes_smirnovp +#define kolmogorov cephes_kolmogorov +#define kolmogi cephes_kolmogi +#define kolmogp cephes_kolmogp +#define kolmogc cephes_kolmogc +#define kolmogci cephes_kolmogci +#define owens_t cephes_owens_t + +#endif diff --git a/gtsam/3rdparty/cephes/cephes/chbevl.c b/gtsam/3rdparty/cephes/cephes/chbevl.c new file mode 100644 index 000000000..a0e9c5c52 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/chbevl.c @@ -0,0 +1,81 @@ +/* chbevl.c + * + * Evaluate Chebyshev series + * + * + * + * SYNOPSIS: + * + * int N; + * double x, y, coef[N], chebevl(); + * + * y = chbevl( x, coef, N ); + * + * + * + * DESCRIPTION: + * + * Evaluates the series + * + * N-1 + * - ' + * y = > coef[i] T (x/2) + * - i + * i=0 + * + * of Chebyshev polynomials Ti at argument x/2. + * + * Coefficients are stored in reverse order, i.e. the zero + * order term is last in the array. Note N is the number of + * coefficients, not the order. + * + * If coefficients are for the interval a to b, x must + * have been transformed to x -> 2(2x - b - a)/(b-a) before + * entering the routine. This maps x from (a, b) to (-1, 1), + * over which the Chebyshev polynomials are defined. + * + * If the coefficients are for the inverted interval, in + * which (a, b) is mapped to (1/b, 1/a), the transformation + * required is x -> 2(2ab/x - b - a)/(b-a). If b is infinity, + * this becomes x -> 4a/x - 1. + * + * + * + * SPEED: + * + * Taking advantage of the recurrence properties of the + * Chebyshev polynomials, the routine requires one more + * addition per loop than evaluating a nested polynomial of + * the same degree. + * + */ + /* chbevl.c */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1985, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" +#include + +double chbevl(double x, double array[], int n) +{ + double b0, b1, b2, *p; + int i; + + p = array; + b0 = *p++; + b1 = 0.0; + i = n - 1; + + do { + b2 = b1; + b1 = b0; + b0 = x * b1 - b2 + *p++; + } + while (--i); + + return (0.5 * (b0 - b2)); +} diff --git a/gtsam/3rdparty/cephes/cephes/chdtr.c b/gtsam/3rdparty/cephes/cephes/chdtr.c new file mode 100644 index 000000000..d576e7a8d --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/chdtr.c @@ -0,0 +1,186 @@ +/* chdtr.c + * + * Chi-square distribution + * + * + * + * SYNOPSIS: + * + * double df, x, y, chdtr(); + * + * y = chdtr( df, x ); + * + * + * + * DESCRIPTION: + * + * Returns the area under the left hand tail (from 0 to x) + * of the Chi square probability density function with + * v degrees of freedom. + * + * + * inf. + * - + * 1 | | v/2-1 -t/2 + * P( x | v ) = ----------- | t e dt + * v/2 - | | + * 2 | (v/2) - + * x + * + * where x is the Chi-square variable. + * + * The incomplete Gamma integral is used, according to the + * formula + * + * y = chdtr( v, x ) = igam( v/2.0, x/2.0 ). + * + * + * The arguments must both be positive. + * + * + * + * ACCURACY: + * + * See igam(). + * + * ERROR MESSAGES: + * + * message condition value returned + * chdtr domain x < 0 or v < 1 0.0 + */ + /* chdtrc() + * + * Complemented Chi-square distribution + * + * + * + * SYNOPSIS: + * + * double v, x, y, chdtrc(); + * + * y = chdtrc( v, x ); + * + * + * + * DESCRIPTION: + * + * Returns the area under the right hand tail (from x to + * infinity) of the Chi square probability density function + * with v degrees of freedom: + * + * + * inf. + * - + * 1 | | v/2-1 -t/2 + * P( x | v ) = ----------- | t e dt + * v/2 - | | + * 2 | (v/2) - + * x + * + * where x is the Chi-square variable. + * + * The incomplete Gamma integral is used, according to the + * formula + * + * y = chdtr( v, x ) = igamc( v/2.0, x/2.0 ). + * + * + * The arguments must both be positive. + * + * + * + * ACCURACY: + * + * See igamc(). + * + * ERROR MESSAGES: + * + * message condition value returned + * chdtrc domain x < 0 or v < 1 0.0 + */ + /* chdtri() + * + * Inverse of complemented Chi-square distribution + * + * + * + * SYNOPSIS: + * + * double df, x, y, chdtri(); + * + * x = chdtri( df, y ); + * + * + * + * + * DESCRIPTION: + * + * Finds the Chi-square argument x such that the integral + * from x to infinity of the Chi-square density is equal + * to the given cumulative probability y. + * + * This is accomplished using the inverse Gamma integral + * function and the relation + * + * x/2 = igamci( df/2, y ); + * + * + * + * + * ACCURACY: + * + * See igami.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * chdtri domain y < 0 or y > 1 0.0 + * v < 1 + * + */ + +/* chdtr() */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +double chdtrc(double df, double x) +{ + + if (x < 0.0) + return 1.0; /* modified by T. Oliphant */ + return (igamc(df / 2.0, x / 2.0)); +} + + + +double chdtr(double df, double x) +{ + + if ((x < 0.0)) { /* || (df < 1.0) ) */ + sf_error("chdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + return (igam(df / 2.0, x / 2.0)); +} + + + +double chdtri(double df, double y) +{ + double x; + + if ((y < 0.0) || (y > 1.0)) { /* || (df < 1.0) ) */ + sf_error("chdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + x = igamci(0.5 * df, y); + return (2.0 * x); +} diff --git a/gtsam/3rdparty/cephes/cephes/const.c b/gtsam/3rdparty/cephes/cephes/const.c new file mode 100644 index 000000000..8631554cc --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/const.c @@ -0,0 +1,129 @@ +/* const.c + * + * Globally declared constants + * + * + * + * SYNOPSIS: + * + * extern double nameofconstant; + * + * + * + * + * DESCRIPTION: + * + * This file contains a number of mathematical constants and + * also some needed size parameters of the computer arithmetic. + * The values are supplied as arrays of hexadecimal integers + * for IEEE arithmetic, and in a normal decimal scientific notation for + * other machines. The particular notation used is determined + * by a symbol (IBMPC, or UNK) defined in the include file + * mconf.h. + * + * The default size parameters are as follows. + * + * For UNK mode: + * MACHEP = 1.38777878078144567553E-17 2**-56 + * MAXLOG = 8.8029691931113054295988E1 log(2**127) + * MINLOG = -8.872283911167299960540E1 log(2**-128) + * + * For IEEE arithmetic (IBMPC): + * MACHEP = 1.11022302462515654042E-16 2**-53 + * MAXLOG = 7.09782712893383996843E2 log(2**1024) + * MINLOG = -7.08396418532264106224E2 log(2**-1022) + * + * The global symbols for mathematical constants are + * SQ2OPI = 7.9788456080286535587989E-1 sqrt( 2/pi ) + * LOGSQ2 = 3.46573590279972654709E-1 log(2)/2 + * THPIO4 = 2.35619449019234492885 3*pi/4 + * + * These lists are subject to change. + */ + +/* const.c */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +#ifdef UNK +double MACHEP = 1.11022302462515654042E-16; /* 2**-53 */ + +#ifdef DENORMAL +double MAXLOG = 7.09782712893383996732E2; /* log(DBL_MAX) */ + + /* double MINLOG = -7.44440071921381262314E2; *//* log(2**-1074) */ +double MINLOG = -7.451332191019412076235E2; /* log(2**-1075) */ +#else +double MAXLOG = 7.08396418532264106224E2; /* log 2**1022 */ +double MINLOG = -7.08396418532264106224E2; /* log 2**-1022 */ +#endif +double SQ2OPI = 7.9788456080286535587989E-1; /* sqrt( 2/pi ) */ +double LOGSQ2 = 3.46573590279972654709E-1; /* log(2)/2 */ +double THPIO4 = 2.35619449019234492885; /* 3*pi/4 */ + +#endif + +#ifdef IBMPC + /* 2**-53 = 1.11022302462515654042E-16 */ +unsigned short MACHEP[4] = { 0x0000, 0x0000, 0x0000, 0x3ca0 }; + +#ifdef DENORMAL + /* log(DBL_MAX) = 7.09782712893383996732224E2 */ +unsigned short MAXLOG[4] = { 0x39ef, 0xfefa, 0x2e42, 0x4086 }; + + /* log(2**-1074) = - -7.44440071921381262314E2 */ +/*unsigned short MINLOG[4] = {0x71c3,0x446d,0x4385,0xc087}; */ +unsigned short MINLOG[4] = { 0x3052, 0xd52d, 0x4910, 0xc087 }; +#else + /* log(2**1022) = 7.08396418532264106224E2 */ +unsigned short MAXLOG[4] = { 0xbcd2, 0xdd7a, 0x232b, 0x4086 }; + + /* log(2**-1022) = - 7.08396418532264106224E2 */ +unsigned short MINLOG[4] = { 0xbcd2, 0xdd7a, 0x232b, 0xc086 }; +#endif + /* 2**1024*(1-MACHEP) = 1.7976931348623158E308 */ +unsigned short SQ2OPI[4] = { 0x3651, 0x33d4, 0x8845, 0x3fe9 }; +unsigned short LOGSQ2[4] = { 0x39ef, 0xfefa, 0x2e42, 0x3fd6 }; +unsigned short THPIO4[4] = { 0x21d2, 0x7f33, 0xd97c, 0x4002 }; + +#endif + +#ifdef MIEEE + /* 2**-53 = 1.11022302462515654042E-16 */ +unsigned short MACHEP[4] = { 0x3ca0, 0x0000, 0x0000, 0x0000 }; + +#ifdef DENORMAL + /* log(2**1024) = 7.09782712893383996843E2 */ +unsigned short MAXLOG[4] = { 0x4086, 0x2e42, 0xfefa, 0x39ef }; + + /* log(2**-1074) = - -7.44440071921381262314E2 */ +/* unsigned short MINLOG[4] = {0xc087,0x4385,0x446d,0x71c3}; */ +unsigned short MINLOG[4] = { 0xc087, 0x4910, 0xd52d, 0x3052 }; +#else + /* log(2**1022) = 7.08396418532264106224E2 */ +unsigned short MAXLOG[4] = { 0x4086, 0x232b, 0xdd7a, 0xbcd2 }; + + /* log(2**-1022) = - 7.08396418532264106224E2 */ +unsigned short MINLOG[4] = { 0xc086, 0x232b, 0xdd7a, 0xbcd2 }; +#endif + /* 2**1024*(1-MACHEP) = 1.7976931348623158E308 */ +unsigned short SQ2OPI[4] = { 0x3fe9, 0x8845, 0x33d4, 0x3651 }; +unsigned short LOGSQ2[4] = { 0x3fd6, 0x2e42, 0xfefa, 0x39ef }; +unsigned short THPIO4[4] = { 0x4002, 0xd97c, 0x7f33, 0x21d2 }; + +#endif + +#ifndef UNK +extern unsigned short MACHEP[]; +extern unsigned short MAXLOG[]; +extern unsigned short UNDLOG[]; +extern unsigned short MINLOG[]; +extern unsigned short SQ2OPI[]; +extern unsigned short LOGSQ2[]; +extern unsigned short THPIO4[]; +#endif diff --git a/gtsam/3rdparty/cephes/cephes/dawsn.c b/gtsam/3rdparty/cephes/cephes/dawsn.c new file mode 100644 index 000000000..7049f191e --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dawsn.c @@ -0,0 +1,160 @@ +/* dawsn.c + * + * Dawson's Integral + * + * + * + * SYNOPSIS: + * + * double x, y, dawsn(); + * + * y = dawsn( x ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * x + * - + * 2 | | 2 + * dawsn(x) = exp( -x ) | exp( t ) dt + * | | + * - + * 0 + * + * Three different rational approximations are employed, for + * the intervals 0 to 3.25; 3.25 to 6.25; and 6.25 up. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,10 10000 6.9e-16 1.0e-16 + * + * + */ + +/* dawsn.c */ + + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" +/* Dawson's integral, interval 0 to 3.25 */ +static double AN[10] = { + 1.13681498971755972054E-11, + 8.49262267667473811108E-10, + 1.94434204175553054283E-8, + 9.53151741254484363489E-7, + 3.07828309874913200438E-6, + 3.52513368520288738649E-4, + -8.50149846724410912031E-4, + 4.22618223005546594270E-2, + -9.17480371773452345351E-2, + 9.99999999999999994612E-1, +}; + +static double AD[11] = { + 2.40372073066762605484E-11, + 1.48864681368493396752E-9, + 5.21265281010541664570E-8, + 1.27258478273186970203E-6, + 2.32490249820789513991E-5, + 3.25524741826057911661E-4, + 3.48805814657162590916E-3, + 2.79448531198828973716E-2, + 1.58874241960120565368E-1, + 5.74918629489320327824E-1, + 1.00000000000000000539E0, +}; + +/* interval 3.25 to 6.25 */ +static double BN[11] = { + 5.08955156417900903354E-1, + -2.44754418142697847934E-1, + 9.41512335303534411857E-2, + -2.18711255142039025206E-2, + 3.66207612329569181322E-3, + -4.23209114460388756528E-4, + 3.59641304793896631888E-5, + -2.14640351719968974225E-6, + 9.10010780076391431042E-8, + -2.40274520828250956942E-9, + 3.59233385440928410398E-11, +}; + +static double BD[10] = { + /* 1.00000000000000000000E0, */ + -6.31839869873368190192E-1, + 2.36706788228248691528E-1, + -5.31806367003223277662E-2, + 8.48041718586295374409E-3, + -9.47996768486665330168E-4, + 7.81025592944552338085E-5, + -4.55875153252442634831E-6, + 1.89100358111421846170E-7, + -4.91324691331920606875E-9, + 7.18466403235734541950E-11, +}; + +/* 6.25 to infinity */ +static double CN[5] = { + -5.90592860534773254987E-1, + 6.29235242724368800674E-1, + -1.72858975380388136411E-1, + 1.64837047825189632310E-2, + -4.86827613020462700845E-4, +}; + +static double CD[5] = { + /* 1.00000000000000000000E0, */ + -2.69820057197544900361E0, + 1.73270799045947845857E0, + -3.93708582281939493482E-1, + 3.44278924041233391079E-2, + -9.73655226040941223894E-4, +}; + +extern double MACHEP; + +double dawsn(double xx) +{ + double x, y; + int sign; + + + sign = 1; + if (xx < 0.0) { + sign = -1; + xx = -xx; + } + + if (xx < 3.25) { + x = xx * xx; + y = xx * polevl(x, AN, 9) / polevl(x, AD, 10); + return (sign * y); + } + + + x = 1.0 / (xx * xx); + + if (xx < 6.25) { + y = 1.0 / xx + x * polevl(x, BN, 10) / (p1evl(x, BD, 10) * xx); + return (sign * 0.5 * y); + } + + + if (xx > 1.0e9) + return ((sign * 0.5) / xx); + + /* 6.25 to infinity */ + y = 1.0 / xx + x * polevl(x, CN, 4) / (p1evl(x, CD, 5) * xx); + return (sign * 0.5 * y); +} diff --git a/gtsam/3rdparty/cephes/cephes/dd_idefs.h b/gtsam/3rdparty/cephes/cephes/dd_idefs.h new file mode 100644 index 000000000..fec97c478 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dd_idefs.h @@ -0,0 +1,198 @@ +/* + * include/dd_inline.h + * + * This work was supported by the Director, Office of Science, Division + * of Mathematical, Information, and Computational Sciences of the + * U.S. Department of Energy under contract numbers DE-AC03-76SF00098 and + * DE-AC02-05CH11231. + * + * Copyright (c) 2003-2009, The Regents of the University of California, + * through Lawrence Berkeley National Laboratory (subject to receipt of + * any required approvals from U.S. Dept. of Energy) All rights reserved. + * + * By downloading or using this software you are agreeing to the modified + * BSD license "BSD-LBNL-License.doc" (see LICENSE.txt). + */ +/* + * Contains small functions (suitable for inlining) in the double-double + * arithmetic package. + */ + +#ifndef _DD_IDEFS_H_ +#define _DD_IDEFS_H_ 1 + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#define _DD_SPLITTER 134217729.0 // = 2^27 + 1 +#define _DD_SPLIT_THRESH 6.69692879491417e+299 // = 2^996 + +/* + ************************************************************************ + The basic routines taking double arguments, returning 1 (or 2) doubles + ************************************************************************ +*/ + +/* Computes fl(a+b) and err(a+b). Assumes |a| >= |b|. */ +static inline double +quick_two_sum(double a, double b, double *err) +{ + volatile double s = a + b; + volatile double c = s - a; + *err = b - c; + return s; +} + +/* Computes fl(a-b) and err(a-b). Assumes |a| >= |b| */ +static inline double +quick_two_diff(double a, double b, double *err) +{ + volatile double s = a - b; + volatile double c = a - s; + *err = c - b; + return s; +} + +/* Computes fl(a+b) and err(a+b). */ +static inline double +two_sum(double a, double b, double *err) +{ + volatile double s = a + b; + volatile double c = s - a; + volatile double d = b - c; + volatile double e = s - c; + *err = (a - e) + d; + return s; +} + +/* Computes fl(a-b) and err(a-b). */ +static inline double +two_diff(double a, double b, double *err) +{ + volatile double s = a - b; + volatile double c = s - a; + volatile double d = b + c; + volatile double e = s - c; + *err = (a - e) - d; + return s; +} + +/* Computes high word and lo word of a */ +static inline void +two_split(double a, double *hi, double *lo) +{ + volatile double temp, tempma; + if (a > _DD_SPLIT_THRESH || a < -_DD_SPLIT_THRESH) { + a *= 3.7252902984619140625e-09; // 2^-28 + temp = _DD_SPLITTER * a; + tempma = temp - a; + *hi = temp - tempma; + *lo = a - *hi; + *hi *= 268435456.0; // 2^28 + *lo *= 268435456.0; // 2^28 + } + else { + temp = _DD_SPLITTER * a; + tempma = temp - a; + *hi = temp - tempma; + *lo = a - *hi; + } +} + +/* Computes fl(a*b) and err(a*b). */ +static inline double +two_prod(double a, double b, double *err) +{ +#ifdef DD_FMS + volatile double p = a * b; + *err = DD_FMS(a, b, p); + return p; +#else + double a_hi, a_lo, b_hi, b_lo; + double p = a * b; + volatile double c, d; + two_split(a, &a_hi, &a_lo); + two_split(b, &b_hi, &b_lo); + c = a_hi * b_hi - p; + d = c + a_hi * b_lo + a_lo * b_hi; + *err = d + a_lo * b_lo; + return p; +#endif /* DD_FMA */ +} + +/* Computes fl(a*a) and err(a*a). Faster than the above method. */ +static inline double +two_sqr(double a, double *err) +{ +#ifdef DD_FMS + volatile double p = a * a; + *err = DD_FMS(a, a, p); + return p; +#else + double hi, lo; + volatile double c; + double q = a * a; + two_split(a, &hi, &lo); + c = hi * hi - q; + *err = (c + 2.0 * hi * lo) + lo * lo; + return q; +#endif /* DD_FMS */ +} + +static inline double +two_div(double a, double b, double *err) +{ + volatile double q1, q2; + double p1, p2; + double s, e; + + q1 = a / b; + + /* Compute a - q1 * b */ + p1 = two_prod(q1, b, &p2); + s = two_diff(a, p1, &e); + e -= p2; + + /* get next approximation */ + q2 = (s + e) / b; + + return quick_two_sum(q1, q2, err); +} + +/* Computes the nearest integer to d. */ +static inline double +two_nint(double d) +{ + if (d == floor(d)) { + return d; + } + return floor(d + 0.5); +} + +/* Computes the truncated integer. */ +static inline double +two_aint(double d) +{ + return (d >= 0.0 ? floor(d) : ceil(d)); +} + + +/* Compare a and b */ +static inline int +two_comp(const double a, const double b) +{ + /* Works for non-NAN inputs */ + return (a < b ? -1 : (a > b ? 1 : 0)); +} + + +#ifdef __cplusplus +} +#endif + +#endif /* _DD_IDEFS_H_ */ diff --git a/gtsam/3rdparty/cephes/cephes/dd_real.c b/gtsam/3rdparty/cephes/cephes/dd_real.c new file mode 100644 index 000000000..c37f57a7b --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dd_real.c @@ -0,0 +1,587 @@ +/* + * src/double2.cc + * + * This work was supported by the Director, Office of Science, Division + * of Mathematical, Information, and Computational Sciences of the + * U.S. Department of Energy under contract numbers DE-AC03-76SF00098 and + * DE-AC02-05CH11231. + * + * Copyright (c) 2003-2009, The Regents of the University of California, + * through Lawrence Berkeley National Laboratory (subject to receipt of + * any required approvals from U.S. Dept. of Energy) All rights reserved. + * + * By downloading or using this software you are agreeing to the modified + * BSD license "BSD-LBNL-License.doc" (see LICENSE.txt). + */ +/* + * Contains implementation of non-inlined functions of double-double + * package. Inlined functions are found in dd_real_inline.h. + */ + +/* + * This code taken from v2.3.18 of the qd package. +*/ + + +#include +#include +#include +#include + +#include "dd_real.h" + +#define _DD_REAL_INIT(A, B) {{A, B}} + +const double DD_C_EPS = 4.93038065763132e-32; // 2^-104 +const double DD_C_MIN_NORMALIZED = 2.0041683600089728e-292; // = 2^(-1022 + 53) + +/* Compile-time initialization of const double2 structs */ + +const double2 DD_C_MAX = + _DD_REAL_INIT(1.79769313486231570815e+308, 9.97920154767359795037e+291); +const double2 DD_C_SAFE_MAX = + _DD_REAL_INIT(1.7976931080746007281e+308, 9.97920154767359795037e+291); +const int _DD_C_NDIGITS = 31; + +const double2 DD_C_ZERO = _DD_REAL_INIT(0.0, 0.0); +const double2 DD_C_ONE = _DD_REAL_INIT(1.0, 0.0); +const double2 DD_C_NEGONE = _DD_REAL_INIT(-1.0, 0.0); + +const double2 DD_C_2PI = + _DD_REAL_INIT(6.283185307179586232e+00, 2.449293598294706414e-16); +const double2 DD_C_PI = + _DD_REAL_INIT(3.141592653589793116e+00, 1.224646799147353207e-16); +const double2 DD_C_PI2 = + _DD_REAL_INIT(1.570796326794896558e+00, 6.123233995736766036e-17); +const double2 DD_C_PI4 = + _DD_REAL_INIT(7.853981633974482790e-01, 3.061616997868383018e-17); +const double2 DD_C_PI16 = + _DD_REAL_INIT(1.963495408493620697e-01, 7.654042494670957545e-18); +const double2 DD_C_3PI4 = + _DD_REAL_INIT(2.356194490192344837e+00, 9.1848509936051484375e-17); + +const double2 DD_C_E = + _DD_REAL_INIT(2.718281828459045091e+00, 1.445646891729250158e-16); +const double2 DD_C_LOG2 = + _DD_REAL_INIT(6.931471805599452862e-01, 2.319046813846299558e-17); +const double2 DD_C_LOG10 = + _DD_REAL_INIT(2.302585092994045901e+00, -2.170756223382249351e-16); + +#ifdef DD_C_NAN_IS_CONST +const double2 DD_C_NAN = _DD_REAL_INIT(NAN, NAN); +const double2 DD_C_INF = _DD_REAL_INIT(INFINITY, INFINITY); +const double2 DD_C_NEGINF = _DD_REAL_INIT(-INFINITY, -INFINITY); +#endif /* NAN */ + + +/* This routine is called whenever a fatal error occurs. */ +static volatile int errCount = 0; +void +dd_error(const char *msg) +{ + errCount++; + /* if (msg) { */ + /* fprintf(stderr, "ERROR %s\n", msg); */ + /* } */ +} + + +int +get_double_expn(double x) +{ + int i = 0; + double y; + if (x == 0.0) { + return INT_MIN; + } + if (isinf(x) || isnan(x)) { + return INT_MAX; + } + + y = fabs(x); + if (y < 1.0) { + while (y < 1.0) { + y *= 2.0; + i++; + } + return -i; + } else if (y >= 2.0) { + while (y >= 2.0) { + y *= 0.5; + i++; + } + return i; + } + return 0; +} + +/* ######################################################################## */ +/* # Exponentiation */ +/* ######################################################################## */ + +/* Computes the square root of the double-double number dd. + NOTE: dd must be a non-negative number. */ + +double2 +dd_sqrt(const double2 a) +{ + /* Strategy: Use Karp's trick: if x is an approximation + to sqrt(a), then + + sqrt(a) = a*x + [a - (a*x)^2] * x / 2 (approx) + + The approximation is accurate to twice the accuracy of x. + Also, the multiplication (a*x) and [-]*x can be done with + only half the precision. + */ + double x, ax; + + if (dd_is_zero(a)) + return DD_C_ZERO; + + if (dd_is_negative(a)) { + dd_error("(dd_sqrt): Negative argument."); + return DD_C_NAN; + } + + x = 1.0 / sqrt(a.x[0]); + ax = a.x[0] * x; + return dd_add_d_d(ax, dd_sub(a, dd_sqr_d(ax)).x[0] * (x * 0.5)); +} + +/* Computes the square root of a double in double-double precision. + NOTE: d must not be negative. */ + +double2 +dd_sqrt_d(double d) +{ + return dd_sqrt(dd_create_d(d)); +} + +/* Computes the n-th root of the double-double number a. + NOTE: n must be a positive integer. + NOTE: If n is even, then a must not be negative. */ + +double2 +dd_nroot(const double2 a, int n) +{ + /* Strategy: Use Newton iteration for the function + + f(x) = x^(-n) - a + + to find its root a^{-1/n}. The iteration is thus + + x' = x + x * (1 - a * x^n) / n + + which converges quadratically. We can then find + a^{1/n} by taking the reciprocal. + */ + double2 r, x; + + if (n <= 0) { + dd_error("(dd_nroot): N must be positive."); + return DD_C_NAN; + } + + if (n % 2 == 0 && dd_is_negative(a)) { + dd_error("(dd_nroot): Negative argument."); + return DD_C_NAN; + } + + if (n == 1) { + return a; + } + if (n == 2) { + return dd_sqrt(a); + } + + if (dd_is_zero(a)) + return DD_C_ZERO; + + /* Note a^{-1/n} = exp(-log(a)/n) */ + r = dd_abs(a); + x = dd_create_d(exp(-log(r.x[0]) / n)); + + /* Perform Newton's iteration. */ + x = dd_add( + x, dd_mul(x, dd_sub_d_dd(1.0, dd_div_dd_d(dd_mul(r, dd_npwr(x, n)), + DD_STATIC_CAST(double, n))))); + if (a.x[0] < 0.0) { + x = dd_neg(x); + } + return dd_inv(x); +} + +/* Computes the n-th power of a double-double number. + NOTE: 0^0 causes an error. */ + +double2 +dd_npwr(const double2 a, int n) +{ + double2 r = a; + double2 s = DD_C_ONE; + int N = abs(n); + if (N == 0) { + if (dd_is_zero(a)) { + dd_error("(dd_npwr): Invalid argument."); + return DD_C_NAN; + } + return DD_C_ONE; + } + + if (N > 1) { + /* Use binary exponentiation */ + while (N > 0) { + if (N % 2 == 1) { + s = dd_mul(s, r); + } + N /= 2; + if (N > 0) { + r = dd_sqr(r); + } + } + } + else { + s = r; + } + + /* Compute the reciprocal if n is negative. */ + if (n < 0) { + return dd_inv(s); + } + + return s; +} + +double2 +dd_npow(const double2 a, int n) +{ + return dd_npwr(a, n); +} + +double2 +dd_pow(const double2 a, const double2 b) +{ + return dd_exp(dd_mul(b, dd_log(a))); +} + +/* ######################################################################## */ +/* # Exp/Log functions */ +/* ######################################################################## */ + +static const double2 inv_fact[] = { + {{1.66666666666666657e-01, 9.25185853854297066e-18}}, + {{4.16666666666666644e-02, 2.31296463463574266e-18}}, + {{8.33333333333333322e-03, 1.15648231731787138e-19}}, + {{1.38888888888888894e-03, -5.30054395437357706e-20}}, + {{1.98412698412698413e-04, 1.72095582934207053e-22}}, + {{2.48015873015873016e-05, 2.15119478667758816e-23}}, + {{2.75573192239858925e-06, -1.85839327404647208e-22}}, + {{2.75573192239858883e-07, 2.37677146222502973e-23}}, + {{2.50521083854417202e-08, -1.44881407093591197e-24}}, + {{2.08767569878681002e-09, -1.20734505911325997e-25}}, + {{1.60590438368216133e-10, 1.25852945887520981e-26}}, + {{1.14707455977297245e-11, 2.06555127528307454e-28}}, + {{7.64716373181981641e-13, 7.03872877733453001e-30}}, + {{4.77947733238738525e-14, 4.39920548583408126e-31}}, + {{2.81145725434552060e-15, 1.65088427308614326e-31}} +}; +//static const int n_inv_fact = sizeof(inv_fact) / sizeof(inv_fact[0]); + +/* Exponential. Computes exp(x) in double-double precision. */ + +double2 +dd_exp(const double2 a) +{ + /* Strategy: We first reduce the size of x by noting that + + exp(kr + m * log(2)) = 2^m * exp(r)^k + + where m and k are integers. By choosing m appropriately + we can make |kr| <= log(2) / 2 = 0.347. Then exp(r) is + evaluated using the familiar Taylor series. Reducing the + argument substantially speeds up the convergence. */ + + const double k = 512.0; + const double inv_k = 1.0 / k; + double m; + double2 r, s, t, p; + int i = 0; + + if (a.x[0] <= -709.0) { + return DD_C_ZERO; + } + + if (a.x[0] >= 709.0) { + return DD_C_INF; + } + + if (dd_is_zero(a)) { + return DD_C_ONE; + } + + if (dd_is_one(a)) { + return DD_C_E; + } + + m = floor(a.x[0] / DD_C_LOG2.x[0] + 0.5); + r = dd_mul_pwr2(dd_sub(a, dd_mul_dd_d(DD_C_LOG2, m)), inv_k); + + p = dd_sqr(r); + s = dd_add(r, dd_mul_pwr2(p, 0.5)); + p = dd_mul(p, r); + t = dd_mul(p, inv_fact[0]); + do { + s = dd_add(s, t); + p = dd_mul(p, r); + ++i; + t = dd_mul(p, inv_fact[i]); + } while (fabs(dd_to_double(t)) > inv_k * DD_C_EPS && i < 5); + + s = dd_add(s, t); + + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(dd_mul_pwr2(s, 2.0), dd_sqr(s)); + s = dd_add(s, DD_C_ONE); + + return dd_ldexp(s, DD_STATIC_CAST(int, m)); +} + +double2 +dd_exp_d(const double a) +{ + return dd_exp(dd_create(a, 0)); +} + + +/* Logarithm. Computes log(x) in double-double precision. + This is a natural logarithm (i.e., base e). */ +double2 +dd_log(const double2 a) +{ + /* Strategy. The Taylor series for log converges much more + slowly than that of exp, due to the lack of the factorial + term in the denominator. Hence this routine instead tries + to determine the root of the function + + f(x) = exp(x) - a + + using Newton iteration. The iteration is given by + + x' = x - f(x)/f'(x) + = x - (1 - a * exp(-x)) + = x + a * exp(-x) - 1. + + Only one iteration is needed, since Newton's iteration + approximately doubles the number of digits per iteration. */ + double2 x; + + if (dd_is_one(a)) { + return DD_C_ZERO; + } + + if (a.x[0] <= 0.0) { + dd_error("(dd_log): Non-positive argument."); + return DD_C_NAN; + } + + x = dd_create_d(log(a.x[0])); /* Initial approximation */ + + /* x = x + a * exp(-x) - 1.0; */ + x = dd_add(x, dd_sub(dd_mul(a, dd_exp(dd_neg(x))), DD_C_ONE)); + return x; +} + + +double2 +dd_log1p(const double2 a) +{ + double2 ans; + double la, elam1, ll; + if (a.x[0] <= -1.0) { + return DD_C_NEGINF; + } + la = log1p(a.x[0]); + elam1 = expm1(la); + ll = log1p(a.x[1] / (1 + a.x[0])); + if (a.x[0] > 0) { + ll -= (elam1 - a.x[0])/(elam1+1); + } + ans = dd_add_d_d(la, ll); + return ans; +} + +double2 +dd_log10(const double2 a) +{ + return dd_div(dd_log(a), DD_C_LOG10); +} + +double2 +dd_log_d(double a) +{ + return dd_log(dd_create(a, 0)); +} + + +static const double2 expm1_numer[] = { + {{-0.028127670288085938, 1.46e-37}}, + {{0.5127815691121048, -4.248816580490825e-17}}, + {{-0.0632631785207471, 4.733650586348708e-18}}, + {{0.01470328560687425, -4.57569727474415e-20}}, + {{-0.0008675686051689528, 2.340010361165805e-20}}, + {{8.812635961829116e-05, 2.619804163788941e-21}}, + {{-2.596308786770631e-06, -1.6196413688647164e-22}}, + {{1.422669108780046e-07, 1.2956999470135368e-23}}, + {{-1.5995603306536497e-09, 5.185121944095551e-26}}, + {{4.526182006900779e-11, -1.9856249941108077e-27}} +}; + +static const double2 expm1_denom[] = { + {{1.0, 0.0}}, + {{-0.4544126470907431, -2.2553855773661143e-17}}, + {{0.09682713193619222, -4.961446925746919e-19}}, + {{-0.012745248725908178, -6.0676821249478945e-19}}, + {{0.001147361387158326, 1.3575817248483204e-20}}, + {{-7.370416847725892e-05, 3.720369981570573e-21}}, + {{3.4087499397791556e-06, -3.3067348191741576e-23}}, + {{-1.1114024704296196e-07, -3.313361038199987e-24}}, + {{2.3987051614110847e-09, 1.102474920537503e-25}}, + {{-2.947734185911159e-11, -9.4795654767864e-28}}, + {{1.32220659910223e-13, 6.440648413523595e-30}} +}; + +// +// Rational approximation of expm1(x) for -1/2 < x < 1/2 +// +static double2 +expm1_rational_approx(const double2 x) +{ + const double2 Y = dd_create(1.028127670288086, 0.0); + const double2 num = dd_polyeval(expm1_numer, 9, x); + const double2 den = dd_polyeval(expm1_denom, 10, x); + return dd_add(dd_mul(x, Y), dd_mul(x, dd_div(num, den))); +} + +// +// This is a translation of Boost's `expm1_imp` for quad precision +// for use with double2. +// + +#define LOG_MAX_VALUE 709.782712893384 + +double2 +dd_expm1(const double2 x) +{ + double2 a = dd_abs(x); + if (dd_hi(a) > 0.5) { + if (dd_hi(a) > LOG_MAX_VALUE) { + if (dd_hi(x) > 0) { + return DD_C_INF; + } + return DD_C_NEGONE; + } + return dd_sub_dd_d(dd_exp(x), 1.0); + } + return expm1_rational_approx(x); +} + + +double2 +dd_rand(void) +{ + static const double m_const = 4.6566128730773926e-10; /* = 2^{-31} */ + double m = m_const; + double2 r = DD_C_ZERO; + double d; + int i; + + /* Strategy: Generate 31 bits at a time, using lrand48 + random number generator. Shift the bits, and reapeat + 4 times. */ + + for (i = 0; i < 4; i++, m *= m_const) { + // d = lrand48() * m; + d = rand() * m; + r = dd_add_dd_d(r, d); + } + + return r; +} + +/* dd_polyeval(c, n, x) + Evaluates the given n-th degree polynomial at x. + The polynomial is given by the array of (n+1) coefficients. */ + +double2 +dd_polyeval(const double2 *c, int n, const double2 x) +{ + /* Just use Horner's method of polynomial evaluation. */ + double2 r = c[n]; + int i; + + for (i = n - 1; i >= 0; i--) { + r = dd_mul(r, x); + r = dd_add(r, c[i]); + } + + return r; +} + +/* dd_polyroot(c, n, x0) + Given an n-th degree polynomial, finds a root close to + the given guess x0. Note that this uses simple Newton + iteration scheme, and does not work for multiple roots. */ + +double2 +dd_polyroot(const double2 *c, int n, const double2 x0, int max_iter, + double thresh) +{ + double2 x = x0; + double2 f; + double2 *d = DD_STATIC_CAST(double2 *, calloc(sizeof(double2), n)); + int conv = 0; + int i; + double max_c = fabs(dd_to_double(c[0])); + double v; + + if (thresh == 0.0) { + thresh = DD_C_EPS; + } + + /* Compute the coefficients of the derivatives. */ + for (i = 1; i <= n; i++) { + v = fabs(dd_to_double(c[i])); + if (v > max_c) { + max_c = v; + } + d[i - 1] = dd_mul_dd_d(c[i], DD_STATIC_CAST(double, i)); + } + thresh *= max_c; + + /* Newton iteration. */ + for (i = 0; i < max_iter; i++) { + f = dd_polyeval(c, n, x); + + if (fabs(dd_to_double(f)) < thresh) { + conv = 1; + break; + } + x = dd_sub(x, (dd_div(f, dd_polyeval(d, n - 1, x)))); + } + free(d); + + if (!conv) { + dd_error("(dd_polyroot): Failed to converge."); + return DD_C_NAN; + } + + return x; +} diff --git a/gtsam/3rdparty/cephes/cephes/dd_real.h b/gtsam/3rdparty/cephes/cephes/dd_real.h new file mode 100644 index 000000000..4e09da143 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dd_real.h @@ -0,0 +1,143 @@ +/* + * include/double2.h + * + * This work was supported by the Director, Office of Science, Division + * of Mathematical, Information, and Computational Sciences of the + * U.S. Department of Energy under contract numbers DE-AC03-76SF00098 and + * DE-AC02-05CH11231. + * + * Copyright (c) 2003-2009, The Regents of the University of California, + * through Lawrence Berkeley National Laboratory (subject to receipt of + * any required approvals from U.S. Dept. of Energy) All rights reserved. + * + * By downloading or using this software you are agreeing to the modified + * BSD license "BSD-LBNL-License.doc" (see LICENSE.txt). + */ +/* + * Double-double precision (>= 106-bit significand) floating point + * arithmetic package based on David Bailey's Fortran-90 double-double + * package, with some changes. See + * + * http://www.nersc.gov/~dhbailey/mpdist/mpdist.html + * + * for the original Fortran-90 version. + * + * Overall structure is similar to that of Keith Brigg's C++ double-double + * package. See + * + * http://www-epidem.plansci.cam.ac.uk/~kbriggs/doubledouble.html + * + * for more details. In particular, the fix for x86 computers is borrowed + * from his code. + * + * Yozo Hida + */ + +#ifndef _DD_REAL_H +#define _DD_REAL_H + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +/* Some configuration defines */ + +/* If fast fused multiply-add is available, define to the correct macro for + using it. It is invoked as DD_FMA(a, b, c) to compute fl(a * b + c). + If correctly rounded multiply-add is not available (or if unsure), + keep it undefined. */ +#ifndef DD_FMA +#ifdef FP_FAST_FMA +#define DD_FMA(A, B, C) fma((A), (B), (C)) +#endif +#endif + +/* Same with fused multiply-subtract */ +#ifndef DD_FMS +#ifdef FP_FAST_FMA +#define DD_FMS(A, B, C) fma((A), (B), (-C)) +#endif +#endif + +#ifdef __cplusplus +#define DD_STATIC_CAST(T, X) (static_cast(X)) +#else +#define DD_STATIC_CAST(T, X) ((T)(X)) +#endif + +/* double2 struct definition, some external always-present double2 constants. +*/ +typedef struct double2 +{ + double x[2]; +} double2; + +extern const double DD_C_EPS; +extern const double DD_C_MIN_NORMALIZED; +extern const double2 DD_C_MAX; +extern const double2 DD_C_SAFE_MAX; +extern const int DD_C_NDIGITS; + +extern const double2 DD_C_2PI; +extern const double2 DD_C_PI; +extern const double2 DD_C_3PI4; +extern const double2 DD_C_PI2; +extern const double2 DD_C_PI4; +extern const double2 DD_C_PI16; +extern const double2 DD_C_E; +extern const double2 DD_C_LOG2; +extern const double2 DD_C_LOG10; +extern const double2 DD_C_ZERO; +extern const double2 DD_C_ONE; +extern const double2 DD_C_NEGONE; + +/* NAN definition in AIX's math.h doesn't make it qualify as constant literal. */ +#if defined(__STDC__) && defined(__STDC_VERSION__) && (__STDC_VERSION__ >= 199901L) && defined(NAN) && !defined(_AIX) +#define DD_C_NAN_IS_CONST +extern const double2 DD_C_NAN; +extern const double2 DD_C_INF; +extern const double2 DD_C_NEGINF; +#else +#define DD_C_NAN (dd_create(NAN, NAN)) +#define DD_C_INF (dd_create(INFINITY, INFINITY)) +#define DD_C_NEGINF (dd_create(-INFINITY, -INFINITY)) +#endif + + +/* Include the inline definitions of functions */ +#include "dd_real_idefs.h" + +/* Non-inline functions */ + +/********** Exponentiation **********/ +double2 dd_npwr(const double2 a, int n); + +/*********** Transcendental Functions ************/ +double2 dd_exp(const double2 a); +double2 dd_log(const double2 a); +double2 dd_expm1(const double2 a); +double2 dd_log1p(const double2 a); +double2 dd_log10(const double2 a); +double2 dd_log_d(double a); + +/* Returns the exponent of the double precision number. + Returns INT_MIN is x is zero, and INT_MAX if x is INF or NaN. */ +int get_double_expn(double x); + +/*********** Polynomial Functions ************/ +double2 dd_polyeval(const double2 *c, int n, const double2 x); + +/*********** Random number generator ************/ +extern double2 dd_rand(void); + + +#ifdef __cplusplus +} +#endif + + +#endif /* _DD_REAL_H */ diff --git a/gtsam/3rdparty/cephes/cephes/dd_real_idefs.h b/gtsam/3rdparty/cephes/cephes/dd_real_idefs.h new file mode 100644 index 000000000..d2b9ac1d6 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/dd_real_idefs.h @@ -0,0 +1,557 @@ +/* + * include/dd_inline.h + * + * This work was supported by the Director, Office of Science, Division + * of Mathematical, Information, and Computational Sciences of the + * U.S. Department of Energy under contract numbers DE-AC03-76SF00098 and + * DE-AC02-05CH11231. + * + * Copyright (c) 2003-2009, The Regents of the University of California, + * through Lawrence Berkeley National Laboratory (subject to receipt of + * any required approvals from U.S. Dept. of Energy) All rights reserved. + * + * By downloading or using this software you are agreeing to the modified + * BSD license "BSD-LBNL-License.doc" (see LICENSE.txt). + */ +/* + * Contains small functions (suitable for inlining) in the double-double + * arithmetic package. + */ + +#ifndef _DD_REAL_IDEFS_H_ +#define _DD_REAL_IDEFS_H_ 1 + +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +#include "dd_idefs.h" + +/* + ************************************************************************ + Now for the double2 routines + ************************************************************************ +*/ + +static inline double +dd_hi(const double2 a) +{ + return a.x[0]; +} + +static inline double +dd_lo(const double2 a) +{ + return a.x[1]; +} + +static inline int +dd_isfinite(const double2 a) +{ + return isfinite(a.x[0]); +} + +static inline int +dd_isinf(const double2 a) +{ + return isinf(a.x[0]); +} + +static inline int +dd_is_zero(const double2 a) +{ + return (a.x[0] == 0.0); +} + +static inline int +dd_is_one(const double2 a) +{ + return (a.x[0] == 1.0 && a.x[1] == 0.0); +} + +static inline int +dd_is_positive(const double2 a) +{ + return (a.x[0] > 0.0); +} + +static inline int +dd_is_negative(const double2 a) +{ + return (a.x[0] < 0.0); +} + +/* Cast to double. */ +static inline double +dd_to_double(const double2 a) +{ + return a.x[0]; +} + +/* Cast to int. */ +static inline int +dd_to_int(const double2 a) +{ + return DD_STATIC_CAST(int, a.x[0]); +} + +/*********** Equality and Other Comparisons ************/ +static inline int +dd_comp(const double2 a, const double2 b) +{ + int cmp = two_comp(a.x[0], b.x[0]); + if (cmp == 0) { + cmp = two_comp(a.x[1], b.x[1]); + } + return cmp; +} + +static inline int +dd_comp_dd_d(const double2 a, double b) +{ + int cmp = two_comp(a.x[0], b); + if (cmp == 0) { + cmp = two_comp(a.x[1], 0); + } + return cmp; +} + +static inline int +dd_comp_d_dd(double a, const double2 b) +{ + int cmp = two_comp(a, b.x[0]); + if (cmp == 0) { + cmp = two_comp(0.0, b.x[1]); + } + return cmp; +} + + +/*********** Creation ************/ +static inline double2 +dd_create(double hi, double lo) +{ + double2 ret = {{hi, lo}}; + return ret; +} + +static inline double2 +dd_zero(void) +{ + return DD_C_ZERO; +} + +static inline double2 +dd_create_d(double hi) +{ + double2 ret = {{hi, 0.0}}; + return ret; +} + +static inline double2 +dd_create_i(int hi) +{ + double2 ret = {{DD_STATIC_CAST(double, hi), 0.0}}; + return ret; +} + +static inline double2 +dd_create_dp(const double *d) +{ + double2 ret = {{d[0], d[1]}}; + return ret; +} + + +/*********** Unary Minus ***********/ +static inline double2 +dd_neg(const double2 a) +{ + double2 ret = {{-a.x[0], -a.x[1]}}; + return ret; +} + +/*********** Rounding ************/ +/* Round to Nearest integer */ +static inline double2 +dd_nint(const double2 a) +{ + double hi = two_nint(a.x[0]); + double lo; + + if (hi == a.x[0]) { + /* High word is an integer already. Round the low word.*/ + lo = two_nint(a.x[1]); + + /* Renormalize. This is needed if x[0] = some integer, x[1] = 1/2.*/ + hi = quick_two_sum(hi, lo, &lo); + } + else { + /* High word is not an integer. */ + lo = 0.0; + if (fabs(hi - a.x[0]) == 0.5 && a.x[1] < 0.0) { + /* There is a tie in the high word, consult the low word + to break the tie. */ + hi -= 1.0; /* NOTE: This does not cause INEXACT. */ + } + } + + return dd_create(hi, lo); +} + +static inline double2 +dd_floor(const double2 a) +{ + double hi = floor(a.x[0]); + double lo = 0.0; + + if (hi == a.x[0]) { + /* High word is integer already. Round the low word. */ + lo = floor(a.x[1]); + hi = quick_two_sum(hi, lo, &lo); + } + + return dd_create(hi, lo); +} + +static inline double2 +dd_ceil(const double2 a) +{ + double hi = ceil(a.x[0]); + double lo = 0.0; + + if (hi == a.x[0]) { + /* High word is integer already. Round the low word. */ + lo = ceil(a.x[1]); + hi = quick_two_sum(hi, lo, &lo); + } + + return dd_create(hi, lo); +} + +static inline double2 +dd_aint(const double2 a) +{ + return (a.x[0] >= 0.0) ? dd_floor(a) : dd_ceil(a); +} + +/* Absolute value */ +static inline double2 +dd_abs(const double2 a) +{ + return (a.x[0] < 0.0 ? dd_neg(a) : a); +} + +static inline double2 +dd_fabs(const double2 a) +{ + return dd_abs(a); +} + + +/*********** Normalizing ***********/ +/* double-double * (2.0 ^ expt) */ +static inline double2 +dd_ldexp(const double2 a, int expt) +{ + return dd_create(ldexp(a.x[0], expt), ldexp(a.x[1], expt)); +} + +static inline double2 +dd_frexp(const double2 a, int *expt) +{ +// r"""return b and l s.t. 0.5<=|b|<1 and 2^l == a +// 0.5<=|b[0]|<1.0 or |b[0]| == 1.0 and b[0]*b[1]<0 +// """ + int exponent; + double man = frexp(a.x[0], &exponent); + double b1 = ldexp(a.x[1], -exponent); + if (fabs(man) == 0.5 && man * b1 < 0) + { + man *=2; + b1 *= 2; + exponent -= 1; + } + *expt = exponent; + return dd_create(man, b1); +} + + +/*********** Additions ************/ +static inline double2 +dd_add_d_d(double a, double b) +{ + double s, e; + s = two_sum(a, b, &e); + return dd_create(s, e); +} + +static inline double2 +dd_add_dd_d(const double2 a, double b) +{ + double s1, s2; + s1 = two_sum(a.x[0], b, &s2); + s2 += a.x[1]; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_add_d_dd(double a, const double2 b) +{ + double s1, s2; + s1 = two_sum(a, b.x[0], &s2); + s2 += b.x[1]; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_ieee_add(const double2 a, const double2 b) +{ + /* This one satisfies IEEE style error bound, + due to K. Briggs and W. Kahan. */ + double s1, s2, t1, t2; + + s1 = two_sum(a.x[0], b.x[0], &s2); + t1 = two_sum(a.x[1], b.x[1], &t2); + s2 += t1; + s1 = quick_two_sum(s1, s2, &s2); + s2 += t2; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_sloppy_add(const double2 a, const double2 b) +{ + /* This is the less accurate version ... obeys Cray-style + error bound. */ + double s, e; + + s = two_sum(a.x[0], b.x[0], &e); + e += (a.x[1] + b.x[1]); + s = quick_two_sum(s, e, &e); + return dd_create(s, e); +} + +static inline double2 +dd_add(const double2 a, const double2 b) +{ + /* Always require IEEE-style error bounds */ + return dd_ieee_add(a, b); +} + +/*********** Subtractions ************/ +/* double-double = double - double */ +static inline double2 +dd_sub_d_d(double a, double b) +{ + double s, e; + s = two_diff(a, b, &e); + return dd_create(s, e); +} + +static inline double2 +dd_sub(const double2 a, const double2 b) +{ + return dd_ieee_add(a, dd_neg(b)); +} + +static inline double2 +dd_sub_dd_d(const double2 a, double b) +{ + double s1, s2; + s1 = two_sum(a.x[0], -b, &s2); + s2 += a.x[1]; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_sub_d_dd(double a, const double2 b) +{ + double s1, s2; + s1 = two_sum(a, -b.x[0], &s2); + s2 -= b.x[1]; + s1 = quick_two_sum(s1, s2, &s2); + return dd_create(s1, s2); +} + + +/*********** Multiplications ************/ +/* double-double = double * double */ +static inline double2 +dd_mul_d_d(double a, double b) +{ + double p, e; + p = two_prod(a, b, &e); + return dd_create(p, e); +} + +/* double-double * double, where double is a power of 2. */ +static inline double2 +dd_mul_pwr2(const double2 a, double b) +{ + return dd_create(a.x[0] * b, a.x[1] * b); +} + +static inline double2 +dd_mul(const double2 a, const double2 b) +{ + double p1, p2; + p1 = two_prod(a.x[0], b.x[0], &p2); + p2 += (a.x[0] * b.x[1] + a.x[1] * b.x[0]); + p1 = quick_two_sum(p1, p2, &p2); + return dd_create(p1, p2); +} + +static inline double2 +dd_mul_dd_d(const double2 a, double b) +{ + double p1, p2, e1, e2; + p1 = two_prod(a.x[0], b, &e1); + p2 = two_prod(a.x[1], b, &e2); + p1 = quick_two_sum(p1, e2 + p2 + e1, &e1); + return dd_create(p1, e1); +} + +static inline double2 +dd_mul_d_dd(double a, const double2 b) +{ + double p1, p2, e1, e2; + p1 = two_prod(a, b.x[0], &e1); + p2 = two_prod(a, b.x[1], &e2); + p1 = quick_two_sum(p1, e2 + p2 + e1, &e1); + return dd_create(p1, e1); +} + + +/*********** Divisions ************/ +static inline double2 +dd_sloppy_div(const double2 a, const double2 b) +{ + double s1, s2; + double q1, q2; + double2 r; + + q1 = a.x[0] / b.x[0]; /* approximate quotient */ + + /* compute this - q1 * dd */ + r = dd_sub(a, dd_mul_dd_d(b, q1)); + s1 = two_diff(a.x[0], r.x[0], &s2); + s2 -= r.x[1]; + s2 += a.x[1]; + + /* get next approximation */ + q2 = (s1 + s2) / b.x[0]; + + /* renormalize */ + r.x[0] = quick_two_sum(q1, q2, &r.x[1]); + return r; +} + +static inline double2 +dd_accurate_div(const double2 a, const double2 b) +{ + double q1, q2, q3; + double2 r; + + q1 = a.x[0] / b.x[0]; /* approximate quotient */ + + r = dd_sub(a, dd_mul_dd_d(b, q1)); + + q2 = r.x[0] / b.x[0]; + r = dd_sub(r, dd_mul_dd_d(b, q2)); + + q3 = r.x[0] / b.x[0]; + + q1 = quick_two_sum(q1, q2, &q2); + r = dd_add_dd_d(dd_create(q1, q2), q3); + return r; +} + +static inline double2 +dd_div(const double2 a, const double2 b) +{ + return dd_accurate_div(a, b); +} + +static inline double2 +dd_div_d_d(double a, double b) +{ + return dd_accurate_div(dd_create_d(a), dd_create_d(b)); +} + +static inline double2 +dd_div_dd_d(const double2 a, double b) +{ + return dd_accurate_div(a, dd_create_d(b)); +} + +static inline double2 +dd_div_d_dd(double a, const double2 b) +{ + return dd_accurate_div(dd_create_d(a), b); +} + +static inline double2 +dd_inv(const double2 a) +{ + return dd_div(DD_C_ONE, a); +} + + +/********** Remainder **********/ +static inline double2 +dd_drem(const double2 a, const double2 b) +{ + double2 n = dd_nint(dd_div(a, b)); + return dd_sub(a, dd_mul(n, b)); +} + +static inline double2 +dd_divrem(const double2 a, const double2 b, double2 *r) +{ + double2 n = dd_nint(dd_div(a, b)); + *r = dd_sub(a, dd_mul(n, b)); + return n; +} + +static inline double2 +dd_fmod(const double2 a, const double2 b) +{ + double2 n = dd_aint(dd_div(a, b)); + return dd_sub(a, dd_mul(b, n)); +} + +/*********** Squaring **********/ +static inline double2 +dd_sqr(const double2 a) +{ + double p1, p2; + double s1, s2; + p1 = two_sqr(a.x[0], &p2); + p2 += 2.0 * a.x[0] * a.x[1]; + p2 += a.x[1] * a.x[1]; + s1 = quick_two_sum(p1, p2, &s2); + return dd_create(s1, s2); +} + +static inline double2 +dd_sqr_d(double a) +{ + double p1, p2; + p1 = two_sqr(a, &p2); + return dd_create(p1, p2); +} + +#ifdef __cplusplus +} +#endif + +#endif /* _DD_REAL_IDEFS_H_ */ diff --git a/gtsam/3rdparty/cephes/cephes/ellie.c b/gtsam/3rdparty/cephes/cephes/ellie.c new file mode 100644 index 000000000..8a2823f3a --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellie.c @@ -0,0 +1,282 @@ +/* ellie.c + * + * Incomplete elliptic integral of the second kind + * + * + * + * SYNOPSIS: + * + * double phi, m, y, ellie(); + * + * y = ellie( phi, m ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * + * phi + * - + * | | + * | 2 + * E(phi_\m) = | sqrt( 1 - m sin t ) dt + * | + * | | + * - + * 0 + * + * of amplitude phi and modulus m, using the arithmetic - + * geometric mean algorithm. + * + * + * + * ACCURACY: + * + * Tested at random arguments with phi in [-10, 10] and m in + * [0, 1]. + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -10,10 150000 3.3e-15 1.4e-16 + */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987, 1993 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ +/* Copyright 2014, Eric W. Moore */ + +/* Incomplete elliptic integral of second kind */ + +#include "mconf.h" + +extern double MACHEP; + +static double ellie_neg_m(double phi, double m); + +double ellie(double phi, double m) +{ + double a, b, c, e, temp; + double lphi, t, E, denom, npio2; + int d, mod, sign; + + if (cephes_isnan(phi) || cephes_isnan(m)) + return NAN; + if (m > 1.0) + return NAN; + if (cephes_isinf(phi)) + return phi; + if (cephes_isinf(m)) + return -m; + if (m == 0.0) + return (phi); + lphi = phi; + npio2 = floor(lphi / M_PI_2); + if (fmod(fabs(npio2), 2.0) == 1.0) + npio2 += 1; + lphi = lphi - npio2 * M_PI_2; + if (lphi < 0.0) { + lphi = -lphi; + sign = -1; + } + else { + sign = 1; + } + a = 1.0 - m; + E = ellpe(m); + if (a == 0.0) { + temp = sin(lphi); + goto done; + } + if (a > 1.0) { + temp = ellie_neg_m(lphi, m); + goto done; + } + + if (lphi < 0.135) { + double m11= (((((-7.0/2816.0)*m + (5.0/1056.0))*m - (7.0/2640.0))*m + + (17.0/41580.0))*m - (1.0/155925.0))*m; + double m9 = ((((-5.0/1152.0)*m + (1.0/144.0))*m - (1.0/360.0))*m + + (1.0/5670.0))*m; + double m7 = ((-m/112.0 + (1.0/84.0))*m - (1.0/315.0))*m; + double m5 = (-m/40.0 + (1.0/30))*m; + double m3 = -m/6.0; + double p2 = lphi * lphi; + + temp = ((((m11*p2 + m9)*p2 + m7)*p2 + m5)*p2 + m3)*p2*lphi + lphi; + goto done; + } + t = tan(lphi); + b = sqrt(a); + /* Thanks to Brian Fitzgerald + * for pointing out an instability near odd multiples of pi/2. */ + if (fabs(t) > 10.0) { + /* Transform the amplitude */ + e = 1.0 / (b * t); + /* ... but avoid multiple recursions. */ + if (fabs(e) < 10.0) { + e = atan(e); + temp = E + m * sin(lphi) * sin(e) - ellie(e, m); + goto done; + } + } + c = sqrt(m); + a = 1.0; + d = 1; + e = 0.0; + mod = 0; + + while (fabs(c / a) > MACHEP) { + temp = b / a; + lphi = lphi + atan(t * temp) + mod * M_PI; + denom = 1 - temp * t * t; + if (fabs(denom) > 10*MACHEP) { + t = t * (1.0 + temp) / denom; + mod = (lphi + M_PI_2) / M_PI; + } + else { + t = tan(lphi); + mod = (int)floor((lphi - atan(t))/M_PI); + } + c = (a - b) / 2.0; + temp = sqrt(a * b); + a = (a + b) / 2.0; + b = temp; + d += d; + e += c * sin(lphi); + } + + temp = E / ellpk(1.0 - m); + temp *= (atan(t) + mod * M_PI) / (d * a); + temp += e; + + done: + + if (sign < 0) + temp = -temp; + temp += npio2 * E; + return (temp); +} + +/* N.B. This will evaluate its arguments multiple times. */ +#define MAX3(a, b, c) (a > b ? (a > c ? a : c) : (b > c ? b : c)) + +/* To calculate legendre's incomplete elliptical integral of the second kind for + * negative m, we use a power series in phi for small m*phi*phi, an asymptotic + * series in m for large m*phi*phi* and the relation to Carlson's symmetric + * integrals, R_F(x,y,z) and R_D(x,y,z). + * + * E(phi, m) = sin(phi) * R_F(cos(phi)^2, 1 - m * sin(phi)^2, 1.0) + * - m * sin(phi)^3 * R_D(cos(phi)^2, 1 - m * sin(phi)^2, 1.0) / 3 + * + * = R_F(c-1, c-m, c) - m * R_D(c-1, c-m, c) / 3 + * + * where c = csc(phi)^2. We use the second form of this for (approximately) + * phi > 1/(sqrt(DBL_MAX) ~ 1e-154, where csc(phi)^2 overflows. Elsewhere we + * use the first form, accounting for the smallness of phi. + * + * The algorithm used is described in Carlson, B. C. Numerical computation of + * real or complex elliptic integrals. (1994) https://arxiv.org/abs/math/9409227 + * Most variable names reflect Carlson's usage. + * + * In this routine, we assume m < 0 and 0 > phi > pi/2. + */ +double ellie_neg_m(double phi, double m) +{ + double x, y, z, x1, y1, z1, ret, Q; + double A0f, Af, Xf, Yf, Zf, E2f, E3f, scalef; + double A0d, Ad, seriesn, seriesd, Xd, Yd, Zd, E2d, E3d, E4d, E5d, scaled; + int n = 0; + double mpp = (m*phi)*phi; + + if (-mpp < 1e-6 && phi < -m) { + return phi + (mpp*phi*phi/30.0 - mpp*mpp/40.0 - mpp/6.0)*phi; + } + + if (-mpp > 1e6) { + double sm = sqrt(-m); + double sp = sin(phi); + double cp = cos(phi); + + double a = -cosm1(phi); + double b1 = log(4*sp*sm/(1+cp)); + double b = -(0.5 + b1) / 2.0 / m; + double c = (0.75 + cp/sp/sp - b1) / 16.0 / m / m; + return (a + b + c) * sm; + } + + if (phi > 1e-153 && m > -1e200) { + double s = sin(phi); + double csc2 = 1.0 / s / s; + scalef = 1.0; + scaled = m / 3.0; + x = 1.0 / tan(phi) / tan(phi); + y = csc2 - m; + z = csc2; + } + else { + scalef = phi; + scaled = mpp * phi / 3.0; + x = 1.0; + y = 1 - mpp; + z = 1.0; + } + + if (x == y && x == z) { + return (scalef + scaled/x)/sqrt(x); + } + + A0f = (x + y + z) / 3.0; + Af = A0f; + A0d = (x + y + 3.0*z) / 5.0; + Ad = A0d; + x1 = x; y1 = y; z1 = z; seriesd = 0.0; seriesn = 1.0; + /* Carlson gives 1/pow(3*r, 1.0/6.0) for this constant. if r == eps, + * it is ~338.38. */ + Q = 400.0 * MAX3(fabs(A0f-x), fabs(A0f-y), fabs(A0f-z)); + + while (Q > fabs(Af) && Q > fabs(Ad) && n <= 100) { + double sx = sqrt(x1); + double sy = sqrt(y1); + double sz = sqrt(z1); + double lam = sx*sy + sx*sz + sy*sz; + seriesd += seriesn / (sz * (z1 + lam)); + x1 = (x1 + lam) / 4.0; + y1 = (y1 + lam) / 4.0; + z1 = (z1 + lam) / 4.0; + Af = (x1 + y1 + z1) / 3.0; + Ad = (Ad + lam) / 4.0; + n += 1; + Q /= 4.0; + seriesn /= 4.0; + } + + Xf = (A0f - x) / Af / (1 << 2*n); + Yf = (A0f - y) / Af / (1 << 2*n); + Zf = -(Xf + Yf); + + E2f = Xf*Yf - Zf*Zf; + E3f = Xf*Yf*Zf; + + ret = scalef * (1.0 - E2f/10.0 + E3f/14.0 + E2f*E2f/24.0 + - 3.0*E2f*E3f/44.0) / sqrt(Af); + + Xd = (A0d - x) / Ad / (1 << 2*n); + Yd = (A0d - y) / Ad / (1 << 2*n); + Zd = -(Xd + Yd)/3.0; + + E2d = Xd*Yd - 6.0*Zd*Zd; + E3d = (3*Xd*Yd - 8.0*Zd*Zd)*Zd; + E4d = 3.0*(Xd*Yd - Zd*Zd)*Zd*Zd; + E5d = Xd*Yd*Zd*Zd*Zd; + + ret -= scaled * (1.0 - 3.0*E2d/14.0 + E3d/6.0 + 9.0*E2d*E2d/88.0 + - 3.0*E4d/22.0 - 9.0*E2d*E3d/52.0 + 3.0*E5d/26.0) + /(1 << 2*n) / Ad / sqrt(Ad); + ret -= 3.0 * scaled * seriesd; + return ret; +} + diff --git a/gtsam/3rdparty/cephes/cephes/ellik.c b/gtsam/3rdparty/cephes/cephes/ellik.c new file mode 100644 index 000000000..ee73e062a --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellik.c @@ -0,0 +1,246 @@ +/* ellik.c + * + * Incomplete elliptic integral of the first kind + * + * + * + * SYNOPSIS: + * + * double phi, m, y, ellik(); + * + * y = ellik( phi, m ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * + * + * phi + * - + * | | + * | dt + * F(phi | m) = | ------------------ + * | 2 + * | | sqrt( 1 - m sin t ) + * - + * 0 + * + * of amplitude phi and modulus m, using the arithmetic - + * geometric mean algorithm. + * + * + * + * + * ACCURACY: + * + * Tested at random points with m in [0, 1] and phi as indicated. + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -10,10 200000 7.4e-16 1.0e-16 + * + * + */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ +/* Copyright 2014, Eric W. Moore */ + +/* Incomplete elliptic integral of first kind */ + +#include "mconf.h" +extern double MACHEP; + +static double ellik_neg_m(double phi, double m); + +double ellik(double phi, double m) +{ + double a, b, c, e, temp, t, K, denom, npio2; + int d, mod, sign; + + if (cephes_isnan(phi) || cephes_isnan(m)) + return NAN; + if (m > 1.0) + return NAN; + if (cephes_isinf(phi) || cephes_isinf(m)) + { + if (cephes_isinf(m) && cephes_isfinite(phi)) + return 0.0; + else if (cephes_isinf(phi) && cephes_isfinite(m)) + return phi; + else + return NAN; + } + if (m == 0.0) + return (phi); + a = 1.0 - m; + if (a == 0.0) { + if (fabs(phi) >= (double)M_PI_2) { + sf_error("ellik", SF_ERROR_SINGULAR, NULL); + return (INFINITY); + } + /* DLMF 19.6.8, and 4.23.42 */ + return asinh(tan(phi)); + } + npio2 = floor(phi / M_PI_2); + if (fmod(fabs(npio2), 2.0) == 1.0) + npio2 += 1; + if (npio2 != 0.0) { + K = ellpk(a); + phi = phi - npio2 * M_PI_2; + } + else + K = 0.0; + if (phi < 0.0) { + phi = -phi; + sign = -1; + } + else + sign = 0; + if (a > 1.0) { + temp = ellik_neg_m(phi, m); + goto done; + } + b = sqrt(a); + t = tan(phi); + if (fabs(t) > 10.0) { + /* Transform the amplitude */ + e = 1.0 / (b * t); + /* ... but avoid multiple recursions. */ + if (fabs(e) < 10.0) { + e = atan(e); + if (npio2 == 0) + K = ellpk(a); + temp = K - ellik(e, m); + goto done; + } + } + a = 1.0; + c = sqrt(m); + d = 1; + mod = 0; + + while (fabs(c / a) > MACHEP) { + temp = b / a; + phi = phi + atan(t * temp) + mod * M_PI; + denom = 1.0 - temp * t * t; + if (fabs(denom) > 10*MACHEP) { + t = t * (1.0 + temp) / denom; + mod = (phi + M_PI_2) / M_PI; + } + else { + t = tan(phi); + mod = (int)floor((phi - atan(t))/M_PI); + } + c = (a - b) / 2.0; + temp = sqrt(a * b); + a = (a + b) / 2.0; + b = temp; + d += d; + } + + temp = (atan(t) + mod * M_PI) / (d * a); + + done: + if (sign < 0) + temp = -temp; + temp += npio2 * K; + return (temp); +} + +/* N.B. This will evaluate its arguments multiple times. */ +#define MAX3(a, b, c) (a > b ? (a > c ? a : c) : (b > c ? b : c)) + +/* To calculate legendre's incomplete elliptical integral of the first kind for + * negative m, we use a power series in phi for small m*phi*phi, an asymptotic + * series in m for large m*phi*phi* and the relation to Carlson's symmetric + * integral of the first kind. + * + * F(phi, m) = sin(phi) * R_F(cos(phi)^2, 1 - m * sin(phi)^2, 1.0) + * = R_F(c-1, c-m, c) + * + * where c = csc(phi)^2. We use the second form of this for (approximately) + * phi > 1/(sqrt(DBL_MAX) ~ 1e-154, where csc(phi)^2 overflows. Elsewhere we + * use the first form, accounting for the smallness of phi. + * + * The algorithm used is described in Carlson, B. C. Numerical computation of + * real or complex elliptic integrals. (1994) https://arxiv.org/abs/math/9409227 + * Most variable names reflect Carlson's usage. + * + * In this routine, we assume m < 0 and 0 > phi > pi/2. + */ +double ellik_neg_m(double phi, double m) +{ + double x, y, z, x1, y1, z1, A0, A, Q, X, Y, Z, E2, E3, scale; + int n = 0; + double mpp = (m*phi)*phi; + + if (-mpp < 1e-6 && phi < -m) { + return phi + (-mpp*phi*phi/30.0 + 3.0*mpp*mpp/40.0 + mpp/6.0)*phi; + } + + if (-mpp > 4e7) { + double sm = sqrt(-m); + double sp = sin(phi); + double cp = cos(phi); + + double a = log(4*sp*sm/(1+cp)); + double b = -(1 + cp/sp/sp - a) / 4 / m; + return (a + b) / sm; + } + + if (phi > 1e-153 && m > -1e305) { + double s = sin(phi); + double csc2 = 1.0 / (s*s); + scale = 1.0; + x = 1.0 / (tan(phi) * tan(phi)); + y = csc2 - m; + z = csc2; + } + else { + scale = phi; + x = 1.0; + y = 1 - m*scale*scale; + z = 1.0; + } + + if (x == y && x == z) { + return scale / sqrt(x); + } + + A0 = (x + y + z) / 3.0; + A = A0; + x1 = x; y1 = y; z1 = z; + /* Carlson gives 1/pow(3*r, 1.0/6.0) for this constant. if r == eps, + * it is ~338.38. */ + Q = 400.0 * MAX3(fabs(A0-x), fabs(A0-y), fabs(A0-z)); + + while (Q > fabs(A) && n <= 100) { + double sx = sqrt(x1); + double sy = sqrt(y1); + double sz = sqrt(z1); + double lam = sx*sy + sx*sz + sy*sz; + x1 = (x1 + lam) / 4.0; + y1 = (y1 + lam) / 4.0; + z1 = (z1 + lam) / 4.0; + A = (x1 + y1 + z1) / 3.0; + n += 1; + Q /= 4; + } + X = (A0 - x) / A / (1 << 2*n); + Y = (A0 - y) / A / (1 << 2*n); + Z = -(X + Y); + + E2 = X*Y - Z*Z; + E3 = X*Y*Z; + + return scale * (1.0 - E2/10.0 + E3/14.0 + E2*E2/24.0 + - 3.0*E2*E3/44.0) / sqrt(A); +} diff --git a/gtsam/3rdparty/cephes/cephes/ellpe.c b/gtsam/3rdparty/cephes/cephes/ellpe.c new file mode 100644 index 000000000..1ef8e0c12 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellpe.c @@ -0,0 +1,108 @@ +/* ellpe.c + * + * Complete elliptic integral of the second kind + * + * + * + * SYNOPSIS: + * + * double m, y, ellpe(); + * + * y = ellpe( m ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * + * pi/2 + * - + * | | 2 + * E(m) = | sqrt( 1 - m sin t ) dt + * | | + * - + * 0 + * + * Where m = 1 - m1, using the approximation + * + * P(x) - x log x Q(x). + * + * Though there are no singularities, the argument m1 is used + * internally rather than m for compatibility with ellpk(). + * + * E(1) = 1; E(0) = pi/2. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 1 10000 2.1e-16 7.3e-17 + * + * + * ERROR MESSAGES: + * + * message condition value returned + * ellpe domain x<0, x>1 0.0 + * + */ + +/* ellpe.c */ + +/* Elliptic integral of second kind */ + +/* + * Cephes Math Library, Release 2.1: February, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + * + * Feb, 2002: altered by Travis Oliphant + * so that it is called with argument m + * (which gets immediately converted to m1 = 1-m) + */ + +#include "mconf.h" + +static double P[] = { + 1.53552577301013293365E-4, + 2.50888492163602060990E-3, + 8.68786816565889628429E-3, + 1.07350949056076193403E-2, + 7.77395492516787092951E-3, + 7.58395289413514708519E-3, + 1.15688436810574127319E-2, + 2.18317996015557253103E-2, + 5.68051945617860553470E-2, + 4.43147180560990850618E-1, + 1.00000000000000000299E0 +}; + +static double Q[] = { + 3.27954898576485872656E-5, + 1.00962792679356715133E-3, + 6.50609489976927491433E-3, + 1.68862163993311317300E-2, + 2.61769742454493659583E-2, + 3.34833904888224918614E-2, + 4.27180926518931511717E-2, + 5.85936634471101055642E-2, + 9.37499997197644278445E-2, + 2.49999999999888314361E-1 +}; + +double ellpe(double x) +{ + x = 1.0 - x; + if (x <= 0.0) { + if (x == 0.0) + return (1.0); + sf_error("ellpe", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (x > 1.0) { + return ellpe(1.0 - 1/x) * sqrt(x); + } + return (polevl(x, P, 10) - log(x) * (x * polevl(x, Q, 9))); +} diff --git a/gtsam/3rdparty/cephes/cephes/ellpj.c b/gtsam/3rdparty/cephes/cephes/ellpj.c new file mode 100644 index 000000000..6891a8244 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellpj.c @@ -0,0 +1,154 @@ +/* ellpj.c + * + * Jacobian Elliptic Functions + * + * + * + * SYNOPSIS: + * + * double u, m, sn, cn, dn, phi; + * int ellpj(); + * + * ellpj( u, m, _&sn, _&cn, _&dn, _&phi ); + * + * + * + * DESCRIPTION: + * + * + * Evaluates the Jacobian elliptic functions sn(u|m), cn(u|m), + * and dn(u|m) of parameter m between 0 and 1, and real + * argument u. + * + * These functions are periodic, with quarter-period on the + * real axis equal to the complete elliptic integral + * ellpk(m). + * + * Relation to incomplete elliptic integral: + * If u = ellik(phi,m), then sn(u|m) = sin(phi), + * and cn(u|m) = cos(phi). Phi is called the amplitude of u. + * + * Computation is by means of the arithmetic-geometric mean + * algorithm, except when m is within 1e-9 of 0 or 1. In the + * latter case with m close to 1, the approximation applies + * only for phi < pi/2. + * + * ACCURACY: + * + * Tested at random points with u between 0 and 10, m between + * 0 and 1. + * + * Absolute error (* = relative error): + * arithmetic function # trials peak rms + * IEEE phi 10000 9.2e-16* 1.4e-16* + * IEEE sn 50000 4.1e-15 4.6e-16 + * IEEE cn 40000 3.6e-15 4.4e-16 + * IEEE dn 10000 1.3e-12 1.8e-14 + * + * Peak error observed in consistency check using addition + * theorem for sn(u+v) was 4e-16 (absolute). Also tested by + * the above relation to the incomplete elliptic integral. + * Accuracy deteriorates when u is large. + * + */ + +/* ellpj.c */ + + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +/* Scipy changes: + * - 07-18-2016: improve evaluation of dn near quarter periods + */ + +#include "mconf.h" +extern double MACHEP; + +int ellpj(double u, double m, double *sn, double *cn, double *dn, double *ph) +{ + double ai, b, phi, t, twon, dnfac; + double a[9], c[9]; + int i; + + /* Check for special cases */ + if (m < 0.0 || m > 1.0 || cephes_isnan(m)) { + sf_error("ellpj", SF_ERROR_DOMAIN, NULL); + *sn = NAN; + *cn = NAN; + *ph = NAN; + *dn = NAN; + return (-1); + } + if (m < 1.0e-9) { + t = sin(u); + b = cos(u); + ai = 0.25 * m * (u - t * b); + *sn = t - ai * b; + *cn = b + ai * t; + *ph = u - ai; + *dn = 1.0 - 0.5 * m * t * t; + return (0); + } + if (m >= 0.9999999999) { + ai = 0.25 * (1.0 - m); + b = cosh(u); + t = tanh(u); + phi = 1.0 / b; + twon = b * sinh(u); + *sn = t + ai * (twon - u) / (b * b); + *ph = 2.0 * atan(exp(u)) - M_PI_2 + ai * (twon - u) / b; + ai *= t * phi; + *cn = phi - ai * (twon - u); + *dn = phi + ai * (twon + u); + return (0); + } + + /* A. G. M. scale. See DLMF 22.20(ii) */ + a[0] = 1.0; + b = sqrt(1.0 - m); + c[0] = sqrt(m); + twon = 1.0; + i = 0; + + while (fabs(c[i] / a[i]) > MACHEP) { + if (i > 7) { + sf_error("ellpj", SF_ERROR_OVERFLOW, NULL); + goto done; + } + ai = a[i]; + ++i; + c[i] = (ai - b) / 2.0; + t = sqrt(ai * b); + a[i] = (ai + b) / 2.0; + b = t; + twon *= 2.0; + } + + done: + /* backward recurrence */ + phi = twon * a[i] * u; + do { + t = c[i] * sin(phi) / a[i]; + b = phi; + phi = (asin(t) + phi) / 2.0; + } + while (--i); + + *sn = sin(phi); + t = cos(phi); + *cn = t; + dnfac = cos(phi - b); + /* See discussion after DLMF 22.20.5 */ + if (fabs(dnfac) < 0.1) { + *dn = sqrt(1 - m*(*sn)*(*sn)); + } + else { + *dn = t / dnfac; + } + *ph = phi; + return (0); +} diff --git a/gtsam/3rdparty/cephes/cephes/ellpk.c b/gtsam/3rdparty/cephes/cephes/ellpk.c new file mode 100644 index 000000000..3842a7403 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ellpk.c @@ -0,0 +1,124 @@ +/* ellpk.c + * + * Complete elliptic integral of the first kind + * + * + * + * SYNOPSIS: + * + * double m1, y, ellpk(); + * + * y = ellpk( m1 ); + * + * + * + * DESCRIPTION: + * + * Approximates the integral + * + * + * + * pi/2 + * - + * | | + * | dt + * K(m) = | ------------------ + * | 2 + * | | sqrt( 1 - m sin t ) + * - + * 0 + * + * where m = 1 - m1, using the approximation + * + * P(x) - log x Q(x). + * + * The argument m1 is used internally rather than m so that the logarithmic + * singularity at m = 1 will be shifted to the origin; this + * preserves maximum accuracy. + * + * K(0) = pi/2. + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,1 30000 2.5e-16 6.8e-17 + * + * ERROR MESSAGES: + * + * message condition value returned + * ellpk domain x<0, x>1 0.0 + * + */ + +/* ellpk.c */ + + +/* + * Cephes Math Library, Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +static double P[] = { + 1.37982864606273237150E-4, + 2.28025724005875567385E-3, + 7.97404013220415179367E-3, + 9.85821379021226008714E-3, + 6.87489687449949877925E-3, + 6.18901033637687613229E-3, + 8.79078273952743772254E-3, + 1.49380448916805252718E-2, + 3.08851465246711995998E-2, + 9.65735902811690126535E-2, + 1.38629436111989062502E0 +}; + +static double Q[] = { + 2.94078955048598507511E-5, + 9.14184723865917226571E-4, + 5.94058303753167793257E-3, + 1.54850516649762399335E-2, + 2.39089602715924892727E-2, + 3.01204715227604046988E-2, + 3.73774314173823228969E-2, + 4.88280347570998239232E-2, + 7.03124996963957469739E-2, + 1.24999999999870820058E-1, + 4.99999999999999999821E-1 +}; + +static double C1 = 1.3862943611198906188E0; /* log(4) */ + +extern double MACHEP; + +double ellpk(double x) +{ + + if (x < 0.0) { + sf_error("ellpk", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + if (x > 1.0) { + if (cephes_isinf(x)) { + return 0.0; + } + return ellpk(1/x)/sqrt(x); + } + + if (x > MACHEP) { + return (polevl(x, P, 10) - log(x) * polevl(x, Q, 10)); + } + else { + if (x == 0.0) { + sf_error("ellpk", SF_ERROR_SINGULAR, NULL); + return (INFINITY); + } + else { + return (C1 - 0.5 * log(x)); + } + } +} diff --git a/gtsam/3rdparty/cephes/cephes/erfinv.c b/gtsam/3rdparty/cephes/cephes/erfinv.c new file mode 100644 index 000000000..f7f49284c --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/erfinv.c @@ -0,0 +1,78 @@ +/* + * mconf configures NANS, INFINITYs etc. for cephes and includes some standard + * headers. Although erfinv and erfcinv are not defined in cephes, erf and erfc + * are. We want to keep the behaviour consistent for the inverse functions and + * so need to include mconf. + */ +#include "mconf.h" + +/* + * Inverse of the error function. + * + * Computes the inverse of the error function on the restricted domain + * -1 < y < 1. This restriction ensures the existence of a unique result + * such that erf(erfinv(y)) = y. + */ +double erfinv(double y) { + const double domain_lb = -1; + const double domain_ub = 1; + + const double thresh = 1e-7; + + /* + * For small arguments, use the Taylor expansion + * erf(y) = 2/\sqrt{\pi} (y - y^3 / 3 + O(y^5)), y\to 0 + * where we only retain the linear term. + * Otherwise, y + 1 loses precision for |y| << 1. + */ + if ((-thresh < y) && (y < thresh)){ + return y / M_2_SQRTPI; + } + if ((domain_lb < y) && (y < domain_ub)) { + return ndtri(0.5 * (y+1)) * M_SQRT1_2; + } + else if (y == domain_lb) { + return -INFINITY; + } + else if (y == domain_ub) { + return INFINITY; + } + else if (cephes_isnan(y)) { + sf_error("erfinv", SF_ERROR_DOMAIN, NULL); + return y; + } + else { + sf_error("erfinv", SF_ERROR_DOMAIN, NULL); + return NAN; + } +} + +/* + * Inverse of the complementary error function. + * + * Computes the inverse of the complimentary error function on the restricted + * domain 0 < y < 2. This restriction ensures the existence of a unique result + * such that erfc(erfcinv(y)) = y. + */ +double erfcinv(double y) { + const double domain_lb = 0; + const double domain_ub = 2; + + if ((domain_lb < y) && (y < domain_ub)) { + return -ndtri(0.5 * y) * M_SQRT1_2; + } + else if (y == domain_lb) { + return INFINITY; + } + else if (y == domain_ub) { + return -INFINITY; + } + else if (cephes_isnan(y)) { + sf_error("erfcinv", SF_ERROR_DOMAIN, NULL); + return y; + } + else { + sf_error("erfcinv", SF_ERROR_DOMAIN, NULL); + return NAN; + } +} diff --git a/gtsam/3rdparty/cephes/cephes/exp10.c b/gtsam/3rdparty/cephes/cephes/exp10.c new file mode 100644 index 000000000..0a71d3c52 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/exp10.c @@ -0,0 +1,115 @@ +/* exp10.c + * + * Base 10 exponential function + * (Common antilogarithm) + * + * + * + * SYNOPSIS: + * + * double x, y, exp10(); + * + * y = exp10( x ); + * + * + * + * DESCRIPTION: + * + * Returns 10 raised to the x power. + * + * Range reduction is accomplished by expressing the argument + * as 10**x = 2**n 10**f, with |f| < 0.5 log10(2). + * The Pade' form + * + * 1 + 2x P(x**2)/( Q(x**2) - P(x**2) ) + * + * is used to approximate 10**f. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -307,+307 30000 2.2e-16 5.5e-17 + * + * ERROR MESSAGES: + * + * message condition value returned + * exp10 underflow x < -MAXL10 0.0 + * exp10 overflow x > MAXL10 INFINITY + * + * IEEE arithmetic: MAXL10 = 308.2547155599167. + * + */ + +/* + * Cephes Math Library Release 2.2: January, 1991 + * Copyright 1984, 1991 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + + +#include "mconf.h" + +static double P[] = { + 4.09962519798587023075E-2, + 1.17452732554344059015E1, + 4.06717289936872725516E2, + 2.39423741207388267439E3, +}; + +static double Q[] = { + /* 1.00000000000000000000E0, */ + 8.50936160849306532625E1, + 1.27209271178345121210E3, + 2.07960819286001865907E3, +}; + +/* static double LOG102 = 3.01029995663981195214e-1; */ +static double LOG210 = 3.32192809488736234787e0; +static double LG102A = 3.01025390625000000000E-1; +static double LG102B = 4.60503898119521373889E-6; + +/* static double MAXL10 = 38.230809449325611792; */ +static double MAXL10 = 308.2547155599167; + +double exp10(double x) +{ + double px, xx; + short n; + + if (cephes_isnan(x)) + return (x); + if (x > MAXL10) { + return (INFINITY); + } + + if (x < -MAXL10) { /* Would like to use MINLOG but can't */ + sf_error("exp10", SF_ERROR_UNDERFLOW, NULL); + return (0.0); + } + + /* Express 10**x = 10**g 2**n + * = 10**g 10**( n log10(2) ) + * = 10**( g + n log10(2) ) + */ + px = floor(LOG210 * x + 0.5); + n = px; + x -= px * LG102A; + x -= px * LG102B; + + /* rational approximation for exponential + * of the fractional part: + * 10**x = 1 + 2x P(x**2)/( Q(x**2) - P(x**2) ) + */ + xx = x * x; + px = x * polevl(xx, P, 3); + x = px / (p1evl(xx, Q, 3) - px); + x = 1.0 + ldexp(x, 1); + + /* multiply by power of 2 */ + x = ldexp(x, n); + + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/exp2.c b/gtsam/3rdparty/cephes/cephes/exp2.c new file mode 100644 index 000000000..14911f59c --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/exp2.c @@ -0,0 +1,108 @@ +/* exp2.c + * + * Base 2 exponential function + * + * + * + * SYNOPSIS: + * + * double x, y, exp2(); + * + * y = exp2( x ); + * + * + * + * DESCRIPTION: + * + * Returns 2 raised to the x power. + * + * Range reduction is accomplished by separating the argument + * into an integer k and fraction f such that + * x k f + * 2 = 2 2. + * + * A Pade' form + * + * 1 + 2x P(x**2) / (Q(x**2) - x P(x**2) ) + * + * approximates 2**x in the basic range [-0.5, 0.5]. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -1022,+1024 30000 1.8e-16 5.4e-17 + * + * + * See exp.c for comments on error amplification. + * + * + * ERROR MESSAGES: + * + * message condition value returned + * exp underflow x < -MAXL2 0.0 + * exp overflow x > MAXL2 INFINITY + * + * For IEEE arithmetic, MAXL2 = 1024. + */ + + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1995 by Stephen L. Moshier + */ + + + +#include "mconf.h" + +static double P[] = { + 2.30933477057345225087E-2, + 2.02020656693165307700E1, + 1.51390680115615096133E3, +}; + +static double Q[] = { + /* 1.00000000000000000000E0, */ + 2.33184211722314911771E2, + 4.36821166879210612817E3, +}; + +#define MAXL2 1024.0 +#define MINL2 -1024.0 + +double exp2(double x) +{ + double px, xx; + short n; + + if (cephes_isnan(x)) + return (x); + if (x > MAXL2) { + return (INFINITY); + } + + if (x < MINL2) { + return (0.0); + } + + xx = x; /* save x */ + /* separate into integer and fractional parts */ + px = floor(x + 0.5); + n = px; + x = x - px; + + /* rational approximation + * exp2(x) = 1 + 2xP(xx)/(Q(xx) - P(xx)) + * where xx = x**2 + */ + xx = x * x; + px = x * polevl(xx, P, 2); + x = px / (p1evl(xx, Q, 2) - px); + x = 1.0 + ldexp(x, 1); + + /* scale by power of 2 */ + x = ldexp(x, n); + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/expn.c b/gtsam/3rdparty/cephes/cephes/expn.c new file mode 100644 index 000000000..2a6ee14c0 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/expn.c @@ -0,0 +1,224 @@ +/* expn.c + * + * Exponential integral En + * + * + * + * SYNOPSIS: + * + * int n; + * double x, y, expn(); + * + * y = expn( n, x ); + * + * + * + * DESCRIPTION: + * + * Evaluates the exponential integral + * + * inf. + * - + * | | -xt + * | e + * E (x) = | ---- dt. + * n | n + * | | t + * - + * 1 + * + * + * Both n and x must be nonnegative. + * + * The routine employs either a power series, a continued + * fraction, or an asymptotic formula depending on the + * relative values of n and x. + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 10000 1.7e-15 3.6e-16 + * + */ + +/* expn.c */ + +/* Cephes Math Library Release 1.1: March, 1985 + * Copyright 1985 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 */ + +/* Sources + * [1] NIST, "The Digital Library of Mathematical Functions", dlmf.nist.gov + */ + +/* Scipy changes: + * - 09-10-2016: improved asymptotic expansion for large n + */ + +#include "mconf.h" +#include "polevl.h" +#include "expn.h" + +#define EUL 0.57721566490153286060 +#define BIG 1.44115188075855872E+17 +extern double MACHEP, MAXLOG; + +static double expn_large_n(int, double); + + +double expn(int n, double x) +{ + double ans, r, t, yk, xk; + double pk, pkm1, pkm2, qk, qkm1, qkm2; + double psi, z; + int i, k; + static double big = BIG; + + if (isnan(x)) { + return NAN; + } + else if (n < 0 || x < 0) { + sf_error("expn", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x > MAXLOG) { + return (0.0); + } + + if (x == 0.0) { + if (n < 2) { + sf_error("expn", SF_ERROR_SINGULAR, NULL); + return (INFINITY); + } + else { + return (1.0 / (n - 1.0)); + } + } + + if (n == 0) { + return (exp(-x) / x); + } + + /* Asymptotic expansion for large n, DLMF 8.20(ii) */ + if (n > 50) { + ans = expn_large_n(n, x); + goto done; + } + + if (x > 1.0) { + goto cfrac; + } + + /* Power series expansion, DLMF 8.19.8 */ + psi = -EUL - log(x); + for (i = 1; i < n; i++) { + psi = psi + 1.0 / i; + } + + z = -x; + xk = 0.0; + yk = 1.0; + pk = 1.0 - n; + if (n == 1) { + ans = 0.0; + } else { + ans = 1.0 / pk; + } + do { + xk += 1.0; + yk *= z / xk; + pk += 1.0; + if (pk != 0.0) { + ans += yk / pk; + } + if (ans != 0.0) + t = fabs(yk / ans); + else + t = 1.0; + } while (t > MACHEP); + k = xk; + t = n; + r = n - 1; + ans = (pow(z, r) * psi / Gamma(t)) - ans; + goto done; + + /* Continued fraction, DLMF 8.19.17 */ + cfrac: + k = 1; + pkm2 = 1.0; + qkm2 = x; + pkm1 = 1.0; + qkm1 = x + n; + ans = pkm1 / qkm1; + + do { + k += 1; + if (k & 1) { + yk = 1.0; + xk = n + (k - 1) / 2; + } else { + yk = x; + xk = k / 2; + } + pk = pkm1 * yk + pkm2 * xk; + qk = qkm1 * yk + qkm2 * xk; + if (qk != 0) { + r = pk / qk; + t = fabs((ans - r) / r); + ans = r; + } else { + t = 1.0; + } + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + if (fabs(pk) > big) { + pkm2 /= big; + pkm1 /= big; + qkm2 /= big; + qkm1 /= big; + } + } while (t > MACHEP); + + ans *= exp(-x); + + done: + return (ans); +} + + +/* Asymptotic expansion for large n, DLMF 8.20(ii) */ +static double expn_large_n(int n, double x) +{ + int k; + double p = n; + double lambda = x/p; + double multiplier = 1/p/(lambda + 1)/(lambda + 1); + double fac = 1; + double res = 1; /* A[0] = 1 */ + double expfac, term; + + expfac = exp(-lambda*p)/(lambda + 1)/p; + if (expfac == 0) { + sf_error("expn", SF_ERROR_UNDERFLOW, NULL); + return 0; + } + + /* Do the k = 1 term outside the loop since A[1] = 1 */ + fac *= multiplier; + res += fac; + + for (k = 2; k < nA; k++) { + fac *= multiplier; + term = fac*polevl(lambda, A[k], Adegs[k]); + res += term; + if (fabs(term) < MACHEP*fabs(res)) { + break; + } + } + + return expfac*res; +} diff --git a/gtsam/3rdparty/cephes/cephes/expn.h b/gtsam/3rdparty/cephes/cephes/expn.h new file mode 100644 index 000000000..8ced02687 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/expn.h @@ -0,0 +1,19 @@ +/* This file was automatically generated by _precompute/expn_asy.py. + * Do not edit it manually! + */ +#define nA 13 +static const double A0[] = {1.00000000000000000}; +static const double A1[] = {1.00000000000000000}; +static const double A2[] = {-2.00000000000000000, 1.00000000000000000}; +static const double A3[] = {6.00000000000000000, -8.00000000000000000, 1.00000000000000000}; +static const double A4[] = {-24.0000000000000000, 58.0000000000000000, -22.0000000000000000, 1.00000000000000000}; +static const double A5[] = {120.000000000000000, -444.000000000000000, 328.000000000000000, -52.0000000000000000, 1.00000000000000000}; +static const double A6[] = {-720.000000000000000, 3708.00000000000000, -4400.00000000000000, 1452.00000000000000, -114.000000000000000, 1.00000000000000000}; +static const double A7[] = {5040.00000000000000, -33984.0000000000000, 58140.0000000000000, -32120.0000000000000, 5610.00000000000000, -240.000000000000000, 1.00000000000000000}; +static const double A8[] = {-40320.0000000000000, 341136.000000000000, -785304.000000000000, 644020.000000000000, -195800.000000000000, 19950.0000000000000, -494.000000000000000, 1.00000000000000000}; +static const double A9[] = {362880.000000000000, -3733920.00000000000, 11026296.0000000000, -12440064.0000000000, 5765500.00000000000, -1062500.00000000000, 67260.0000000000000, -1004.00000000000000, 1.00000000000000000}; +static const double A10[] = {-3628800.00000000000, 44339040.0000000000, -162186912.000000000, 238904904.000000000, -155357384.000000000, 44765000.0000000000, -5326160.00000000000, 218848.000000000000, -2026.00000000000000, 1.00000000000000000}; +static const double A11[] = {39916800.0000000000, -568356480.000000000, 2507481216.00000000, -4642163952.00000000, 4002695088.00000000, -1648384304.00000000, 314369720.000000000, -25243904.0000000000, 695038.000000000000, -4072.00000000000000, 1.00000000000000000}; +static const double A12[] = {-479001600.000000000, 7827719040.00000000, -40788301824.0000000, 92199790224.0000000, -101180433024.000000, 56041398784.0000000, -15548960784.0000000, 2051482776.00000000, -114876376.000000000, 2170626.00000000000, -8166.00000000000000, 1.00000000000000000}; +static const double *A[] = {A0, A1, A2, A3, A4, A5, A6, A7, A8, A9, A10, A11, A12}; +static const int Adegs[] = {0, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11}; diff --git a/gtsam/3rdparty/cephes/cephes/fdtr.c b/gtsam/3rdparty/cephes/cephes/fdtr.c new file mode 100644 index 000000000..9c119ed8f --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/fdtr.c @@ -0,0 +1,216 @@ +/* fdtr.c + * + * F distribution + * + * + * + * SYNOPSIS: + * + * double df1, df2; + * double x, y, fdtr(); + * + * y = fdtr( df1, df2, x ); + * + * DESCRIPTION: + * + * Returns the area from zero to x under the F density + * function (also known as Snedcor's density or the + * variance ratio density). This is the density + * of x = (u1/df1)/(u2/df2), where u1 and u2 are random + * variables having Chi square distributions with df1 + * and df2 degrees of freedom, respectively. + * + * The incomplete beta integral is used, according to the + * formula + * + * P(x) = incbet( df1/2, df2/2, (df1*x/(df2 + df1*x) ). + * + * + * The arguments a and b are greater than zero, and x is + * nonnegative. + * + * ACCURACY: + * + * Tested at random points (a,b,x). + * + * x a,b Relative error: + * arithmetic domain domain # trials peak rms + * IEEE 0,1 0,100 100000 9.8e-15 1.7e-15 + * IEEE 1,5 0,100 100000 6.5e-15 3.5e-16 + * IEEE 0,1 1,10000 100000 2.2e-11 3.3e-12 + * IEEE 1,5 1,10000 100000 1.1e-11 1.7e-13 + * See also incbet.c. + * + * + * ERROR MESSAGES: + * + * message condition value returned + * fdtr domain a<0, b<0, x<0 0.0 + * + */ + +/* fdtrc() + * + * Complemented F distribution + * + * + * + * SYNOPSIS: + * + * double df1, df2; + * double x, y, fdtrc(); + * + * y = fdtrc( df1, df2, x ); + * + * DESCRIPTION: + * + * Returns the area from x to infinity under the F density + * function (also known as Snedcor's density or the + * variance ratio density). + * + * + * inf. + * - + * 1 | | a-1 b-1 + * 1-P(x) = ------ | t (1-t) dt + * B(a,b) | | + * - + * x + * + * + * The incomplete beta integral is used, according to the + * formula + * + * P(x) = incbet( df2/2, df1/2, (df2/(df2 + df1*x) ). + * + * + * ACCURACY: + * + * Tested at random points (a,b,x) in the indicated intervals. + * x a,b Relative error: + * arithmetic domain domain # trials peak rms + * IEEE 0,1 1,100 100000 3.7e-14 5.9e-16 + * IEEE 1,5 1,100 100000 8.0e-15 1.6e-15 + * IEEE 0,1 1,10000 100000 1.8e-11 3.5e-13 + * IEEE 1,5 1,10000 100000 2.0e-11 3.0e-12 + * See also incbet.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * fdtrc domain a<0, b<0, x<0 0.0 + * + */ + +/* fdtri() + * + * Inverse of F distribution + * + * + * + * SYNOPSIS: + * + * double df1, df2; + * double x, p, fdtri(); + * + * x = fdtri( df1, df2, p ); + * + * DESCRIPTION: + * + * Finds the F density argument x such that the integral + * from -infinity to x of the F density is equal to the + * given probability p. + * + * This is accomplished using the inverse beta integral + * function and the relations + * + * z = incbi( df2/2, df1/2, p ) + * x = df2 (1-z) / (df1 z). + * + * Note: the following relations hold for the inverse of + * the uncomplemented F distribution: + * + * z = incbi( df1/2, df2/2, p ) + * x = df2 z / (df1 (1-z)). + * + * ACCURACY: + * + * Tested at random points (a,b,p). + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * For p between .001 and 1: + * IEEE 1,100 100000 8.3e-15 4.7e-16 + * IEEE 1,10000 100000 2.1e-11 1.4e-13 + * For p between 10^-6 and 10^-3: + * IEEE 1,100 50000 1.3e-12 8.4e-15 + * IEEE 1,10000 50000 3.0e-12 4.8e-14 + * See also fdtrc.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * fdtri domain p <= 0 or p > 1 NaN + * v < 1 + * + */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + + +#include "mconf.h" + + +double fdtrc(double a, double b, double x) +{ + double w; + + if ((a <= 0.0) || (b <= 0.0) || (x < 0.0)) { + sf_error("fdtrc", SF_ERROR_DOMAIN, NULL); + return NAN; + } + w = b / (b + a * x); + return incbet(0.5 * b, 0.5 * a, w); +} + + +double fdtr(double a, double b, double x) +{ + double w; + + if ((a <= 0.0) || (b <= 0.0) || (x < 0.0)) { + sf_error("fdtr", SF_ERROR_DOMAIN, NULL); + return NAN; + } + w = a * x; + w = w / (b + w); + return incbet(0.5 * a, 0.5 * b, w); +} + + +double fdtri(double a, double b, double y) +{ + double w, x; + + if ((a <= 0.0) || (b <= 0.0) || (y <= 0.0) || (y > 1.0)) { + sf_error("fdtri", SF_ERROR_DOMAIN, NULL); + return NAN; + } + y = 1.0 - y; + /* Compute probability for x = 0.5. */ + w = incbet(0.5 * b, 0.5 * a, 0.5); + /* If that is greater than y, then the solution w < .5. + * Otherwise, solve at 1-y to remove cancellation in (b - b*w). */ + if (w > y || y < 0.001) { + w = incbi(0.5 * b, 0.5 * a, y); + x = (b - b * w) / (a * w); + } + else { + w = incbi(0.5 * a, 0.5 * b, 1.0 - y); + x = b * w / (a * (1.0 - w)); + } + return x; +} diff --git a/gtsam/3rdparty/cephes/cephes/fresnl.c b/gtsam/3rdparty/cephes/cephes/fresnl.c new file mode 100644 index 000000000..50620fa2e --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/fresnl.c @@ -0,0 +1,219 @@ +/* fresnl.c + * + * Fresnel integral + * + * + * + * SYNOPSIS: + * + * double x, S, C; + * void fresnl(); + * + * fresnl( x, _&S, _&C ); + * + * + * DESCRIPTION: + * + * Evaluates the Fresnel integrals + * + * x + * - + * | | + * C(x) = | cos(pi/2 t**2) dt, + * | | + * - + * 0 + * + * x + * - + * | | + * S(x) = | sin(pi/2 t**2) dt. + * | | + * - + * 0 + * + * + * The integrals are evaluated by a power series for x < 1. + * For x >= 1 auxiliary functions f(x) and g(x) are employed + * such that + * + * C(x) = 0.5 + f(x) sin( pi/2 x**2 ) - g(x) cos( pi/2 x**2 ) + * S(x) = 0.5 - f(x) cos( pi/2 x**2 ) - g(x) sin( pi/2 x**2 ) + * + * + * + * ACCURACY: + * + * Relative error. + * + * Arithmetic function domain # trials peak rms + * IEEE S(x) 0, 10 10000 2.0e-15 3.2e-16 + * IEEE C(x) 0, 10 10000 1.8e-15 3.3e-16 + */ + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +/* S(x) for small x */ +static double sn[6] = { + -2.99181919401019853726E3, + 7.08840045257738576863E5, + -6.29741486205862506537E7, + 2.54890880573376359104E9, + -4.42979518059697779103E10, + 3.18016297876567817986E11, +}; + +static double sd[6] = { + /* 1.00000000000000000000E0, */ + 2.81376268889994315696E2, + 4.55847810806532581675E4, + 5.17343888770096400730E6, + 4.19320245898111231129E8, + 2.24411795645340920940E10, + 6.07366389490084639049E11, +}; + +/* C(x) for small x */ +static double cn[6] = { + -4.98843114573573548651E-8, + 9.50428062829859605134E-6, + -6.45191435683965050962E-4, + 1.88843319396703850064E-2, + -2.05525900955013891793E-1, + 9.99999999999999998822E-1, +}; + +static double cd[7] = { + 3.99982968972495980367E-12, + 9.15439215774657478799E-10, + 1.25001862479598821474E-7, + 1.22262789024179030997E-5, + 8.68029542941784300606E-4, + 4.12142090722199792936E-2, + 1.00000000000000000118E0, +}; + +/* Auxiliary function f(x) */ +static double fn[10] = { + 4.21543555043677546506E-1, + 1.43407919780758885261E-1, + 1.15220955073585758835E-2, + 3.45017939782574027900E-4, + 4.63613749287867322088E-6, + 3.05568983790257605827E-8, + 1.02304514164907233465E-10, + 1.72010743268161828879E-13, + 1.34283276233062758925E-16, + 3.76329711269987889006E-20, +}; + +static double fd[10] = { + /* 1.00000000000000000000E0, */ + 7.51586398353378947175E-1, + 1.16888925859191382142E-1, + 6.44051526508858611005E-3, + 1.55934409164153020873E-4, + 1.84627567348930545870E-6, + 1.12699224763999035261E-8, + 3.60140029589371370404E-11, + 5.88754533621578410010E-14, + 4.52001434074129701496E-17, + 1.25443237090011264384E-20, +}; + +/* Auxiliary function g(x) */ +static double gn[11] = { + 5.04442073643383265887E-1, + 1.97102833525523411709E-1, + 1.87648584092575249293E-2, + 6.84079380915393090172E-4, + 1.15138826111884280931E-5, + 9.82852443688422223854E-8, + 4.45344415861750144738E-10, + 1.08268041139020870318E-12, + 1.37555460633261799868E-15, + 8.36354435630677421531E-19, + 1.86958710162783235106E-22, +}; + +static double gd[11] = { + /* 1.00000000000000000000E0, */ + 1.47495759925128324529E0, + 3.37748989120019970451E-1, + 2.53603741420338795122E-2, + 8.14679107184306179049E-4, + 1.27545075667729118702E-5, + 1.04314589657571990585E-7, + 4.60680728146520428211E-10, + 1.10273215066240270757E-12, + 1.38796531259578871258E-15, + 8.39158816283118707363E-19, + 1.86958710162783236342E-22, +}; + +extern double MACHEP; + +int fresnl(double xxa, double *ssa, double *cca) +{ + double f, g, cc, ss, c, s, t, u; + double x, x2; + + if (cephes_isinf(xxa)) { + cc = 0.5; + ss = 0.5; + goto done; + } + + x = fabs(xxa); + x2 = x * x; + if (x2 < 2.5625) { + t = x2 * x2; + ss = x * x2 * polevl(t, sn, 5) / p1evl(t, sd, 6); + cc = x * polevl(t, cn, 5) / polevl(t, cd, 6); + goto done; + } + + if (x > 36974.0) { + /* + * http://functions.wolfram.com/GammaBetaErf/FresnelC/06/02/ + * http://functions.wolfram.com/GammaBetaErf/FresnelS/06/02/ + */ + cc = 0.5 + 1/(M_PI*x) * sin(M_PI*x*x/2); + ss = 0.5 - 1/(M_PI*x) * cos(M_PI*x*x/2); + goto done; + } + + + /* Asymptotic power series auxiliary functions + * for large argument + */ + x2 = x * x; + t = M_PI * x2; + u = 1.0 / (t * t); + t = 1.0 / t; + f = 1.0 - u * polevl(u, fn, 9) / p1evl(u, fd, 10); + g = t * polevl(u, gn, 10) / p1evl(u, gd, 11); + + t = M_PI_2 * x2; + c = cos(t); + s = sin(t); + t = M_PI * x; + cc = 0.5 + (f * s - g * c) / t; + ss = 0.5 - (f * c + g * s) / t; + + done: + if (xxa < 0.0) { + cc = -cc; + ss = -ss; + } + + *cca = cc; + *ssa = ss; + return (0); +} diff --git a/gtsam/3rdparty/cephes/cephes/gamma.c b/gtsam/3rdparty/cephes/cephes/gamma.c new file mode 100644 index 000000000..2a61defed --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/gamma.c @@ -0,0 +1,364 @@ +/* + * Gamma function + * + * + * + * SYNOPSIS: + * + * double x, y, Gamma(); + * + * y = Gamma( x ); + * + * + * + * DESCRIPTION: + * + * Returns Gamma function of the argument. The result is + * correctly signed. + * + * Arguments |x| <= 34 are reduced by recurrence and the function + * approximated by a rational function of degree 6/7 in the + * interval (2,3). Large arguments are handled by Stirling's + * formula. Large negative arguments are made positive using + * a reflection formula. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -170,-33 20000 2.3e-15 3.3e-16 + * IEEE -33, 33 20000 9.4e-16 2.2e-16 + * IEEE 33, 171.6 20000 2.3e-15 3.2e-16 + * + * Error for arguments outside the test range will be larger + * owing to error amplification by the exponential function. + * + */ + +/* lgam() + * + * Natural logarithm of Gamma function + * + * + * + * SYNOPSIS: + * + * double x, y, lgam(); + * + * y = lgam( x ); + * + * + * + * DESCRIPTION: + * + * Returns the base e (2.718...) logarithm of the absolute + * value of the Gamma function of the argument. + * + * For arguments greater than 13, the logarithm of the Gamma + * function is approximated by the logarithmic version of + * Stirling's formula using a polynomial approximation of + * degree 4. Arguments between -33 and +33 are reduced by + * recurrence to the interval [2,3] of a rational approximation. + * The cosecant reflection formula is employed for arguments + * less than -33. + * + * Arguments greater than MAXLGM return INFINITY and an error + * message. MAXLGM = 2.556348e305 for IEEE arithmetic. + * + * + * + * ACCURACY: + * + * + * arithmetic domain # trials peak rms + * IEEE 0, 3 28000 5.4e-16 1.1e-16 + * IEEE 2.718, 2.556e305 40000 3.5e-16 8.3e-17 + * The error criterion was relative when the function magnitude + * was greater than one but absolute when it was less than one. + * + * The following test used the relative error criterion, though + * at certain points the relative error could be much higher than + * indicated. + * IEEE -200, -4 10000 4.8e-16 1.3e-16 + * + */ + +/* + * Cephes Math Library Release 2.2: July, 1992 + * Copyright 1984, 1987, 1989, 1992 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + + +#include "mconf.h" + +static double P[] = { + 1.60119522476751861407E-4, + 1.19135147006586384913E-3, + 1.04213797561761569935E-2, + 4.76367800457137231464E-2, + 2.07448227648435975150E-1, + 4.94214826801497100753E-1, + 9.99999999999999996796E-1 +}; + +static double Q[] = { + -2.31581873324120129819E-5, + 5.39605580493303397842E-4, + -4.45641913851797240494E-3, + 1.18139785222060435552E-2, + 3.58236398605498653373E-2, + -2.34591795718243348568E-1, + 7.14304917030273074085E-2, + 1.00000000000000000320E0 +}; + +#define MAXGAM 171.624376956302725 +static double LOGPI = 1.14472988584940017414; + +/* Stirling's formula for the Gamma function */ +static double STIR[5] = { + 7.87311395793093628397E-4, + -2.29549961613378126380E-4, + -2.68132617805781232825E-3, + 3.47222221605458667310E-3, + 8.33333333333482257126E-2, +}; + +#define MAXSTIR 143.01608 +static double SQTPI = 2.50662827463100050242E0; + +extern double MAXLOG; +static double stirf(double); + +/* Gamma function computed by Stirling's formula. + * The polynomial STIR is valid for 33 <= x <= 172. + */ +static double stirf(double x) +{ + double y, w, v; + + if (x >= MAXGAM) { + return (INFINITY); + } + w = 1.0 / x; + w = 1.0 + w * polevl(w, STIR, 4); + y = exp(x); + if (x > MAXSTIR) { /* Avoid overflow in pow() */ + v = pow(x, 0.5 * x - 0.25); + y = v * (v / y); + } + else { + y = pow(x, x - 0.5) / y; + } + y = SQTPI * y * w; + return (y); +} + + +double Gamma(double x) +{ + double p, q, z; + int i; + int sgngam = 1; + + if (!cephes_isfinite(x)) { + return x; + } + q = fabs(x); + + if (q > 33.0) { + if (x < 0.0) { + p = floor(q); + if (p == q) { + gamnan: + sf_error("Gamma", SF_ERROR_OVERFLOW, NULL); + return (INFINITY); + } + i = p; + if ((i & 1) == 0) + sgngam = -1; + z = q - p; + if (z > 0.5) { + p += 1.0; + z = q - p; + } + z = q * sin(M_PI * z); + if (z == 0.0) { + return (sgngam * INFINITY); + } + z = fabs(z); + z = M_PI / (z * stirf(q)); + } + else { + z = stirf(x); + } + return (sgngam * z); + } + + z = 1.0; + while (x >= 3.0) { + x -= 1.0; + z *= x; + } + + while (x < 0.0) { + if (x > -1.E-9) + goto small; + z /= x; + x += 1.0; + } + + while (x < 2.0) { + if (x < 1.e-9) + goto small; + z /= x; + x += 1.0; + } + + if (x == 2.0) + return (z); + + x -= 2.0; + p = polevl(x, P, 6); + q = polevl(x, Q, 7); + return (z * p / q); + + small: + if (x == 0.0) { + goto gamnan; + } + else + return (z / ((1.0 + 0.5772156649015329 * x) * x)); +} + + + +/* A[]: Stirling's formula expansion of log Gamma + * B[], C[]: log Gamma function between 2 and 3 + */ +static double A[] = { + 8.11614167470508450300E-4, + -5.95061904284301438324E-4, + 7.93650340457716943945E-4, + -2.77777777730099687205E-3, + 8.33333333333331927722E-2 +}; + +static double B[] = { + -1.37825152569120859100E3, + -3.88016315134637840924E4, + -3.31612992738871184744E5, + -1.16237097492762307383E6, + -1.72173700820839662146E6, + -8.53555664245765465627E5 +}; + +static double C[] = { + /* 1.00000000000000000000E0, */ + -3.51815701436523470549E2, + -1.70642106651881159223E4, + -2.20528590553854454839E5, + -1.13933444367982507207E6, + -2.53252307177582951285E6, + -2.01889141433532773231E6 +}; + +/* log( sqrt( 2*pi ) ) */ +static double LS2PI = 0.91893853320467274178; + +#define MAXLGM 2.556348e305 + + +/* Logarithm of Gamma function */ +double lgam(double x) +{ + int sign; + return lgam_sgn(x, &sign); +} + +double lgam_sgn(double x, int *sign) +{ + double p, q, u, w, z; + int i; + + *sign = 1; + + if (!cephes_isfinite(x)) + return x; + + if (x < -34.0) { + q = -x; + w = lgam_sgn(q, sign); + p = floor(q); + if (p == q) { + lgsing: + sf_error("lgam", SF_ERROR_SINGULAR, NULL); + return (INFINITY); + } + i = p; + if ((i & 1) == 0) + *sign = -1; + else + *sign = 1; + z = q - p; + if (z > 0.5) { + p += 1.0; + z = p - q; + } + z = q * sin(M_PI * z); + if (z == 0.0) + goto lgsing; + /* z = log(M_PI) - log( z ) - w; */ + z = LOGPI - log(z) - w; + return (z); + } + + if (x < 13.0) { + z = 1.0; + p = 0.0; + u = x; + while (u >= 3.0) { + p -= 1.0; + u = x + p; + z *= u; + } + while (u < 2.0) { + if (u == 0.0) + goto lgsing; + z /= u; + p += 1.0; + u = x + p; + } + if (z < 0.0) { + *sign = -1; + z = -z; + } + else + *sign = 1; + if (u == 2.0) + return (log(z)); + p -= 2.0; + x = x + p; + p = x * polevl(x, B, 5) / p1evl(x, C, 6); + return (log(z) + p); + } + + if (x > MAXLGM) { + return (*sign * INFINITY); + } + + q = (x - 0.5) * log(x) - x + LS2PI; + if (x > 1.0e8) + return (q); + + p = 1.0 / (x * x); + if (x >= 1000.0) + q += ((7.9365079365079365079365e-4 * p + - 2.7777777777777777777778e-3) * p + + 0.0833333333333333333333) / x; + else + q += polevl(p, A, 4) / x; + return (q); +} diff --git a/gtsam/3rdparty/cephes/cephes/gammasgn.c b/gtsam/3rdparty/cephes/cephes/gammasgn.c new file mode 100644 index 000000000..9d74318ff --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/gammasgn.c @@ -0,0 +1,25 @@ +#include "mconf.h" + +double gammasgn(double x) +{ + double fx; + + if (isnan(x)) { + return x; + } + if (x > 0) { + return 1.0; + } + else { + fx = floor(x); + if (x - fx == 0.0) { + return 0.0; + } + else if ((int)fx % 2) { + return -1.0; + } + else { + return 1.0; + } + } +} diff --git a/gtsam/3rdparty/cephes/cephes/gdtr.c b/gtsam/3rdparty/cephes/cephes/gdtr.c new file mode 100644 index 000000000..597c8d4d9 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/gdtr.c @@ -0,0 +1,132 @@ +/* gdtr.c + * + * Gamma distribution function + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, gdtr(); + * + * y = gdtr( a, b, x ); + * + * + * + * DESCRIPTION: + * + * Returns the integral from zero to x of the Gamma probability + * density function: + * + * + * x + * b - + * a | | b-1 -at + * y = ----- | t e dt + * - | | + * | (b) - + * 0 + * + * The incomplete Gamma integral is used, according to the + * relation + * + * y = igam( b, ax ). + * + * + * ACCURACY: + * + * See igam(). + * + * ERROR MESSAGES: + * + * message condition value returned + * gdtr domain x < 0 0.0 + * + */ + /* gdtrc.c + * + * Complemented Gamma distribution function + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, gdtrc(); + * + * y = gdtrc( a, b, x ); + * + * + * + * DESCRIPTION: + * + * Returns the integral from x to infinity of the Gamma + * probability density function: + * + * + * inf. + * b - + * a | | b-1 -at + * y = ----- | t e dt + * - | | + * | (b) - + * x + * + * The incomplete Gamma integral is used, according to the + * relation + * + * y = igamc( b, ax ). + * + * + * ACCURACY: + * + * See igamc(). + * + * ERROR MESSAGES: + * + * message condition value returned + * gdtrc domain x < 0 0.0 + * + */ + +/* gdtr() */ + + +/* + * Cephes Math Library Release 2.3: March,1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + + +double gdtr(double a, double b, double x) +{ + + if (x < 0.0) { + sf_error("gdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + return (igam(b, a * x)); +} + + +double gdtrc(double a, double b, double x) +{ + + if (x < 0.0) { + sf_error("gdtrc", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + return (igamc(b, a * x)); +} + + +double gdtri(double a, double b, double y) +{ + + if ((y < 0.0) || (y > 1.0) || (a <= 0.0) || (b < 0.0)) { + sf_error("gdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + return (igamci(b, 1.0 - y) / a); +} diff --git a/gtsam/3rdparty/cephes/cephes/hyp2f1.c b/gtsam/3rdparty/cephes/cephes/hyp2f1.c new file mode 100644 index 000000000..7f0a84d02 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/hyp2f1.c @@ -0,0 +1,569 @@ +/* hyp2f1.c + * + * Gauss hypergeometric function F + * 2 1 + * + * + * SYNOPSIS: + * + * double a, b, c, x, y, hyp2f1(); + * + * y = hyp2f1( a, b, c, x ); + * + * + * DESCRIPTION: + * + * + * hyp2f1( a, b, c, x ) = F ( a, b; c; x ) + * 2 1 + * + * inf. + * - a(a+1)...(a+k) b(b+1)...(b+k) k+1 + * = 1 + > ----------------------------- x . + * - c(c+1)...(c+k) (k+1)! + * k = 0 + * + * Cases addressed are + * Tests and escapes for negative integer a, b, or c + * Linear transformation if c - a or c - b negative integer + * Special case c = a or c = b + * Linear transformation for x near +1 + * Transformation for x < -0.5 + * Psi function expansion if x > 0.5 and c - a - b integer + * Conditionally, a recurrence on c to make c-a-b > 0 + * + * x < -1 AMS 15.3.7 transformation applied (Travis Oliphant) + * valid for b,a,c,(b-a) != integer and (c-a),(c-b) != negative integer + * + * x >= 1 is rejected (unless special cases are present) + * + * The parameters a, b, c are considered to be integer + * valued if they are within 1.0e-14 of the nearest integer + * (1.0e-13 for IEEE arithmetic). + * + * ACCURACY: + * + * + * Relative error (-1 < x < 1): + * arithmetic domain # trials peak rms + * IEEE -1,7 230000 1.2e-11 5.2e-14 + * + * Several special cases also tested with a, b, c in + * the range -7 to 7. + * + * ERROR MESSAGES: + * + * A "partial loss of precision" message is printed if + * the internally estimated relative error exceeds 1^-12. + * A "singularity" message is printed on overflow or + * in cases not addressed (such as x < -1). + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1992, 2000 by Stephen L. Moshier + */ + +#include +#include +#include + +#include "mconf.h" + +#define EPS 1.0e-13 +#define EPS2 1.0e-10 + +#define ETHRESH 1.0e-12 + +#define MAX_ITERATIONS 10000 + +extern double MACHEP; + +/* hys2f1 and hyp2f1ra depend on each other, so we need this prototype */ +static double hyp2f1ra(double a, double b, double c, double x, double *loss); + +/* Defining power series expansion of Gauss hypergeometric function */ +/* The `loss` parameter estimates loss of significance */ +static double hys2f1(double a, double b, double c, double x, double *loss) { + double f, g, h, k, m, s, u, umax; + int i; + int ib, intflag = 0; + + if (fabs(b) > fabs(a)) { + /* Ensure that |a| > |b| ... */ + f = b; + b = a; + a = f; + } + + ib = round(b); + + if (fabs(b - ib) < EPS && ib <= 0 && fabs(b) < fabs(a)) { + /* .. except when `b` is a smaller negative integer */ + f = b; + b = a; + a = f; + intflag = 1; + } + + if ((fabs(a) > fabs(c) + 1 || intflag) && fabs(c - a) > 2 && fabs(a) > 2) { + /* |a| >> |c| implies that large cancellation error is to be expected. + * + * We try to reduce it with the recurrence relations + */ + return hyp2f1ra(a, b, c, x, loss); + } + + i = 0; + umax = 0.0; + f = a; + g = b; + h = c; + s = 1.0; + u = 1.0; + k = 0.0; + do { + if (fabs(h) < EPS) { + *loss = 1.0; + return INFINITY; + } + m = k + 1.0; + u = u * ((f + k) * (g + k) * x / ((h + k) * m)); + s += u; + k = fabs(u); /* remember largest term summed */ + if (k > umax) umax = k; + k = m; + if (++i > MAX_ITERATIONS) { /* should never happen */ + *loss = 1.0; + return (s); + } + } while (s == 0 || fabs(u / s) > MACHEP); + + /* return estimated relative error */ + *loss = (MACHEP * umax) / fabs(s) + (MACHEP * i); + + return (s); +} + +/* Apply transformations for |x| near 1 then call the power series */ +static double hyt2f1(double a, double b, double c, double x, double *loss) { + double p, q, r, s, t, y, w, d, err, err1; + double ax, id, d1, d2, e, y1; + int i, aid, sign; + + int ia, ib, neg_int_a = 0, neg_int_b = 0; + + ia = round(a); + ib = round(b); + + if (a <= 0 && fabs(a - ia) < EPS) { /* a is a negative integer */ + neg_int_a = 1; + } + + if (b <= 0 && fabs(b - ib) < EPS) { /* b is a negative integer */ + neg_int_b = 1; + } + + err = 0.0; + s = 1.0 - x; + if (x < -0.5 && !(neg_int_a || neg_int_b)) { + if (b > a) + y = pow(s, -a) * hys2f1(a, c - b, c, -x / s, &err); + + else + y = pow(s, -b) * hys2f1(c - a, b, c, -x / s, &err); + + goto done; + } + + d = c - a - b; + id = round(d); /* nearest integer to d */ + + if (x > 0.9 && !(neg_int_a || neg_int_b)) { + if (fabs(d - id) > EPS) { + int sgngam; + + /* test for integer c-a-b */ + /* Try the power series first */ + y = hys2f1(a, b, c, x, &err); + if (err < ETHRESH) goto done; + /* If power series fails, then apply AMS55 #15.3.6 */ + q = hys2f1(a, b, 1.0 - d, s, &err); + sign = 1; + w = lgam_sgn(d, &sgngam); + sign *= sgngam; + w -= lgam_sgn(c - a, &sgngam); + sign *= sgngam; + w -= lgam_sgn(c - b, &sgngam); + sign *= sgngam; + q *= sign * exp(w); + r = pow(s, d) * hys2f1(c - a, c - b, d + 1.0, s, &err1); + sign = 1; + w = lgam_sgn(-d, &sgngam); + sign *= sgngam; + w -= lgam_sgn(a, &sgngam); + sign *= sgngam; + w -= lgam_sgn(b, &sgngam); + sign *= sgngam; + r *= sign * exp(w); + y = q + r; + + q = fabs(q); /* estimate cancellation error */ + r = fabs(r); + if (q > r) r = q; + err += err1 + (MACHEP * r) / y; + + y *= gamma(c); + goto done; + } else { + /* Psi function expansion, AMS55 #15.3.10, #15.3.11, #15.3.12 + * + * Although AMS55 does not explicitly state it, this expansion fails + * for negative integer a or b, since the psi and Gamma functions + * involved have poles. + */ + + if (id >= 0.0) { + e = d; + d1 = d; + d2 = 0.0; + aid = id; + } else { + e = -d; + d1 = 0.0; + d2 = d; + aid = -id; + } + + ax = log(s); + + /* sum for t = 0 */ + y = psi(1.0) + psi(1.0 + e) - psi(a + d1) - psi(b + d1) - ax; + y /= gamma(e + 1.0); + + p = (a + d1) * (b + d1) * s / gamma(e + 2.0); /* Poch for t=1 */ + t = 1.0; + do { + r = psi(1.0 + t) + psi(1.0 + t + e) - psi(a + t + d1) - + psi(b + t + d1) - ax; + q = p * r; + y += q; + p *= s * (a + t + d1) / (t + 1.0); + p *= (b + t + d1) / (t + 1.0 + e); + t += 1.0; + if (t > MAX_ITERATIONS) { /* should never happen */ + sf_error("hyp2f1", SF_ERROR_SLOW, NULL); + *loss = 1.0; + return NAN; + } + } while (y == 0 || fabs(q / y) > EPS); + + if (id == 0.0) { + y *= gamma(c) / (gamma(a) * gamma(b)); + goto psidon; + } + + y1 = 1.0; + + if (aid == 1) goto nosum; + + t = 0.0; + p = 1.0; + for (i = 1; i < aid; i++) { + r = 1.0 - e + t; + p *= s * (a + t + d2) * (b + t + d2) / r; + t += 1.0; + p /= t; + y1 += p; + } + nosum: + p = gamma(c); + y1 *= gamma(e) * p / (gamma(a + d1) * gamma(b + d1)); + + y *= p / (gamma(a + d2) * gamma(b + d2)); + if ((aid & 1) != 0) y = -y; + + q = pow(s, id); /* s to the id power */ + if (id > 0.0) + y *= q; + else + y1 *= q; + + y += y1; + psidon: + goto done; + } + } + + /* Use defining power series if no special cases */ + y = hys2f1(a, b, c, x, &err); + +done: + *loss = err; + return (y); +} + +/* + 15.4.2 Abramowitz & Stegun. +*/ +static double hyp2f1_neg_c_equal_bc(double a, double b, double x) { + double k; + double collector = 1; + double sum = 1; + double collector_max = 1; + + if (!(fabs(b) < 1e5)) { + return NAN; + } + + for (k = 1; k <= -b; k++) { + collector *= (a + k - 1) * x / k; + collector_max = fmax(fabs(collector), collector_max); + sum += collector; + } + + if (1e-16 * (1 + collector_max / fabs(sum)) > 1e-7) { + return NAN; + } + + return sum; +} + +double hyp2f1(double a, double b, double c, double x) { + double d, d1, d2, e; + double p, q, r, s, y, ax; + double ia, ib, ic, id, err; + double t1; + int i, aid; + int neg_int_a = 0, neg_int_b = 0; + int neg_int_ca_or_cb = 0; + + err = 0.0; + ax = fabs(x); + s = 1.0 - x; + ia = round(a); /* nearest integer to a */ + ib = round(b); + + if (x == 0.0) { + return 1.0; + } + + d = c - a - b; + id = round(d); + + if ((a == 0 || b == 0) && c != 0) { + return 1.0; + } + + if (a <= 0 && fabs(a - ia) < EPS) { /* a is a negative integer */ + neg_int_a = 1; + } + + if (b <= 0 && fabs(b - ib) < EPS) { /* b is a negative integer */ + neg_int_b = 1; + } + + if (d <= -1 && !(fabs(d - id) > EPS && s < 0) && !(neg_int_a || neg_int_b)) { + return pow(s, d) * hyp2f1(c - a, c - b, c, x); + } + if (d <= 0 && x == 1 && !(neg_int_a || neg_int_b)) goto hypdiv; + + if (ax < 1.0 || x == -1.0) { + /* 2F1(a,b;b;x) = (1-x)**(-a) */ + if (fabs(b - c) < EPS) { /* b = c */ + if (neg_int_b) { + y = hyp2f1_neg_c_equal_bc(a, b, x); + } else { + y = pow(s, -a); /* s to the -a power */ + } + goto hypdon; + } + if (fabs(a - c) < EPS) { /* a = c */ + y = pow(s, -b); /* s to the -b power */ + goto hypdon; + } + } + + if (c <= 0.0) { + ic = round(c); /* nearest integer to c */ + if (fabs(c - ic) < EPS) { /* c is a negative integer */ + /* check if termination before explosion */ + if (neg_int_a && (ia > ic)) goto hypok; + if (neg_int_b && (ib > ic)) goto hypok; + goto hypdiv; + } + } + + if (neg_int_a || neg_int_b) /* function is a polynomial */ + goto hypok; + + t1 = fabs(b - a); + if (x < -2.0 && fabs(t1 - round(t1)) > EPS) { + /* This transform has a pole for b-a integer, and + * may produce large cancellation errors for |1/x| close 1 + */ + p = hyp2f1(a, 1 - c + a, 1 - b + a, 1.0 / x); + q = hyp2f1(b, 1 - c + b, 1 - a + b, 1.0 / x); + p *= pow(-x, -a); + q *= pow(-x, -b); + t1 = gamma(c); + s = t1 * gamma(b - a) / (gamma(b) * gamma(c - a)); + y = t1 * gamma(a - b) / (gamma(a) * gamma(c - b)); + return s * p + y * q; + } else if (x < -1.0) { + if (fabs(a) < fabs(b)) { + return pow(s, -a) * hyp2f1(a, c - b, c, x / (x - 1)); + } else { + return pow(s, -b) * hyp2f1(b, c - a, c, x / (x - 1)); + } + } + + if (ax > 1.0) /* series diverges */ + goto hypdiv; + + p = c - a; + ia = round(p); /* nearest integer to c-a */ + if ((ia <= 0.0) && (fabs(p - ia) < EPS)) /* negative int c - a */ + neg_int_ca_or_cb = 1; + + r = c - b; + ib = round(r); /* nearest integer to c-b */ + if ((ib <= 0.0) && (fabs(r - ib) < EPS)) /* negative int c - b */ + neg_int_ca_or_cb = 1; + + id = round(d); /* nearest integer to d */ + q = fabs(d - id); + + /* Thanks to Christian Burger + * for reporting a bug here. */ + if (fabs(ax - 1.0) < EPS) { /* |x| == 1.0 */ + if (x > 0.0) { + if (neg_int_ca_or_cb) { + if (d >= 0.0) + goto hypf; + else + goto hypdiv; + } + if (d <= 0.0) goto hypdiv; + y = gamma(c) * gamma(d) / (gamma(p) * gamma(r)); + goto hypdon; + } + if (d <= -1.0) goto hypdiv; + } + + /* Conditionally make d > 0 by recurrence on c + * AMS55 #15.2.27 + */ + if (d < 0.0) { + /* Try the power series first */ + y = hyt2f1(a, b, c, x, &err); + if (err < ETHRESH) goto hypdon; + /* Apply the recurrence if power series fails */ + err = 0.0; + aid = 2 - id; + e = c + aid; + d2 = hyp2f1(a, b, e, x); + d1 = hyp2f1(a, b, e + 1.0, x); + q = a + b + 1.0; + for (i = 0; i < aid; i++) { + r = e - 1.0; + y = (e * (r - (2.0 * e - q) * x) * d2 + (e - a) * (e - b) * x * d1) / + (e * r * s); + e = r; + d1 = d2; + d2 = y; + } + goto hypdon; + } + + if (neg_int_ca_or_cb) goto hypf; /* negative integer c-a or c-b */ + +hypok: + y = hyt2f1(a, b, c, x, &err); + +hypdon: + if (err > ETHRESH) { + sf_error("hyp2f1", SF_ERROR_LOSS, NULL); + /* printf( "Estimated err = %.2e\n", err ); */ + } + return (y); + + /* The transformation for c-a or c-b negative integer + * AMS55 #15.3.3 + */ +hypf: + y = pow(s, d) * hys2f1(c - a, c - b, c, x, &err); + goto hypdon; + + /* The alarm exit */ +hypdiv: + sf_error("hyp2f1", SF_ERROR_OVERFLOW, NULL); + return INFINITY; +} + +/* + * Evaluate hypergeometric function by two-term recurrence in `a`. + * + * This avoids some of the loss of precision in the strongly alternating + * hypergeometric series, and can be used to reduce the `a` and `b` parameters + * to smaller values. + * + * AMS55 #15.2.10 + */ +static double hyp2f1ra(double a, double b, double c, double x, double *loss) { + double f2, f1, f0; + int n; + double t, err, da; + + /* Don't cross c or zero */ + if ((c < 0 && a <= c) || (c >= 0 && a >= c)) { + da = round(a - c); + } else { + da = round(a); + } + t = a - da; + + *loss = 0; + + assert(da != 0); + + if (fabs(da) > MAX_ITERATIONS) { + /* Too expensive to compute this value, so give up */ + sf_error("hyp2f1", SF_ERROR_NO_RESULT, NULL); + *loss = 1.0; + return NAN; + } + + if (da < 0) { + /* Recurse down */ + f2 = 0; + f1 = hys2f1(t, b, c, x, &err); + *loss += err; + f0 = hys2f1(t - 1, b, c, x, &err); + *loss += err; + t -= 1; + for (n = 1; n < -da; ++n) { + f2 = f1; + f1 = f0; + f0 = -(2 * t - c - t * x + b * x) / (c - t) * f1 - + t * (x - 1) / (c - t) * f2; + t -= 1; + } + } else { + /* Recurse up */ + f2 = 0; + f1 = hys2f1(t, b, c, x, &err); + *loss += err; + f0 = hys2f1(t + 1, b, c, x, &err); + *loss += err; + t += 1; + for (n = 1; n < da; ++n) { + f2 = f1; + f1 = f0; + f0 = -((2 * t - c - t * x + b * x) * f1 + (c - t) * f2) / (t * (x - 1)); + t += 1; + } + } + + return f0; +} diff --git a/gtsam/3rdparty/cephes/cephes/hyperg.c b/gtsam/3rdparty/cephes/cephes/hyperg.c new file mode 100644 index 000000000..ac23e7133 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/hyperg.c @@ -0,0 +1,362 @@ +/* hyperg.c + * + * Confluent hypergeometric function + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, hyperg(); + * + * y = hyperg( a, b, x ); + * + * + * + * DESCRIPTION: + * + * Computes the confluent hypergeometric function + * + * 1 2 + * a x a(a+1) x + * F ( a,b;x ) = 1 + ---- + --------- + ... + * 1 1 b 1! b(b+1) 2! + * + * Many higher transcendental functions are special cases of + * this power series. + * + * As is evident from the formula, b must not be a negative + * integer or zero unless a is an integer with 0 >= a > b. + * + * The routine attempts both a direct summation of the series + * and an asymptotic expansion. In each case error due to + * roundoff, cancellation, and nonconvergence is estimated. + * The result with smaller estimated error is returned. + * + * + * + * ACCURACY: + * + * Tested at random points (a, b, x), all three variables + * ranging from 0 to 30. + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 1.8e-14 1.1e-15 + * + * Larger errors can be observed when b is near a negative + * integer or zero. Certain combinations of arguments yield + * serious cancellation error in the power series summation + * and also are not in the region of near convergence of the + * asymptotic series. An error message is printed if the + * self-estimated relative error is greater than 1.0e-12. + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1988, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" +#include + +extern double MACHEP; + + +/* the `type` parameter determines what converging factor to use */ +static double hyp2f0(double a, double b, double x, int type, double *err) +{ + double a0, alast, t, tlast, maxt; + double n, an, bn, u, sum, temp; + + an = a; + bn = b; + a0 = 1.0e0; + alast = 1.0e0; + sum = 0.0; + n = 1.0e0; + t = 1.0e0; + tlast = 1.0e9; + maxt = 0.0; + + do { + if (an == 0) + goto pdone; + if (bn == 0) + goto pdone; + + u = an * (bn * x / n); + + /* check for blowup */ + temp = fabs(u); + if ((temp > 1.0) && (maxt > (DBL_MAX / temp))) + goto error; + + a0 *= u; + t = fabs(a0); + + /* terminating condition for asymptotic series: + * the series is divergent (if a or b is not a negative integer), + * but its leading part can be used as an asymptotic expansion + */ + if (t > tlast) + goto ndone; + + tlast = t; + sum += alast; /* the sum is one term behind */ + alast = a0; + + if (n > 200) + goto ndone; + + an += 1.0e0; + bn += 1.0e0; + n += 1.0e0; + if (t > maxt) + maxt = t; + } + while (t > MACHEP); + + + pdone: /* series converged! */ + + /* estimate error due to roundoff and cancellation */ + *err = fabs(MACHEP * (n + maxt)); + + alast = a0; + goto done; + + ndone: /* series did not converge */ + + /* The following "Converging factors" are supposed to improve accuracy, + * but do not actually seem to accomplish very much. */ + + n -= 1.0; + x = 1.0 / x; + + switch (type) { /* "type" given as subroutine argument */ + case 1: + alast *= + (0.5 + (0.125 + 0.25 * b - 0.5 * a + 0.25 * x - 0.25 * n) / x); + break; + + case 2: + alast *= 2.0 / 3.0 - b + 2.0 * a + x - n; + break; + + default: + ; + } + + /* estimate error due to roundoff, cancellation, and nonconvergence */ + *err = MACHEP * (n + maxt) + fabs(a0); + + done: + sum += alast; + return (sum); + + /* series blew up: */ + error: + *err = INFINITY; + sf_error("hyperg", SF_ERROR_NO_RESULT, NULL); + return (sum); +} + + +/* asymptotic formula for hypergeometric function: + * + * ( -a + * -- ( |z| + * | (b) ( -------- 2f0( a, 1+a-b, -1/x ) + * ( -- + * ( | (b-a) + * + * + * x a-b ) + * e |x| ) + * + -------- 2f0( b-a, 1-a, 1/x ) ) + * -- ) + * | (a) ) + */ + +static double hy1f1a(double a, double b, double x, double *err) +{ + double h1, h2, t, u, temp, acanc, asum, err1, err2; + + if (x == 0) { + acanc = 1.0; + asum = INFINITY; + goto adone; + } + temp = log(fabs(x)); + t = x + temp * (a - b); + u = -temp * a; + + if (b > 0) { + temp = lgam(b); + t += temp; + u += temp; + } + + h1 = hyp2f0(a, a - b + 1, -1.0 / x, 1, &err1); + + temp = exp(u) / gamma(b - a); + h1 *= temp; + err1 *= temp; + + h2 = hyp2f0(b - a, 1.0 - a, 1.0 / x, 2, &err2); + + if (a < 0) + temp = exp(t) / gamma(a); + else + temp = exp(t - lgam(a)); + + h2 *= temp; + err2 *= temp; + + if (x < 0.0) + asum = h1; + else + asum = h2; + + acanc = fabs(err1) + fabs(err2); + + if (b < 0) { + temp = gamma(b); + asum *= temp; + acanc *= fabs(temp); + } + + + if (asum != 0.0) + acanc /= fabs(asum); + + if (acanc != acanc) + /* nan */ + acanc = 1.0; + + if (asum == INFINITY || asum == -INFINITY) + /* infinity */ + acanc = 0; + + acanc *= 30.0; /* fudge factor, since error of asymptotic formula + * often seems this much larger than advertised */ + + adone: + *err = acanc; + return (asum); +} + + +/* Power series summation for confluent hypergeometric function */ +static double hy1f1p(double a, double b, double x, double *err) +{ + double n, a0, sum, t, u, temp, maxn; + double an, bn, maxt; + double y, c, sumc; + + + /* set up for power series summation */ + an = a; + bn = b; + a0 = 1.0; + sum = 1.0; + c = 0.0; + n = 1.0; + t = 1.0; + maxt = 0.0; + *err = 1.0; + + maxn = 200.0 + 2 * fabs(a) + 2 * fabs(b); + + while (t > MACHEP) { + if (bn == 0) { /* check bn first since if both */ + sf_error("hyperg", SF_ERROR_SINGULAR, NULL); + return (INFINITY); /* an and bn are zero it is */ + } + if (an == 0) /* a singularity */ + return (sum); + if (n > maxn) { + /* too many terms; take the last one as error estimate */ + c = fabs(c) + fabs(t) * 50.0; + goto pdone; + } + u = x * (an / (bn * n)); + + /* check for blowup */ + temp = fabs(u); + if ((temp > 1.0) && (maxt > (DBL_MAX / temp))) { + *err = 1.0; /* blowup: estimate 100% error */ + return sum; + } + + a0 *= u; + + y = a0 - c; + sumc = sum + y; + c = (sumc - sum) - y; + sum = sumc; + + t = fabs(a0); + + an += 1.0; + bn += 1.0; + n += 1.0; + } + + pdone: + + /* estimate error due to roundoff and cancellation */ + if (sum != 0.0) { + *err = fabs(c / sum); + } + else { + *err = fabs(c); + } + + if (*err != *err) { + /* nan */ + *err = 1.0; + } + + return (sum); +} + + + +double hyperg(double a, double b, double x) +{ + double asum, psum, acanc, pcanc, temp; + + /* See if a Kummer transformation will help */ + temp = b - a; + if (fabs(temp) < 0.001 * fabs(a)) + return (exp(x) * hyperg(temp, b, -x)); + + + /* Try power & asymptotic series, starting from the one that is likely OK */ + if (fabs(x) < 10 + fabs(a) + fabs(b)) { + psum = hy1f1p(a, b, x, &pcanc); + if (pcanc < 1.0e-15) + goto done; + asum = hy1f1a(a, b, x, &acanc); + } + else { + psum = hy1f1a(a, b, x, &pcanc); + if (pcanc < 1.0e-15) + goto done; + asum = hy1f1p(a, b, x, &acanc); + } + + /* Pick the result with less estimated error */ + + if (acanc < pcanc) { + pcanc = acanc; + psum = asum; + } + + done: + if (pcanc > 1.0e-12) + sf_error("hyperg", SF_ERROR_LOSS, NULL); + + return (psum); +} diff --git a/gtsam/3rdparty/cephes/cephes/i0.c b/gtsam/3rdparty/cephes/cephes/i0.c new file mode 100644 index 000000000..4e85d556e --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/i0.c @@ -0,0 +1,180 @@ +/* i0.c + * + * Modified Bessel function of order zero + * + * + * + * SYNOPSIS: + * + * double x, y, i0(); + * + * y = i0( x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of order zero of the + * argument. + * + * The function is defined as i0(x) = j0( ix ). + * + * The range is partitioned into the two intervals [0,8] and + * (8, infinity). Chebyshev polynomial expansions are employed + * in each interval. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 5.8e-16 1.4e-16 + * + */ + /* i0e.c + * + * Modified Bessel function of order zero, + * exponentially scaled + * + * + * + * SYNOPSIS: + * + * double x, y, i0e(); + * + * y = i0e( x ); + * + * + * + * DESCRIPTION: + * + * Returns exponentially scaled modified Bessel function + * of order zero of the argument. + * + * The function is defined as i0e(x) = exp(-|x|) j0( ix ). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 5.4e-16 1.2e-16 + * See i0(). + * + */ + +/* i0.c */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +/* Chebyshev coefficients for exp(-x) I0(x) + * in the interval [0,8]. + * + * lim(x->0){ exp(-x) I0(x) } = 1. + */ +static double A[] = { + -4.41534164647933937950E-18, + 3.33079451882223809783E-17, + -2.43127984654795469359E-16, + 1.71539128555513303061E-15, + -1.16853328779934516808E-14, + 7.67618549860493561688E-14, + -4.85644678311192946090E-13, + 2.95505266312963983461E-12, + -1.72682629144155570723E-11, + 9.67580903537323691224E-11, + -5.18979560163526290666E-10, + 2.65982372468238665035E-9, + -1.30002500998624804212E-8, + 6.04699502254191894932E-8, + -2.67079385394061173391E-7, + 1.11738753912010371815E-6, + -4.41673835845875056359E-6, + 1.64484480707288970893E-5, + -5.75419501008210370398E-5, + 1.88502885095841655729E-4, + -5.76375574538582365885E-4, + 1.63947561694133579842E-3, + -4.32430999505057594430E-3, + 1.05464603945949983183E-2, + -2.37374148058994688156E-2, + 4.93052842396707084878E-2, + -9.49010970480476444210E-2, + 1.71620901522208775349E-1, + -3.04682672343198398683E-1, + 6.76795274409476084995E-1 +}; + +/* Chebyshev coefficients for exp(-x) sqrt(x) I0(x) + * in the inverted interval [8,infinity]. + * + * lim(x->inf){ exp(-x) sqrt(x) I0(x) } = 1/sqrt(2pi). + */ +static double B[] = { + -7.23318048787475395456E-18, + -4.83050448594418207126E-18, + 4.46562142029675999901E-17, + 3.46122286769746109310E-17, + -2.82762398051658348494E-16, + -3.42548561967721913462E-16, + 1.77256013305652638360E-15, + 3.81168066935262242075E-15, + -9.55484669882830764870E-15, + -4.15056934728722208663E-14, + 1.54008621752140982691E-14, + 3.85277838274214270114E-13, + 7.18012445138366623367E-13, + -1.79417853150680611778E-12, + -1.32158118404477131188E-11, + -3.14991652796324136454E-11, + 1.18891471078464383424E-11, + 4.94060238822496958910E-10, + 3.39623202570838634515E-9, + 2.26666899049817806459E-8, + 2.04891858946906374183E-7, + 2.89137052083475648297E-6, + 6.88975834691682398426E-5, + 3.36911647825569408990E-3, + 8.04490411014108831608E-1 +}; + +double i0(double x) +{ + double y; + + if (x < 0) + x = -x; + if (x <= 8.0) { + y = (x / 2.0) - 2.0; + return (exp(x) * chbevl(y, A, 30)); + } + + return (exp(x) * chbevl(32.0 / x - 2.0, B, 25) / sqrt(x)); + +} + + + + +double i0e(double x) +{ + double y; + + if (x < 0) + x = -x; + if (x <= 8.0) { + y = (x / 2.0) - 2.0; + return (chbevl(y, A, 30)); + } + + return (chbevl(32.0 / x - 2.0, B, 25) / sqrt(x)); + +} diff --git a/gtsam/3rdparty/cephes/cephes/i1.c b/gtsam/3rdparty/cephes/cephes/i1.c new file mode 100644 index 000000000..4553873f2 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/i1.c @@ -0,0 +1,184 @@ +/* i1.c + * + * Modified Bessel function of order one + * + * + * + * SYNOPSIS: + * + * double x, y, i1(); + * + * y = i1( x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of order one of the + * argument. + * + * The function is defined as i1(x) = -i j1( ix ). + * + * The range is partitioned into the two intervals [0,8] and + * (8, infinity). Chebyshev polynomial expansions are employed + * in each interval. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.9e-15 2.1e-16 + * + * + */ + /* i1e.c + * + * Modified Bessel function of order one, + * exponentially scaled + * + * + * + * SYNOPSIS: + * + * double x, y, i1e(); + * + * y = i1e( x ); + * + * + * + * DESCRIPTION: + * + * Returns exponentially scaled modified Bessel function + * of order one of the argument. + * + * The function is defined as i1(x) = -i exp(-|x|) j1( ix ). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 2.0e-15 2.0e-16 + * See i1(). + * + */ + +/* i1.c 2 */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1985, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +/* Chebyshev coefficients for exp(-x) I1(x) / x + * in the interval [0,8]. + * + * lim(x->0){ exp(-x) I1(x) / x } = 1/2. + */ + +static double A[] = { + 2.77791411276104639959E-18, + -2.11142121435816608115E-17, + 1.55363195773620046921E-16, + -1.10559694773538630805E-15, + 7.60068429473540693410E-15, + -5.04218550472791168711E-14, + 3.22379336594557470981E-13, + -1.98397439776494371520E-12, + 1.17361862988909016308E-11, + -6.66348972350202774223E-11, + 3.62559028155211703701E-10, + -1.88724975172282928790E-9, + 9.38153738649577178388E-9, + -4.44505912879632808065E-8, + 2.00329475355213526229E-7, + -8.56872026469545474066E-7, + 3.47025130813767847674E-6, + -1.32731636560394358279E-5, + 4.78156510755005422638E-5, + -1.61760815825896745588E-4, + 5.12285956168575772895E-4, + -1.51357245063125314899E-3, + 4.15642294431288815669E-3, + -1.05640848946261981558E-2, + 2.47264490306265168283E-2, + -5.29459812080949914269E-2, + 1.02643658689847095384E-1, + -1.76416518357834055153E-1, + 2.52587186443633654823E-1 +}; + +/* Chebyshev coefficients for exp(-x) sqrt(x) I1(x) + * in the inverted interval [8,infinity]. + * + * lim(x->inf){ exp(-x) sqrt(x) I1(x) } = 1/sqrt(2pi). + */ +static double B[] = { + 7.51729631084210481353E-18, + 4.41434832307170791151E-18, + -4.65030536848935832153E-17, + -3.20952592199342395980E-17, + 2.96262899764595013876E-16, + 3.30820231092092828324E-16, + -1.88035477551078244854E-15, + -3.81440307243700780478E-15, + 1.04202769841288027642E-14, + 4.27244001671195135429E-14, + -2.10154184277266431302E-14, + -4.08355111109219731823E-13, + -7.19855177624590851209E-13, + 2.03562854414708950722E-12, + 1.41258074366137813316E-11, + 3.25260358301548823856E-11, + -1.89749581235054123450E-11, + -5.58974346219658380687E-10, + -3.83538038596423702205E-9, + -2.63146884688951950684E-8, + -2.51223623787020892529E-7, + -3.88256480887769039346E-6, + -1.10588938762623716291E-4, + -9.76109749136146840777E-3, + 7.78576235018280120474E-1 +}; + +double i1(double x) +{ + double y, z; + + z = fabs(x); + if (z <= 8.0) { + y = (z / 2.0) - 2.0; + z = chbevl(y, A, 29) * z * exp(z); + } + else { + z = exp(z) * chbevl(32.0 / z - 2.0, B, 25) / sqrt(z); + } + if (x < 0.0) + z = -z; + return (z); +} + +/* i1e() */ + +double i1e(double x) +{ + double y, z; + + z = fabs(x); + if (z <= 8.0) { + y = (z / 2.0) - 2.0; + z = chbevl(y, A, 29) * z; + } + else { + z = chbevl(32.0 / z - 2.0, B, 25) / sqrt(z); + } + if (x < 0.0) + z = -z; + return (z); +} diff --git a/gtsam/3rdparty/cephes/cephes/igam.c b/gtsam/3rdparty/cephes/cephes/igam.c new file mode 100644 index 000000000..75f871ec5 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/igam.c @@ -0,0 +1,423 @@ +/* igam.c + * + * Incomplete Gamma integral + * + * + * + * SYNOPSIS: + * + * double a, x, y, igam(); + * + * y = igam( a, x ); + * + * DESCRIPTION: + * + * The function is defined by + * + * x + * - + * 1 | | -t a-1 + * igam(a,x) = ----- | e t dt. + * - | | + * | (a) - + * 0 + * + * + * In this implementation both arguments must be positive. + * The integral is evaluated by either a power series or + * continued fraction expansion, depending on the relative + * values of a and x. + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 200000 3.6e-14 2.9e-15 + * IEEE 0,100 300000 9.9e-14 1.5e-14 + */ + /* igamc() + * + * Complemented incomplete Gamma integral + * + * + * + * SYNOPSIS: + * + * double a, x, y, igamc(); + * + * y = igamc( a, x ); + * + * DESCRIPTION: + * + * The function is defined by + * + * + * igamc(a,x) = 1 - igam(a,x) + * + * inf. + * - + * 1 | | -t a-1 + * = ----- | e t dt. + * - | | + * | (a) - + * x + * + * + * In this implementation both arguments must be positive. + * The integral is evaluated by either a power series or + * continued fraction expansion, depending on the relative + * values of a and x. + * + * ACCURACY: + * + * Tested at random a, x. + * a x Relative error: + * arithmetic domain domain # trials peak rms + * IEEE 0.5,100 0,100 200000 1.9e-14 1.7e-15 + * IEEE 0.01,0.5 0,100 200000 1.4e-13 1.6e-15 + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1985, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +/* Sources + * [1] "The Digital Library of Mathematical Functions", dlmf.nist.gov + * [2] Maddock et. al., "Incomplete Gamma Functions", + * https://www.boost.org/doc/libs/1_61_0/libs/math/doc/html/math_toolkit/sf_gamma/igamma.html + */ + +/* Scipy changes: + * - 05-01-2016: added asymptotic expansion for igam to improve the + * a ~ x regime. + * - 06-19-2016: additional series expansion added for igamc to + * improve accuracy at small arguments. + * - 06-24-2016: better choice of domain for the asymptotic series; + * improvements in accuracy for the asymptotic series when a and x + * are very close. + */ + +#include "mconf.h" +#include "lanczos.h" +#include "igam.h" + +#ifdef MAXITER +#undef MAXITER +#endif + +#define MAXITER 2000 +#define IGAM 1 +#define IGAMC 0 +#define SMALL 20 +#define LARGE 200 +#define SMALLRATIO 0.3 +#define LARGERATIO 4.5 + +extern double MACHEP, MAXLOG; +static double big = 4.503599627370496e15; +static double biginv = 2.22044604925031308085e-16; + +static double igamc_continued_fraction(double, double); +static double igam_series(double, double); +static double igamc_series(double, double); +static double asymptotic_series(double, double, int); + + +double igam(double a, double x) +{ + double absxma_a; + + if (x < 0 || a < 0) { + sf_error("gammainc", SF_ERROR_DOMAIN, NULL); + return NAN; + } else if (a == 0) { + if (x > 0) { + return 1; + } else { + return NAN; + } + } else if (x == 0) { + /* Zero integration limit */ + return 0; + } else if (isinf(a)) { + if (isinf(x)) { + return NAN; + } + return 0; + } else if (isinf(x)) { + return 1; + } + + /* Asymptotic regime where a ~ x; see [2]. */ + absxma_a = fabs(x - a) / a; + if ((a > SMALL) && (a < LARGE) && (absxma_a < SMALLRATIO)) { + return asymptotic_series(a, x, IGAM); + } else if ((a > LARGE) && (absxma_a < LARGERATIO / sqrt(a))) { + return asymptotic_series(a, x, IGAM); + } + + if ((x > 1.0) && (x > a)) { + return (1.0 - igamc(a, x)); + } + + return igam_series(a, x); +} + + +double igamc(double a, double x) +{ + double absxma_a; + + if (x < 0 || a < 0) { + sf_error("gammaincc", SF_ERROR_DOMAIN, NULL); + return NAN; + } else if (a == 0) { + if (x > 0) { + return 0; + } else { + return NAN; + } + } else if (x == 0) { + return 1; + } else if (isinf(a)) { + if (isinf(x)) { + return NAN; + } + return 1; + } else if (isinf(x)) { + return 0; + } + + /* Asymptotic regime where a ~ x; see [2]. */ + absxma_a = fabs(x - a) / a; + if ((a > SMALL) && (a < LARGE) && (absxma_a < SMALLRATIO)) { + return asymptotic_series(a, x, IGAMC); + } else if ((a > LARGE) && (absxma_a < LARGERATIO / sqrt(a))) { + return asymptotic_series(a, x, IGAMC); + } + + /* Everywhere else; see [2]. */ + if (x > 1.1) { + if (x < a) { + return 1.0 - igam_series(a, x); + } else { + return igamc_continued_fraction(a, x); + } + } else if (x <= 0.5) { + if (-0.4 / log(x) < a) { + return 1.0 - igam_series(a, x); + } else { + return igamc_series(a, x); + } + } else { + if (x * 1.1 < a) { + return 1.0 - igam_series(a, x); + } else { + return igamc_series(a, x); + } + } +} + + +/* Compute + * + * x^a * exp(-x) / gamma(a) + * + * corrected from (15) and (16) in [2] by replacing exp(x - a) with + * exp(a - x). + */ +double igam_fac(double a, double x) +{ + double ax, fac, res, num; + + if (fabs(a - x) > 0.4 * fabs(a)) { + ax = a * log(x) - x - lgam(a); + if (ax < -MAXLOG) { + sf_error("igam", SF_ERROR_UNDERFLOW, NULL); + return 0.0; + } + return exp(ax); + } + + fac = a + lanczos_g - 0.5; + res = sqrt(fac / exp(1)) / lanczos_sum_expg_scaled(a); + + if ((a < 200) && (x < 200)) { + res *= exp(a - x) * pow(x / fac, a); + } else { + num = x - a - lanczos_g + 0.5; + res *= exp(a * log1pmx(num / fac) + x * (0.5 - lanczos_g) / fac); + } + + return res; +} + + +/* Compute igamc using DLMF 8.9.2. */ +static double igamc_continued_fraction(double a, double x) +{ + int i; + double ans, ax, c, yc, r, t, y, z; + double pk, pkm1, pkm2, qk, qkm1, qkm2; + + ax = igam_fac(a, x); + if (ax == 0.0) { + return 0.0; + } + + /* continued fraction */ + y = 1.0 - a; + z = x + y + 1.0; + c = 0.0; + pkm2 = 1.0; + qkm2 = x; + pkm1 = x + 1.0; + qkm1 = z * x; + ans = pkm1 / qkm1; + + for (i = 0; i < MAXITER; i++) { + c += 1.0; + y += 1.0; + z += 2.0; + yc = y * c; + pk = pkm1 * z - pkm2 * yc; + qk = qkm1 * z - qkm2 * yc; + if (qk != 0) { + r = pk / qk; + t = fabs((ans - r) / r); + ans = r; + } + else + t = 1.0; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + if (fabs(pk) > big) { + pkm2 *= biginv; + pkm1 *= biginv; + qkm2 *= biginv; + qkm1 *= biginv; + } + if (t <= MACHEP) { + break; + } + } + + return (ans * ax); +} + + +/* Compute igam using DLMF 8.11.4. */ +static double igam_series(double a, double x) +{ + int i; + double ans, ax, c, r; + + ax = igam_fac(a, x); + if (ax == 0.0) { + return 0.0; + } + + /* power series */ + r = a; + c = 1.0; + ans = 1.0; + + for (i = 0; i < MAXITER; i++) { + r += 1.0; + c *= x / r; + ans += c; + if (c <= MACHEP * ans) { + break; + } + } + + return (ans * ax / a); +} + + +/* Compute igamc using DLMF 8.7.3. This is related to the series in + * igam_series but extra care is taken to avoid cancellation. + */ +static double igamc_series(double a, double x) +{ + int n; + double fac = 1; + double sum = 0; + double term, logx; + + for (n = 1; n < MAXITER; n++) { + fac *= -x / n; + term = fac / (a + n); + sum += term; + if (fabs(term) <= MACHEP * fabs(sum)) { + break; + } + } + + logx = log(x); + term = -expm1(a * logx - lgam1p(a)); + return term - exp(a * logx - lgam(a)) * sum; +} + + +/* Compute igam/igamc using DLMF 8.12.3/8.12.4. */ +static double asymptotic_series(double a, double x, int func) +{ + int k, n, sgn; + int maxpow = 0; + double lambda = x / a; + double sigma = (x - a) / a; + double eta, res, ck, ckterm, term, absterm; + double absoldterm = INFINITY; + double etapow[N] = {1}; + double sum = 0; + double afac = 1; + + if (func == IGAM) { + sgn = -1; + } else { + sgn = 1; + } + + if (lambda > 1) { + eta = sqrt(-2 * log1pmx(sigma)); + } else if (lambda < 1) { + eta = -sqrt(-2 * log1pmx(sigma)); + } else { + eta = 0; + } + res = 0.5 * erfc(sgn * eta * sqrt(a / 2)); + + for (k = 0; k < K; k++) { + ck = d[k][0]; + for (n = 1; n < N; n++) { + if (n > maxpow) { + etapow[n] = eta * etapow[n-1]; + maxpow += 1; + } + ckterm = d[k][n]*etapow[n]; + ck += ckterm; + if (fabs(ckterm) < MACHEP * fabs(ck)) { + break; + } + } + term = ck * afac; + absterm = fabs(term); + if (absterm > absoldterm) { + break; + } + sum += term; + if (absterm < MACHEP * fabs(sum)) { + break; + } + absoldterm = absterm; + afac /= a; + } + res += sgn * exp(-0.5 * a * eta * eta) * sum / sqrt(2 * M_PI * a); + + return res; +} diff --git a/gtsam/3rdparty/cephes/cephes/igam.h b/gtsam/3rdparty/cephes/cephes/igam.h new file mode 100644 index 000000000..0bc310633 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/igam.h @@ -0,0 +1,38 @@ +/* This file was automatically generated by _precomp/gammainc.py. + * Do not edit it manually! + */ + +#ifndef IGAM_H +#define IGAM_H + +#define K 25 +#define N 25 + +static const double d[K][N] = +{{-3.3333333333333333e-1, 8.3333333333333333e-2, -1.4814814814814815e-2, 1.1574074074074074e-3, 3.527336860670194e-4, -1.7875514403292181e-4, 3.9192631785224378e-5, -2.1854485106799922e-6, -1.85406221071516e-6, 8.296711340953086e-7, -1.7665952736826079e-7, 6.7078535434014986e-9, 1.0261809784240308e-8, -4.3820360184533532e-9, 9.1476995822367902e-10, -2.551419399494625e-11, -5.8307721325504251e-11, 2.4361948020667416e-11, -5.0276692801141756e-12, 1.1004392031956135e-13, 3.3717632624009854e-13, -1.3923887224181621e-13, 2.8534893807047443e-14, -5.1391118342425726e-16, -1.9752288294349443e-15}, +{-1.8518518518518519e-3, -3.4722222222222222e-3, 2.6455026455026455e-3, -9.9022633744855967e-4, 2.0576131687242798e-4, -4.0187757201646091e-7, -1.8098550334489978e-5, 7.6491609160811101e-6, -1.6120900894563446e-6, 4.6471278028074343e-9, 1.378633446915721e-7, -5.752545603517705e-8, 1.1951628599778147e-8, -1.7543241719747648e-11, -1.0091543710600413e-9, 4.1627929918425826e-10, -8.5639070264929806e-11, 6.0672151016047586e-14, 7.1624989648114854e-12, -2.9331866437714371e-12, 5.9966963656836887e-13, -2.1671786527323314e-16, -4.9783399723692616e-14, 2.0291628823713425e-14, -4.13125571381061e-15}, +{4.1335978835978836e-3, -2.6813271604938272e-3, 7.7160493827160494e-4, 2.0093878600823045e-6, -1.0736653226365161e-4, 5.2923448829120125e-5, -1.2760635188618728e-5, 3.4235787340961381e-8, 1.3721957309062933e-6, -6.298992138380055e-7, 1.4280614206064242e-7, -2.0477098421990866e-10, -1.4092529910867521e-8, 6.228974084922022e-9, -1.3670488396617113e-9, 9.4283561590146782e-13, 1.2872252400089318e-10, -5.5645956134363321e-11, 1.1975935546366981e-11, -4.1689782251838635e-15, -1.0940640427884594e-12, 4.6622399463901357e-13, -9.905105763906906e-14, 1.8931876768373515e-17, 8.8592218725911273e-15}, +{6.4943415637860082e-4, 2.2947209362139918e-4, -4.6918949439525571e-4, 2.6772063206283885e-4, -7.5618016718839764e-5, -2.3965051138672967e-7, 1.1082654115347302e-5, -5.6749528269915966e-6, 1.4230900732435884e-6, -2.7861080291528142e-11, -1.6958404091930277e-7, 8.0994649053880824e-8, -1.9111168485973654e-8, 2.3928620439808118e-12, 2.0620131815488798e-9, -9.4604966618551322e-10, 2.1541049775774908e-10, -1.388823336813903e-14, -2.1894761681963939e-11, 9.7909989511716851e-12, -2.1782191880180962e-12, 6.2088195734079014e-17, 2.126978363279737e-13, -9.3446887915174333e-14, 2.0453671226782849e-14}, +{-8.618882909167117e-4, 7.8403922172006663e-4, -2.9907248030319018e-4, -1.4638452578843418e-6, 6.6414982154651222e-5, -3.9683650471794347e-5, 1.1375726970678419e-5, 2.5074972262375328e-10, -1.6954149536558306e-6, 8.9075075322053097e-7, -2.2929348340008049e-7, 2.956794137544049e-11, 2.8865829742708784e-8, -1.4189739437803219e-8, 3.4463580499464897e-9, -2.3024517174528067e-13, -3.9409233028046405e-10, 1.8602338968504502e-10, -4.356323005056618e-11, 1.2786001016296231e-15, 4.6792750266579195e-12, -2.1492464706134829e-12, 4.9088156148096522e-13, -6.3385914848915603e-18, -5.0453320690800944e-14}, +{-3.3679855336635815e-4, -6.9728137583658578e-5, 2.7727532449593921e-4, -1.9932570516188848e-4, 6.7977804779372078e-5, 1.419062920643967e-7, -1.3594048189768693e-5, 8.0184702563342015e-6, -2.2914811765080952e-6, -3.252473551298454e-10, 3.4652846491085265e-7, -1.8447187191171343e-7, 4.8240967037894181e-8, -1.7989466721743515e-14, -6.3061945000135234e-9, 3.1624176287745679e-9, -7.8409242536974293e-10, 5.1926791652540407e-15, 9.3589442423067836e-11, -4.5134262161632782e-11, 1.0799129993116827e-11, -3.661886712685252e-17, -1.210902069055155e-12, 5.6807435849905643e-13, -1.3249659916340829e-13}, +{5.3130793646399222e-4, -5.9216643735369388e-4, 2.7087820967180448e-4, 7.9023532326603279e-7, -8.1539693675619688e-5, 5.6116827531062497e-5, -1.8329116582843376e-5, -3.0796134506033048e-9, 3.4651553688036091e-6, -2.0291327396058604e-6, 5.7887928631490037e-7, 2.338630673826657e-13, -8.8286007463304835e-8, 4.7435958880408128e-8, -1.2545415020710382e-8, 8.6496488580102925e-14, 1.6846058979264063e-9, -8.5754928235775947e-10, 2.1598224929232125e-10, -7.6132305204761539e-16, -2.6639822008536144e-11, 1.3065700536611057e-11, -3.1799163902367977e-12, 4.7109761213674315e-18, 3.6902800842763467e-13}, +{3.4436760689237767e-4, 5.1717909082605922e-5, -3.3493161081142236e-4, 2.812695154763237e-4, -1.0976582244684731e-4, -1.2741009095484485e-7, 2.7744451511563644e-5, -1.8263488805711333e-5, 5.7876949497350524e-6, 4.9387589339362704e-10, -1.0595367014026043e-6, 6.1667143761104075e-7, -1.7562973359060462e-7, -1.2974473287015439e-12, 2.695423606288966e-8, -1.4578352908731271e-8, 3.887645959386175e-9, -3.8810022510194121e-17, -5.3279941738772867e-10, 2.7437977643314845e-10, -6.9957960920705679e-11, 2.5899863874868481e-17, 8.8566890996696381e-12, -4.403168815871311e-12, 1.0865561947091654e-12}, +{-6.5262391859530942e-4, 8.3949872067208728e-4, -4.3829709854172101e-4, -6.969091458420552e-7, 1.6644846642067548e-4, -1.2783517679769219e-4, 4.6299532636913043e-5, 4.5579098679227077e-9, -1.0595271125805195e-5, 6.7833429048651666e-6, -2.1075476666258804e-6, -1.7213731432817145e-11, 3.7735877416110979e-7, -2.1867506700122867e-7, 6.2202288040189269e-8, 6.5977038267330006e-16, -9.5903864974256858e-9, 5.2132144922808078e-9, -1.3991589583935709e-9, 5.382058999060575e-16, 1.9484714275467745e-10, -1.0127287556389682e-10, 2.6077347197254926e-11, -5.0904186999932993e-18, -3.3721464474854592e-12}, +{-5.9676129019274625e-4, -7.2048954160200106e-5, 6.7823088376673284e-4, -6.4014752602627585e-4, 2.7750107634328704e-4, 1.8197008380465151e-7, -8.4795071170685032e-5, 6.105192082501531e-5, -2.1073920183404862e-5, -8.8585890141255994e-10, 4.5284535953805377e-6, -2.8427815022504408e-6, 8.7082341778646412e-7, 3.6886101871706965e-12, -1.5344695190702061e-7, 8.862466778790695e-8, -2.5184812301826817e-8, -1.0225912098215092e-14, 3.8969470758154777e-9, -2.1267304792235635e-9, 5.7370135528051385e-10, -1.887749850169741e-19, -8.0931538694657866e-11, 4.2382723283449199e-11, -1.1002224534207726e-11}, +{1.3324454494800656e-3, -1.9144384985654775e-3, 1.1089369134596637e-3, 9.932404122642299e-7, -5.0874501293093199e-4, 4.2735056665392884e-4, -1.6858853767910799e-4, -8.1301893922784998e-9, 4.5284402370562147e-5, -3.127053674781734e-5, 1.044986828530338e-5, 4.8435226265680926e-11, -2.1482565873456258e-6, 1.329369701097492e-6, -4.0295693092101029e-7, -1.7567877666323291e-13, 7.0145043163668257e-8, -4.040787734999483e-8, 1.1474026743371963e-8, 3.9642746853563325e-18, -1.7804938269892714e-9, 9.7480262548731646e-10, -2.6405338676507616e-10, 5.794875163403742e-18, 3.7647749553543836e-11}, +{1.579727660730835e-3, 1.6251626278391582e-4, -2.0633421035543276e-3, 2.1389686185689098e-3, -1.0108559391263003e-3, -3.9912705529919201e-7, 3.6235025084764691e-4, -2.8143901463712154e-4, 1.0449513336495887e-4, 2.1211418491830297e-9, -2.5779417251947842e-5, 1.7281818956040463e-5, -5.6413773872904282e-6, -1.1024320105776174e-11, 1.1223224418895175e-6, -6.8693396379526735e-7, 2.0653236975414887e-7, 4.6714772409838506e-14, -3.5609886164949055e-8, 2.0470855345905963e-8, -5.8091738633283358e-9, -1.332821287582869e-16, 9.0354604391335133e-10, -4.9598782517330834e-10, 1.3481607129399749e-10}, +{-4.0725121195140166e-3, 6.4033628338080698e-3, -4.0410161081676618e-3, -2.183732802866233e-6, 2.1740441801254639e-3, -1.9700440518418892e-3, 8.3595469747962458e-4, 1.9445447567109655e-8, -2.5779387120421696e-4, 1.9009987368139304e-4, -6.7696499937438965e-5, -1.4440629666426572e-10, 1.5712512518742269e-5, -1.0304008744776893e-5, 3.304517767401387e-6, 7.9829760242325709e-13, -6.4097794149313004e-7, 3.8894624761300056e-7, -1.1618347644948869e-7, -2.816808630596451e-15, 1.9878012911297093e-8, -1.1407719956357511e-8, 3.2355857064185555e-9, 4.1759468293455945e-20, -5.0423112718105824e-10}, +{-5.9475779383993003e-3, -5.4016476789260452e-4, 8.7910413550767898e-3, -9.8576315587856125e-3, 5.0134695031021538e-3, 1.2807521786221875e-6, -2.0626019342754683e-3, 1.7109128573523058e-3, -6.7695312714133799e-4, -6.9011545676562133e-9, 1.8855128143995902e-4, -1.3395215663491969e-4, 4.6263183033528039e-5, 4.0034230613321351e-11, -1.0255652921494033e-5, 6.612086372797651e-6, -2.0913022027253008e-6, -2.0951775649603837e-13, 3.9756029041993247e-7, -2.3956211978815887e-7, 7.1182883382145864e-8, 8.925574873053455e-16, -1.2101547235064676e-8, 6.9350618248334386e-9, -1.9661464453856102e-9}, +{1.7402027787522711e-2, -2.9527880945699121e-2, 2.0045875571402799e-2, 7.0289515966903407e-6, -1.2375421071343148e-2, 1.1976293444235254e-2, -5.4156038466518525e-3, -6.3290893396418616e-8, 1.8855118129005065e-3, -1.473473274825001e-3, 5.5515810097708387e-4, 5.2406834412550662e-10, -1.4357913535784836e-4, 9.9181293224943297e-5, -3.3460834749478311e-5, -3.5755837291098993e-12, 7.1560851960630076e-6, -4.5516802628155526e-6, 1.4236576649271475e-6, 1.8803149082089664e-14, -2.6623403898929211e-7, 1.5950642189595716e-7, -4.7187514673841102e-8, -6.5107872958755177e-17, 7.9795091026746235e-9}, +{3.0249124160905891e-2, 2.4817436002649977e-3, -4.9939134373457022e-2, 5.9915643009307869e-2, -3.2483207601623391e-2, -5.7212968652103441e-6, 1.5085251778569354e-2, -1.3261324005088445e-2, 5.5515262632426148e-3, 3.0263182257030016e-8, -1.7229548406756723e-3, 1.2893570099929637e-3, -4.6845138348319876e-4, -1.830259937893045e-10, 1.1449739014822654e-4, -7.7378565221244477e-5, 2.5625836246985201e-5, 1.0766165333192814e-12, -5.3246809282422621e-6, 3.349634863064464e-6, -1.0381253128684018e-6, -5.608909920621128e-15, 1.9150821930676591e-7, -1.1418365800203486e-7, 3.3654425209171788e-8}, +{-9.9051020880159045e-2, 1.7954011706123486e-1, -1.2989606383463778e-1, -3.1478872752284357e-5, 9.0510635276848131e-2, -9.2828824411184397e-2, 4.4412112839877808e-2, 2.7779236316835888e-7, -1.7229543805449697e-2, 1.4182925050891573e-2, -5.6214161633747336e-3, -2.39598509186381e-9, 1.6029634366079908e-3, -1.1606784674435773e-3, 4.1001337768153873e-4, 1.8365800754090661e-11, -9.5844256563655903e-5, 6.3643062337764708e-5, -2.076250624489065e-5, -1.1806020912804483e-13, 4.2131808239120649e-6, -2.6262241337012467e-6, 8.0770620494930662e-7, 6.0125912123632725e-16, -1.4729737374018841e-7}, +{-1.9994542198219728e-1, -1.5056113040026424e-2, 3.6470239469348489e-1, -4.6435192311733545e-1, 2.6640934719197893e-1, 3.4038266027147191e-5, -1.3784338709329624e-1, 1.276467178337056e-1, -5.6213828755200985e-2, -1.753150885483011e-7, 1.9235592956768113e-2, -1.5088821281095315e-2, 5.7401854451350123e-3, 1.0622382710310225e-9, -1.5335082692563998e-3, 1.0819320643228214e-3, -3.7372510193945659e-4, -6.6170909729031985e-12, 8.4263617380909628e-5, -5.5150706827483479e-5, 1.7769536448348069e-5, 3.8827923210205533e-14, -3.53513697488768e-6, 2.1865832130045269e-6, -6.6812849447625594e-7}, +{7.2438608504029431e-1, -1.3918010932653375, 1.0654143352413968, 1.876173868950258e-4, -8.2705501176152696e-1, 8.9352433347828414e-1, -4.4971003995291339e-1, -1.6107401567546652e-6, 1.9235590165271091e-1, -1.6597702160042609e-1, 6.8882222681814333e-2, 1.3910091724608687e-8, -2.146911561508663e-2, 1.6228980898865892e-2, -5.9796016172584256e-3, -1.1287469112826745e-10, 1.5167451119784857e-3, -1.0478634293553899e-3, 3.5539072889126421e-4, 8.1704322111801517e-13, -7.7773013442452395e-5, 5.0291413897007722e-5, -1.6035083867000518e-5, 1.2469354315487605e-14, 3.1369106244517615e-6}, +{1.6668949727276811, 1.165462765994632e-1, -3.3288393225018906, 4.4692325482864037, -2.6977693045875807, -2.600667859891061e-4, 1.5389017615694539, -1.4937962361134612, 6.8881964633233148e-1, 1.3077482004552385e-6, -2.5762963325596288e-1, 2.1097676102125449e-1, -8.3714408359219882e-2, -7.7920428881354753e-9, 2.4267923064833599e-2, -1.7813678334552311e-2, 6.3970330388900056e-3, 4.9430807090480523e-11, -1.5554602758465635e-3, 1.0561196919903214e-3, -3.5277184460472902e-4, 9.3002334645022459e-14, 7.5285855026557172e-5, -4.8186515569156351e-5, 1.5227271505597605e-5}, +{-6.6188298861372935, 1.3397985455142589e+1, -1.0789350606845146e+1, -1.4352254537875018e-3, 9.2333694596189809, -1.0456552819547769e+1, 5.5105526029033471, 1.2024439690716742e-5, -2.5762961164755816, 2.3207442745387179, -1.0045728797216284, -1.0207833290021914e-7, 3.3975092171169466e-1, -2.6720517450757468e-1, 1.0235252851562706e-1, 8.4329730484871625e-10, -2.7998284958442595e-2, 2.0066274144976813e-2, -7.0554368915086242e-3, 1.9402238183698188e-12, 1.6562888105449611e-3, -1.1082898580743683e-3, 3.654545161310169e-4, -5.1290032026971794e-11, -7.6340103696869031e-5}, +{-1.7112706061976095e+1, -1.1208044642899116, 3.7131966511885444e+1, -5.2298271025348962e+1, 3.3058589696624618e+1, 2.4791298976200222e-3, -2.061089403411526e+1, 2.088672775145582e+1, -1.0045703956517752e+1, -1.2238783449063012e-5, 4.0770134274221141, -3.473667358470195, 1.4329352617312006, 7.1359914411879712e-8, -4.4797257159115612e-1, 3.4112666080644461e-1, -1.2699786326594923e-1, -2.8953677269081528e-10, 3.3125776278259863e-2, -2.3274087021036101e-2, 8.0399993503648882e-3, -1.177805216235265e-9, -1.8321624891071668e-3, 1.2108282933588665e-3, -3.9479941246822517e-4}, +{7.389033153567425e+1, -1.5680141270402273e+2, 1.322177542759164e+2, 1.3692876877324546e-2, -1.2366496885920151e+2, 1.4620689391062729e+2, -8.0365587724865346e+1, -1.1259851148881298e-4, 4.0770132196179938e+1, -3.8210340013273034e+1, 1.719522294277362e+1, 9.3519707955168356e-7, -6.2716159907747034, 5.1168999071852637, -2.0319658112299095, -4.9507215582761543e-9, 5.9626397294332597e-1, -4.4220765337238094e-1, 1.6079998700166273e-1, -2.4733786203223402e-8, -4.0307574759979762e-2, 2.7849050747097869e-2, -9.4751858992054221e-3, 6.419922235909132e-6, 2.1250180774699461e-3}, +{2.1216837098382522e+2, 1.3107863022633868e+1, -4.9698285932871748e+2, 7.3121595266969204e+2, -4.8213821720890847e+2, -2.8817248692894889e-2, 3.2616720302947102e+2, -3.4389340280087117e+2, 1.7195193870816232e+2, 1.4038077378096158e-4, -7.52594195897599e+1, 6.651969984520934e+1, -2.8447519748152462e+1, -7.613702615875391e-7, 9.5402237105304373, -7.5175301113311376, 2.8943997568871961, -4.6612194999538201e-7, -8.0615149598794088e-1, 5.8483006570631029e-1, -2.0845408972964956e-1, 1.4765818959305817e-4, 5.1000433863753019e-2, -3.3066252141883665e-2, 1.5109265210467774e-2}, +{-9.8959643098322368e+2, 2.1925555360905233e+3, -1.9283586782723356e+3, -1.5925738122215253e-1, 1.9569985945919857e+3, -2.4072514765081556e+3, 1.3756149959336496e+3, 1.2920735237496668e-3, -7.525941715948055e+2, 7.3171668742208716e+2, -3.4137023466220065e+2, -9.9857390260608043e-6, 1.3356313181291573e+2, -1.1276295161252794e+2, 4.6310396098204458e+1, -7.9237387133614756e-6, -1.4510726927018646e+1, 1.1111771248100563e+1, -4.1690817945270892, 3.1008219800117808e-3, 1.1220095449981468, -7.6052379926149916e-1, 3.6262236505085254e-1, 2.216867741940747e-1, 4.8683443692930507e-1}}; + +#endif diff --git a/gtsam/3rdparty/cephes/cephes/igami.c b/gtsam/3rdparty/cephes/cephes/igami.c new file mode 100644 index 000000000..97fc93ff4 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/igami.c @@ -0,0 +1,339 @@ +/* + * (C) Copyright John Maddock 2006. + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ +#include "mconf.h" + +static double find_inverse_s(double, double); +static double didonato_SN(double, double, unsigned, double); +static double find_inverse_gamma(double, double, double); + + +static double find_inverse_s(double p, double q) +{ + /* + * 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. + * + * See equation 32. + */ + double s, t; + double a[4] = {0.213623493715853, 4.28342155967104, + 11.6616720288968, 3.31125922108741}; + double b[5] = {0.3611708101884203e-1, 1.27364489782223, + 6.40691597760039, 6.61053765625462, 1}; + + if (p < 0.5) { + t = sqrt(-2 * log(p)); + } + else { + t = sqrt(-2 * log(q)); + } + s = t - polevl(t, a, 3) / polevl(t, b, 4); + if(p < 0.5) + s = -s; + return s; +} + + +static double didonato_SN(double a, double x, unsigned N, double tolerance) +{ + /* + * 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. + * + * See equation 34. + */ + double sum = 1.0; + + if (N >= 1) { + unsigned i; + double partial = x / (a + 1); + + sum += partial; + for(i = 2; i <= N; ++i) { + partial *= x / (a + i); + sum += partial; + if(partial < tolerance) { + break; + } + } + } + return sum; +} + + +static double find_inverse_gamma(double a, double p, double q) +{ + /* + * In order to understand what's going on here, you will + * need to refer to: + * + * 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. + */ + double result; + + if (a == 1) { + if (q > 0.9) { + result = -log1p(-p); + } + else { + result = -log(q); + } + } + else if (a < 1) { + double g = Gamma(a); + double b = q * g; + + if ((b > 0.6) || ((b >= 0.45) && (a >= 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. + */ + double u; + if((b * q > 1e-8) && (q > 1e-5)) { + u = pow(p * g * a, 1 / a); + } + else { + u = exp((-q / a) - SCIPY_EULER); + } + result = u / (1 - (u / (a + 1))); + } + else if ((a < 0.3) && (b >= 0.35)) { + /* DiDonato & Morris Eq 22: */ + double t = exp(-SCIPY_EULER - b); + double u = t * exp(t); + result = t * exp(u); + } + else if ((b > 0.15) || (a >= 0.3)) { + /* DiDonato & Morris Eq 23: */ + double y = -log(b); + double 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: */ + double y = -log(b); + double 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: */ + double y = -log(b); + double c1 = (a - 1) * log(y); + double c1_2 = c1 * c1; + double c1_3 = c1_2 * c1; + double c1_4 = c1_2 * c1_2; + double a_2 = a * a; + double a_3 = a_2 * a; + + double c2 = (a - 1) * (1 + c1); + double c3 = (a - 1) * (-(c1_2 / 2) + + (a - 2) * c1 + + (3 * a - 5) / 2); + double 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); + double 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); + + double y_2 = y * y; + double y_3 = y_2 * y; + double 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 31: */ + double s = find_inverse_s(p, q); + + double s_2 = s * s; + double s_3 = s_2 * s; + double s_4 = s_2 * s_2; + double s_5 = s_4 * s; + double ra = sqrt(a); + + double w = a + s * ra + (s_2 - 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; + } + else if (p > 0.5) { + if (w < 3 * a) { + result = w; + } + else { + double D = fmax(2, a * (a - 1)); + double lg = lgam(a); + double lb = log(q) + lg; + if (lb < -D * 2.3) { + /* DiDonato and Morris Eq 25: */ + double y = -lb; + double c1 = (a - 1) * log(y); + double c1_2 = c1 * c1; + double c1_3 = c1_2 * c1; + double c1_4 = c1_2 * c1_2; + double a_2 = a * a; + double a_3 = a_2 * a; + + double c2 = (a - 1) * (1 + c1); + double c3 = (a - 1) * (-(c1_2 / 2) + + (a - 2) * c1 + + (3 * a - 5) / 2); + double 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); + double 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); + + double y_2 = y * y; + double y_3 = y_2 * y; + double 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: */ + double 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 { + double z = w; + double ap1 = a + 1; + double ap2 = a + 2; + if (w < 0.15 * ap1) { + /* DiDonato and Morris Eq 35: */ + double v = log(p) + lgam(ap1); + z = exp((v + w) / a); + s = log1p(z / ap1 * (1 + z / ap2)); + z = exp((v + z - s) / a); + s = log1p(z / ap1 * (1 + z / ap2)); + z = exp((v + z - s) / a); + s = 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; + } + else { + /* DiDonato and Morris Eq 36: */ + double ls = log(didonato_SN(a, z, 100, 1e-4)); + double v = log(p) + lgam(ap1); + z = exp((v + z - ls) / a); + result = z * (1 - (a * log(z) - z - v + ls) / (a - z)); + } + } + } + return result; +} + + +double igami(double a, double p) +{ + int i; + double x, fac, f_fp, fpp_fp; + + if (isnan(a) || isnan(p)) { + return NAN; + } + else if ((a < 0) || (p < 0) || (p > 1)) { + sf_error("gammaincinv", SF_ERROR_DOMAIN, NULL); + } + else if (p == 0.0) { + return 0.0; + } + else if (p == 1.0) { + return INFINITY; + } + else if (p > 0.9) { + return igamci(a, 1 - p); + } + + x = find_inverse_gamma(a, p, 1 - p); + /* Halley's method */ + for (i = 0; i < 3; i++) { + fac = igam_fac(a, x); + if (fac == 0.0) { + return x; + } + f_fp = (igam(a, x) - p) * x / fac; + /* The ratio of the first and second derivatives simplifies */ + fpp_fp = -1.0 + (a - 1) / x; + if (isinf(fpp_fp)) { + /* Resort to Newton's method in the case of overflow */ + x = x - f_fp; + } + else { + x = x - f_fp / (1.0 - 0.5 * f_fp * fpp_fp); + } + } + + return x; +} + + +double igamci(double a, double q) +{ + int i; + double x, fac, f_fp, fpp_fp; + + if (isnan(a) || isnan(q)) { + return NAN; + } + else if ((a < 0.0) || (q < 0.0) || (q > 1.0)) { + sf_error("gammainccinv", SF_ERROR_DOMAIN, NULL); + } + else if (q == 0.0) { + return INFINITY; + } + else if (q == 1.0) { + return 0.0; + } + else if (q > 0.9) { + return igami(a, 1 - q); + } + + x = find_inverse_gamma(a, 1 - q, q); + for (i = 0; i < 3; i++) { + fac = igam_fac(a, x); + if (fac == 0.0) { + return x; + } + f_fp = (igamc(a, x) - q) * x / (-fac); + fpp_fp = -1.0 + (a - 1) / x; + if (isinf(fpp_fp)) { + x = x - f_fp; + } + else { + x = x - f_fp / (1.0 - 0.5 * f_fp * fpp_fp); + } + } + + return x; +} diff --git a/gtsam/3rdparty/cephes/cephes/incbet.c b/gtsam/3rdparty/cephes/cephes/incbet.c new file mode 100644 index 000000000..b03427f4f --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/incbet.c @@ -0,0 +1,369 @@ +/* incbet.c + * + * Incomplete beta integral + * + * + * SYNOPSIS: + * + * double a, b, x, y, incbet(); + * + * y = incbet( a, b, x ); + * + * + * DESCRIPTION: + * + * Returns incomplete beta integral of the arguments, evaluated + * from zero to x. The function is defined as + * + * x + * - - + * | (a+b) | | a-1 b-1 + * ----------- | t (1-t) dt. + * - - | | + * | (a) | (b) - + * 0 + * + * The domain of definition is 0 <= x <= 1. In this + * implementation a and b are restricted to positive values. + * The integral from x to 1 may be obtained by the symmetry + * relation + * + * 1 - incbet( a, b, x ) = incbet( b, a, 1-x ). + * + * The integral is evaluated by a continued fraction expansion + * or, when b*x is small, by a power series. + * + * ACCURACY: + * + * Tested at uniformly distributed random points (a,b,x) with a and b + * in "domain" and x between 0 and 1. + * Relative error + * arithmetic domain # trials peak rms + * IEEE 0,5 10000 6.9e-15 4.5e-16 + * IEEE 0,85 250000 2.2e-13 1.7e-14 + * IEEE 0,1000 30000 5.3e-12 6.3e-13 + * IEEE 0,10000 250000 9.3e-11 7.1e-12 + * IEEE 0,100000 10000 8.7e-10 4.8e-11 + * Outputs smaller than the IEEE gradual underflow threshold + * were excluded from these statistics. + * + * ERROR MESSAGES: + * message condition value returned + * incbet domain x<0, x>1 0.0 + * incbet underflow 0.0 + */ + + +/* + * Cephes Math Library, Release 2.3: March, 1995 + * Copyright 1984, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +#define MAXGAM 171.624376956302725 + +extern double MACHEP, MINLOG, MAXLOG; + +static double big = 4.503599627370496e15; +static double biginv = 2.22044604925031308085e-16; + + +/* Power series for incomplete beta integral. + * Use when b*x is small and x not too close to 1. */ + +static double pseries(double a, double b, double x) +{ + double s, t, u, v, n, t1, z, ai; + + ai = 1.0 / a; + u = (1.0 - b) * x; + v = u / (a + 1.0); + t1 = v; + t = u; + n = 2.0; + s = 0.0; + z = MACHEP * ai; + while (fabs(v) > z) { + u = (n - b) * x / n; + t *= u; + v = t / (a + n); + s += v; + n += 1.0; + } + s += t1; + s += ai; + + u = a * log(x); + if ((a + b) < MAXGAM && fabs(u) < MAXLOG) { + t = 1.0 / beta(a, b); + s = s * t * pow(x, a); + } + else { + t = -lbeta(a,b) + u + log(s); + if (t < MINLOG) + s = 0.0; + else + s = exp(t); + } + return (s); +} + + +/* Continued fraction expansion #1 for incomplete beta integral */ + +static double incbcf(double a, double b, double x) +{ + double xk, pk, pkm1, pkm2, qk, qkm1, qkm2; + double k1, k2, k3, k4, k5, k6, k7, k8; + double r, t, ans, thresh; + int n; + + k1 = a; + k2 = a + b; + k3 = a; + k4 = a + 1.0; + k5 = 1.0; + k6 = b - 1.0; + k7 = k4; + k8 = a + 2.0; + + pkm2 = 0.0; + qkm2 = 1.0; + pkm1 = 1.0; + qkm1 = 1.0; + ans = 1.0; + r = 1.0; + n = 0; + thresh = 3.0 * MACHEP; + do { + + xk = -(x * k1 * k2) / (k3 * k4); + pk = pkm1 + pkm2 * xk; + qk = qkm1 + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + xk = (x * k5 * k6) / (k7 * k8); + pk = pkm1 + pkm2 * xk; + qk = qkm1 + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + if (qk != 0) + r = pk / qk; + if (r != 0) { + t = fabs((ans - r) / r); + ans = r; + } + else + t = 1.0; + + if (t < thresh) + goto cdone; + + k1 += 1.0; + k2 += 1.0; + k3 += 2.0; + k4 += 2.0; + k5 += 1.0; + k6 -= 1.0; + k7 += 2.0; + k8 += 2.0; + + if ((fabs(qk) + fabs(pk)) > big) { + pkm2 *= biginv; + pkm1 *= biginv; + qkm2 *= biginv; + qkm1 *= biginv; + } + if ((fabs(qk) < biginv) || (fabs(pk) < biginv)) { + pkm2 *= big; + pkm1 *= big; + qkm2 *= big; + qkm1 *= big; + } + } + while (++n < 300); + + cdone: + return (ans); +} + + +/* Continued fraction expansion #2 for incomplete beta integral */ + +static double incbd(double a, double b, double x) +{ + double xk, pk, pkm1, pkm2, qk, qkm1, qkm2; + double k1, k2, k3, k4, k5, k6, k7, k8; + double r, t, ans, z, thresh; + int n; + + k1 = a; + k2 = b - 1.0; + k3 = a; + k4 = a + 1.0; + k5 = 1.0; + k6 = a + b; + k7 = a + 1.0;; + k8 = a + 2.0; + + pkm2 = 0.0; + qkm2 = 1.0; + pkm1 = 1.0; + qkm1 = 1.0; + z = x / (1.0 - x); + ans = 1.0; + r = 1.0; + n = 0; + thresh = 3.0 * MACHEP; + do { + + xk = -(z * k1 * k2) / (k3 * k4); + pk = pkm1 + pkm2 * xk; + qk = qkm1 + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + xk = (z * k5 * k6) / (k7 * k8); + pk = pkm1 + pkm2 * xk; + qk = qkm1 + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + if (qk != 0) + r = pk / qk; + if (r != 0) { + t = fabs((ans - r) / r); + ans = r; + } + else + t = 1.0; + + if (t < thresh) + goto cdone; + + k1 += 1.0; + k2 -= 1.0; + k3 += 2.0; + k4 += 2.0; + k5 += 1.0; + k6 += 1.0; + k7 += 2.0; + k8 += 2.0; + + if ((fabs(qk) + fabs(pk)) > big) { + pkm2 *= biginv; + pkm1 *= biginv; + qkm2 *= biginv; + qkm1 *= biginv; + } + if ((fabs(qk) < biginv) || (fabs(pk) < biginv)) { + pkm2 *= big; + pkm1 *= big; + qkm2 *= big; + qkm1 *= big; + } + } + while (++n < 300); + cdone: + return (ans); +} + + +double incbet(double aa, double bb, double xx) +{ + double a, b, t, x, xc, w, y; + int flag; + + if (aa <= 0.0 || bb <= 0.0) + goto domerr; + + if ((xx <= 0.0) || (xx >= 1.0)) { + if (xx == 0.0) + return (0.0); + if (xx == 1.0) + return (1.0); + domerr: + sf_error("incbet", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + flag = 0; + if ((bb * xx) <= 1.0 && xx <= 0.95) { + t = pseries(aa, bb, xx); + goto done; + } + + w = 1.0 - xx; + + /* Reverse a and b if x is greater than the mean. */ + if (xx > (aa / (aa + bb))) { + flag = 1; + a = bb; + b = aa; + xc = xx; + x = w; + } + else { + a = aa; + b = bb; + xc = w; + x = xx; + } + + if (flag == 1 && (b * x) <= 1.0 && x <= 0.95) { + t = pseries(a, b, x); + goto done; + } + + /* Choose expansion for better convergence. */ + y = x * (a + b - 2.0) - (a - 1.0); + if (y < 0.0) + w = incbcf(a, b, x); + else + w = incbd(a, b, x) / xc; + + /* Multiply w by the factor + * a b _ _ _ + * x (1-x) | (a+b) / ( a | (a) | (b) ) . */ + + y = a * log(x); + t = b * log(xc); + if ((a + b) < MAXGAM && fabs(y) < MAXLOG && fabs(t) < MAXLOG) { + t = pow(xc, b); + t *= pow(x, a); + t /= a; + t *= w; + t *= 1.0 / beta(a, b); + goto done; + } + /* Resort to logarithms. */ + y += t - lbeta(a,b); + y += log(w / a); + if (y < MINLOG) + t = 0.0; + else + t = exp(y); + + done: + + if (flag == 1) { + if (t <= MACHEP) + t = 1.0 - MACHEP; + else + t = 1.0 - t; + } + return (t); +} + + diff --git a/gtsam/3rdparty/cephes/cephes/incbi.c b/gtsam/3rdparty/cephes/cephes/incbi.c new file mode 100644 index 000000000..747c43f53 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/incbi.c @@ -0,0 +1,275 @@ +/* incbi() + * + * Inverse of incomplete beta integral + * + * + * + * SYNOPSIS: + * + * double a, b, x, y, incbi(); + * + * x = incbi( a, b, y ); + * + * + * + * DESCRIPTION: + * + * Given y, the function finds x such that + * + * incbet( a, b, x ) = y . + * + * The routine performs interval halving or Newton iterations to find the + * root of incbet(a,b,x) - y = 0. + * + * + * ACCURACY: + * + * Relative error: + * x a,b + * arithmetic domain domain # trials peak rms + * IEEE 0,1 .5,10000 50000 5.8e-12 1.3e-13 + * IEEE 0,1 .25,100 100000 1.8e-13 3.9e-15 + * IEEE 0,1 0,5 50000 1.1e-12 5.5e-15 + * VAX 0,1 .5,100 25000 3.5e-14 1.1e-15 + * With a and b constrained to half-integer or integer values: + * IEEE 0,1 .5,10000 50000 5.8e-12 1.1e-13 + * IEEE 0,1 .5,100 100000 1.7e-14 7.9e-16 + * With a = .5, b constrained to half-integer or integer values: + * IEEE 0,1 .5,10000 10000 8.3e-11 1.0e-11 + */ + + +/* + * Cephes Math Library Release 2.4: March,1996 + * Copyright 1984, 1996 by Stephen L. Moshier + */ + +#include "mconf.h" + +extern double MACHEP, MAXLOG, MINLOG; + +double incbi(double aa, double bb, double yy0) +{ + double a, b, y0, d, y, x, x0, x1, lgm, yp, di, dithresh, yl, yh, xt; + int i, rflg, dir, nflg; + + + i = 0; + if (yy0 <= 0) + return (0.0); + if (yy0 >= 1.0) + return (1.0); + x0 = 0.0; + yl = 0.0; + x1 = 1.0; + yh = 1.0; + nflg = 0; + + if (aa <= 1.0 || bb <= 1.0) { + dithresh = 1.0e-6; + rflg = 0; + a = aa; + b = bb; + y0 = yy0; + x = a / (a + b); + y = incbet(a, b, x); + goto ihalve; + } + else { + dithresh = 1.0e-4; + } + /* approximation to inverse function */ + + yp = -ndtri(yy0); + + if (yy0 > 0.5) { + rflg = 1; + a = bb; + b = aa; + y0 = 1.0 - yy0; + yp = -yp; + } + else { + rflg = 0; + a = aa; + b = bb; + y0 = yy0; + } + + lgm = (yp * yp - 3.0) / 6.0; + x = 2.0 / (1.0 / (2.0 * a - 1.0) + 1.0 / (2.0 * b - 1.0)); + d = yp * sqrt(x + lgm) / x + - (1.0 / (2.0 * b - 1.0) - 1.0 / (2.0 * a - 1.0)) + * (lgm + 5.0 / 6.0 - 2.0 / (3.0 * x)); + d = 2.0 * d; + if (d < MINLOG) { + x = 1.0; + goto under; + } + x = a / (a + b * exp(d)); + y = incbet(a, b, x); + yp = (y - y0) / y0; + if (fabs(yp) < 0.2) + goto newt; + + /* Resort to interval halving if not close enough. */ + ihalve: + + dir = 0; + di = 0.5; + for (i = 0; i < 100; i++) { + if (i != 0) { + x = x0 + di * (x1 - x0); + if (x == 1.0) + x = 1.0 - MACHEP; + if (x == 0.0) { + di = 0.5; + x = x0 + di * (x1 - x0); + if (x == 0.0) + goto under; + } + y = incbet(a, b, x); + yp = (x1 - x0) / (x1 + x0); + if (fabs(yp) < dithresh) + goto newt; + yp = (y - y0) / y0; + if (fabs(yp) < dithresh) + goto newt; + } + if (y < y0) { + x0 = x; + yl = y; + if (dir < 0) { + dir = 0; + di = 0.5; + } + else if (dir > 3) + di = 1.0 - (1.0 - di) * (1.0 - di); + else if (dir > 1) + di = 0.5 * di + 0.5; + else + di = (y0 - y) / (yh - yl); + dir += 1; + if (x0 > 0.75) { + if (rflg == 1) { + rflg = 0; + a = aa; + b = bb; + y0 = yy0; + } + else { + rflg = 1; + a = bb; + b = aa; + y0 = 1.0 - yy0; + } + x = 1.0 - x; + y = incbet(a, b, x); + x0 = 0.0; + yl = 0.0; + x1 = 1.0; + yh = 1.0; + goto ihalve; + } + } + else { + x1 = x; + if (rflg == 1 && x1 < MACHEP) { + x = 0.0; + goto done; + } + yh = y; + if (dir > 0) { + dir = 0; + di = 0.5; + } + else if (dir < -3) + di = di * di; + else if (dir < -1) + di = 0.5 * di; + else + di = (y - y0) / (yh - yl); + dir -= 1; + } + } + sf_error("incbi", SF_ERROR_LOSS, NULL); + if (x0 >= 1.0) { + x = 1.0 - MACHEP; + goto done; + } + if (x <= 0.0) { + under: + sf_error("incbi", SF_ERROR_UNDERFLOW, NULL); + x = 0.0; + goto done; + } + + newt: + + if (nflg) + goto done; + nflg = 1; + lgm = lgam(a + b) - lgam(a) - lgam(b); + + for (i = 0; i < 8; i++) { + /* Compute the function at this point. */ + if (i != 0) + y = incbet(a, b, x); + if (y < yl) { + x = x0; + y = yl; + } + else if (y > yh) { + x = x1; + y = yh; + } + else if (y < y0) { + x0 = x; + yl = y; + } + else { + x1 = x; + yh = y; + } + if (x == 1.0 || x == 0.0) + break; + /* Compute the derivative of the function at this point. */ + d = (a - 1.0) * log(x) + (b - 1.0) * log(1.0 - x) + lgm; + if (d < MINLOG) + goto done; + if (d > MAXLOG) + break; + d = exp(d); + /* Compute the step to the next approximation of x. */ + d = (y - y0) / d; + xt = x - d; + if (xt <= x0) { + y = (x - x0) / (x1 - x0); + xt = x0 + 0.5 * y * (x - x0); + if (xt <= 0.0) + break; + } + if (xt >= x1) { + y = (x1 - x) / (x1 - x0); + xt = x1 - 0.5 * y * (x1 - x); + if (xt >= 1.0) + break; + } + x = xt; + if (fabs(d / x) < 128.0 * MACHEP) + goto done; + } + /* Did not converge. */ + dithresh = 256.0 * MACHEP; + goto ihalve; + + done: + + if (rflg) { + if (x <= MACHEP) + x = 1.0 - MACHEP; + else + x = 1.0 - x; + } + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/j0.c b/gtsam/3rdparty/cephes/cephes/j0.c new file mode 100644 index 000000000..094ef6cef --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/j0.c @@ -0,0 +1,246 @@ +/* j0.c + * + * Bessel function of order zero + * + * + * + * SYNOPSIS: + * + * double x, y, j0(); + * + * y = j0( x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of order zero of the argument. + * + * The domain is divided into the intervals [0, 5] and + * (5, infinity). In the first interval the following rational + * approximation is used: + * + * + * 2 2 + * (w - r ) (w - r ) P (w) / Q (w) + * 1 2 3 8 + * + * 2 + * where w = x and the two r's are zeros of the function. + * + * In the second interval, the Hankel asymptotic expansion + * is employed with two rational functions of degree 6/6 + * and 7/7. + * + * + * + * ACCURACY: + * + * Absolute error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 60000 4.2e-16 1.1e-16 + * + */ + /* y0.c + * + * Bessel function of the second kind, order zero + * + * + * + * SYNOPSIS: + * + * double x, y, y0(); + * + * y = y0( x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of the second kind, of order + * zero, of the argument. + * + * The domain is divided into the intervals [0, 5] and + * (5, infinity). In the first interval a rational approximation + * R(x) is employed to compute + * y0(x) = R(x) + 2 * log(x) * j0(x) / M_PI. + * Thus a call to j0() is required. + * + * In the second interval, the Hankel asymptotic expansion + * is employed with two rational functions of degree 6/6 + * and 7/7. + * + * + * + * ACCURACY: + * + * Absolute error, when y0(x) < 1; else relative error: + * + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.3e-15 1.6e-16 + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier + */ + +/* Note: all coefficients satisfy the relative error criterion + * except YP, YQ which are designed for absolute error. */ + +#include "mconf.h" + +static double PP[7] = { + 7.96936729297347051624E-4, + 8.28352392107440799803E-2, + 1.23953371646414299388E0, + 5.44725003058768775090E0, + 8.74716500199817011941E0, + 5.30324038235394892183E0, + 9.99999999999999997821E-1, +}; + +static double PQ[7] = { + 9.24408810558863637013E-4, + 8.56288474354474431428E-2, + 1.25352743901058953537E0, + 5.47097740330417105182E0, + 8.76190883237069594232E0, + 5.30605288235394617618E0, + 1.00000000000000000218E0, +}; + +static double QP[8] = { + -1.13663838898469149931E-2, + -1.28252718670509318512E0, + -1.95539544257735972385E1, + -9.32060152123768231369E1, + -1.77681167980488050595E2, + -1.47077505154951170175E2, + -5.14105326766599330220E1, + -6.05014350600728481186E0, +}; + +static double QQ[7] = { + /* 1.00000000000000000000E0, */ + 6.43178256118178023184E1, + 8.56430025976980587198E2, + 3.88240183605401609683E3, + 7.24046774195652478189E3, + 5.93072701187316984827E3, + 2.06209331660327847417E3, + 2.42005740240291393179E2, +}; + +static double YP[8] = { + 1.55924367855235737965E4, + -1.46639295903971606143E7, + 5.43526477051876500413E9, + -9.82136065717911466409E11, + 8.75906394395366999549E13, + -3.46628303384729719441E15, + 4.42733268572569800351E16, + -1.84950800436986690637E16, +}; + +static double YQ[7] = { + /* 1.00000000000000000000E0, */ + 1.04128353664259848412E3, + 6.26107330137134956842E5, + 2.68919633393814121987E8, + 8.64002487103935000337E10, + 2.02979612750105546709E13, + 3.17157752842975028269E15, + 2.50596256172653059228E17, +}; + +/* 5.783185962946784521175995758455807035071 */ +static double DR1 = 5.78318596294678452118E0; + +/* 30.47126234366208639907816317502275584842 */ +static double DR2 = 3.04712623436620863991E1; + +static double RP[4] = { + -4.79443220978201773821E9, + 1.95617491946556577543E12, + -2.49248344360967716204E14, + 9.70862251047306323952E15, +}; + +static double RQ[8] = { + /* 1.00000000000000000000E0, */ + 4.99563147152651017219E2, + 1.73785401676374683123E5, + 4.84409658339962045305E7, + 1.11855537045356834862E10, + 2.11277520115489217587E12, + 3.10518229857422583814E14, + 3.18121955943204943306E16, + 1.71086294081043136091E18, +}; + +extern double SQ2OPI; + +double j0(double x) +{ + double w, z, p, q, xn; + + if (x < 0) + x = -x; + + if (x <= 5.0) { + z = x * x; + if (x < 1.0e-5) + return (1.0 - z / 4.0); + + p = (z - DR1) * (z - DR2); + p = p * polevl(z, RP, 3) / p1evl(z, RQ, 8); + return (p); + } + + w = 5.0 / x; + q = 25.0 / (x * x); + p = polevl(q, PP, 6) / polevl(q, PQ, 6); + q = polevl(q, QP, 7) / p1evl(q, QQ, 7); + xn = x - M_PI_4; + p = p * cos(xn) - w * q * sin(xn); + return (p * SQ2OPI / sqrt(x)); +} + +/* y0() 2 */ +/* Bessel function of second kind, order zero */ + +/* Rational approximation coefficients YP[], YQ[] are used here. + * The function computed is y0(x) - 2 * log(x) * j0(x) / M_PI, + * whose value at x = 0 is 2 * ( log(0.5) + EUL ) / M_PI + * = 0.073804295108687225. + */ + +double y0(double x) +{ + double w, z, p, q, xn; + + if (x <= 5.0) { + if (x == 0.0) { + sf_error("y0", SF_ERROR_SINGULAR, NULL); + return -INFINITY; + } + else if (x < 0.0) { + sf_error("y0", SF_ERROR_DOMAIN, NULL); + return NAN; + } + z = x * x; + w = polevl(z, YP, 7) / p1evl(z, YQ, 7); + w += M_2_PI * log(x) * j0(x); + return (w); + } + + w = 5.0 / x; + z = 25.0 / (x * x); + p = polevl(z, PP, 6) / polevl(z, PQ, 6); + q = polevl(z, QP, 7) / p1evl(z, QQ, 7); + xn = x - M_PI_4; + p = p * sin(xn) + w * q * cos(xn); + return (p * SQ2OPI / sqrt(x)); +} diff --git a/gtsam/3rdparty/cephes/cephes/j1.c b/gtsam/3rdparty/cephes/cephes/j1.c new file mode 100644 index 000000000..123194de8 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/j1.c @@ -0,0 +1,225 @@ +/* j1.c + * + * Bessel function of order one + * + * + * + * SYNOPSIS: + * + * double x, y, j1(); + * + * y = j1( x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of order one of the argument. + * + * The domain is divided into the intervals [0, 8] and + * (8, infinity). In the first interval a 24 term Chebyshev + * expansion is used. In the second, the asymptotic + * trigonometric representation is employed using two + * rational functions of degree 5/5. + * + * + * + * ACCURACY: + * + * Absolute error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 2.6e-16 1.1e-16 + * + * + */ + /* y1.c + * + * Bessel function of second kind of order one + * + * + * + * SYNOPSIS: + * + * double x, y, y1(); + * + * y = y1( x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of the second kind of order one + * of the argument. + * + * The domain is divided into the intervals [0, 8] and + * (8, infinity). In the first interval a 25 term Chebyshev + * expansion is used, and a call to j1() is required. + * In the second, the asymptotic trigonometric representation + * is employed using two rational functions of degree 5/5. + * + * + * + * ACCURACY: + * + * Absolute error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.0e-15 1.3e-16 + * + * (error criterion relative when |y1| > 1). + * + */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1989, 2000 by Stephen L. Moshier + */ + +/* + * #define PIO4 .78539816339744830962 + * #define THPIO4 2.35619449019234492885 + * #define SQ2OPI .79788456080286535588 + */ + +#include "mconf.h" + +static double RP[4] = { + -8.99971225705559398224E8, + 4.52228297998194034323E11, + -7.27494245221818276015E13, + 3.68295732863852883286E15, +}; + +static double RQ[8] = { + /* 1.00000000000000000000E0, */ + 6.20836478118054335476E2, + 2.56987256757748830383E5, + 8.35146791431949253037E7, + 2.21511595479792499675E10, + 4.74914122079991414898E12, + 7.84369607876235854894E14, + 8.95222336184627338078E16, + 5.32278620332680085395E18, +}; + +static double PP[7] = { + 7.62125616208173112003E-4, + 7.31397056940917570436E-2, + 1.12719608129684925192E0, + 5.11207951146807644818E0, + 8.42404590141772420927E0, + 5.21451598682361504063E0, + 1.00000000000000000254E0, +}; + +static double PQ[7] = { + 5.71323128072548699714E-4, + 6.88455908754495404082E-2, + 1.10514232634061696926E0, + 5.07386386128601488557E0, + 8.39985554327604159757E0, + 5.20982848682361821619E0, + 9.99999999999999997461E-1, +}; + +static double QP[8] = { + 5.10862594750176621635E-2, + 4.98213872951233449420E0, + 7.58238284132545283818E1, + 3.66779609360150777800E2, + 7.10856304998926107277E2, + 5.97489612400613639965E2, + 2.11688757100572135698E2, + 2.52070205858023719784E1, +}; + +static double QQ[7] = { + /* 1.00000000000000000000E0, */ + 7.42373277035675149943E1, + 1.05644886038262816351E3, + 4.98641058337653607651E3, + 9.56231892404756170795E3, + 7.99704160447350683650E3, + 2.82619278517639096600E3, + 3.36093607810698293419E2, +}; + +static double YP[6] = { + 1.26320474790178026440E9, + -6.47355876379160291031E11, + 1.14509511541823727583E14, + -8.12770255501325109621E15, + 2.02439475713594898196E17, + -7.78877196265950026825E17, +}; + +static double YQ[8] = { + /* 1.00000000000000000000E0, */ + 5.94301592346128195359E2, + 2.35564092943068577943E5, + 7.34811944459721705660E7, + 1.87601316108706159478E10, + 3.88231277496238566008E12, + 6.20557727146953693363E14, + 6.87141087355300489866E16, + 3.97270608116560655612E18, +}; + + +static double Z1 = 1.46819706421238932572E1; +static double Z2 = 4.92184563216946036703E1; + +extern double THPIO4, SQ2OPI; + +double j1(double x) +{ + double w, z, p, q, xn; + + w = x; + if (x < 0) + return -j1(-x); + + if (w <= 5.0) { + z = x * x; + w = polevl(z, RP, 3) / p1evl(z, RQ, 8); + w = w * x * (z - Z1) * (z - Z2); + return (w); + } + + w = 5.0 / x; + z = w * w; + p = polevl(z, PP, 6) / polevl(z, PQ, 6); + q = polevl(z, QP, 7) / p1evl(z, QQ, 7); + xn = x - THPIO4; + p = p * cos(xn) - w * q * sin(xn); + return (p * SQ2OPI / sqrt(x)); +} + + +double y1(double x) +{ + double w, z, p, q, xn; + + if (x <= 5.0) { + if (x == 0.0) { + sf_error("y1", SF_ERROR_SINGULAR, NULL); + return -INFINITY; + } + else if (x <= 0.0) { + sf_error("y1", SF_ERROR_DOMAIN, NULL); + return NAN; + } + z = x * x; + w = x * (polevl(z, YP, 5) / p1evl(z, YQ, 8)); + w += M_2_PI * (j1(x) * log(x) - 1.0 / x); + return (w); + } + + w = 5.0 / x; + z = w * w; + p = polevl(z, PP, 6) / polevl(z, PQ, 6); + q = polevl(z, QP, 7) / p1evl(z, QQ, 7); + xn = x - THPIO4; + p = p * sin(xn) + w * q * cos(xn); + return (p * SQ2OPI / sqrt(x)); +} diff --git a/gtsam/3rdparty/cephes/cephes/jv.c b/gtsam/3rdparty/cephes/cephes/jv.c new file mode 100644 index 000000000..3434c18f3 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/jv.c @@ -0,0 +1,841 @@ +/* jv.c + * + * Bessel function of noninteger order + * + * + * + * SYNOPSIS: + * + * double v, x, y, jv(); + * + * y = jv( v, x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of order v of the argument, + * where v is real. Negative x is allowed if v is an integer. + * + * Several expansions are included: the ascending power + * series, the Hankel expansion, and two transitional + * expansions for large v. If v is not too large, it + * is reduced by recurrence to a region of best accuracy. + * The transitional expansions give 12D accuracy for v > 500. + * + * + * + * ACCURACY: + * Results for integer v are indicated by *, where x and v + * both vary from -125 to +125. Otherwise, + * x ranges from 0 to 125, v ranges as indicated by "domain." + * Error criterion is absolute, except relative when |jv()| > 1. + * + * arithmetic v domain x domain # trials peak rms + * IEEE 0,125 0,125 100000 4.6e-15 2.2e-16 + * IEEE -125,0 0,125 40000 5.4e-11 3.7e-13 + * IEEE 0,500 0,500 20000 4.4e-15 4.0e-16 + * Integer v: + * IEEE -125,125 -125,125 50000 3.5e-15* 1.9e-16* + * + */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1989, 1992, 2000 by Stephen L. Moshier + */ + + +#include "mconf.h" +#define CEPHES_DEBUG 0 + +#if CEPHES_DEBUG +#include +#endif + +#define MAXGAM 171.624376956302725 + +extern double MACHEP, MINLOG, MAXLOG; + +#define BIG 1.44115188075855872E+17 + +static double jvs(double n, double x); +static double hankel(double n, double x); +static double recur(double *n, double x, double *newn, int cancel); +static double jnx(double n, double x); +static double jnt(double n, double x); + +double jv(double n, double x) +{ + double k, q, t, y, an; + int i, sign, nint; + + nint = 0; /* Flag for integer n */ + sign = 1; /* Flag for sign inversion */ + an = fabs(n); + y = floor(an); + if (y == an) { + nint = 1; + i = an - 16384.0 * floor(an / 16384.0); + if (n < 0.0) { + if (i & 1) + sign = -sign; + n = an; + } + if (x < 0.0) { + if (i & 1) + sign = -sign; + x = -x; + } + if (n == 0.0) + return (j0(x)); + if (n == 1.0) + return (sign * j1(x)); + } + + if ((x < 0.0) && (y != an)) { + sf_error("Jv", SF_ERROR_DOMAIN, NULL); + y = NAN; + goto done; + } + + if (x == 0 && n < 0 && !nint) { + sf_error("Jv", SF_ERROR_OVERFLOW, NULL); + return INFINITY / gamma(n + 1); + } + + y = fabs(x); + + if (y * y < fabs(n + 1) * MACHEP) { + return pow(0.5 * x, n) / gamma(n + 1); + } + + k = 3.6 * sqrt(y); + t = 3.6 * sqrt(an); + if ((y < t) && (an > 21.0)) + return (sign * jvs(n, x)); + if ((an < k) && (y > 21.0)) + return (sign * hankel(n, x)); + + if (an < 500.0) { + /* Note: if x is too large, the continued fraction will fail; but then the + * Hankel expansion can be used. */ + if (nint != 0) { + k = 0.0; + q = recur(&n, x, &k, 1); + if (k == 0.0) { + y = j0(x) / q; + goto done; + } + if (k == 1.0) { + y = j1(x) / q; + goto done; + } + } + + if (an > 2.0 * y) + goto rlarger; + + if ((n >= 0.0) && (n < 20.0) + && (y > 6.0) && (y < 20.0)) { + /* Recur backwards from a larger value of n */ + rlarger: + k = n; + + y = y + an + 1.0; + if (y < 30.0) + y = 30.0; + y = n + floor(y - n); + q = recur(&y, x, &k, 0); + y = jvs(y, x) * q; + goto done; + } + + if (k <= 30.0) { + k = 2.0; + } + else if (k < 90.0) { + k = (3 * k) / 4; + } + if (an > (k + 3.0)) { + if (n < 0.0) + k = -k; + q = n - floor(n); + k = floor(k) + q; + if (n > 0.0) + q = recur(&n, x, &k, 1); + else { + t = k; + k = n; + q = recur(&t, x, &k, 1); + k = t; + } + if (q == 0.0) { + y = 0.0; + goto done; + } + } + else { + k = n; + q = 1.0; + } + + /* boundary between convergence of + * power series and Hankel expansion + */ + y = fabs(k); + if (y < 26.0) + t = (0.0083 * y + 0.09) * y + 12.9; + else + t = 0.9 * y; + + if (x > t) + y = hankel(k, x); + else + y = jvs(k, x); +#if CEPHES_DEBUG + printf("y = %.16e, recur q = %.16e\n", y, q); +#endif + if (n > 0.0) + y /= q; + else + y *= q; + } + + else { + /* For large n, use the uniform expansion or the transitional expansion. + * But if x is of the order of n**2, these may blow up, whereas the + * Hankel expansion will then work. + */ + if (n < 0.0) { + sf_error("Jv", SF_ERROR_LOSS, NULL); + y = NAN; + goto done; + } + t = x / n; + t /= n; + if (t > 0.3) + y = hankel(n, x); + else + y = jnx(n, x); + } + + done:return (sign * y); +} + +/* Reduce the order by backward recurrence. + * AMS55 #9.1.27 and 9.1.73. + */ + +static double recur(double *n, double x, double *newn, int cancel) +{ + double pkm2, pkm1, pk, qkm2, qkm1; + + /* double pkp1; */ + double k, ans, qk, xk, yk, r, t, kf; + static double big = BIG; + int nflag, ctr; + int miniter, maxiter; + + /* Continued fraction for Jn(x)/Jn-1(x) + * AMS 9.1.73 + * + * x -x^2 -x^2 + * ------ --------- --------- ... + * 2 n + 2(n+1) + 2(n+2) + + * + * Compute it with the simplest possible algorithm. + * + * This continued fraction starts to converge when (|n| + m) > |x|. + * Hence, at least |x|-|n| iterations are necessary before convergence is + * achieved. There is a hard limit set below, m <= 30000, which is chosen + * so that no branch in `jv` requires more iterations to converge. + * The exact maximum number is (500/3.6)^2 - 500 ~ 19000 + */ + + maxiter = 22000; + miniter = fabs(x) - fabs(*n); + if (miniter < 1) + miniter = 1; + + if (*n < 0.0) + nflag = 1; + else + nflag = 0; + + fstart: + +#if CEPHES_DEBUG + printf("recur: n = %.6e, newn = %.6e, cfrac = ", *n, *newn); +#endif + + pkm2 = 0.0; + qkm2 = 1.0; + pkm1 = x; + qkm1 = *n + *n; + xk = -x * x; + yk = qkm1; + ans = 0.0; /* ans=0.0 ensures that t=1.0 in the first iteration */ + ctr = 0; + do { + yk += 2.0; + pk = pkm1 * yk + pkm2 * xk; + qk = qkm1 * yk + qkm2 * xk; + pkm2 = pkm1; + pkm1 = pk; + qkm2 = qkm1; + qkm1 = qk; + + /* check convergence */ + if (qk != 0 && ctr > miniter) + r = pk / qk; + else + r = 0.0; + + if (r != 0) { + t = fabs((ans - r) / r); + ans = r; + } + else { + t = 1.0; + } + + if (++ctr > maxiter) { + sf_error("jv", SF_ERROR_UNDERFLOW, NULL); + goto done; + } + if (t < MACHEP) + goto done; + + /* renormalize coefficients */ + if (fabs(pk) > big) { + pkm2 /= big; + pkm1 /= big; + qkm2 /= big; + qkm1 /= big; + } + } + while (t > MACHEP); + + done: + if (ans == 0) + ans = 1.0; + +#if CEPHES_DEBUG + printf("%.6e\n", ans); +#endif + + /* Change n to n-1 if n < 0 and the continued fraction is small */ + if (nflag > 0) { + if (fabs(ans) < 0.125) { + nflag = -1; + *n = *n - 1.0; + goto fstart; + } + } + + + kf = *newn; + + /* backward recurrence + * 2k + * J (x) = --- J (x) - J (x) + * k-1 x k k+1 + */ + + pk = 1.0; + pkm1 = 1.0 / ans; + k = *n - 1.0; + r = 2 * k; + do { + pkm2 = (pkm1 * r - pk * x) / x; + /* pkp1 = pk; */ + pk = pkm1; + pkm1 = pkm2; + r -= 2.0; + /* + * t = fabs(pkp1) + fabs(pk); + * if( (k > (kf + 2.5)) && (fabs(pkm1) < 0.25*t) ) + * { + * k -= 1.0; + * t = x*x; + * pkm2 = ( (r*(r+2.0)-t)*pk - r*x*pkp1 )/t; + * pkp1 = pk; + * pk = pkm1; + * pkm1 = pkm2; + * r -= 2.0; + * } + */ + k -= 1.0; + } + while (k > (kf + 0.5)); + + /* Take the larger of the last two iterates + * on the theory that it may have less cancellation error. + */ + + if (cancel) { + if ((kf >= 0.0) && (fabs(pk) > fabs(pkm1))) { + k += 1.0; + pkm2 = pk; + } + } + *newn = k; +#if CEPHES_DEBUG + printf("newn %.6e rans %.6e\n", k, pkm2); +#endif + return (pkm2); +} + + + +/* Ascending power series for Jv(x). + * AMS55 #9.1.10. + */ + +static double jvs(double n, double x) +{ + double t, u, y, z, k; + int ex, sgngam; + + z = -x * x / 4.0; + u = 1.0; + y = u; + k = 1.0; + t = 1.0; + + while (t > MACHEP) { + u *= z / (k * (n + k)); + y += u; + k += 1.0; + if (y != 0) + t = fabs(u / y); + } +#if CEPHES_DEBUG + printf("power series=%.5e ", y); +#endif + t = frexp(0.5 * x, &ex); + ex = ex * n; + if ((ex > -1023) + && (ex < 1023) + && (n > 0.0) + && (n < (MAXGAM - 1.0))) { + t = pow(0.5 * x, n) / gamma(n + 1.0); +#if CEPHES_DEBUG + printf("pow(.5*x, %.4e)/gamma(n+1)=%.5e\n", n, t); +#endif + y *= t; + } + else { +#if CEPHES_DEBUG + z = n * log(0.5 * x); + k = lgam(n + 1.0); + t = z - k; + printf("log pow=%.5e, lgam(%.4e)=%.5e\n", z, n + 1.0, k); +#else + t = n * log(0.5 * x) - lgam_sgn(n + 1.0, &sgngam); +#endif + if (y < 0) { + sgngam = -sgngam; + y = -y; + } + t += log(y); +#if CEPHES_DEBUG + printf("log y=%.5e\n", log(y)); +#endif + if (t < -MAXLOG) { + return (0.0); + } + if (t > MAXLOG) { + sf_error("Jv", SF_ERROR_OVERFLOW, NULL); + return (INFINITY); + } + y = sgngam * exp(t); + } + return (y); +} + +/* Hankel's asymptotic expansion + * for large x. + * AMS55 #9.2.5. + */ + +static double hankel(double n, double x) +{ + double t, u, z, k, sign, conv; + double p, q, j, m, pp, qq; + int flag; + + m = 4.0 * n * n; + j = 1.0; + z = 8.0 * x; + k = 1.0; + p = 1.0; + u = (m - 1.0) / z; + q = u; + sign = 1.0; + conv = 1.0; + flag = 0; + t = 1.0; + pp = 1.0e38; + qq = 1.0e38; + + while (t > MACHEP) { + k += 2.0; + j += 1.0; + sign = -sign; + u *= (m - k * k) / (j * z); + p += sign * u; + k += 2.0; + j += 1.0; + u *= (m - k * k) / (j * z); + q += sign * u; + t = fabs(u / p); + if (t < conv) { + conv = t; + qq = q; + pp = p; + flag = 1; + } + /* stop if the terms start getting larger */ + if ((flag != 0) && (t > conv)) { +#if CEPHES_DEBUG + printf("Hankel: convergence to %.4E\n", conv); +#endif + goto hank1; + } + } + + hank1: + u = x - (0.5 * n + 0.25) * M_PI; + t = sqrt(2.0 / (M_PI * x)) * (pp * cos(u) - qq * sin(u)); +#if CEPHES_DEBUG + printf("hank: %.6e\n", t); +#endif + return (t); +} + + +/* Asymptotic expansion for large n. + * AMS55 #9.3.35. + */ + +static double lambda[] = { + 1.0, + 1.041666666666666666666667E-1, + 8.355034722222222222222222E-2, + 1.282265745563271604938272E-1, + 2.918490264641404642489712E-1, + 8.816272674437576524187671E-1, + 3.321408281862767544702647E+0, + 1.499576298686255465867237E+1, + 7.892301301158651813848139E+1, + 4.744515388682643231611949E+2, + 3.207490090890661934704328E+3 +}; + +static double mu[] = { + 1.0, + -1.458333333333333333333333E-1, + -9.874131944444444444444444E-2, + -1.433120539158950617283951E-1, + -3.172272026784135480967078E-1, + -9.424291479571202491373028E-1, + -3.511203040826354261542798E+0, + -1.572726362036804512982712E+1, + -8.228143909718594444224656E+1, + -4.923553705236705240352022E+2, + -3.316218568547972508762102E+3 +}; + +static double P1[] = { + -2.083333333333333333333333E-1, + 1.250000000000000000000000E-1 +}; + +static double P2[] = { + 3.342013888888888888888889E-1, + -4.010416666666666666666667E-1, + 7.031250000000000000000000E-2 +}; + +static double P3[] = { + -1.025812596450617283950617E+0, + 1.846462673611111111111111E+0, + -8.912109375000000000000000E-1, + 7.324218750000000000000000E-2 +}; + +static double P4[] = { + 4.669584423426247427983539E+0, + -1.120700261622299382716049E+1, + 8.789123535156250000000000E+0, + -2.364086914062500000000000E+0, + 1.121520996093750000000000E-1 +}; + +static double P5[] = { + -2.8212072558200244877E1, + 8.4636217674600734632E1, + -9.1818241543240017361E1, + 4.2534998745388454861E1, + -7.3687943594796316964E0, + 2.27108001708984375E-1 +}; + +static double P6[] = { + 2.1257013003921712286E2, + -7.6525246814118164230E2, + 1.0599904525279998779E3, + -6.9957962737613254123E2, + 2.1819051174421159048E2, + -2.6491430486951555525E1, + 5.7250142097473144531E-1 +}; + +static double P7[] = { + -1.9194576623184069963E3, + 8.0617221817373093845E3, + -1.3586550006434137439E4, + 1.1655393336864533248E4, + -5.3056469786134031084E3, + 1.2009029132163524628E3, + -1.0809091978839465550E2, + 1.7277275025844573975E0 +}; + + +static double jnx(double n, double x) +{ + double zeta, sqz, zz, zp, np; + double cbn, n23, t, z, sz; + double pp, qq, z32i, zzi; + double ak, bk, akl, bkl; + int sign, doa, dob, nflg, k, s, tk, tkp1, m; + static double u[8]; + static double ai, aip, bi, bip; + + /* Test for x very close to n. Use expansion for transition region if so. */ + cbn = cbrt(n); + z = (x - n) / cbn; + if (fabs(z) <= 0.7) + return (jnt(n, x)); + + z = x / n; + zz = 1.0 - z * z; + if (zz == 0.0) + return (0.0); + + if (zz > 0.0) { + sz = sqrt(zz); + t = 1.5 * (log((1.0 + sz) / z) - sz); /* zeta ** 3/2 */ + zeta = cbrt(t * t); + nflg = 1; + } + else { + sz = sqrt(-zz); + t = 1.5 * (sz - acos(1.0 / z)); + zeta = -cbrt(t * t); + nflg = -1; + } + z32i = fabs(1.0 / t); + sqz = cbrt(t); + + /* Airy function */ + n23 = cbrt(n * n); + t = n23 * zeta; + +#if CEPHES_DEBUG + printf("zeta %.5E, Airy(%.5E)\n", zeta, t); +#endif + airy(t, &ai, &aip, &bi, &bip); + + /* polynomials in expansion */ + u[0] = 1.0; + zzi = 1.0 / zz; + u[1] = polevl(zzi, P1, 1) / sz; + u[2] = polevl(zzi, P2, 2) / zz; + u[3] = polevl(zzi, P3, 3) / (sz * zz); + pp = zz * zz; + u[4] = polevl(zzi, P4, 4) / pp; + u[5] = polevl(zzi, P5, 5) / (pp * sz); + pp *= zz; + u[6] = polevl(zzi, P6, 6) / pp; + u[7] = polevl(zzi, P7, 7) / (pp * sz); + +#if CEPHES_DEBUG + for (k = 0; k <= 7; k++) + printf("u[%d] = %.5E\n", k, u[k]); +#endif + + pp = 0.0; + qq = 0.0; + np = 1.0; + /* flags to stop when terms get larger */ + doa = 1; + dob = 1; + akl = INFINITY; + bkl = INFINITY; + + for (k = 0; k <= 3; k++) { + tk = 2 * k; + tkp1 = tk + 1; + zp = 1.0; + ak = 0.0; + bk = 0.0; + for (s = 0; s <= tk; s++) { + if (doa) { + if ((s & 3) > 1) + sign = nflg; + else + sign = 1; + ak += sign * mu[s] * zp * u[tk - s]; + } + + if (dob) { + m = tkp1 - s; + if (((m + 1) & 3) > 1) + sign = nflg; + else + sign = 1; + bk += sign * lambda[s] * zp * u[m]; + } + zp *= z32i; + } + + if (doa) { + ak *= np; + t = fabs(ak); + if (t < akl) { + akl = t; + pp += ak; + } + else + doa = 0; + } + + if (dob) { + bk += lambda[tkp1] * zp * u[0]; + bk *= -np / sqz; + t = fabs(bk); + if (t < bkl) { + bkl = t; + qq += bk; + } + else + dob = 0; + } +#if CEPHES_DEBUG + printf("a[%d] %.5E, b[%d] %.5E\n", k, ak, k, bk); +#endif + if (np < MACHEP) + break; + np /= n * n; + } + + /* normalizing factor ( 4*zeta/(1 - z**2) )**1/4 */ + t = 4.0 * zeta / zz; + t = sqrt(sqrt(t)); + + t *= ai * pp / cbrt(n) + aip * qq / (n23 * n); + return (t); +} + +/* Asymptotic expansion for transition region, + * n large and x close to n. + * AMS55 #9.3.23. + */ + +static double PF2[] = { + -9.0000000000000000000e-2, + 8.5714285714285714286e-2 +}; + +static double PF3[] = { + 1.3671428571428571429e-1, + -5.4920634920634920635e-2, + -4.4444444444444444444e-3 +}; + +static double PF4[] = { + 1.3500000000000000000e-3, + -1.6036054421768707483e-1, + 4.2590187590187590188e-2, + 2.7330447330447330447e-3 +}; + +static double PG1[] = { + -2.4285714285714285714e-1, + 1.4285714285714285714e-2 +}; + +static double PG2[] = { + -9.0000000000000000000e-3, + 1.9396825396825396825e-1, + -1.1746031746031746032e-2 +}; + +static double PG3[] = { + 1.9607142857142857143e-2, + -1.5983694083694083694e-1, + 6.3838383838383838384e-3 +}; + + +static double jnt(double n, double x) +{ + double z, zz, z3; + double cbn, n23, cbtwo; + double ai, aip, bi, bip; /* Airy functions */ + double nk, fk, gk, pp, qq; + double F[5], G[4]; + int k; + + cbn = cbrt(n); + z = (x - n) / cbn; + cbtwo = cbrt(2.0); + + /* Airy function */ + zz = -cbtwo * z; + airy(zz, &ai, &aip, &bi, &bip); + + /* polynomials in expansion */ + zz = z * z; + z3 = zz * z; + F[0] = 1.0; + F[1] = -z / 5.0; + F[2] = polevl(z3, PF2, 1) * zz; + F[3] = polevl(z3, PF3, 2); + F[4] = polevl(z3, PF4, 3) * z; + G[0] = 0.3 * zz; + G[1] = polevl(z3, PG1, 1); + G[2] = polevl(z3, PG2, 2) * z; + G[3] = polevl(z3, PG3, 2) * zz; +#if CEPHES_DEBUG + for (k = 0; k <= 4; k++) + printf("F[%d] = %.5E\n", k, F[k]); + for (k = 0; k <= 3; k++) + printf("G[%d] = %.5E\n", k, G[k]); +#endif + pp = 0.0; + qq = 0.0; + nk = 1.0; + n23 = cbrt(n * n); + + for (k = 0; k <= 4; k++) { + fk = F[k] * nk; + pp += fk; + if (k != 4) { + gk = G[k] * nk; + qq += gk; + } +#if CEPHES_DEBUG + printf("fk[%d] %.5E, gk[%d] %.5E\n", k, fk, k, gk); +#endif + nk /= n23; + } + + fk = cbtwo * ai * pp / cbn + cbrt(4.0) * aip * qq / n; + return (fk); +} diff --git a/gtsam/3rdparty/cephes/cephes/k0.c b/gtsam/3rdparty/cephes/cephes/k0.c new file mode 100644 index 000000000..c5b31a1bf --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/k0.c @@ -0,0 +1,178 @@ +/* k0.c + * + * Modified Bessel function, third kind, order zero + * + * + * + * SYNOPSIS: + * + * double x, y, k0(); + * + * y = k0( x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of the third kind + * of order zero of the argument. + * + * The range is partitioned into the two intervals [0,8] and + * (8, infinity). Chebyshev polynomial expansions are employed + * in each interval. + * + * + * + * ACCURACY: + * + * Tested at 2000 random points between 0 and 8. Peak absolute + * error (relative when K0 > 1) was 1.46e-14; rms, 4.26e-15. + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.2e-15 1.6e-16 + * + * ERROR MESSAGES: + * + * message condition value returned + * K0 domain x <= 0 INFINITY + * + */ + /* k0e() + * + * Modified Bessel function, third kind, order zero, + * exponentially scaled + * + * + * + * SYNOPSIS: + * + * double x, y, k0e(); + * + * y = k0e( x ); + * + * + * + * DESCRIPTION: + * + * Returns exponentially scaled modified Bessel function + * of the third kind of order zero of the argument. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.4e-15 1.4e-16 + * See k0(). + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +/* Chebyshev coefficients for K0(x) + log(x/2) I0(x) + * in the interval [0,2]. The odd order coefficients are all + * zero; only the even order coefficients are listed. + * + * lim(x->0){ K0(x) + log(x/2) I0(x) } = -EUL. + */ + +static double A[] = { + 1.37446543561352307156E-16, + 4.25981614279661018399E-14, + 1.03496952576338420167E-11, + 1.90451637722020886025E-9, + 2.53479107902614945675E-7, + 2.28621210311945178607E-5, + 1.26461541144692592338E-3, + 3.59799365153615016266E-2, + 3.44289899924628486886E-1, + -5.35327393233902768720E-1 +}; + +/* Chebyshev coefficients for exp(x) sqrt(x) K0(x) + * in the inverted interval [2,infinity]. + * + * lim(x->inf){ exp(x) sqrt(x) K0(x) } = sqrt(pi/2). + */ +static double B[] = { + 5.30043377268626276149E-18, + -1.64758043015242134646E-17, + 5.21039150503902756861E-17, + -1.67823109680541210385E-16, + 5.51205597852431940784E-16, + -1.84859337734377901440E-15, + 6.34007647740507060557E-15, + -2.22751332699166985548E-14, + 8.03289077536357521100E-14, + -2.98009692317273043925E-13, + 1.14034058820847496303E-12, + -4.51459788337394416547E-12, + 1.85594911495471785253E-11, + -7.95748924447710747776E-11, + 3.57739728140030116597E-10, + -1.69753450938905987466E-9, + 8.57403401741422608519E-9, + -4.66048989768794782956E-8, + 2.76681363944501510342E-7, + -1.83175552271911948767E-6, + 1.39498137188764993662E-5, + -1.28495495816278026384E-4, + 1.56988388573005337491E-3, + -3.14481013119645005427E-2, + 2.44030308206595545468E0 +}; + +double k0(double x) +{ + double y, z; + + if (x == 0.0) { + sf_error("k0", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + else if (x < 0.0) { + sf_error("k0", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x <= 2.0) { + y = x * x - 2.0; + y = chbevl(y, A, 10) - log(0.5 * x) * i0(x); + return (y); + } + z = 8.0 / x - 2.0; + y = exp(-x) * chbevl(z, B, 25) / sqrt(x); + return (y); +} + + + + +double k0e(double x) +{ + double y; + + if (x == 0.0) { + sf_error("k0e", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + else if (x < 0.0) { + sf_error("k0e", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x <= 2.0) { + y = x * x - 2.0; + y = chbevl(y, A, 10) - log(0.5 * x) * i0(x); + return (y * exp(x)); + } + + y = chbevl(8.0 / x - 2.0, B, 25) / sqrt(x); + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/k1.c b/gtsam/3rdparty/cephes/cephes/k1.c new file mode 100644 index 000000000..fc33e5c0e --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/k1.c @@ -0,0 +1,179 @@ +/* k1.c + * + * Modified Bessel function, third kind, order one + * + * + * + * SYNOPSIS: + * + * double x, y, k1(); + * + * y = k1( x ); + * + * + * + * DESCRIPTION: + * + * Computes the modified Bessel function of the third kind + * of order one of the argument. + * + * The range is partitioned into the two intervals [0,2] and + * (2, infinity). Chebyshev polynomial expansions are employed + * in each interval. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 1.2e-15 1.6e-16 + * + * ERROR MESSAGES: + * + * message condition value returned + * k1 domain x <= 0 INFINITY + * + */ + /* k1e.c + * + * Modified Bessel function, third kind, order one, + * exponentially scaled + * + * + * + * SYNOPSIS: + * + * double x, y, k1e(); + * + * y = k1e( x ); + * + * + * + * DESCRIPTION: + * + * Returns exponentially scaled modified Bessel function + * of the third kind of order one of the argument: + * + * k1e(x) = exp(x) * k1(x). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 7.8e-16 1.2e-16 + * See k1(). + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +/* Chebyshev coefficients for x(K1(x) - log(x/2) I1(x)) + * in the interval [0,2]. + * + * lim(x->0){ x(K1(x) - log(x/2) I1(x)) } = 1. + */ + +static double A[] = { + -7.02386347938628759343E-18, + -2.42744985051936593393E-15, + -6.66690169419932900609E-13, + -1.41148839263352776110E-10, + -2.21338763073472585583E-8, + -2.43340614156596823496E-6, + -1.73028895751305206302E-4, + -6.97572385963986435018E-3, + -1.22611180822657148235E-1, + -3.53155960776544875667E-1, + 1.52530022733894777053E0 +}; + +/* Chebyshev coefficients for exp(x) sqrt(x) K1(x) + * in the interval [2,infinity]. + * + * lim(x->inf){ exp(x) sqrt(x) K1(x) } = sqrt(pi/2). + */ +static double B[] = { + -5.75674448366501715755E-18, + 1.79405087314755922667E-17, + -5.68946255844285935196E-17, + 1.83809354436663880070E-16, + -6.05704724837331885336E-16, + 2.03870316562433424052E-15, + -7.01983709041831346144E-15, + 2.47715442448130437068E-14, + -8.97670518232499435011E-14, + 3.34841966607842919884E-13, + -1.28917396095102890680E-12, + 5.13963967348173025100E-12, + -2.12996783842756842877E-11, + 9.21831518760500529508E-11, + -4.19035475934189648750E-10, + 2.01504975519703286596E-9, + -1.03457624656780970260E-8, + 5.74108412545004946722E-8, + -3.50196060308781257119E-7, + 2.40648494783721712015E-6, + -1.93619797416608296024E-5, + 1.95215518471351631108E-4, + -2.85781685962277938680E-3, + 1.03923736576817238437E-1, + 2.72062619048444266945E0 +}; + +extern double MINLOG; + +double k1(double x) +{ + double y, z; + + if (x == 0.0) { + sf_error("k1", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + else if (x < 0.0) { + sf_error("k1", SF_ERROR_DOMAIN, NULL); + return NAN; + } + z = 0.5 * x; + + if (x <= 2.0) { + y = x * x - 2.0; + y = log(z) * i1(x) + chbevl(y, A, 11) / x; + return (y); + } + + return (exp(-x) * chbevl(8.0 / x - 2.0, B, 25) / sqrt(x)); +} + + + + +double k1e(double x) +{ + double y; + + if (x == 0.0) { + sf_error("k1e", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + else if (x < 0.0) { + sf_error("k1e", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x <= 2.0) { + y = x * x - 2.0; + y = log(0.5 * x) * i1(x) + chbevl(y, A, 11) / x; + return (y * exp(x)); + } + + return (chbevl(8.0 / x - 2.0, B, 25) / sqrt(x)); +} diff --git a/gtsam/3rdparty/cephes/cephes/kn.c b/gtsam/3rdparty/cephes/cephes/kn.c new file mode 100644 index 000000000..ff7584a15 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/kn.c @@ -0,0 +1,235 @@ +/* kn.c + * + * Modified Bessel function, third kind, integer order + * + * + * + * SYNOPSIS: + * + * double x, y, kn(); + * int n; + * + * y = kn( n, x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of the third kind + * of order n of the argument. + * + * The range is partitioned into the two intervals [0,9.55] and + * (9.55, infinity). An ascending power series is used in the + * low range, and an asymptotic expansion in the high range. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,30 90000 1.8e-8 3.0e-10 + * + * Error is high only near the crossover point x = 9.55 + * between the two expansions used. + */ + + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1988, 2000 by Stephen L. Moshier + */ + + +/* + * Algorithm for Kn. + * n-1 + * -n - (n-k-1)! 2 k + * K (x) = 0.5 (x/2) > -------- (-x /4) + * n - k! + * k=0 + * + * inf. 2 k + * n n - (x /4) + * + (-1) 0.5(x/2) > {p(k+1) + p(n+k+1) - 2log(x/2)} --------- + * - k! (n+k)! + * k=0 + * + * where p(m) is the psi function: p(1) = -EUL and + * + * m-1 + * - + * p(m) = -EUL + > 1/k + * - + * k=1 + * + * For large x, + * 2 2 2 + * u-1 (u-1 )(u-3 ) + * K (z) = sqrt(pi/2z) exp(-z) { 1 + ------- + ------------ + ...} + * v 1 2 + * 1! (8z) 2! (8z) + * asymptotically, where + * + * 2 + * u = 4 v . + * + */ + +#include "mconf.h" +#include + +#define EUL 5.772156649015328606065e-1 +#define MAXFAC 31 +extern double MACHEP, MAXLOG; + +double kn(int nn, double x) +{ + double k, kf, nk1f, nkf, zn, t, s, z0, z; + double ans, fn, pn, pk, zmn, tlg, tox; + int i, n; + + if (nn < 0) + n = -nn; + else + n = nn; + + if (n > MAXFAC) { + overf: + sf_error("kn", SF_ERROR_OVERFLOW, NULL); + return (INFINITY); + } + + if (x <= 0.0) { + if (x < 0.0) { + sf_error("kn", SF_ERROR_DOMAIN, NULL); + return NAN; + } + else { + sf_error("kn", SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + } + + + if (x > 9.55) + goto asymp; + + ans = 0.0; + z0 = 0.25 * x * x; + fn = 1.0; + pn = 0.0; + zmn = 1.0; + tox = 2.0 / x; + + if (n > 0) { + /* compute factorial of n and psi(n) */ + pn = -EUL; + k = 1.0; + for (i = 1; i < n; i++) { + pn += 1.0 / k; + k += 1.0; + fn *= k; + } + + zmn = tox; + + if (n == 1) { + ans = 1.0 / x; + } + else { + nk1f = fn / n; + kf = 1.0; + s = nk1f; + z = -z0; + zn = 1.0; + for (i = 1; i < n; i++) { + nk1f = nk1f / (n - i); + kf = kf * i; + zn *= z; + t = nk1f * zn / kf; + s += t; + if ((DBL_MAX - fabs(t)) < fabs(s)) + goto overf; + if ((tox > 1.0) && ((DBL_MAX / tox) < zmn)) + goto overf; + zmn *= tox; + } + s *= 0.5; + t = fabs(s); + if ((zmn > 1.0) && ((DBL_MAX / zmn) < t)) + goto overf; + if ((t > 1.0) && ((DBL_MAX / t) < zmn)) + goto overf; + ans = s * zmn; + } + } + + + tlg = 2.0 * log(0.5 * x); + pk = -EUL; + if (n == 0) { + pn = pk; + t = 1.0; + } + else { + pn = pn + 1.0 / n; + t = 1.0 / fn; + } + s = (pk + pn - tlg) * t; + k = 1.0; + do { + t *= z0 / (k * (k + n)); + pk += 1.0 / k; + pn += 1.0 / (k + n); + s += (pk + pn - tlg) * t; + k += 1.0; + } + while (fabs(t / s) > MACHEP); + + s = 0.5 * s / zmn; + if (n & 1) + s = -s; + ans += s; + + return (ans); + + + + /* Asymptotic expansion for Kn(x) */ + /* Converges to 1.4e-17 for x > 18.4 */ + + asymp: + + if (x > MAXLOG) { + sf_error("kn", SF_ERROR_UNDERFLOW, NULL); + return (0.0); + } + k = n; + pn = 4.0 * k * k; + pk = 1.0; + z0 = 8.0 * x; + fn = 1.0; + t = 1.0; + s = t; + nkf = INFINITY; + i = 0; + do { + z = pn - pk * pk; + t = t * z / (fn * z0); + nk1f = fabs(t); + if ((i >= n) && (nk1f > nkf)) { + goto adone; + } + nkf = nk1f; + s += t; + fn += 1.0; + pk += 2.0; + i += 1; + } + while (fabs(t / s) > MACHEP); + + adone: + ans = exp(-x) * sqrt(M_PI / (2.0 * x)) * s; + return (ans); +} diff --git a/gtsam/3rdparty/cephes/cephes/kolmogorov.c b/gtsam/3rdparty/cephes/cephes/kolmogorov.c new file mode 100644 index 000000000..2135e0ebb --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/kolmogorov.c @@ -0,0 +1,1147 @@ +/* File altered for inclusion in cephes module for Python: + * Main loop commented out.... */ +/* Travis Oliphant Nov. 1998 */ + +/* Re Kolmogorov statistics, here is Birnbaum and Tingey's (actually it was already present + * in Smirnov's paper) formula for the + * distribution of D+, the maximum of all positive deviations between a + * theoretical distribution function P(x) and an empirical one Sn(x) + * from n samples. + * + * + + * D = sup [P(x) - S (x)] + * n -inf < x < inf n + * + * + * [n(1-d)] + * + - v-1 n-v + * Pr{D > d} = > C d (d + v/n) (1 - d - v/n) + * n - n v + * v=0 + * + * (also equals the following sum, but note the terms may be large and alternating in sign) + * See Smirnov 1944, Dwass 1959 + * n + * - v-1 n-v + * = 1 - > C d (d + v/n) (1 - d - v/n) + * - n v + * v=[n(1-d)]+1 + * + * [n(1-d)] is the largest integer not exceeding n(1-d). + * nCv is the number of combinations of n things taken v at a time. + + * Sources: + * [1] Smirnov, N.V. "Approximate laws of distribution of random variables from empirical data" + * Usp. Mat. Nauk, 1944. http://mi.mathnet.ru/umn8798 + * [2] Birnbaum, Z. W. and Tingey, Fred H. + * "One-Sided Confidence Contours for Probability Distribution Functions", + * Ann. Math. Statist. 1951. https://doi.org/10.1214/aoms/1177729550 + * [3] Dwass, Meyer, "The Distribution of a Generalized $\mathrm{D}^+_n$ Statistic", + * Ann. Math. Statist., 1959. https://doi.org/10.1214/aoms/1177706085 + * [4] van Mulbregt, Paul, "Computing the Cumulative Distribution Function and Quantiles of the One-sided Kolmogorov-Smirnov Statistic" + * http://arxiv.org/abs/1802.06966 + * [5] van Mulbregt, Paul, "Computing the Cumulative Distribution Function and Quantiles of the limit of the Two-sided Kolmogorov-Smirnov Statistic" + * https://arxiv.org/abs/1803.00426 + * + */ + +#include "mconf.h" +#include +#include +#include + + +/* ************************************************************************ */ +/* Algorithm Configuration */ + +/* + * Kolmogorov Two-sided: + * Switchover between the two series to compute K(x) + * 0 <= x <= KOLMOG_CUTOVER and + * KOLMOG_CUTOVER < x < infty + */ +#define KOLMOG_CUTOVER 0.82 + + +/* + * Smirnov One-sided: + * n larger than SMIRNOV_MAX_COMPUTE_N will result in an approximation + */ +const int SMIRNOV_MAX_COMPUTE_N = 1000000; + +/* + * Use the upper sum formula, if the number of terms is at most SM_UPPER_MAX_TERMS, + * and n is at least SM_UPPERSUM_MIN_N + * Don't use the upper sum if lots of terms are involved as the series alternates + * sign and the terms get much bigger than 1. + */ +#define SM_UPPER_MAX_TERMS 3 +#define SM_UPPERSUM_MIN_N 10 + +/* ************************************************************************ */ +/* ************************************************************************ */ + +/* Assuming LOW and HIGH are constants. */ +#define CLIP(X, LOW, HIGH) ((X) < LOW ? LOW : MIN(X, HIGH)) +#ifndef MIN +#define MIN(a,b) (((a) < (b)) ? (a) : (b)) +#endif +#ifndef MAX +#define MAX(a,b) (((a) < (b)) ? (b) : (a)) +#endif + +/* from cephes constants */ +extern double MINLOG; + +/* exp() of anything below this returns 0 */ +static const int MIN_EXPABLE = (-708 - 38); + +#ifndef LOGSQRT2PI +#define LOGSQRT2PI 0.91893853320467274178032973640561764 +#endif + +/* Struct to hold the CDF, SF and PDF, which are computed simultaneously */ +typedef struct ThreeProbs { + double sf; + double cdf; + double pdf; +} ThreeProbs; + +#define RETURN_3PROBS(PSF, PCDF, PDF) \ + ret.cdf = (PCDF); \ + ret.sf = (PSF); \ + ret.pdf = (PDF); \ + return ret; + +static const double _xtol = DBL_EPSILON; +static const double _rtol = 2*DBL_EPSILON; + +static int +_within_tol(double x, double y, double atol, double rtol) +{ + double diff = fabs(x-y); + int result = (diff <= (atol + rtol * fabs(y))); + return result; +} + +#include "dd_real.h" + +/* Shorten some of the double-double names for readibility */ +#define valueD dd_to_double +#define add_dd dd_add_d_d +#define sub_dd dd_sub_d_d +#define mul_dd dd_mul_d_d +#define neg_D dd_neg +#define div_dd dd_div_d_d +#define add_DD dd_add +#define sub_DD dd_sub +#define mul_DD dd_mul +#define div_DD dd_div +#define add_Dd dd_add_dd_d +#define add_dD dd_add_d_dd +#define sub_Dd dd_sub_dd_d +#define sub_dD dd_sub_d_dd +#define mul_Dd dd_mul_dd_d +#define mul_dD dd_mul_d_dd +#define div_Dd dd_div_dd_d +#define div_dD dd_div_d_dd +#define frexpD dd_frexp +#define ldexpD dd_ldexp +#define logD dd_log +#define log1pD dd_log1p + + +/* ************************************************************************ */ +/* Kolmogorov : Two-sided **************************** */ +/* ************************************************************************ */ + +static ThreeProbs +_kolmogorov(double x) +{ + double P = 1.0; + double D = 0; + double sf, cdf, pdf; + ThreeProbs ret; + + if (isnan(x)) { + RETURN_3PROBS(NAN, NAN, NAN); + } + if (x <= 0) { + RETURN_3PROBS(1.0, 0.0, 0); + } + /* x <= 0.040611972203751713 */ + if (x <= (double)M_PI/sqrt(-MIN_EXPABLE * 8)) { + RETURN_3PROBS(1.0, 0.0, 0); + } + + P = 1.0; + if (x <= KOLMOG_CUTOVER) { + /* + * u = e^(-pi^2/(8x^2)) + * w = sqrt(2pi)/x + * P = w*u * (1 + u^8 + u^24 + u^48 + ...) + */ + double w = sqrt(2 * M_PI)/x; + double logu8 = -M_PI * M_PI/(x * x); /* log(u^8) */ + double u = exp(logu8/8); + if (u == 0) { + /* + * P = w*u, but u < 1e-308, and w > 1, + * so compute as logs, then exponentiate + */ + double logP = logu8/8 + log(w); + P = exp(logP); + } else { + /* Just unroll the loop, 3 iterations */ + double u8 = exp(logu8); + double u8cub = pow(u8, 3); + P = 1 + u8cub * P; + D = 5*5 + u8cub * D; + P = 1 + u8*u8 * P; + D = 3*3 + u8*u8 * D; + P = 1 + u8 * P; + D = 1*1 + u8 * D; + + D = M_PI * M_PI/4/(x*x) * D - P; + D *= w * u/x; + P = w * u * P; + } + cdf = P; + sf = 1-P; + pdf = D; + } + else { + /* + * v = e^(-2x^2) + * P = 2 (v - v^4 + v^9 - v^16 + ...) + * = 2v(1 - v^3*(1 - v^5*(1 - v^7*(1 - ...))) + */ + double logv = -2*x*x; + double v = exp(logv); + /* + * Want q^((2k-1)^2)(1-q^(4k-1)) / q(1-q^3) < epsilon to break out of loop. + * With KOLMOG_CUTOVER ~ 0.82, k <= 4. Just unroll the loop, 4 iterations + */ + double vsq = v*v; + double v3 = pow(v, 3); + double vpwr; + + vpwr = v3*v3*v; /* v**7 */ + P = 1 - vpwr * P; /* P <- 1 - (1-v**(2k-1)) * P */ + D = 3*3 - vpwr * D; + + vpwr = v3*vsq; + P = 1 - vpwr * P; + D = 2*2 - vpwr * D; + + vpwr = v3; + P = 1 - vpwr * P; + D = 1*1 - vpwr * D; + + P = 2 * v * P; + D = 8 * v * x * D; + sf = P; + cdf = 1 - sf; + pdf = D; + } + pdf = MAX(0, pdf); + cdf = CLIP(cdf, 0, 1); + sf = CLIP(sf, 0, 1); + RETURN_3PROBS(sf, cdf, pdf); +} + + +/* Find x such kolmogorov(x)=psf, kolmogc(x)=pcdf */ +static double +_kolmogi(double psf, double pcdf) +{ + double x, t; + double xmin = 0; + double xmax = INFINITY; + int iterations; + double a = xmin, b = xmax; + + if (!(psf >= 0.0 && pcdf >= 0.0 && pcdf <= 1.0 && psf <= 1.0)) { + sf_error("kolmogi", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (fabs(1.0 - pcdf - psf) > 4* DBL_EPSILON) { + sf_error("kolmogi", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (pcdf == 0.0) { + return 0.0; + } + if (psf == 0.0) { + return INFINITY; + } + + if (pcdf <= 0.5) { + /* p ~ (sqrt(2pi)/x) *exp(-pi^2/8x^2). Generate lower and upper bounds */ + double logpcdf = log(pcdf); + const double SQRT2 = M_SQRT2; + /* Now that 1 >= x >= sqrt(p) */ + /* Iterate twice: x <- pi/(sqrt(8) sqrt(log(sqrt(2pi)) - log(x) - log(pdf))) */ + a = M_PI / (2 * SQRT2 * sqrt(-(logpcdf + logpcdf/2 - LOGSQRT2PI))); + b = M_PI / (2 * SQRT2 * sqrt(-(logpcdf + 0 - LOGSQRT2PI))); + a = M_PI / (2 * SQRT2 * sqrt(-(logpcdf + log(a) - LOGSQRT2PI))); + b = M_PI / (2 * SQRT2 * sqrt(-(logpcdf + log(b) - LOGSQRT2PI))); + x = (a + b) / 2.0; + } + else { + /* + * Based on the approximation p ~ 2 exp(-2x^2) + * Found that needed to replace psf with a slightly smaller number in the second element + * as otherwise _kolmogorov(b) came back as a very small number but with + * the same sign as _kolmogorov(a) + * kolmogi(0.5) = 0.82757355518990772 + * so (1-q^(-(4-1)*2*x^2)) = (1-exp(-6*0.8275^2) ~ (1-exp(-4.1) + */ + const double jiggerb = 256 * DBL_EPSILON; + double pba = psf/(1.0 - exp(-4))/2, pbb = psf * (1 - jiggerb)/2; + double q0; + a = sqrt(-0.5 * log(pba)); + b = sqrt(-0.5 * log(pbb)); + /* + * Use inversion of + * p = q - q^4 + q^9 - q^16 + ...: + * q = p + p^4 + 4p^7 - p^9 + 22p^10 - 13p^12 + 140*p^13 ... + */ + { + double p = psf/2.0; + double p2 = p*p; + double p3 = p*p*p; + q0 = 1 + p3 * (1 + p3 * (4 + p2 *(-1 + p*(22 + p2* (-13 + 140 * p))))); + q0 *= p; + } + x = sqrt(-log(q0) / 2); + if (x < a || x > b) { + x = (a+b)/2; + } + } + assert(a <= b); + + iterations = 0; + do { + double x0 = x; + ThreeProbs probs = _kolmogorov(x0); + double df = ((pcdf < 0.5) ? (pcdf - probs.cdf) : (probs.sf - psf)); + double dfdx; + + if (fabs(df) == 0) { + break; + } + /* Update the bracketing interval */ + if (df > 0 && x > a) { + a = x; + } else if (df < 0 && x < b) { + b = x; + } + + dfdx = -probs.pdf; + if (fabs(dfdx) <= 0.0) { + x = (a+b)/2; + t = x0 - x; + } else { + t = df/dfdx; + x = x0 - t; + } + + /* + * Check out-of-bounds. + * Not expecting this to happen often --- kolmogorov is convex near x=infinity and + * concave near x=0, and we should be approaching from the correct side. + * If out-of-bounds, replace x with a midpoint of the bracket. + */ + if (x >= a && x <= b) { + if (_within_tol(x, x0, _xtol, _rtol)) { + break; + } + if ((x == a) || (x == b)) { + x = (a + b) / 2.0; + /* If the bracket is already so small ... */ + if (x == a || x == b) { + break; + } + } + } else { + x = (a + b) / 2.0; + if (_within_tol(x, x0, _xtol, _rtol)) { + break; + } + } + + if (++iterations > MAXITER) { + sf_error("kolmogi", SF_ERROR_SLOW, NULL); + break; + } + } while(1); + return (x); +} + + +double +kolmogorov(double x) +{ + if (isnan(x)) { + return NAN; + } + return _kolmogorov(x).sf; +} + +double +kolmogc(double x) +{ + if (isnan(x)) { + return NAN; + } + return _kolmogorov(x).cdf; +} + +double +kolmogp(double x) +{ + if (isnan(x)) { + return NAN; + } + if (x <= 0) { + return -0.0; + } + return -_kolmogorov(x).pdf; +} + +/* Functional inverse of Kolmogorov survival statistic for two-sided test. + * Finds x such that kolmogorov(x) = p. + */ +double +kolmogi(double p) +{ + if (isnan(p)) { + return NAN; + } + return _kolmogi(p, 1-p); +} + +/* Functional inverse of Kolmogorov cumulative statistic for two-sided test. + * Finds x such that kolmogc(x) = p = (or kolmogorov(x) = 1-p). + */ +double +kolmogci(double p) +{ + if (isnan(p)) { + return NAN; + } + return _kolmogi(1-p, p); +} + + + +/* ************************************************************************ */ +/* ********** Smirnov : One-sided ***************************************** */ +/* ************************************************************************ */ + +static double +nextPowerOf2(double x) +{ + double q = ldexp(x, 1-DBL_MANT_DIG); + double L = fabs(q+x); + if (L == 0) { + L = fabs(x); + } else { + int Lint = (int)(L); + if (Lint == L) { + L = Lint; + } + } + return L; +} + +static double +modNX(int n, double x, int *pNXFloor, double *pNX) +{ + /* + * Compute floor(n*x) and remainder *exactly*. + * If remainder is too close to 1 (E.g. (1, -DBL_EPSILON/2)) + * round up and adjust */ + double2 alphaD, nxD, nxfloorD; + int nxfloor; + double alpha; + + nxD = mul_dd(n, x); + nxfloorD = dd_floor(nxD); + alphaD = sub_DD(nxD, nxfloorD); + alpha = dd_hi(alphaD); + nxfloor = dd_to_int(nxfloorD); + assert(alpha >= 0); + assert(alpha <= 1); + if (alpha == 1) { + nxfloor += 1; + alpha = 0; + } + assert(alpha < 1.0); + *pNX = dd_to_double(nxD); + *pNXFloor = nxfloor; + return alpha; +} + +/* + * The binomial coefficient C overflows a 64 bit double, as the 11-bit + * exponent is too small. + * Store C as (Cman:double2, Cexpt:int). + * I.e a Mantissa/significand, and an exponent. + * Cman lies between 0.5 and 1, and the exponent has >=32-bit. + */ +static void +updateBinomial(double2 *Cman, int *Cexpt, int n, int j) +{ + int expt; + double2 rat = div_dd(n - j, j + 1.0); + double2 man2 = mul_DD(*Cman, rat); + man2 = frexpD(man2, &expt); + assert (!dd_is_zero(man2)); + *Cexpt += expt; + *Cman = man2; +} + + +static double2 +pow_D(double2 a, int m) +{ + /* + * Using dd_npwr() here would be quite time-consuming. + * Tradeoff accuracy-time by using pow(). + */ + double ans, r, adj; + if (m <= 0) { + if (m == 0) { + return DD_C_ONE; + } + return dd_inv(pow_D(a, -m)); + } + if (dd_is_zero(a)) { + return DD_C_ZERO; + } + ans = pow(a.x[0], m); + r = a.x[1]/a.x[0]; + adj = m*r; + if (fabs(adj) > 1e-8) { + if (fabs(adj) < 1e-4) { + /* Take 1st two terms of Taylor Series for (1+r)^m */ + adj += (m*r) * ((m-1)/2.0 * r); + } else { + /* Take exp of scaled log */ + adj = expm1(m*log1p(r)); + } + } + return dd_add_d_d(ans, ans*adj); +} + +static double +pow2(double a, double b, int m) +{ + return dd_to_double(pow_D(add_dd(a, b), m)); +} + +/* + * Not 1024 as too big. Want _MAX_EXPONENT < 1023-52 so as to keep both + * elements of the double2 normalized + */ +#define _MAX_EXPONENT 960 + +#define RETURN_M_E(MAND, EXPT) \ + *pExponent = EXPT;\ + return MAND; + + +static double2 +pow2Scaled_D(double2 a, int m, int *pExponent) +{ + /* Compute a^m = significand*2^expt and return as (significand, expt) */ + double2 ans, y; + int ansE, yE; + int maxExpt = _MAX_EXPONENT; + int q, r, y2mE, y2rE, y2mqE; + double2 y2r, y2m, y2mq; + + if (m <= 0) + { + int aE1, aE2; + if (m == 0) { + RETURN_M_E(DD_C_ONE, 0); + } + ans = pow2Scaled_D(a, -m, &aE1); + ans = frexpD(dd_inv(ans), &aE2); + ansE = -aE1 + aE2; + RETURN_M_E(ans, ansE); + } + y = frexpD(a, &yE); + if (m == 1) { + RETURN_M_E(y, yE); + } + /* + * y ^ maxExpt >= 2^{-960} + * => maxExpt = 960 / log2(y.x[0]) = 708 / log(y.x[0]) + * = 665/((1-y.x[0] + y.x[0]^2/2 - ...) + * <= 665/(1-y.x[0]) + * Quick check to see if we might need to break up the exponentiation + */ + if (m*(y.x[0]-1) / y.x[0] < -_MAX_EXPONENT * M_LN2) { + /* Now do it carefully, calling log() */ + double lg2y = log(y.x[0]) / M_LN2; + double lgAns = m * lg2y; + if (lgAns <= -_MAX_EXPONENT) { + maxExpt = (int)(nextPowerOf2(-_MAX_EXPONENT / lg2y + 1)/2); + } + } + if (m <= maxExpt) + { + double2 ans1 = pow_D(y, m); + ans = frexpD(ans1, &ansE); + ansE += m * yE; + RETURN_M_E(ans, ansE); + } + + q = m / maxExpt; + r = m % maxExpt; + /* y^m = (y^maxExpt)^q * y^r */ + y2r = pow2Scaled_D(y, r, &y2rE); + y2m = pow2Scaled_D(y, maxExpt, &y2mE); + y2mq = pow2Scaled_D(y2m, q, &y2mqE); + ans = frexpD(mul_DD(y2r, y2mq), &ansE); + y2mqE += y2mE * q; + ansE += y2mqE + y2rE; + ansE += m * yE; + RETURN_M_E(ans, ansE); +} + + +static double2 +pow4_D(double a, double b, double c, double d, int m) +{ + /* Compute ((a+b)/(c+d)) ^ m */ + double2 A, C, X; + if (m <= 0){ + if (m == 0) { + return DD_C_ONE; + } + return pow4_D(c, d, a, b, -m); + } + A = add_dd(a, b); + C = add_dd(c, d); + if (dd_is_zero(A)) { + return (dd_is_zero(C) ? DD_C_NAN : DD_C_ZERO); + } + if (dd_is_zero(C)) { + return (dd_is_negative(A) ? DD_C_NEGINF : DD_C_INF); + } + X = div_DD(A, C); + return pow_D(X, m); +} + +static double +pow4(double a, double b, double c, double d, int m) +{ + double2 ret = pow4_D(a, b, c, d, m); + return dd_to_double(ret); +} + + +static double2 +logpow4_D(double a, double b, double c, double d, int m) +{ + /* + * Compute log(((a+b)/(c+d)) ^ m) + * == m * log((a+b)/(c+d)) + * == m * log( 1 + (a+b-c-d)/(c+d)) + */ + double2 ans; + double2 A, C, X; + if (m == 0) { + return DD_C_ZERO; + } + A = add_dd(a, b); + C = add_dd(c, d); + if (dd_is_zero(A)) { + return (dd_is_zero(C) ? DD_C_ZERO : DD_C_NEGINF); + } + if (dd_is_zero(C)) { + return DD_C_INF; + } + X = div_DD(A, C); + assert(X.x[0] >= 0); + if (0.5 <= X.x[0] && X.x[0] <= 1.5) { + double2 A1 = sub_DD(A, C); + double2 X1 = div_DD(A1, C); + ans = log1pD(X1); + } else { + ans = logD(X); + } + ans = mul_dD(m, ans); + return ans; +} + +static double +logpow4(double a, double b, double c, double d, int m) +{ + double2 ans = logpow4_D(a, b, c, d, m); + return dd_to_double(ans); +} + +/* + * Compute a single term in the summation, A_v(n, x): + * A_v(n, x) = Binomial(n,v) * (1-x-v/n)^(n-v) * (x+v/n)^(v-1) + */ +static void +computeAv(int n, double x, int v, double2 Cman, int Cexpt, + double2 *pt1, double2 *pt2, double2 *pAv) +{ + int t1E, t2E, ansE; + double2 Av; + double2 t2x = sub_Dd(div_dd(n - v, n), x); /* 1 - x - v/n */ + double2 t2 = pow2Scaled_D(t2x, n-v, &t2E); + double2 t1x = add_Dd(div_dd(v, n), x); /* x + v/n */ + double2 t1 = pow2Scaled_D(t1x, v-1, &t1E); + double2 ans = mul_DD(t1, t2); + ans = mul_DD(ans, Cman); + ansE = Cexpt + t1E + t2E; + Av = ldexpD(ans, ansE); + *pAv = Av; + *pt1 = t1; + *pt2 = t2; +} + + +static ThreeProbs +_smirnov(int n, double x) +{ + double nx, alpha; + double2 AjSum = DD_C_ZERO; + double2 dAjSum = DD_C_ZERO; + double cdf, sf, pdf; + + int bUseUpperSum; + int nxfl, n1mxfl, n1mxceil; + ThreeProbs ret; + + if (!(n > 0 && x >= 0.0 && x <= 1.0)) { + RETURN_3PROBS(NAN, NAN, NAN); + } + if (n == 1) { + RETURN_3PROBS(1-x, x, 1.0); + } + if (x == 0.0) { + RETURN_3PROBS(1.0, 0.0, 1.0); + } + if (x == 1.0) { + RETURN_3PROBS(0.0, 1.0, 0.0); + } + + alpha = modNX(n, x, &nxfl, &nx); + n1mxfl = n - nxfl - (alpha == 0 ? 0 : 1); + n1mxceil = n - nxfl; + /* + * If alpha is 0, don't actually want to include the last term + * in either the lower or upper summations. + */ + if (alpha == 0) { + n1mxfl -= 1; + n1mxceil += 1; + } + + /* Special case: x <= 1/n */ + if (nxfl == 0 || (nxfl == 1 && alpha == 0)) { + double t = pow2(1, x, n-1); + pdf = (nx + 1) * t / (1+x); + cdf = x * t; + sf = 1 - cdf; + /* Adjust if x=1/n *exactly* */ + if (nxfl == 1) { + assert(alpha == 0); + pdf -= 0.5; + } + RETURN_3PROBS(sf, cdf, pdf); + } + /* Special case: x is so big, the sf underflows double64 */ + if (-2 * n * x*x < MINLOG) { + RETURN_3PROBS(0, 1, 0); + } + /* Special case: x >= 1 - 1/n */ + if (nxfl >= n-1) { + sf = pow2(1, -x, n); + cdf = 1 - sf; + pdf = n * sf/(1-x); + RETURN_3PROBS(sf, cdf, pdf); + } + /* Special case: n is so big, take too long to compute */ + if (n > SMIRNOV_MAX_COMPUTE_N) { + /* p ~ e^(-(6nx+1)^2 / 18n) */ + double logp = -pow(6.0*n*x+1, 2)/18.0/n; + /* Maximise precision for small p-value. */ + if (logp < -M_LN2) { + sf = exp(logp); + cdf = 1 - sf; + } else { + cdf = -expm1(logp); + sf = 1 - cdf; + } + pdf = (6.0*n*x+1) * 2 * sf/3; + RETURN_3PROBS(sf, cdf, pdf); + } + { + /* + * Use the upper sum if n is large enough, and x is small enough and + * the number of terms is going to be small enough. + * Otherwise it just drops accuracy, about 1.6bits * nUpperTerms + */ + int nUpperTerms = n - n1mxceil + 1; + bUseUpperSum = (nUpperTerms <= 1 && x < 0.5); + bUseUpperSum = (bUseUpperSum || + ((n >= SM_UPPERSUM_MIN_N) + && (nUpperTerms <= SM_UPPER_MAX_TERMS) + && (x <= 0.5 / sqrt(n)))); + } + + { + int start=0, step=1, nTerms=n1mxfl+1; + int j, firstJ = 0; + int vmid = n/2; + double2 Cman = DD_C_ONE; + int Cexpt = 0; + double2 Aj, dAj, t1, t2, dAjCoeff; + double2 oneOverX = div_dd(1, x); + + if (bUseUpperSum) { + start = n; + step = -1; + nTerms = n - n1mxceil + 1; + + t1 = pow4_D(1, x, 1, 0, n - 1); + t2 = DD_C_ONE; + Aj = t1; + + dAjCoeff = div_dD(n - 1, add_dd(1, x)); + dAjCoeff = add_DD(dAjCoeff, oneOverX); + } else { + t1 = oneOverX; + t2 = pow4_D(1, -x, 1, 0, n); + Aj = div_Dd(t2, x); + + dAjCoeff = div_DD(sub_dD(-1, mul_dd(n - 1, x)), sub_dd(1, x)); + dAjCoeff = div_Dd(dAjCoeff, x); + dAjCoeff = add_DD(dAjCoeff, oneOverX); + } + + dAj = mul_DD(Aj, dAjCoeff); + AjSum = add_DD(AjSum, Aj); + dAjSum = add_DD(dAjSum, dAj); + + updateBinomial(&Cman, &Cexpt, n, 0); + firstJ ++; + + for (j = firstJ; j < nTerms; j += 1) { + int v = start + j * step; + + computeAv(n, x, v, Cman, Cexpt, &t1, &t2, &Aj); + + if (dd_isfinite(Aj) && !dd_is_zero(Aj)) { + /* coeff = 1/x + (j-1)/(x+j/n) - (n-j)/(1-x-j/n) */ + dAjCoeff = sub_DD(div_dD((n * (v - 1)), add_dd(nxfl + v, alpha)), + div_dD(((n - v) * n), sub_dd(n - nxfl - v, alpha))); + dAjCoeff = add_DD(dAjCoeff, oneOverX); + dAj = mul_DD(Aj, dAjCoeff); + + assert(dd_isfinite(Aj)); + AjSum = add_DD(AjSum, Aj); + dAjSum = add_DD(dAjSum, dAj); + } + /* Safe to terminate early? */ + if (!dd_is_zero(Aj)) { + if ((4*(nTerms-j) * fabs(dd_to_double(Aj)) < DBL_EPSILON * dd_to_double(AjSum)) + && (j != nTerms - 1)) { + break; + } + } + else if (j > vmid) { + assert(dd_is_zero(Aj)); + break; + } + + updateBinomial(&Cman, &Cexpt, n, j); + } + assert(dd_isfinite(AjSum)); + assert(dd_isfinite(dAjSum)); + { + double2 derivD = mul_dD(x, dAjSum); + double2 probD = mul_dD(x, AjSum); + double deriv = dd_to_double(derivD); + double prob = dd_to_double(probD); + + assert (nx != 1 || alpha > 0); + if (step < 0) { + cdf = prob; + sf = 1-prob; + pdf = deriv; + } else { + cdf = 1-prob; + sf = prob; + pdf = -deriv; + } + } + } + + pdf = MAX(0, pdf); + cdf = CLIP(cdf, 0, 1); + sf = CLIP(sf, 0, 1); + RETURN_3PROBS(sf, cdf, pdf); +} + +/* + * Functional inverse of Smirnov distribution + * finds x such that smirnov(n, x) = psf; smirnovc(n, x) = pcdf). + */ +static double +_smirnovi(int n, double psf, double pcdf) +{ + /* + * Need to use a bracketing NR algorithm here and be very careful + * about the starting point. + */ + double x, logpcdf; + int iterations = 0; + int function_calls = 0; + double a=0, b=1; + double maxlogpcdf, psfrootn; + double dx, dxold; + + if (!(n > 0 && psf >= 0.0 && pcdf >= 0.0 && pcdf <= 1.0 && psf <= 1.0)) { + sf_error("smirnovi", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (fabs(1.0 - pcdf - psf) > 4* DBL_EPSILON) { + sf_error("smirnovi", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + /* STEP 1: Handle psf==0, or pcdf == 0 */ + if (pcdf == 0.0) { + return 0.0; + } + if (psf == 0.0) { + return 1.0; + } + /* STEP 2: Handle n=1 */ + if (n == 1) { + return pcdf; + } + + /* STEP 3 Handle psf *very* close to 0. Correspond to (n-1)/n < x < 1 */ + psfrootn = pow(psf, 1.0 / n); + /* xmin > 1 - 1.0 / n */ + if (n < 150 && n*psfrootn <= 1) { + /* Solve exactly. */ + x = 1 - psfrootn; + return x; + } + + logpcdf = (pcdf < 0.5 ? log(pcdf) : log1p(-psf)); + + /* + * STEP 4 Find bracket and initial estimate for use in N-R + * 4(a) Handle 0 < x <= 1/n: pcdf = x * (1+x)^*(n-1) + */ + maxlogpcdf = logpow4(1, 0.0, n, 0, 1) + logpow4(n, 1, n, 0, n - 1); + if (logpcdf <= maxlogpcdf) { + double xmin = pcdf / SCIPY_El; + double xmax = pcdf; + double P1 = pow4(n, 1, n, 0, n - 1) / n; + double R = pcdf/P1; + double z0 = R; + /* + * Do one iteration of N-R solving: z*e^(z-1) = R, with z0=pcdf/P1 + * z <- z - (z exp(z-1) - pcdf)/((z+1)exp(z-1)) + * If z_0 = R, z_1 = R(1-exp(1-R))/(R+1) + */ + if (R >= 1) { + /* + * R=1 is OK; + * R>1 can happen due to truncation error for x = (1-1/n)+-eps + */ + R = 1; + x = R/n; + return x; + } + z0 = (z0*z0 + R * exp(1-z0))/(1+z0); + x = z0/n; + a = xmin*(1 - 4 * DBL_EPSILON); + a = MAX(a, 0); + b = xmax * (1 + 4 * DBL_EPSILON); + b = MIN(b, 1.0/n); + x = CLIP(x, a, b); + } + else + { + /* 4(b) : 1/n < x < (n-1)/n */ + double xmin = 1 - psfrootn; + double logpsf = (psf < 0.5 ? log(psf) : log1p(-pcdf)); + double xmax = sqrt(-logpsf / (2.0L * n)); + double xmax6 = xmax - 1.0L / (6 * n); + a = xmin; + b = xmax; + /* Allow for a little rounding error */ + a *= 1 - 4 * DBL_EPSILON; + b *= 1 + 4 * DBL_EPSILON; + a = MAX(xmin, 1.0/n); + b = MIN(xmax, 1-1.0/n); + x = xmax6; + } + if (x < a || x > b) { + x = (a + b)/2; + } + assert (x < 1); + + /* + * Skip computing fa, fb as that takes cycles and the exact values + * are not needed. + */ + + /* STEP 5 Run N-R. + * smirnov should be well-enough behaved for NR starting at this location. + * Use smirnov(n, x)-psf, or pcdf - smirnovc(n, x), whichever has smaller p. + */ + dxold = b - a; + dx = dxold; + do { + double dfdx, x0 = x, deltax, df; + assert(x < 1); + assert(x > 0); + { + ThreeProbs probs = _smirnov(n, x0); + ++function_calls; + df = ((pcdf < 0.5) ? (pcdf - probs.cdf) : (probs.sf - psf)); + dfdx = -probs.pdf; + } + if (df == 0) { + return x; + } + /* Update the bracketing interval */ + if (df > 0 && x > a) { + a = x; + } else if (df < 0 && x < b) { + b = x; + } + + if (dfdx == 0) { + /* + * x was not within tolerance, but now we hit a 0 derivative. + * This implies that x >> 1/sqrt(n), and even then |smirnovp| >= |smirnov| + * so this condition is unexpected. Do a bisection step. + */ + x = (a+b)/2; + deltax = x0 - x; + } else { + deltax = df / dfdx; + x = x0 - deltax; + } + /* + * Check out-of-bounds. + * Not expecting this to happen ofen --- smirnov is convex near x=1 and + * concave near x=0, and we should be approaching from the correct side. + * If out-of-bounds, replace x with a midpoint of the bracket. + * Also check fast enough convergence. + */ + if ((a <= x) && (x <= b) && (fabs(2 * deltax) <= fabs(dxold) || fabs(dxold) < 256 * DBL_EPSILON)) { + dxold = dx; + dx = deltax; + } else { + dxold = dx; + dx = dx / 2; + x = (a + b) / 2; + deltax = x0 - x; + } + /* + * Note that if psf is close to 1, f(x) -> 1, f'(x) -> -1. + * => abs difference |x-x0| is approx |f(x)-p| >= DBL_EPSILON, + * => |x-x0|/x >= DBL_EPSILON/x. + * => cannot use a purely relative criteria as it will fail for x close to 0. + */ + if (_within_tol(x, x0, (psf < 0.5 ? 0 : _xtol), _rtol)) { + break; + } + if (++iterations > MAXITER) { + sf_error("smirnovi", SF_ERROR_SLOW, NULL); + return (x); + } + } while (1); + return x; +} + + +double +smirnov(int n, double d) +{ + ThreeProbs probs; + if (isnan(d)) { + return NAN; + } + probs = _smirnov(n, d); + return probs.sf; +} + +double +smirnovc(int n, double d) +{ + ThreeProbs probs; + if (isnan(d)) { + return NAN; + } + probs = _smirnov(n, d); + return probs.cdf; +} + + +/* + * Derivative of smirnov(n, d) + * One interior point of discontinuity at d=1/n. +*/ +double +smirnovp(int n, double d) +{ + ThreeProbs probs; + if (!(n > 0 && d >= 0.0 && d <= 1.0)) { + return (NAN); + } + if (n == 1) { + /* Slope is always -1 for n=1, even at d = 1.0 */ + return -1.0; + } + if (d == 1.0) { + return -0.0; + } + /* + * If d is 0, the derivative is discontinuous, but approaching + * from the right the limit is -1 + */ + if (d == 0.0) { + return -1.0; + } + probs = _smirnov(n, d); + return -probs.pdf; +} + + +double +smirnovi(int n, double p) +{ + if (isnan(p)) { + return NAN; + } + return _smirnovi(n, p, 1-p); +} + +double +smirnovci(int n, double p) +{ + if (isnan(p)) { + return NAN; + } + return _smirnovi(n, 1-p, p); +} diff --git a/gtsam/3rdparty/cephes/cephes/lanczos.c b/gtsam/3rdparty/cephes/cephes/lanczos.c new file mode 100644 index 000000000..f92a8d208 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/lanczos.c @@ -0,0 +1,56 @@ +/* (C) Copyright John Maddock 2006. + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ + +/* Scipy changes: + * - 06-22-2016: Removed all code not related to double precision and + * ported to c for use in Cephes + */ + +#include "mconf.h" +#include "lanczos.h" + + +static double lanczos_sum(double x) +{ + return ratevl(x, lanczos_num, + sizeof(lanczos_num) / sizeof(lanczos_num[0]) - 1, + lanczos_denom, + sizeof(lanczos_denom) / sizeof(lanczos_denom[0]) - 1); +} + + +double lanczos_sum_expg_scaled(double x) +{ + return ratevl(x, lanczos_sum_expg_scaled_num, + sizeof(lanczos_sum_expg_scaled_num) / sizeof(lanczos_sum_expg_scaled_num[0]) - 1, + lanczos_sum_expg_scaled_denom, + sizeof(lanczos_sum_expg_scaled_denom) / sizeof(lanczos_sum_expg_scaled_denom[0]) - 1); +} + + +static double lanczos_sum_near_1(double dx) +{ + double result = 0; + unsigned k; + + for (k = 1; k <= sizeof(lanczos_sum_near_1_d)/sizeof(lanczos_sum_near_1_d[0]); ++k) { + result += (-lanczos_sum_near_1_d[k-1]*dx)/(k*dx + k*k); + } + return result; +} + + +static double lanczos_sum_near_2(double dx) +{ + double result = 0; + double x = dx + 2; + unsigned k; + + for(k = 1; k <= sizeof(lanczos_sum_near_2_d)/sizeof(lanczos_sum_near_2_d[0]); ++k) { + result += (-lanczos_sum_near_2_d[k-1]*dx)/(x + k*x + k*k - 1); + } + return result; +} diff --git a/gtsam/3rdparty/cephes/cephes/lanczos.h b/gtsam/3rdparty/cephes/cephes/lanczos.h new file mode 100644 index 000000000..92ab8c1b2 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/lanczos.h @@ -0,0 +1,133 @@ +/* (C) Copyright John Maddock 2006. + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ + +/* Both lanczos.h and lanczos.c were formed from Boost's lanczos.hpp + * + * Scipy changes: + * - 06-22-2016: Removed all code not related to double precision and + * ported to c for use in Cephes. Note that the order of the + * coefficients is reversed to match the behavior of polevl. + */ + +/* + * Optimal values for G for each N are taken from + * https://web.viu.ca/pughg/phdThesis/phdThesis.pdf, + * as are the theoretical error bounds. + * + * Constants calculated using the method described by Godfrey + * https://my.fit.edu/~gabdo/gamma.txt and elaborated by Toth at + * https://www.rskey.org/gamma.htm using NTL::RR at 1000 bit precision. + */ + +/* + * Lanczos Coefficients for N=13 G=6.024680040776729583740234375 + * Max experimental error (with arbitrary precision arithmetic) 1.196214e-17 + * Generated with compiler: Microsoft Visual C++ version 8.0 on Win32 at Mar 23 2006 + * + * Use for double precision. + */ + +#ifndef LANCZOS_H +#define LANCZOS_H + + +static const double lanczos_num[13] = { + 2.506628274631000270164908177133837338626, + 210.8242777515793458725097339207133627117, + 8071.672002365816210638002902272250613822, + 186056.2653952234950402949897160456992822, + 2876370.628935372441225409051620849613599, + 31426415.58540019438061423162831820536287, + 248874557.8620541565114603864132294232163, + 1439720407.311721673663223072794912393972, + 6039542586.35202800506429164430729792107, + 17921034426.03720969991975575445893111267, + 35711959237.35566804944018545154716670596, + 42919803642.64909876895789904700198885093, + 23531376880.41075968857200767445163675473 +}; + +static const double lanczos_denom[13] = { + 1, + 66, + 1925, + 32670, + 357423, + 2637558, + 13339535, + 45995730, + 105258076, + 150917976, + 120543840, + 39916800, + 0 +}; + +static const double lanczos_sum_expg_scaled_num[13] = { + 0.006061842346248906525783753964555936883222, + 0.5098416655656676188125178644804694509993, + 19.51992788247617482847860966235652136208, + 449.9445569063168119446858607650988409623, + 6955.999602515376140356310115515198987526, + 75999.29304014542649875303443598909137092, + 601859.6171681098786670226533699352302507, + 3481712.15498064590882071018964774556468, + 14605578.08768506808414169982791359218571, + 43338889.32467613834773723740590533316085, + 86363131.28813859145546927288977868422342, + 103794043.1163445451906271053616070238554, + 56906521.91347156388090791033559122686859 +}; + +static const double lanczos_sum_expg_scaled_denom[13] = { + 1, + 66, + 1925, + 32670, + 357423, + 2637558, + 13339535, + 45995730, + 105258076, + 150917976, + 120543840, + 39916800, + 0 +}; + +static const double lanczos_sum_near_1_d[12] = { + 0.3394643171893132535170101292240837927725e-9, + -0.2499505151487868335680273909354071938387e-8, + 0.8690926181038057039526127422002498960172e-8, + -0.1933117898880828348692541394841204288047e-7, + 0.3075580174791348492737947340039992829546e-7, + -0.2752907702903126466004207345038327818713e-7, + -0.1515973019871092388943437623825208095123e-5, + 0.004785200610085071473880915854204301886437, + -0.1993758927614728757314233026257810172008, + 1.483082862367253753040442933770164111678, + -3.327150580651624233553677113928873034916, + 2.208709979316623790862569924861841433016 +}; + +static const double lanczos_sum_near_2_d[12] = { + 0.1009141566987569892221439918230042368112e-8, + -0.7430396708998719707642735577238449585822e-8, + 0.2583592566524439230844378948704262291927e-7, + -0.5746670642147041587497159649318454348117e-7, + 0.9142922068165324132060550591210267992072e-7, + -0.8183698410724358930823737982119474130069e-7, + -0.4506604409707170077136555010018549819192e-5, + 0.01422519127192419234315002746252160965831, + -0.5926941084905061794445733628891024027949, + 4.408830289125943377923077727900630927902, + -9.8907772644920670589288081640128194231, + 6.565936202082889535528455955485877361223 +}; + +static const double lanczos_g = 6.024680040776729583740234375; + +#endif diff --git a/gtsam/3rdparty/cephes/cephes/mconf.h b/gtsam/3rdparty/cephes/cephes/mconf.h new file mode 100644 index 000000000..9f3deb628 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/mconf.h @@ -0,0 +1,109 @@ +/* mconf.h + * + * Common include file for math routines + * + * + * + * SYNOPSIS: + * + * #include "mconf.h" + * + * + * + * DESCRIPTION: + * + * The file includes a conditional assembly definition for the type of + * computer arithmetic (IEEE, Motorola IEEE, or UNKnown). + * + * For little-endian computers, such as IBM PC, that follow the + * IEEE Standard for Binary Floating Point Arithmetic (ANSI/IEEE + * Std 754-1985), the symbol IBMPC should be defined. These + * numbers have 53-bit significands. In this mode, constants + * are provided as arrays of hexadecimal 16 bit integers. + * + * Big-endian IEEE format is denoted MIEEE. On some RISC + * systems such as Sun SPARC, double precision constants + * must be stored on 8-byte address boundaries. Since integer + * arrays may be aligned differently, the MIEEE configuration + * may fail on such machines. + * + * To accommodate other types of computer arithmetic, all + * constants are also provided in a normal decimal radix + * which one can hope are correctly converted to a suitable + * format by the available C language compiler. To invoke + * this mode, define the symbol UNK. + * + * An important difference among these modes is a predefined + * set of machine arithmetic constants for each. The numbers + * MACHEP (the machine roundoff error), MAXNUM (largest number + * represented), and several other parameters are preset by + * the configuration symbol. Check the file const.c to + * ensure that these values are correct for your computer. + * + * Configurations NANS, INFINITIES, MINUSZERO, and DENORMAL + * may fail on many systems. Verify that they are supposed + * to work on your computer. + */ + +/* + * Cephes Math Library Release 2.3: June, 1995 + * Copyright 1984, 1987, 1989, 1995 by Stephen L. Moshier + */ + +#ifndef CEPHES_MCONF_H +#define CEPHES_MCONF_H + +#include +#include + +#include "cephes_names.h" +#include "cephes.h" +#include "polevl.h" +#include "sf_error.h" + +#define MAXITER 500 +#define EDOM 33 +#define ERANGE 34 + +/* Type of computer arithmetic */ + +/* UNKnown arithmetic, invokes coefficients given in + * normal decimal format. Beware of range boundary + * problems (MACHEP, MAXLOG, etc. in const.c) and + * roundoff problems in pow.c: + * (Sun SPARCstation) + */ + +/* SciPy note: by defining UNK, we prevent the compiler from + * casting integers to floating point numbers. If the Endianness + * is detected incorrectly, this causes problems on some platforms. + */ +#define UNK 1 + +/* Define to support tiny denormal numbers, else undefine. */ +#define DENORMAL 1 + +#define gamma Gamma + +/* + * Enable loop unrolling on GCC and use faster isnan et al. + */ +#if !defined(__clang__) && defined(__GNUC__) && defined(__GNUC_MINOR__) +#if __GNUC__ >= 5 || (__GNUC__ == 4 && __GNUC_MINOR__ >= 4) +#pragma GCC optimize("unroll-loops") +#define cephes_isnan(x) __builtin_isnan(x) +#define cephes_isinf(x) __builtin_isinf(x) +#define cephes_isfinite(x) __builtin_isfinite(x) +#endif +#endif +#ifndef cephes_isnan +#define cephes_isnan(x) isnan(x) +#define cephes_isinf(x) isinf(x) +#define cephes_isfinite(x) isfinite(x) +#endif + +/* Constants needed that are not available in the C standard library */ +#define SCIPY_EULER 0.577215664901532860606512090082402431 /* Euler constant */ +#define SCIPY_El 2.718281828459045235360287471352662498L /* e as long double */ + +#endif /* CEPHES_MCONF_H */ diff --git a/gtsam/3rdparty/cephes/cephes/nbdtr.c b/gtsam/3rdparty/cephes/cephes/nbdtr.c new file mode 100644 index 000000000..7697f257e --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/nbdtr.c @@ -0,0 +1,207 @@ +/* nbdtr.c + * + * Negative binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, nbdtr(); + * + * y = nbdtr( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms 0 through k of the negative + * binomial distribution: + * + * k + * -- ( n+j-1 ) n j + * > ( ) p (1-p) + * -- ( j ) + * j=0 + * + * In a sequence of Bernoulli trials, this is the probability + * that k or fewer failures precede the nth success. + * + * The terms are not computed individually; instead the incomplete + * beta integral is employed, according to the formula + * + * y = nbdtr( k, n, p ) = incbet( n, k+1, p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * Tested at random points (a,b,p), with p between 0 and 1. + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,100 100000 1.7e-13 8.8e-15 + * See also incbet.c. + * + */ + /* nbdtrc.c + * + * Complemented negative binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, nbdtrc(); + * + * y = nbdtrc( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms k+1 to infinity of the negative + * binomial distribution: + * + * inf + * -- ( n+j-1 ) n j + * > ( ) p (1-p) + * -- ( j ) + * j=k+1 + * + * The terms are not computed individually; instead the incomplete + * beta integral is employed, according to the formula + * + * y = nbdtrc( k, n, p ) = incbet( k+1, n, 1-p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * Tested at random points (a,b,p), with p between 0 and 1. + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,100 100000 1.7e-13 8.8e-15 + * See also incbet.c. + */ + +/* nbdtrc + * + * Complemented negative binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, nbdtrc(); + * + * y = nbdtrc( k, n, p ); + * + * DESCRIPTION: + * + * Returns the sum of the terms k+1 to infinity of the negative + * binomial distribution: + * + * inf + * -- ( n+j-1 ) n j + * > ( ) p (1-p) + * -- ( j ) + * j=k+1 + * + * The terms are not computed individually; instead the incomplete + * beta integral is employed, according to the formula + * + * y = nbdtrc( k, n, p ) = incbet( k+1, n, 1-p ). + * + * The arguments must be positive, with p ranging from 0 to 1. + * + * ACCURACY: + * + * See incbet.c. + */ + /* nbdtri + * + * Functional inverse of negative binomial distribution + * + * + * + * SYNOPSIS: + * + * int k, n; + * double p, y, nbdtri(); + * + * p = nbdtri( k, n, y ); + * + * DESCRIPTION: + * + * Finds the argument p such that nbdtr(k,n,p) is equal to y. + * + * ACCURACY: + * + * Tested at random points (a,b,y), with y between 0 and 1. + * + * a,b Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,100 100000 1.5e-14 8.5e-16 + * See also incbi.c. + */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +double nbdtrc(int k, int n, double p) +{ + double dk, dn; + + if ((p < 0.0) || (p > 1.0)) + goto domerr; + if (k < 0) { + domerr: + sf_error("nbdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + dk = k + 1; + dn = n; + return (incbet(dk, dn, 1.0 - p)); +} + + + +double nbdtr(int k, int n, double p) +{ + double dk, dn; + + if ((p < 0.0) || (p > 1.0)) + goto domerr; + if (k < 0) { + domerr: + sf_error("nbdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + dk = k + 1; + dn = n; + return (incbet(dn, dk, p)); +} + + + +double nbdtri(int k, int n, double p) +{ + double dk, dn, w; + + if ((p < 0.0) || (p > 1.0)) + goto domerr; + if (k < 0) { + domerr: + sf_error("nbdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + dk = k + 1; + dn = n; + w = incbi(dn, dk, p); + return (w); +} diff --git a/gtsam/3rdparty/cephes/cephes/ndtr.c b/gtsam/3rdparty/cephes/cephes/ndtr.c new file mode 100644 index 000000000..168e98b5a --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ndtr.c @@ -0,0 +1,305 @@ +/* ndtr.c + * + * Normal distribution function + * + * + * + * SYNOPSIS: + * + * double x, y, ndtr(); + * + * y = ndtr( x ); + * + * + * + * DESCRIPTION: + * + * Returns the area under the Gaussian probability density + * function, integrated from minus infinity to x: + * + * x + * - + * 1 | | 2 + * ndtr(x) = --------- | exp( - t /2 ) dt + * sqrt(2pi) | | + * - + * -inf. + * + * = ( 1 + erf(z) ) / 2 + * = erfc(z) / 2 + * + * where z = x/sqrt(2). Computation is via the functions + * erf and erfc. + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -13,0 30000 3.4e-14 6.7e-15 + * + * + * ERROR MESSAGES: + * + * message condition value returned + * erfc underflow x > 37.519379347 0.0 + * + */ +/* erf.c + * + * Error function + * + * + * + * SYNOPSIS: + * + * double x, y, erf(); + * + * y = erf( x ); + * + * + * + * DESCRIPTION: + * + * The integral is + * + * x + * - + * 2 | | 2 + * erf(x) = -------- | exp( - t ) dt. + * sqrt(pi) | | + * - + * 0 + * + * For 0 <= |x| < 1, erf(x) = x * P4(x**2)/Q5(x**2); otherwise + * erf(x) = 1 - erfc(x). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,1 30000 3.7e-16 1.0e-16 + * + */ +/* erfc.c + * + * Complementary error function + * + * + * + * SYNOPSIS: + * + * double x, y, erfc(); + * + * y = erfc( x ); + * + * + * + * DESCRIPTION: + * + * + * 1 - erf(x) = + * + * inf. + * - + * 2 | | 2 + * erfc(x) = -------- | exp( - t ) dt + * sqrt(pi) | | + * - + * x + * + * + * For small x, erfc(x) = 1 - erf(x); otherwise rational + * approximations are computed. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,26.6417 30000 5.7e-14 1.5e-14 + */ + + +/* + * Cephes Math Library Release 2.2: June, 1992 + * Copyright 1984, 1987, 1988, 1992 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include /* DBL_EPSILON */ +#include "mconf.h" + +extern double MAXLOG; + +static double P[] = { + 2.46196981473530512524E-10, + 5.64189564831068821977E-1, + 7.46321056442269912687E0, + 4.86371970985681366614E1, + 1.96520832956077098242E2, + 5.26445194995477358631E2, + 9.34528527171957607540E2, + 1.02755188689515710272E3, + 5.57535335369399327526E2 +}; + +static double Q[] = { + /* 1.00000000000000000000E0, */ + 1.32281951154744992508E1, + 8.67072140885989742329E1, + 3.54937778887819891062E2, + 9.75708501743205489753E2, + 1.82390916687909736289E3, + 2.24633760818710981792E3, + 1.65666309194161350182E3, + 5.57535340817727675546E2 +}; + +static double R[] = { + 5.64189583547755073984E-1, + 1.27536670759978104416E0, + 5.01905042251180477414E0, + 6.16021097993053585195E0, + 7.40974269950448939160E0, + 2.97886665372100240670E0 +}; + +static double S[] = { + /* 1.00000000000000000000E0, */ + 2.26052863220117276590E0, + 9.39603524938001434673E0, + 1.20489539808096656605E1, + 1.70814450747565897222E1, + 9.60896809063285878198E0, + 3.36907645100081516050E0 +}; + +static double T[] = { + 9.60497373987051638749E0, + 9.00260197203842689217E1, + 2.23200534594684319226E3, + 7.00332514112805075473E3, + 5.55923013010394962768E4 +}; + +static double U[] = { + /* 1.00000000000000000000E0, */ + 3.35617141647503099647E1, + 5.21357949780152679795E2, + 4.59432382970980127987E3, + 2.26290000613890934246E4, + 4.92673942608635921086E4 +}; + +#define UTHRESH 37.519379347 + + +double ndtr(double a) +{ + double x, y, z; + + if (cephes_isnan(a)) { + sf_error("ndtr", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + x = a * M_SQRT1_2; + z = fabs(x); + + if (z < M_SQRT1_2) { + y = 0.5 + 0.5 * erf(x); + } + else { + y = 0.5 * erfc(z); + if (x > 0) { + y = 1.0 - y; + } + } + + return y; +} + + +double erfc(double a) +{ + double p, q, x, y, z; + + if (cephes_isnan(a)) { + sf_error("erfc", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (a < 0.0) { + x = -a; + } + else { + x = a; + } + + if (x < 1.0) { + return 1.0 - erf(a); + } + + z = -a * a; + + if (z < -MAXLOG) { + goto under; + } + + z = exp(z); + + if (x < 8.0) { + p = polevl(x, P, 8); + q = p1evl(x, Q, 8); + } + else { + p = polevl(x, R, 5); + q = p1evl(x, S, 6); + } + y = (z * p) / q; + + if (a < 0) { + y = 2.0 - y; + } + + if (y != 0.0) { + return y; + } + +under: + sf_error("erfc", SF_ERROR_UNDERFLOW, NULL); + if (a < 0) { + return 2.0; + } + else { + return 0.0; + } +} + + + +double erf(double x) +{ + double y, z; + + if (cephes_isnan(x)) { + sf_error("erf", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + if (x < 0.0) { + return -erf(-x); + } + + if (fabs(x) > 1.0) { + return (1.0 - erfc(x)); + } + z = x * x; + + y = x * polevl(z, T, 4) / p1evl(z, U, 5); + return y; +} diff --git a/gtsam/3rdparty/cephes/cephes/ndtri.c b/gtsam/3rdparty/cephes/cephes/ndtri.c new file mode 100644 index 000000000..e7fe5cce0 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/ndtri.c @@ -0,0 +1,176 @@ +/* ndtri.c + * + * Inverse of Normal distribution function + * + * + * + * SYNOPSIS: + * + * double x, y, ndtri(); + * + * x = ndtri( y ); + * + * + * + * DESCRIPTION: + * + * Returns the argument, x, for which the area under the + * Gaussian probability density function (integrated from + * minus infinity to x) is equal to y. + * + * + * For small arguments 0 < y < exp(-2), the program computes + * z = sqrt( -2.0 * log(y) ); then the approximation is + * x = z - log(z)/z - (1/z) P(1/z) / Q(1/z). + * There are two rational functions P/Q, one for 0 < y < exp(-32) + * and the other for y up to exp(-2). For larger arguments, + * w = y - 0.5, and x/sqrt(2pi) = w + w**3 R(w**2)/S(w**2)). + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0.125, 1 20000 7.2e-16 1.3e-16 + * IEEE 3e-308, 0.135 50000 4.6e-16 9.8e-17 + * + * + * ERROR MESSAGES: + * + * message condition value returned + * ndtri domain x < 0 NAN + * ndtri domain x > 1 NAN + * + */ + + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +/* sqrt(2pi) */ +static double s2pi = 2.50662827463100050242E0; + +/* approximation for 0 <= |y - 0.5| <= 3/8 */ +static double P0[5] = { + -5.99633501014107895267E1, + 9.80010754185999661536E1, + -5.66762857469070293439E1, + 1.39312609387279679503E1, + -1.23916583867381258016E0, +}; + +static double Q0[8] = { + /* 1.00000000000000000000E0, */ + 1.95448858338141759834E0, + 4.67627912898881538453E0, + 8.63602421390890590575E1, + -2.25462687854119370527E2, + 2.00260212380060660359E2, + -8.20372256168333339912E1, + 1.59056225126211695515E1, + -1.18331621121330003142E0, +}; + +/* Approximation for interval z = sqrt(-2 log y ) between 2 and 8 + * i.e., y between exp(-2) = .135 and exp(-32) = 1.27e-14. + */ +static double P1[9] = { + 4.05544892305962419923E0, + 3.15251094599893866154E1, + 5.71628192246421288162E1, + 4.40805073893200834700E1, + 1.46849561928858024014E1, + 2.18663306850790267539E0, + -1.40256079171354495875E-1, + -3.50424626827848203418E-2, + -8.57456785154685413611E-4, +}; + +static double Q1[8] = { + /* 1.00000000000000000000E0, */ + 1.57799883256466749731E1, + 4.53907635128879210584E1, + 4.13172038254672030440E1, + 1.50425385692907503408E1, + 2.50464946208309415979E0, + -1.42182922854787788574E-1, + -3.80806407691578277194E-2, + -9.33259480895457427372E-4, +}; + +/* Approximation for interval z = sqrt(-2 log y ) between 8 and 64 + * i.e., y between exp(-32) = 1.27e-14 and exp(-2048) = 3.67e-890. + */ + +static double P2[9] = { + 3.23774891776946035970E0, + 6.91522889068984211695E0, + 3.93881025292474443415E0, + 1.33303460815807542389E0, + 2.01485389549179081538E-1, + 1.23716634817820021358E-2, + 3.01581553508235416007E-4, + 2.65806974686737550832E-6, + 6.23974539184983293730E-9, +}; + +static double Q2[8] = { + /* 1.00000000000000000000E0, */ + 6.02427039364742014255E0, + 3.67983563856160859403E0, + 1.37702099489081330271E0, + 2.16236993594496635890E-1, + 1.34204006088543189037E-2, + 3.28014464682127739104E-4, + 2.89247864745380683936E-6, + 6.79019408009981274425E-9, +}; + +double ndtri(double y0) +{ + double x, y, z, y2, x0, x1; + int code; + + if (y0 == 0.0) { + return -INFINITY; + } + if (y0 == 1.0) { + return INFINITY; + } + if (y0 < 0.0 || y0 > 1.0) { + sf_error("ndtri", SF_ERROR_DOMAIN, NULL); + return NAN; + } + code = 1; + y = y0; + if (y > (1.0 - 0.13533528323661269189)) { /* 0.135... = exp(-2) */ + y = 1.0 - y; + code = 0; + } + + if (y > 0.13533528323661269189) { + y = y - 0.5; + y2 = y * y; + x = y + y * (y2 * polevl(y2, P0, 4) / p1evl(y2, Q0, 8)); + x = x * s2pi; + return (x); + } + + x = sqrt(-2.0 * log(y)); + x0 = x - log(x) / x; + + z = 1.0 / x; + if (x < 8.0) /* y > exp(-32) = 1.2664165549e-14 */ + x1 = z * polevl(z, P1, 8) / p1evl(z, Q1, 8); + else + x1 = z * polevl(z, P2, 8) / p1evl(z, Q2, 8); + x = x0 - x1; + if (code != 0) + x = -x; + return (x); +} diff --git a/gtsam/3rdparty/cephes/cephes/owens_t.c b/gtsam/3rdparty/cephes/cephes/owens_t.c new file mode 100644 index 000000000..6eb063510 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/owens_t.c @@ -0,0 +1,364 @@ +/* Copyright Benjamin Sobotta 2012 + * + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ + +/* + * Reference: + * Mike Patefield, David Tandy + * FAST AND ACCURATE CALCULATION OF OWEN'S T-FUNCTION + * Journal of Statistical Software, 5 (5), 1-25 + */ +#include "mconf.h" + +static const int SELECT_METHOD[] = { + 0, 0, 1, 12, 12, 12, 12, 12, 12, 12, 12, 15, 15, 15, 8, + 0, 1, 1, 2, 2, 4, 4, 13, 13, 14, 14, 15, 15, 15, 8, + 1, 1, 2, 2, 2, 4, 4, 14, 14, 14, 14, 15, 15, 15, 9, + 1, 1, 2, 4, 4, 4, 4, 6, 6, 15, 15, 15, 15, 15, 9, + 1, 2 , 2, 4, 4, 5 , 5, 7, 7, 16 ,16, 16, 11, 11, 10, + 1, 2 , 4, 4 , 4, 5 , 5, 7, 7, 16, 16, 16, 11, 11, 11, + 1, 2 , 3, 3, 5, 5 , 7, 7, 16, 16, 16, 16, 16, 11, 11, + 1, 2 , 3 , 3 , 5, 5, 17, 17, 17, 17, 16, 16, 16, 11, 11 +}; + +static const double HRANGE[] = {0.02, 0.06, 0.09, 0.125, 0.26, 0.4, 0.6, 1.6, + 1.7, 2.33, 2.4, 3.36, 3.4, 4.8}; + +static const double ARANGE[] = {0.025, 0.09, 0.15, 0.36, 0.5, 0.9, 0.99999}; + +static const double ORD[] = {2, 3, 4, 5, 7, 10, 12, 18, 10, 20, 30, 0, 4, 7, + 8, 20, 0, 0}; + +static const int METHODS[] = {1, 1, 1, 1, 1, 1, 1, 1, 2, 2, 2, 3, 4, 4, 4, 4, + 5, 6}; + +static const double C[] = { + 0.99999999999999999999999729978162447266851932041876728736094298092917625009873, + -0.99999999999999999999467056379678391810626533251885323416799874878563998732905968, + 0.99999999999999999824849349313270659391127814689133077036298754586814091034842536, + -0.9999999999999997703859616213643405880166422891953033591551179153879839440241685, + 0.99999999999998394883415238173334565554173013941245103172035286759201504179038147, + -0.9999999999993063616095509371081203145247992197457263066869044528823599399470977, + 0.9999999999797336340409464429599229870590160411238245275855903767652432017766116267, + -0.999999999574958412069046680119051639753412378037565521359444170241346845522403274, + 0.9999999933226234193375324943920160947158239076786103108097456617750134812033362048, + -0.9999999188923242461073033481053037468263536806742737922476636768006622772762168467, + 0.9999992195143483674402853783549420883055129680082932629160081128947764415749728967, + -0.999993935137206712830997921913316971472227199741857386575097250553105958772041501, + 0.99996135597690552745362392866517133091672395614263398912807169603795088421057688716, + -0.99979556366513946026406788969630293820987757758641211293079784585126692672425362469, + 0.999092789629617100153486251423850590051366661947344315423226082520411961968929483, + -0.996593837411918202119308620432614600338157335862888580671450938858935084316004769854, + 0.98910017138386127038463510314625339359073956513420458166238478926511821146316469589567, + -0.970078558040693314521331982203762771512160168582494513347846407314584943870399016019, + 0.92911438683263187495758525500033707204091967947532160289872782771388170647150321633673, + -0.8542058695956156057286980736842905011429254735181323743367879525470479126968822863, + 0.73796526033030091233118357742803709382964420335559408722681794195743240930748630755, + -0.58523469882837394570128599003785154144164680587615878645171632791404210655891158, + 0.415997776145676306165661663581868460503874205343014196580122174949645271353372263, + -0.2588210875241943574388730510317252236407805082485246378222935376279663808416534365, + 0.1375535825163892648504646951500265585055789019410617565727090346559210218472356689, + -0.0607952766325955730493900985022020434830339794955745989150270485056436844239206648, + 0.0216337683299871528059836483840390514275488679530797294557060229266785853764115, + -0.00593405693455186729876995814181203900550014220428843483927218267309209471516256, + 0.0011743414818332946510474576182739210553333860106811865963485870668929503649964142, + -1.489155613350368934073453260689881330166342484405529981510694514036264969925132E-4, + 9.072354320794357587710929507988814669454281514268844884841547607134260303118208E-6 +}; + +static const double PTS[] = { + 0.35082039676451715489E-02, 0.31279042338030753740E-01, + 0.85266826283219451090E-01, 0.16245071730812277011E+00, + 0.25851196049125434828E+00, 0.36807553840697533536E+00, + 0.48501092905604697475E+00, 0.60277514152618576821E+00, + 0.71477884217753226516E+00, 0.81475510988760098605E+00, + 0.89711029755948965867E+00, 0.95723808085944261843E+00, + 0.99178832974629703586E+00 +}; + +static const double WTS[] = { + 0.18831438115323502887E-01, 0.18567086243977649478E-01, + 0.18042093461223385584E-01, 0.17263829606398753364E-01, + 0.16243219975989856730E-01, 0.14994592034116704829E-01, + 0.13535474469662088392E-01, 0.11886351605820165233E-01, + 0.10070377242777431897E-01, 0.81130545742299586629E-02, + 0.60419009528470238773E-02, 0.38862217010742057883E-02, + 0.16793031084546090448E-02 +}; + + +static int get_method(double h, double a) { + int ihint, iaint, i; + + ihint = 14; + iaint = 7; + + for (i = 0; i < 14; i++) { + if (h <= HRANGE[i]) { + ihint = i; + break; + } + } + + for (i = 0; i < 7; i++) { + if (a <= ARANGE[i]) { + iaint = i; + break; + } + } + return SELECT_METHOD[iaint * 15 + ihint]; +} + + +static double owens_t_norm1(double x) { + return erf(x / sqrt(2)) / 2; +} + + +static double owens_t_norm2(double x) { + return erfc(x / sqrt(2)) / 2; +} + + +static double owensT1(double h, double a, double m) { + int j = 1; + int jj = 1; + + double hs = -0.5 * h * h; + double dhs = exp(hs); + double as = a * a; + double aj = a / (2 * M_PI); + double dj = expm1(hs); + double gj = hs * dhs; + + double val = atan(a) / (2 * M_PI); + + while (1) { + val += dj*aj / jj; + + if (m <= j) { + break; + } + j++; + jj += 2; + aj *= as; + dj = gj - dj; + gj *= hs / j; + } + + return val; +} + + +static double owensT2(double h, double a, double ah, double m) { + int i = 1; + int maxi = 2 * m + 1; + double hs = h * h; + double as = -a * a; + double y = 1.0 / hs; + double val = 0.0; + double vi = a*exp(-0.5 * ah * ah) / sqrt(2 * M_PI); + double z = (ndtr(ah) - 0.5) / h; + + while (1) { + val += z; + if (maxi <= i) { + break; + } + z = y * (vi - i * z); + vi *= as; + i += 2; + } + val *= exp(-0.5 * hs) / sqrt(2 * M_PI); + + return val; +} + + +static double owensT3(double h, double a, double ah) { + double aa, hh, y, vi, zi, result; + int i; + + aa = a * a; + hh = h * h; + y = 1 / hh; + + vi = a * exp(-ah * ah/ 2) / sqrt(2 * M_PI); + zi = owens_t_norm1(ah) / h; + result = 0; + + for(i = 0; i<= 30; i++) { + result += zi * C[i]; + zi = y * ((2 * i + 1) * zi - vi); + vi *= aa; + } + + result *= exp(-hh / 2) / sqrt(2 * M_PI); + + return result; +} + + +static double owensT4(double h, double a, double m) { + double maxi, hh, naa, ai, yi, result; + int i; + + maxi = 2 * m + 1; + hh = h * h; + naa = -a * a; + + i = 1; + ai = a * exp(-hh * (1 - naa) / 2) / (2 * M_PI); + yi = 1; + result = 0; + + while (1) { + result += ai * yi; + + if (maxi <= i) { + break; + } + + i += 2; + yi = (1 - hh * yi) / i; + ai *= naa; + } + + return result; +} + + +static double owensT5(double h, double a) { + double result, r, aa, nhh; + int i; + + result = 0; + r = 0; + aa = a * a; + nhh = -0.5 * h * h; + + for (i = 1; i < 14; i++) { + r = 1 + aa * PTS[i - 1]; + result += WTS[i - 1] * exp(nhh * r) / r; + } + + result *= a; + + return result; +} + + +static double owensT6(double h, double a) { + double normh, y, r, result; + + normh = owens_t_norm2(h); + y = 1 - a; + r = atan2(y, (1 + a)); + result = normh * (1 - normh) / 2; + + if (r != 0) { + result -= r * exp(-y * h * h / (2 * r)) / (2 * M_PI); + } + + return result; +} + + +static double owens_t_dispatch(double h, double a, double ah) { + int index, meth_code; + double m, result; + + if (h == 0) { + return atan(a) / (2 * M_PI); + } + if (a == 0) { + return 0; + } + if (a == 1) { + return owens_t_norm2(-h) * owens_t_norm2(h) / 2; + } + + index = get_method(h, a); + m = ORD[index]; + meth_code = METHODS[index]; + + switch(meth_code) { + case 1: + result = owensT1(h, a, m); + break; + case 2: + result = owensT2(h, a, ah, m); + break; + case 3: + result = owensT3(h, a, ah); + break; + case 4: + result = owensT4(h, a, m); + break; + case 5: + result = owensT5(h, a); + break; + case 6: + result = owensT6(h, a); + break; + default: + result = NAN; + } + + return result; +} + + +double owens_t(double h, double a) { + double result, fabs_a, fabs_ah, normh, normah; + + if (cephes_isnan(h) || cephes_isnan(a)) { + return NAN; + } + + /* exploit that T(-h,a) == T(h,a) */ + h = fabs(h); + + /* + * Use equation (2) in the paper to remap the arguments such that + * h >= 0 and 0 <= a <= 1 for the call of the actual computation + * routine. + */ + fabs_a = fabs(a); + fabs_ah = fabs_a * h; + + if (fabs_a == INFINITY) { + /* See page 13 in the paper */ + result = 0.5 * owens_t_norm2(h); + } + else if (h == INFINITY) { + result = 0; + } + else if (fabs_a <= 1) { + result = owens_t_dispatch(h, fabs_a, fabs_ah); + } + else { + if (fabs_ah <= 0.67) { + normh = owens_t_norm1(h); + normah = owens_t_norm1(fabs_ah); + result = 0.25 - normh * normah - + owens_t_dispatch(fabs_ah, (1 / fabs_a), h); + } + else { + normh = owens_t_norm2(h); + normah = owens_t_norm2(fabs_ah); + result = (normh + normah) / 2 - normh * normah - + owens_t_dispatch(fabs_ah, (1 / fabs_a), h); + } + } + + if (a < 0) { + /* exploit that T(h,-a) == -T(h,a) */ + return -result; + } + + return result; +} diff --git a/gtsam/3rdparty/cephes/cephes/pdtr.c b/gtsam/3rdparty/cephes/cephes/pdtr.c new file mode 100644 index 000000000..0249074d9 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/pdtr.c @@ -0,0 +1,173 @@ +/* pdtr.c + * + * Poisson distribution + * + * + * + * SYNOPSIS: + * + * int k; + * double m, y, pdtr(); + * + * y = pdtr( k, m ); + * + * + * + * DESCRIPTION: + * + * Returns the sum of the first k terms of the Poisson + * distribution: + * + * k j + * -- -m m + * > e -- + * -- j! + * j=0 + * + * The terms are not summed directly; instead the incomplete + * Gamma integral is employed, according to the relation + * + * y = pdtr( k, m ) = igamc( k+1, m ). + * + * The arguments must both be nonnegative. + * + * + * + * ACCURACY: + * + * See igamc(). + * + */ +/* pdtrc() + * + * Complemented poisson distribution + * + * + * + * SYNOPSIS: + * + * int k; + * double m, y, pdtrc(); + * + * y = pdtrc( k, m ); + * + * + * + * DESCRIPTION: + * + * Returns the sum of the terms k+1 to infinity of the Poisson + * distribution: + * + * inf. j + * -- -m m + * > e -- + * -- j! + * j=k+1 + * + * The terms are not summed directly; instead the incomplete + * Gamma integral is employed, according to the formula + * + * y = pdtrc( k, m ) = igam( k+1, m ). + * + * The arguments must both be nonnegative. + * + * + * + * ACCURACY: + * + * See igam.c. + * + */ +/* pdtri() + * + * Inverse Poisson distribution + * + * + * + * SYNOPSIS: + * + * int k; + * double m, y, pdtr(); + * + * m = pdtri( k, y ); + * + * + * + * + * DESCRIPTION: + * + * Finds the Poisson variable x such that the integral + * from 0 to x of the Poisson density is equal to the + * given probability y. + * + * This is accomplished using the inverse Gamma integral + * function and the relation + * + * m = igamci( k+1, y ). + * + * + * + * + * ACCURACY: + * + * See igami.c. + * + * ERROR MESSAGES: + * + * message condition value returned + * pdtri domain y < 0 or y >= 1 0.0 + * k < 0 + * + */ + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" + +double pdtrc(double k, double m) +{ + double v; + + if (k < 0.0 || m < 0.0) { + sf_error("pdtrc", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (m == 0.0) { + return 0.0; + } + v = floor(k) + 1; + return (igam(v, m)); +} + + +double pdtr(double k, double m) +{ + double v; + + if (k < 0 || m < 0) { + sf_error("pdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (m == 0.0) { + return 1.0; + } + v = floor(k) + 1; + return (igamc(v, m)); +} + + +double pdtri(int k, double y) +{ + double v; + + if ((k < 0) || (y < 0.0) || (y >= 1.0)) { + sf_error("pdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + v = k + 1; + v = igamci(v, y); + return (v); +} diff --git a/gtsam/3rdparty/cephes/cephes/poch.c b/gtsam/3rdparty/cephes/cephes/poch.c new file mode 100644 index 000000000..4c04fa14e --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/poch.c @@ -0,0 +1,81 @@ +/* + * Pochhammer symbol (a)_m = gamma(a + m) / gamma(a) + */ +#include "mconf.h" + +static double is_nonpos_int(double x) +{ + return x <= 0 && x == ceil(x) && fabs(x) < 1e13; +} + +double poch(double a, double m) +{ + double r; + + r = 1.0; + + /* + * 1. Reduce magnitude of `m` to |m| < 1 by using recurrence relations. + * + * This may end up in over/underflow, but then the function itself either + * diverges or goes to zero. In case the remainder goes to the opposite + * direction, we end up returning 0*INF = NAN, which is OK. + */ + + /* Recurse down */ + while (m >= 1.0) { + if (a + m == 1) { + break; + } + m -= 1.0; + r *= (a + m); + if (!isfinite(r) || r == 0) { + break; + } + } + + /* Recurse up */ + while (m <= -1.0) { + if (a + m == 0) { + break; + } + r /= (a + m); + m += 1.0; + if (!isfinite(r) || r == 0) { + break; + } + } + + /* + * 2. Evaluate function with reduced `m` + * + * Now either `m` is not big, or the `r` product has over/underflown. + * If so, the function itself does similarly. + */ + + if (m == 0) { + /* Easy case */ + return r; + } + else if (a > 1e4 && fabs(m) <= 1) { + /* Avoid loss of precision */ + return r * pow(a, m) * ( + 1 + + m*(m-1)/(2*a) + + m*(m-1)*(m-2)*(3*m-1)/(24*a*a) + + m*m*(m-1)*(m-1)*(m-2)*(m-3)/(48*a*a*a) + ); + } + + /* Check for infinity */ + if (is_nonpos_int(a + m) && !is_nonpos_int(a) && a + m != m) { + return INFINITY; + } + + /* Check for zero */ + if (!is_nonpos_int(a + m) && is_nonpos_int(a)) { + return 0; + } + + return r * exp(lgam(a + m) - lgam(a)) * gammasgn(a + m) * gammasgn(a); +} diff --git a/gtsam/3rdparty/cephes/cephes/polevl.h b/gtsam/3rdparty/cephes/cephes/polevl.h new file mode 100644 index 000000000..eb23ddf88 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/polevl.h @@ -0,0 +1,165 @@ +/* polevl.c + * p1evl.c + * + * Evaluate polynomial + * + * + * + * SYNOPSIS: + * + * int N; + * double x, y, coef[N+1], polevl[]; + * + * y = polevl( x, coef, N ); + * + * + * + * DESCRIPTION: + * + * Evaluates polynomial of degree N: + * + * 2 N + * y = C + C x + C x +...+ C x + * 0 1 2 N + * + * Coefficients are stored in reverse order: + * + * coef[0] = C , ..., coef[N] = C . + * N 0 + * + * The function p1evl() assumes that c_N = 1.0 so that coefficent + * is omitted from the array. Its calling arguments are + * otherwise the same as polevl(). + * + * + * SPEED: + * + * In the interest of speed, there are no checks for out + * of bounds arithmetic. This routine is used by most of + * the functions in the library. Depending on available + * equipment features, the user may wish to rewrite the + * program in microcode or assembly language. + * + */ + +/* + * Cephes Math Library Release 2.1: December, 1988 + * Copyright 1984, 1987, 1988 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +/* Sources: + * [1] Holin et. al., "Polynomial and Rational Function Evaluation", + * https://www.boost.org/doc/libs/1_61_0/libs/math/doc/html/math_toolkit/roots/rational.html + */ + +/* Scipy changes: + * - 06-23-2016: add code for evaluating rational functions + */ + +#ifndef CEPHES_POLEV +#define CEPHES_POLEV + +#include + +static inline double polevl(double x, const double coef[], int N) +{ + double ans; + int i; + const double *p; + + p = coef; + ans = *p++; + i = N; + + do + ans = ans * x + *p++; + while (--i); + + return (ans); +} + +/* p1evl() */ +/* N + * Evaluate polynomial when coefficient of x is 1.0. + * That is, C_{N} is assumed to be 1, and that coefficient + * is not included in the input array coef. + * coef must have length N and contain the polynomial coefficients + * stored as + * coef[0] = C_{N-1} + * coef[1] = C_{N-2} + * ... + * coef[N-2] = C_1 + * coef[N-1] = C_0 + * Otherwise same as polevl. + */ + +static inline double p1evl(double x, const double coef[], int N) +{ + double ans; + const double *p; + int i; + + p = coef; + ans = x + *p++; + i = N - 1; + + do + ans = ans * x + *p++; + while (--i); + + return (ans); +} + +/* Evaluate a rational function. See [1]. */ + +static inline double ratevl(double x, const double num[], int M, + const double denom[], int N) +{ + int i, dir; + double y, num_ans, denom_ans; + double absx = fabs(x); + const double *p; + + if (absx > 1) { + /* Evaluate as a polynomial in 1/x. */ + dir = -1; + p = num + M; + y = 1 / x; + } else { + dir = 1; + p = num; + y = x; + } + + /* Evaluate the numerator */ + num_ans = *p; + p += dir; + for (i = 1; i <= M; i++) { + num_ans = num_ans * y + *p; + p += dir; + } + + /* Evaluate the denominator */ + if (absx > 1) { + p = denom + N; + } else { + p = denom; + } + + denom_ans = *p; + p += dir; + for (i = 1; i <= N; i++) { + denom_ans = denom_ans * y + *p; + p += dir; + } + + if (absx > 1) { + i = N - M; + return pow(x, i) * num_ans / denom_ans; + } else { + return num_ans / denom_ans; + } +} + +#endif diff --git a/gtsam/3rdparty/cephes/cephes/psi.c b/gtsam/3rdparty/cephes/cephes/psi.c new file mode 100644 index 000000000..190c6d162 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/psi.c @@ -0,0 +1,205 @@ +/* psi.c + * + * Psi (digamma) function + * + * + * SYNOPSIS: + * + * double x, y, psi(); + * + * y = psi( x ); + * + * + * DESCRIPTION: + * + * d - + * psi(x) = -- ln | (x) + * dx + * + * is the logarithmic derivative of the gamma function. + * For integer x, + * n-1 + * - + * psi(n) = -EUL + > 1/k. + * - + * k=1 + * + * This formula is used for 0 < n <= 10. If x is negative, it + * is transformed to a positive argument by the reflection + * formula psi(1-x) = psi(x) + pi cot(pi x). + * For general positive x, the argument is made greater than 10 + * using the recurrence psi(x+1) = psi(x) + 1/x. + * Then the following asymptotic expansion is applied: + * + * inf. B + * - 2k + * psi(x) = log(x) - 1/2x - > ------- + * - 2k + * k=1 2k x + * + * where the B2k are Bernoulli numbers. + * + * ACCURACY: + * Relative error (except absolute when |psi| < 1): + * arithmetic domain # trials peak rms + * IEEE 0,30 30000 1.3e-15 1.4e-16 + * IEEE -30,0 40000 1.5e-15 2.2e-16 + * + * ERROR MESSAGES: + * message condition value returned + * psi singularity x integer <=0 INFINITY + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1992, 2000 by Stephen L. Moshier + */ + +/* + * Code for the rational approximation on [1, 2] is: + * + * (C) Copyright John Maddock 2006. + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. (See accompanying file + * LICENSE_1_0.txt or copy at https://www.boost.org/LICENSE_1_0.txt) + */ + +#include "mconf.h" + +static double A[] = { + 8.33333333333333333333E-2, + -2.10927960927960927961E-2, + 7.57575757575757575758E-3, + -4.16666666666666666667E-3, + 3.96825396825396825397E-3, + -8.33333333333333333333E-3, + 8.33333333333333333333E-2 +}; + + +static double digamma_imp_1_2(double x) +{ + /* + * Rational approximation on [1, 2] taken from Boost. + * + * Now for the approximation, we use the form: + * + * digamma(x) = (x - root) * (Y + R(x-1)) + * + * Where root is the location of the positive root of digamma, + * Y is a constant, and R is optimised for low absolute error + * compared to Y. + * + * Maximum Deviation Found: 1.466e-18 + * At double precision, max error found: 2.452e-17 + */ + double r, g; + + static const float Y = 0.99558162689208984f; + + static const double root1 = 1569415565.0 / 1073741824.0; + static const double root2 = (381566830.0 / 1073741824.0) / 1073741824.0; + static const double root3 = 0.9016312093258695918615325266959189453125e-19; + + static double P[] = { + -0.0020713321167745952, + -0.045251321448739056, + -0.28919126444774784, + -0.65031853770896507, + -0.32555031186804491, + 0.25479851061131551 + }; + static double Q[] = { + -0.55789841321675513e-6, + 0.0021284987017821144, + 0.054151797245674225, + 0.43593529692665969, + 1.4606242909763515, + 2.0767117023730469, + 1.0 + }; + g = x - root1; + g -= root2; + g -= root3; + r = polevl(x - 1.0, P, 5) / polevl(x - 1.0, Q, 6); + + return g * Y + g * r; +} + + +static double psi_asy(double x) +{ + double y, z; + + if (x < 1.0e17) { + z = 1.0 / (x * x); + y = z * polevl(z, A, 6); + } + else { + y = 0.0; + } + + return log(x) - (0.5 / x) - y; +} + + +double psi(double x) +{ + double y = 0.0; + double q, r; + int i, n; + + if (isnan(x)) { + return x; + } + else if (x == INFINITY) { + return x; + } + else if (x == -INFINITY) { + return NAN; + } + else if (x == 0) { + sf_error("psi", SF_ERROR_SINGULAR, NULL); + return copysign(INFINITY, -x); + } + else if (x < 0.0) { + /* argument reduction before evaluating tan(pi * x) */ + r = modf(x, &q); + if (r == 0.0) { + sf_error("psi", SF_ERROR_SINGULAR, NULL); + return NAN; + } + y = -M_PI / tan(M_PI * r); + x = 1.0 - x; + } + + /* check for positive integer up to 10 */ + if ((x <= 10.0) && (x == floor(x))) { + n = (int)x; + for (i = 1; i < n; i++) { + y += 1.0 / i; + } + y -= SCIPY_EULER; + return y; + } + + /* use the recurrence relation to move x into [1, 2] */ + if (x < 1.0) { + y -= 1.0 / x; + x += 1.0; + } + else if (x < 10.0) { + while (x > 2.0) { + x -= 1.0; + y += 1.0 / x; + } + } + if ((1.0 <= x) && (x <= 2.0)) { + y += digamma_imp_1_2(x); + return y; + } + + /* x is large, use the asymptotic series */ + y += psi_asy(x); + return y; +} diff --git a/gtsam/3rdparty/cephes/cephes/rgamma.c b/gtsam/3rdparty/cephes/cephes/rgamma.c new file mode 100644 index 000000000..6420ccaa9 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/rgamma.c @@ -0,0 +1,128 @@ +/* rgamma.c + * + * Reciprocal Gamma function + * + * + * + * SYNOPSIS: + * + * double x, y, rgamma(); + * + * y = rgamma( x ); + * + * + * + * DESCRIPTION: + * + * Returns one divided by the Gamma function of the argument. + * + * The function is approximated by a Chebyshev expansion in + * the interval [0,1]. Range reduction is by recurrence + * for arguments between -34.034 and +34.84425627277176174. + * 0 is returned for positive arguments outside this + * range. For arguments less than -34.034 the cosecant + * reflection formula is applied; lograrithms are employed + * to avoid unnecessary overflow. + * + * The reciprocal Gamma function has no singularities, + * but overflow and underflow may occur for large arguments. + * These conditions return either INFINITY or 0 with + * appropriate sign. + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -30,+30 30000 1.1e-15 2.0e-16 + * For arguments less than -34.034 the peak error is on the + * order of 5e-15 (DEC), excepting overflow or underflow. + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1985, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +/* Chebyshev coefficients for reciprocal Gamma function + * in interval 0 to 1. Function is 1/(x Gamma(x)) - 1 + */ + +static double R[] = { + 3.13173458231230000000E-17, + -6.70718606477908000000E-16, + 2.20039078172259550000E-15, + 2.47691630348254132600E-13, + -6.60074100411295197440E-12, + 5.13850186324226978840E-11, + 1.08965386454418662084E-9, + -3.33964630686836942556E-8, + 2.68975996440595483619E-7, + 2.96001177518801696639E-6, + -8.04814124978471142852E-5, + 4.16609138709688864714E-4, + 5.06579864028608725080E-3, + -6.41925436109158228810E-2, + -4.98558728684003594785E-3, + 1.27546015610523951063E-1 +}; + +static char name[] = "rgamma"; + +extern double MAXLOG; + + +double rgamma(double x) +{ + double w, y, z; + int sign; + + if (x > 34.84425627277176174) { + return exp(-lgam(x)); + } + if (x < -34.034) { + w = -x; + z = sinpi(w); + if (z == 0.0) { + return 0.0; + } + if (z < 0.0) { + sign = 1; + z = -z; + } + else { + sign = -1; + } + + y = log(w * z) - log(M_PI) + lgam(w); + if (y < -MAXLOG) { + sf_error(name, SF_ERROR_UNDERFLOW, NULL); + return (sign * 0.0); + } + if (y > MAXLOG) { + sf_error(name, SF_ERROR_OVERFLOW, NULL); + return (sign * INFINITY); + } + return (sign * exp(y)); + } + z = 1.0; + w = x; + + while (w > 1.0) { /* Downward recurrence */ + w -= 1.0; + z *= w; + } + while (w < 0.0) { /* Upward recurrence */ + z /= w; + w += 1.0; + } + if (w == 0.0) /* Nonpositive integer */ + return (0.0); + if (w == 1.0) /* Other integer */ + return (1.0 / z); + + y = w * (1.0 + chbevl(4.0 * w - 2.0, R, 16)) / z; + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/round.c b/gtsam/3rdparty/cephes/cephes/round.c new file mode 100644 index 000000000..0ed1f1415 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/round.c @@ -0,0 +1,63 @@ +/* round.c + * + * Round double to nearest or even integer valued double + * + * + * + * SYNOPSIS: + * + * double x, y, round(); + * + * y = round(x); + * + * + * + * DESCRIPTION: + * + * Returns the nearest integer to x as a double precision + * floating point result. If x ends in 0.5 exactly, the + * nearest even integer is chosen. + * + * + * + * ACCURACY: + * + * If x is greater than 1/(2*MACHEP), its closest machine + * representation is already an integer, so rounding does + * not change it. + */ + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +double round(double x) +{ + double y, r; + + /* Largest integer <= x */ + y = floor(x); + + /* Fractional part */ + r = x - y; + + /* Round up to nearest. */ + if (r > 0.5) + goto rndup; + + /* Round to even */ + if (r == 0.5) { + r = y - 2.0 * floor(0.5 * y); + if (r == 1.0) { + rndup: + y += 1.0; + } + } + + /* Else round down. */ + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/scipy_iv.c b/gtsam/3rdparty/cephes/cephes/scipy_iv.c new file mode 100644 index 000000000..e7bb22011 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/scipy_iv.c @@ -0,0 +1,654 @@ +/* iv.c + * + * Modified Bessel function of noninteger order + * + * + * + * SYNOPSIS: + * + * double v, x, y, iv(); + * + * y = iv( v, x ); + * + * + * + * DESCRIPTION: + * + * Returns modified Bessel function of order v of the + * argument. If x is negative, v must be integer valued. + * + */ +/* iv.c */ +/* Modified Bessel function of noninteger order */ +/* If x < 0, then v must be an integer. */ + + +/* + * Parts of the code are copyright: + * + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 1988, 2000 by Stephen L. Moshier + * + * And other parts: + * + * Copyright (c) 2006 Xiaogang Zhang + * Use, modification and distribution are subject to the + * Boost Software License, Version 1.0. + * + * Boost Software License - Version 1.0 - August 17th, 2003 + * + * Permission is hereby granted, free of charge, to any person or + * organization obtaining a copy of the software and accompanying + * documentation covered by this license (the "Software") to use, reproduce, + * display, distribute, execute, and transmit the Software, and to prepare + * derivative works of the Software, and to permit third-parties to whom the + * Software is furnished to do so, all subject to the following: + * + * The copyright notices in the Software and this entire statement, + * including the above license grant, this restriction and the following + * disclaimer, must be included in all copies of the Software, in whole or + * in part, and all derivative works of the Software, unless such copies or + * derivative works are solely in the form of machine-executable object code + * generated by a source language processor. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS + * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF + * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE, TITLE AND + * NON-INFRINGEMENT. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR ANYONE + * DISTRIBUTING THE SOFTWARE BE LIABLE FOR ANY DAMAGES OR OTHER LIABILITY, + * WHETHER IN CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + * + * And the rest are: + * + * Copyright (C) 2009 Pauli Virtanen + * Distributed under the same license as Scipy. + * + */ + +#include "mconf.h" +#include +#include + +extern double MACHEP; + +static double iv_asymptotic(double v, double x); +static void ikv_asymptotic_uniform(double v, double x, double *Iv, double *Kv); +static void ikv_temme(double v, double x, double *Iv, double *Kv); + +double iv(double v, double x) +{ + int sign; + double t, ax, res; + + if (isnan(v) || isnan(x)) { + return NAN; + } + + /* If v is a negative integer, invoke symmetry */ + t = floor(v); + if (v < 0.0) { + if (t == v) { + v = -v; /* symmetry */ + t = -t; + } + } + /* If x is negative, require v to be an integer */ + sign = 1; + if (x < 0.0) { + if (t != v) { + sf_error("iv", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + if (v != 2.0 * floor(v / 2.0)) { + sign = -1; + } + } + + /* Avoid logarithm singularity */ + if (x == 0.0) { + if (v == 0.0) { + return 1.0; + } + if (v < 0.0) { + sf_error("iv", SF_ERROR_OVERFLOW, NULL); + return INFINITY; + } + else + return 0.0; + } + + ax = fabs(x); + if (fabs(v) > 50) { + /* + * Uniform asymptotic expansion for large orders. + * + * This appears to overflow slightly later than the Boost + * implementation of Temme's method. + */ + ikv_asymptotic_uniform(v, ax, &res, NULL); + } + else { + /* Otherwise: Temme's method */ + ikv_temme(v, ax, &res, NULL); + } + res *= sign; + return res; +} + + +/* + * Compute Iv from (AMS5 9.7.1), asymptotic expansion for large |z| + * Iv ~ exp(x)/sqrt(2 pi x) ( 1 + (4*v*v-1)/8x + (4*v*v-1)(4*v*v-9)/8x/2! + ...) + */ +static double iv_asymptotic(double v, double x) +{ + double mu; + double sum, term, prefactor, factor; + int k; + + prefactor = exp(x) / sqrt(2 * M_PI * x); + + if (prefactor == INFINITY) { + return prefactor; + } + + mu = 4 * v * v; + sum = 1.0; + term = 1.0; + k = 1; + + do { + factor = (mu - (2 * k - 1) * (2 * k - 1)) / (8 * x) / k; + if (k > 100) { + /* didn't converge */ + sf_error("iv(iv_asymptotic)", SF_ERROR_NO_RESULT, NULL); + break; + } + term *= -factor; + sum += term; + ++k; + } while (fabs(term) > MACHEP * fabs(sum)); + return sum * prefactor; +} + + +/* + * Uniform asymptotic expansion factors, (AMS5 9.3.9; AMS5 9.3.10) + * + * Computed with: + * -------------------- + import numpy as np + t = np.poly1d([1,0]) + def up1(p): + return .5*t*t*(1-t*t)*p.deriv() + 1/8. * ((1-5*t*t)*p).integ() + us = [np.poly1d([1])] + for k in range(10): + us.append(up1(us[-1])) + n = us[-1].order + for p in us: + print "{" + ", ".join(["0"]*(n-p.order) + map(repr, p)) + "}," + print "N_UFACTORS", len(us) + print "N_UFACTOR_TERMS", us[-1].order + 1 + * -------------------- + */ +#define N_UFACTORS 11 +#define N_UFACTOR_TERMS 31 +static const double asymptotic_ufactors[N_UFACTORS][N_UFACTOR_TERMS] = { + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, 0, 0, 0, 1}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0, 0, 0, -0.20833333333333334, 0.0, 0.125, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 0, 0.3342013888888889, 0.0, -0.40104166666666669, 0.0, 0.0703125, 0.0, + 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + -1.0258125964506173, 0.0, 1.8464626736111112, 0.0, + -0.89121093750000002, 0.0, 0.0732421875, 0.0, 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, + 4.6695844234262474, 0.0, -11.207002616222995, 0.0, 8.78912353515625, + 0.0, -2.3640869140624998, 0.0, 0.112152099609375, 0.0, 0.0, 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, -28.212072558200244, 0.0, + 84.636217674600744, 0.0, -91.818241543240035, 0.0, 42.534998745388457, + 0.0, -7.3687943594796312, 0.0, 0.22710800170898438, 0.0, 0.0, 0.0, + 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 212.5701300392171, 0.0, + -765.25246814118157, 0.0, 1059.9904525279999, 0.0, + -699.57962737613275, 0.0, 218.19051174421159, 0.0, + -26.491430486951554, 0.0, 0.57250142097473145, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 0, 0, 0, -1919.4576623184068, 0.0, + 8061.7221817373083, 0.0, -13586.550006434136, 0.0, 11655.393336864536, + 0.0, -5305.6469786134048, 0.0, 1200.9029132163525, 0.0, + -108.09091978839464, 0.0, 1.7277275025844574, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0}, + {0, 0, 0, 0, 0, 0, 20204.291330966149, 0.0, -96980.598388637503, 0.0, + 192547.0012325315, 0.0, -203400.17728041555, 0.0, 122200.46498301747, + 0.0, -41192.654968897557, 0.0, 7109.5143024893641, 0.0, + -493.915304773088, 0.0, 6.074042001273483, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0}, + {0, 0, 0, -242919.18790055133, 0.0, 1311763.6146629769, 0.0, + -2998015.9185381061, 0.0, 3763271.2976564039, 0.0, + -2813563.2265865342, 0.0, 1268365.2733216248, 0.0, + -331645.17248456361, 0.0, 45218.768981362737, 0.0, + -2499.8304818112092, 0.0, 24.380529699556064, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0}, + {3284469.8530720375, 0.0, -19706819.11843222, 0.0, 50952602.492664628, + 0.0, -74105148.211532637, 0.0, 66344512.274729028, 0.0, + -37567176.660763353, 0.0, 13288767.166421819, 0.0, + -2785618.1280864552, 0.0, 308186.40461266245, 0.0, + -13886.089753717039, 0.0, 110.01714026924674, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0} +}; + + +/* + * Compute Iv, Kv from (AMS5 9.7.7 + 9.7.8), asymptotic expansion for large v + */ +static void ikv_asymptotic_uniform(double v, double x, + double *i_value, double *k_value) +{ + double i_prefactor, k_prefactor; + double t, t2, eta, z; + double i_sum, k_sum, term, divisor; + int k, n; + int sign = 1; + + if (v < 0) { + /* Negative v; compute I_{-v} and K_{-v} and use (AMS 9.6.2) */ + sign = -1; + v = -v; + } + + z = x / v; + t = 1 / sqrt(1 + z * z); + t2 = t * t; + eta = sqrt(1 + z * z) + log(z / (1 + 1 / t)); + + i_prefactor = sqrt(t / (2 * M_PI * v)) * exp(v * eta); + i_sum = 1.0; + + k_prefactor = sqrt(M_PI * t / (2 * v)) * exp(-v * eta); + k_sum = 1.0; + + divisor = v; + for (n = 1; n < N_UFACTORS; ++n) { + /* + * Evaluate u_k(t) with Horner's scheme; + * (using the knowledge about which coefficients are zero) + */ + term = 0; + for (k = N_UFACTOR_TERMS - 1 - 3 * n; + k < N_UFACTOR_TERMS - n; k += 2) { + term *= t2; + term += asymptotic_ufactors[n][k]; + } + for (k = 1; k < n; k += 2) { + term *= t2; + } + if (n % 2 == 1) { + term *= t; + } + + /* Sum terms */ + term /= divisor; + i_sum += term; + k_sum += (n % 2 == 0) ? term : -term; + + /* Check convergence */ + if (fabs(term) < MACHEP) { + break; + } + + divisor *= v; + } + + if (fabs(term) > 1e-3 * fabs(i_sum)) { + /* Didn't converge */ + sf_error("ikv_asymptotic_uniform", SF_ERROR_NO_RESULT, NULL); + } + if (fabs(term) > MACHEP * fabs(i_sum)) { + /* Some precision lost */ + sf_error("ikv_asymptotic_uniform", SF_ERROR_LOSS, NULL); + } + + if (k_value != NULL) { + /* symmetric in v */ + *k_value = k_prefactor * k_sum; + } + + if (i_value != NULL) { + if (sign == 1) { + *i_value = i_prefactor * i_sum; + } + else { + /* (AMS 9.6.2) */ + *i_value = (i_prefactor * i_sum + + (2 / M_PI) * sin(M_PI * v) * k_prefactor * k_sum); + } + } +} + + +/* + * The following code originates from the Boost C++ library, + * from file `boost/math/special_functions/detail/bessel_ik.hpp`, + * converted from C++ to C. + */ + +#ifdef DEBUG +#define BOOST_ASSERT(a) assert(a) +#else +#define BOOST_ASSERT(a) +#endif + +/* + * Modified Bessel functions of the first and second kind of fractional order + * + * Calculate K(v, x) and K(v+1, x) by method analogous to + * Temme, Journal of Computational Physics, vol 21, 343 (1976) + */ +static int temme_ik_series(double v, double x, double *K, double *K1) +{ + double f, h, p, q, coef, sum, sum1, tolerance; + double a, b, c, d, sigma, gamma1, gamma2; + unsigned long k; + double gp; + double gm; + + + /* + * |x| <= 2, Temme series converge rapidly + * |x| > 2, the larger the |x|, the slower the convergence + */ + BOOST_ASSERT(fabs(x) <= 2); + BOOST_ASSERT(fabs(v) <= 0.5f); + + gp = gamma(v + 1) - 1; + gm = gamma(-v + 1) - 1; + + a = log(x / 2); + b = exp(v * a); + sigma = -a * v; + c = fabs(v) < MACHEP ? 1 : sin(M_PI * v) / (v * M_PI); + d = fabs(sigma) < MACHEP ? 1 : sinh(sigma) / sigma; + gamma1 = fabs(v) < MACHEP ? -SCIPY_EULER : (0.5f / v) * (gp - gm) * c; + gamma2 = (2 + gp + gm) * c / 2; + + /* initial values */ + p = (gp + 1) / (2 * b); + q = (1 + gm) * b / 2; + f = (cosh(sigma) * gamma1 + d * (-a) * gamma2) / c; + h = p; + coef = 1; + sum = coef * f; + sum1 = coef * h; + + /* series summation */ + tolerance = MACHEP; + for (k = 1; k < MAXITER; k++) { + f = (k * f + p + q) / (k * k - v * v); + p /= k - v; + q /= k + v; + h = p - k * f; + coef *= x * x / (4 * k); + sum += coef * f; + sum1 += coef * h; + if (fabs(coef * f) < fabs(sum) * tolerance) { + break; + } + } + if (k == MAXITER) { + sf_error("ikv_temme(temme_ik_series)", SF_ERROR_NO_RESULT, NULL); + } + + *K = sum; + *K1 = 2 * sum1 / x; + + return 0; +} + +/* Evaluate continued fraction fv = I_(v+1) / I_v, derived from + * Abramowitz and Stegun, Handbook of Mathematical Functions, 1972, 9.1.73 */ +static int CF1_ik(double v, double x, double *fv) +{ + double C, D, f, a, b, delta, tiny, tolerance; + unsigned long k; + + + /* + * |x| <= |v|, CF1_ik converges rapidly + * |x| > |v|, CF1_ik needs O(|x|) iterations to converge + */ + + /* + * modified Lentz's method, see + * Lentz, Applied Optics, vol 15, 668 (1976) + */ + tolerance = 2 * MACHEP; + tiny = 1 / sqrt(DBL_MAX); + C = f = tiny; /* b0 = 0, replace with tiny */ + D = 0; + for (k = 1; k < MAXITER; k++) { + a = 1; + b = 2 * (v + k) / x; + C = b + a / C; + D = b + a * D; + if (C == 0) { + C = tiny; + } + if (D == 0) { + D = tiny; + } + D = 1 / D; + delta = C * D; + f *= delta; + if (fabs(delta - 1) <= tolerance) { + break; + } + } + if (k == MAXITER) { + sf_error("ikv_temme(CF1_ik)", SF_ERROR_NO_RESULT, NULL); + } + + *fv = f; + + return 0; +} + +/* + * Calculate K(v, x) and K(v+1, x) by evaluating continued fraction + * z1 / z0 = U(v+1.5, 2v+1, 2x) / U(v+0.5, 2v+1, 2x), see + * Thompson and Barnett, Computer Physics Communications, vol 47, 245 (1987) + */ +static int CF2_ik(double v, double x, double *Kv, double *Kv1) +{ + + double S, C, Q, D, f, a, b, q, delta, tolerance, current, prev; + unsigned long k; + + /* + * |x| >= |v|, CF2_ik converges rapidly + * |x| -> 0, CF2_ik fails to converge + */ + + BOOST_ASSERT(fabs(x) > 1); + + /* + * Steed's algorithm, see Thompson and Barnett, + * Journal of Computational Physics, vol 64, 490 (1986) + */ + tolerance = MACHEP; + a = v * v - 0.25f; + b = 2 * (x + 1); /* b1 */ + D = 1 / b; /* D1 = 1 / b1 */ + f = delta = D; /* f1 = delta1 = D1, coincidence */ + prev = 0; /* q0 */ + current = 1; /* q1 */ + Q = C = -a; /* Q1 = C1 because q1 = 1 */ + S = 1 + Q * delta; /* S1 */ + for (k = 2; k < MAXITER; k++) { /* starting from 2 */ + /* continued fraction f = z1 / z0 */ + a -= 2 * (k - 1); + b += 2; + D = 1 / (b + a * D); + delta *= b * D - 1; + f += delta; + + /* series summation S = 1 + \sum_{n=1}^{\infty} C_n * z_n / z_0 */ + q = (prev - (b - 2) * current) / a; + prev = current; + current = q; /* forward recurrence for q */ + C *= -a / k; + Q += C * q; + S += Q * delta; + + /* S converges slower than f */ + if (fabs(Q * delta) < fabs(S) * tolerance) { + break; + } + } + if (k == MAXITER) { + sf_error("ikv_temme(CF2_ik)", SF_ERROR_NO_RESULT, NULL); + } + + *Kv = sqrt(M_PI / (2 * x)) * exp(-x) / S; + *Kv1 = *Kv * (0.5f + v + x + (v * v - 0.25f) * f) / x; + + return 0; +} + +/* Flags for what to compute */ +enum { + need_i = 0x1, + need_k = 0x2 +}; + +/* + * Compute I(v, x) and K(v, x) simultaneously by Temme's method, see + * Temme, Journal of Computational Physics, vol 19, 324 (1975) + */ +static void ikv_temme(double v, double x, double *Iv_p, double *Kv_p) +{ + /* Kv1 = K_(v+1), fv = I_(v+1) / I_v */ + /* Ku1 = K_(u+1), fu = I_(u+1) / I_u */ + double u, Iv, Kv, Kv1, Ku, Ku1, fv; + double W, current, prev, next; + int reflect = 0; + unsigned n, k; + int kind; + + kind = 0; + if (Iv_p != NULL) { + kind |= need_i; + } + if (Kv_p != NULL) { + kind |= need_k; + } + + if (v < 0) { + reflect = 1; + v = -v; /* v is non-negative from here */ + kind |= need_k; + } + n = round(v); + u = v - n; /* -1/2 <= u < 1/2 */ + + if (x < 0) { + if (Iv_p != NULL) + *Iv_p = NAN; + if (Kv_p != NULL) + *Kv_p = NAN; + sf_error("ikv_temme", SF_ERROR_DOMAIN, NULL); + return; + } + if (x == 0) { + Iv = (v == 0) ? 1 : 0; + if (kind & need_k) { + sf_error("ikv_temme", SF_ERROR_OVERFLOW, NULL); + Kv = INFINITY; + } + else { + Kv = NAN; /* any value will do */ + } + + if (reflect && (kind & need_i)) { + double z = (u + n % 2); + + Iv = sin((double)M_PI * z) == 0 ? Iv : INFINITY; + if (Iv == INFINITY || Iv == -INFINITY) { + sf_error("ikv_temme", SF_ERROR_OVERFLOW, NULL); + } + } + + if (Iv_p != NULL) { + *Iv_p = Iv; + } + if (Kv_p != NULL) { + *Kv_p = Kv; + } + return; + } + /* x is positive until reflection */ + W = 1 / x; /* Wronskian */ + if (x <= 2) { /* x in (0, 2] */ + temme_ik_series(u, x, &Ku, &Ku1); /* Temme series */ + } + else { /* x in (2, \infty) */ + CF2_ik(u, x, &Ku, &Ku1); /* continued fraction CF2_ik */ + } + prev = Ku; + current = Ku1; + for (k = 1; k <= n; k++) { /* forward recurrence for K */ + next = 2 * (u + k) * current / x + prev; + prev = current; + current = next; + } + Kv = prev; + Kv1 = current; + if (kind & need_i) { + double lim = (4 * v * v + 10) / (8 * x); + + lim *= lim; + lim *= lim; + lim /= 24; + if ((lim < MACHEP * 10) && (x > 100)) { + /* + * x is huge compared to v, CF1 may be very slow + * to converge so use asymptotic expansion for large + * x case instead. Note that the asymptotic expansion + * isn't very accurate - so it's deliberately very hard + * to get here - probably we're going to overflow: + */ + Iv = iv_asymptotic(v, x); + } + else { + CF1_ik(v, x, &fv); /* continued fraction CF1_ik */ + Iv = W / (Kv * fv + Kv1); /* Wronskian relation */ + } + } + else { + Iv = NAN; /* any value will do */ + } + + if (reflect) { + double z = (u + n % 2); + + if (Iv_p != NULL) { + *Iv_p = Iv + (2 / M_PI) * sin(M_PI * z) * Kv; /* reflection formula */ + } + if (Kv_p != NULL) { + *Kv_p = Kv; + } + } + else { + if (Iv_p != NULL) { + *Iv_p = Iv; + } + if (Kv_p != NULL) { + *Kv_p = Kv; + } + } + return; +} diff --git a/gtsam/3rdparty/cephes/cephes/sf_error.c b/gtsam/3rdparty/cephes/cephes/sf_error.c new file mode 100644 index 000000000..95a47c797 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sf_error.c @@ -0,0 +1,45 @@ +#include "sf_error.h" + +#include +#include + +const char *sf_error_messages[] = {"no error", + "singularity", + "underflow", + "overflow", + "too slow convergence", + "loss of precision", + "no result obtained", + "domain error", + "invalid input argument", + "other error", + NULL}; + +/* If this isn't volatile clang tries to optimize it away */ +static volatile sf_action_t sf_error_actions[] = { + SF_ERROR_IGNORE, /* SF_ERROR_OK */ + SF_ERROR_IGNORE, /* SF_ERROR_SINGULAR */ + SF_ERROR_IGNORE, /* SF_ERROR_UNDERFLOW */ + SF_ERROR_IGNORE, /* SF_ERROR_OVERFLOW */ + SF_ERROR_IGNORE, /* SF_ERROR_SLOW */ + SF_ERROR_IGNORE, /* SF_ERROR_LOSS */ + SF_ERROR_IGNORE, /* SF_ERROR_NO_RESULT */ + SF_ERROR_IGNORE, /* SF_ERROR_DOMAIN */ + SF_ERROR_IGNORE, /* SF_ERROR_ARG */ + SF_ERROR_IGNORE, /* SF_ERROR_OTHER */ + SF_ERROR_IGNORE /* SF_ERROR__LAST */ +}; + +void sf_error_set_action(sf_error_t code, sf_action_t action) { + sf_error_actions[(int)code] = action; +} + +sf_action_t sf_error_get_action(sf_error_t code) { + return sf_error_actions[(int)code]; +} + +void sf_error(const char *func_name, sf_error_t code, const char *fmt, ...) { + va_list ap; + va_start(ap, fmt); + va_end(ap); +} diff --git a/gtsam/3rdparty/cephes/cephes/sf_error.h b/gtsam/3rdparty/cephes/cephes/sf_error.h new file mode 100644 index 000000000..43986df81 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sf_error.h @@ -0,0 +1,38 @@ +#ifndef SF_ERROR_H_ +#define SF_ERROR_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +typedef enum { + SF_ERROR_OK = 0, /* no error */ + SF_ERROR_SINGULAR, /* singularity encountered */ + SF_ERROR_UNDERFLOW, /* floating point underflow */ + SF_ERROR_OVERFLOW, /* floating point overflow */ + SF_ERROR_SLOW, /* too many iterations required */ + SF_ERROR_LOSS, /* loss of precision */ + SF_ERROR_NO_RESULT, /* no result obtained */ + SF_ERROR_DOMAIN, /* out of domain */ + SF_ERROR_ARG, /* invalid input parameter */ + SF_ERROR_OTHER, /* unclassified error */ + SF_ERROR__LAST +} sf_error_t; + +typedef enum { + SF_ERROR_IGNORE = 0, /* Ignore errors */ + SF_ERROR_WARN, /* Warn on errors */ + SF_ERROR_RAISE /* Raise on errors */ +} sf_action_t; + +extern const char *sf_error_messages[]; +void sf_error(const char *func_name, sf_error_t code, const char *fmt, ...); +void sf_error_check_fpe(const char *func_name); +void sf_error_set_action(sf_error_t code, sf_action_t action); +sf_action_t sf_error_get_action(sf_error_t code); + +#ifdef __cplusplus +} +#endif + +#endif /* SF_ERROR_H_ */ diff --git a/gtsam/3rdparty/cephes/cephes/shichi.c b/gtsam/3rdparty/cephes/cephes/shichi.c new file mode 100644 index 000000000..75104e724 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/shichi.c @@ -0,0 +1,305 @@ +/* shichi.c + * + * Hyperbolic sine and cosine integrals + * + * + * + * SYNOPSIS: + * + * double x, Chi, Shi, shichi(); + * + * shichi( x, &Chi, &Shi ); + * + * + * DESCRIPTION: + * + * Approximates the integrals + * + * x + * - + * | | cosh t - 1 + * Chi(x) = eul + ln x + | ----------- dt, + * | | t + * - + * 0 + * + * x + * - + * | | sinh t + * Shi(x) = | ------ dt + * | | t + * - + * 0 + * + * where eul = 0.57721566490153286061 is Euler's constant. + * The integrals are evaluated by power series for x < 8 + * and by Chebyshev expansions for x between 8 and 88. + * For large x, both functions approach exp(x)/2x. + * Arguments greater than 88 in magnitude return INFINITY. + * + * + * ACCURACY: + * + * Test interval 0 to 88. + * Relative error: + * arithmetic function # trials peak rms + * IEEE Shi 30000 6.9e-16 1.6e-16 + * Absolute error, except relative when |Chi| > 1: + * IEEE Chi 30000 8.4e-16 1.4e-16 + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + + +#include "mconf.h" + +/* x exp(-x) shi(x), inverted interval 8 to 18 */ +static double S1[] = { + 1.83889230173399459482E-17, + -9.55485532279655569575E-17, + 2.04326105980879882648E-16, + 1.09896949074905343022E-15, + -1.31313534344092599234E-14, + 5.93976226264314278932E-14, + -3.47197010497749154755E-14, + -1.40059764613117131000E-12, + 9.49044626224223543299E-12, + -1.61596181145435454033E-11, + -1.77899784436430310321E-10, + 1.35455469767246947469E-9, + -1.03257121792819495123E-9, + -3.56699611114982536845E-8, + 1.44818877384267342057E-7, + 7.82018215184051295296E-7, + -5.39919118403805073710E-6, + -3.12458202168959833422E-5, + 8.90136741950727517826E-5, + 2.02558474743846862168E-3, + 2.96064440855633256972E-2, + 1.11847751047257036625E0 +}; + +/* x exp(-x) shi(x), inverted interval 18 to 88 */ +static double S2[] = { + -1.05311574154850938805E-17, + 2.62446095596355225821E-17, + 8.82090135625368160657E-17, + -3.38459811878103047136E-16, + -8.30608026366935789136E-16, + 3.93397875437050071776E-15, + 1.01765565969729044505E-14, + -4.21128170307640802703E-14, + -1.60818204519802480035E-13, + 3.34714954175994481761E-13, + 2.72600352129153073807E-12, + 1.66894954752839083608E-12, + -3.49278141024730899554E-11, + -1.58580661666482709598E-10, + -1.79289437183355633342E-10, + 1.76281629144264523277E-9, + 1.69050228879421288846E-8, + 1.25391771228487041649E-7, + 1.16229947068677338732E-6, + 1.61038260117376323993E-5, + 3.49810375601053973070E-4, + 1.28478065259647610779E-2, + 1.03665722588798326712E0 +}; + +/* x exp(-x) chin(x), inverted interval 8 to 18 */ +static double C1[] = { + -8.12435385225864036372E-18, + 2.17586413290339214377E-17, + 5.22624394924072204667E-17, + -9.48812110591690559363E-16, + 5.35546311647465209166E-15, + -1.21009970113732918701E-14, + -6.00865178553447437951E-14, + 7.16339649156028587775E-13, + -2.93496072607599856104E-12, + -1.40359438136491256904E-12, + 8.76302288609054966081E-11, + -4.40092476213282340617E-10, + -1.87992075640569295479E-10, + 1.31458150989474594064E-8, + -4.75513930924765465590E-8, + -2.21775018801848880741E-7, + 1.94635531373272490962E-6, + 4.33505889257316408893E-6, + -6.13387001076494349496E-5, + -3.13085477492997465138E-4, + 4.97164789823116062801E-4, + 2.64347496031374526641E-2, + 1.11446150876699213025E0 +}; + +/* x exp(-x) chin(x), inverted interval 18 to 88 */ +static double C2[] = { + 8.06913408255155572081E-18, + -2.08074168180148170312E-17, + -5.98111329658272336816E-17, + 2.68533951085945765591E-16, + 4.52313941698904694774E-16, + -3.10734917335299464535E-15, + -4.42823207332531972288E-15, + 3.49639695410806959872E-14, + 6.63406731718911586609E-14, + -3.71902448093119218395E-13, + -1.27135418132338309016E-12, + 2.74851141935315395333E-12, + 2.33781843985453438400E-11, + 2.71436006377612442764E-11, + -2.56600180000355990529E-10, + -1.61021375163803438552E-9, + -4.72543064876271773512E-9, + -3.00095178028681682282E-9, + 7.79387474390914922337E-8, + 1.06942765566401507066E-6, + 1.59503164802313196374E-5, + 3.49592575153777996871E-4, + 1.28475387530065247392E-2, + 1.03665693917934275131E0 +}; + +static double hyp3f0(double a1, double a2, double a3, double z); + +/* Sine and cosine integrals */ + +extern double MACHEP; + +int shichi(double x, double *si, double *ci) +{ + double k, z, c, s, a, b; + short sign; + + if (x < 0.0) { + sign = -1; + x = -x; + } + else + sign = 0; + + + if (x == 0.0) { + *si = 0.0; + *ci = -INFINITY; + return (0); + } + + if (x >= 8.0) + goto chb; + + if (x >= 88.0) + goto asymp; + + z = x * x; + + /* Direct power series expansion */ + a = 1.0; + s = 1.0; + c = 0.0; + k = 2.0; + + do { + a *= z / k; + c += a / k; + k += 1.0; + a /= k; + s += a / k; + k += 1.0; + } + while (fabs(a / s) > MACHEP); + + s *= x; + goto done; + + +chb: + /* Chebyshev series expansions */ + if (x < 18.0) { + a = (576.0 / x - 52.0) / 10.0; + k = exp(x) / x; + s = k * chbevl(a, S1, 22); + c = k * chbevl(a, C1, 23); + goto done; + } + + if (x <= 88.0) { + a = (6336.0 / x - 212.0) / 70.0; + k = exp(x) / x; + s = k * chbevl(a, S2, 23); + c = k * chbevl(a, C2, 24); + goto done; + } + +asymp: + if (x > 1000) { + *si = INFINITY; + *ci = INFINITY; + } + else { + /* Asymptotic expansions + * http://functions.wolfram.com/GammaBetaErf/CoshIntegral/06/02/ + * http://functions.wolfram.com/GammaBetaErf/SinhIntegral/06/02/0001/ + */ + a = hyp3f0(0.5, 1, 1, 4.0/(x*x)); + b = hyp3f0(1, 1, 1.5, 4.0/(x*x)); + *si = cosh(x)/x * a + sinh(x)/(x*x) * b; + *ci = sinh(x)/x * a + cosh(x)/(x*x) * b; + } + if (sign) { + *si = -*si; + } + return 0; + +done: + if (sign) + s = -s; + + *si = s; + + *ci = SCIPY_EULER + log(x) + c; + return (0); +} + + +/* + * Evaluate 3F0(a1, a2, a3; z) + * + * The series is only asymptotic, so this requires z large enough. + */ +static double hyp3f0(double a1, double a2, double a3, double z) +{ + int n, maxiter; + double err, sum, term, m; + + m = pow(z, -1.0/3); + if (m < 50) { + maxiter = m; + } + else { + maxiter = 50; + } + + term = 1.0; + sum = term; + for (n = 0; n < maxiter; ++n) { + term *= (a1 + n) * (a2 + n) * (a3 + n) * z / (n + 1); + sum += term; + if (fabs(term) < 1e-13 * fabs(sum) || term == 0) { + break; + } + } + + err = fabs(term); + + if (err > 1e-13 * fabs(sum)) { + return NAN; + } + + return sum; +} diff --git a/gtsam/3rdparty/cephes/cephes/sici.c b/gtsam/3rdparty/cephes/cephes/sici.c new file mode 100644 index 000000000..7bb79bc25 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sici.c @@ -0,0 +1,276 @@ +/* sici.c + * + * Sine and cosine integrals + * + * + * + * SYNOPSIS: + * + * double x, Ci, Si, sici(); + * + * sici( x, &Si, &Ci ); + * + * + * DESCRIPTION: + * + * Evaluates the integrals + * + * x + * - + * | cos t - 1 + * Ci(x) = eul + ln x + | --------- dt, + * | t + * - + * 0 + * x + * - + * | sin t + * Si(x) = | ----- dt + * | t + * - + * 0 + * + * where eul = 0.57721566490153286061 is Euler's constant. + * The integrals are approximated by rational functions. + * For x > 8 auxiliary functions f(x) and g(x) are employed + * such that + * + * Ci(x) = f(x) sin(x) - g(x) cos(x) + * Si(x) = pi/2 - f(x) cos(x) - g(x) sin(x) + * + * + * ACCURACY: + * Test interval = [0,50]. + * Absolute error, except relative when > 1: + * arithmetic function # trials peak rms + * IEEE Si 30000 4.4e-16 7.3e-17 + * IEEE Ci 30000 6.9e-16 5.1e-17 + */ + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +static double SN[] = { + -8.39167827910303881427E-11, + 4.62591714427012837309E-8, + -9.75759303843632795789E-6, + 9.76945438170435310816E-4, + -4.13470316229406538752E-2, + 1.00000000000000000302E0, +}; + +static double SD[] = { + 2.03269266195951942049E-12, + 1.27997891179943299903E-9, + 4.41827842801218905784E-7, + 9.96412122043875552487E-5, + 1.42085239326149893930E-2, + 9.99999999999999996984E-1, +}; + +static double CN[] = { + 2.02524002389102268789E-11, + -1.35249504915790756375E-8, + 3.59325051419993077021E-6, + -4.74007206873407909465E-4, + 2.89159652607555242092E-2, + -1.00000000000000000080E0, +}; + +static double CD[] = { + 4.07746040061880559506E-12, + 3.06780997581887812692E-9, + 1.23210355685883423679E-6, + 3.17442024775032769882E-4, + 5.10028056236446052392E-2, + 4.00000000000000000080E0, +}; + +static double FN4[] = { + 4.23612862892216586994E0, + 5.45937717161812843388E0, + 1.62083287701538329132E0, + 1.67006611831323023771E-1, + 6.81020132472518137426E-3, + 1.08936580650328664411E-4, + 5.48900223421373614008E-7, +}; + +static double FD4[] = { + /* 1.00000000000000000000E0, */ + 8.16496634205391016773E0, + 7.30828822505564552187E0, + 1.86792257950184183883E0, + 1.78792052963149907262E-1, + 7.01710668322789753610E-3, + 1.10034357153915731354E-4, + 5.48900252756255700982E-7, +}; + +static double FN8[] = { + 4.55880873470465315206E-1, + 7.13715274100146711374E-1, + 1.60300158222319456320E-1, + 1.16064229408124407915E-2, + 3.49556442447859055605E-4, + 4.86215430826454749482E-6, + 3.20092790091004902806E-8, + 9.41779576128512936592E-11, + 9.70507110881952024631E-14, +}; + +static double FD8[] = { + /* 1.00000000000000000000E0, */ + 9.17463611873684053703E-1, + 1.78685545332074536321E-1, + 1.22253594771971293032E-2, + 3.58696481881851580297E-4, + 4.92435064317881464393E-6, + 3.21956939101046018377E-8, + 9.43720590350276732376E-11, + 9.70507110881952025725E-14, +}; + +static double GN4[] = { + 8.71001698973114191777E-2, + 6.11379109952219284151E-1, + 3.97180296392337498885E-1, + 7.48527737628469092119E-2, + 5.38868681462177273157E-3, + 1.61999794598934024525E-4, + 1.97963874140963632189E-6, + 7.82579040744090311069E-9, +}; + +static double GD4[] = { + /* 1.00000000000000000000E0, */ + 1.64402202413355338886E0, + 6.66296701268987968381E-1, + 9.88771761277688796203E-2, + 6.22396345441768420760E-3, + 1.73221081474177119497E-4, + 2.02659182086343991969E-6, + 7.82579218933534490868E-9, +}; + +static double GN8[] = { + 6.97359953443276214934E-1, + 3.30410979305632063225E-1, + 3.84878767649974295920E-2, + 1.71718239052347903558E-3, + 3.48941165502279436777E-5, + 3.47131167084116673800E-7, + 1.70404452782044526189E-9, + 3.85945925430276600453E-12, + 3.14040098946363334640E-15, +}; + +static double GD8[] = { + /* 1.00000000000000000000E0, */ + 1.68548898811011640017E0, + 4.87852258695304967486E-1, + 4.67913194259625806320E-2, + 1.90284426674399523638E-3, + 3.68475504442561108162E-5, + 3.57043223443740838771E-7, + 1.72693748966316146736E-9, + 3.87830166023954706752E-12, + 3.14040098946363335242E-15, +}; + +extern double MACHEP; + + +int sici(double x, double *si, double *ci) +{ + double z, c, s, f, g; + short sign; + + if (x < 0.0) { + sign = -1; + x = -x; + } + else + sign = 0; + + + if (x == 0.0) { + *si = 0.0; + *ci = -INFINITY; + return (0); + } + + + if (x > 1.0e9) { + if (cephes_isinf(x)) { + if (sign == -1) { + *si = -M_PI_2; + *ci = NAN; + } + else { + *si = M_PI_2; + *ci = 0; + } + return 0; + } + *si = M_PI_2 - cos(x) / x; + *ci = sin(x) / x; + } + + + + if (x > 4.0) + goto asympt; + + z = x * x; + s = x * polevl(z, SN, 5) / polevl(z, SD, 5); + c = z * polevl(z, CN, 5) / polevl(z, CD, 5); + + if (sign) + s = -s; + *si = s; + *ci = SCIPY_EULER + log(x) + c; /* real part if x < 0 */ + return (0); + + + + /* The auxiliary functions are: + * + * + * *si = *si - M_PI_2; + * c = cos(x); + * s = sin(x); + * + * t = *ci * s - *si * c; + * a = *ci * c + *si * s; + * + * *si = t; + * *ci = -a; + */ + + + asympt: + + s = sin(x); + c = cos(x); + z = 1.0 / (x * x); + if (x < 8.0) { + f = polevl(z, FN4, 6) / (x * p1evl(z, FD4, 7)); + g = z * polevl(z, GN4, 7) / p1evl(z, GD4, 7); + } + else { + f = polevl(z, FN8, 8) / (x * p1evl(z, FD8, 8)); + g = z * polevl(z, GN8, 8) / p1evl(z, GD8, 9); + } + *si = M_PI_2 - f * c - g * s; + if (sign) + *si = -(*si); + *ci = f * s - g * c; + + return (0); +} diff --git a/gtsam/3rdparty/cephes/cephes/sindg.c b/gtsam/3rdparty/cephes/cephes/sindg.c new file mode 100644 index 000000000..d9c37ebdb --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sindg.c @@ -0,0 +1,219 @@ +/* sindg.c + * + * Circular sine of angle in degrees + * + * + * + * SYNOPSIS: + * + * double x, y, sindg(); + * + * y = sindg( x ); + * + * + * + * DESCRIPTION: + * + * Range reduction is into intervals of 45 degrees. + * + * Two polynomial approximating functions are employed. + * Between 0 and pi/4 the sine is approximated by + * x + x**3 P(x**2). + * Between pi/4 and pi/2 the cosine is represented as + * 1 - x**2 P(x**2). + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE +-1000 30000 2.3e-16 5.6e-17 + * + * ERROR MESSAGES: + * + * message condition value returned + * sindg total loss x > 1.0e14 (IEEE) 0.0 + * + */ + /* cosdg.c + * + * Circular cosine of angle in degrees + * + * + * + * SYNOPSIS: + * + * double x, y, cosdg(); + * + * y = cosdg( x ); + * + * + * + * DESCRIPTION: + * + * Range reduction is into intervals of 45 degrees. + * + * Two polynomial approximating functions are employed. + * Between 0 and pi/4 the cosine is approximated by + * 1 - x**2 P(x**2). + * Between pi/4 and pi/2 the sine is represented as + * x + x**3 P(x**2). + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE +-1000 30000 2.1e-16 5.7e-17 + * See also sin(). + * + */ + +/* Cephes Math Library Release 2.0: April, 1987 + * Copyright 1985, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 */ + +#include "mconf.h" + +static double sincof[] = { + 1.58962301572218447952E-10, + -2.50507477628503540135E-8, + 2.75573136213856773549E-6, + -1.98412698295895384658E-4, + 8.33333333332211858862E-3, + -1.66666666666666307295E-1 +}; + +static double coscof[] = { + 1.13678171382044553091E-11, + -2.08758833757683644217E-9, + 2.75573155429816611547E-7, + -2.48015872936186303776E-5, + 1.38888888888806666760E-3, + -4.16666666666666348141E-2, + 4.99999999999999999798E-1 +}; + +static double PI180 = 1.74532925199432957692E-2; /* pi/180 */ +static double lossth = 1.0e14; + +double sindg(double x) +{ + double y, z, zz; + int j, sign; + + /* make argument positive but save the sign */ + sign = 1; + if (x < 0) { + x = -x; + sign = -1; + } + + if (x > lossth) { + sf_error("sindg", SF_ERROR_NO_RESULT, NULL); + return (0.0); + } + + y = floor(x / 45.0); /* integer part of x/M_PI_4 */ + + /* strip high bits of integer part to prevent integer overflow */ + z = ldexp(y, -4); + z = floor(z); /* integer part of y/8 */ + z = y - ldexp(z, 4); /* y - 16 * (y/16) */ + + j = z; /* convert to integer for tests on the phase angle */ + /* map zeros to origin */ + if (j & 1) { + j += 1; + y += 1.0; + } + j = j & 07; /* octant modulo 360 degrees */ + /* reflect in x axis */ + if (j > 3) { + sign = -sign; + j -= 4; + } + + z = x - y * 45.0; /* x mod 45 degrees */ + z *= PI180; /* multiply by pi/180 to convert to radians */ + zz = z * z; + + if ((j == 1) || (j == 2)) { + y = 1.0 - zz * polevl(zz, coscof, 6); + } + else { + y = z + z * (zz * polevl(zz, sincof, 5)); + } + + if (sign < 0) + y = -y; + + return (y); +} + + +double cosdg(double x) +{ + double y, z, zz; + int j, sign; + + /* make argument positive */ + sign = 1; + if (x < 0) + x = -x; + + if (x > lossth) { + sf_error("cosdg", SF_ERROR_NO_RESULT, NULL); + return (0.0); + } + + y = floor(x / 45.0); + z = ldexp(y, -4); + z = floor(z); /* integer part of y/8 */ + z = y - ldexp(z, 4); /* y - 16 * (y/16) */ + + /* integer and fractional part modulo one octant */ + j = z; + if (j & 1) { /* map zeros to origin */ + j += 1; + y += 1.0; + } + j = j & 07; + if (j > 3) { + j -= 4; + sign = -sign; + } + + if (j > 1) + sign = -sign; + + z = x - y * 45.0; /* x mod 45 degrees */ + z *= PI180; /* multiply by pi/180 to convert to radians */ + + zz = z * z; + + if ((j == 1) || (j == 2)) { + y = z + z * (zz * polevl(zz, sincof, 5)); + } + else { + y = 1.0 - zz * polevl(zz, coscof, 6); + } + + if (sign < 0) + y = -y; + + return (y); +} + + +/* Degrees, minutes, seconds to radians: */ + +/* 1 arc second, in radians = 4.848136811095359935899141023579479759563533023727e-6 */ +static double P64800 = + 4.848136811095359935899141023579479759563533023727e-6; + +double radian(double d, double m, double s) +{ + return (((d * 60.0 + m) * 60.0 + s) * P64800); +} diff --git a/gtsam/3rdparty/cephes/cephes/sinpi.c b/gtsam/3rdparty/cephes/cephes/sinpi.c new file mode 100644 index 000000000..f0e52f990 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/sinpi.c @@ -0,0 +1,54 @@ +/* + * Implement sin(pi * x) and cos(pi * x) for real x. Since the periods + * of these functions are integral (and thus representable in double + * precision), it's possible to compute them with greater accuracy + * than sin(x) and cos(x). + */ +#include "mconf.h" + + +/* Compute sin(pi * x). */ +double sinpi(double x) +{ + double s = 1.0; + double r; + + if (x < 0.0) { + x = -x; + s = -1.0; + } + + r = fmod(x, 2.0); + if (r < 0.5) { + return s*sin(M_PI*r); + } + else if (r > 1.5) { + return s*sin(M_PI*(r - 2.0)); + } + else { + return -s*sin(M_PI*(r - 1.0)); + } +} + + +/* Compute cos(pi * x) */ +double cospi(double x) +{ + double r; + + if (x < 0.0) { + x = -x; + } + + r = fmod(x, 2.0); + if (r == 0.5) { + // We don't want to return -0.0 + return 0.0; + } + if (r < 1.0) { + return -sin(M_PI*(r - 0.5)); + } + else { + return sin(M_PI*(r - 1.5)); + } +} diff --git a/gtsam/3rdparty/cephes/cephes/spence.c b/gtsam/3rdparty/cephes/cephes/spence.c new file mode 100644 index 000000000..48e1c4087 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/spence.c @@ -0,0 +1,125 @@ +/* spence.c + * + * Dilogarithm + * + * + * + * SYNOPSIS: + * + * double x, y, spence(); + * + * y = spence( x ); + * + * + * + * DESCRIPTION: + * + * Computes the integral + * + * x + * - + * | | log t + * spence(x) = - | ----- dt + * | | t - 1 + * - + * 1 + * + * for x >= 0. A rational approximation gives the integral in + * the interval (0.5, 1.5). Transformation formulas for 1/x + * and 1-x are employed outside the basic expansion range. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,4 30000 3.9e-15 5.4e-16 + * + * + */ + +/* spence.c */ + + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1985, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +static double A[8] = { + 4.65128586073990045278E-5, + 7.31589045238094711071E-3, + 1.33847639578309018650E-1, + 8.79691311754530315341E-1, + 2.71149851196553469920E0, + 4.25697156008121755724E0, + 3.29771340985225106936E0, + 1.00000000000000000126E0, +}; + +static double B[8] = { + 6.90990488912553276999E-4, + 2.54043763932544379113E-2, + 2.82974860602568089943E-1, + 1.41172597751831069617E0, + 3.63800533345137075418E0, + 5.03278880143316990390E0, + 3.54771340985225096217E0, + 9.99999999999999998740E-1, +}; + +extern double MACHEP; + +double spence(double x) +{ + double w, y, z; + int flag; + + if (x < 0.0) { + sf_error("spence", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + if (x == 1.0) + return (0.0); + + if (x == 0.0) + return (M_PI * M_PI / 6.0); + + flag = 0; + + if (x > 2.0) { + x = 1.0 / x; + flag |= 2; + } + + if (x > 1.5) { + w = (1.0 / x) - 1.0; + flag |= 2; + } + + else if (x < 0.5) { + w = -x; + flag |= 1; + } + + else + w = x - 1.0; + + + y = -w * polevl(w, A, 7) / polevl(w, B, 7); + + if (flag & 1) + y = (M_PI * M_PI) / 6.0 - log(x) * log(1.0 - x) - y; + + if (flag & 2) { + z = log(x); + y = -0.5 * z * z - y; + } + + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/stdtr.c b/gtsam/3rdparty/cephes/cephes/stdtr.c new file mode 100644 index 000000000..5a37536be --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/stdtr.c @@ -0,0 +1,203 @@ +/* stdtr.c + * + * Student's t distribution + * + * + * + * SYNOPSIS: + * + * double t, stdtr(); + * short k; + * + * y = stdtr( k, t ); + * + * + * DESCRIPTION: + * + * Computes the integral from minus infinity to t of the Student + * t distribution with integer k > 0 degrees of freedom: + * + * t + * - + * | | + * - | 2 -(k+1)/2 + * | ( (k+1)/2 ) | ( x ) + * ---------------------- | ( 1 + --- ) dx + * - | ( k ) + * sqrt( k pi ) | ( k/2 ) | + * | | + * - + * -inf. + * + * Relation to incomplete beta integral: + * + * 1 - stdtr(k,t) = 0.5 * incbet( k/2, 1/2, z ) + * where + * z = k/(k + t**2). + * + * For t < -2, this is the method of computation. For higher t, + * a direct method is derived from integration by parts. + * Since the function is symmetric about t=0, the area under the + * right tail of the density is found by calling the function + * with -t instead of t. + * + * ACCURACY: + * + * Tested at random 1 <= k <= 25. The "domain" refers to t. + * Relative error: + * arithmetic domain # trials peak rms + * IEEE -100,-2 50000 5.9e-15 1.4e-15 + * IEEE -2,100 500000 2.7e-15 4.9e-17 + */ + +/* stdtri.c + * + * Functional inverse of Student's t distribution + * + * + * + * SYNOPSIS: + * + * double p, t, stdtri(); + * int k; + * + * t = stdtri( k, p ); + * + * + * DESCRIPTION: + * + * Given probability p, finds the argument t such that stdtr(k,t) + * is equal to p. + * + * ACCURACY: + * + * Tested at random 1 <= k <= 100. The "domain" refers to p: + * Relative error: + * arithmetic domain # trials peak rms + * IEEE .001,.999 25000 5.7e-15 8.0e-16 + * IEEE 10^-6,.001 25000 2.0e-12 2.9e-14 + */ + + +/* + * Cephes Math Library Release 2.3: March, 1995 + * Copyright 1984, 1987, 1995 by Stephen L. Moshier + */ + +#include "mconf.h" +#include + +extern double MACHEP; + +double stdtr(int k, double t) +{ + double x, rk, z, f, tz, p, xsqk; + int j; + + if (k <= 0) { + sf_error("stdtr", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + if (t == 0) + return (0.5); + + if (t < -2.0) { + rk = k; + z = rk / (rk + t * t); + p = 0.5 * incbet(0.5 * rk, 0.5, z); + return (p); + } + + /* compute integral from -t to + t */ + + if (t < 0) + x = -t; + else + x = t; + + rk = k; /* degrees of freedom */ + z = 1.0 + (x * x) / rk; + + /* test if k is odd or even */ + if ((k & 1) != 0) { + + /* computation for odd k */ + + xsqk = x / sqrt(rk); + p = atan(xsqk); + if (k > 1) { + f = 1.0; + tz = 1.0; + j = 3; + while ((j <= (k - 2)) && ((tz / f) > MACHEP)) { + tz *= (j - 1) / (z * j); + f += tz; + j += 2; + } + p += f * xsqk / z; + } + p *= 2.0 / M_PI; + } + + + else { + + /* computation for even k */ + + f = 1.0; + tz = 1.0; + j = 2; + + while ((j <= (k - 2)) && ((tz / f) > MACHEP)) { + tz *= (j - 1) / (z * j); + f += tz; + j += 2; + } + p = f * x / sqrt(z * rk); + } + + /* common exit */ + + + if (t < 0) + p = -p; /* note destruction of relative accuracy */ + + p = 0.5 + 0.5 * p; + return (p); +} + +double stdtri(int k, double p) +{ + double t, rk, z; + int rflg; + + if (k <= 0 || p <= 0.0 || p >= 1.0) { + sf_error("stdtri", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + rk = k; + + if (p > 0.25 && p < 0.75) { + if (p == 0.5) + return (0.0); + z = 1.0 - 2.0 * p; + z = incbi(0.5, 0.5 * rk, fabs(z)); + t = sqrt(rk * z / (1.0 - z)); + if (p < 0.5) + t = -t; + return (t); + } + rflg = -1; + if (p >= 0.5) { + p = 1.0 - p; + rflg = 1; + } + z = incbi(0.5 * rk, 0.5, 2.0 * p); + + if (DBL_MAX * z < rk) + return (rflg * INFINITY); + t = sqrt(rk / z - rk); + return (rflg * t); +} diff --git a/gtsam/3rdparty/cephes/cephes/struve.c b/gtsam/3rdparty/cephes/cephes/struve.c new file mode 100644 index 000000000..26c86fa2d --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/struve.c @@ -0,0 +1,408 @@ +/* + * Compute the Struve function. + * + * Notes + * ----- + * + * We use three expansions for the Struve function discussed in [1]: + * + * - power series + * - expansion in Bessel functions + * - asymptotic large-z expansion + * + * Rounding errors are estimated based on the largest terms in the sums. + * + * ``struve_convergence.py`` plots the convergence regions of the different + * expansions. + * + * (i) + * + * Looking at the error in the asymptotic expansion, one finds that + * it's not worth trying if z ~> 0.7 * v + 12 for v > 0. + * + * (ii) + * + * The Bessel function expansion tends to fail for |z| >~ |v| and is not tried + * there. + * + * For Struve H it covers the quadrant v > z where the power series may fail to + * produce reasonable results. + * + * (iii) + * + * The three expansions together cover for Struve H the region z > 0, v real. + * + * They also cover Struve L, except that some loss of precision may occur around + * the transition region z ~ 0.7 |v|, v < 0, |v| >> 1 where the function changes + * rapidly. + * + * (iv) + * + * The power series is evaluated in double-double precision. This fixes accuracy + * issues in Struve H for |v| << |z| before the asymptotic expansion kicks in. + * Moreover, it improves the Struve L behavior for negative v. + * + * + * References + * ---------- + * [1] NIST Digital Library of Mathematical Functions + * https://dlmf.nist.gov/11 + */ + +/* + * Copyright (C) 2013 Pauli Virtanen + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * a. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * b. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * c. Neither the name of Enthought nor the names of the SciPy Developers + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDERS OR CONTRIBUTORS + * BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, + * OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF + * THE POSSIBILITY OF SUCH DAMAGE. + */ + +#include "mconf.h" +#include "dd_real.h" + +// #include "amos_wrappers.h" + +#define STRUVE_MAXITER 10000 +#define SUM_EPS 1e-16 /* be sure we are in the tail of the sum */ +#define SUM_TINY 1e-100 +#define GOOD_EPS 1e-12 +#define ACCEPTABLE_EPS 1e-7 +#define ACCEPTABLE_ATOL 1e-300 + +#define MIN(a, b) ((a) < (b) ? (a) : (b)) + +double struve_power_series(double v, double x, int is_h, double *err); +double struve_asymp_large_z(double v, double z, int is_h, double *err); +double struve_bessel_series(double v, double z, int is_h, double *err); + +static double bessel_y(double v, double x); +static double bessel_j(double v, double x); +static double struve_hl(double v, double x, int is_h); + +double struve_h(double v, double z) +{ + return struve_hl(v, z, 1); +} + +double struve_l(double v, double z) +{ + return struve_hl(v, z, 0); +} + +static double struve_hl(double v, double z, int is_h) +{ + double value[4], err[4], tmp; + int n; + + if (z < 0) { + n = v; + if (v == n) { + tmp = (n % 2 == 0) ? -1 : 1; + return tmp * struve_hl(v, -z, is_h); + } + else { + return NAN; + } + } + else if (z == 0) { + if (v < -1) { + return gammasgn(v + 1.5) * INFINITY; + } + else if (v == -1) { + return 2 / sqrt(M_PI) / Gamma(0.5); + } + else { + return 0; + } + } + + n = -v - 0.5; + if (n == -v - 0.5 && n > 0) { + if (is_h) { + return (n % 2 == 0 ? 1 : -1) * bessel_j(n + 0.5, z); + } + else { + return iv(n + 0.5, z); + } + } + + /* Try the asymptotic expansion */ + if (z >= 0.7*v + 12) { + value[0] = struve_asymp_large_z(v, z, is_h, &err[0]); + if (err[0] < GOOD_EPS * fabs(value[0])) { + return value[0]; + } + } + else { + err[0] = INFINITY; + } + + /* Try power series */ + value[1] = struve_power_series(v, z, is_h, &err[1]); + if (err[1] < GOOD_EPS * fabs(value[1])) { + return value[1]; + } + + /* Try bessel series */ + if (fabs(z) < fabs(v) + 20) { + value[2] = struve_bessel_series(v, z, is_h, &err[2]); + if (err[2] < GOOD_EPS * fabs(value[2])) { + return value[2]; + } + } + else { + err[2] = INFINITY; + } + + /* Return the best of the three, if it is acceptable */ + n = 0; + if (err[1] < err[n]) n = 1; + if (err[2] < err[n]) n = 2; + if (err[n] < ACCEPTABLE_EPS * fabs(value[n]) || err[n] < ACCEPTABLE_ATOL) { + return value[n]; + } + + /* Maybe it really is an overflow? */ + tmp = -lgam(v + 1.5) + (v + 1)*log(z/2); + if (!is_h) { + tmp = fabs(tmp); + } + if (tmp > 700) { + sf_error("struve", SF_ERROR_OVERFLOW, NULL); + return INFINITY * gammasgn(v + 1.5); + } + + /* Failure */ + sf_error("struve", SF_ERROR_NO_RESULT, NULL); + return NAN; +} + + +/* + * Power series for Struve H and L + * https://dlmf.nist.gov/11.2.1 + * + * Starts to converge roughly at |n| > |z| + */ +double struve_power_series(double v, double z, int is_h, double *err) +{ + int n, sgn; + double term, sum, maxterm, scaleexp, tmp; + double2 cterm, csum, cdiv, z2, c2v, ctmp; + + if (is_h) { + sgn = -1; + } + else { + sgn = 1; + } + + tmp = -lgam(v + 1.5) + (v + 1)*log(z/2); + if (tmp < -600 || tmp > 600) { + /* Scale exponent to postpone underflow/overflow */ + scaleexp = tmp/2; + tmp -= scaleexp; + } + else { + scaleexp = 0; + } + + term = 2 / sqrt(M_PI) * exp(tmp) * gammasgn(v + 1.5); + sum = term; + maxterm = 0; + + cterm = dd_create_d(term); + csum = dd_create_d(sum); + z2 = dd_create_d(sgn*z*z); + c2v = dd_create_d(2*v); + + for (n = 0; n < STRUVE_MAXITER; ++n) { + /* cdiv = (3 + 2*n) * (3 + 2*n + 2*v)) */ + cdiv = dd_create_d(3 + 2*n); + ctmp = dd_create_d(3 + 2*n); + ctmp = dd_add(ctmp, c2v); + cdiv = dd_mul(cdiv, ctmp); + + /* cterm *= z2 / cdiv */ + cterm = dd_mul(cterm, z2); + cterm = dd_div(cterm, cdiv); + + csum = dd_add(csum, cterm); + + term = dd_to_double(cterm); + sum = dd_to_double(csum); + + if (fabs(term) > maxterm) { + maxterm = fabs(term); + } + if (fabs(term) < SUM_TINY * fabs(sum) || term == 0 || !isfinite(sum)) { + break; + } + } + + *err = fabs(term) + fabs(maxterm) * 1e-22; + + if (scaleexp != 0) { + sum *= exp(scaleexp); + *err *= exp(scaleexp); + } + + if (sum == 0 && term == 0 && v < 0 && !is_h) { + /* Spurious underflow */ + *err = INFINITY; + return NAN; + } + + return sum; +} + + +/* + * Bessel series + * https://dlmf.nist.gov/11.4.19 + */ +double struve_bessel_series(double v, double z, int is_h, double *err) +{ + int n; + double term, cterm, sum, maxterm; + + if (is_h && v < 0) { + /* Works less reliably in this region */ + *err = INFINITY; + return NAN; + } + + sum = 0; + maxterm = 0; + + cterm = sqrt(z / (2*M_PI)); + + for (n = 0; n < STRUVE_MAXITER; ++n) { + if (is_h) { + term = cterm * bessel_j(n + v + 0.5, z) / (n + 0.5); + cterm *= z/2 / (n + 1); + } + else { + term = cterm * iv(n + v + 0.5, z) / (n + 0.5); + cterm *= -z/2 / (n + 1); + } + sum += term; + if (fabs(term) > maxterm) { + maxterm = fabs(term); + } + if (fabs(term) < SUM_EPS * fabs(sum) || term == 0 || !isfinite(sum)) { + break; + } + } + + *err = fabs(term) + fabs(maxterm) * 1e-16; + + /* Account for potential underflow of the Bessel functions */ + *err += 1e-300 * fabs(cterm); + + return sum; +} + + +/* + * Large-z expansion for Struve H and L + * https://dlmf.nist.gov/11.6.1 + */ +double struve_asymp_large_z(double v, double z, int is_h, double *err) +{ + int n, sgn, maxiter; + double term, sum, maxterm; + double m; + + if (is_h) { + sgn = -1; + } + else { + sgn = 1; + } + + /* Asymptotic expansion divergenge point */ + m = z/2; + if (m <= 0) { + maxiter = 0; + } + else if (m > STRUVE_MAXITER) { + maxiter = STRUVE_MAXITER; + } + else { + maxiter = (int)m; + } + if (maxiter == 0) { + *err = INFINITY; + return NAN; + } + + if (z < v) { + /* Exclude regions where our error estimation fails */ + *err = INFINITY; + return NAN; + } + + /* Evaluate sum */ + term = -sgn / sqrt(M_PI) * exp(-lgam(v + 0.5) + (v - 1) * log(z/2)) * gammasgn(v + 0.5); + sum = term; + maxterm = 0; + + for (n = 0; n < maxiter; ++n) { + term *= sgn * (1 + 2*n) * (1 + 2*n - 2*v) / (z*z); + sum += term; + if (fabs(term) > maxterm) { + maxterm = fabs(term); + } + if (fabs(term) < SUM_EPS * fabs(sum) || term == 0 || !isfinite(sum)) { + break; + } + } + + if (is_h) { + sum += bessel_y(v, z); + } + else { + sum += iv(v, z); + } + + /* + * This error estimate is strictly speaking valid only for + * n > v - 0.5, but numerical results indicate that it works + * reasonably. + */ + *err = fabs(term) + fabs(maxterm) * 1e-16; + + return sum; +} + + +static double bessel_y(double v, double x) +{ + return cbesy_wrap_real(v, x); +} + +static double bessel_j(double v, double x) +{ + return cbesj_wrap_real(v, x); +} diff --git a/gtsam/3rdparty/cephes/cephes/tandg.c b/gtsam/3rdparty/cephes/cephes/tandg.c new file mode 100644 index 000000000..1ea86329b --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/tandg.c @@ -0,0 +1,141 @@ +/* tandg.c + * + * Circular tangent of argument in degrees + * + * + * + * SYNOPSIS: + * + * double x, y, tandg(); + * + * y = tandg( x ); + * + * + * + * DESCRIPTION: + * + * Returns the circular tangent of the argument x in degrees. + * + * Range reduction is modulo pi/4. A rational function + * x + x**3 P(x**2)/Q(x**2) + * is employed in the basic interval [0, pi/4]. + * + * + * + * ACCURACY: + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 0,10 30000 3.2e-16 8.4e-17 + * + * ERROR MESSAGES: + * + * message condition value returned + * tandg total loss x > 1.0e14 (IEEE) 0.0 + * tandg singularity x = 180 k + 90 INFINITY + */ + /* cotdg.c + * + * Circular cotangent of argument in degrees + * + * + * + * SYNOPSIS: + * + * double x, y, cotdg(); + * + * y = cotdg( x ); + * + * + * + * DESCRIPTION: + * + * Returns the circular cotangent of the argument x in degrees. + * + * Range reduction is modulo pi/4. A rational function + * x + x**3 P(x**2)/Q(x**2) + * is employed in the basic interval [0, pi/4]. + * + * + * ERROR MESSAGES: + * + * message condition value returned + * cotdg total loss x > 1.0e14 (IEEE) 0.0 + * cotdg singularity x = 180 k INFINITY + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" + +static double PI180 = 1.74532925199432957692E-2; +static double lossth = 1.0e14; + +static double tancot(double, int); + +double tandg(double x) +{ + return (tancot(x, 0)); +} + + +double cotdg(double x) +{ + return (tancot(x, 1)); +} + + +static double tancot(double xx, int cotflg) +{ + double x; + int sign; + + /* make argument positive but save the sign */ + if (xx < 0) { + x = -xx; + sign = -1; + } + else { + x = xx; + sign = 1; + } + + if (x > lossth) { + sf_error("tandg", SF_ERROR_NO_RESULT, NULL); + return 0.0; + } + + /* modulo 180 */ + x = x - 180.0 * floor(x / 180.0); + if (cotflg) { + if (x <= 90.0) { + x = 90.0 - x; + } + else { + x = x - 90.0; + sign *= -1; + } + } + else { + if (x > 90.0) { + x = 180.0 - x; + sign *= -1; + } + } + if (x == 0.0) { + return 0.0; + } + else if (x == 45.0) { + return sign * 1.0; + } + else if (x == 90.0) { + sf_error((cotflg ? "cotdg" : "tandg"), SF_ERROR_SINGULAR, NULL); + return INFINITY; + } + /* x is now transformed into [0, 90) */ + return sign * tan(x * PI180); +} diff --git a/gtsam/3rdparty/cephes/cephes/tukey.c b/gtsam/3rdparty/cephes/cephes/tukey.c new file mode 100644 index 000000000..751314a87 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/tukey.c @@ -0,0 +1,68 @@ + +/* Compute the CDF of the Tukey-Lambda distribution + * using a bracketing search with special checks + * + * The PPF of the Tukey-lambda distribution is + * G(p) = (p**lam + (1-p)**lam) / lam + * + * Author: Travis Oliphant + */ + +#include + +#define SMALLVAL 1e-4 +#define EPS 1.0e-14 +#define MAXCOUNT 60 + +double tukeylambdacdf(double x, double lmbda) +{ + double pmin, pmid, pmax, plow, phigh, xeval; + int count; + + if (isnan(x) || isnan(lmbda)) { + return NAN; + } + + xeval = 1.0 / lmbda; + if (lmbda > 0.0) { + if (x <= (-xeval)) { + return 0.0; + } + if (x >= xeval) { + return 1.0; + } + } + + if ((-SMALLVAL < lmbda) && (lmbda < SMALLVAL)) { + if (x >= 0) { + return 1.0 / (1.0 + exp(-x)); + } + else { + return exp(x) / (1.0 + exp(x)); + } + } + + pmin = 0.0; + pmid = 0.5; + pmax = 1.0; + plow = pmin; + phigh = pmax; + count = 0; + + while ((count < MAXCOUNT) && (fabs(pmid - plow) > EPS)) { + xeval = (pow(pmid, lmbda) - pow(1.0 - pmid, lmbda)) / lmbda; + if (xeval == x) { + return pmid; + } + if (xeval > x) { + phigh = pmid; + pmid = (pmid + plow) / 2.0; + } + else { + plow = pmid; + pmid = (pmid + phigh) / 2.0; + } + count++; + } + return pmid; +} diff --git a/gtsam/3rdparty/cephes/cephes/unity.c b/gtsam/3rdparty/cephes/cephes/unity.c new file mode 100644 index 000000000..76bc7f08d --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/unity.c @@ -0,0 +1,190 @@ +/* unity.c + * + * Relative error approximations for function arguments near + * unity. + * + * log1p(x) = log(1+x) + * expm1(x) = exp(x) - 1 + * cosm1(x) = cos(x) - 1 + * lgam1p(x) = lgam(1+x) + * + */ + +/* Scipy changes: + * - 06-10-2016: added lgam1p + */ + +#include "mconf.h" + +extern double MACHEP; + + + +/* log1p(x) = log(1 + x) */ + +/* Coefficients for log(1+x) = x - x**2/2 + x**3 P(x)/Q(x) + * 1/sqrt(2) <= x < sqrt(2) + * Theoretical peak relative error = 2.32e-20 + */ +static const double LP[] = { + 4.5270000862445199635215E-5, + 4.9854102823193375972212E-1, + 6.5787325942061044846969E0, + 2.9911919328553073277375E1, + 6.0949667980987787057556E1, + 5.7112963590585538103336E1, + 2.0039553499201281259648E1, +}; + +static const double LQ[] = { + /* 1.0000000000000000000000E0, */ + 1.5062909083469192043167E1, + 8.3047565967967209469434E1, + 2.2176239823732856465394E2, + 3.0909872225312059774938E2, + 2.1642788614495947685003E2, + 6.0118660497603843919306E1, +}; + +double log1p(double x) +{ + double z; + + z = 1.0 + x; + if ((z < M_SQRT1_2) || (z > M_SQRT2)) + return (log(z)); + z = x * x; + z = -0.5 * z + x * (z * polevl(x, LP, 6) / p1evl(x, LQ, 6)); + return (x + z); +} + + +/* log(1 + x) - x */ +double log1pmx(double x) +{ + if (fabs(x) < 0.5) { + int n; + double xfac = x; + double term; + double res = 0; + + for(n = 2; n < MAXITER; n++) { + xfac *= -x; + term = xfac / n; + res += term; + if (fabs(term) < MACHEP * fabs(res)) { + break; + } + } + return res; + } + else { + return log1p(x) - x; + } +} + + +/* expm1(x) = exp(x) - 1 */ + +/* e^x = 1 + 2x P(x^2)/( Q(x^2) - P(x^2) ) + * -0.5 <= x <= 0.5 + */ + +static double EP[3] = { + 1.2617719307481059087798E-4, + 3.0299440770744196129956E-2, + 9.9999999999999999991025E-1, +}; + +static double EQ[4] = { + 3.0019850513866445504159E-6, + 2.5244834034968410419224E-3, + 2.2726554820815502876593E-1, + 2.0000000000000000000897E0, +}; + +double expm1(double x) +{ + double r, xx; + + if (!cephes_isfinite(x)) { + if (cephes_isnan(x)) { + return x; + } + else if (x > 0) { + return x; + } + else { + return -1.0; + } + + } + if ((x < -0.5) || (x > 0.5)) + return (exp(x) - 1.0); + xx = x * x; + r = x * polevl(xx, EP, 2); + r = r / (polevl(xx, EQ, 3) - r); + return (r + r); +} + + + +/* cosm1(x) = cos(x) - 1 */ + +static double coscof[7] = { + 4.7377507964246204691685E-14, + -1.1470284843425359765671E-11, + 2.0876754287081521758361E-9, + -2.7557319214999787979814E-7, + 2.4801587301570552304991E-5, + -1.3888888888888872993737E-3, + 4.1666666666666666609054E-2, +}; + +double cosm1(double x) +{ + double xx; + + if ((x < -M_PI_4) || (x > M_PI_4)) + return (cos(x) - 1.0); + xx = x * x; + xx = -0.5 * xx + xx * xx * polevl(xx, coscof, 6); + return xx; +} + + +/* Compute lgam(x + 1) around x = 0 using its Taylor series. */ +static double lgam1p_taylor(double x) +{ + int n; + double xfac, coeff, res; + + if (x == 0) { + return 0; + } + res = -SCIPY_EULER * x; + xfac = -x; + for (n = 2; n < 42; n++) { + xfac *= -x; + coeff = zeta(n, 1) * xfac / n; + res += coeff; + if (fabs(coeff) < MACHEP * fabs(res)) { + break; + } + } + + return res; +} + + +/* Compute lgam(x + 1). */ +double lgam1p(double x) +{ + if (fabs(x) <= 0.5) { + return lgam1p_taylor(x); + } else if (fabs(x - 1) < 0.5) { + return log(x) + lgam1p_taylor(x - 1); + } else { + return lgam(x + 1); + } +} diff --git a/gtsam/3rdparty/cephes/cephes/yn.c b/gtsam/3rdparty/cephes/cephes/yn.c new file mode 100644 index 000000000..c02ff0acd --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/yn.c @@ -0,0 +1,105 @@ +/* yn.c + * + * Bessel function of second kind of integer order + * + * + * + * SYNOPSIS: + * + * double x, y, yn(); + * int n; + * + * y = yn( n, x ); + * + * + * + * DESCRIPTION: + * + * Returns Bessel function of order n, where n is a + * (possibly negative) integer. + * + * The function is evaluated by forward recurrence on + * n, starting with values computed by the routines + * y0() and y1(). + * + * If n = 0 or 1 the routine for y0 or y1 is called + * directly. + * + * + * + * ACCURACY: + * + * + * Absolute error, except relative + * when y > 1: + * arithmetic domain # trials peak rms + * IEEE 0, 30 30000 3.4e-15 4.3e-16 + * + * + * ERROR MESSAGES: + * + * message condition value returned + * yn singularity x = 0 INFINITY + * yn overflow INFINITY + * + * Spot checked against tables for x, n between 0 and 100. + * + */ + +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" +extern double MAXLOG; + +double yn(int n, double x) +{ + double an, anm1, anm2, r; + int k, sign; + + if (n < 0) { + n = -n; + if ((n & 1) == 0) /* -1**n */ + sign = 1; + else + sign = -1; + } + else + sign = 1; + + + if (n == 0) + return (sign * y0(x)); + if (n == 1) + return (sign * y1(x)); + + /* test for overflow */ + if (x == 0.0) { + sf_error("yn", SF_ERROR_SINGULAR, NULL); + return -INFINITY * sign; + } + else if (x < 0.0) { + sf_error("yn", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + /* forward recurrence on n */ + + anm2 = y0(x); + anm1 = y1(x); + k = 1; + r = 2 * k; + do { + an = r * anm1 / x - anm2; + anm2 = anm1; + anm1 = an; + r += 2.0; + ++k; + } + while (k < n); + + + return (sign * an); +} diff --git a/gtsam/3rdparty/cephes/cephes/yv.c b/gtsam/3rdparty/cephes/cephes/yv.c new file mode 100644 index 000000000..e61a15521 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/yv.c @@ -0,0 +1,46 @@ +/* + * Cephes Math Library Release 2.8: June, 2000 + * Copyright 1984, 1987, 2000 by Stephen L. Moshier + */ + +#include "mconf.h" + +extern double MACHEP; + + +/* + * Bessel function of noninteger order + */ +double yv(double v, double x) +{ + double y, t; + int n; + + n = v; + if (n == v) { + y = yn(n, x); + return (y); + } + else if (v == floor(v)) { + /* Zero in denominator. */ + sf_error("yv", SF_ERROR_DOMAIN, NULL); + return NAN; + } + + t = M_PI * v; + y = (cos(t) * jv(v, x) - jv(-v, x)) / sin(t); + + if (cephes_isinf(y)) { + if (v > 0) { + sf_error("yv", SF_ERROR_OVERFLOW, NULL); + return -INFINITY; + } + else if (v < -1e10) { + /* Whether it's +inf or -inf is numerically ill-defined. */ + sf_error("yv", SF_ERROR_DOMAIN, NULL); + return NAN; + } + } + + return (y); +} diff --git a/gtsam/3rdparty/cephes/cephes/zeta.c b/gtsam/3rdparty/cephes/cephes/zeta.c new file mode 100644 index 000000000..554933a24 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/zeta.c @@ -0,0 +1,160 @@ +/* zeta.c + * + * Riemann zeta function of two arguments + * + * + * + * SYNOPSIS: + * + * double x, q, y, zeta(); + * + * y = zeta( x, q ); + * + * + * + * DESCRIPTION: + * + * + * + * inf. + * - -x + * zeta(x,q) = > (k+q) + * - + * k=0 + * + * where x > 1 and q is not a negative integer or zero. + * The Euler-Maclaurin summation formula is used to obtain + * the expansion + * + * n + * - -x + * zeta(x,q) = > (k+q) + * - + * k=1 + * + * 1-x inf. B x(x+1)...(x+2j) + * (n+q) 1 - 2j + * + --------- - ------- + > -------------------- + * x-1 x - x+2j+1 + * 2(n+q) j=1 (2j)! (n+q) + * + * where the B2j are Bernoulli numbers. Note that (see zetac.c) + * zeta(x,1) = zetac(x) + 1. + * + * + * + * ACCURACY: + * + * + * + * REFERENCE: + * + * Gradshteyn, I. S., and I. M. Ryzhik, Tables of Integrals, + * Series, and Products, p. 1073; Academic Press, 1980. + * + */ + +/* + * Cephes Math Library Release 2.0: April, 1987 + * Copyright 1984, 1987 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" +extern double MACHEP; + +/* Expansion coefficients + * for Euler-Maclaurin summation formula + * (2k)! / B2k + * where B2k are Bernoulli numbers + */ +static double A[] = { + 12.0, + -720.0, + 30240.0, + -1209600.0, + 47900160.0, + -1.8924375803183791606e9, /*1.307674368e12/691 */ + 7.47242496e10, + -2.950130727918164224e12, /*1.067062284288e16/3617 */ + 1.1646782814350067249e14, /*5.109094217170944e18/43867 */ + -4.5979787224074726105e15, /*8.028576626982912e20/174611 */ + 1.8152105401943546773e17, /*1.5511210043330985984e23/854513 */ + -7.1661652561756670113e18 /*1.6938241367317436694528e27/236364091 */ +}; + +/* 30 Nov 86 -- error in third coefficient fixed */ + + +double zeta(double x, double q) +{ + int i; + double a, b, k, s, t, w; + + if (x == 1.0) + goto retinf; + + if (x < 1.0) { + domerr: + sf_error("zeta", SF_ERROR_DOMAIN, NULL); + return (NAN); + } + + if (q <= 0.0) { + if (q == floor(q)) { + sf_error("zeta", SF_ERROR_SINGULAR, NULL); + retinf: + return (INFINITY); + } + if (x != floor(x)) + goto domerr; /* because q^-x not defined */ + } + + /* Asymptotic expansion + * https://dlmf.nist.gov/25.11#E43 + */ + if (q > 1e8) { + return (1/(x - 1) + 1/(2*q)) * pow(q, 1 - x); + } + + /* Euler-Maclaurin summation formula */ + + /* Permit negative q but continue sum until n+q > +9 . + * This case should be handled by a reflection formula. + * If q<0 and x is an integer, there is a relation to + * the polyGamma function. + */ + s = pow(q, -x); + a = q; + i = 0; + b = 0.0; + while ((i < 9) || (a <= 9.0)) { + i += 1; + a += 1.0; + b = pow(a, -x); + s += b; + if (fabs(b / s) < MACHEP) + goto done; + } + + w = a; + s += b * w / (x - 1.0); + s -= 0.5 * b; + a = 1.0; + k = 0.0; + for (i = 0; i < 12; i++) { + a *= x + k; + b /= w; + t = a * b / A[i]; + s = s + t; + t = fabs(t / s); + if (t < MACHEP) + goto done; + k += 1.0; + a *= x + k; + b /= w; + k += 1.0; + } +done: + return (s); +} diff --git a/gtsam/3rdparty/cephes/cephes/zetac.c b/gtsam/3rdparty/cephes/cephes/zetac.c new file mode 100644 index 000000000..841433183 --- /dev/null +++ b/gtsam/3rdparty/cephes/cephes/zetac.c @@ -0,0 +1,345 @@ +/* zetac.c + * + * Riemann zeta function + * + * + * + * SYNOPSIS: + * + * double x, y, zetac(); + * + * y = zetac( x ); + * + * + * + * DESCRIPTION: + * + * + * + * inf. + * - -x + * zetac(x) = > k , x > 1, + * - + * k=2 + * + * is related to the Riemann zeta function by + * + * Riemann zeta(x) = zetac(x) + 1. + * + * Extension of the function definition for x < 1 is implemented. + * Zero is returned for x > log2(INFINITY). + * + * ACCURACY: + * + * Tabulated values have full machine accuracy. + * + * Relative error: + * arithmetic domain # trials peak rms + * IEEE 1,50 10000 9.8e-16 1.3e-16 + * + * + */ + +/* + * Cephes Math Library Release 2.1: January, 1989 + * Copyright 1984, 1987, 1989 by Stephen L. Moshier + * Direct inquiries to 30 Frost Street, Cambridge, MA 02140 + */ + +#include "mconf.h" +#include "lanczos.h" + +/* Riemann zeta(x) - 1 + * for integer arguments between 0 and 30. + */ +static const double azetac[] = { + -1.50000000000000000000E0, + 0.0, /* Not used; zetac(1.0) is infinity. */ + 6.44934066848226436472E-1, + 2.02056903159594285400E-1, + 8.23232337111381915160E-2, + 3.69277551433699263314E-2, + 1.73430619844491397145E-2, + 8.34927738192282683980E-3, + 4.07735619794433937869E-3, + 2.00839282608221441785E-3, + 9.94575127818085337146E-4, + 4.94188604119464558702E-4, + 2.46086553308048298638E-4, + 1.22713347578489146752E-4, + 6.12481350587048292585E-5, + 3.05882363070204935517E-5, + 1.52822594086518717326E-5, + 7.63719763789976227360E-6, + 3.81729326499983985646E-6, + 1.90821271655393892566E-6, + 9.53962033872796113152E-7, + 4.76932986787806463117E-7, + 2.38450502727732990004E-7, + 1.19219925965311073068E-7, + 5.96081890512594796124E-8, + 2.98035035146522801861E-8, + 1.49015548283650412347E-8, + 7.45071178983542949198E-9, + 3.72533402478845705482E-9, + 1.86265972351304900640E-9, + 9.31327432419668182872E-10 +}; + +/* 2**x (1 - 1/x) (zeta(x) - 1) = P(1/x)/Q(1/x), 1 <= x <= 10 */ +static double P[9] = { + 5.85746514569725319540E11, + 2.57534127756102572888E11, + 4.87781159567948256438E10, + 5.15399538023885770696E9, + 3.41646073514754094281E8, + 1.60837006880656492731E7, + 5.92785467342109522998E5, + 1.51129169964938823117E4, + 2.01822444485997955865E2, +}; + +static double Q[8] = { + /* 1.00000000000000000000E0, */ + 3.90497676373371157516E11, + 5.22858235368272161797E10, + 5.64451517271280543351E9, + 3.39006746015350418834E8, + 1.79410371500126453702E7, + 5.66666825131384797029E5, + 1.60382976810944131506E4, + 1.96436237223387314144E2, +}; + +/* log(zeta(x) - 1 - 2**-x), 10 <= x <= 50 */ +static double A[11] = { + 8.70728567484590192539E6, + 1.76506865670346462757E8, + 2.60889506707483264896E10, + 5.29806374009894791647E11, + 2.26888156119238241487E13, + 3.31884402932705083599E14, + 5.13778997975868230192E15, + -1.98123688133907171455E15, + -9.92763810039983572356E16, + 7.82905376180870586444E16, + 9.26786275768927717187E16, +}; + +static double B[10] = { + /* 1.00000000000000000000E0, */ + -7.92625410563741062861E6, + -1.60529969932920229676E8, + -2.37669260975543221788E10, + -4.80319584350455169857E11, + -2.07820961754173320170E13, + -2.96075404507272223680E14, + -4.86299103694609136686E15, + 5.34589509675789930199E15, + 5.71464111092297631292E16, + -1.79915597658676556828E16, +}; + +/* (1-x) (zeta(x) - 1), 0 <= x <= 1 */ +static double R[6] = { + -3.28717474506562731748E-1, + 1.55162528742623950834E1, + -2.48762831680821954401E2, + 1.01050368053237678329E3, + 1.26726061410235149405E4, + -1.11578094770515181334E5, +}; + +static double S[5] = { + /* 1.00000000000000000000E0, */ + 1.95107674914060531512E1, + 3.17710311750646984099E2, + 3.03835500874445748734E3, + 2.03665876435770579345E4, + 7.43853965136767874343E4, +}; + +static double TAYLOR0[10] = { + -1.0000000009110164892, + -1.0000000057646759799, + -9.9999983138417361078e-1, + -1.0000013011460139596, + -1.000001940896320456, + -9.9987929950057116496e-1, + -1.000785194477042408, + -1.0031782279542924256, + -9.1893853320467274178e-1, + -1.5, +}; + +#define MAXL2 127 +#define SQRT_2_PI 0.79788456080286535587989 + +extern double MACHEP; + +static double zeta_reflection(double); +static double zetac_smallneg(double); +static double zetac_positive(double); + + +/* + * Riemann zeta function, minus one + */ +double zetac(double x) +{ + if (isnan(x)) { + return x; + } + else if (x == -INFINITY) { + return NAN; + } + else if (x < 0.0 && x > -0.01) { + return zetac_smallneg(x); + } + else if (x < 0.0) { + return zeta_reflection(-x) - 1; + } + else { + return zetac_positive(x); + } +} + + +/* + * Riemann zeta function + */ +double riemann_zeta(double x) +{ + if (isnan(x)) { + return x; + } + else if (x == -INFINITY) { + return NAN; + } + else if (x < 0.0 && x > -0.01) { + return 1 + zetac_smallneg(x); + } + else if (x < 0.0) { + return zeta_reflection(-x); + } + else { + return 1 + zetac_positive(x); + } +} + + +/* + * Compute zetac for positive arguments + */ +static inline double zetac_positive(double x) +{ + int i; + double a, b, s, w; + + if (x == 1.0) { + return INFINITY; + } + + if (x >= MAXL2) { + /* because first term is 2**-x */ + return 0.0; + } + + /* Tabulated values for integer argument */ + w = floor(x); + if (w == x) { + i = x; + if (i < 31) { +#ifdef UNK + return (azetac[i]); +#else + return (*(double *) &azetac[4 * i]); +#endif + } + } + + if (x < 1.0) { + w = 1.0 - x; + a = polevl(x, R, 5) / (w * p1evl(x, S, 5)); + return a; + } + + if (x <= 10.0) { + b = pow(2.0, x) * (x - 1.0); + w = 1.0 / x; + s = (x * polevl(w, P, 8)) / (b * p1evl(w, Q, 8)); + return s; + } + + if (x <= 50.0) { + b = pow(2.0, -x); + w = polevl(x, A, 10) / p1evl(x, B, 10); + w = exp(w) + b; + return w; + } + + /* Basic sum of inverse powers */ + s = 0.0; + a = 1.0; + do { + a += 2.0; + b = pow(a, -x); + s += b; + } + while (b / s > MACHEP); + + b = pow(2.0, -x); + s = (s + b) / (1.0 - b); + return s; +} + + +/* + * Compute zetac for small negative x. We can't use the reflection + * formula because to double precision 1 - x = 1 and zetac(1) = inf. + */ +static inline double zetac_smallneg(double x) +{ + return polevl(x, TAYLOR0, 9); +} + + +/* + * Compute zetac using the reflection formula (see DLMF 25.4.2) plus + * the Lanczos approximation for Gamma to avoid overflow. + */ +static inline double zeta_reflection(double x) +{ + double base, large_term, small_term, hx, x_shift; + + hx = x / 2; + if (hx == floor(hx)) { + /* Hit a zero of the sine factor */ + return 0; + } + + /* Reduce the argument to sine */ + x_shift = fmod(x, 4); + small_term = -SQRT_2_PI * sin(0.5 * M_PI * x_shift); + small_term *= lanczos_sum_expg_scaled(x + 1) * zeta(x + 1, 1); + + /* Group large terms together to prevent overflow */ + base = (x + lanczos_g + 0.5) / (2 * M_PI * M_E); + large_term = pow(base, x + 0.5); + if (isfinite(large_term)) { + return large_term * small_term; + } + /* + * We overflowed, but we might be able to stave off overflow by + * factoring in the small term earlier. To do this we compute + * + * (sqrt(large_term) * small_term) * sqrt(large_term) + * + * Since we only call this method for negative x bounded away from + * zero, the small term can only be as small sine on that region; + * i.e. about machine epsilon. This means that if the above still + * overflows, then there was truly no avoiding it. + */ + large_term = pow(base, 0.5 * x + 0.25); + return (large_term * small_term) * large_term; +} From 3538488b28cd18fb9b87dc4ce104800005473ee6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 09:30:26 -0500 Subject: [PATCH 067/208] refactor Cephes CMakeLists.txt to allow building from parent directory --- gtsam/3rdparty/cephes/CMakeLists.txt | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt index fdc17ea61..8ee91569b 100644 --- a/gtsam/3rdparty/cephes/CMakeLists.txt +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -19,6 +19,9 @@ set(CEPHES_HEADER_FILES cephes/polevl.h cephes/sf_error.h) +# Add header files +install(FILES ${CEPHES_HEADER_FILES} DESTINATION include/gtsam/3rdparty/cephes) + set(CEPHES_SOURCES cephes/airy.c cephes/bdtr.c @@ -70,7 +73,6 @@ set(CEPHES_SOURCES cephes/psi.c cephes/rgamma.c cephes/round.c - # cephes/scipy_iv.c cephes/sf_error.c cephes/shichi.c cephes/sici.c @@ -87,16 +89,23 @@ set(CEPHES_SOURCES cephes/zetac.c) # Add library source files -add_library(${PROJECT_NAME} SHARED ${CEPHES_SOURCES}) +add_library(cephes-gtsam SHARED ${CEPHES_SOURCES}) # Add include directory (aka headers) -target_include_directories(${PROJECT_NAME} PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) +target_include_directories( + cephes-gtsam BEFORE PUBLIC $ + $) set_target_properties( - ${PROJECT_NAME} + cephes-gtsam PROPERTIES VERSION ${PROJECT_VERSION} SOVERSION ${PROJECT_VERSION_MAJOR} - PUBLIC_HEADER ${CEPHES_HEADER_FILES} + # PUBLIC_HEADER ${CEPHES_HEADER_FILES} C_STANDARD 99) -install(FILES ${CEPHES_HEADER_FILES} DESTINATION include/gtsam/3rdparty/cephes) +install( + TARGETS cephes-gtsam + EXPORT GTSAM-exports + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) From 5481159f95296193b1accd525bcafaa0617c03ce Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 09:31:08 -0500 Subject: [PATCH 068/208] Link to cephes from gtsam --- CMakeLists.txt | 1 + cmake/HandleCephes.cmake | 47 ++++++++++++++++++++++++++++++++++++++++ gtsam/CMakeLists.txt | 3 +++ 3 files changed, 51 insertions(+) create mode 100644 cmake/HandleCephes.cmake diff --git a/CMakeLists.txt b/CMakeLists.txt index 16848a721..8e0497f8d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,6 +72,7 @@ include(cmake/HandleCCache.cmake) # ccache include(cmake/HandleCPack.cmake) # CPack include(cmake/HandleEigen.cmake) # Eigen3 include(cmake/HandleMetis.cmake) # metis +include(cmake/HandleCephes.cmake) # cephes include(cmake/HandleMKL.cmake) # MKL include(cmake/HandleOpenMP.cmake) # OpenMP include(cmake/HandlePerfTools.cmake) # Google perftools diff --git a/cmake/HandleCephes.cmake b/cmake/HandleCephes.cmake new file mode 100644 index 000000000..6ef406987 --- /dev/null +++ b/cmake/HandleCephes.cmake @@ -0,0 +1,47 @@ +# ############################################################################## +# Cephes library + +# For both system or bundle version, a cmake target "cephes-gtsam-if" is defined +# (interface library) + +option( + GTSAM_USE_SYSTEM_CEPHES + "Find and use system-installed cephes. If 'off', use the one bundled with GTSAM" + OFF) + +if(GTSAM_USE_SYSTEM_CEPHES) + # # Debian package: libmetis-dev + + # find_path(METIS_INCLUDE_DIR metis.h REQUIRED) find_library(METIS_LIBRARY + # metis REQUIRED) + + # if(METIS_INCLUDE_DIR AND METIS_LIBRARY) mark_as_advanced(METIS_INCLUDE_DIR) + # mark_as_advanced(METIS_LIBRARY) + + # add_library(cephes-gtsam-if INTERFACE) + # target_include_directories(cephes-gtsam-if BEFORE INTERFACE + # ${METIS_INCLUDE_DIR} # gtsam_unstable/partition/FindSeparator-inl.h uses + # internal metislib.h API # via extern "C" + # $ + # $ ) + # target_link_libraries(cephes-gtsam-if INTERFACE ${METIS_LIBRARY}) endif() + +else() + # Bundled version: + add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/cephes) + + list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam) + set(GTSAM_EXPORTED_TARGETS + "${GTSAM_EXPORTED_TARGETS}" + PARENT_SCOPE) + + add_library(cephes-gtsam-if INTERFACE) + target_link_libraries(cephes-gtsam-if INTERFACE cephes-gtsam) + +endif() + +list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam-if) +install( + TARGETS cephes-gtsam-if + EXPORT GTSAM-exports + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}) diff --git a/gtsam/CMakeLists.txt b/gtsam/CMakeLists.txt index e35f5aada..1fc8e4570 100644 --- a/gtsam/CMakeLists.txt +++ b/gtsam/CMakeLists.txt @@ -110,6 +110,9 @@ if(GTSAM_SUPPORT_NESTED_DISSECTION) list(APPEND GTSAM_ADDITIONAL_LIBRARIES metis-gtsam-if) endif() +# Link to cephes library +list(APPEND GTSAM_ADDITIONAL_LIBRARIES cephes-gtsam-if) + # Versions set(gtsam_version ${GTSAM_VERSION_STRING}) set(gtsam_soversion ${GTSAM_VERSION_MAJOR}) From ea81675393e3bc13b30f3519c88453363cf2a93b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 09:36:04 -0500 Subject: [PATCH 069/208] 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; From 3cde40ddc8c2c0dd9f4ded69c5a2ae65723c791c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 10:06:44 -0500 Subject: [PATCH 070/208] OS-based improved support --- cmake/HandleCephes.cmake | 3 --- gtsam/3rdparty/cephes/CMakeLists.txt | 14 +++++++++++++- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/cmake/HandleCephes.cmake b/cmake/HandleCephes.cmake index 6ef406987..837f7ad22 100644 --- a/cmake/HandleCephes.cmake +++ b/cmake/HandleCephes.cmake @@ -31,9 +31,6 @@ else() add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/cephes) list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam) - set(GTSAM_EXPORTED_TARGETS - "${GTSAM_EXPORTED_TARGETS}" - PARENT_SCOPE) add_library(cephes-gtsam-if INTERFACE) target_link_libraries(cephes-gtsam-if INTERFACE cephes-gtsam) diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt index 8ee91569b..e840e9e49 100644 --- a/gtsam/3rdparty/cephes/CMakeLists.txt +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -100,9 +100,21 @@ set_target_properties( cephes-gtsam PROPERTIES VERSION ${PROJECT_VERSION} SOVERSION ${PROJECT_VERSION_MAJOR} - # PUBLIC_HEADER ${CEPHES_HEADER_FILES} C_STANDARD 99) +if(WIN32) + set_target_properties( + cephes-gtsam + PROPERTIES PREFIX "" + COMPILE_FLAGS /w + RUNTIME_OUTPUT_DIRECTORY "${PROJECT_BINARY_DIR}/../../../bin") +endif() + +if(APPLE) + set_target_properties(cephes-gtsam PROPERTIES INSTALL_NAME_DIR + "${CMAKE_INSTALL_PREFIX}/lib") +endif() + install( TARGETS cephes-gtsam EXPORT GTSAM-exports From 5806f5f98cd5e74aa3ffc50573b7101a8de5caaa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 10:35:44 -0500 Subject: [PATCH 071/208] add M_PI definition if unavailable (e.g. in Windows) --- gtsam/3rdparty/cephes/cephes/mconf.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/gtsam/3rdparty/cephes/cephes/mconf.h b/gtsam/3rdparty/cephes/cephes/mconf.h index 9f3deb628..08119ef85 100644 --- a/gtsam/3rdparty/cephes/cephes/mconf.h +++ b/gtsam/3rdparty/cephes/cephes/mconf.h @@ -102,6 +102,10 @@ #define cephes_isfinite(x) isfinite(x) #endif +#if !defined(M_PI) +#define M_PI 3.14159265358979323846 +#endif + /* Constants needed that are not available in the C standard library */ #define SCIPY_EULER 0.577215664901532860606512090082402431 /* Euler constant */ #define SCIPY_El 2.718281828459045235360287471352662498L /* e as long double */ From 8db9e0146a195e9b30a5a5df61e55dc66ef69686 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 12:00:47 -0500 Subject: [PATCH 072/208] additional M_PI definitions --- gtsam/3rdparty/cephes/cephes/mconf.h | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/gtsam/3rdparty/cephes/cephes/mconf.h b/gtsam/3rdparty/cephes/cephes/mconf.h index 08119ef85..e200b9b03 100644 --- a/gtsam/3rdparty/cephes/cephes/mconf.h +++ b/gtsam/3rdparty/cephes/cephes/mconf.h @@ -102,9 +102,15 @@ #define cephes_isfinite(x) isfinite(x) #endif +/* M_PI et al. are not defined in math.h in C99, even with _USE_MATH_DEFINES */ #if !defined(M_PI) #define M_PI 3.14159265358979323846 #endif +#ifndef M_PI_2 +#define M_PI_2 1.57079632679489661923 /* pi/2 */ +#define M_1_PI 0.31830988618379067154 /* 1/pi */ +#define M_2_PI 0.63661977236758134308 /* 2/pi */ +#endif /* Constants needed that are not available in the C standard library */ #define SCIPY_EULER 0.577215664901532860606512090082402431 /* Euler constant */ From b1ce501afe385196a0efdd25d0913ead4c82da16 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 12:12:38 -0500 Subject: [PATCH 073/208] hopefully last of M_ definitions --- gtsam/3rdparty/cephes/cephes/mconf.h | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/gtsam/3rdparty/cephes/cephes/mconf.h b/gtsam/3rdparty/cephes/cephes/mconf.h index e200b9b03..c59d17a47 100644 --- a/gtsam/3rdparty/cephes/cephes/mconf.h +++ b/gtsam/3rdparty/cephes/cephes/mconf.h @@ -110,6 +110,19 @@ #define M_PI_2 1.57079632679489661923 /* pi/2 */ #define M_1_PI 0.31830988618379067154 /* 1/pi */ #define M_2_PI 0.63661977236758134308 /* 2/pi */ +#define M_E 2.71828182845904523536 +#define M_LOG2E 1.44269504088896340736 +#define M_LOG10E 0.434294481903251827651 +#define M_LN2 0.693147180559945309417 +#define M_LN10 2.30258509299404568402 +#define M_PI 3.14159265358979323846 +#define M_PI_2 1.57079632679489661923 +#define M_PI_4 0.785398163397448309616 +#define M_1_PI 0.318309886183790671538 +#define M_2_PI 0.636619772367581343076 +#define M_2_SQRTPI 1.12837916709551257390 +#define M_SQRT2 1.41421356237309504880 +#define M_SQRT1_2 0.707106781186547524401 #endif /* Constants needed that are not available in the C standard library */ From 70f1d4a80457b22dea93979e676e173869f3c6b4 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 12:51:09 -0500 Subject: [PATCH 074/208] mark GTSAM_EXPORT and update docstring --- gtsam/nonlinear/internal/ChiSquaredInverse.h | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/internal/ChiSquaredInverse.h b/gtsam/nonlinear/internal/ChiSquaredInverse.h index 78eb73f67..5a25a030e 100644 --- a/gtsam/nonlinear/internal/ChiSquaredInverse.h +++ b/gtsam/nonlinear/internal/ChiSquaredInverse.h @@ -21,6 +21,7 @@ #pragma once #include +#include namespace gtsam { @@ -29,14 +30,19 @@ namespace internal { /** * @brief Compute the quantile function of the Chi-Squared distribution. * + * The quantile function of the Chi-squared distribution is the quantile of + * the specific (inverse) incomplete Gamma distribution. + * + * We have a dedicated function so we can unit test any issues easily while also + * allowing it to be updated in the future without any backwards-compatibility + * issues. + * * @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); +double GTSAM_EXPORT chi_squared_quantile(const double dofs, + const double alpha) { return 2 * cephes_igami(dofs / 2, alpha); } From 98444aba3e1b0e6ecbfcdced442d9c16ffe14aa7 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 13:29:28 -0500 Subject: [PATCH 075/208] another windows fix --- gtsam/nonlinear/internal/ChiSquaredInverse.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/nonlinear/internal/ChiSquaredInverse.h b/gtsam/nonlinear/internal/ChiSquaredInverse.h index 5a25a030e..e0284c313 100644 --- a/gtsam/nonlinear/internal/ChiSquaredInverse.h +++ b/gtsam/nonlinear/internal/ChiSquaredInverse.h @@ -13,7 +13,8 @@ * @file ChiSquaredInverse.h * @brief Implementation of the Chi Squared inverse function. * - * Uses the cephes 3rd party library to help with gamma inverse functions. + * Uses the cephes 3rd party library to help with + * incomplete gamma inverse functions. * * @author Varun Agrawal */ @@ -41,9 +42,8 @@ namespace internal { * @param alpha Quantile value * @return double */ -double GTSAM_EXPORT chi_squared_quantile(const double dofs, - const double alpha) { - return 2 * cephes_igami(dofs / 2, alpha); +double chi_squared_quantile(const double dofs, const double alpha) { + return 2 * igami(dofs / 2, alpha); } } // namespace internal From e0b8c5292ade142b25dd521777b2eff02eea210e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 28 Dec 2023 14:05:39 -0500 Subject: [PATCH 076/208] kill testChiSquaredInverse --- gtsam/nonlinear/internal/ChiSquaredInverse.h | 7 ---- .../nonlinear/tests/testChiSquaredInverse.cpp | 37 ------------------- 2 files changed, 44 deletions(-) delete mode 100644 gtsam/nonlinear/tests/testChiSquaredInverse.cpp diff --git a/gtsam/nonlinear/internal/ChiSquaredInverse.h b/gtsam/nonlinear/internal/ChiSquaredInverse.h index e0284c313..dbf83f92b 100644 --- a/gtsam/nonlinear/internal/ChiSquaredInverse.h +++ b/gtsam/nonlinear/internal/ChiSquaredInverse.h @@ -22,10 +22,8 @@ #pragma once #include -#include namespace gtsam { - namespace internal { /** @@ -34,10 +32,6 @@ namespace internal { * The quantile function of the Chi-squared distribution is the quantile of * the specific (inverse) incomplete Gamma distribution. * - * We have a dedicated function so we can unit test any issues easily while also - * allowing it to be updated in the future without any backwards-compatibility - * issues. - * * @param dofs Degrees of freedom * @param alpha Quantile value * @return double @@ -47,5 +41,4 @@ double chi_squared_quantile(const double dofs, const double alpha) { } } // namespace internal - } // namespace gtsam diff --git a/gtsam/nonlinear/tests/testChiSquaredInverse.cpp b/gtsam/nonlinear/tests/testChiSquaredInverse.cpp deleted file mode 100644 index b9ff64fdb..000000000 --- a/gtsam/nonlinear/tests/testChiSquaredInverse.cpp +++ /dev/null @@ -1,37 +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 testChiSquaredInverse.cpp - * @date July 10, 2023 - * @author Varun Agrawal - * @brief Tests for Chi-squared distribution. - */ - -#include -#include -#include - -using namespace gtsam; - -/* ************************************************************************* */ -TEST(ChiSquaredInverse, ChiSqInv) { - double barcSq = internal::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); -} -/* ************************************************************************* */ From ba93dec850f0ce45b93271158ab9f778cfe7cc53 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 29 Dec 2023 11:18:58 -0500 Subject: [PATCH 077/208] only used built in version of Cephes since there doesn't seem to be an easy packaged version --- cmake/HandleCephes.cmake | 33 ++++----------------------------- 1 file changed, 4 insertions(+), 29 deletions(-) diff --git a/cmake/HandleCephes.cmake b/cmake/HandleCephes.cmake index 837f7ad22..9addddd60 100644 --- a/cmake/HandleCephes.cmake +++ b/cmake/HandleCephes.cmake @@ -4,38 +4,13 @@ # For both system or bundle version, a cmake target "cephes-gtsam-if" is defined # (interface library) -option( - GTSAM_USE_SYSTEM_CEPHES - "Find and use system-installed cephes. If 'off', use the one bundled with GTSAM" - OFF) -if(GTSAM_USE_SYSTEM_CEPHES) - # # Debian package: libmetis-dev +add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/cephes) - # find_path(METIS_INCLUDE_DIR metis.h REQUIRED) find_library(METIS_LIBRARY - # metis REQUIRED) +list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam) - # if(METIS_INCLUDE_DIR AND METIS_LIBRARY) mark_as_advanced(METIS_INCLUDE_DIR) - # mark_as_advanced(METIS_LIBRARY) - - # add_library(cephes-gtsam-if INTERFACE) - # target_include_directories(cephes-gtsam-if BEFORE INTERFACE - # ${METIS_INCLUDE_DIR} # gtsam_unstable/partition/FindSeparator-inl.h uses - # internal metislib.h API # via extern "C" - # $ - # $ ) - # target_link_libraries(cephes-gtsam-if INTERFACE ${METIS_LIBRARY}) endif() - -else() - # Bundled version: - add_subdirectory(${GTSAM_SOURCE_DIR}/gtsam/3rdparty/cephes) - - list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam) - - add_library(cephes-gtsam-if INTERFACE) - target_link_libraries(cephes-gtsam-if INTERFACE cephes-gtsam) - -endif() +add_library(cephes-gtsam-if INTERFACE) +target_link_libraries(cephes-gtsam-if INTERFACE cephes-gtsam) list(APPEND GTSAM_EXPORTED_TARGETS cephes-gtsam-if) install( From 687667add2da920f0cc3121bf460437e85afa576 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 29 Dec 2023 13:44:52 -0500 Subject: [PATCH 078/208] minor formatting --- CMakeLists.txt | 2 +- gtsam/3rdparty/cephes/cephes/cephes_names.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e0497f8d..66b1804af 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -72,7 +72,7 @@ include(cmake/HandleCCache.cmake) # ccache include(cmake/HandleCPack.cmake) # CPack include(cmake/HandleEigen.cmake) # Eigen3 include(cmake/HandleMetis.cmake) # metis -include(cmake/HandleCephes.cmake) # cephes +include(cmake/HandleCephes.cmake) # cephes include(cmake/HandleMKL.cmake) # MKL include(cmake/HandleOpenMP.cmake) # OpenMP include(cmake/HandlePerfTools.cmake) # Google perftools diff --git a/gtsam/3rdparty/cephes/cephes/cephes_names.h b/gtsam/3rdparty/cephes/cephes/cephes_names.h index 5322feb38..94be8c880 100644 --- a/gtsam/3rdparty/cephes/cephes/cephes_names.h +++ b/gtsam/3rdparty/cephes/cephes/cephes_names.h @@ -73,7 +73,7 @@ #define psi cephes_psi #define rgamma cephes_rgamma #define riemann_zeta cephes_riemann_zeta -// #define round cephes_round +// #define round cephes_round // Commented out since it clashes with std::round #define shichi cephes_shichi #define sici cephes_sici #define radian cephes_radian From b15bcb166e4efb633714236a51877fab2eef4447 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 29 Dec 2023 14:43:39 -0500 Subject: [PATCH 079/208] replace structured binding with exclusive access --- gtsam/navigation/NavState.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/NavState.cpp b/gtsam/navigation/NavState.cpp index 4410d629c..3e2817752 100644 --- a/gtsam/navigation/NavState.cpp +++ b/gtsam/navigation/NavState.cpp @@ -106,7 +106,8 @@ bool NavState::equals(const NavState& other, double tol) const { //------------------------------------------------------------------------------ NavState NavState::retract(const Vector9& xi, // OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const { - auto [nRb, n_t, n_v] = (*this); + Rot3 nRb = R_; + Point3 n_t = t_, n_v = v_; Matrix3 D_bRc_xi, D_R_nRb, D_t_nRb, D_v_nRb; const Rot3 bRc = Rot3::Expmap(dR(xi), H2 ? &D_bRc_xi : 0); const Rot3 nRc = nRb.compose(bRc, H1 ? &D_R_nRb : 0); From ddd38abe6b591baf265d99c03909f1cd8a3c5064 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 12:19:45 -0500 Subject: [PATCH 080/208] add Cephes README and License --- gtsam/3rdparty/cephes/LICENSE.txt | 7 +++++++ gtsam/3rdparty/cephes/README.md | 22 ++++++++++++++++++++++ 2 files changed, 29 insertions(+) create mode 100644 gtsam/3rdparty/cephes/LICENSE.txt create mode 100644 gtsam/3rdparty/cephes/README.md diff --git a/gtsam/3rdparty/cephes/LICENSE.txt b/gtsam/3rdparty/cephes/LICENSE.txt new file mode 100644 index 000000000..6c2842a96 --- /dev/null +++ b/gtsam/3rdparty/cephes/LICENSE.txt @@ -0,0 +1,7 @@ +MIT License + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. \ No newline at end of file diff --git a/gtsam/3rdparty/cephes/README.md b/gtsam/3rdparty/cephes/README.md new file mode 100644 index 000000000..63e3a6c8c --- /dev/null +++ b/gtsam/3rdparty/cephes/README.md @@ -0,0 +1,22 @@ +# README + +This is a vendored version of the Cephes Mathematical Library. The source code can be found on [netlib.org](https://www.netlib.org/cephes/). + +The software is provided with an [MIT License](https://smath.com/en-US/view/CephesMathLibrary/license). + +## Original Readme + +Some software in this archive may be from the book _Methods and +Programs for Mathematical Functions_ (Prentice-Hall or Simon & Schuster +International, 1989) or from the Cephes Mathematical Library, a +commercial product. In either event, it is copyrighted by the author. +What you see here may be used freely but it comes with no support or +guarantee. + +The two known misprints in the book are repaired here in the +source listings for the gamma function and the incomplete beta +integral. + + +Stephen L. Moshier +moshier@na-net.ornl.gov From 04bee2b8738ea193a453e37f7ed47cd48f94bd49 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 12:37:50 -0500 Subject: [PATCH 081/208] add cross platform extern marking to cephes.h --- gtsam/3rdparty/cephes/cephes.h | 235 +++++++++++++++--------------- gtsam/3rdparty/cephes/dllexport.h | 25 ++++ 2 files changed, 146 insertions(+), 114 deletions(-) create mode 100644 gtsam/3rdparty/cephes/dllexport.h diff --git a/gtsam/3rdparty/cephes/cephes.h b/gtsam/3rdparty/cephes/cephes.h index 629733eef..d5b59d895 100644 --- a/gtsam/3rdparty/cephes/cephes.h +++ b/gtsam/3rdparty/cephes/cephes.h @@ -2,159 +2,166 @@ #define CEPHES_H #include "cephes/cephes_names.h" +#include "dllexport.h" #ifdef __cplusplus extern "C" { #endif -extern int airy(double x, double *ai, double *aip, double *bi, double *bip); +CEPHES_EXTERN_EXPORT int airy(double x, double *ai, double *aip, double *bi, + double *bip); -extern double bdtrc(double k, int n, double p); -extern double bdtr(double k, int n, double p); -extern double bdtri(double k, int n, double y); +CEPHES_EXTERN_EXPORT double bdtrc(double k, int n, double p); +CEPHES_EXTERN_EXPORT double bdtr(double k, int n, double p); +CEPHES_EXTERN_EXPORT double bdtri(double k, int n, double y); -extern double besselpoly(double a, double lambda, double nu); +CEPHES_EXTERN_EXPORT double besselpoly(double a, double lambda, double nu); -extern double beta(double a, double b); -extern double lbeta(double a, double b); +CEPHES_EXTERN_EXPORT double beta(double a, double b); +CEPHES_EXTERN_EXPORT double lbeta(double a, double b); -extern double btdtr(double a, double b, double x); +CEPHES_EXTERN_EXPORT double btdtr(double a, double b, double x); -extern double cbrt(double x); -extern double chbevl(double x, double array[], int n); -extern double chdtrc(double df, double x); -extern double chdtr(double df, double x); -extern double chdtri(double df, double y); -extern double dawsn(double xx); +CEPHES_EXTERN_EXPORT double cbrt(double x); +CEPHES_EXTERN_EXPORT double chbevl(double x, double array[], int n); +CEPHES_EXTERN_EXPORT double chdtrc(double df, double x); +CEPHES_EXTERN_EXPORT double chdtr(double df, double x); +CEPHES_EXTERN_EXPORT double chdtri(double df, double y); +CEPHES_EXTERN_EXPORT double dawsn(double xx); -extern double ellie(double phi, double m); -extern double ellik(double phi, double m); -extern double ellpe(double x); +CEPHES_EXTERN_EXPORT double ellie(double phi, double m); +CEPHES_EXTERN_EXPORT double ellik(double phi, double m); +CEPHES_EXTERN_EXPORT double ellpe(double x); -extern int ellpj(double u, double m, double *sn, double *cn, double *dn, double *ph); -extern double ellpk(double x); -extern double exp10(double x); -extern double exp2(double x); +CEPHES_EXTERN_EXPORT int ellpj(double u, double m, double *sn, double *cn, + double *dn, double *ph); +CEPHES_EXTERN_EXPORT double ellpk(double x); +CEPHES_EXTERN_EXPORT double exp10(double x); +CEPHES_EXTERN_EXPORT double exp2(double x); -extern double expn(int n, double x); +CEPHES_EXTERN_EXPORT double expn(int n, double x); -extern double fdtrc(double a, double b, double x); -extern double fdtr(double a, double b, double x); -extern double fdtri(double a, double b, double y); +CEPHES_EXTERN_EXPORT double fdtrc(double a, double b, double x); +CEPHES_EXTERN_EXPORT double fdtr(double a, double b, double x); +CEPHES_EXTERN_EXPORT double fdtri(double a, double b, double y); -extern int fresnl(double xxa, double *ssa, double *cca); -extern double Gamma(double x); -extern double lgam(double x); -extern double lgam_sgn(double x, int *sign); -extern double gammasgn(double x); +CEPHES_EXTERN_EXPORT int fresnl(double xxa, double *ssa, double *cca); +CEPHES_EXTERN_EXPORT double Gamma(double x); +CEPHES_EXTERN_EXPORT double lgam(double x); +CEPHES_EXTERN_EXPORT double lgam_sgn(double x, int *sign); +CEPHES_EXTERN_EXPORT double gammasgn(double x); -extern double gdtr(double a, double b, double x); -extern double gdtrc(double a, double b, double x); -extern double gdtri(double a, double b, double y); +CEPHES_EXTERN_EXPORT double gdtr(double a, double b, double x); +CEPHES_EXTERN_EXPORT double gdtrc(double a, double b, double x); +CEPHES_EXTERN_EXPORT double gdtri(double a, double b, double y); -extern double hyp2f1(double a, double b, double c, double x); -extern double hyperg(double a, double b, double x); -extern double threef0(double a, double b, double c, double x, double *err); +CEPHES_EXTERN_EXPORT double hyp2f1(double a, double b, double c, double x); +CEPHES_EXTERN_EXPORT double hyperg(double a, double b, double x); +CEPHES_EXTERN_EXPORT double threef0(double a, double b, double c, double x, + double *err); -extern double i0(double x); -extern double i0e(double x); -extern double i1(double x); -extern double i1e(double x); -extern double igamc(double a, double x); -extern double igam(double a, double x); -extern double igam_fac(double a, double x); -extern double igamci(double a, double q); -extern double igami(double a, double p); +CEPHES_EXTERN_EXPORT double i0(double x); +CEPHES_EXTERN_EXPORT double i0e(double x); +CEPHES_EXTERN_EXPORT double i1(double x); +CEPHES_EXTERN_EXPORT double i1e(double x); +CEPHES_EXTERN_EXPORT double igamc(double a, double x); +CEPHES_EXTERN_EXPORT double igam(double a, double x); +CEPHES_EXTERN_EXPORT double igam_fac(double a, double x); +CEPHES_EXTERN_EXPORT double igamci(double a, double q); +CEPHES_EXTERN_EXPORT double igami(double a, double p); -extern double incbet(double aa, double bb, double xx); -extern double incbi(double aa, double bb, double yy0); +CEPHES_EXTERN_EXPORT double incbet(double aa, double bb, double xx); +CEPHES_EXTERN_EXPORT double incbi(double aa, double bb, double yy0); -extern double iv(double v, double x); -extern double j0(double x); -extern double y0(double x); -extern double j1(double x); -extern double y1(double x); +CEPHES_EXTERN_EXPORT double iv(double v, double x); +CEPHES_EXTERN_EXPORT double j0(double x); +CEPHES_EXTERN_EXPORT double y0(double x); +CEPHES_EXTERN_EXPORT double j1(double x); +CEPHES_EXTERN_EXPORT double y1(double x); -extern double jn(int n, double x); -extern double jv(double n, double x); -extern double k0(double x); -extern double k0e(double x); -extern double k1(double x); -extern double k1e(double x); -extern double kn(int nn, double x); +CEPHES_EXTERN_EXPORT double jn(int n, double x); +CEPHES_EXTERN_EXPORT double jv(double n, double x); +CEPHES_EXTERN_EXPORT double k0(double x); +CEPHES_EXTERN_EXPORT double k0e(double x); +CEPHES_EXTERN_EXPORT double k1(double x); +CEPHES_EXTERN_EXPORT double k1e(double x); +CEPHES_EXTERN_EXPORT double kn(int nn, double x); -extern double nbdtrc(int k, int n, double p); -extern double nbdtr(int k, int n, double p); -extern double nbdtri(int k, int n, double p); +CEPHES_EXTERN_EXPORT double nbdtrc(int k, int n, double p); +CEPHES_EXTERN_EXPORT double nbdtr(int k, int n, double p); +CEPHES_EXTERN_EXPORT double nbdtri(int k, int n, double p); -extern double ndtr(double a); -extern double log_ndtr(double a); -extern double erfc(double a); -extern double erf(double x); -extern double erfinv(double y); -extern double erfcinv(double y); -extern double ndtri(double y0); +CEPHES_EXTERN_EXPORT double ndtr(double a); +CEPHES_EXTERN_EXPORT double log_ndtr(double a); +CEPHES_EXTERN_EXPORT double erfc(double a); +CEPHES_EXTERN_EXPORT double erf(double x); +CEPHES_EXTERN_EXPORT double erfinv(double y); +CEPHES_EXTERN_EXPORT double erfcinv(double y); +CEPHES_EXTERN_EXPORT double ndtri(double y0); -extern double pdtrc(double k, double m); -extern double pdtr(double k, double m); -extern double pdtri(int k, double y); +CEPHES_EXTERN_EXPORT double pdtrc(double k, double m); +CEPHES_EXTERN_EXPORT double pdtr(double k, double m); +CEPHES_EXTERN_EXPORT double pdtri(int k, double y); -extern double poch(double x, double m); +CEPHES_EXTERN_EXPORT double poch(double x, double m); -extern double psi(double x); +CEPHES_EXTERN_EXPORT double psi(double x); -extern double rgamma(double x); -extern double round(double x); +CEPHES_EXTERN_EXPORT double rgamma(double x); +CEPHES_EXTERN_EXPORT double round(double x); -extern int shichi(double x, double *si, double *ci); -extern int sici(double x, double *si, double *ci); +CEPHES_EXTERN_EXPORT int shichi(double x, double *si, double *ci); +CEPHES_EXTERN_EXPORT int sici(double x, double *si, double *ci); -extern double radian(double d, double m, double s); -extern double sindg(double x); -extern double sinpi(double x); -extern double cosdg(double x); -extern double cospi(double x); +CEPHES_EXTERN_EXPORT double radian(double d, double m, double s); +CEPHES_EXTERN_EXPORT double sindg(double x); +CEPHES_EXTERN_EXPORT double sinpi(double x); +CEPHES_EXTERN_EXPORT double cosdg(double x); +CEPHES_EXTERN_EXPORT double cospi(double x); -extern double spence(double x); +CEPHES_EXTERN_EXPORT double spence(double x); -extern double stdtr(int k, double t); -extern double stdtri(int k, double p); +CEPHES_EXTERN_EXPORT double stdtr(int k, double t); +CEPHES_EXTERN_EXPORT double stdtri(int k, double p); -extern double struve_h(double v, double x); -extern double struve_l(double v, double x); -extern double struve_power_series(double v, double x, int is_h, double *err); -extern double struve_asymp_large_z(double v, double z, int is_h, double *err); -extern double struve_bessel_series(double v, double z, int is_h, double *err); +CEPHES_EXTERN_EXPORT double struve_h(double v, double x); +CEPHES_EXTERN_EXPORT double struve_l(double v, double x); +CEPHES_EXTERN_EXPORT double struve_power_series(double v, double x, int is_h, + double *err); +CEPHES_EXTERN_EXPORT double struve_asymp_large_z(double v, double z, int is_h, + double *err); +CEPHES_EXTERN_EXPORT double struve_bessel_series(double v, double z, int is_h, + double *err); -extern double yv(double v, double x); +CEPHES_EXTERN_EXPORT double yv(double v, double x); -extern double tandg(double x); -extern double cotdg(double x); +CEPHES_EXTERN_EXPORT double tandg(double x); +CEPHES_EXTERN_EXPORT double cotdg(double x); -extern double log1p(double x); -extern double log1pmx(double x); -extern double expm1(double x); -extern double cosm1(double x); -extern double lgam1p(double x); +CEPHES_EXTERN_EXPORT double log1p(double x); +CEPHES_EXTERN_EXPORT double log1pmx(double x); +CEPHES_EXTERN_EXPORT double expm1(double x); +CEPHES_EXTERN_EXPORT double cosm1(double x); +CEPHES_EXTERN_EXPORT double lgam1p(double x); -extern double yn(int n, double x); -extern double zeta(double x, double q); -extern double zetac(double x); +CEPHES_EXTERN_EXPORT double yn(int n, double x); +CEPHES_EXTERN_EXPORT double zeta(double x, double q); +CEPHES_EXTERN_EXPORT double zetac(double x); -extern double smirnov(int n, double d); -extern double smirnovi(int n, double p); -extern double smirnovp(int n, double d); -extern double smirnovc(int n, double d); -extern double smirnovci(int n, double p); -extern double kolmogorov(double x); -extern double kolmogi(double p); -extern double kolmogp(double x); -extern double kolmogc(double x); -extern double kolmogci(double p); +CEPHES_EXTERN_EXPORT double smirnov(int n, double d); +CEPHES_EXTERN_EXPORT double smirnovi(int n, double p); +CEPHES_EXTERN_EXPORT double smirnovp(int n, double d); +CEPHES_EXTERN_EXPORT double smirnovc(int n, double d); +CEPHES_EXTERN_EXPORT double smirnovci(int n, double p); +CEPHES_EXTERN_EXPORT double kolmogorov(double x); +CEPHES_EXTERN_EXPORT double kolmogi(double p); +CEPHES_EXTERN_EXPORT double kolmogp(double x); +CEPHES_EXTERN_EXPORT double kolmogc(double x); +CEPHES_EXTERN_EXPORT double kolmogci(double p); -extern double lanczos_sum_expg_scaled(double x); +CEPHES_EXTERN_EXPORT double lanczos_sum_expg_scaled(double x); -extern double owens_t(double h, double a); +CEPHES_EXTERN_EXPORT double owens_t(double h, double a); #ifdef __cplusplus } diff --git a/gtsam/3rdparty/cephes/dllexport.h b/gtsam/3rdparty/cephes/dllexport.h new file mode 100644 index 000000000..525587164 --- /dev/null +++ b/gtsam/3rdparty/cephes/dllexport.h @@ -0,0 +1,25 @@ +// Macros for exporting DLL symbols on Windows +// Usage example: +// In header file: +// class CEPHES_EXPORT MyClass { ... }; +// +// Results in the following declarations: +// When included while compiling the library itself: +// class __declspec(dllexport) MyClass { ... }; +// When included while compiling other code against the library: +// class __declspec(dllimport) MyClass { ... }; + +#pragma once + +#ifdef _WIN32 +# define CEPHES_EXPORT __declspec(dllimport) +# define CEPHES_EXTERN_EXPORT __declspec(dllimport) +#else +#ifdef __APPLE__ +# define CEPHES_EXPORT __attribute__((visibility("default"))) +# define CEPHES_EXTERN_EXPORT extern +#else +# define CEPHES_EXPORT +# define CEPHES_EXTERN_EXPORT extern +#endif +#endif From 0d058100e57d187de3b7c4638addf21da1c8158b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 14:44:12 -0500 Subject: [PATCH 082/208] update wrapper for LM with Ordering parameter --- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 2 ++ gtsam/nonlinear/nonlinear.i | 11 ++++++++--- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index d63f8b5ff..522dc626b 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -333,6 +333,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, const auto &factor = pair.second; if (!factor) return 1.0; // TODO(dellaert): not loving this. + // Logspace version of: // exp(-factor->error(kEmpty)) / pair.first->normalizationConstant(); return -factor->error(kEmpty) - pair.first->logNormalizationConstant(); }; @@ -345,6 +346,7 @@ hybridElimination(const HybridGaussianFactorGraph &factors, AlgebraicDecisionTree probabilities = DecisionTree( logProbabilities, [&max_log](const double x) { return exp(x - max_log); }); + // probabilities.print("", DefaultKeyFormatter); probabilities = probabilities.normalize(probabilities.sum()); return { diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 3f5fb1dd5..113f08e0f 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -378,17 +378,22 @@ typedef gtsam::GncOptimizer> G #include virtual class LevenbergMarquardtOptimizer : gtsam::NonlinearOptimizer { LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, - const gtsam::Values& initialValues); + const gtsam::Values& initialValues, + const gtsam::LevenbergMarquardtParams& params = + gtsam::LevenbergMarquardtParams()); LevenbergMarquardtOptimizer(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& initialValues, - const gtsam::LevenbergMarquardtParams& params); + const gtsam::Ordering& ordering, + const gtsam::LevenbergMarquardtParams& params = + gtsam::LevenbergMarquardtParams()); + double lambda() const; void print(string s = "") const; }; #include class ISAM2GaussNewtonParams { - ISAM2GaussNewtonParams(); + ISAM2GaussNewtonParams(double _wildfireThreshold = 0.001); void print(string s = "") const; From 651f99925b98a0bd5cb531927af3f3ef7f9c6af2 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:13:02 -0500 Subject: [PATCH 083/208] print logNormalizationConstant for Gaussian conditionals --- gtsam/hybrid/GaussianMixture.cpp | 2 ++ .../tests/testHybridNonlinearFactorGraph.cpp | 16 ++++++++++++++++ gtsam/linear/GaussianConditional.cpp | 1 + gtsam/linear/tests/testGaussianConditional.cpp | 3 +++ 4 files changed, 22 insertions(+) diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 61b40e566..b7840429b 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -174,6 +174,8 @@ void GaussianMixture::print(const std::string &s, std::cout << "(" << formatter(dk.first) << ", " << dk.second << "), "; } std::cout << "\n"; + std::cout << " logNormalizationConstant: " << logConstant_ << "\n" + << std::endl; conditionals_.print( "", [&](Key k) { return formatter(k); }, [&](const GaussianConditional::shared_ptr &gf) -> std::string { diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 93081d309..1dcd0bbb1 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -675,33 +675,41 @@ factor 6: P( m1 | m0 ): size: 3 conditional 0: Hybrid P( x0 | x1 m0) Discrete Keys = (m0, 2), + logNormalizationConstant: 1.38862 + Choice(m0) 0 Leaf p(x0 | x1) R = [ 10.0499 ] S[x1] = [ -0.0995037 ] d = [ -9.85087 ] + logNormalizationConstant: 1.38862 No noise model 1 Leaf p(x0 | x1) R = [ 10.0499 ] S[x1] = [ -0.0995037 ] d = [ -9.95037 ] + logNormalizationConstant: 1.38862 No noise model conditional 1: Hybrid P( x1 | x2 m0 m1) Discrete Keys = (m0, 2), (m1, 2), + logNormalizationConstant: 1.3935 + Choice(m1) 0 Choice(m0) 0 0 Leaf p(x1 | x2) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -9.99901 ] + logNormalizationConstant: 1.3935 No noise model 0 1 Leaf p(x1 | x2) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -9.90098 ] + logNormalizationConstant: 1.3935 No noise model 1 Choice(m0) @@ -709,16 +717,20 @@ conditional 1: Hybrid P( x1 | x2 m0 m1) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -10.098 ] + logNormalizationConstant: 1.3935 No noise model 1 1 Leaf p(x1 | x2) R = [ 10.099 ] S[x2] = [ -0.0990196 ] d = [ -10 ] + logNormalizationConstant: 1.3935 No noise model conditional 2: Hybrid P( x2 | m0 m1) Discrete Keys = (m0, 2), (m1, 2), + logNormalizationConstant: 1.38857 + Choice(m1) 0 Choice(m0) 0 0 Leaf p(x2) @@ -726,6 +738,7 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.1489 ] mean: 1 elements x2: -1.0099 + logNormalizationConstant: 1.38857 No noise model 0 1 Leaf p(x2) @@ -733,6 +746,7 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.1479 ] mean: 1 elements x2: -1.0098 + logNormalizationConstant: 1.38857 No noise model 1 Choice(m0) @@ -741,6 +755,7 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.0504 ] mean: 1 elements x2: -1.0001 + logNormalizationConstant: 1.38857 No noise model 1 1 Leaf p(x2) @@ -748,6 +763,7 @@ conditional 2: Hybrid P( x2 | m0 m1) d = [ -10.0494 ] mean: 1 elements x2: -1 + logNormalizationConstant: 1.38857 No noise model )"; diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 4ec1d8b95..fb7058282 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -121,6 +121,7 @@ namespace gtsam { const auto mean = solve({}); // solve for mean. mean.print(" mean", formatter); } + cout << " logNormalizationConstant: " << logNormalizationConstant() << std::endl; if (model_) model_->print(" Noise model: "); else diff --git a/gtsam/linear/tests/testGaussianConditional.cpp b/gtsam/linear/tests/testGaussianConditional.cpp index a4a722012..dcd821889 100644 --- a/gtsam/linear/tests/testGaussianConditional.cpp +++ b/gtsam/linear/tests/testGaussianConditional.cpp @@ -516,6 +516,7 @@ TEST(GaussianConditional, Print) { " d = [ 20 40 ]\n" " mean: 1 elements\n" " x0: 20 40\n" + " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected, conditional, "GaussianConditional")); @@ -530,6 +531,7 @@ TEST(GaussianConditional, Print) { " S[x1] = [ -1 -2 ]\n" " [ -3 -4 ]\n" " d = [ 20 40 ]\n" + " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected1, conditional1, "GaussianConditional")); @@ -545,6 +547,7 @@ TEST(GaussianConditional, Print) { " S[y1] = [ -5 -6 ]\n" " [ -7 -8 ]\n" " d = [ 20 40 ]\n" + " logNormalizationConstant: -4.0351\n" "isotropic dim=2 sigma=3\n"; EXPECT(assert_print_equal(expected2, conditional2, "GaussianConditional")); } From 114c86fc6bd1f81c57d5c9653cf15f8dd39596c8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:15:37 -0500 Subject: [PATCH 084/208] GaussianConditional wrapper for arbitrary number of keys --- gtsam/linear/linear.i | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index d4a045f09..6862646ae 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -489,12 +489,17 @@ virtual class GaussianConditional : gtsam::JacobianFactor { GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, size_t name2, Matrix T, const gtsam::noiseModel::Diagonal* sigmas); + GaussianConditional(const vector> terms, + size_t nrFrontals, Vector d, + const gtsam::noiseModel::Diagonal* sigmas); // Constructors with no noise model GaussianConditional(size_t key, Vector d, Matrix R); GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S); GaussianConditional(size_t key, Vector d, Matrix R, size_t name1, Matrix S, size_t name2, Matrix T); + GaussianConditional(const gtsam::KeyVector& keys, size_t nrFrontals, + const gtsam::VerticalBlockMatrix& augmentedMatrix); // Named constructors static gtsam::GaussianConditional FromMeanAndStddev(gtsam::Key key, From 82e0c0dae10e58e5a15d3930baeb031585e7946f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:26:05 -0500 Subject: [PATCH 085/208] take comment all the way --- gtsam/linear/GaussianConditional.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/linear/GaussianConditional.cpp b/gtsam/linear/GaussianConditional.cpp index 4ec1d8b95..d59070aaf 100644 --- a/gtsam/linear/GaussianConditional.cpp +++ b/gtsam/linear/GaussianConditional.cpp @@ -186,7 +186,10 @@ namespace gtsam { size_t n = d().size(); // Sigma = (R'R)^{-1}, det(Sigma) = det((R'R)^{-1}) = det(R'R)^{-1} // log det(Sigma) = -log(det(R'R)) = -2*log(det(R)) - // Hence, log det(Sigma)) = - 2.0 * logDeterminant() + // Hence, log det(Sigma)) = -2.0 * logDeterminant() + // which gives log = -0.5*n*log(2*pi) - 0.5*(-2.0 * logDeterminant()) + // = -0.5*n*log(2*pi) + (0.5*2.0 * logDeterminant()) + // = -0.5*n*log(2*pi) + logDeterminant() return -0.5 * n * log2pi + logDeterminant(); } From 8a61c49bb3ca447af3d7bac86c1694a73c0b398e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:32:21 -0500 Subject: [PATCH 086/208] add model_selection method to HybridBayesNet --- gtsam/hybrid/HybridBayesNet.cpp | 50 ++++++++++++++++----------------- gtsam/hybrid/HybridBayesNet.h | 13 +++++++++ 2 files changed, 37 insertions(+), 26 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 0352d7962..81f4badea 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -283,35 +283,20 @@ GaussianBayesNetValTree HybridBayesNet::assembleTree() const { } /* ************************************************************************* */ -HybridValues HybridBayesNet::optimize() const { - // Collect all the discrete factors to compute MPE - DiscreteFactorGraph discrete_fg; - +AlgebraicDecisionTree HybridBayesNet::model_selection() const { /* - Perform the integration of L(X;M,Z)P(X|M) - which is the model selection term. + To perform model selection, we need: + q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) - By Bayes' rule, P(X|M,Z) ∝ L(X;M,Z)P(X|M), - hence L(X;M,Z)P(X|M) is the unnormalized probabilty of - the joint Gaussian distribution. + If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) + thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) + = exp(log(q) - log(k)) = exp(-error - log(k)) + = exp(-(error + log(k))), + where error is computed at the corresponding MAP point, gbn.error(mu). - This can be computed by multiplying all the exponentiated errors - of each of the conditionals, which we do below in hybrid case. - */ - /* - To perform model selection, we need: - q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) + So we compute (error + log(k)) and exponentiate later + */ - If q(mu; M, Z) = exp(-error) & k = 1.0 / sqrt((2*pi)^n*det(Sigma)) - thus, q * sqrt((2*pi)^n*det(Sigma)) = q/k = exp(log(q/k)) - = exp(log(q) - log(k)) = exp(-error - log(k)) - = exp(-(error + log(k))), - where error is computed at the corresponding MAP point, gbn.error(mu). - - So we compute (error + log(k)) and exponentiate later - */ - - std::set discreteKeySet; GaussianBayesNetValTree bnTree = assembleTree(); GaussianBayesNetValTree bn_error = bnTree.apply( @@ -356,6 +341,19 @@ HybridValues HybridBayesNet::optimize() const { [&max_log](const double &x) { return std::exp(x - max_log); }); model_selection = model_selection.normalize(model_selection.sum()); + return model_selection; +} + +/* ************************************************************************* */ +HybridValues HybridBayesNet::optimize() const { + // Collect all the discrete factors to compute MPE + DiscreteFactorGraph discrete_fg; + + // Compute model selection term + AlgebraicDecisionTree model_selection_term = model_selection(); + + // Get the set of all discrete keys involved in model selection + std::set discreteKeySet; for (auto &&conditional : *this) { if (conditional->isDiscrete()) { discrete_fg.push_back(conditional->asDiscrete()); @@ -380,7 +378,7 @@ HybridValues HybridBayesNet::optimize() const { if (discreteKeySet.size() > 0) { discrete_fg.push_back(DecisionTreeFactor( DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()), - model_selection)); + model_selection_term)); } // Solve for the MPE diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index f0cdbaaf9..8acdd5b1b 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -120,6 +120,19 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { GaussianBayesNetValTree assembleTree() const; + /* + Perform the integration of L(X;M,Z)P(X|M) + which is the model selection term. + + By Bayes' rule, P(X|M,Z) ∝ L(X;M,Z)P(X|M), + hence L(X;M,Z)P(X|M) is the unnormalized probabilty of + the joint Gaussian distribution. + + This can be computed by multiplying all the exponentiated errors + of each of the conditionals. + */ + AlgebraicDecisionTree model_selection() const; + /** * @brief Solve the HybridBayesNet by first computing the MPE of all the * discrete variables and then optimizing the continuous variables based on From 3ba54eb25da317a81327eae0de3a4a382d9bee2d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:37:49 -0500 Subject: [PATCH 087/208] improved docstrings --- gtsam/hybrid/GaussianMixture.h | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index a52cd6c82..65c631ece 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -67,13 +67,14 @@ class GTSAM_EXPORT GaussianMixture double logConstant_; ///< log of the normalization constant. /** - * @brief Convert a DecisionTree of factors into a DT of Gaussian FGs. + * @brief Convert a DecisionTree of factors into + * a DecisionTree of Gaussian factor graphs. */ GaussianFactorGraphTree asGaussianFactorGraphTree() const; /** * @brief Convert a DecisionTree of conditionals into - * a DT of Gaussian Bayes nets. + * a DecisionTree of Gaussian Bayes nets. */ GaussianBayesNetTree asGaussianBayesNetTree() const; From bb95cd40d6a4434bf3bacffd155252a24e9266ef Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:38:25 -0500 Subject: [PATCH 088/208] remove `using std::dynamic_pointer_cast;` --- gtsam/hybrid/HybridBayesNet.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 81f4badea..933ef2703 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -26,8 +26,6 @@ static std::mt19937_64 kRandomNumberGenerator(42); namespace gtsam { -using std::dynamic_pointer_cast; - /* ************************************************************************ */ // Throw a runtime exception for method specified in string s, // and conditional f: @@ -253,9 +251,9 @@ GaussianBayesNetValTree HybridBayesNet::assembleTree() const { for (auto &f : factors_) { // TODO(dellaert): just use a virtual method defined in HybridFactor. - if (auto gm = dynamic_pointer_cast(f)) { + if (auto gm = std::dynamic_pointer_cast(f)) { result = gm->add(result); - } else if (auto hc = dynamic_pointer_cast(f)) { + } else if (auto hc = std::dynamic_pointer_cast(f)) { if (auto gm = hc->asMixture()) { result = gm->add(result); } else if (auto g = hc->asGaussian()) { @@ -265,7 +263,7 @@ GaussianBayesNetValTree HybridBayesNet::assembleTree() const { // TODO(dellaert): in C++20, we can use std::visit. continue; } - } else if (dynamic_pointer_cast(f)) { + } else if (std::dynamic_pointer_cast(f)) { // Don't do anything for discrete-only factors // since we want to evaluate continuous values only. continue; From 6d50de8c1cc10f7b778373666c46dd1e730d1f5e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 3 Jan 2024 16:40:04 -0500 Subject: [PATCH 089/208] docstring for HybridBayesNet::assembleTree --- gtsam/hybrid/HybridBayesNet.h | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 8acdd5b1b..cedb379c2 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -118,6 +118,14 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { return evaluate(values); } + /** + * @brief Assemble a DecisionTree of (GaussianBayesNet, double) leaves for + * each discrete assignment. + * The included double value is used to make + * constructing the model selection term cleaner and more efficient. + * + * @return GaussianBayesNetValTree + */ GaussianBayesNetValTree assembleTree() const; /* From bc3b96a6e81e67f04445398d6abf511c8e10f070 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 5 Jan 2024 03:24:50 -0500 Subject: [PATCH 090/208] rename error to errorTree when it returns an AlgebraicDecisionTree --- gtsam/discrete/DecisionTreeFactor.cpp | 2 +- gtsam/discrete/DecisionTreeFactor.h | 2 +- gtsam/discrete/DiscreteFactor.h | 2 +- gtsam/discrete/TableFactor.cpp | 4 ++-- gtsam/discrete/TableFactor.h | 2 +- gtsam/discrete/tests/testDecisionTreeFactor.cpp | 2 +- gtsam/hybrid/GaussianMixture.cpp | 6 +++--- gtsam/hybrid/GaussianMixture.h | 2 +- gtsam/hybrid/GaussianMixtureFactor.cpp | 6 +++--- gtsam/hybrid/GaussianMixtureFactor.h | 2 +- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 6 +++--- gtsam/hybrid/HybridGaussianFactorGraph.h | 3 ++- gtsam/hybrid/MixtureFactor.h | 6 +++--- gtsam/hybrid/tests/testGaussianMixture.cpp | 4 ++-- gtsam/hybrid/tests/testGaussianMixtureFactor.cpp | 2 +- gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp | 2 +- gtsam/hybrid/tests/testMixtureFactor.cpp | 3 ++- gtsam_unstable/discrete/AllDiff.h | 2 +- gtsam_unstable/discrete/BinaryAllDiff.h | 2 +- gtsam_unstable/discrete/Domain.h | 2 +- gtsam_unstable/discrete/SingleValue.h | 2 +- 21 files changed, 33 insertions(+), 31 deletions(-) diff --git a/gtsam/discrete/DecisionTreeFactor.cpp b/gtsam/discrete/DecisionTreeFactor.cpp index cbb26016c..c56818448 100644 --- a/gtsam/discrete/DecisionTreeFactor.cpp +++ b/gtsam/discrete/DecisionTreeFactor.cpp @@ -63,7 +63,7 @@ namespace gtsam { } /* ************************************************************************ */ - AlgebraicDecisionTree DecisionTreeFactor::error() const { + AlgebraicDecisionTree DecisionTreeFactor::errorTree() const { // Get all possible assignments DiscreteKeys dkeys = discreteKeys(); // Reverse to make cartesian product output a more natural ordering. diff --git a/gtsam/discrete/DecisionTreeFactor.h b/gtsam/discrete/DecisionTreeFactor.h index 5e0acc056..784b11e51 100644 --- a/gtsam/discrete/DecisionTreeFactor.h +++ b/gtsam/discrete/DecisionTreeFactor.h @@ -293,7 +293,7 @@ namespace gtsam { double error(const HybridValues& values) const override; /// Compute error for each assignment and return as a tree - AlgebraicDecisionTree error() const override; + AlgebraicDecisionTree errorTree() const override; /// @} diff --git a/gtsam/discrete/DiscreteFactor.h b/gtsam/discrete/DiscreteFactor.h index e84533655..771efbe5b 100644 --- a/gtsam/discrete/DiscreteFactor.h +++ b/gtsam/discrete/DiscreteFactor.h @@ -105,7 +105,7 @@ class GTSAM_EXPORT DiscreteFactor : public Factor { double error(const HybridValues& c) const override; /// Compute error for each assignment and return as a tree - virtual AlgebraicDecisionTree error() const = 0; + virtual AlgebraicDecisionTree errorTree() const = 0; /// Multiply in a DecisionTreeFactor and return the result as /// DecisionTreeFactor diff --git a/gtsam/discrete/TableFactor.cpp b/gtsam/discrete/TableFactor.cpp index be5f2af5b..b360617f5 100644 --- a/gtsam/discrete/TableFactor.cpp +++ b/gtsam/discrete/TableFactor.cpp @@ -169,8 +169,8 @@ double TableFactor::error(const HybridValues& values) const { } /* ************************************************************************ */ -AlgebraicDecisionTree TableFactor::error() const { - return toDecisionTreeFactor().error(); +AlgebraicDecisionTree TableFactor::errorTree() const { + return toDecisionTreeFactor().errorTree(); } /* ************************************************************************ */ diff --git a/gtsam/discrete/TableFactor.h b/gtsam/discrete/TableFactor.h index 40ed231fd..228b36337 100644 --- a/gtsam/discrete/TableFactor.h +++ b/gtsam/discrete/TableFactor.h @@ -359,7 +359,7 @@ class GTSAM_EXPORT TableFactor : public DiscreteFactor { double error(const HybridValues& values) const override; /// Compute error for each assignment and return as a tree - AlgebraicDecisionTree error() const override; + AlgebraicDecisionTree errorTree() const override; /// @} }; diff --git a/gtsam/discrete/tests/testDecisionTreeFactor.cpp b/gtsam/discrete/tests/testDecisionTreeFactor.cpp index 69ee52662..d764da7bf 100644 --- a/gtsam/discrete/tests/testDecisionTreeFactor.cpp +++ b/gtsam/discrete/tests/testDecisionTreeFactor.cpp @@ -75,7 +75,7 @@ TEST(DecisionTreeFactor, Error) { // Create factors DecisionTreeFactor f(X & Y & Z, "2 5 3 6 4 7 25 55 35 65 45 75"); - auto errors = f.error(); + auto errors = f.errorTree(); // regression AlgebraicDecisionTree expected( {X, Y, Z}, diff --git a/gtsam/hybrid/GaussianMixture.cpp b/gtsam/hybrid/GaussianMixture.cpp index 753e35bf0..c105a329e 100644 --- a/gtsam/hybrid/GaussianMixture.cpp +++ b/gtsam/hybrid/GaussianMixture.cpp @@ -313,14 +313,14 @@ AlgebraicDecisionTree GaussianMixture::logProbability( } /* *******************************************************************************/ -AlgebraicDecisionTree GaussianMixture::error( +AlgebraicDecisionTree GaussianMixture::errorTree( const VectorValues &continuousValues) const { auto errorFunc = [&](const GaussianConditional::shared_ptr &conditional) { return conditional->error(continuousValues) + // logConstant_ - conditional->logNormalizationConstant(); }; - DecisionTree errorTree(conditionals_, errorFunc); - return errorTree; + DecisionTree error_tree(conditionals_, errorFunc); + return error_tree; } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixture.h b/gtsam/hybrid/GaussianMixture.h index 0b68fcfd0..521a4ca7a 100644 --- a/gtsam/hybrid/GaussianMixture.h +++ b/gtsam/hybrid/GaussianMixture.h @@ -214,7 +214,7 @@ class GTSAM_EXPORT GaussianMixture * @return AlgebraicDecisionTree A decision tree on the discrete keys * only, with the leaf values as the error for each assignment. */ - AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + AlgebraicDecisionTree errorTree(const VectorValues &continuousValues) const; /** * @brief Compute the logProbability of this Gaussian Mixture. diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index 0c7ff0e87..a3db16d04 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -102,14 +102,14 @@ GaussianFactorGraphTree GaussianMixtureFactor::asGaussianFactorGraphTree() } /* *******************************************************************************/ -AlgebraicDecisionTree GaussianMixtureFactor::error( +AlgebraicDecisionTree GaussianMixtureFactor::errorTree( const VectorValues &continuousValues) const { // functor to convert from sharedFactor to double error value. auto errorFunc = [&continuousValues](const sharedFactor &gf) { return gf->error(continuousValues); }; - DecisionTree errorTree(factors_, errorFunc); - return errorTree; + DecisionTree error_tree(factors_, errorFunc); + return error_tree; } /* *******************************************************************************/ diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 1325cfe93..63ca9e923 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -135,7 +135,7 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { * @return AlgebraicDecisionTree A decision tree with the same keys * as the factors involved, and leaf values as the error. */ - AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + AlgebraicDecisionTree errorTree(const VectorValues &continuousValues) const; /** * @brief Compute the log-likelihood, including the log-normalizing constant. diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index 7eaefbf85..bdfac8468 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -420,7 +420,7 @@ EliminateHybrid(const HybridGaussianFactorGraph &factors, } /* ************************************************************************ */ -AlgebraicDecisionTree HybridGaussianFactorGraph::error( +AlgebraicDecisionTree HybridGaussianFactorGraph::errorTree( const VectorValues &continuousValues) const { AlgebraicDecisionTree error_tree(0.0); @@ -431,7 +431,7 @@ AlgebraicDecisionTree HybridGaussianFactorGraph::error( if (auto gaussianMixture = dynamic_pointer_cast(f)) { // Compute factor error and add it. - error_tree = error_tree + gaussianMixture->error(continuousValues); + error_tree = error_tree + gaussianMixture->errorTree(continuousValues); } else if (auto gaussian = dynamic_pointer_cast(f)) { // If continuous only, get the (double) error // and add it to the error_tree @@ -460,7 +460,7 @@ double HybridGaussianFactorGraph::probPrime(const HybridValues &values) const { /* ************************************************************************ */ AlgebraicDecisionTree HybridGaussianFactorGraph::probPrime( const VectorValues &continuousValues) const { - AlgebraicDecisionTree error_tree = this->error(continuousValues); + AlgebraicDecisionTree error_tree = this->errorTree(continuousValues); AlgebraicDecisionTree prob_tree = error_tree.apply([](double error) { // NOTE: The 0.5 term is handled by each factor return exp(-error); diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.h b/gtsam/hybrid/HybridGaussianFactorGraph.h index b3f159150..f924b7a1c 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.h +++ b/gtsam/hybrid/HybridGaussianFactorGraph.h @@ -161,7 +161,8 @@ class GTSAM_EXPORT HybridGaussianFactorGraph * @param continuousValues Continuous values at which to compute the error. * @return AlgebraicDecisionTree */ - AlgebraicDecisionTree error(const VectorValues& continuousValues) const; + AlgebraicDecisionTree errorTree( + const VectorValues& continuousValues) const; /** * @brief Compute unnormalized probability \f$ P(X | M, Z) \f$ diff --git a/gtsam/hybrid/MixtureFactor.h b/gtsam/hybrid/MixtureFactor.h index df8e0193a..09a641b48 100644 --- a/gtsam/hybrid/MixtureFactor.h +++ b/gtsam/hybrid/MixtureFactor.h @@ -131,13 +131,13 @@ class MixtureFactor : public HybridFactor { * @return AlgebraicDecisionTree A decision tree with the same keys * as the factor, and leaf values as the error. */ - AlgebraicDecisionTree error(const Values& continuousValues) const { + AlgebraicDecisionTree errorTree(const Values& continuousValues) const { // functor to convert from sharedFactor to double error value. auto errorFunc = [continuousValues](const sharedFactor& factor) { return factor->error(continuousValues); }; - DecisionTree errorTree(factors_, errorFunc); - return errorTree; + DecisionTree result(factors_, errorFunc); + return result; } /** diff --git a/gtsam/hybrid/tests/testGaussianMixture.cpp b/gtsam/hybrid/tests/testGaussianMixture.cpp index f15c06165..4da61912e 100644 --- a/gtsam/hybrid/tests/testGaussianMixture.cpp +++ b/gtsam/hybrid/tests/testGaussianMixture.cpp @@ -97,7 +97,7 @@ TEST(GaussianMixture, LogProbability) { /// Check error. TEST(GaussianMixture, Error) { using namespace equal_constants; - auto actual = mixture.error(vv); + auto actual = mixture.errorTree(vv); // Check result. std::vector discrete_keys = {mode}; @@ -134,7 +134,7 @@ TEST(GaussianMixture, Likelihood) { std::vector leaves = {conditionals[0]->likelihood(vv)->error(vv), conditionals[1]->likelihood(vv)->error(vv)}; AlgebraicDecisionTree expected(discrete_keys, leaves); - EXPECT(assert_equal(expected, likelihood->error(vv), 1e-6)); + EXPECT(assert_equal(expected, likelihood->errorTree(vv), 1e-6)); // Check that the ratio of probPrime to evaluate is the same for all modes. std::vector ratio(2); diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 75ba5a059..9cc7e6bfd 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -178,7 +178,7 @@ TEST(GaussianMixtureFactor, Error) { continuousValues.insert(X(2), Vector2(1, 1)); // error should return a tree of errors, with nodes for each discrete value. - AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); + AlgebraicDecisionTree error_tree = mixtureFactor.errorTree(continuousValues); std::vector discrete_keys = {m1}; // Error values for regression test diff --git a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp index b240e1626..98a8a794f 100644 --- a/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridGaussianFactorGraph.cpp @@ -580,7 +580,7 @@ TEST(HybridGaussianFactorGraph, ErrorAndProbPrimeTree) { HybridBayesNet::shared_ptr hybridBayesNet = graph.eliminateSequential(); HybridValues delta = hybridBayesNet->optimize(); - auto error_tree = graph.error(delta.continuous()); + auto error_tree = graph.errorTree(delta.continuous()); std::vector discrete_keys = {{M(0), 2}, {M(1), 2}}; std::vector leaves = {0.9998558, 0.4902432, 0.5193694, 0.0097568}; diff --git a/gtsam/hybrid/tests/testMixtureFactor.cpp b/gtsam/hybrid/tests/testMixtureFactor.cpp index 67a7fd8ae..0b2564403 100644 --- a/gtsam/hybrid/tests/testMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testMixtureFactor.cpp @@ -97,7 +97,8 @@ TEST(MixtureFactor, Error) { continuousValues.insert(X(1), 0); continuousValues.insert(X(2), 1); - AlgebraicDecisionTree error_tree = mixtureFactor.error(continuousValues); + AlgebraicDecisionTree error_tree = + mixtureFactor.errorTree(continuousValues); DiscreteKey m1(1, 2); std::vector discrete_keys = {m1}; diff --git a/gtsam_unstable/discrete/AllDiff.h b/gtsam_unstable/discrete/AllDiff.h index 9c8e62ecd..d7a63eae0 100644 --- a/gtsam_unstable/discrete/AllDiff.h +++ b/gtsam_unstable/discrete/AllDiff.h @@ -54,7 +54,7 @@ class GTSAM_UNSTABLE_EXPORT AllDiff : public Constraint { DecisionTreeFactor operator*(const DecisionTreeFactor& f) const override; /// Compute error for each assignment and return as a tree - AlgebraicDecisionTree error() const override { + AlgebraicDecisionTree errorTree() const override { throw std::runtime_error("AllDiff::error not implemented"); } diff --git a/gtsam_unstable/discrete/BinaryAllDiff.h b/gtsam_unstable/discrete/BinaryAllDiff.h index 33f6562b4..18b335092 100644 --- a/gtsam_unstable/discrete/BinaryAllDiff.h +++ b/gtsam_unstable/discrete/BinaryAllDiff.h @@ -93,7 +93,7 @@ class BinaryAllDiff : public Constraint { } /// Compute error for each assignment and return as a tree - AlgebraicDecisionTree error() const override { + AlgebraicDecisionTree errorTree() const override { throw std::runtime_error("BinaryAllDiff::error not implemented"); } }; diff --git a/gtsam_unstable/discrete/Domain.h b/gtsam_unstable/discrete/Domain.h index ca7340a9f..7f7b717c2 100644 --- a/gtsam_unstable/discrete/Domain.h +++ b/gtsam_unstable/discrete/Domain.h @@ -70,7 +70,7 @@ class GTSAM_UNSTABLE_EXPORT Domain : public Constraint { } /// Compute error for each assignment and return as a tree - AlgebraicDecisionTree error() const override { + AlgebraicDecisionTree errorTree() const override { throw std::runtime_error("Domain::error not implemented"); } diff --git a/gtsam_unstable/discrete/SingleValue.h b/gtsam_unstable/discrete/SingleValue.h index f57f24b42..3f7f22d6a 100644 --- a/gtsam_unstable/discrete/SingleValue.h +++ b/gtsam_unstable/discrete/SingleValue.h @@ -50,7 +50,7 @@ class GTSAM_UNSTABLE_EXPORT SingleValue : public Constraint { } /// Compute error for each assignment and return as a tree - AlgebraicDecisionTree error() const override { + AlgebraicDecisionTree errorTree() const override { throw std::runtime_error("SingleValue::error not implemented"); } From 7ea1bbcfc3f051a1ee938137790021f3fb4e5c0d Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 5 Jan 2024 15:04:15 -0500 Subject: [PATCH 091/208] replace error with errorTree --- gtsam/hybrid/HybridBayesNet.cpp | 4 ++-- gtsam/hybrid/HybridBayesNet.h | 3 ++- gtsam/hybrid/HybridGaussianFactorGraph.cpp | 8 ++++---- gtsam/hybrid/HybridNonlinearFactorGraph.cpp | 8 ++++---- gtsam/hybrid/tests/testHybridBayesNet.cpp | 2 +- 5 files changed, 13 insertions(+), 12 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index 31177ddb7..b02967555 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -282,7 +282,7 @@ HybridValues HybridBayesNet::sample() const { } /* ************************************************************************* */ -AlgebraicDecisionTree HybridBayesNet::error( +AlgebraicDecisionTree HybridBayesNet::errorTree( const VectorValues &continuousValues) const { AlgebraicDecisionTree result(0.0); @@ -290,7 +290,7 @@ AlgebraicDecisionTree HybridBayesNet::error( for (auto &&conditional : *this) { if (auto gm = conditional->asMixture()) { // If conditional is hybrid, compute error for all assignments. - result = result + gm->error(continuousValues); + result = result + gm->errorTree(continuousValues); } else if (auto gc = conditional->asGaussian()) { // If continuous, get the error and add it to the result diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 22e03bba9..032cd55b9 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -187,7 +187,8 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { * @param continuousValues Continuous values at which to compute the error. * @return AlgebraicDecisionTree */ - AlgebraicDecisionTree error(const VectorValues &continuousValues) const; + AlgebraicDecisionTree errorTree( + const VectorValues &continuousValues) const; /** * @brief Error method using HybridValues which returns specific error for diff --git a/gtsam/hybrid/HybridGaussianFactorGraph.cpp b/gtsam/hybrid/HybridGaussianFactorGraph.cpp index e372d0361..b764dc9e0 100644 --- a/gtsam/hybrid/HybridGaussianFactorGraph.cpp +++ b/gtsam/hybrid/HybridGaussianFactorGraph.cpp @@ -99,7 +99,7 @@ void HybridGaussianFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - gmf->error(values.continuous()).print("", keyFormatter); + gmf->errorTree(values.continuous()).print("", keyFormatter); std::cout << std::endl; } } else if (auto hc = std::dynamic_pointer_cast(factor)) { @@ -113,12 +113,12 @@ void HybridGaussianFactorGraph::printErrors( std::cout << "error = " << hc->asGaussian()->error(values) << "\n"; } else if (hc->isDiscrete()) { std::cout << "error = "; - hc->asDiscrete()->error().print("", keyFormatter); + hc->asDiscrete()->errorTree().print("", keyFormatter); std::cout << "\n"; } else { // Is hybrid std::cout << "error = "; - hc->asMixture()->error(values.continuous()).print(); + hc->asMixture()->errorTree(values.continuous()).print(); std::cout << "\n"; } } @@ -141,7 +141,7 @@ void HybridGaussianFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - df->error().print("", keyFormatter); + df->errorTree().print("", keyFormatter); } } else { diff --git a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp index e0dfd413c..cdd448412 100644 --- a/gtsam/hybrid/HybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/HybridNonlinearFactorGraph.cpp @@ -66,7 +66,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - mf->error(values.nonlinear()).print("", keyFormatter); + mf->errorTree(values.nonlinear()).print("", keyFormatter); std::cout << std::endl; } } else if (auto gmf = @@ -77,7 +77,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - gmf->error(values.continuous()).print("", keyFormatter); + gmf->errorTree(values.continuous()).print("", keyFormatter); std::cout << std::endl; } } else if (auto gm = std::dynamic_pointer_cast(factor)) { @@ -87,7 +87,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - gm->error(values.continuous()).print("", keyFormatter); + gm->errorTree(values.continuous()).print("", keyFormatter); std::cout << std::endl; } } else if (auto nf = std::dynamic_pointer_cast(factor)) { @@ -121,7 +121,7 @@ void HybridNonlinearFactorGraph::printErrors( } else { factor->print(ss.str(), keyFormatter); std::cout << "error = "; - df->error().print("", keyFormatter); + df->errorTree().print("", keyFormatter); std::cout << std::endl; } diff --git a/gtsam/hybrid/tests/testHybridBayesNet.cpp b/gtsam/hybrid/tests/testHybridBayesNet.cpp index 66985cc78..00dc36cd0 100644 --- a/gtsam/hybrid/tests/testHybridBayesNet.cpp +++ b/gtsam/hybrid/tests/testHybridBayesNet.cpp @@ -182,7 +182,7 @@ TEST(HybridBayesNet, Error) { values.insert(X(1), Vector1(1)); AlgebraicDecisionTree actual_errors = - bayesNet.error(values.continuous()); + bayesNet.errorTree(values.continuous()); // Regression. // Manually added all the error values from the 3 conditional types. From 0430fee3777482886ec3ca510d79bbc073186f5a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 7 Jan 2024 15:49:33 -0500 Subject: [PATCH 092/208] improved naming and documentation --- gtsam/hybrid/HybridBayesNet.cpp | 18 +++++++++--------- gtsam/hybrid/HybridBayesNet.h | 9 ++++++--- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/gtsam/hybrid/HybridBayesNet.cpp b/gtsam/hybrid/HybridBayesNet.cpp index e9870e6bf..23ee72215 100644 --- a/gtsam/hybrid/HybridBayesNet.cpp +++ b/gtsam/hybrid/HybridBayesNet.cpp @@ -281,7 +281,7 @@ GaussianBayesNetValTree HybridBayesNet::assembleTree() const { } /* ************************************************************************* */ -AlgebraicDecisionTree HybridBayesNet::model_selection() const { +AlgebraicDecisionTree HybridBayesNet::modelSelection() const { /* To perform model selection, we need: q(mu; M, Z) * sqrt((2*pi)^n*det(Sigma)) @@ -330,16 +330,16 @@ AlgebraicDecisionTree HybridBayesNet::model_selection() const { }); // Compute model selection term (with help from ADT methods) - AlgebraicDecisionTree model_selection_term = + AlgebraicDecisionTree modelSelectionTerm = (errorTree + log_norm_constants) * -1; - double max_log = model_selection_term.max(); - AlgebraicDecisionTree model_selection = DecisionTree( - model_selection_term, + double max_log = modelSelectionTerm.max(); + modelSelectionTerm = DecisionTree( + modelSelectionTerm, [&max_log](const double &x) { return std::exp(x - max_log); }); - model_selection = model_selection.normalize(model_selection.sum()); + modelSelectionTerm = modelSelectionTerm.normalize(modelSelectionTerm.sum()); - return model_selection; + return modelSelectionTerm; } /* ************************************************************************* */ @@ -348,7 +348,7 @@ HybridValues HybridBayesNet::optimize() const { DiscreteFactorGraph discrete_fg; // Compute model selection term - AlgebraicDecisionTree model_selection_term = model_selection(); + AlgebraicDecisionTree modelSelectionTerm = modelSelection(); // Get the set of all discrete keys involved in model selection std::set discreteKeySet; @@ -376,7 +376,7 @@ HybridValues HybridBayesNet::optimize() const { if (discreteKeySet.size() > 0) { discrete_fg.push_back(DecisionTreeFactor( DiscreteKeys(discreteKeySet.begin(), discreteKeySet.end()), - model_selection_term)); + modelSelectionTerm)); } // Solve for the MPE diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 757e60aea..9d16a4e14 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -129,8 +129,11 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { GaussianBayesNetValTree assembleTree() const; /* - Perform the integration of L(X;M,Z)P(X|M) - which is the model selection term. + Compute L(M;Z), the likelihood of the discrete model M + given the measurements Z. + This is called the model selection term. + + To do so, we perform the integration of L(M;Z) ∝ L(X;M,Z)P(X|M). By Bayes' rule, P(X|M,Z) ∝ L(X;M,Z)P(X|M), hence L(X;M,Z)P(X|M) is the unnormalized probabilty of @@ -139,7 +142,7 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { This can be computed by multiplying all the exponentiated errors of each of the conditionals. */ - AlgebraicDecisionTree model_selection() const; + AlgebraicDecisionTree modelSelection() const; /** * @brief Solve the HybridBayesNet by first computing the MPE of all the From afcb93390ad05a93a8d9966e16184b935d76f4a8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 7 Jan 2024 15:54:40 -0500 Subject: [PATCH 093/208] document return type --- gtsam/hybrid/HybridBayesNet.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/hybrid/HybridBayesNet.h b/gtsam/hybrid/HybridBayesNet.h index 9d16a4e14..fe9010b1f 100644 --- a/gtsam/hybrid/HybridBayesNet.h +++ b/gtsam/hybrid/HybridBayesNet.h @@ -141,6 +141,8 @@ class GTSAM_EXPORT HybridBayesNet : public BayesNet { This can be computed by multiplying all the exponentiated errors of each of the conditionals. + + Return a tree where each leaf value is L(M_i;Z). */ AlgebraicDecisionTree modelSelection() const; From c5bfd524e0ca8fa8c49fe5ad8183492b748b81ad Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 12 Jan 2024 10:28:02 -0500 Subject: [PATCH 094/208] better printing of GaussianMixtureFactor --- gtsam/hybrid/GaussianMixtureFactor.cpp | 4 +++- gtsam/hybrid/GaussianMixtureFactor.h | 5 ++--- gtsam/hybrid/tests/testGaussianMixtureFactor.cpp | 3 ++- gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp | 2 ++ 4 files changed, 9 insertions(+), 5 deletions(-) diff --git a/gtsam/hybrid/GaussianMixtureFactor.cpp b/gtsam/hybrid/GaussianMixtureFactor.cpp index a3db16d04..4e138acfa 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.cpp +++ b/gtsam/hybrid/GaussianMixtureFactor.cpp @@ -54,7 +54,9 @@ bool GaussianMixtureFactor::equals(const HybridFactor &lf, double tol) const { /* *******************************************************************************/ void GaussianMixtureFactor::print(const std::string &s, const KeyFormatter &formatter) const { - HybridFactor::print(s, formatter); + std::cout << (s.empty() ? "" : s + "\n"); + std::cout << "GaussianMixtureFactor" << std::endl; + HybridFactor::print("", formatter); std::cout << "{\n"; if (factors_.empty()) { std::cout << " empty" << std::endl; diff --git a/gtsam/hybrid/GaussianMixtureFactor.h b/gtsam/hybrid/GaussianMixtureFactor.h index 63ca9e923..913426a98 100644 --- a/gtsam/hybrid/GaussianMixtureFactor.h +++ b/gtsam/hybrid/GaussianMixtureFactor.h @@ -107,9 +107,8 @@ class GTSAM_EXPORT GaussianMixtureFactor : public HybridFactor { bool equals(const HybridFactor &lf, double tol = 1e-9) const override; - void print( - const std::string &s = "GaussianMixtureFactor\n", - const KeyFormatter &formatter = DefaultKeyFormatter) const override; + void print(const std::string &s = "", const KeyFormatter &formatter = + DefaultKeyFormatter) const override; /// @} /// @name Standard API diff --git a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp index 9cc7e6bfd..11b01f074 100644 --- a/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp +++ b/gtsam/hybrid/tests/testGaussianMixtureFactor.cpp @@ -106,7 +106,8 @@ TEST(GaussianMixtureFactor, Printing) { GaussianMixtureFactor mixtureFactor({X(1), X(2)}, {m1}, factors); std::string expected = - R"(Hybrid [x1 x2; 1]{ + R"(GaussianMixtureFactor +Hybrid [x1 x2; 1]{ Choice(1) 0 Leaf : A[x1] = [ diff --git a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp index 93081d309..0d4bf27c4 100644 --- a/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp +++ b/gtsam/hybrid/tests/testHybridNonlinearFactorGraph.cpp @@ -510,6 +510,7 @@ factor 0: b = [ -10 ] No noise model factor 1: +GaussianMixtureFactor Hybrid [x0 x1; m0]{ Choice(m0) 0 Leaf : @@ -534,6 +535,7 @@ Hybrid [x0 x1; m0]{ } factor 2: +GaussianMixtureFactor Hybrid [x1 x2; m1]{ Choice(m1) 0 Leaf : From a4a0fefd84418fedbe89834b8f78541523661032 Mon Sep 17 00:00:00 2001 From: Bernd Pfrommer Date: Fri, 12 Jan 2024 17:28:12 -0500 Subject: [PATCH 095/208] libcephes only needs 3.10, allowing build on ubuntu 18.04 --- gtsam/3rdparty/cephes/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt index e840e9e49..946448160 100644 --- a/gtsam/3rdparty/cephes/CMakeLists.txt +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.12) +cmake_minimum_required(VERSION 3.10) enable_testing() project( cephes From 2971d9e74ebac099e7741b78612fd5a69ece4035 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 8 Dec 2023 01:25:41 +0200 Subject: [PATCH 096/208] Compile gtsam python for windows --- .github/scripts/python.sh | 7 ++-- .github/workflows/build-python.yml | 55 +++++++++++++++++++++++++-- gtsam/base/utilities.h | 4 +- gtsam/discrete/DiscreteValues.h | 4 +- gtsam/geometry/Rot3.h | 2 +- gtsam/nonlinear/CustomFactor.h | 2 +- gtsam/sfm/DsfTrackGenerator.h | 2 +- python/CMakeLists.txt | 60 ++++++++++++++++++++++++++++-- python/setup.py.in | 3 +- 9 files changed, 121 insertions(+), 18 deletions(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index 08b8084a0..c851c7e36 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -65,14 +65,13 @@ function build() # Set to 2 cores so that Actions does not error out during resource provisioning. cmake --build build -j2 - $PYTHON -m pip install --user build/python + cmake --build build --target python-install } function test() { - cd $GITHUB_WORKSPACE/python/gtsam/tests - $PYTHON -m unittest discover -v - cd $GITHUB_WORKSPACE + cmake --build build --target python-test + cmake --build build --target python-test-unstable } # select between build or test diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 91bc4e80a..676fe8ea0 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -18,9 +18,11 @@ jobs: CTEST_PARALLEL_LEVEL: 2 CMAKE_BUILD_TYPE: ${{ matrix.build_type }} PYTHON_VERSION: ${{ matrix.python_version }} + BOOST_VERSION: 1.72.0 + BOOST_EXE: boost_1_72_0-msvc-14.2 strategy: - fail-fast: true + fail-fast: false matrix: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. @@ -30,6 +32,7 @@ jobs: ubuntu-20.04-gcc-9-tbb, ubuntu-20.04-clang-9, macOS-11-xcode-13.4.1, + windows-2019-msbuild, ] build_type: [Release] @@ -56,6 +59,10 @@ jobs: compiler: xcode version: "13.4.1" + - name: windows-2019-msbuild + os: windows-2019 + platform: 64 + steps: - name: Checkout uses: actions/checkout@v3 @@ -97,29 +104,71 @@ jobs: echo "CC=clang" >> $GITHUB_ENV echo "CXX=clang++" >> $GITHUB_ENV + - name: Setup msbuild (Windows) + if: runner.os == 'Windows' + uses: ilammy/msvc-dev-cmd@v1 + with: + arch: x${{matrix.platform}} + + - name: Setup python (Windows) + uses: actions/setup-python@v4 + if: runner.os == 'Windows' + with: + python-version: ${{ matrix.python_version }} + + - name: Install ninja (Windows) + if: runner.os == 'Windows' + shell: bash + run: | + choco install ninja + ninja --version + where ninja + + - name: Install Boost (Windows) + if: runner.os == 'Windows' + shell: powershell + run: | + # Snippet from: https://github.com/actions/virtual-environments/issues/2667 + $BOOST_PATH = "C:\hostedtoolcache\windows\Boost\$env:BOOST_VERSION\x86_64" + + # Use the prebuilt binary for Windows + $Url = "https://sourceforge.net/projects/boost/files/boost-binaries/$env:BOOST_VERSION/$env:BOOST_EXE-${{matrix.platform}}.exe" + (New-Object System.Net.WebClient).DownloadFile($Url, "$env:TEMP\boost.exe") + Start-Process -Wait -FilePath "$env:TEMP\boost.exe" "/SILENT","/SP-","/SUPPRESSMSGBOXES","/DIR=$BOOST_PATH" + + # Set the BOOST_ROOT variable + echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV + - name: Set GTSAM_WITH_TBB Flag if: matrix.flag == 'tbb' run: | echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" - - name: Set Swap Space + - name: Set Swap Space (Linux) if: runner.os == 'Linux' uses: pierotofy/set-swap-space@master with: swap-size-gb: 6 - - name: Install System Dependencies + - name: Install System Dependencies (Linux, macOS) + if: runner.os != 'Windows' run: | bash .github/scripts/python.sh -d - name: Install Python Dependencies + shell: bash run: python$PYTHON_VERSION -m pip install -r python/dev_requirements.txt - name: Build + shell: bash run: | bash .github/scripts/python.sh -b - name: Test + # Disable running tests for windows because some of them are failing. + # Remove this condition when you want to run tests on windows CI. + if: runner.os != 'Windows' + shell: bash run: | bash .github/scripts/python.sh -t diff --git a/gtsam/base/utilities.h b/gtsam/base/utilities.h index 03e9636da..a67c5d1b6 100644 --- a/gtsam/base/utilities.h +++ b/gtsam/base/utilities.h @@ -4,6 +4,8 @@ #include #include +#include + namespace gtsam { /** * For Python __str__(). @@ -11,7 +13,7 @@ namespace gtsam { * of an object when it prints to cout. * https://stackoverflow.com/questions/5419356/redirect-stdout-stderr-to-a-string */ -struct RedirectCout { +struct GTSAM_EXPORT RedirectCout { /// constructor -- redirect stdout buffer to a stringstream buffer RedirectCout() : ssBuffer_(), coutBuffer_(std::cout.rdbuf(ssBuffer_.rdbuf())) {} diff --git a/gtsam/discrete/DiscreteValues.h b/gtsam/discrete/DiscreteValues.h index 9ec08302b..9fdff014c 100644 --- a/gtsam/discrete/DiscreteValues.h +++ b/gtsam/discrete/DiscreteValues.h @@ -126,12 +126,12 @@ inline std::vector cartesianProduct(const DiscreteKeys& keys) { } /// Free version of markdown. -std::string markdown(const DiscreteValues& values, +std::string GTSAM_EXPORT markdown(const DiscreteValues& values, const KeyFormatter& keyFormatter = DefaultKeyFormatter, const DiscreteValues::Names& names = {}); /// Free version of html. -std::string html(const DiscreteValues& values, +std::string GTSAM_EXPORT html(const DiscreteValues& values, const KeyFormatter& keyFormatter = DefaultKeyFormatter, const DiscreteValues::Names& names = {}); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 2b9c5a45a..7e05ee4da 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -396,7 +396,7 @@ class GTSAM_EXPORT Rot3 : public LieGroup { Matrix3 AdjointMap() const { return matrix(); } // Chart at origin, depends on compile-time flag ROT3_DEFAULT_COORDINATES_MODE - struct ChartAtOrigin { + struct GTSAM_EXPORT ChartAtOrigin { static Rot3 Retract(const Vector3& v, ChartJacobian H = {}); static Vector3 Local(const Rot3& r, ChartJacobian H = {}); }; diff --git a/gtsam/nonlinear/CustomFactor.h b/gtsam/nonlinear/CustomFactor.h index ac2942032..c4015db37 100644 --- a/gtsam/nonlinear/CustomFactor.h +++ b/gtsam/nonlinear/CustomFactor.h @@ -42,7 +42,7 @@ using CustomErrorFunction = std::function; * correspondence indices, from each image. * @param Length-N list of keypoints, for N images/cameras. */ -std::vector tracksFromPairwiseMatches( +std::vector GTSAM_EXPORT tracksFromPairwiseMatches( const MatchIndicesMap& matches, const KeypointsVector& keypoints, bool verbose = false); diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index f874c2f21..ba55ac2af 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -94,6 +94,14 @@ set(interface_headers set(GTSAM_PYTHON_TARGET gtsam_py) set(GTSAM_PYTHON_UNSTABLE_TARGET gtsam_unstable_py) +set(GTSAM_OUTPUT_NAME "gtsam") +set(GTSAM_UNSTABLE_OUTPUT_NAME "gtsam_unstable") + +if(MSVC) + set(GTSAM_OUTPUT_NAME "gtsam_py") + set(GTSAM_UNSTABLE_OUTPUT_NAME "gtsam_unstable_py") +endif() + pybind_wrap(${GTSAM_PYTHON_TARGET} # target "${interface_headers}" # interface_headers "gtsam.cpp" # generated_cpp @@ -109,12 +117,30 @@ pybind_wrap(${GTSAM_PYTHON_TARGET} # target set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE - OUTPUT_NAME "gtsam" + OUTPUT_NAME "${GTSAM_OUTPUT_NAME}" LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam" DEBUG_POSTFIX "" # Otherwise you will have a wrong name RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name ) +if(WIN32) + set_target_properties(${GTSAM_PYTHON_TARGET} PROPERTIES + SUFFIX ".pyd" + ) + ADD_CUSTOM_COMMAND(TARGET ${GTSAM_PYTHON_TARGET} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/${GTSAM_OUTPUT_NAME}.pyd" + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/gtsam.pyd" + ) + ADD_CUSTOM_COMMAND(TARGET ${GTSAM_PYTHON_TARGET} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "$;$" + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam/" + COMMAND_EXPAND_LISTS + VERBATIM + ) +endif() + # Set the path for the GTSAM python module set(GTSAM_MODULE_PATH ${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam) @@ -188,7 +214,7 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) set_target_properties(${GTSAM_PYTHON_UNSTABLE_TARGET} PROPERTIES INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" INSTALL_RPATH_USE_LINK_PATH TRUE - OUTPUT_NAME "gtsam_unstable" + OUTPUT_NAME "${GTSAM_UNSTABLE_OUTPUT_NAME}" LIBRARY_OUTPUT_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable" DEBUG_POSTFIX "" # Otherwise you will have a wrong name RELWITHDEBINFO_POSTFIX "" # Otherwise you will have a wrong name @@ -208,13 +234,39 @@ if(GTSAM_UNSTABLE_BUILD_PYTHON) # Add gtsam_unstable to the install target list(APPEND GTSAM_PYTHON_DEPENDENCIES ${GTSAM_PYTHON_UNSTABLE_TARGET}) - + if(WIN32) + set_target_properties(${GTSAM_PYTHON_UNSTABLE_TARGET} PROPERTIES + SUFFIX ".pyd" + ) + ADD_CUSTOM_COMMAND(TARGET ${GTSAM_PYTHON_UNSTABLE_TARGET} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/${GTSAM_UNSTABLE_OUTPUT_NAME}.pyd" + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/gtsam_unstable.pyd" + ) + ADD_CUSTOM_COMMAND(TARGET ${GTSAM_PYTHON_UNSTABLE_TARGET} POST_BUILD + COMMAND ${CMAKE_COMMAND} -E copy_if_different + "$;$" + "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/" + COMMAND_EXPAND_LISTS + VERBATIM + ) + endif() + # Custom make command to run all GTSAM_UNSTABLE Python tests + add_custom_target( + python-test-unstable + COMMAND + ${CMAKE_COMMAND} -E env # add package to python path so no need to install + "PYTHONPATH=${GTSAM_PYTHON_BUILD_DIRECTORY}/$ENV{PYTHONPATH}" + ${PYTHON_EXECUTABLE} -m unittest discover -v -s . + DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} ${GTSAM_PYTHON_TEST_FILES} + WORKING_DIRECTORY "${GTSAM_PYTHON_BUILD_DIRECTORY}/gtsam_unstable/tests" + ) endif() # Add custom target so we can install with `make python-install` set(GTSAM_PYTHON_INSTALL_TARGET python-install) add_custom_target(${GTSAM_PYTHON_INSTALL_TARGET} - COMMAND ${PYTHON_EXECUTABLE} -m pip install . + COMMAND ${PYTHON_EXECUTABLE} -m pip install --user . DEPENDS ${GTSAM_PYTHON_DEPENDENCIES} WORKING_DIRECTORY ${GTSAM_PYTHON_BUILD_DIRECTORY}) diff --git a/python/setup.py.in b/python/setup.py.in index e15e39075..824a6656e 100644 --- a/python/setup.py.in +++ b/python/setup.py.in @@ -11,7 +11,8 @@ print("PACKAGES: ", packages) package_data = { '': [ "./*.so", - "./*.dll" + "./*.dll", + "./*.pyd", ] } From 8023df456d9a4c59ce23979a2ab32953dfc520e3 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Sun, 31 Dec 2023 23:19:29 +0200 Subject: [PATCH 097/208] add windows template specialization --- .github/workflows/build-python.yml | 3 -- gtsam/nonlinear/Values-inl.h | 53 ++++++++++++++++++++++++++++++ 2 files changed, 53 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 676fe8ea0..bb10b27c3 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -166,9 +166,6 @@ jobs: bash .github/scripts/python.sh -b - name: Test - # Disable running tests for windows because some of them are failing. - # Remove this condition when you want to run tests on windows CI. - if: runner.os != 'Windows' shell: bash run: | bash .github/scripts/python.sh -t diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index a93f9570e..6da87d71c 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -197,6 +197,59 @@ namespace gtsam { } }; +#ifdef _WIN32 + // Handle dynamic matrices + template + struct handle_matrix, true> { + inline Eigen::Matrix operator()(Key j, const Value* const pointer) { + auto ptr = dynamic_cast>*>(pointer); + if (ptr) { + // value returns a const Matrix&, and the return makes a copy !!!!! + return ptr->value(); + } else { + // If a fixed matrix was stored, we end up here as well. + throw ValuesIncorrectType(j, typeid(*pointer), typeid(Eigen::Matrix)); + } + } + }; + + // Handle fixed matrices + template + struct handle_matrix, false> { + inline Eigen::Matrix operator()(Key j, const Value* const pointer) { + auto ptr = dynamic_cast>*>(pointer); + if (ptr) { + // value returns a const MatrixMN&, and the return makes a copy !!!!! + return ptr->value(); + } else { + Matrix A; + // Check if a dynamic matrix was stored + auto ptr = dynamic_cast*>(pointer); + if (ptr) { + A = ptr->value(); + } else { + // Or a dynamic vector + A = handle_matrix()(j, pointer); // will throw if not.... + } + // Yes: check size, and throw if not a match + if (A.rows() != M || A.cols() != N) + throw NoMatchFoundForFixed(M, N, A.rows(), A.cols()); + else + return A; // copy but not malloc + } + } + }; + + // Handle matrices + template + struct handle> { + Eigen::Matrix operator()(Key j, const Value* const pointer) { + return handle_matrix, + (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer); + } + }; +#endif // #ifdef _WIN32 + } // internal /* ************************************************************************* */ From 85cae70cefe58d5ffb336e8fb77f73fc83c8b753 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Thu, 4 Jan 2024 08:51:19 +0200 Subject: [PATCH 098/208] revert lines according to review comments. --- .github/scripts/python.sh | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/.github/scripts/python.sh b/.github/scripts/python.sh index c851c7e36..0c04eeec3 100644 --- a/.github/scripts/python.sh +++ b/.github/scripts/python.sh @@ -70,8 +70,16 @@ function build() function test() { - cmake --build build --target python-test - cmake --build build --target python-test-unstable + cd $GITHUB_WORKSPACE/python/gtsam/tests + $PYTHON -m unittest discover -v + cd $GITHUB_WORKSPACE + + cd $GITHUB_WORKSPACE/python/gtsam_unstable/tests + $PYTHON -m unittest discover -v + cd $GITHUB_WORKSPACE + + # cmake --build build --target python-test + # cmake --build build --target python-test-unstable } # select between build or test From d1ab94f51c979652b924b2323dd3e3e4a2f58655 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 5 Jan 2024 11:05:03 +0200 Subject: [PATCH 099/208] Add comments explain the windows workaround. --- gtsam/nonlinear/Values-inl.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 6da87d71c..1fe909a11 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -197,6 +197,8 @@ namespace gtsam { } }; +// Added this section for compile gtsam python on windows. +// msvc don't deduct the template arguments correctly, due possible bug in msvc. #ifdef _WIN32 // Handle dynamic matrices template From 3b969bf94c2e562c07e3e7c118d43dd82627cf4c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jan 2024 15:04:09 -0500 Subject: [PATCH 100/208] Squashed 'wrap/' changes from bd57210d9..cdcf23207 cdcf23207 Merge pull request #163 from borglab/dunder-methods 22d429cc2 use STL functions instead of container methods abe8818ab update pyparsing to 3.1.1 a9e896d0c add unit tests 535fe4f14 wrap supported dunder methods 079687eac parse dunder methods in interface file git-subtree-dir: wrap git-subtree-split: cdcf23207fbb03457c5d9dbfc2b0b57e515b5f3d --- gtwrap/interface_parser/classes.py | 48 +++++++++++--- gtwrap/interface_parser/function.py | 2 +- gtwrap/interface_parser/tokens.py | 1 + gtwrap/pybind_wrapper.py | 64 +++++++++++++++++++ gtwrap/template_instantiator/classes.py | 3 + requirements.txt | 2 +- tests/expected/matlab/FastSet.m | 36 +++++++++++ tests/expected/matlab/MyFactorPosePoint2.m | 12 ++-- tests/expected/matlab/class_wrapper.cpp | 74 ++++++++++++++++++---- tests/expected/python/class_pybind.cpp | 6 ++ tests/fixtures/class.i | 9 +++ tests/test_interface_parser.py | 22 +++++-- 12 files changed, 245 insertions(+), 34 deletions(-) create mode 100644 tests/expected/matlab/FastSet.m diff --git a/gtwrap/interface_parser/classes.py b/gtwrap/interface_parser/classes.py index b63a0b5eb..8967bea93 100644 --- a/gtwrap/interface_parser/classes.py +++ b/gtwrap/interface_parser/classes.py @@ -12,13 +12,14 @@ Author: Duy Nguyen Ta, Fan Jiang, Matthew Sklar, Varun Agrawal, and Frank Dellae from typing import Any, Iterable, List, Union -from pyparsing import Literal, Optional, ZeroOrMore # type: ignore +from pyparsing import ZeroOrMore # type: ignore +from pyparsing import Literal, Optional, Word, alphas from .enum import Enum from .function import ArgumentList, ReturnType from .template import Template -from .tokens import (CLASS, COLON, CONST, IDENT, LBRACE, LPAREN, OPERATOR, - RBRACE, RPAREN, SEMI_COLON, STATIC, VIRTUAL) +from .tokens import (CLASS, COLON, CONST, DUNDER, IDENT, LBRACE, LPAREN, + OPERATOR, RBRACE, RPAREN, SEMI_COLON, STATIC, VIRTUAL) from .type import TemplatedType, Typename from .utils import collect_namespaces from .variable import Variable @@ -212,6 +213,26 @@ class Operator: ) +class DunderMethod: + """Special Python double-underscore (dunder) methods, e.g. __iter__, __contains__""" + rule = ( + DUNDER # + + (Word(alphas))("name") # + + DUNDER # + + LPAREN # + + ArgumentList.rule("args_list") # + + RPAREN # + + SEMI_COLON # BR + ).setParseAction(lambda t: DunderMethod(t.name, t.args_list)) + + def __init__(self, name: str, args: ArgumentList): + self.name = name + self.args = args + + def __repr__(self) -> str: + return f"DunderMethod: __{self.name}__({self.args})" + + class Class: """ Rule to parse a class defined in the interface file. @@ -223,11 +244,13 @@ class Class: }; ``` """ + class Members: """ Rule for all the members within a class. """ - rule = ZeroOrMore(Constructor.rule # + rule = ZeroOrMore(DunderMethod.rule # + ^ Constructor.rule # ^ Method.rule # ^ StaticMethod.rule # ^ Variable.rule # @@ -235,11 +258,12 @@ class Class: ^ Enum.rule # ).setParseAction(lambda t: Class.Members(t.asList())) - def __init__(self, - members: List[Union[Constructor, Method, StaticMethod, - Variable, Operator]]): + def __init__(self, members: List[Union[Constructor, Method, + StaticMethod, Variable, + Operator, Enum, DunderMethod]]): self.ctors = [] self.methods = [] + self.dunder_methods = [] self.static_methods = [] self.properties = [] self.operators = [] @@ -251,6 +275,8 @@ class Class: self.methods.append(m) elif isinstance(m, StaticMethod): self.static_methods.append(m) + elif isinstance(m, DunderMethod): + self.dunder_methods.append(m) elif isinstance(m, Variable): self.properties.append(m) elif isinstance(m, Operator): @@ -271,8 +297,8 @@ class Class: + SEMI_COLON # BR ).setParseAction(lambda t: Class( t.template, t.is_virtual, t.name, t.parent_class, t.members.ctors, t. - members.methods, t.members.static_methods, t.members.properties, t. - members.operators, t.members.enums)) + members.methods, t.members.static_methods, t.members.dunder_methods, t. + members.properties, t.members.operators, t.members.enums)) def __init__( self, @@ -283,6 +309,7 @@ class Class: ctors: List[Constructor], methods: List[Method], static_methods: List[StaticMethod], + dunder_methods: List[DunderMethod], properties: List[Variable], operators: List[Operator], enums: List[Enum], @@ -308,6 +335,7 @@ class Class: self.ctors = ctors self.methods = methods self.static_methods = static_methods + self.dunder_methods = dunder_methods self.properties = properties self.operators = operators self.enums = enums @@ -326,6 +354,8 @@ class Class: method.parent = self for static_method in self.static_methods: static_method.parent = self + for dunder_method in self.dunder_methods: + dunder_method.parent = self for _property in self.properties: _property.parent = self diff --git a/gtwrap/interface_parser/function.py b/gtwrap/interface_parser/function.py index b40884488..5385c744f 100644 --- a/gtwrap/interface_parser/function.py +++ b/gtwrap/interface_parser/function.py @@ -82,7 +82,7 @@ class ArgumentList: return ArgumentList([]) def __repr__(self) -> str: - return repr(tuple(self.args_list)) + return ",".join([repr(x) for x in self.args_list]) def __len__(self) -> int: return len(self.args_list) diff --git a/gtwrap/interface_parser/tokens.py b/gtwrap/interface_parser/tokens.py index 02e6d82f8..11c99d19c 100644 --- a/gtwrap/interface_parser/tokens.py +++ b/gtwrap/interface_parser/tokens.py @@ -22,6 +22,7 @@ RAW_POINTER, SHARED_POINTER, REF = map(Literal, "@*&") LPAREN, RPAREN, LBRACE, RBRACE, COLON, SEMI_COLON = map(Suppress, "(){}:;") LOPBRACK, ROPBRACK, COMMA, EQUAL = map(Suppress, "<>,=") +DUNDER = Suppress(Literal("__")) # Default argument passed to functions/methods. # Allow anything up to ',' or ';' except when they diff --git a/gtwrap/pybind_wrapper.py b/gtwrap/pybind_wrapper.py index 78730a909..479c2d67d 100755 --- a/gtwrap/pybind_wrapper.py +++ b/gtwrap/pybind_wrapper.py @@ -45,6 +45,8 @@ class PybindWrapper: 'continue', 'global', 'pass' ] + self.dunder_methods = ('len', 'contains', 'iter') + # amount of indentation to add before each function/method declaration. self.method_indent = '\n' + (' ' * 8) @@ -153,6 +155,51 @@ class PybindWrapper: suffix=suffix) return ret + def _wrap_dunder(self, + method, + cpp_class, + prefix, + suffix, + method_suffix=""): + """ + Wrap a Python double-underscore (dunder) method. + + E.g. __len__() gets wrapped as `.def("__len__", [](gtsam::KeySet* self) {return self->size();})` + + Supported methods are: + - __contains__(T x) + - __len__() + - __iter__() + """ + py_method = method.name + method_suffix + args_names = method.args.names() + py_args_names = self._py_args_names(method.args) + args_signature_with_names = self._method_args_signature(method.args) + + if method.name == 'len': + function_call = "return std::distance(self->begin(), self->end());" + elif method.name == 'contains': + function_call = f"return std::find(self->begin(), self->end(), {method.args.args_list[0].name}) != self->end();" + elif method.name == 'iter': + function_call = "return py::make_iterator(self->begin(), self->end());" + + ret = ('{prefix}.def("__{py_method}__",' + '[]({self}{opt_comma}{args_signature_with_names}){{' + '{function_call}' + '}}' + '{py_args_names}){suffix}'.format( + prefix=prefix, + py_method=py_method, + self=f"{cpp_class}* self", + opt_comma=', ' if args_names else '', + args_signature_with_names=args_signature_with_names, + function_call=function_call, + py_args_names=py_args_names, + suffix=suffix, + )) + + return ret + def _wrap_method(self, method, cpp_class, @@ -235,6 +282,20 @@ class PybindWrapper: return ret + def wrap_dunder_methods(self, + methods, + cpp_class, + prefix='\n' + ' ' * 8, + suffix=''): + res = "" + for method in methods: + res += self._wrap_dunder(method=method, + cpp_class=cpp_class, + prefix=prefix, + suffix=suffix) + + return res + def wrap_methods(self, methods, cpp_class, @@ -398,6 +459,7 @@ class PybindWrapper: '{wrapped_ctors}' '{wrapped_methods}' '{wrapped_static_methods}' + '{wrapped_dunder_methods}' '{wrapped_properties}' '{wrapped_operators};\n'.format( class_declaration=class_declaration, @@ -406,6 +468,8 @@ class PybindWrapper: instantiated_class.methods, cpp_class), wrapped_static_methods=self.wrap_methods( instantiated_class.static_methods, cpp_class), + wrapped_dunder_methods=self.wrap_dunder_methods( + instantiated_class.dunder_methods, cpp_class), wrapped_properties=self.wrap_properties( instantiated_class.properties, cpp_class), wrapped_operators=self.wrap_operators( diff --git a/gtwrap/template_instantiator/classes.py b/gtwrap/template_instantiator/classes.py index ce51d5b96..702654678 100644 --- a/gtwrap/template_instantiator/classes.py +++ b/gtwrap/template_instantiator/classes.py @@ -57,6 +57,8 @@ class InstantiatedClass(parser.Class): # Instantiate all instance methods self.methods = self.instantiate_methods(typenames) + + self.dunder_methods = original.dunder_methods super().__init__( self.template, @@ -66,6 +68,7 @@ class InstantiatedClass(parser.Class): self.ctors, self.methods, self.static_methods, + self.dunder_methods, self.properties, self.operators, self.enums, diff --git a/requirements.txt b/requirements.txt index 0aac9302e..f43fdda61 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,2 +1,2 @@ -pyparsing==2.4.7 +pyparsing==3.1.1 pytest>=6.2.4 diff --git a/tests/expected/matlab/FastSet.m b/tests/expected/matlab/FastSet.m new file mode 100644 index 000000000..4d2a1813e --- /dev/null +++ b/tests/expected/matlab/FastSet.m @@ -0,0 +1,36 @@ +%class FastSet, see Doxygen page for details +%at https://gtsam.org/doxygen/ +% +%-------Constructors------- +%FastSet() +% +classdef FastSet < handle + properties + ptr_FastSet = 0 + end + methods + function obj = FastSet(varargin) + if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) + my_ptr = varargin{2}; + class_wrapper(73, my_ptr); + elseif nargin == 0 + my_ptr = class_wrapper(74); + else + error('Arguments do not match any overload of FastSet constructor'); + end + obj.ptr_FastSet = my_ptr; + end + + function delete(obj) + class_wrapper(75, obj.ptr_FastSet); + end + + function display(obj), obj.print(''); end + %DISPLAY Calls print on the object + function disp(obj), obj.display; end + %DISP Calls print on the object + end + + methods(Static = true) + end +end diff --git a/tests/expected/matlab/MyFactorPosePoint2.m b/tests/expected/matlab/MyFactorPosePoint2.m index 4a30bd489..ac5b134f9 100644 --- a/tests/expected/matlab/MyFactorPosePoint2.m +++ b/tests/expected/matlab/MyFactorPosePoint2.m @@ -15,9 +15,9 @@ classdef MyFactorPosePoint2 < handle function obj = MyFactorPosePoint2(varargin) if nargin == 2 && isa(varargin{1}, 'uint64') && varargin{1} == uint64(5139824614673773682) my_ptr = varargin{2}; - class_wrapper(73, my_ptr); + class_wrapper(76, my_ptr); elseif nargin == 4 && isa(varargin{1},'numeric') && isa(varargin{2},'numeric') && isa(varargin{3},'double') && isa(varargin{4},'gtsam.noiseModel.Base') - my_ptr = class_wrapper(74, varargin{1}, varargin{2}, varargin{3}, varargin{4}); + my_ptr = class_wrapper(77, varargin{1}, varargin{2}, varargin{3}, varargin{4}); else error('Arguments do not match any overload of MyFactorPosePoint2 constructor'); end @@ -25,7 +25,7 @@ classdef MyFactorPosePoint2 < handle end function delete(obj) - class_wrapper(75, obj.ptr_MyFactorPosePoint2); + class_wrapper(78, obj.ptr_MyFactorPosePoint2); end function display(obj), obj.print(''); end @@ -36,19 +36,19 @@ classdef MyFactorPosePoint2 < handle % PRINT usage: print(string s, KeyFormatter keyFormatter) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 2 && isa(varargin{1},'char') && isa(varargin{2},'gtsam.KeyFormatter') - class_wrapper(76, this, varargin{:}); + class_wrapper(79, this, varargin{:}); return end % PRINT usage: print(string s) : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 1 && isa(varargin{1},'char') - class_wrapper(77, this, varargin{:}); + class_wrapper(80, this, varargin{:}); return end % PRINT usage: print() : returns void % Doxygen can be found at https://gtsam.org/doxygen/ if length(varargin) == 0 - class_wrapper(78, this, varargin{:}); + class_wrapper(81, this, varargin{:}); return end error('Arguments do not match any overload of function MyFactorPosePoint2.print'); diff --git a/tests/expected/matlab/class_wrapper.cpp b/tests/expected/matlab/class_wrapper.cpp index c4be52018..e33f14238 100644 --- a/tests/expected/matlab/class_wrapper.cpp +++ b/tests/expected/matlab/class_wrapper.cpp @@ -31,6 +31,8 @@ typedef std::set*> Collector_ForwardKinematic static Collector_ForwardKinematics collector_ForwardKinematics; typedef std::set*> Collector_TemplatedConstructor; static Collector_TemplatedConstructor collector_TemplatedConstructor; +typedef std::set*> Collector_FastSet; +static Collector_FastSet collector_FastSet; typedef std::set*> Collector_MyFactorPosePoint2; static Collector_MyFactorPosePoint2 collector_MyFactorPosePoint2; @@ -101,6 +103,12 @@ void _deleteAllObjects() collector_TemplatedConstructor.erase(iter++); anyDeleted = true; } } + { for(Collector_FastSet::iterator iter = collector_FastSet.begin(); + iter != collector_FastSet.end(); ) { + delete *iter; + collector_FastSet.erase(iter++); + anyDeleted = true; + } } { for(Collector_MyFactorPosePoint2::iterator iter = collector_MyFactorPosePoint2.begin(); iter != collector_MyFactorPosePoint2.end(); ) { delete *iter; @@ -844,7 +852,40 @@ void TemplatedConstructor_deconstructor_72(int nargout, mxArray *out[], int narg delete self; } -void MyFactorPosePoint2_collectorInsertAndMakeBase_73(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void FastSet_collectorInsertAndMakeBase_73(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef std::shared_ptr Shared; + + Shared *self = *reinterpret_cast (mxGetData(in[0])); + collector_FastSet.insert(self); +} + +void FastSet_constructor_74(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + mexAtExit(&_deleteAllObjects); + typedef std::shared_ptr Shared; + + Shared *self = new Shared(new FastSet()); + collector_FastSet.insert(self); + out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL); + *reinterpret_cast (mxGetData(out[0])) = self; +} + +void FastSet_deconstructor_75(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +{ + typedef std::shared_ptr Shared; + checkArguments("delete_FastSet",nargout,nargin,1); + Shared *self = *reinterpret_cast(mxGetData(in[0])); + Collector_FastSet::iterator item; + item = collector_FastSet.find(self); + if(item != collector_FastSet.end()) { + collector_FastSet.erase(item); + } + delete self; +} + +void MyFactorPosePoint2_collectorInsertAndMakeBase_76(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef std::shared_ptr> Shared; @@ -853,7 +894,7 @@ void MyFactorPosePoint2_collectorInsertAndMakeBase_73(int nargout, mxArray *out[ collector_MyFactorPosePoint2.insert(self); } -void MyFactorPosePoint2_constructor_74(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_constructor_77(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { mexAtExit(&_deleteAllObjects); typedef std::shared_ptr> Shared; @@ -868,7 +909,7 @@ void MyFactorPosePoint2_constructor_74(int nargout, mxArray *out[], int nargin, *reinterpret_cast (mxGetData(out[0])) = self; } -void MyFactorPosePoint2_deconstructor_75(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_deconstructor_78(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { typedef std::shared_ptr> Shared; checkArguments("delete_MyFactorPosePoint2",nargout,nargin,1); @@ -881,7 +922,7 @@ void MyFactorPosePoint2_deconstructor_75(int nargout, mxArray *out[], int nargin delete self; } -void MyFactorPosePoint2_print_76(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_79(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,2); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -890,7 +931,7 @@ void MyFactorPosePoint2_print_76(int nargout, mxArray *out[], int nargin, const obj->print(s,keyFormatter); } -void MyFactorPosePoint2_print_77(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_80(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,1); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -898,7 +939,7 @@ void MyFactorPosePoint2_print_77(int nargout, mxArray *out[], int nargin, const obj->print(s,gtsam::DefaultKeyFormatter); } -void MyFactorPosePoint2_print_78(int nargout, mxArray *out[], int nargin, const mxArray *in[]) +void MyFactorPosePoint2_print_81(int nargout, mxArray *out[], int nargin, const mxArray *in[]) { checkArguments("print",nargout,nargin-1,0); auto obj = unwrap_shared_ptr>(in[0], "ptr_MyFactorPosePoint2"); @@ -1137,22 +1178,31 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[]) TemplatedConstructor_deconstructor_72(nargout, out, nargin-1, in+1); break; case 73: - MyFactorPosePoint2_collectorInsertAndMakeBase_73(nargout, out, nargin-1, in+1); + FastSet_collectorInsertAndMakeBase_73(nargout, out, nargin-1, in+1); break; case 74: - MyFactorPosePoint2_constructor_74(nargout, out, nargin-1, in+1); + FastSet_constructor_74(nargout, out, nargin-1, in+1); break; case 75: - MyFactorPosePoint2_deconstructor_75(nargout, out, nargin-1, in+1); + FastSet_deconstructor_75(nargout, out, nargin-1, in+1); break; case 76: - MyFactorPosePoint2_print_76(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_collectorInsertAndMakeBase_76(nargout, out, nargin-1, in+1); break; case 77: - MyFactorPosePoint2_print_77(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_constructor_77(nargout, out, nargin-1, in+1); break; case 78: - MyFactorPosePoint2_print_78(nargout, out, nargin-1, in+1); + MyFactorPosePoint2_deconstructor_78(nargout, out, nargin-1, in+1); + break; + case 79: + MyFactorPosePoint2_print_79(nargout, out, nargin-1, in+1); + break; + case 80: + MyFactorPosePoint2_print_80(nargout, out, nargin-1, in+1); + break; + case 81: + MyFactorPosePoint2_print_81(nargout, out, nargin-1, in+1); break; } } catch(const std::exception& e) { diff --git a/tests/expected/python/class_pybind.cpp b/tests/expected/python/class_pybind.cpp index 86d69c2e0..2292f46be 100644 --- a/tests/expected/python/class_pybind.cpp +++ b/tests/expected/python/class_pybind.cpp @@ -91,6 +91,12 @@ PYBIND11_MODULE(class_py, m_) { .def(py::init(), py::arg("arg")) .def(py::init(), py::arg("arg")); + py::class_>(m_, "FastSet") + .def(py::init<>()) + .def("__len__",[](FastSet* self){return std::distance(self->begin(), self->end());}) + .def("__contains__",[](FastSet* self, size_t key){return std::find(self->begin(), self->end(), key) != self->end();}, py::arg("key")) + .def("__iter__",[](FastSet* self){return py::make_iterator(self->begin(), self->end());}); + py::class_, std::shared_ptr>>(m_, "MyFactorPosePoint2") .def(py::init>(), py::arg("key1"), py::arg("key2"), py::arg("measured"), py::arg("noiseModel")) .def("print",[](MyFactor* self, const string& s, const gtsam::KeyFormatter& keyFormatter){ py::scoped_ostream_redirect output; self->print(s, keyFormatter);}, py::arg("s") = "factor: ", py::arg("keyFormatter") = gtsam::DefaultKeyFormatter) diff --git a/tests/fixtures/class.i b/tests/fixtures/class.i index 766f55329..775bbc737 100644 --- a/tests/fixtures/class.i +++ b/tests/fixtures/class.i @@ -145,3 +145,12 @@ class TemplatedConstructor { class SuperCoolFactor; typedef SuperCoolFactor SuperCoolFactorPose3; + +/// @brief class with dunder methods for container behavior +class FastSet { + FastSet(); + + __len__(); + __contains__(size_t key); + __iter__(); +}; \ No newline at end of file diff --git a/tests/test_interface_parser.py b/tests/test_interface_parser.py index 45415995f..2a923b3c5 100644 --- a/tests/test_interface_parser.py +++ b/tests/test_interface_parser.py @@ -18,11 +18,12 @@ import unittest sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) -from gtwrap.interface_parser import (ArgumentList, Class, Constructor, Enum, - Enumerator, ForwardDeclaration, - GlobalFunction, Include, Method, Module, - Namespace, Operator, ReturnType, - StaticMethod, TemplatedType, Type, +from gtwrap.interface_parser import (ArgumentList, Class, Constructor, + DunderMethod, Enum, Enumerator, + ForwardDeclaration, GlobalFunction, + Include, Method, Module, Namespace, + Operator, ReturnType, StaticMethod, + TemplatedType, Type, TypedefTemplateInstantiation, Typename, Variable) from gtwrap.template_instantiator.classes import InstantiatedClass @@ -344,6 +345,17 @@ class TestInterfaceParser(unittest.TestCase): self.assertEqual(1, len(ret.args)) self.assertEqual("const T & name", ret.args.args_list[0].to_cpp()) + def test_dunder_method(self): + """Test for special python dunder methods.""" + iter_string = "__iter__();" + ret = DunderMethod.rule.parse_string(iter_string)[0] + self.assertEqual("iter", ret.name) + + contains_string = "__contains__(size_t key);" + ret = DunderMethod.rule.parse_string(contains_string)[0] + self.assertEqual("contains", ret.name) + self.assertTrue(len(ret.args) == 1) + def test_operator_overload(self): """Test for operator overloading.""" # Unary operator From f4ea5511535b21b203d9c540bceb4d2c0c80145c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jan 2024 15:17:16 -0500 Subject: [PATCH 101/208] add dunder methods to interface file --- gtsam/gtsam.i | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 834d5a147..9cead2a1c 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -39,6 +39,11 @@ class KeyList { void remove(size_t key); void serialize() const; + + // Specual dunder methods for Python wrapping + __len__(); + __contains__(size_t key); + __iter__(); }; // Actually a FastSet @@ -64,6 +69,11 @@ class KeySet { bool count(size_t key) const; // returns true if value exists void serialize() const; + + // Specual dunder methods for Python wrapping + __len__(); + __contains__(size_t key); + __iter__(); }; // Actually a vector, needed for Matlab @@ -85,6 +95,11 @@ class KeyVector { void push_back(size_t key) const; void serialize() const; + + // Specual dunder methods for Python wrapping + __len__(); + __contains__(size_t key); + __iter__(); }; // Actually a FastMap From b2e4fa2112397f5120e113e90670dea6c84d6d07 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jan 2024 15:17:26 -0500 Subject: [PATCH 102/208] tests for dunder methods --- python/gtsam/tests/test_Utilities.py | 36 +++++++++++++++++++++++++++- 1 file changed, 35 insertions(+), 1 deletion(-) diff --git a/python/gtsam/tests/test_Utilities.py b/python/gtsam/tests/test_Utilities.py index 851684f12..3dd472c75 100644 --- a/python/gtsam/tests/test_Utilities.py +++ b/python/gtsam/tests/test_Utilities.py @@ -12,13 +12,14 @@ Author: Varun Agrawal import unittest import numpy as np +from gtsam.utils.test_case import GtsamTestCase import gtsam -from gtsam.utils.test_case import GtsamTestCase class TestUtilites(GtsamTestCase): """Test various GTSAM utilities.""" + def test_createKeyList(self): """Test createKeyList.""" I = [0, 1, 2] @@ -28,6 +29,17 @@ class TestUtilites(GtsamTestCase): kl = gtsam.utilities.createKeyList("s", I) self.assertEqual(kl.size(), 3) + def test_KeyList_iteration(self): + """Tests for KeyList iteration""" + I = [0, 1, 2] + kl = gtsam.utilities.createKeyList(I) + + self.assertEqual(len(kl), len(I)) + + for i, key in enumerate(kl): + self.assertTrue(key in kl) + self.assertEqual(I[i], key) + def test_createKeyVector(self): """Test createKeyVector.""" I = [0, 1, 2] @@ -37,6 +49,17 @@ class TestUtilites(GtsamTestCase): kl = gtsam.utilities.createKeyVector("s", I) self.assertEqual(len(kl), 3) + def test_KeyVector_iteration(self): + """Tests for KeyVector iteration""" + I = [0, 1, 2] + kv = gtsam.utilities.createKeyVector(I) + + self.assertEqual(len(kv), len(I)) + + for i, key in enumerate(kv): + self.assertTrue(key in kv) + self.assertEqual(I[i], key) + def test_createKeySet(self): """Test createKeySet.""" I = [0, 1, 2] @@ -46,6 +69,17 @@ class TestUtilites(GtsamTestCase): kl = gtsam.utilities.createKeySet("s", I) self.assertEqual(kl.size(), 3) + def test_KeySet_iteration(self): + """Tests for KeySet iteration""" + I = [0, 1, 2] + ks = gtsam.utilities.createKeySet(I) + + self.assertEqual(len(ks), len(I)) + + for i, key in enumerate(ks): + self.assertTrue(key in ks) + self.assertEqual(I[i], key) + def test_extractPoint2(self): """Test extractPoint2.""" initial = gtsam.Values() From b104fd66901d63ce27949a311bf2b720d78e9d75 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Tue, 16 Jan 2024 22:36:08 +0200 Subject: [PATCH 103/208] fail-fast: true --- .github/workflows/build-python.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index bb10b27c3..520e94c09 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -22,7 +22,7 @@ jobs: BOOST_EXE: boost_1_72_0-msvc-14.2 strategy: - fail-fast: false + fail-fast: true matrix: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. From 1979f027d5994893e952906e8e2d80428cac90a1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jan 2024 15:46:01 -0500 Subject: [PATCH 104/208] fix comment typo --- gtsam/gtsam.i | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 9cead2a1c..6d77e8eda 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -40,7 +40,7 @@ class KeyList { void serialize() const; - // Specual dunder methods for Python wrapping + // Special dunder methods for Python wrapping __len__(); __contains__(size_t key); __iter__(); @@ -70,7 +70,7 @@ class KeySet { void serialize() const; - // Specual dunder methods for Python wrapping + // Special dunder methods for Python wrapping __len__(); __contains__(size_t key); __iter__(); @@ -96,7 +96,7 @@ class KeyVector { void serialize() const; - // Specual dunder methods for Python wrapping + // Special dunder methods for Python wrapping __len__(); __contains__(size_t key); __iter__(); From 6cc16a3943f48992c0090e9ebe88f8c13ac7407c Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 16 Jan 2024 19:32:20 -0500 Subject: [PATCH 105/208] delete cephes_names.h to remove collisions --- gtsam/3rdparty/cephes/CMakeLists.txt | 1 - gtsam/3rdparty/cephes/cephes.h | 1 - gtsam/3rdparty/cephes/cephes/cephes_names.h | 114 -------------------- gtsam/3rdparty/cephes/cephes/mconf.h | 1 - 4 files changed, 117 deletions(-) delete mode 100644 gtsam/3rdparty/cephes/cephes/cephes_names.h diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt index 946448160..5940d39d2 100644 --- a/gtsam/3rdparty/cephes/CMakeLists.txt +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -8,7 +8,6 @@ project( set(CEPHES_HEADER_FILES cephes.h - cephes/cephes_names.h cephes/dd_idefs.h cephes/dd_real.h cephes/dd_real_idefs.h diff --git a/gtsam/3rdparty/cephes/cephes.h b/gtsam/3rdparty/cephes/cephes.h index d5b59d895..ed53e521b 100644 --- a/gtsam/3rdparty/cephes/cephes.h +++ b/gtsam/3rdparty/cephes/cephes.h @@ -1,7 +1,6 @@ #ifndef CEPHES_H #define CEPHES_H -#include "cephes/cephes_names.h" #include "dllexport.h" #ifdef __cplusplus diff --git a/gtsam/3rdparty/cephes/cephes/cephes_names.h b/gtsam/3rdparty/cephes/cephes/cephes_names.h deleted file mode 100644 index 94be8c880..000000000 --- a/gtsam/3rdparty/cephes/cephes/cephes_names.h +++ /dev/null @@ -1,114 +0,0 @@ -#ifndef CEPHES_NAMES_H -#define CEPHES_NAMES_H - -#define airy cephes_airy -#define bdtrc cephes_bdtrc -#define bdtr cephes_bdtr -#define bdtri cephes_bdtri -#define besselpoly cephes_besselpoly -#define beta cephes_beta -#define lbeta cephes_lbeta -#define btdtr cephes_btdtr -#define cbrt cephes_cbrt -#define chdtrc cephes_chdtrc -#define chbevl cephes_chbevl -#define chdtr cephes_chdtr -#define chdtri cephes_chdtri -#define dawsn cephes_dawsn -#define ellie cephes_ellie -#define ellik cephes_ellik -#define ellpe cephes_ellpe -#define ellpj cephes_ellpj -#define ellpk cephes_ellpk -#define exp10 cephes_exp10 -#define exp2 cephes_exp2 -#define expn cephes_expn -#define fdtrc cephes_fdtrc -#define fdtr cephes_fdtr -#define fdtri cephes_fdtri -#define fresnl cephes_fresnl -#define Gamma cephes_Gamma -#define lgam cephes_lgam -#define lgam_sgn cephes_lgam_sgn -#define gammasgn cephes_gammasgn -#define gdtr cephes_gdtr -#define gdtrc cephes_gdtrc -#define gdtri cephes_gdtri -#define hyp2f1 cephes_hyp2f1 -#define hyperg cephes_hyperg -#define i0 cephes_i0 -#define i0e cephes_i0e -#define i1 cephes_i1 -#define i1e cephes_i1e -#define igamc cephes_igamc -#define igam cephes_igam -#define igami cephes_igami -#define incbet cephes_incbet -#define incbi cephes_incbi -#define iv cephes_iv -#define j0 cephes_j0 -#define y0 cephes_y0 -#define j1 cephes_j1 -#define y1 cephes_y1 -#define jn cephes_jn -#define jv cephes_jv -#define k0 cephes_k0 -#define k0e cephes_k0e -#define k1 cephes_k1 -#define k1e cephes_k1e -#define kn cephes_kn -#define nbdtrc cephes_nbdtrc -#define nbdtr cephes_nbdtr -#define nbdtri cephes_nbdtri -#define ndtr cephes_ndtr -#define erfc cephes_erfc -#define erf cephes_erf -#define erfinv cephes_erfinv -#define erfcinv cephes_erfcinv -#define ndtri cephes_ndtri -#define pdtrc cephes_pdtrc -#define pdtr cephes_pdtr -#define pdtri cephes_pdtri -#define poch cephes_poch -#define psi cephes_psi -#define rgamma cephes_rgamma -#define riemann_zeta cephes_riemann_zeta -// #define round cephes_round // Commented out since it clashes with std::round -#define shichi cephes_shichi -#define sici cephes_sici -#define radian cephes_radian -#define sindg cephes_sindg -#define sinpi cephes_sinpi -#define cosdg cephes_cosdg -#define cospi cephes_cospi -#define sincos cephes_sincos -#define spence cephes_spence -#define stdtr cephes_stdtr -#define stdtri cephes_stdtri -#define struve_h cephes_struve_h -#define struve_l cephes_struve_l -#define struve_power_series cephes_struve_power_series -#define struve_asymp_large_z cephes_struve_asymp_large_z -#define struve_bessel_series cephes_struve_bessel_series -#define yv cephes_yv -#define tandg cephes_tandg -#define cotdg cephes_cotdg -#define log1p cephes_log1p -#define expm1 cephes_expm1 -#define cosm1 cephes_cosm1 -#define yn cephes_yn -#define zeta cephes_zeta -#define zetac cephes_zetac -#define smirnov cephes_smirnov -#define smirnovc cephes_smirnovc -#define smirnovi cephes_smirnovi -#define smirnovci cephes_smirnovci -#define smirnovp cephes_smirnovp -#define kolmogorov cephes_kolmogorov -#define kolmogi cephes_kolmogi -#define kolmogp cephes_kolmogp -#define kolmogc cephes_kolmogc -#define kolmogci cephes_kolmogci -#define owens_t cephes_owens_t - -#endif diff --git a/gtsam/3rdparty/cephes/cephes/mconf.h b/gtsam/3rdparty/cephes/cephes/mconf.h index c59d17a47..5e971afad 100644 --- a/gtsam/3rdparty/cephes/cephes/mconf.h +++ b/gtsam/3rdparty/cephes/cephes/mconf.h @@ -56,7 +56,6 @@ #include #include -#include "cephes_names.h" #include "cephes.h" #include "polevl.h" #include "sf_error.h" From 6680d7de4cdc8dd5ed3222cec4aad19d26da893d Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Wed, 17 Jan 2024 08:03:39 +0200 Subject: [PATCH 106/208] remove swap from python ci --- .github/workflows/build-python.yml | 6 ------ 1 file changed, 6 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 520e94c09..de9d755ba 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -145,12 +145,6 @@ jobs: echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" - - name: Set Swap Space (Linux) - if: runner.os == 'Linux' - uses: pierotofy/set-swap-space@master - with: - swap-size-gb: 6 - - name: Install System Dependencies (Linux, macOS) if: runner.os != 'Windows' run: | From 046a9f5b436a206fecc356fb36456fe1bb4c3df8 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 26 Jan 2024 11:58:14 +0200 Subject: [PATCH 107/208] Ubuntu 22.04 for python tbb. hopefully will solve the hang problem. --- .github/workflows/build-python.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index de9d755ba..d8dfce0ee 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -29,7 +29,7 @@ jobs: name: [ ubuntu-20.04-gcc-9, - ubuntu-20.04-gcc-9-tbb, + ubuntu-22.04-gcc-9-tbb, ubuntu-20.04-clang-9, macOS-11-xcode-13.4.1, windows-2019-msbuild, @@ -43,8 +43,8 @@ jobs: compiler: gcc version: "9" - - name: ubuntu-20.04-gcc-9-tbb - os: ubuntu-20.04 + - name: ubuntu-22.04-gcc-9-tbb + os: ubuntu-22.04 compiler: gcc version: "9" flag: tbb From 17a4e4c17479ea6ec05494b9cd9780a3927f6d44 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Sat, 10 Feb 2024 12:52:24 +0200 Subject: [PATCH 108/208] #if BOOST_VERSION < 108000 --- gtsam/base/std_optional_serialization.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 5c250eab4..0ab778761 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -11,6 +11,8 @@ // Defined only if boost serialization is enabled #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION +// Only for old boost +#if BOOST_VERSION < 108000 #pragma once #include #include @@ -99,3 +101,4 @@ void serialize(Archive& ar, std::optional& t, const unsigned int version) { } // namespace serialization } // namespace boost #endif +#endif From 26f57ce3b2b84a068761adb4dc5c740861a419ac Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 22 Feb 2024 15:07:24 -0500 Subject: [PATCH 109/208] wrap barometric factor --- gtsam/base/std_optional_serialization.h | 3 +++ gtsam/navigation/BarometricFactor.h | 2 +- gtsam/navigation/navigation.i | 21 +++++++++++++++++++-- 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 5c250eab4..ac0c16c87 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -56,6 +56,8 @@ namespace std { template<> struct is_trivially_move_constructible& t, const unsigned int version) { } // namespace serialization } // namespace boost #endif +#endif diff --git a/gtsam/navigation/BarometricFactor.h b/gtsam/navigation/BarometricFactor.h index 38677ed58..70cae8d36 100644 --- a/gtsam/navigation/BarometricFactor.h +++ b/gtsam/navigation/BarometricFactor.h @@ -91,7 +91,7 @@ class GTSAM_EXPORT BarometricFactor : public NoiseModelFactorN { -0.00649; } - inline double baroOut(const double& meters) { + inline double baroOut(const double& meters) const { double temp = 15.04 - 0.00649 * meters; return 101.29 * std::pow(((temp + 273.1) / 288.08), 5.256); } diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 8e6090e06..92864c18a 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -294,7 +294,7 @@ virtual class GPSFactor : gtsam::NonlinearFactor{ // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GPSFactor& expected, double tol); + bool equals(const gtsam::NonlinearFactor& expected, double tol); // Standard Interface gtsam::Point3 measurementIn() const; @@ -307,12 +307,29 @@ virtual class GPSFactor2 : gtsam::NonlinearFactor { // Testable void print(string s = "", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const; - bool equals(const gtsam::GPSFactor2& expected, double tol); + bool equals(const gtsam::NonlinearFactor& expected, double tol); // Standard Interface gtsam::Point3 measurementIn() const; }; +#include +virtual class BarometricFactor : gtsam::NonlinearFactor { + BarometricFactor(); + BarometricFactor(size_t key, size_t baroKey, const double& baroIn, + const gtsam::noiseModel::Base* model); + + // Testable + void print(string s = "", const gtsam::KeyFormatter& keyFormatter = + gtsam::DefaultKeyFormatter) const; + bool equals(const gtsam::NonlinearFactor& expected, double tol); + + // Standard Interface + const double& measurementIn() const; + double heightOut(double n) const; + double baroOut(const double& meters) const; +}; + #include virtual class Scenario { gtsam::Pose3 pose(double t) const; From a47539506b3a6893de31bc95b1b9a9b9a5149096 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Feb 2024 08:39:47 -0500 Subject: [PATCH 110/208] undo serialization header change --- gtsam/base/std_optional_serialization.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index ac0c16c87..5c250eab4 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -56,8 +56,6 @@ namespace std { template<> struct is_trivially_move_constructible& t, const unsigned int version) { } // namespace serialization } // namespace boost #endif -#endif From 9b2c4787ac7132cd04f44139da7b0286ac491434 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Feb 2024 08:44:36 -0500 Subject: [PATCH 111/208] Revert "Merge pull request #1719 from talregev/TalR/python_tbb_ubuntu_22_04" This reverts commit f724f303889cbde32b097289d64cc771a9ee4369, reversing changes made to 448132af2746ff7164db88ac2cadd93cedbd0813. --- .github/workflows/build-python.yml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index d8dfce0ee..de9d755ba 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -29,7 +29,7 @@ jobs: name: [ ubuntu-20.04-gcc-9, - ubuntu-22.04-gcc-9-tbb, + ubuntu-20.04-gcc-9-tbb, ubuntu-20.04-clang-9, macOS-11-xcode-13.4.1, windows-2019-msbuild, @@ -43,8 +43,8 @@ jobs: compiler: gcc version: "9" - - name: ubuntu-22.04-gcc-9-tbb - os: ubuntu-22.04 + - name: ubuntu-20.04-gcc-9-tbb + os: ubuntu-20.04 compiler: gcc version: "9" flag: tbb From 67cf8706d837126c96586b16e9cbbb147169a84b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Feb 2024 08:50:51 -0500 Subject: [PATCH 112/208] Revert "Merge pull request #1714 from talregev/TalR/remove_swap" This reverts commit 448132af2746ff7164db88ac2cadd93cedbd0813, reversing changes made to 2dfd15e16ce072931ee58e33866383919b38d8c0. --- .github/workflows/build-python.yml | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index de9d755ba..520e94c09 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -145,6 +145,12 @@ jobs: echo "GTSAM_WITH_TBB=ON" >> $GITHUB_ENV echo "GTSAM Uses TBB" + - name: Set Swap Space (Linux) + if: runner.os == 'Linux' + uses: pierotofy/set-swap-space@master + with: + swap-size-gb: 6 + - name: Install System Dependencies (Linux, macOS) if: runner.os != 'Windows' run: | From 0d7d159203ad5fa436b755ddd0133afd5668e0a1 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Feb 2024 09:14:39 -0500 Subject: [PATCH 113/208] fix for boost serialization collision --- gtsam/base/std_optional_serialization.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 5c250eab4..079cb7745 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -56,6 +56,7 @@ namespace std { template<> struct is_trivially_move_constructible& t, const unsigned int version) { } // namespace serialization } // namespace boost #endif +#endif From 2379cfab9a366fac203a2ef15329194cefd127d8 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 26 Feb 2024 09:35:46 -0500 Subject: [PATCH 114/208] change C++ version to check against --- gtsam/base/std_optional_serialization.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 079cb7745..34556ada4 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -56,7 +56,7 @@ namespace std { template<> struct is_trivially_move_constructible Date: Mon, 26 Feb 2024 17:04:15 -0500 Subject: [PATCH 115/208] correctly fix name collisions due to newly merged Boost PR --- gtsam/base/std_optional_serialization.h | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 34556ada4..2fc829a85 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -55,8 +55,14 @@ namespace std { template<> struct is_trivially_move_constructible Date: Thu, 29 Feb 2024 15:12:57 -0800 Subject: [PATCH 116/208] Close unmatched endif --- gtsam/base/std_optional_serialization.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 444c31703..7e30ae4d3 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -8,12 +8,12 @@ * Functionality to serialize std::optional to boost::archive * Inspired from this PR: https://github.com/boostorg/serialization/pull/163 * ---------------------------------------------------------------------------- */ +#pragma once // Defined only if boost serialization is enabled #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION // Only for old boost #if BOOST_VERSION < 108000 -#pragma once #include #include @@ -107,5 +107,6 @@ void serialize(Archive& ar, std::optional& t, const unsigned int version) { } // namespace serialization } // namespace boost -#endif -#endif +#endif // BOOST_VERSION < 108400 +#endif // BOOST_VERSION < 108000 +#endif // GTSAM_ENABLE_BOOST_SERIALIZATION From 8f17b5cfbb34a007a137b2aa4f8293f4cc3cf1b5 Mon Sep 17 00:00:00 2001 From: Ashwin Gupta Date: Fri, 1 Mar 2024 16:22:43 -0800 Subject: [PATCH 117/208] export Similarity3 specalized BetweenFactor to python --- gtsam/geometry/Similarity3.h | 12 ++++++++++++ gtsam/slam/slam.i | 3 ++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Similarity3.h b/gtsam/geometry/Similarity3.h index cd4af89bc..05bd0431e 100644 --- a/gtsam/geometry/Similarity3.h +++ b/gtsam/geometry/Similarity3.h @@ -202,6 +202,18 @@ class GTSAM_EXPORT Similarity3 : public LieGroup { /// @{ private: + + #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int /*version*/) { + ar & BOOST_SERIALIZATION_NVP(R_); + ar & BOOST_SERIALIZATION_NVP(t_); + ar & BOOST_SERIALIZATION_NVP(s_); + } + #endif + /// Calculate expmap and logmap coefficients. static Matrix3 GetV(Vector3 w, double lambda); diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 7135137bb..64977a2a5 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -7,12 +7,13 @@ namespace gtsam { #include #include #include +#include // ###### #include template virtual class BetweenFactor : gtsam::NoiseModelFactor { BetweenFactor(size_t key1, size_t key2, const T& relativePose, From 4947a101329772fbe530eb773d232ce3e5d5c744 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sun, 3 Mar 2024 10:20:29 -0500 Subject: [PATCH 118/208] remove macro block for boost version 108000 --- gtsam/base/std_optional_serialization.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/gtsam/base/std_optional_serialization.h b/gtsam/base/std_optional_serialization.h index 7e30ae4d3..7e0f2e844 100644 --- a/gtsam/base/std_optional_serialization.h +++ b/gtsam/base/std_optional_serialization.h @@ -12,8 +12,6 @@ // Defined only if boost serialization is enabled #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION -// Only for old boost -#if BOOST_VERSION < 108000 #include #include @@ -108,5 +106,4 @@ void serialize(Archive& ar, std::optional& t, const unsigned int version) { } // namespace serialization } // namespace boost #endif // BOOST_VERSION < 108400 -#endif // BOOST_VERSION < 108000 #endif // GTSAM_ENABLE_BOOST_SERIALIZATION From 5a81dc000b23bd80185db9f6141114caf91d0481 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Fri, 26 Jan 2024 01:51:24 +0200 Subject: [PATCH 119/208] fix ambiguous call to overloaded function CameraSet.h(331) --- .github/workflows/build-python.yml | 12 +++++++++--- .github/workflows/build-windows.yml | 27 ++++++++++++++++----------- gtsam/geometry/CameraSet.h | 4 ++++ gtsam/nonlinear/Values-inl.h | 2 ++ 4 files changed, 31 insertions(+), 14 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 520e94c09..ce3685f87 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -32,7 +32,7 @@ jobs: ubuntu-20.04-gcc-9-tbb, ubuntu-20.04-clang-9, macOS-11-xcode-13.4.1, - windows-2019-msbuild, + windows-2022-msbuild, ] build_type: [Release] @@ -59,8 +59,8 @@ jobs: compiler: xcode version: "13.4.1" - - name: windows-2019-msbuild - os: windows-2019 + - name: windows-2022-msbuild + os: windows-2022 platform: 64 steps: @@ -109,6 +109,12 @@ jobs: uses: ilammy/msvc-dev-cmd@v1 with: arch: x${{matrix.platform}} + toolset: 14.38 + + - name: cl version (Windows) + if: runner.os == 'Windows' + shell: cmd + run: cl - name: Setup python (Windows) uses: actions/setup-python@v4 diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index f0568394f..a73842a98 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -27,7 +27,7 @@ jobs: # Github Actions requires a single row to be added to the build matrix. # See https://help.github.com/en/articles/workflow-syntax-for-github-actions. name: [ - windows-2019-cl, + windows-2022-cl, ] build_type: [ @@ -37,12 +37,25 @@ jobs: build_unstable: [ON] include: - - name: windows-2019-cl - os: windows-2019 + - name: windows-2022-cl + os: windows-2022 compiler: cl platform: 64 steps: + - name: Checkout + uses: actions/checkout@v3 + + - name: Setup msbuild + uses: ilammy/msvc-dev-cmd@v1 + with: + arch: x${{ matrix.platform }} + toolset: 14.38 + + - name: cl version + shell: cmd + run: cl + - name: Install Dependencies shell: powershell run: | @@ -91,14 +104,6 @@ jobs: # Set the BOOST_ROOT variable echo "BOOST_ROOT=$BOOST_PATH" >> $env:GITHUB_ENV - - name: Checkout - uses: actions/checkout@v3 - - - name: Setup msbuild - uses: ilammy/msvc-dev-cmd@v1 - with: - arch: x${{ matrix.platform }} - - name: Configuration shell: bash run: | diff --git a/gtsam/geometry/CameraSet.h b/gtsam/geometry/CameraSet.h index cf4beb883..26d4952c8 100644 --- a/gtsam/geometry/CameraSet.h +++ b/gtsam/geometry/CameraSet.h @@ -327,12 +327,16 @@ class CameraSet : public std::vector> { * g = F' * (b - E * P * E' * b) * Fixed size version */ +#ifdef _WIN32 +#if _MSC_VER < 1937 template // N = 2 or 3 static SymmetricBlockMatrix SchurComplement( const FBlocks& Fs, const Matrix& E, const Eigen::Matrix& P, const Vector& b) { return SchurComplement(Fs, E, P, b); } +#endif +#endif /// Computes Point Covariance P, with lambda parameter template // N = 2 or 3 (point dimension) diff --git a/gtsam/nonlinear/Values-inl.h b/gtsam/nonlinear/Values-inl.h index 1fe909a11..59ec2089d 100644 --- a/gtsam/nonlinear/Values-inl.h +++ b/gtsam/nonlinear/Values-inl.h @@ -200,6 +200,7 @@ namespace gtsam { // Added this section for compile gtsam python on windows. // msvc don't deduct the template arguments correctly, due possible bug in msvc. #ifdef _WIN32 +#if _MSC_VER < 1937 // Handle dynamic matrices template struct handle_matrix, true> { @@ -250,6 +251,7 @@ namespace gtsam { (M == Eigen::Dynamic || N == Eigen::Dynamic)>()(j, pointer); } }; +#endif // #if _MSC_VER < 1937 #endif // #ifdef _WIN32 } // internal From 865288a4dd1b5fd8b9255f044c0f8d2195341f56 Mon Sep 17 00:00:00 2001 From: Michal Nowicki Date: Mon, 18 Mar 2024 08:47:11 +0100 Subject: [PATCH 120/208] Fixing strength of bias constraints in IMUKittiExampleGPS.cpp --- examples/IMUKittiExampleGPS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/IMUKittiExampleGPS.cpp b/examples/IMUKittiExampleGPS.cpp index cb60b2516..66ebba8b2 100644 --- a/examples/IMUKittiExampleGPS.cpp +++ b/examples/IMUKittiExampleGPS.cpp @@ -241,7 +241,6 @@ int main(int argc, char* argv[]) { "-- Starting main loop: inference is performed at each time step, but we " "plot trajectory every 10 steps\n"); size_t j = 0; - size_t included_imu_measurement_count = 0; for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { // At each non=IMU measurement we initialize a new node in the graph @@ -249,6 +248,7 @@ int main(int argc, char* argv[]) { auto current_vel_key = V(i); auto current_bias_key = B(i); double t = gps_measurements[i].time; + size_t included_imu_measurement_count = 0; if (i == first_gps_pose) { // Create initial estimate and prior on initial pose, velocity, and biases From bbdf4e1ef3edd976a5c34fdb05314c033c3a9561 Mon Sep 17 00:00:00 2001 From: Nikhil Khedekar Date: Fri, 22 Mar 2024 17:50:06 +0100 Subject: [PATCH 121/208] removed self include --- gtsam/navigation/PreintegrationCombinedParams.h | 1 - 1 file changed, 1 deletion(-) diff --git a/gtsam/navigation/PreintegrationCombinedParams.h b/gtsam/navigation/PreintegrationCombinedParams.h index 7d775d9e4..6be05c082 100644 --- a/gtsam/navigation/PreintegrationCombinedParams.h +++ b/gtsam/navigation/PreintegrationCombinedParams.h @@ -25,7 +25,6 @@ /* GTSAM includes */ #include #include -#include #include #include From afd3330acb932b78ad5b2d55238d1f7418d804e0 Mon Sep 17 00:00:00 2001 From: Adam Rutkowski Date: Mon, 25 Mar 2024 16:05:47 -0500 Subject: [PATCH 122/208] added VectorValues::sorted() --- gtsam/linear/VectorValues.cpp | 22 ++++++++++++++-------- gtsam/linear/VectorValues.h | 3 +++ 2 files changed, 17 insertions(+), 8 deletions(-) diff --git a/gtsam/linear/VectorValues.cpp b/gtsam/linear/VectorValues.cpp index 075e3b9ec..7440c0b2b 100644 --- a/gtsam/linear/VectorValues.cpp +++ b/gtsam/linear/VectorValues.cpp @@ -64,6 +64,13 @@ namespace gtsam { } } + /* ************************************************************************ */ + std::map VectorValues::sorted() const { + std::map ordered; + for (const auto& kv : *this) ordered.emplace(kv); + return ordered; + } + /* ************************************************************************ */ VectorValues VectorValues::Zero(const VectorValues& other) { @@ -130,11 +137,7 @@ namespace gtsam { GTSAM_EXPORT std::ostream& operator<<(std::ostream& os, const VectorValues& v) { // Change print depending on whether we are using TBB #ifdef GTSAM_USE_TBB - std::map sorted; - for (const auto& [key, value] : v) { - sorted.emplace(key, value); - } - for (const auto& [key, value] : sorted) + for (const auto& [key, value] : v.sorted()) #else for (const auto& [key,value] : v) #endif @@ -176,7 +179,12 @@ namespace gtsam { // Copy vectors Vector result(totalDim); DenseIndex pos = 0; +#ifdef GTSAM_USE_TBB + // TBB uses un-ordered map, so inefficiently order them: + for (const auto& [key, value] : sorted()) { +#else for (const auto& [key, value] : *this) { +#endif result.segment(pos, value.size()) = value; pos += value.size(); } @@ -392,9 +400,7 @@ namespace gtsam { // Print out all rows. #ifdef GTSAM_USE_TBB // TBB uses un-ordered map, so inefficiently order them: - std::map ordered; - for (const auto& kv : *this) ordered.emplace(kv); - for (const auto& kv : ordered) { + for (const auto& kv : sorted()) { #else for (const auto& kv : *this) { #endif diff --git a/gtsam/linear/VectorValues.h b/gtsam/linear/VectorValues.h index 478cfd770..7fbd43ffc 100644 --- a/gtsam/linear/VectorValues.h +++ b/gtsam/linear/VectorValues.h @@ -77,6 +77,9 @@ namespace gtsam { typedef ConcurrentMap Values; ///< Collection of Vectors making up a VectorValues Values values_; ///< Vectors making up this VectorValues + /** Sort by key (primarily for use with TBB, which uses an unordered map)*/ + std::map sorted() const; + public: typedef Values::iterator iterator; ///< Iterator over vector values typedef Values::const_iterator const_iterator; ///< Const iterator over vector values From 1a591ace59541ea1a24b4b356d689afef5d465fc Mon Sep 17 00:00:00 2001 From: ShuangLiu1992 Date: Sat, 29 Apr 2023 22:10:36 +0100 Subject: [PATCH 123/208] Fix JacobianFactor and HessianFactor on platforms where Eigen::Index is not the same size as gtsam::Key Fix JacobianFactor and HessianFactor on platforms where Eigen::Index is not the same size as gtsam::Key Eigen::Index is defined as std::ptrdiff_t (size_t), which is not always the same size as gtsam::Key (uint64) --- gtsam/linear/JacobianFactor.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 579f6cbc2..3cfb5ce7b 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -40,8 +40,8 @@ using namespace std; namespace gtsam { // Typedefs used in constructors below. -using Dims = std::vector; -using Pairs = std::vector>; +using Dims = std::vector; +using Pairs = std::vector>; /* ************************************************************************* */ JacobianFactor::JacobianFactor() : From 84a44eeca14c3a1a4d2d79516845ced603dd77ac Mon Sep 17 00:00:00 2001 From: ShuangLiu1992 Date: Sat, 29 Apr 2023 22:12:13 +0100 Subject: [PATCH 124/208] Update HessianFactor.cpp Fix JacobianFactor and HessianFactor on platforms where Eigen::Index is not the same size as gtsam::Key Eigen::Index is defined as std::ptrdiff_t (size_t), which is not always the same size as gtsam::Key (uint64) --- gtsam/linear/HessianFactor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 376509069..8980b5030 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -38,7 +38,7 @@ using namespace std; namespace gtsam { // Typedefs used in constructors below. -using Dims = std::vector; +using Dims = std::vector; /* ************************************************************************* */ void HessianFactor::Allocate(const Scatter& scatter) { From ac5934822e64f0b9ab4df959c39f3a20767fe564 Mon Sep 17 00:00:00 2001 From: ShuangLiu1992 Date: Sat, 29 Apr 2023 22:24:28 +0100 Subject: [PATCH 125/208] Update HessianFactor.cpp add static_cast to make compiler happy --- gtsam/linear/HessianFactor.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 8980b5030..3c3050f90 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -72,7 +72,7 @@ HessianFactor::HessianFactor() : /* ************************************************************************* */ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) - : GaussianFactor(KeyVector{j}), info_(Dims{G.cols(), 1}) { + : GaussianFactor(KeyVector{j}), info_(Dims{static_cast(G.cols()), 1}) { if (G.rows() != G.cols() || G.rows() != g.size()) throw invalid_argument( "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); @@ -85,7 +85,7 @@ HessianFactor::HessianFactor(Key j, const Matrix& G, const Vector& g, double f) // error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) = 0.5*(x'*G*x - 2*x'*G*mu + mu'*G*mu) // where G = inv(Sigma), g = G*mu, f = mu'*G*mu = mu'*g HessianFactor::HessianFactor(Key j, const Vector& mu, const Matrix& Sigma) - : GaussianFactor(KeyVector{j}), info_(Dims{Sigma.cols(), 1}) { + : GaussianFactor(KeyVector{j}), info_(Dims{static_cast(Sigma.cols()), 1}) { if (Sigma.rows() != Sigma.cols() || Sigma.rows() != mu.size()) throw invalid_argument( "Attempting to construct HessianFactor with inconsistent matrix and/or vector dimensions"); @@ -99,7 +99,7 @@ HessianFactor::HessianFactor(Key j1, Key j2, const Matrix& G11, const Matrix& G12, const Vector& g1, const Matrix& G22, const Vector& g2, double f) : GaussianFactor(KeyVector{j1,j2}), info_( - Dims{G11.cols(),G22.cols(),1}) { + Dims{static_cast(G11.cols()),static_cast(G22.cols()),1}) { info_.setDiagonalBlock(0, G11); info_.setOffDiagonalBlock(0, 1, G12); info_.setDiagonalBlock(1, G22); @@ -113,7 +113,7 @@ HessianFactor::HessianFactor(Key j1, Key j2, Key j3, const Matrix& G11, const Matrix& G23, const Vector& g2, const Matrix& G33, const Vector& g3, double f) : GaussianFactor(KeyVector{j1,j2,j3}), info_( - Dims{G11.cols(),G22.cols(),G33.cols(),1}) { + Dims{static_cast(G11.cols()),static_cast(G22.cols()),static_cast(G33.cols()),1}) { if (G11.rows() != G11.cols() || G11.rows() != G12.rows() || G11.rows() != G13.rows() || G11.rows() != g1.size() || G22.cols() != G12.cols() || G33.cols() != G13.cols() From 0fee5cb76e7a04b590ff0dc1051950da5b265340 Mon Sep 17 00:00:00 2001 From: zzodo Date: Thu, 25 Apr 2024 14:16:36 +0900 Subject: [PATCH 126/208] Add missing header in 3rdparty/cephes --- gtsam/3rdparty/cephes/CMakeLists.txt | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/3rdparty/cephes/CMakeLists.txt b/gtsam/3rdparty/cephes/CMakeLists.txt index 5940d39d2..190b73db9 100644 --- a/gtsam/3rdparty/cephes/CMakeLists.txt +++ b/gtsam/3rdparty/cephes/CMakeLists.txt @@ -16,7 +16,8 @@ set(CEPHES_HEADER_FILES cephes/lanczos.h cephes/mconf.h cephes/polevl.h - cephes/sf_error.h) + cephes/sf_error.h + dllexport.h) # Add header files install(FILES ${CEPHES_HEADER_FILES} DESTINATION include/gtsam/3rdparty/cephes) From 42b27e4484e9deea43a87b648d40b1b1db2d86e4 Mon Sep 17 00:00:00 2001 From: Vincent Richard Date: Mon, 13 May 2024 09:16:53 +0900 Subject: [PATCH 127/208] fix typo --- gtsam/navigation/PreintegrationBase.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/navigation/PreintegrationBase.cpp b/gtsam/navigation/PreintegrationBase.cpp index f573c4010..7f998987b 100644 --- a/gtsam/navigation/PreintegrationBase.cpp +++ b/gtsam/navigation/PreintegrationBase.cpp @@ -129,7 +129,7 @@ NavState PreintegrationBase::predict(const NavState& state_i, Matrix9 D_predict_state, D_predict_delta; NavState state_j = state_i.retract(xi, H1 ? &D_predict_state : nullptr, - H2 || H2 ? &D_predict_delta : nullptr); + H1 || H2 ? &D_predict_delta : nullptr); if (H1) *H1 = D_predict_state + D_predict_delta * D_delta_state; if (H2) From e116123ea58323b893d1a93041ec0dae15e6b892 Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Mon, 27 May 2024 13:31:43 -0400 Subject: [PATCH 128/208] Add a few functions to python wrapper --- gtsam/geometry/geometry.i | 10 ++++++++++ gtsam/navigation/navigation.i | 2 +- gtsam/nonlinear/nonlinear.i | 3 +++ gtsam/slam/slam.i | 3 ++- 4 files changed, 16 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/geometry.i b/gtsam/geometry/geometry.i index 01161817b..3cf78989c 100644 --- a/gtsam/geometry/geometry.i +++ b/gtsam/geometry/geometry.i @@ -69,6 +69,7 @@ class StereoPoint2 { // Standard Constructors StereoPoint2(); StereoPoint2(double uL, double uR, double v); + StereoPoint2(const gtsam::Vector3 &v); // Testable void print(string s = "") const; @@ -836,6 +837,12 @@ class Cal3_S2Stereo { Cal3_S2Stereo(double fx, double fy, double s, double u0, double v0, double b); Cal3_S2Stereo(Vector v); + // Manifold + static size_t Dim(); + size_t dim() const; + gtsam::Cal3_S2Stereo retract(Vector v) const; + Vector localCoordinates(const gtsam::Cal3_S2Stereo& c) const; + // Testable void print(string s = "") const; bool equals(const gtsam::Cal3_S2Stereo& K, double tol) const; @@ -846,8 +853,11 @@ class Cal3_S2Stereo { double skew() const; double px() const; double py() const; + Matrix K() const; gtsam::Point2 principalPoint() const; double baseline() const; + Vector6 vector() const; + Matrix inverse() const; }; #include diff --git a/gtsam/navigation/navigation.i b/gtsam/navigation/navigation.i index 92864c18a..aad6f9851 100644 --- a/gtsam/navigation/navigation.i +++ b/gtsam/navigation/navigation.i @@ -200,7 +200,7 @@ class PreintegratedCombinedMeasurements { const gtsam::imuBias::ConstantBias& bias) const; }; -virtual class CombinedImuFactor: gtsam::NonlinearFactor { +virtual class CombinedImuFactor: gtsam::NoiseModelFactor { CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, const gtsam::PreintegratedCombinedMeasurements& CombinedPreintegratedMeasurements); diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 3f5fb1dd5..c39cfd22a 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -130,6 +130,7 @@ virtual class NonlinearFactor : gtsam::Factor { virtual class NoiseModelFactor : gtsam::NonlinearFactor { bool equals(const gtsam::NoiseModelFactor& other, double tol) const; gtsam::noiseModel::Base* noiseModel() const; + gtsam::NoiseModelFactor* cloneWithNewNoiseModel(gtsam::noiseModel::Base* newNoise) const; Vector unwhitenedError(const gtsam::Values& x) const; Vector whitenedError(const gtsam::Values& x) const; }; @@ -320,6 +321,8 @@ virtual class GncParams { enum Verbosity { SILENT, SUMMARY, + MU, + WEIGHTS, VALUES }; }; diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 64977a2a5..8dc10e51c 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -170,7 +170,8 @@ template virtual class GenericStereoFactor : gtsam::NoiseModelFactor { GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, size_t poseKey, - size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); + size_t landmarkKey, const gtsam::Cal3_S2Stereo* K, + POSE body_P_sensor); gtsam::StereoPoint2 measured() const; gtsam::Cal3_S2Stereo* calibration() const; From d7e2e1cdf6e74cbf4e92b199713e797f084c8a2e Mon Sep 17 00:00:00 2001 From: Easton Potokar Date: Mon, 27 May 2024 15:58:05 -0400 Subject: [PATCH 129/208] Expanded stereofactor python constructor --- gtsam/slam/StereoFactor.h | 5 +++++ gtsam/slam/slam.i | 13 +++++++++++++ 2 files changed, 18 insertions(+) diff --git a/gtsam/slam/StereoFactor.h b/gtsam/slam/StereoFactor.h index a2d428d11..5adafcf3a 100644 --- a/gtsam/slam/StereoFactor.h +++ b/gtsam/slam/StereoFactor.h @@ -169,6 +169,11 @@ public: /** return flag for throwing cheirality exceptions */ inline bool throwCheirality() const { return throwCheirality_; } + /** return the (optional) sensor pose with respect to the vehicle frame */ + const std::optional& body_P_sensor() const { + return body_P_sensor_; + } + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ diff --git a/gtsam/slam/slam.i b/gtsam/slam/slam.i index 8dc10e51c..97b198b2f 100644 --- a/gtsam/slam/slam.i +++ b/gtsam/slam/slam.i @@ -168,10 +168,23 @@ typedef gtsam::SmartProjectionPoseFactor #include template virtual class GenericStereoFactor : gtsam::NoiseModelFactor { + GenericStereoFactor(const gtsam::StereoPoint2& measured, + const gtsam::noiseModel::Base* noiseModel, size_t poseKey, + size_t landmarkKey, const gtsam::Cal3_S2Stereo* K); GenericStereoFactor(const gtsam::StereoPoint2& measured, const gtsam::noiseModel::Base* noiseModel, size_t poseKey, size_t landmarkKey, const gtsam::Cal3_S2Stereo* K, POSE body_P_sensor); + + GenericStereoFactor(const gtsam::StereoPoint2& measured, + const gtsam::noiseModel::Base* noiseModel, size_t poseKey, + size_t landmarkKey, const gtsam::Cal3_S2Stereo* K, + bool throwCheirality, bool verboseCheirality); + GenericStereoFactor(const gtsam::StereoPoint2& measured, + const gtsam::noiseModel::Base* noiseModel, size_t poseKey, + size_t landmarkKey, const gtsam::Cal3_S2Stereo* K, + bool throwCheirality, bool verboseCheirality, + POSE body_P_sensor); gtsam::StereoPoint2 measured() const; gtsam::Cal3_S2Stereo* calibration() const; From 760b799ed8111b4413f39784c932d566290dbb19 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 29 May 2024 15:26:55 -0400 Subject: [PATCH 130/208] Add EliminateQR to Python --- gtsam/linear/linear.i | 36 ++++++++++++++++++++++++++++++------ 1 file changed, 30 insertions(+), 6 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index d4a045f09..640a1a1a4 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -127,7 +127,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { - Cauchy(double k); + Cauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); // enabling serialization functionality @@ -138,7 +138,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { - Tukey(double k); + Tukey(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::Tukey* Create(double k); // enabling serialization functionality @@ -149,7 +149,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { - Welsch(double k); + Welsch(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::Welsch* Create(double k); // enabling serialization functionality @@ -160,7 +160,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { - GemanMcClure(double c); + GemanMcClure(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); // enabling serialization functionality @@ -171,7 +171,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { }; virtual class DCS: gtsam::noiseModel::mEstimator::Base { - DCS(double c); + DCS(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::DCS* Create(double c); // enabling serialization functionality @@ -182,7 +182,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base { }; virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { - L2WithDeadZone(double k); + L2WithDeadZone(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); // enabling serialization functionality @@ -203,6 +203,28 @@ virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base { double loss(double error) const; }; +virtual class AsymmetricCauchy: gtsam::noiseModel::mEstimator::Base { + AsymmetricCauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); + static gtsam::noiseModel::mEstimator::AsymmetricCauchy* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + +virtual class TwoSidedHuberCauchy: gtsam::noiseModel::mEstimator::Base { + TwoSidedHuberCauchy(double k, double k_huber, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); + static gtsam::noiseModel::mEstimator::TwoSidedHuberCauchy* Create(double k); + + // enabling serialization functionality + void serializable() const; + + double weight(double error) const; + double loss(double error) const; +}; + virtual class Custom: gtsam::noiseModel::mEstimator::Base { Custom(gtsam::noiseModel::mEstimator::CustomWeightFunction weight, gtsam::noiseModel::mEstimator::CustomLossFunction loss, @@ -356,6 +378,8 @@ virtual class JacobianFactor : gtsam::GaussianFactor { void serialize() const; }; +pair EliminateQR(const gtsam::GaussianFactorGraph& factors, const gtsam::Ordering& keys); + #include virtual class HessianFactor : gtsam::GaussianFactor { //Constructors From cb30fddfc8aba930bd28c1d47c88effe4432ef55 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 29 May 2024 15:31:54 -0400 Subject: [PATCH 131/208] Try fixing Windows build --- .github/workflows/build-windows.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index a73842a98..766662a36 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -50,7 +50,7 @@ jobs: uses: ilammy/msvc-dev-cmd@v1 with: arch: x${{ matrix.platform }} - toolset: 14.38 + toolset: 14.2 - name: cl version shell: cmd From 02784de742858f1068b174071976b3759c8836b1 Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 29 May 2024 19:26:20 -0400 Subject: [PATCH 132/208] Try fixing Windows build --- .github/workflows/build-python.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index ce3685f87..4ce603023 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -109,7 +109,7 @@ jobs: uses: ilammy/msvc-dev-cmd@v1 with: arch: x${{matrix.platform}} - toolset: 14.38 + toolset: 14.2 - name: cl version (Windows) if: runner.os == 'Windows' From 6f408fe1c0fc5f33daa49740c98945cb9b4747be Mon Sep 17 00:00:00 2001 From: Fan Jiang Date: Wed, 29 May 2024 19:34:49 -0400 Subject: [PATCH 133/208] Remove local changes --- gtsam/linear/linear.i | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/gtsam/linear/linear.i b/gtsam/linear/linear.i index 640a1a1a4..2bf55bba1 100644 --- a/gtsam/linear/linear.i +++ b/gtsam/linear/linear.i @@ -127,6 +127,7 @@ virtual class Huber: gtsam::noiseModel::mEstimator::Base { }; virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { + Cauchy(double k); Cauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::Cauchy* Create(double k); @@ -138,6 +139,7 @@ virtual class Cauchy: gtsam::noiseModel::mEstimator::Base { }; virtual class Tukey: gtsam::noiseModel::mEstimator::Base { + Tukey(double k); Tukey(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::Tukey* Create(double k); @@ -149,6 +151,7 @@ virtual class Tukey: gtsam::noiseModel::mEstimator::Base { }; virtual class Welsch: gtsam::noiseModel::mEstimator::Base { + Welsch(double k); Welsch(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::Welsch* Create(double k); @@ -160,6 +163,7 @@ virtual class Welsch: gtsam::noiseModel::mEstimator::Base { }; virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { + GemanMcClure(double c); GemanMcClure(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::GemanMcClure* Create(double c); @@ -171,6 +175,7 @@ virtual class GemanMcClure: gtsam::noiseModel::mEstimator::Base { }; virtual class DCS: gtsam::noiseModel::mEstimator::Base { + DCS(double c); DCS(double c, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::DCS* Create(double c); @@ -182,6 +187,7 @@ virtual class DCS: gtsam::noiseModel::mEstimator::Base { }; virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { + L2WithDeadZone(double k); L2WithDeadZone(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::L2WithDeadZone* Create(double k); @@ -193,6 +199,7 @@ virtual class L2WithDeadZone: gtsam::noiseModel::mEstimator::Base { }; virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base { + AsymmetricTukey(double k); AsymmetricTukey(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::AsymmetricTukey* Create(double k); @@ -204,6 +211,7 @@ virtual class AsymmetricTukey: gtsam::noiseModel::mEstimator::Base { }; virtual class AsymmetricCauchy: gtsam::noiseModel::mEstimator::Base { + AsymmetricCauchy(double k); AsymmetricCauchy(double k, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); static gtsam::noiseModel::mEstimator::AsymmetricCauchy* Create(double k); @@ -214,17 +222,6 @@ virtual class AsymmetricCauchy: gtsam::noiseModel::mEstimator::Base { double loss(double error) const; }; -virtual class TwoSidedHuberCauchy: gtsam::noiseModel::mEstimator::Base { - TwoSidedHuberCauchy(double k, double k_huber, gtsam::noiseModel::mEstimator::Base::ReweightScheme reweight); - static gtsam::noiseModel::mEstimator::TwoSidedHuberCauchy* Create(double k); - - // enabling serialization functionality - void serializable() const; - - double weight(double error) const; - double loss(double error) const; -}; - virtual class Custom: gtsam::noiseModel::mEstimator::Base { Custom(gtsam::noiseModel::mEstimator::CustomWeightFunction weight, gtsam::noiseModel::mEstimator::CustomLossFunction loss, From 344a36c568c7fdd22d256dd57afa38ccd5701019 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 8 Jun 2024 16:11:07 -0700 Subject: [PATCH 134/208] Added clarification --- gtsam/navigation/AHRSFactor.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 39969a8f3..14548eafd 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -54,8 +54,10 @@ void PreintegratedAhrsMeasurements::integrateMeasurement( PreintegratedRotation::integrateMeasurement(measuredOmega, biasHat_, deltaT, &D_incrR_integratedOmega, &Fr); - // first order uncertainty propagation - // the deltaT allows to pass from continuous time noise to discrete time noise + // First order uncertainty propagation + // The deltaT allows to pass from continuous time noise to discrete time + // noise. Comparing with the IMUFactor.cpp implementation, the latter is an + // approximation for C * (wCov / dt) * C.transpose(), with C \approx I * dt. preintMeasCov_ = Fr * preintMeasCov_ * Fr.transpose() + p().gyroscopeCovariance * deltaT; } From e81b38114c27fea973226575d5af1351967a6fcc Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 8 Jun 2024 16:11:24 -0700 Subject: [PATCH 135/208] Ignore clangd cache --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index e3f7613fe..6c8ac97b5 100644 --- a/.gitignore +++ b/.gitignore @@ -19,3 +19,4 @@ CMakeLists.txt.user* xcode/ /Dockerfile /python/gtsam/notebooks/.ipynb_checkpoints/ellipses-checkpoint.ipynb +.cache/ From c18191d98df6fcc717d62ba7043b5d35d78713bb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 8 Jun 2024 16:11:55 -0700 Subject: [PATCH 136/208] Cleaned up tests --- gtsam/navigation/tests/testAHRSFactor.cpp | 397 ++++++++-------------- 1 file changed, 148 insertions(+), 249 deletions(-) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 89fdd4f71..5f192d9c7 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -18,12 +18,17 @@ * @author Varun Agrawal */ -#include -#include -#include -#include -#include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include #include @@ -32,33 +37,21 @@ using namespace std; using namespace gtsam; // Convenience for named keys -using symbol_shorthand::X; -using symbol_shorthand::V; using symbol_shorthand::B; +using symbol_shorthand::X; -Vector3 kZeroOmegaCoriolis(0,0,0); +Vector3 kZeroOmegaCoriolis(0, 0, 0); // Define covariance matrices -double accNoiseVar = 0.01; -const Matrix3 kMeasuredAccCovariance = accNoiseVar * I_3x3; +double gyroNoiseVar = 0.01; +const Matrix3 kMeasuredOmegaCovariance = gyroNoiseVar * I_3x3; //****************************************************************************** namespace { -Vector callEvaluateError(const AHRSFactor& factor, const Rot3 rot_i, - const Rot3 rot_j, const Vector3& bias) { - return factor.evaluateError(rot_i, rot_j, bias); -} - -Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i, - const Rot3 rot_j, const Vector3& bias) { - return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3)); -} - -PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements( - const Vector3& bias, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3::Zero()) { - PreintegratedAhrsMeasurements result(bias, I_3x3); +PreintegratedAhrsMeasurements integrateMeasurements( + const Vector3& biasHat, const list& measuredOmegas, + const list& deltaTs) { + PreintegratedAhrsMeasurements result(biasHat, I_3x3); list::const_iterator itOmega = measuredOmegas.begin(); list::const_iterator itDeltaT = deltaTs.begin(); @@ -68,79 +61,59 @@ PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements( return result; } - -Rot3 evaluatePreintegratedMeasurementsRotation( - const Vector3& bias, const list& measuredOmegas, - const list& deltaTs, - const Vector3& initialRotationRate = Vector3::Zero()) { - return Rot3( - evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs, - initialRotationRate).deltaRij()); -} - -Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega, - const double deltaT) { - return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); -} - -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { - return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); -} -} +} // namespace //****************************************************************************** -TEST( AHRSFactor, PreintegratedAhrsMeasurements ) { +TEST(AHRSFactor, PreintegratedAhrsMeasurements) { // Linearization point - Vector3 bias(0,0,0); ///< Current estimate of angular rate bias + Vector3 biasHat(0, 0, 0); ///< Current estimate of angular rate bias // Measurements Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0); double deltaT = 0.5; // Expected preintegrated values - Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI / 100.0, 0.0, 0.0); - double expectedDeltaT1(0.5); + Rot3 expectedDeltaR1 = Rot3::Roll(0.5 * M_PI / 100.0); // Actual preintegrated values - PreintegratedAhrsMeasurements actual1(bias, Z_3x3); + PreintegratedAhrsMeasurements actual1(biasHat, kMeasuredOmegaCovariance); actual1.integrateMeasurement(measuredOmega, deltaT); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); - DOUBLES_EQUAL(expectedDeltaT1, actual1.deltaTij(), 1e-6); + DOUBLES_EQUAL(deltaT, actual1.deltaTij(), 1e-6); + + // Check the covariance + Matrix3 expectedMeasCov = kMeasuredOmegaCovariance * deltaT; + EXPECT(assert_equal(expectedMeasCov, actual1.preintMeasCov(), 1e-6)); // Integrate again - Rot3 expectedDeltaR2 = Rot3::RzRyRx(2.0 * 0.5 * M_PI / 100.0, 0.0, 0.0); - double expectedDeltaT2(1); + Rot3 expectedDeltaR2 = Rot3::Roll(2.0 * 0.5 * M_PI / 100.0); // Actual preintegrated values PreintegratedAhrsMeasurements actual2 = actual1; actual2.integrateMeasurement(measuredOmega, deltaT); EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6)); - DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); + DOUBLES_EQUAL(deltaT * 2, actual2.deltaTij(), 1e-6); } //****************************************************************************** -TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) { - Matrix3 gyroscopeCovariance = Matrix3::Ones()*0.4; +TEST(AHRSFactor, PreintegratedAhrsMeasurementsConstructor) { + Matrix3 gyroscopeCovariance = I_3x3 * 0.4; Vector3 omegaCoriolis(0.1, 0.5, 0.9); PreintegratedRotationParams params(gyroscopeCovariance, omegaCoriolis); - Vector3 bias(1.0,2.0,3.0); ///< Current estimate of angular rate bias + Vector3 bias(1.0, 2.0, 3.0); ///< Current estimate of angular rate bias Rot3 deltaRij(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); double deltaTij = 0.02; - Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5; - Matrix3 preintMeasCov = Matrix3::Ones()*0.2; + Matrix3 delRdelBiasOmega = I_3x3 * 0.5; + Matrix3 preintMeasCov = I_3x3 * 0.2; PreintegratedAhrsMeasurements actualPim( - std::make_shared(params), - bias, - deltaTij, - deltaRij, - delRdelBiasOmega, - preintMeasCov); + std::make_shared(params), bias, deltaTij, + deltaRij, delRdelBiasOmega, preintMeasCov); EXPECT(assert_equal(gyroscopeCovariance, - actualPim.p().getGyroscopeCovariance(), 1e-6)); - EXPECT(assert_equal(omegaCoriolis, - *(actualPim.p().getOmegaCoriolis()), 1e-6)); + actualPim.p().getGyroscopeCovariance(), 1e-6)); + EXPECT( + assert_equal(omegaCoriolis, *(actualPim.p().getOmegaCoriolis()), 1e-6)); EXPECT(assert_equal(bias, actualPim.biasHat(), 1e-6)); DOUBLES_EQUAL(deltaTij, actualPim.deltaTij(), 1e-6); EXPECT(assert_equal(deltaRij, Rot3(actualPim.deltaRij()), 1e-6)); @@ -151,198 +124,148 @@ TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) { /* ************************************************************************* */ TEST(AHRSFactor, Error) { // Linearization point - Vector3 bias(0.,0.,0.); // Bias - Rot3 x1(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); - Rot3 x2(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0)); + Vector3 bias(0., 0., 0.); // Bias + Rot3 Ri(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); + Rot3 Rj(Rot3::RzRyRx(M_PI / 12.0 + M_PI / 100.0, M_PI / 6.0, M_PI / 4.0)); // Measurements - Vector3 measuredOmega; - measuredOmega << M_PI / 100, 0, 0; + Vector3 measuredOmega(M_PI / 100, 0, 0); double deltaT = 1.0; - PreintegratedAhrsMeasurements pim(bias, Z_3x3); + PreintegratedAhrsMeasurements pim(bias, kMeasuredOmegaCovariance); pim.integrateMeasurement(measuredOmega, deltaT); // Create factor AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, {}); - Vector3 errorActual = factor.evaluateError(x1, x2, bias); - - // Expected error - Vector3 errorExpected(3); - errorExpected << 0, 0, 0; + // Check value + Vector3 errorActual = factor.evaluateError(Ri, Rj, bias); + Vector3 errorExpected(0, 0, 0); EXPECT(assert_equal(Vector(errorExpected), Vector(errorActual), 1e-6)); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); - Matrix H2e = numericalDerivative11( - std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); - Matrix H3e = numericalDerivative11( - std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); - - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); - Matrix RH2e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); - - // Actual Jacobians - Matrix H1a, H2a, H3a; - (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a); - - // rotations - EXPECT(assert_equal(RH1e, H1a, 1e-5)); - // 1e-5 needs to be added only when using quaternions for rotations - - EXPECT(assert_equal(H2e, H2a, 1e-5)); - - // rotations - EXPECT(assert_equal(RH2e, H2a, 1e-5)); - // 1e-5 needs to be added only when using quaternions for rotations - - EXPECT(assert_equal(H3e, H3a, 1e-5)); - // 1e-5 needs to be added only when using quaternions for rotations + // Check Derivatives + Values values; + values.insert(X(1), Ri); + values.insert(X(2), Rj); + values.insert(B(1), bias); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } /* ************************************************************************* */ TEST(AHRSFactor, ErrorWithBiases) { // Linearization point - Vector3 bias(0, 0, 0.3); - Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0))); - Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); + Rot3 Ri(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0))); + Rot3 Rj(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); // Measurements - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3); double deltaT = 1.0; - - PreintegratedAhrsMeasurements pim(Vector3(0,0,0), - Z_3x3); + PreintegratedAhrsMeasurements pim(Vector3(0, 0, 0), kMeasuredOmegaCovariance); pim.integrateMeasurement(measuredOmega, deltaT); // Create factor AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); - Vector errorActual = factor.evaluateError(x1, x2, bias); - - // Expected error - Vector errorExpected(3); - errorExpected << 0, 0, 0; + // Check value + Vector3 errorExpected(0, 0, 0); + Vector3 errorActual = factor.evaluateError(Ri, Rj, bias); EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); - Matrix H2e = numericalDerivative11( - std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); - Matrix H3e = numericalDerivative11( - std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); - - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); - Matrix RH2e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); - Matrix RH3e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias); - - // Actual Jacobians - Matrix H1a, H2a, H3a; - (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a); - - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); + // Check Derivatives + Values values; + values.insert(X(1), Ri); + values.insert(X(2), Rj); + values.insert(B(1), bias); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } //****************************************************************************** -TEST( AHRSFactor, PartialDerivativeExpmap ) { +TEST(AHRSFactor, PartialDerivativeExpmap) { // Linearization point - Vector3 biasOmega(0,0,0); + Vector3 biasOmega(0, 0, 0); // Measurements - Vector3 measuredOmega; - measuredOmega << 0.1, 0, 0; + Vector3 measuredOmega(0.1, 0, 0); double deltaT = 0.5; + auto f = [&](const Vector3& biasOmega) { + return Rot3::Expmap((measuredOmega - biasOmega) * deltaT); + }; + // Compute numerical derivatives - Matrix expectedDelRdelBiasOmega = numericalDerivative11( - std::bind(&evaluateRotation, measuredOmega, std::placeholders::_1, deltaT), biasOmega); + Matrix expectedH = numericalDerivative11(f, biasOmega); - const Matrix3 Jr = Rot3::ExpmapDerivative( - (measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = + Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); - Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 actualH = -Jr * deltaT; // the delta bias appears with the minus sign // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); + EXPECT(assert_equal(expectedH, actualH, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations - } //****************************************************************************** -TEST( AHRSFactor, PartialDerivativeLogmap ) { +TEST(AHRSFactor, PartialDerivativeLogmap) { // Linearization point - Vector3 thetahat; - thetahat << 0.1, 0.1, 0; ///< Current estimate of rotation rate bias + Vector3 thetaHat(0.1, 0.1, 0); ///< Current estimate of rotation rate bias - // Measurements - Vector3 deltatheta; - deltatheta << 0, 0, 0; + auto f = [thetaHat](const Vector3 deltaTheta) { + return Rot3::Logmap( + Rot3::Expmap(thetaHat).compose(Rot3::Expmap(deltaTheta))); + }; // Compute numerical derivatives - Matrix expectedDelFdeltheta = numericalDerivative11( - std::bind(&evaluateLogRotation, thetahat, std::placeholders::_1), deltatheta); + Vector3 deltaTheta(0, 0, 0); + Matrix expectedH = numericalDerivative11(f, deltaTheta); - const Vector3 x = thetahat; // parametrization of so(3) - const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ - double normx = x.norm(); - const Matrix3 actualDelFdeltheta = I_3x3 + 0.5 * X - + (1 / (normx * normx) - (1 + cos(normx)) / (2 * normx * sin(normx))) * X - * X; + const Vector3 x = thetaHat; // parametrization of so(3) + const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ + double norm = x.norm(); + const Matrix3 actualH = + I_3x3 + 0.5 * X + + (1 / (norm * norm) - (1 + cos(norm)) / (2 * norm * sin(norm))) * X * X; // Compare Jacobians - EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); - + EXPECT(assert_equal(expectedH, actualH)); } //****************************************************************************** -TEST( AHRSFactor, fistOrderExponential ) { +TEST(AHRSFactor, fistOrderExponential) { // Linearization point - Vector3 biasOmega(0,0,0); + Vector3 biasOmega(0, 0, 0); // Measurements - Vector3 measuredOmega; - measuredOmega << 0.1, 0, 0; + Vector3 measuredOmega(0.1, 0, 0); double deltaT = 1.0; // change w.r.t. linearization point double alpha = 0.0; - Vector3 deltabiasOmega; - deltabiasOmega << alpha, alpha, alpha; + Vector3 deltaBiasOmega(alpha, alpha, alpha); - const Matrix3 Jr = Rot3::ExpmapDerivative( - (measuredOmega - biasOmega) * deltaT); + const Matrix3 Jr = + Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); - Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 delRdelBiasOmega = + -Jr * deltaT; // the delta bias appears with the minus sign - const Matrix expectedRot = Rot3::Expmap( - (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); + const Matrix expectedRot = + Rot3::Expmap((measuredOmega - biasOmega - deltaBiasOmega) * deltaT) + .matrix(); const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); - const Matrix3 actualRot = hatRot - * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); + const Matrix3 actualRot = + hatRot * Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix(); // Compare Jacobians EXPECT(assert_equal(expectedRot, actualRot)); } //****************************************************************************** -TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { +TEST(AHRSFactor, FirstOrderPreIntegratedMeasurements) { // Linearization point - Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias + Vector3 bias = Vector3::Zero(); ///< Current estimate of rotation rate bias Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.1, 0.1)), Point3(1, 0, 1)); @@ -361,98 +284,76 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) { // Actual preintegrated values PreintegratedAhrsMeasurements preintegrated = - evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs, - Vector3(M_PI / 100.0, 0.0, 0.0)); + integrateMeasurements(bias, measuredOmegas, deltaTs); + + auto f = [&](const Vector3& bias) { + return integrateMeasurements(bias, measuredOmegas, deltaTs).deltaRij(); + }; // Compute numerical derivatives - Matrix expectedDelRdelBias = - numericalDerivative11( - std::bind(&evaluatePreintegratedMeasurementsRotation, std::placeholders::_1, - measuredOmegas, deltaTs, Vector3(M_PI / 100.0, 0.0, 0.0)), bias); + Matrix expectedDelRdelBias = numericalDerivative11(f, bias); Matrix expectedDelRdelBiasOmega = expectedDelRdelBias.rightCols(3); // Compare Jacobians - EXPECT( - assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega(), 1e-3)); + EXPECT(assert_equal(expectedDelRdelBiasOmega, + preintegrated.delRdelBiasOmega(), 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } -#include -#include - //****************************************************************************** -TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) { - +TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) { Vector3 bias(0, 0, 0.3); - Rot3 x1(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0))); - Rot3 x2(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); + Rot3 Ri(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0))); + Rot3 Rj(Rot3::Expmap(Vector3(0, 0, M_PI / 4.0 + M_PI / 10.0))); // Measurements Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1; - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10.0 + 0.3; + Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3); double deltaT = 1.0; const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), - Point3(1, 0, 0)); + Point3(1, 0, 0)); - PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance); + PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredOmegaCovariance); pim.integrateMeasurement(measuredOmega, deltaT); // Check preintegrated covariance - EXPECT(assert_equal(kMeasuredAccCovariance, pim.preintMeasCov())); + EXPECT(assert_equal(kMeasuredOmegaCovariance, pim.preintMeasCov())); // Create factor AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis); - // Expected Jacobians - Matrix H1e = numericalDerivative11( - std::bind(&callEvaluateError, factor, std::placeholders::_1, x2, bias), x1); - Matrix H2e = numericalDerivative11( - std::bind(&callEvaluateError, factor, x1, std::placeholders::_1, bias), x2); - Matrix H3e = numericalDerivative11( - std::bind(&callEvaluateError, factor, x1, x2, std::placeholders::_1), bias); - - // Check rotation Jacobians - Matrix RH1e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, std::placeholders::_1, x2, bias), x1); - Matrix RH2e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, x1, std::placeholders::_1, bias), x2); - Matrix RH3e = numericalDerivative11( - std::bind(&evaluateRotationError, factor, x1, x2, std::placeholders::_1), bias); - - // Actual Jacobians - Matrix H1a, H2a, H3a; - (void) factor.evaluateError(x1, x2, bias, H1a, H2a, H3a); - - EXPECT(assert_equal(H1e, H1a)); - EXPECT(assert_equal(H2e, H2a)); - EXPECT(assert_equal(H3e, H3a)); + // Check Derivatives + Values values; + values.insert(X(1), Ri); + values.insert(X(2), Rj); + values.insert(B(1), bias); + EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } + //****************************************************************************** -TEST (AHRSFactor, predictTest) { - Vector3 bias(0,0,0); +TEST(AHRSFactor, predictTest) { + Vector3 bias(0, 0, 0); // Measurements - Vector3 measuredOmega; - measuredOmega << 0, 0, M_PI / 10.0; + Vector3 measuredOmega(0, 0, M_PI / 10.0); double deltaT = 0.2; - PreintegratedAhrsMeasurements pim(bias, kMeasuredAccCovariance); + PreintegratedAhrsMeasurements pim(bias, kMeasuredOmegaCovariance); for (int i = 0; i < 1000; ++i) { pim.integrateMeasurement(measuredOmega, deltaT); } // Check preintegrated covariance - Matrix expectedMeasCov(3,3); - expectedMeasCov = 200*kMeasuredAccCovariance; + Matrix expectedMeasCov(3, 3); + expectedMeasCov = 200 * kMeasuredOmegaCovariance; EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov())); AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); // Predict Rot3 x; - Rot3 expectedRot = Rot3::Ypr(20*M_PI, 0, 0); + Rot3 expectedRot = Rot3::Ypr(20 * M_PI, 0, 0); Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis); EXPECT(assert_equal(expectedRot, actualRot, 1e-6)); @@ -462,29 +363,27 @@ TEST (AHRSFactor, predictTest) { // Actual Jacobians Matrix H; - (void) pim.predict(bias,H); + (void)pim.predict(bias, H); EXPECT(assert_equal(expectedH, H, 1e-8)); } //****************************************************************************** -#include -#include -TEST (AHRSFactor, graphTest) { +TEST(AHRSFactor, graphTest) { // linearization point - Rot3 x1(Rot3::RzRyRx(0, 0, 0)); - Rot3 x2(Rot3::RzRyRx(0, M_PI / 4, 0)); - Vector3 bias(0,0,0); + Rot3 Ri(Rot3::RzRyRx(0, 0, 0)); + Rot3 Rj(Rot3::RzRyRx(0, M_PI / 4, 0)); + Vector3 bias(0, 0, 0); // PreIntegrator Vector3 biasHat(0, 0, 0); - PreintegratedAhrsMeasurements pim(biasHat, kMeasuredAccCovariance); + PreintegratedAhrsMeasurements pim(biasHat, kMeasuredOmegaCovariance); // Pre-integrate measurements Vector3 measuredOmega(0, M_PI / 20, 0); double deltaT = 1; // Create Factor - noiseModel::Base::shared_ptr model = // + noiseModel::Base::shared_ptr model = // noiseModel::Gaussian::Covariance(pim.preintMeasCov()); NonlinearFactorGraph graph; Values values; @@ -492,10 +391,10 @@ TEST (AHRSFactor, graphTest) { pim.integrateMeasurement(measuredOmega, deltaT); } - // pim.print("Pre integrated measurementes"); + // pim.print("Pre integrated measurements"); AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); - values.insert(X(1), x1); - values.insert(X(2), x2); + values.insert(X(1), Ri); + values.insert(X(2), Rj); values.insert(B(1), bias); graph.push_back(factor); LevenbergMarquardtOptimizer optimizer(graph, values); From d72f31fbc629dd60e5b61b9a2149d6f232867e1a Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 00:25:33 -0700 Subject: [PATCH 137/208] Actually use displacement in test --- gtsam/navigation/tests/testAHRSFactor.cpp | 32 ++++++++++++++++++++--- 1 file changed, 28 insertions(+), 4 deletions(-) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 5f192d9c7..9d512d595 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -30,6 +30,7 @@ #include #include +#include #include using namespace std::placeholders; @@ -300,6 +301,29 @@ TEST(AHRSFactor, FirstOrderPreIntegratedMeasurements) { // 1e-3 needs to be added only when using quaternions for rotations } +//****************************************************************************** +// TEST(AHRSFactor, PimWithBodyDisplacement) { +// Vector3 bias(0, 0, 0.3); +// Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3); +// double deltaT = 1.0; + +// auto p = std::make_shared(); +// p->gyroscopeCovariance = kMeasuredOmegaCovariance; +// p->body_P_sensor = Pose3(Rot3::Roll(M_PI_2), Point3(0, 0, 0)); +// PreintegratedAhrsMeasurements pim(p, Vector3::Zero()); + +// pim.integrateMeasurement(measuredOmega, deltaT); + +// Vector3 biasOmegaIncr(0.01, 0.0, 0.0); +// Matrix3 actualH; +// pim.biascorrectedDeltaRij(biasOmegaIncr, actualH); + +// // Numerical derivative using a lambda function: +// auto f = [&](const Vector3& bias) { return pim.biascorrectedDeltaRij(bias); }; +// Matrix3 expectedH = numericalDerivative11(f, bias); +// EXPECT(assert_equal(expectedH, actualH)); +// } + //****************************************************************************** TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) { Vector3 bias(0, 0, 0.3); @@ -312,10 +336,10 @@ TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) { Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3); double deltaT = 1.0; - const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), - Point3(1, 0, 0)); - - PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredOmegaCovariance); + auto p = std::make_shared(); + p->gyroscopeCovariance = kMeasuredOmegaCovariance; + p->body_P_sensor = Pose3(Rot3::Expmap(Vector3(1, 2, 3)), Point3(1, 0, 0)); + PreintegratedAhrsMeasurements pim(p, Vector3::Zero()); pim.integrateMeasurement(measuredOmega, deltaT); From d290f8326841552847fd711073e9b2708f6c6e75 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 00:25:56 -0700 Subject: [PATCH 138/208] Test incrementalRotation --- gtsam/navigation/PreintegratedRotation.cpp | 10 +- gtsam/navigation/PreintegratedRotation.h | 36 ++++-- .../tests/testPreintegratedRotation.cpp | 122 ++++++++++++++++++ 3 files changed, 154 insertions(+), 14 deletions(-) create mode 100644 gtsam/navigation/tests/testPreintegratedRotation.cpp diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index a9d4a28bb..cd8f2b64c 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -89,13 +89,13 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega, return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! } -void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, - const Vector3& biasHat, double deltaT, +void PreintegratedRotation::integrateMeasurement( + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, OptionalJacobian<3, 3> F) { Matrix3 D_incrR_integratedOmega; const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, - D_incrR_integratedOmega); + D_incrR_integratedOmega); // If asked, pass first derivative as well if (optional_D_incrR_integratedOmega) { @@ -108,8 +108,8 @@ void PreintegratedRotation::integrateMeasurement(const Vector3& measuredOmega, // Update Jacobian const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ - - D_incrR_integratedOmega * deltaT; + delRdelBiasOmega_ = + incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT; } Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr, diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index c80399f14..ed7b6f29c 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -65,7 +65,6 @@ struct GTSAM_EXPORT PreintegratedRotationParams { friend class boost::serialization::access; template void serialize(ARCHIVE & ar, const unsigned int /*version*/) { - namespace bs = ::boost::serialization; ar & BOOST_SERIALIZATION_NVP(gyroscopeCovariance); ar & BOOST_SERIALIZATION_NVP(body_P_sensor); @@ -159,15 +158,34 @@ class GTSAM_EXPORT PreintegratedRotation { /// @name Main functionality /// @{ - /// Take the gyro measurement, correct it using the (constant) bias estimate - /// and possibly the sensor pose, and then integrate it forward in time to yield - /// an incremental rotation. - Rot3 incrementalRotation(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> D_incrR_integratedOmega) const; + /** + * @brief Take the gyro measurement, correct it using the (constant) bias + * estimate and possibly the sensor pose, and then integrate it forward in + * time to yield an incremental rotation. + * @param measuredOmega The measured angular velocity (as given by the sensor) + * @param biasHat The bias estimate + * @param deltaT The time interval + * @param D_incrR_integratedOmega Jacobian of the incremental rotation w.r.t. + * delta_T * (measuredOmega - biasHat), possibly rotated by body_R_sensor. + * @return The incremental rotation + */ + Rot3 incrementalRotation( + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega) const; - /// Calculate an incremental rotation given the gyro measurement and a time interval, - /// and update both deltaTij_ and deltaRij_. - void integrateMeasurement(const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + /** + * @brief Calculate an incremental rotation given the gyro measurement and a + * time interval, and update both deltaTij_ and deltaRij_. + * @param measuredOmega The measured angular velocity (as given by the sensor) + * @param biasHat The bias estimate + * @param deltaT The time interval + * @param D_incrR_integratedOmega Optional Jacobian of the incremental + * rotation w.r.t. the integrated angular velocity + * @param F Optional Jacobian of the incremental rotation w.r.t. the bias + * estimate + */ + void integrateMeasurement(const Vector3& measuredOmega, + const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega = {}, OptionalJacobian<3, 3> F = {}); diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp new file mode 100644 index 000000000..e264a6929 --- /dev/null +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -0,0 +1,122 @@ +/* ---------------------------------------------------------------------------- + + * 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 testPreintegratedRotation.cpp + * @brief Unit test for PreintegratedRotation + * @author Frank Dellaert + */ + +#include +#include +#include + +#include + +#include "gtsam/base/Matrix.h" +#include "gtsam/base/Vector.h" + +using namespace gtsam; + +//****************************************************************************** +namespace simple_roll { +auto p = std::make_shared(); +PreintegratedRotation pim(p); +const double roll = 0.1; +const Vector3 measuredOmega(roll, 0, 0); +const Vector3 biasHat(0, 0, 0); +const double deltaT = 0.5; +} // namespace simple_roll + +//****************************************************************************** +TEST(PreintegratedRotation, incrementalRotation) { + using namespace simple_roll; + + // Check the value. + Matrix3 D_incrR_integratedOmega; + const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT, + D_incrR_integratedOmega); + Rot3 expected = Rot3::Roll(roll * deltaT); + EXPECT(assert_equal(expected, incrR, 1e-9)); + + // Lambda for numerical derivative: + auto f = [&](const Vector3& x, const Vector3& y) { + return pim.incrementalRotation(x, y, deltaT, {}); + }; + + // NOTE(frank): these derivatives as computed by the function violate the + // "Jacobian contract". We should refactor this. It's not clear that the + // deltaT factor is actually understood in calling code. + + // Check derivative with respect to measuredOmega + EXPECT(assert_equal( + numericalDerivative21(f, measuredOmega, biasHat), + deltaT * D_incrR_integratedOmega)); + + // Check derivative with respect to biasHat + EXPECT(assert_equal( + numericalDerivative22(f, measuredOmega, biasHat), + -deltaT * D_incrR_integratedOmega)); +} + +//****************************************************************************** +static std::shared_ptr paramsWithTransform() { + auto p = std::make_shared(); + p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}}); + return p; +} + +namespace roll_in_rotated_frame { +auto p = paramsWithTransform(); +PreintegratedRotation pim(p); +const double roll = 0.1; +const Vector3 measuredOmega(roll, 0, 0); +const Vector3 biasHat(0, 0, 0); +const double deltaT = 0.5; +} // namespace roll_in_rotated_frame + +//****************************************************************************** +TEST(PreintegratedRotation, incrementalRotationWithTransform) { + using namespace roll_in_rotated_frame; + + // Check the value. + Matrix3 D_incrR_integratedOmega; + const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT, + D_incrR_integratedOmega); + Rot3 expected = Rot3::Pitch(roll * deltaT); + EXPECT(assert_equal(expected, incrR, 1e-9)); + + // Lambda for numerical derivative: + auto f = [&](const Vector3& x, const Vector3& y) { + return pim.incrementalRotation(x, y, deltaT, {}); + }; + + // NOTE(frank): Here, once again, the derivatives are weird, as they do not + // take the rotation into account. They *are* the derivatives of the rotated + // omegas, but not the derivatives with respect to the function arguments. + + // Check derivative with respect to measuredOmega + EXPECT(assert_equal( + numericalDerivative21(f, measuredOmega, biasHat), + deltaT * D_incrR_integratedOmega)); + + // Check derivative with respect to biasHat + EXPECT(assert_equal( + numericalDerivative22(f, measuredOmega, biasHat), + -deltaT * D_incrR_integratedOmega)); +} + +//****************************************************************************** +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +//****************************************************************************** From 2dad81d671247c5b7d2ba8ad5688c16108193bd4 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 08:59:48 -0700 Subject: [PATCH 139/208] Function object with sane Jacobian --- gtsam/navigation/PreintegratedRotation.cpp | 21 ++--- gtsam/navigation/PreintegratedRotation.h | 48 ++++++++--- .../tests/testPreintegratedRotation.cpp | 81 +++++++++---------- 3 files changed, 83 insertions(+), 67 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index cd8f2b64c..720593558 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -68,17 +68,15 @@ bool PreintegratedRotation::equals(const PreintegratedRotation& other, && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); } -Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega, - const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> D_incrR_integratedOmega) const { - +Rot3 PreintegratedRotation::IncrementalRotation::operator()( + const Vector3& bias, OptionalJacobian<3, 3> H_bias) const { // First we compensate the measurements for the bias - Vector3 correctedOmega = measuredOmega - biasHat; + Vector3 correctedOmega = measuredOmega - bias; // Then compensate for sensor-body displacement: we express the quantities // (originally in the IMU frame) into the body frame - if (p_->body_P_sensor) { - Matrix3 body_R_sensor = p_->body_P_sensor->rotation().matrix(); + if (body_P_sensor) { + const Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); // rotation rate vector in the body frame correctedOmega = body_R_sensor * correctedOmega; } @@ -86,7 +84,10 @@ Rot3 PreintegratedRotation::incrementalRotation(const Vector3& measuredOmega, // rotation vector describing rotation increment computed from the // current rotation rate measurement const Vector3 integratedOmega = correctedOmega * deltaT; - return Rot3::Expmap(integratedOmega, D_incrR_integratedOmega); // expensive !! + Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !! + if (H_bias) + *H_bias *= -deltaT; // Correct so accurately reflects bias derivative + return incrR; } void PreintegratedRotation::integrateMeasurement( @@ -94,8 +95,8 @@ void PreintegratedRotation::integrateMeasurement( OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, OptionalJacobian<3, 3> F) { Matrix3 D_incrR_integratedOmega; - const Rot3 incrR = incrementalRotation(measuredOmega, biasHat, deltaT, - D_incrR_integratedOmega); + IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + const Rot3 incrR = f(biasHat, D_incrR_integratedOmega); // If asked, pass first derivative as well if (optional_D_incrR_integratedOmega) { diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index ed7b6f29c..d5f930063 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -21,9 +21,9 @@ #pragma once -#include #include #include +#include namespace gtsam { @@ -159,19 +159,25 @@ class GTSAM_EXPORT PreintegratedRotation { /// @{ /** - * @brief Take the gyro measurement, correct it using the (constant) bias - * estimate and possibly the sensor pose, and then integrate it forward in - * time to yield an incremental rotation. + * @brief Function object for incremental rotation. * @param measuredOmega The measured angular velocity (as given by the sensor) - * @param biasHat The bias estimate - * @param deltaT The time interval - * @param D_incrR_integratedOmega Jacobian of the incremental rotation w.r.t. - * delta_T * (measuredOmega - biasHat), possibly rotated by body_R_sensor. - * @return The incremental rotation + * @param deltaT The time interval over which the rotation is integrated. + * @param body_P_sensor Optional transform between body and IMU. */ - Rot3 incrementalRotation( - const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> D_incrR_integratedOmega) const; + struct IncrementalRotation { + const Vector3& measuredOmega; + const double deltaT; + const std::optional& body_P_sensor; + + /** + * @brief Integrate angular velocity, but corrected by bias. + * @param bias The bias estimate + * @param H_bias Jacobian of the rotation w.r.t. bias. + * @return The incremental rotation + */ + Rot3 operator()(const Vector3& biasHat, + OptionalJacobian<3, 3> H_bias = {}) const; + }; /** * @brief Calculate an incremental rotation given the gyro measurement and a @@ -198,6 +204,24 @@ class GTSAM_EXPORT PreintegratedRotation { /// @} + /// @name Deprecated API + /// @{ + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 + /// @deprecated: use IncrementalRotation functor with sane Jacobian + inline Rot3 GTSAM_DEPRECATED incrementalRotation( + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega) const { + IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + Rot3 incrR = f(biasHat, D_incrR_integratedOmega); + // Backwards compatible "weird" Jacobian, no longer used. + if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT; + return incrR; + } +#endif + + /// @} + private: #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /** Serialization function */ diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index e264a6929..89b714534 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -30,40 +30,36 @@ using namespace gtsam; namespace simple_roll { auto p = std::make_shared(); PreintegratedRotation pim(p); -const double roll = 0.1; -const Vector3 measuredOmega(roll, 0, 0); -const Vector3 biasHat(0, 0, 0); +const double omega = 0.1; +const Vector3 measuredOmega(omega, 0, 0); +const Vector3 bias(0, 0, 0); const double deltaT = 0.5; } // namespace simple_roll //****************************************************************************** -TEST(PreintegratedRotation, incrementalRotation) { +TEST(PreintegratedRotation, IncrementalRotation) { using namespace simple_roll; // Check the value. - Matrix3 D_incrR_integratedOmega; - const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT, - D_incrR_integratedOmega); - Rot3 expected = Rot3::Roll(roll * deltaT); + Matrix3 H_bias; + PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, + p->getBodyPSensor()}; + const Rot3 incrR = f(bias, H_bias); + Rot3 expected = Rot3::Roll(omega * deltaT); EXPECT(assert_equal(expected, incrR, 1e-9)); - // Lambda for numerical derivative: - auto f = [&](const Vector3& x, const Vector3& y) { + // Check the derivative: + EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + + // Ephemeral test for deprecated Jacobian: + Matrix3 D_incrR_integratedOmega; + (void)pim.incrementalRotation(measuredOmega, bias, deltaT, + D_incrR_integratedOmega); + auto g = [&](const Vector3& x, const Vector3& y) { return pim.incrementalRotation(x, y, deltaT, {}); }; - - // NOTE(frank): these derivatives as computed by the function violate the - // "Jacobian contract". We should refactor this. It's not clear that the - // deltaT factor is actually understood in calling code. - - // Check derivative with respect to measuredOmega EXPECT(assert_equal( - numericalDerivative21(f, measuredOmega, biasHat), - deltaT * D_incrR_integratedOmega)); - - // Check derivative with respect to biasHat - EXPECT(assert_equal( - numericalDerivative22(f, measuredOmega, biasHat), + numericalDerivative22(g, measuredOmega, bias), -deltaT * D_incrR_integratedOmega)); } @@ -77,40 +73,35 @@ static std::shared_ptr paramsWithTransform() { namespace roll_in_rotated_frame { auto p = paramsWithTransform(); PreintegratedRotation pim(p); -const double roll = 0.1; -const Vector3 measuredOmega(roll, 0, 0); -const Vector3 biasHat(0, 0, 0); +const double omega = 0.1; +const Vector3 measuredOmega(omega, 0, 0); +const Vector3 bias(0, 0, 0); const double deltaT = 0.5; } // namespace roll_in_rotated_frame //****************************************************************************** -TEST(PreintegratedRotation, incrementalRotationWithTransform) { +TEST(PreintegratedRotation, IncrementalRotationWithTransform) { using namespace roll_in_rotated_frame; // Check the value. - Matrix3 D_incrR_integratedOmega; - const Rot3 incrR = pim.incrementalRotation(measuredOmega, biasHat, deltaT, - D_incrR_integratedOmega); - Rot3 expected = Rot3::Pitch(roll * deltaT); - EXPECT(assert_equal(expected, incrR, 1e-9)); + Matrix3 H_bias; + PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, + p->getBodyPSensor()}; + Rot3 expected = Rot3::Pitch(omega * deltaT); + EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); - // Lambda for numerical derivative: - auto f = [&](const Vector3& x, const Vector3& y) { + // Check the derivative: + EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + + // Ephemeral test for deprecated Jacobian: + Matrix3 D_incrR_integratedOmega; + (void)pim.incrementalRotation(measuredOmega, bias, deltaT, + D_incrR_integratedOmega); + auto g = [&](const Vector3& x, const Vector3& y) { return pim.incrementalRotation(x, y, deltaT, {}); }; - - // NOTE(frank): Here, once again, the derivatives are weird, as they do not - // take the rotation into account. They *are* the derivatives of the rotated - // omegas, but not the derivatives with respect to the function arguments. - - // Check derivative with respect to measuredOmega EXPECT(assert_equal( - numericalDerivative21(f, measuredOmega, biasHat), - deltaT * D_incrR_integratedOmega)); - - // Check derivative with respect to biasHat - EXPECT(assert_equal( - numericalDerivative22(f, measuredOmega, biasHat), + numericalDerivative22(g, measuredOmega, bias), -deltaT * D_incrR_integratedOmega)); } From b531df7004a7cfe156142271972d87a734873f5f Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 10:07:57 -0700 Subject: [PATCH 140/208] Fixed derivative with transform, deprecated integrateMeasurement --- gtsam/navigation/AHRSFactor.cpp | 7 +- gtsam/navigation/PreintegratedRotation.cpp | 45 ++++-- gtsam/navigation/PreintegratedRotation.h | 26 ++-- .../tests/testPreintegratedRotation.cpp | 139 ++++++++++++++---- 4 files changed, 155 insertions(+), 62 deletions(-) diff --git a/gtsam/navigation/AHRSFactor.cpp b/gtsam/navigation/AHRSFactor.cpp index 14548eafd..f23e41ec8 100644 --- a/gtsam/navigation/AHRSFactor.cpp +++ b/gtsam/navigation/AHRSFactor.cpp @@ -49,10 +49,9 @@ void PreintegratedAhrsMeasurements::resetIntegration() { //------------------------------------------------------------------------------ void PreintegratedAhrsMeasurements::integrateMeasurement( const Vector3& measuredOmega, double deltaT) { - - Matrix3 D_incrR_integratedOmega, Fr; - PreintegratedRotation::integrateMeasurement(measuredOmega, - biasHat_, deltaT, &D_incrR_integratedOmega, &Fr); + Matrix3 Fr; + PreintegratedRotation::integrateGyroMeasurement(measuredOmega, biasHat_, + deltaT, &Fr); // First order uncertainty propagation // The deltaT allows to pass from continuous time noise to discrete time diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 720593558..bbc607800 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -74,34 +74,32 @@ Rot3 PreintegratedRotation::IncrementalRotation::operator()( Vector3 correctedOmega = measuredOmega - bias; // Then compensate for sensor-body displacement: we express the quantities - // (originally in the IMU frame) into the body frame + // (originally in the IMU frame) into the body frame. If Jacobian is + // requested, the rotation matrix is obtained as `rotate` Jacobian. + Matrix3 body_R_sensor; if (body_P_sensor) { - const Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); // rotation rate vector in the body frame - correctedOmega = body_R_sensor * correctedOmega; + correctedOmega = body_P_sensor->rotation().rotate( + correctedOmega, {}, H_bias ? &body_R_sensor : nullptr); } // rotation vector describing rotation increment computed from the // current rotation rate measurement const Vector3 integratedOmega = correctedOmega * deltaT; Rot3 incrR = Rot3::Expmap(integratedOmega, H_bias); // expensive !! - if (H_bias) + if (H_bias) { *H_bias *= -deltaT; // Correct so accurately reflects bias derivative + if (body_P_sensor) *H_bias *= body_R_sensor; + } return incrR; } -void PreintegratedRotation::integrateMeasurement( - const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, +void PreintegratedRotation::integrateGyroMeasurement( + const Vector3& measuredOmega, const Vector3& bias, double deltaT, OptionalJacobian<3, 3> F) { - Matrix3 D_incrR_integratedOmega; + Matrix3 H_bias; IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; - const Rot3 incrR = f(biasHat, D_incrR_integratedOmega); - - // If asked, pass first derivative as well - if (optional_D_incrR_integratedOmega) { - *optional_D_incrR_integratedOmega << D_incrR_integratedOmega; - } + const Rot3 incrR = f(bias, H_bias); // Update deltaTij and rotation deltaTij_ += deltaT; @@ -109,8 +107,23 @@ void PreintegratedRotation::integrateMeasurement( // Update Jacobian const Matrix3 incrRt = incrR.transpose(); - delRdelBiasOmega_ = - incrRt * delRdelBiasOmega_ - D_incrR_integratedOmega * deltaT; + delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + H_bias; +} + +// deprecated! +void PreintegratedRotation::integrateMeasurement( + const Vector3& measuredOmega, const Vector3& bias, double deltaT, + OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, + OptionalJacobian<3, 3> F) { + integrateGyroMeasurement(measuredOmega, bias, deltaT, F); + + // If asked, pass obsolete Jacobians as well + if (optional_D_incrR_integratedOmega) { + Matrix3 H_bias; + IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + const Rot3 incrR = f(bias, H_bias); + *optional_D_incrR_integratedOmega << H_bias / -deltaT; + } } Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr, diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index d5f930063..b87f4208b 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -175,7 +175,7 @@ class GTSAM_EXPORT PreintegratedRotation { * @param H_bias Jacobian of the rotation w.r.t. bias. * @return The incremental rotation */ - Rot3 operator()(const Vector3& biasHat, + Rot3 operator()(const Vector3& bias, OptionalJacobian<3, 3> H_bias = {}) const; }; @@ -183,17 +183,13 @@ class GTSAM_EXPORT PreintegratedRotation { * @brief Calculate an incremental rotation given the gyro measurement and a * time interval, and update both deltaTij_ and deltaRij_. * @param measuredOmega The measured angular velocity (as given by the sensor) - * @param biasHat The bias estimate + * @param bias The bias estimate * @param deltaT The time interval - * @param D_incrR_integratedOmega Optional Jacobian of the incremental - * rotation w.r.t. the integrated angular velocity - * @param F Optional Jacobian of the incremental rotation w.r.t. the bias - * estimate + * @param F Jacobian of internal compose, used in AhrsFactor. */ - void integrateMeasurement(const Vector3& measuredOmega, - const Vector3& biasHat, double deltaT, - OptionalJacobian<3, 3> D_incrR_integratedOmega = {}, - OptionalJacobian<3, 3> F = {}); + void integrateGyroMeasurement(const Vector3& measuredOmega, + const Vector3& bias, double deltaT, + OptionalJacobian<3, 3> F = {}); /// Return a bias corrected version of the integrated rotation, with optional Jacobian Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr, @@ -210,14 +206,20 @@ class GTSAM_EXPORT PreintegratedRotation { #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 /// @deprecated: use IncrementalRotation functor with sane Jacobian inline Rot3 GTSAM_DEPRECATED incrementalRotation( - const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, + const Vector3& measuredOmega, const Vector3& bias, double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const { IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; - Rot3 incrR = f(biasHat, D_incrR_integratedOmega); + Rot3 incrR = f(bias, D_incrR_integratedOmega); // Backwards compatible "weird" Jacobian, no longer used. if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT; return incrR; } + + /// @deprecated: returned hard-to-understand Jacobian D_incrR_integratedOmega. + void GTSAM_DEPRECATED integrateMeasurement( + const Vector3& measuredOmega, const Vector3& bias, double deltaT, + OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F); + #endif /// @} diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index 89b714534..8e7e4e9e5 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -27,18 +27,28 @@ using namespace gtsam; //****************************************************************************** -namespace simple_roll { -auto p = std::make_shared(); -PreintegratedRotation pim(p); +// Example where gyro measures small rotation about x-axis, with bias. +namespace biased_x_rotation { const double omega = 0.1; -const Vector3 measuredOmega(omega, 0, 0); -const Vector3 bias(0, 0, 0); +const Vector3 trueOmega(omega, 0, 0); +const Vector3 bias(1, 2, 3); +const Vector3 measuredOmega = trueOmega + bias; const double deltaT = 0.5; -} // namespace simple_roll +} // namespace biased_x_rotation + +// Create params where x and y axes are exchanged. +static std::shared_ptr paramsWithTransform() { + auto p = std::make_shared(); + p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}}); + return p; +} //****************************************************************************** -TEST(PreintegratedRotation, IncrementalRotation) { - using namespace simple_roll; +TEST(PreintegratedRotation, integrateMeasurement) { + // Example where IMU is identical to body frame, then omega is roll + using namespace biased_x_rotation; + auto p = std::make_shared(); + PreintegratedRotation pim(p); // Check the value. Matrix3 H_bias; @@ -51,6 +61,32 @@ TEST(PreintegratedRotation, IncrementalRotation) { // Check the derivative: EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + // Check value of deltaRij() after integration. + Matrix3 F; + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); + + // Check that system matrix F is the first derivative of compose: + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); + + // Make sure delRdelBiasOmega is H_bias after integration. + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); +} + +//****************************************************************************** +TEST(PreintegratedRotation, Deprecated) { + // Example where IMU is identical to body frame, then omega is roll + using namespace biased_x_rotation; + auto p = std::make_shared(); + PreintegratedRotation pim(p); + + // Check the value. + Matrix3 H_bias; + PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, + p->getBodyPSensor()}; + Rot3 expected = Rot3::Roll(omega * deltaT); + EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); + // Ephemeral test for deprecated Jacobian: Matrix3 D_incrR_integratedOmega; (void)pim.incrementalRotation(measuredOmega, bias, deltaT, @@ -58,30 +94,32 @@ TEST(PreintegratedRotation, IncrementalRotation) { auto g = [&](const Vector3& x, const Vector3& y) { return pim.incrementalRotation(x, y, deltaT, {}); }; - EXPECT(assert_equal( - numericalDerivative22(g, measuredOmega, bias), - -deltaT * D_incrR_integratedOmega)); -} + const Matrix3 oldJacobian = + numericalDerivative22(g, measuredOmega, bias); + EXPECT(assert_equal(oldJacobian, -deltaT * D_incrR_integratedOmega)); -//****************************************************************************** -static std::shared_ptr paramsWithTransform() { - auto p = std::make_shared(); - p->setBodyPSensor({Rot3::Yaw(M_PI_2), {0, 0, 0}}); - return p; -} + // Check deprecated version. + Matrix3 D_incrR_integratedOmega2, F; + pim.integrateMeasurement(measuredOmega, bias, deltaT, + D_incrR_integratedOmega2, F); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); -namespace roll_in_rotated_frame { -auto p = paramsWithTransform(); -PreintegratedRotation pim(p); -const double omega = 0.1; -const Vector3 measuredOmega(omega, 0, 0); -const Vector3 bias(0, 0, 0); -const double deltaT = 0.5; -} // namespace roll_in_rotated_frame + // Check that system matrix F is the first derivative of compose: + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); + + // Check that deprecated Jacobian is correct. + EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9)); + + // Make sure delRdelBiasOmega is H_bias after integration. + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); +} //****************************************************************************** TEST(PreintegratedRotation, IncrementalRotationWithTransform) { - using namespace roll_in_rotated_frame; + // Example where IMU is rotated, so measured omega indicates pitch. + using namespace biased_x_rotation; + auto p = paramsWithTransform(); + PreintegratedRotation pim(p); // Check the value. Matrix3 H_bias; @@ -93,6 +131,32 @@ TEST(PreintegratedRotation, IncrementalRotationWithTransform) { // Check the derivative: EXPECT(assert_equal(numericalDerivative11(f, bias), H_bias)); + // Check value of deltaRij() after integration. + Matrix3 F; + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT, F); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); + + // Check that system matrix F is the first derivative of compose: + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); + + // Make sure delRdelBiasOmega is H_bias after integration. + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); +} + +//****************************************************************************** +TEST(PreintegratedRotation, DeprecatedWithTransform) { + // Example where IMU is rotated, so measured omega indicates pitch. + using namespace biased_x_rotation; + auto p = paramsWithTransform(); + PreintegratedRotation pim(p); + + // Check the value. + Matrix3 H_bias; + PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, + p->getBodyPSensor()}; + Rot3 expected = Rot3::Pitch(omega * deltaT); + EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); + // Ephemeral test for deprecated Jacobian: Matrix3 D_incrR_integratedOmega; (void)pim.incrementalRotation(measuredOmega, bias, deltaT, @@ -100,9 +164,24 @@ TEST(PreintegratedRotation, IncrementalRotationWithTransform) { auto g = [&](const Vector3& x, const Vector3& y) { return pim.incrementalRotation(x, y, deltaT, {}); }; - EXPECT(assert_equal( - numericalDerivative22(g, measuredOmega, bias), - -deltaT * D_incrR_integratedOmega)); + const Matrix3 oldJacobian = + numericalDerivative22(g, measuredOmega, bias); + EXPECT(assert_equal(oldJacobian, -deltaT * D_incrR_integratedOmega)); + + // Check deprecated version. + Matrix3 D_incrR_integratedOmega2, F; + pim.integrateMeasurement(measuredOmega, bias, deltaT, + D_incrR_integratedOmega2, F); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); + + // Check that system matrix F is the first derivative of compose: + EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); + + // Check that deprecated Jacobian is correct. + EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9)); + + // Make sure delRdelBiasOmega is H_bias after integration. + EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); } //****************************************************************************** From 1ac286f97a6fb482c26d4a9c9d042c4fd065e74d Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 10:10:01 -0700 Subject: [PATCH 141/208] Removed (passing) tests of deprecated --- .../tests/testPreintegratedRotation.cpp | 86 +------------------ 1 file changed, 2 insertions(+), 84 deletions(-) diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index 8e7e4e9e5..bc51bf9d3 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -44,7 +44,7 @@ static std::shared_ptr paramsWithTransform() { } //****************************************************************************** -TEST(PreintegratedRotation, integrateMeasurement) { +TEST(PreintegratedRotation, integrateGyroMeasurement) { // Example where IMU is identical to body frame, then omega is roll using namespace biased_x_rotation; auto p = std::make_shared(); @@ -74,48 +74,7 @@ TEST(PreintegratedRotation, integrateMeasurement) { } //****************************************************************************** -TEST(PreintegratedRotation, Deprecated) { - // Example where IMU is identical to body frame, then omega is roll - using namespace biased_x_rotation; - auto p = std::make_shared(); - PreintegratedRotation pim(p); - - // Check the value. - Matrix3 H_bias; - PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, - p->getBodyPSensor()}; - Rot3 expected = Rot3::Roll(omega * deltaT); - EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); - - // Ephemeral test for deprecated Jacobian: - Matrix3 D_incrR_integratedOmega; - (void)pim.incrementalRotation(measuredOmega, bias, deltaT, - D_incrR_integratedOmega); - auto g = [&](const Vector3& x, const Vector3& y) { - return pim.incrementalRotation(x, y, deltaT, {}); - }; - const Matrix3 oldJacobian = - numericalDerivative22(g, measuredOmega, bias); - EXPECT(assert_equal(oldJacobian, -deltaT * D_incrR_integratedOmega)); - - // Check deprecated version. - Matrix3 D_incrR_integratedOmega2, F; - pim.integrateMeasurement(measuredOmega, bias, deltaT, - D_incrR_integratedOmega2, F); - EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); - - // Check that system matrix F is the first derivative of compose: - EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); - - // Check that deprecated Jacobian is correct. - EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9)); - - // Make sure delRdelBiasOmega is H_bias after integration. - EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); -} - -//****************************************************************************** -TEST(PreintegratedRotation, IncrementalRotationWithTransform) { +TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) { // Example where IMU is rotated, so measured omega indicates pitch. using namespace biased_x_rotation; auto p = paramsWithTransform(); @@ -143,47 +102,6 @@ TEST(PreintegratedRotation, IncrementalRotationWithTransform) { EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); } -//****************************************************************************** -TEST(PreintegratedRotation, DeprecatedWithTransform) { - // Example where IMU is rotated, so measured omega indicates pitch. - using namespace biased_x_rotation; - auto p = paramsWithTransform(); - PreintegratedRotation pim(p); - - // Check the value. - Matrix3 H_bias; - PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, - p->getBodyPSensor()}; - Rot3 expected = Rot3::Pitch(omega * deltaT); - EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); - - // Ephemeral test for deprecated Jacobian: - Matrix3 D_incrR_integratedOmega; - (void)pim.incrementalRotation(measuredOmega, bias, deltaT, - D_incrR_integratedOmega); - auto g = [&](const Vector3& x, const Vector3& y) { - return pim.incrementalRotation(x, y, deltaT, {}); - }; - const Matrix3 oldJacobian = - numericalDerivative22(g, measuredOmega, bias); - EXPECT(assert_equal(oldJacobian, -deltaT * D_incrR_integratedOmega)); - - // Check deprecated version. - Matrix3 D_incrR_integratedOmega2, F; - pim.integrateMeasurement(measuredOmega, bias, deltaT, - D_incrR_integratedOmega2, F); - EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); - - // Check that system matrix F is the first derivative of compose: - EXPECT(assert_equal(pim.deltaRij().inverse().AdjointMap(), F)); - - // Check that deprecated Jacobian is correct. - EXPECT(assert_equal(D_incrR_integratedOmega, D_incrR_integratedOmega2, 1e-9)); - - // Make sure delRdelBiasOmega is H_bias after integration. - EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); -} - //****************************************************************************** int main() { TestResult tr; From 0a3fdcc3a4b43e04e09fc6535f5c5625982e9303 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 12:24:45 -0700 Subject: [PATCH 142/208] biascorrectedDeltaRij tests --- gtsam/navigation/PreintegratedRotation.cpp | 10 +-- gtsam/navigation/PreintegratedRotation.h | 69 ++++++++++--------- gtsam/navigation/tests/testAHRSFactor.cpp | 23 ------- .../tests/testPreintegratedRotation.cpp | 30 ++++++++ 4 files changed, 71 insertions(+), 61 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index bbc607800..b10d8f772 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -95,11 +95,11 @@ Rot3 PreintegratedRotation::IncrementalRotation::operator()( } void PreintegratedRotation::integrateGyroMeasurement( - const Vector3& measuredOmega, const Vector3& bias, double deltaT, + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> F) { Matrix3 H_bias; IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; - const Rot3 incrR = f(bias, H_bias); + const Rot3 incrR = f(biasHat, H_bias); // Update deltaTij and rotation deltaTij_ += deltaT; @@ -112,16 +112,16 @@ void PreintegratedRotation::integrateGyroMeasurement( // deprecated! void PreintegratedRotation::integrateMeasurement( - const Vector3& measuredOmega, const Vector3& bias, double deltaT, + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, OptionalJacobian<3, 3> F) { - integrateGyroMeasurement(measuredOmega, bias, deltaT, F); + integrateGyroMeasurement(measuredOmega, biasHat, deltaT, F); // If asked, pass obsolete Jacobians as well if (optional_D_incrR_integratedOmega) { Matrix3 H_bias; IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; - const Rot3 incrR = f(bias, H_bias); + const Rot3 incrR = f(biasHat, H_bias); *optional_D_incrR_integratedOmega << H_bias / -deltaT; } } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index b87f4208b..a36674062 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -135,18 +135,10 @@ class GTSAM_EXPORT PreintegratedRotation { /// @name Access instance variables /// @{ - const std::shared_ptr& params() const { - return p_; - } - const double& deltaTij() const { - return deltaTij_; - } - const Rot3& deltaRij() const { - return deltaRij_; - } - const Matrix3& delRdelBiasOmega() const { - return delRdelBiasOmega_; - } + const std::shared_ptr& params() const { return p_; } + const double& deltaTij() const { return deltaTij_; } + const Rot3& deltaRij() const { return deltaRij_; } + const Matrix3& delRdelBiasOmega() const { return delRdelBiasOmega_; } /// @} /// @name Testable @@ -158,6 +150,35 @@ class GTSAM_EXPORT PreintegratedRotation { /// @name Main functionality /// @{ + /** + * @brief Calculate an incremental rotation given the gyro measurement and a + * time interval, and update both deltaTij_ and deltaRij_. + * @param measuredOmega The measured angular velocity (as given by the sensor) + * @param bias The biasHat estimate + * @param deltaT The time interval + * @param F Jacobian of internal compose, used in AhrsFactor. + */ + void integrateGyroMeasurement(const Vector3& measuredOmega, + const Vector3& biasHat, double deltaT, + OptionalJacobian<3, 3> F = {}); + + /** + * @brief Return a bias corrected version of the integrated rotation. + * @param biasOmegaIncr An increment with respect to biasHat used above. + * @param H Jacobian of the correction w.r.t. the bias increment. + * @note The *key* functionality of this class used in optimizing the bias. + */ + Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr, + OptionalJacobian<3, 3> H = {}) const; + + /// Integrate coriolis correction in body frame rot_i + Vector3 integrateCoriolis(const Rot3& rot_i) const; + + /// @} + + /// @name Internal, exposed for testing only + /// @{ + /** * @brief Function object for incremental rotation. * @param measuredOmega The measured angular velocity (as given by the sensor) @@ -179,25 +200,6 @@ class GTSAM_EXPORT PreintegratedRotation { OptionalJacobian<3, 3> H_bias = {}) const; }; - /** - * @brief Calculate an incremental rotation given the gyro measurement and a - * time interval, and update both deltaTij_ and deltaRij_. - * @param measuredOmega The measured angular velocity (as given by the sensor) - * @param bias The bias estimate - * @param deltaT The time interval - * @param F Jacobian of internal compose, used in AhrsFactor. - */ - void integrateGyroMeasurement(const Vector3& measuredOmega, - const Vector3& bias, double deltaT, - OptionalJacobian<3, 3> F = {}); - - /// Return a bias corrected version of the integrated rotation, with optional Jacobian - Rot3 biascorrectedDeltaRij(const Vector3& biasOmegaIncr, - OptionalJacobian<3, 3> H = {}) const; - - /// Integrate coriolis correction in body frame rot_i - Vector3 integrateCoriolis(const Rot3& rot_i) const; - /// @} /// @name Deprecated API @@ -215,9 +217,10 @@ class GTSAM_EXPORT PreintegratedRotation { return incrR; } - /// @deprecated: returned hard-to-understand Jacobian D_incrR_integratedOmega. + /// @deprecated: use integrateGyroMeasurement from now on + /// @note this returned hard-to-understand Jacobian D_incrR_integratedOmega. void GTSAM_DEPRECATED integrateMeasurement( - const Vector3& measuredOmega, const Vector3& bias, double deltaT, + const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega, OptionalJacobian<3, 3> F); #endif diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 9d512d595..95674ce78 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -301,29 +301,6 @@ TEST(AHRSFactor, FirstOrderPreIntegratedMeasurements) { // 1e-3 needs to be added only when using quaternions for rotations } -//****************************************************************************** -// TEST(AHRSFactor, PimWithBodyDisplacement) { -// Vector3 bias(0, 0, 0.3); -// Vector3 measuredOmega(0, 0, M_PI / 10.0 + 0.3); -// double deltaT = 1.0; - -// auto p = std::make_shared(); -// p->gyroscopeCovariance = kMeasuredOmegaCovariance; -// p->body_P_sensor = Pose3(Rot3::Roll(M_PI_2), Point3(0, 0, 0)); -// PreintegratedAhrsMeasurements pim(p, Vector3::Zero()); - -// pim.integrateMeasurement(measuredOmega, deltaT); - -// Vector3 biasOmegaIncr(0.01, 0.0, 0.0); -// Matrix3 actualH; -// pim.biascorrectedDeltaRij(biasOmegaIncr, actualH); - -// // Numerical derivative using a lambda function: -// auto f = [&](const Vector3& bias) { return pim.biascorrectedDeltaRij(bias); }; -// Matrix3 expectedH = numericalDerivative11(f, bias); -// EXPECT(assert_equal(expectedH, actualH)); -// } - //****************************************************************************** TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) { Vector3 bias(0, 0, 0.3); diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index bc51bf9d3..b6486d030 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -71,6 +71,22 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) { // Make sure delRdelBiasOmega is H_bias after integration. EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); + + // Check if we make a correction to the bias, the value and Jacobian are + // correct. Note that the bias is subtracted from the measurement, and the + // integration time is taken into account, so we expect -deltaT*delta change. + Matrix3 H; + const double delta = 0.05; + const Vector3 biasOmegaIncr(delta, 0, 0); + Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)); + EXPECT(assert_equal(Rot3::Roll((omega - delta) * deltaT), corrected, 1e-9)); + + // TODO(frank): again the derivative is not the *sane* one! + // auto g = [&](const Vector3& increment) { + // return pim.biascorrectedDeltaRij(increment, {}); + // }; + // EXPECT(assert_equal(numericalDerivative11(g, Z_3x1), H)); } //****************************************************************************** @@ -100,6 +116,20 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) { // Make sure delRdelBiasOmega is H_bias after integration. EXPECT(assert_equal(H_bias, pim.delRdelBiasOmega())); + + // Check the bias correction in same way, but will now yield pitch change. + Matrix3 H; + const double delta = 0.05; + const Vector3 biasOmegaIncr(delta, 0, 0); + Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + EQUALITY(Vector3(0, -deltaT * delta, 0), expected.logmap(corrected)); + EXPECT(assert_equal(Rot3::Pitch((omega - delta) * deltaT), corrected, 1e-9)); + + // TODO(frank): again the derivative is not the *sane* one! + // auto g = [&](const Vector3& increment) { + // return pim.biascorrectedDeltaRij(increment, {}); + // }; + // EXPECT(assert_equal(numericalDerivative11(g, Z_3x1), H)); } //****************************************************************************** From ca50c82badf579f9b4d0dbc28f3e125b9a08f6fb Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 13:51:24 -0700 Subject: [PATCH 143/208] CompareWithPreintegratedRotation --- .../tests/testManifoldPreintegration.cpp | 53 +++++++++++++++++++ 1 file changed, 53 insertions(+) diff --git a/gtsam/navigation/tests/testManifoldPreintegration.cpp b/gtsam/navigation/tests/testManifoldPreintegration.cpp index f04663943..4016240cf 100644 --- a/gtsam/navigation/tests/testManifoldPreintegration.cpp +++ b/gtsam/navigation/tests/testManifoldPreintegration.cpp @@ -109,6 +109,59 @@ TEST(ManifoldPreintegration, computeError) { EXPECT(assert_equal(numericalDerivative33(f, x1, x2, bias), aH3, 1e-9)); } +/* ************************************************************************* */ +TEST(ManifoldPreintegration, CompareWithPreintegratedRotation) { + // Create a PreintegratedRotation object + auto p = std::make_shared(); + PreintegratedRotation pim(p); + + // Integrate a single measurement + const double omega = 0.1; + const Vector3 trueOmega(omega, 0, 0); + const Vector3 bias(1, 2, 3); + const Vector3 measuredOmega = trueOmega + bias; + const double deltaT = 0.5; + + // Check the accumulated rotation. + Rot3 expected = Rot3::Roll(omega * deltaT); + pim.integrateGyroMeasurement(measuredOmega, bias, deltaT); + EXPECT(assert_equal(expected, pim.deltaRij(), 1e-9)); + + // Now do the same for a ManifoldPreintegration object + imuBias::ConstantBias biasHat {Z_3x1, bias}; + ManifoldPreintegration manifoldPim(testing::Params(), biasHat); + manifoldPim.integrateMeasurement(Z_3x1, measuredOmega, deltaT); + EXPECT(assert_equal(expected, manifoldPim.deltaRij(), 1e-9)); + + // Check their internal Jacobians are the same: + EXPECT(assert_equal(pim.delRdelBiasOmega(), manifoldPim.delRdelBiasOmega())); + + // Check PreintegratedRotation::biascorrectedDeltaRij. + Matrix3 H; + const double delta = 0.05; + const Vector3 biasOmegaIncr(delta, 0, 0); + Rot3 corrected = pim.biascorrectedDeltaRij(biasOmegaIncr, H); + EQUALITY(Vector3(-deltaT * delta, 0, 0), expected.logmap(corrected)); + const Rot3 expected2 = Rot3::Roll((omega - delta) * deltaT); + EXPECT(assert_equal(expected2, corrected, 1e-9)); + + // Check ManifoldPreintegration::biasCorrectedDelta. + imuBias::ConstantBias bias_i {Z_3x1, bias + biasOmegaIncr}; + Matrix96 H2; + Vector9 biasCorrected = manifoldPim.biasCorrectedDelta(bias_i, H2); + Vector9 expected3; + expected3 << Rot3::Logmap(expected2), Z_3x1, Z_3x1; + EXPECT(assert_equal(expected3, biasCorrected, 1e-9)); + + // Check that this one is sane: + auto g = [&](const Vector3& increment) { + return manifoldPim.biasCorrectedDelta({Z_3x1, bias + increment}, {}); + }; + EXPECT(assert_equal(numericalDerivative11(g, Z_3x1), + H2.rightCols<3>(), + 1e-4)); // NOTE(frank): reduced tolerance +} + /* ************************************************************************* */ int main() { TestResult tr; From e47b4e5b2cfa02457840d314325652e2120dd590 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 14:34:04 -0700 Subject: [PATCH 144/208] Add a strong end-to-end test --- gtsam/navigation/tests/testAHRSFactor.cpp | 106 ++++++++++++++++++---- gtsam/navigation/tests/testImuFactor.cpp | 48 +++++----- 2 files changed, 113 insertions(+), 41 deletions(-) diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index 95674ce78..570e531d7 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -29,9 +29,12 @@ #include #include #include +#include #include #include +#include +#include "gtsam/nonlinear/LevenbergMarquardtParams.h" using namespace std::placeholders; using namespace std; @@ -39,7 +42,7 @@ using namespace gtsam; // Convenience for named keys using symbol_shorthand::B; -using symbol_shorthand::X; +using symbol_shorthand::R; Vector3 kZeroOmegaCoriolis(0, 0, 0); @@ -136,7 +139,7 @@ TEST(AHRSFactor, Error) { pim.integrateMeasurement(measuredOmega, deltaT); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis, {}); + AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis, {}); // Check value Vector3 errorActual = factor.evaluateError(Ri, Rj, bias); @@ -145,8 +148,8 @@ TEST(AHRSFactor, Error) { // Check Derivatives Values values; - values.insert(X(1), Ri); - values.insert(X(2), Rj); + values.insert(R(1), Ri); + values.insert(R(2), Rj); values.insert(B(1), bias); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } @@ -165,7 +168,7 @@ TEST(AHRSFactor, ErrorWithBiases) { pim.integrateMeasurement(measuredOmega, deltaT); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); + AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis); // Check value Vector3 errorExpected(0, 0, 0); @@ -174,8 +177,8 @@ TEST(AHRSFactor, ErrorWithBiases) { // Check Derivatives Values values; - values.insert(X(1), Ri); - values.insert(X(2), Rj); + values.insert(R(1), Ri); + values.insert(R(2), Rj); values.insert(B(1), bias); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } @@ -324,12 +327,12 @@ TEST(AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement) { EXPECT(assert_equal(kMeasuredOmegaCovariance, pim.preintMeasCov())); // Create factor - AHRSFactor factor(X(1), X(2), B(1), pim, omegaCoriolis); + AHRSFactor factor(R(1), R(2), B(1), pim, omegaCoriolis); // Check Derivatives Values values; - values.insert(X(1), Ri); - values.insert(X(2), Rj); + values.insert(R(1), Ri); + values.insert(R(2), Rj); values.insert(B(1), bias); EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-6); } @@ -350,7 +353,7 @@ TEST(AHRSFactor, predictTest) { expectedMeasCov = 200 * kMeasuredOmegaCovariance; EXPECT(assert_equal(expectedMeasCov, pim.preintMeasCov())); - AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); + AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis); // Predict Rot3 x; @@ -368,7 +371,6 @@ TEST(AHRSFactor, predictTest) { EXPECT(assert_equal(expectedH, H, 1e-8)); } //****************************************************************************** - TEST(AHRSFactor, graphTest) { // linearization point Rot3 Ri(Rot3::RzRyRx(0, 0, 0)); @@ -393,15 +395,87 @@ TEST(AHRSFactor, graphTest) { } // pim.print("Pre integrated measurements"); - AHRSFactor factor(X(1), X(2), B(1), pim, kZeroOmegaCoriolis); - values.insert(X(1), Ri); - values.insert(X(2), Rj); + AHRSFactor factor(R(1), R(2), B(1), pim, kZeroOmegaCoriolis); + values.insert(R(1), Ri); + values.insert(R(2), Rj); values.insert(B(1), bias); graph.push_back(factor); LevenbergMarquardtOptimizer optimizer(graph, values); Values result = optimizer.optimize(); Rot3 expectedRot(Rot3::RzRyRx(0, M_PI / 4, 0)); - EXPECT(assert_equal(expectedRot, result.at(X(2)))); + EXPECT(assert_equal(expectedRot, result.at(R(2)))); +} + +/* ************************************************************************* */ +TEST(AHRSFactor, bodyPSensorWithBias) { + using noiseModel::Diagonal; + + int numRotations = 10; + const Vector3 noiseBetweenBiasSigma(3.0e-6, 3.0e-6, 3.0e-6); + SharedDiagonal biasNoiseModel = Diagonal::Sigmas(noiseBetweenBiasSigma); + + // Measurements in the sensor frame: + const double omega = 0.1; + const Vector3 realOmega(omega, 0, 0); + const Vector3 realBias(1, 2, 3); // large ! + const Vector3 measuredOmega = realOmega + realBias; + + auto p = std::make_shared(); + p->body_P_sensor = Pose3(Rot3::Yaw(M_PI_2), Point3(0, 0, 0)); + p->gyroscopeCovariance = 1e-8 * I_3x3; + double deltaT = 0.005; + + // Specify noise values on priors + const Vector3 priorNoisePoseSigmas(0.001, 0.001, 0.001); + const Vector3 priorNoiseBiasSigmas(0.5e-1, 0.5e-1, 0.5e-1); + SharedDiagonal priorNoisePose = Diagonal::Sigmas(priorNoisePoseSigmas); + SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas); + + // Create a factor graph with priors on initial pose, velocity and bias + NonlinearFactorGraph graph; + Values values; + + graph.addPrior(R(0), Rot3(), priorNoisePose); + values.insert(R(0), Rot3()); + + // The key to this test is that we specify the bias, in the sensor frame, as + // known a priori. We also create factors below that encode our assumption + // that this bias is constant over time In theory, after optimization, we + // should recover that same bias estimate + graph.addPrior(B(0), realBias, priorNoiseBias); + values.insert(B(0), realBias); + + // Now add IMU factors and bias noise models + const Vector3 zeroBias(0, 0, 0); + for (int i = 1; i < numRotations; i++) { + PreintegratedAhrsMeasurements pim(p, realBias); + for (int j = 0; j < 200; ++j) + pim.integrateMeasurement(measuredOmega, deltaT); + + // Create factors + graph.emplace_shared(R(i - 1), R(i), B(i - 1), pim); + graph.emplace_shared >(B(i - 1), B(i), zeroBias, + biasNoiseModel); + + values.insert(R(i), Rot3()); + values.insert(B(i), realBias); + } + + // Finally, optimize, and get bias at last time step + LevenbergMarquardtParams params; + // params.setVerbosityLM("SUMMARY"); + Values result = LevenbergMarquardtOptimizer(graph, values, params).optimize(); + const Vector3 biasActual = result.at(B(numRotations - 1)); + + // Bias should be a self-fulfilling prophesy: + EXPECT(assert_equal(realBias, biasActual, 1e-3)); + + // Check that the successive rotations are all `omega` apart: + for (int i = 0; i < numRotations; i++) { + Rot3 expectedRot = Rot3::Pitch(omega * i); + Rot3 actualRot = result.at(R(i)); + EXPECT(assert_equal(expectedRot, actualRot, 1e-3)); + } } //****************************************************************************** diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 3c8f426cb..d4bc763ee 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -410,33 +410,33 @@ TEST(ImuFactor, PartialDerivative_wrt_Bias) { const Matrix3 Jr = Rot3::ExpmapDerivative((measuredOmega - biasOmega) * deltaT); - Matrix3 actualdelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign + Matrix3 actualDelRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-9)); + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualDelRdelBiasOmega, 1e-9)); } /* ************************************************************************* */ TEST(ImuFactor, PartialDerivativeLogmap) { // Linearization point - Vector3 thetahat(0.1, 0.1, 0); // Current estimate of rotation rate bias + Vector3 thetaHat(0.1, 0.1, 0); // Current estimate of rotation rate bias // Measurements - Vector3 deltatheta(0, 0, 0); + Vector3 deltaTheta(0, 0, 0); - auto evaluateLogRotation = [=](const Vector3 deltatheta) { + auto evaluateLogRotation = [=](const Vector3 delta) { return Rot3::Logmap( - Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); + Rot3::Expmap(thetaHat).compose(Rot3::Expmap(delta))); }; // Compute numerical derivatives - Matrix expectedDelFdeltheta = - numericalDerivative11(evaluateLogRotation, deltatheta); + Matrix expectedDelFdelTheta = + numericalDerivative11(evaluateLogRotation, deltaTheta); - Matrix3 actualDelFdeltheta = Rot3::LogmapDerivative(thetahat); + Matrix3 actualDelFdelTheta = Rot3::LogmapDerivative(thetaHat); // Compare Jacobians - EXPECT(assert_equal(expectedDelFdeltheta, actualDelFdeltheta)); + EXPECT(assert_equal(expectedDelFdelTheta, actualDelFdelTheta)); } /* ************************************************************************* */ @@ -450,8 +450,8 @@ TEST(ImuFactor, fistOrderExponential) { // change w.r.t. linearization point double alpha = 0.0; - Vector3 deltabiasOmega; - deltabiasOmega << alpha, alpha, alpha; + Vector3 deltaBiasOmega; + deltaBiasOmega << alpha, alpha, alpha; const Matrix3 Jr = Rot3::ExpmapDerivative( (measuredOmega - biasOmega) * deltaT); @@ -459,13 +459,12 @@ TEST(ImuFactor, fistOrderExponential) { Matrix3 delRdelBiasOmega = -Jr * deltaT; // the delta bias appears with the minus sign const Matrix expectedRot = Rot3::Expmap( - (measuredOmega - biasOmega - deltabiasOmega) * deltaT).matrix(); + (measuredOmega - biasOmega - deltaBiasOmega) * deltaT).matrix(); const Matrix3 hatRot = Rot3::Expmap((measuredOmega - biasOmega) * deltaT).matrix(); const Matrix3 actualRot = hatRot - * Rot3::Expmap(delRdelBiasOmega * deltabiasOmega).matrix(); - // hatRot * (I_3x3 + skewSymmetric(delRdelBiasOmega * deltabiasOmega)); + * Rot3::Expmap(delRdelBiasOmega * deltaBiasOmega).matrix(); // This is a first order expansion so the equality is only an approximation EXPECT(assert_equal(expectedRot, actualRot)); @@ -728,7 +727,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { using noiseModel::Diagonal; typedef Bias Bias; - int numFactors = 10; + int numPoses = 10; Vector6 noiseBetweenBiasSigma; noiseBetweenBiasSigma << Vector3(2.0e-5, 2.0e-5, 2.0e-5), Vector3(3.0e-6, 3.0e-6, 3.0e-6); @@ -761,7 +760,7 @@ TEST(ImuFactor, bodyPSensorWithBias) { SharedDiagonal priorNoiseBias = Diagonal::Sigmas(priorNoiseBiasSigmas); Vector3 zeroVel(0, 0, 0); - // Create a factor graph with priors on initial pose, vlocity and bias + // Create a factor graph with priors on initial pose, velocity and bias NonlinearFactorGraph graph; Values values; @@ -780,9 +779,8 @@ TEST(ImuFactor, bodyPSensorWithBias) { // Now add IMU factors and bias noise models Bias zeroBias(Vector3(0, 0, 0), Vector3(0, 0, 0)); - for (int i = 1; i < numFactors; i++) { - PreintegratedImuMeasurements pim = PreintegratedImuMeasurements(p, - priorBias); + for (int i = 1; i < numPoses; i++) { + PreintegratedImuMeasurements pim(p, priorBias); for (int j = 0; j < 200; ++j) pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT); @@ -796,8 +794,8 @@ TEST(ImuFactor, bodyPSensorWithBias) { } // Finally, optimize, and get bias at last time step - Values results = LevenbergMarquardtOptimizer(graph, values).optimize(); - Bias biasActual = results.at(B(numFactors - 1)); + Values result = LevenbergMarquardtOptimizer(graph, values).optimize(); + Bias biasActual = result.at(B(numPoses - 1)); // And compare it with expected value (our prior) Bias biasExpected(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); @@ -851,11 +849,11 @@ struct ImuFactorMergeTest { actual_pim02.preintegrated(), tol)); EXPECT(assert_equal(pim02_expected, actual_pim02, tol)); - ImuFactor::shared_ptr factor01 = + auto factor01 = std::make_shared(X(0), V(0), X(1), V(1), B(0), pim01); - ImuFactor::shared_ptr factor12 = + auto factor12 = std::make_shared(X(1), V(1), X(2), V(2), B(0), pim12); - ImuFactor::shared_ptr factor02_expected = std::make_shared( + auto factor02_expected = std::make_shared( X(0), V(0), X(2), V(2), B(0), pim02_expected); ImuFactor::shared_ptr factor02_merged = ImuFactor::Merge(factor01, factor12); From fccf3cedd9d8cdcdf34dfb4af6e3f061a2ecda99 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 15:10:12 -0700 Subject: [PATCH 145/208] Make sure compiles without deprecated --- gtsam/navigation/PreintegratedRotation.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index b10d8f772..374a09359 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -110,7 +110,7 @@ void PreintegratedRotation::integrateGyroMeasurement( delRdelBiasOmega_ = incrRt * delRdelBiasOmega_ + H_bias; } -// deprecated! +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V43 void PreintegratedRotation::integrateMeasurement( const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> optional_D_incrR_integratedOmega, @@ -125,6 +125,7 @@ void PreintegratedRotation::integrateMeasurement( *optional_D_incrR_integratedOmega << H_bias / -deltaT; } } +#endif Rot3 PreintegratedRotation::biascorrectedDeltaRij(const Vector3& biasOmegaIncr, OptionalJacobian<3, 3> H) const { From 1bb26f86afd26b6a786bf2748c1f939bde4f6173 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 21:34:37 -0700 Subject: [PATCH 146/208] Attempt to resolve Windows linking --- gtsam/navigation/PreintegratedRotation.cpp | 6 ++- gtsam/navigation/PreintegratedRotation.h | 50 +++++++++---------- .../tests/testPreintegratedRotation.cpp | 6 +-- 3 files changed, 30 insertions(+), 32 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index 374a09359..ebf98bc15 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -68,7 +68,8 @@ bool PreintegratedRotation::equals(const PreintegratedRotation& other, && equal_with_abs_tol(delRdelBiasOmega_, other.delRdelBiasOmega_, tol); } -Rot3 PreintegratedRotation::IncrementalRotation::operator()( +namespace internal { +Rot3 IncrementalRotation::operator()( const Vector3& bias, OptionalJacobian<3, 3> H_bias) const { // First we compensate the measurements for the bias Vector3 correctedOmega = measuredOmega - bias; @@ -93,12 +94,13 @@ Rot3 PreintegratedRotation::IncrementalRotation::operator()( } return incrR; } +} // namespace internal void PreintegratedRotation::integrateGyroMeasurement( const Vector3& measuredOmega, const Vector3& biasHat, double deltaT, OptionalJacobian<3, 3> F) { Matrix3 H_bias; - IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; const Rot3 incrR = f(biasHat, H_bias); // Update deltaTij and rotation diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index a36674062..3ff3eb3f1 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -176,32 +176,6 @@ class GTSAM_EXPORT PreintegratedRotation { /// @} - /// @name Internal, exposed for testing only - /// @{ - - /** - * @brief Function object for incremental rotation. - * @param measuredOmega The measured angular velocity (as given by the sensor) - * @param deltaT The time interval over which the rotation is integrated. - * @param body_P_sensor Optional transform between body and IMU. - */ - struct IncrementalRotation { - const Vector3& measuredOmega; - const double deltaT; - const std::optional& body_P_sensor; - - /** - * @brief Integrate angular velocity, but corrected by bias. - * @param bias The bias estimate - * @param H_bias Jacobian of the rotation w.r.t. bias. - * @return The incremental rotation - */ - Rot3 operator()(const Vector3& bias, - OptionalJacobian<3, 3> H_bias = {}) const; - }; - - /// @} - /// @name Deprecated API /// @{ @@ -250,4 +224,28 @@ class GTSAM_EXPORT PreintegratedRotation { template <> struct traits : public Testable {}; +namespace internal { +/** + * @brief Function object for incremental rotation. + * @param measuredOmega The measured angular velocity (as given by the sensor) + * @param deltaT The time interval over which the rotation is integrated. + * @param body_P_sensor Optional transform between body and IMU. + */ +struct IncrementalRotation { + const Vector3& measuredOmega; + const double deltaT; + const std::optional& body_P_sensor; + + /** + * @brief Integrate angular velocity, but corrected by bias. + * @param bias The bias estimate + * @param H_bias Jacobian of the rotation w.r.t. bias. + * @return The incremental rotation + */ + Rot3 operator()(const Vector3& bias, + OptionalJacobian<3, 3> H_bias = {}) const; +}; + +} // namespace internal + } /// namespace gtsam diff --git a/gtsam/navigation/tests/testPreintegratedRotation.cpp b/gtsam/navigation/tests/testPreintegratedRotation.cpp index b6486d030..1e27ca1e4 100644 --- a/gtsam/navigation/tests/testPreintegratedRotation.cpp +++ b/gtsam/navigation/tests/testPreintegratedRotation.cpp @@ -52,8 +52,7 @@ TEST(PreintegratedRotation, integrateGyroMeasurement) { // Check the value. Matrix3 H_bias; - PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, - p->getBodyPSensor()}; + internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; const Rot3 incrR = f(bias, H_bias); Rot3 expected = Rot3::Roll(omega * deltaT); EXPECT(assert_equal(expected, incrR, 1e-9)); @@ -98,8 +97,7 @@ TEST(PreintegratedRotation, integrateGyroMeasurementWithTransform) { // Check the value. Matrix3 H_bias; - PreintegratedRotation::IncrementalRotation f{measuredOmega, deltaT, - p->getBodyPSensor()}; + internal::IncrementalRotation f{measuredOmega, deltaT, p->getBodyPSensor()}; Rot3 expected = Rot3::Pitch(omega * deltaT); EXPECT(assert_equal(expected, f(bias, H_bias), 1e-9)); From ab73b956e0209f31d8df92d1e9eff645a8176a37 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 21:47:15 -0700 Subject: [PATCH 147/208] Compile deprecated sections --- gtsam/navigation/PreintegratedRotation.cpp | 2 +- gtsam/navigation/PreintegratedRotation.h | 50 +++++++++++----------- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/gtsam/navigation/PreintegratedRotation.cpp b/gtsam/navigation/PreintegratedRotation.cpp index ebf98bc15..04e201a34 100644 --- a/gtsam/navigation/PreintegratedRotation.cpp +++ b/gtsam/navigation/PreintegratedRotation.cpp @@ -122,7 +122,7 @@ void PreintegratedRotation::integrateMeasurement( // If asked, pass obsolete Jacobians as well if (optional_D_incrR_integratedOmega) { Matrix3 H_bias; - IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; const Rot3 incrR = f(biasHat, H_bias); *optional_D_incrR_integratedOmega << H_bias / -deltaT; } diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 3ff3eb3f1..146a47a19 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -27,6 +27,30 @@ namespace gtsam { +namespace internal { +/** + * @brief Function object for incremental rotation. + * @param measuredOmega The measured angular velocity (as given by the sensor) + * @param deltaT The time interval over which the rotation is integrated. + * @param body_P_sensor Optional transform between body and IMU. + */ +struct IncrementalRotation { + const Vector3& measuredOmega; + const double deltaT; + const std::optional& body_P_sensor; + + /** + * @brief Integrate angular velocity, but corrected by bias. + * @param bias The bias estimate + * @param H_bias Jacobian of the rotation w.r.t. bias. + * @return The incremental rotation + */ + Rot3 operator()(const Vector3& bias, + OptionalJacobian<3, 3> H_bias = {}) const; +}; + +} // namespace internal + /// Parameters for pre-integration: /// Usage: Create just a single Params and pass a shared pointer to the constructor struct GTSAM_EXPORT PreintegratedRotationParams { @@ -184,7 +208,7 @@ class GTSAM_EXPORT PreintegratedRotation { inline Rot3 GTSAM_DEPRECATED incrementalRotation( const Vector3& measuredOmega, const Vector3& bias, double deltaT, OptionalJacobian<3, 3> D_incrR_integratedOmega) const { - IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; + internal::IncrementalRotation f{measuredOmega, deltaT, p_->body_P_sensor}; Rot3 incrR = f(bias, D_incrR_integratedOmega); // Backwards compatible "weird" Jacobian, no longer used. if (D_incrR_integratedOmega) *D_incrR_integratedOmega /= -deltaT; @@ -224,28 +248,4 @@ class GTSAM_EXPORT PreintegratedRotation { template <> struct traits : public Testable {}; -namespace internal { -/** - * @brief Function object for incremental rotation. - * @param measuredOmega The measured angular velocity (as given by the sensor) - * @param deltaT The time interval over which the rotation is integrated. - * @param body_P_sensor Optional transform between body and IMU. - */ -struct IncrementalRotation { - const Vector3& measuredOmega; - const double deltaT; - const std::optional& body_P_sensor; - - /** - * @brief Integrate angular velocity, but corrected by bias. - * @param bias The bias estimate - * @param H_bias Jacobian of the rotation w.r.t. bias. - * @return The incremental rotation - */ - Rot3 operator()(const Vector3& bias, - OptionalJacobian<3, 3> H_bias = {}) const; -}; - -} // namespace internal - } /// namespace gtsam From f2bc3640cf671c1a09995ff709ae09e49c4653f5 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sun, 9 Jun 2024 23:16:38 -0700 Subject: [PATCH 148/208] Add GTSAM_EXPORT --- gtsam/navigation/PreintegratedRotation.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gtsam/navigation/PreintegratedRotation.h b/gtsam/navigation/PreintegratedRotation.h index 146a47a19..5c36da05b 100644 --- a/gtsam/navigation/PreintegratedRotation.h +++ b/gtsam/navigation/PreintegratedRotation.h @@ -24,6 +24,7 @@ #include #include #include +#include "gtsam/dllexport.h" namespace gtsam { @@ -34,7 +35,7 @@ namespace internal { * @param deltaT The time interval over which the rotation is integrated. * @param body_P_sensor Optional transform between body and IMU. */ -struct IncrementalRotation { +struct GTSAM_EXPORT IncrementalRotation { const Vector3& measuredOmega; const double deltaT; const std::optional& body_P_sensor; From 6419b0d749b08ce0b0a5a806ac6a55942a288d69 Mon Sep 17 00:00:00 2001 From: Tal Regev Date: Thu, 13 Jun 2024 14:20:20 +0300 Subject: [PATCH 149/208] Build all tests for windows --- .github/workflows/build-windows.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 766662a36..17306b66f 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -124,6 +124,7 @@ jobs: # https://stackoverflow.com/a/24470998/1236990 cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam cmake --build build -j4 --config ${{ matrix.build_type }} --target gtsam_unstable + cmake --build build -j4 --config ${{ matrix.build_type }} --target all.tests - name: Test shell: bash From afee6ed106b739ea8c2d8ebd34193170947d873f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 25 Jun 2024 18:02:55 -0400 Subject: [PATCH 150/208] update MSVC toolset version --- .github/workflows/build-python.yml | 2 +- .github/workflows/build-windows.yml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index 4ce603023..a3e3e3347 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -109,7 +109,7 @@ jobs: uses: ilammy/msvc-dev-cmd@v1 with: arch: x${{matrix.platform}} - toolset: 14.2 + toolset: 14.40 - name: cl version (Windows) if: runner.os == 'Windows' diff --git a/.github/workflows/build-windows.yml b/.github/workflows/build-windows.yml index 17306b66f..dcf742c05 100644 --- a/.github/workflows/build-windows.yml +++ b/.github/workflows/build-windows.yml @@ -50,7 +50,7 @@ jobs: uses: ilammy/msvc-dev-cmd@v1 with: arch: x${{ matrix.platform }} - toolset: 14.2 + toolset: 14.40 - name: cl version shell: cmd From f29ece9b60e5c1dfd35dab300fcf82a7fa88e94e Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Wed, 26 Jun 2024 12:46:36 -0400 Subject: [PATCH 151/208] update setup-python action version --- .github/workflows/build-python.yml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/.github/workflows/build-python.yml b/.github/workflows/build-python.yml index a3e3e3347..879777cc9 100644 --- a/.github/workflows/build-python.yml +++ b/.github/workflows/build-python.yml @@ -2,8 +2,8 @@ name: Python CI on: [pull_request] -# Every time you make a push to your PR, it cancel immediately the previous checks, -# and start a new one. The other runner will be available more quickly to your PR. +# Every time you make a push to your PR, it cancel immediately the previous checks, +# and start a new one. The other runner will be available more quickly to your PR. concurrency: group: ${{ github.workflow }}-${{ github.head_ref || github.run_id }} cancel-in-progress: true @@ -19,7 +19,7 @@ jobs: CMAKE_BUILD_TYPE: ${{ matrix.build_type }} PYTHON_VERSION: ${{ matrix.python_version }} BOOST_VERSION: 1.72.0 - BOOST_EXE: boost_1_72_0-msvc-14.2 + BOOST_EXE: boost_1_72_0-msvc-14.2 strategy: fail-fast: true @@ -117,7 +117,7 @@ jobs: run: cl - name: Setup python (Windows) - uses: actions/setup-python@v4 + uses: actions/setup-python@v5 if: runner.os == 'Windows' with: python-version: ${{ matrix.python_version }} From 75f9a901f3ee471172592ae92d604136e37b6cb6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Thu, 27 Jun 2024 21:54:50 -0400 Subject: [PATCH 152/208] restrict numpy to under v2.0.0 --- python/requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/requirements.txt b/python/requirements.txt index 099cc80d6..81193bc55 100644 --- a/python/requirements.txt +++ b/python/requirements.txt @@ -1 +1 @@ -numpy>=1.11.0 +numpy~=1.11.0 From 788826a72a3462337ebcd074e67680e450d8fcaa Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 28 Jun 2024 09:15:58 -0400 Subject: [PATCH 153/208] more explicit numpy version to avoid compiler issues --- python/requirements.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/requirements.txt b/python/requirements.txt index 81193bc55..d117e48f9 100644 --- a/python/requirements.txt +++ b/python/requirements.txt @@ -1 +1 @@ -numpy~=1.11.0 +numpy>=1.11.0,<2.0.0 From 5a97336f746cbb5e4347f6a3dee4a030528e97a6 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 28 Jun 2024 11:03:51 -0400 Subject: [PATCH 154/208] Squashed 'wrap/' changes from cdcf23207..38a1bfa01 38a1bfa01 Merge pull request #166 from borglab/pybind-update 129e81b3e fix for compiling on Clang dcdacc833 upgrade to pybind11 v2.13.1 16934a98d Merge pull request #165 from borglab/remove-matrix-assumptions ff222ecf9 update DOCS.md 48232621e remove unused fixture e60ed9f10 unit test showing better control of Vector and Matrix definitions 2559b4f95 remove special checks for Vector and Matrix, leaving the onus on the developer git-subtree-dir: wrap git-subtree-split: 38a1bfa0123e36561104502e86f89a569873638b --- DOCS.md | 4 +- gtwrap/interface_parser/type.py | 18 +- pybind11/.appveyor.yml | 2 +- pybind11/.github/CONTRIBUTING.md | 2 +- pybind11/.github/dependabot.yml | 10 +- pybind11/.github/labeler.yml | 15 +- pybind11/.github/labeler_merged.yml | 9 +- pybind11/.github/workflows/ci.yml | 274 +++++++++------ pybind11/.github/workflows/configure.yml | 12 +- pybind11/.github/workflows/format.yml | 8 +- pybind11/.github/workflows/labeler.yml | 2 +- pybind11/.github/workflows/pip.yml | 39 ++- pybind11/.github/workflows/upstream.yml | 14 +- pybind11/.pre-commit-config.yaml | 47 +-- pybind11/.readthedocs.yml | 21 +- pybind11/CMakeLists.txt | 65 +++- pybind11/README.rst | 13 +- pybind11/docs/advanced/embedding.rst | 2 +- pybind11/docs/advanced/exceptions.rst | 22 +- pybind11/docs/advanced/functions.rst | 2 +- pybind11/docs/advanced/misc.rst | 29 ++ pybind11/docs/advanced/pycpp/numpy.rst | 2 - pybind11/docs/benchmark.py | 4 +- pybind11/docs/changelog.rst | 299 ++++++++++++++++- pybind11/docs/compiling.rst | 247 +++++++++----- pybind11/docs/conf.py | 3 +- pybind11/docs/release.rst | 128 ++++--- pybind11/docs/requirements.in | 6 + pybind11/docs/requirements.txt | 281 +++++++++++++++- pybind11/docs/upgrade.rst | 28 ++ pybind11/include/pybind11/buffer_info.h | 28 +- pybind11/include/pybind11/cast.h | 166 ++++++++- pybind11/include/pybind11/detail/class.h | 155 +++++---- pybind11/include/pybind11/detail/common.h | 41 ++- pybind11/include/pybind11/detail/descr.h | 5 +- pybind11/include/pybind11/detail/init.h | 2 +- pybind11/include/pybind11/detail/internals.h | 283 ++++++++++------ .../pybind11/detail/type_caster_base.h | 137 +++++--- pybind11/include/pybind11/eigen/tensor.h | 5 +- pybind11/include/pybind11/embed.h | 3 - pybind11/include/pybind11/functional.h | 3 +- pybind11/include/pybind11/gil.h | 58 ++-- .../include/pybind11/gil_safe_call_once.h | 91 +++++ pybind11/include/pybind11/numpy.h | 183 ++++++++-- pybind11/include/pybind11/pybind11.h | 316 +++++++++++++----- pybind11/include/pybind11/pytypes.h | 59 +++- pybind11/include/pybind11/stl.h | 15 +- pybind11/include/pybind11/stl/filesystem.h | 3 +- pybind11/include/pybind11/stl_bind.h | 87 ++--- pybind11/include/pybind11/typing.h | 239 +++++++++++++ pybind11/noxfile.py | 62 ++-- pybind11/pybind11/__init__.py | 6 +- pybind11/pybind11/__main__.py | 1 + pybind11/pybind11/_version.py | 6 +- pybind11/pybind11/commands.py | 2 + pybind11/pybind11/setup_helpers.py | 36 +- pybind11/pyproject.toml | 39 +-- pybind11/setup.cfg | 4 +- pybind11/setup.py | 15 +- pybind11/tests/CMakeLists.txt | 15 +- pybind11/tests/conftest.py | 3 + pybind11/tests/cross_module_gil_utils.cpp | 3 + ...s_module_interleaved_error_already_set.cpp | 7 +- .../tests/eigen_tensor_avoid_stl_array.cpp | 4 +- pybind11/tests/env.py | 4 + .../tests/extra_python_package/test_files.py | 5 + .../extra_setuptools/test_setuphelper.py | 2 + .../tests/pybind11_cross_module_tests.cpp | 10 +- pybind11/tests/pybind11_tests.cpp | 18 +- pybind11/tests/pybind11_tests.h | 13 + pybind11/tests/pytest.ini | 1 + pybind11/tests/requirements.txt | 20 +- pybind11/tests/test_async.py | 2 + pybind11/tests/test_buffers.py | 9 + pybind11/tests/test_builtin_casters.cpp | 23 +- pybind11/tests/test_builtin_casters.py | 6 +- pybind11/tests/test_call_policies.cpp | 8 +- pybind11/tests/test_call_policies.py | 2 + pybind11/tests/test_callbacks.py | 9 + pybind11/tests/test_chrono.py | 2 + pybind11/tests/test_class.cpp | 5 +- pybind11/tests/test_class.py | 22 +- .../tests/test_cmake_build/CMakeLists.txt | 3 +- .../installed_embed/CMakeLists.txt | 6 +- .../installed_function/CMakeLists.txt | 6 +- .../installed_target/CMakeLists.txt | 6 +- pybind11/tests/test_cmake_build/main.cpp | 2 +- .../subdirectory_embed/CMakeLists.txt | 12 +- .../subdirectory_function/CMakeLists.txt | 12 +- .../subdirectory_target/CMakeLists.txt | 12 +- pybind11/tests/test_cmake_build/test.py | 2 + pybind11/tests/test_const_name.py | 2 + .../tests/test_constants_and_functions.cpp | 4 + .../tests/test_constants_and_functions.py | 2 + pybind11/tests/test_copy_move.cpp | 15 +- pybind11/tests/test_copy_move.py | 8 + pybind11/tests/test_custom_type_casters.cpp | 24 +- pybind11/tests/test_custom_type_casters.py | 2 + pybind11/tests/test_custom_type_setup.py | 2 + pybind11/tests/test_docstring_options.cpp | 36 +- pybind11/tests/test_docstring_options.py | 2 + pybind11/tests/test_eigen_matrix.cpp | 15 + pybind11/tests/test_eigen_matrix.py | 11 +- pybind11/tests/test_eigen_tensor.inl | 3 +- pybind11/tests/test_eigen_tensor.py | 2 + pybind11/tests/test_embed/CMakeLists.txt | 7 + pybind11/tests/test_embed/external_module.cpp | 2 +- pybind11/tests/test_embed/test_interpreter.py | 2 + pybind11/tests/test_embed/test_trampoline.py | 2 + pybind11/tests/test_enum.py | 10 +- pybind11/tests/test_eval.py | 2 + pybind11/tests/test_eval_call.py | 1 + pybind11/tests/test_exceptions.cpp | 67 +++- pybind11/tests/test_exceptions.h | 2 +- pybind11/tests/test_exceptions.py | 25 +- pybind11/tests/test_factory_constructors.py | 6 +- pybind11/tests/test_gil_scoped.py | 2 + pybind11/tests/test_iostream.py | 20 +- pybind11/tests/test_kwargs_and_defaults.cpp | 60 +++- pybind11/tests/test_kwargs_and_defaults.py | 45 ++- pybind11/tests/test_local_bindings.py | 2 + .../tests/test_methods_and_attributes.cpp | 3 +- pybind11/tests/test_methods_and_attributes.py | 36 +- pybind11/tests/test_modules.py | 2 + pybind11/tests/test_multiple_inheritance.py | 2 + pybind11/tests/test_numpy_array.cpp | 19 +- pybind11/tests/test_numpy_array.py | 20 +- pybind11/tests/test_numpy_dtypes.cpp | 35 +- pybind11/tests/test_numpy_dtypes.py | 20 +- pybind11/tests/test_numpy_vectorize.py | 2 + pybind11/tests/test_opaque_types.py | 2 + pybind11/tests/test_operator_overloading.py | 2 + pybind11/tests/test_pickling.py | 2 + .../test_python_multiple_inheritance.cpp | 45 +++ .../tests/test_python_multiple_inheritance.py | 36 ++ pybind11/tests/test_pytypes.cpp | 109 +++++- pybind11/tests/test_pytypes.py | 156 ++++++++- .../tests/test_sequences_and_iterators.cpp | 19 ++ .../tests/test_sequences_and_iterators.py | 15 + pybind11/tests/test_smart_ptr.cpp | 27 +- pybind11/tests/test_smart_ptr.py | 2 + pybind11/tests/test_stl.cpp | 10 +- pybind11/tests/test_stl.py | 26 +- pybind11/tests/test_stl_binders.cpp | 85 ++++- pybind11/tests/test_stl_binders.py | 52 ++- pybind11/tests/test_tagbased_polymorphic.py | 2 + pybind11/tests/test_thread.cpp | 2 +- pybind11/tests/test_thread.py | 2 + .../tests/test_type_caster_pyobject_ptr.cpp | 45 ++- .../tests/test_type_caster_pyobject_ptr.py | 18 + pybind11/tests/test_union.py | 2 + pybind11/tests/test_unnamed_namespace_a.py | 2 + pybind11/tests/test_unnamed_namespace_b.py | 2 + .../tests/test_vector_unique_ptr_member.py | 2 + pybind11/tests/test_virtual_functions.py | 2 + pybind11/tools/FindPythonLibsNew.cmake | 29 +- .../codespell_ignore_lines_from_errors.py | 5 +- pybind11/tools/libsize.py | 2 + pybind11/tools/make_changelog.py | 32 +- pybind11/tools/pybind11Common.cmake | 58 +++- pybind11/tools/pybind11Config.cmake.in | 2 +- .../tools/pybind11GuessPythonExtSuffix.cmake | 86 +++++ pybind11/tools/pybind11NewTools.cmake | 193 ++++++++--- pybind11/tools/pybind11Tools.cmake | 20 +- .../test-pybind11GuessPythonExtSuffix.cmake | 161 +++++++++ tests/expected/matlab/MyFactorPosePoint2.m | 12 +- tests/expected/matlab/class_wrapper.cpp | 99 +++++- tests/expected/python/class_pybind.cpp | 3 + tests/expected/python/inheritance_pybind.cpp | 8 +- tests/fixtures/class.i | 27 +- tests/fixtures/functions.i | 8 +- tests/fixtures/geometry.i | 2 +- tests/fixtures/inheritance.i | 4 +- tests/fixtures/namespaces.i | 8 +- tests/fixtures/operator.i | 2 +- 175 files changed, 4557 insertions(+), 1305 deletions(-) create mode 100644 pybind11/docs/requirements.in create mode 100644 pybind11/include/pybind11/gil_safe_call_once.h create mode 100644 pybind11/include/pybind11/typing.h create mode 100644 pybind11/tests/test_python_multiple_inheritance.cpp create mode 100644 pybind11/tests/test_python_multiple_inheritance.py create mode 100644 pybind11/tools/pybind11GuessPythonExtSuffix.cmake create mode 100644 pybind11/tools/test-pybind11GuessPythonExtSuffix.cmake diff --git a/DOCS.md b/DOCS.md index ecb6ee985..e7457e4a0 100644 --- a/DOCS.md +++ b/DOCS.md @@ -12,7 +12,7 @@ The python wrapper supports keyword arguments for functions/methods. Hence, the - Only one Method/Constructor per line, though methods/constructors can extend across multiple lines. - Methods can return - - Eigen types: `Matrix`, `Vector`. + - Eigen types: `gtsam::Matrix`, `gtsam::Vector`. - C/C++ basic types: `string`, `bool`, `size_t`, `int`, `double`, `char`, `unsigned char`. - `void` - Any class with which be copied with `std::make_shared()`. @@ -34,7 +34,7 @@ The python wrapper supports keyword arguments for functions/methods. Hence, the - Overloads are supported, but arguments of different types *have* to have different names. - Arguments to functions can be any of - - Eigen types: `Matrix`, `Vector`. + - Eigen types: `gtsam::Matrix`, `gtsam::Vector`. - Eigen types and classes as an optionally const reference. - C/C++ basic types: `string`, `bool`, `size_t`, `size_t`, `double`, `char`, `unsigned char`. - Any class with which be copied with `std::make_shared()` (except Eigen). diff --git a/gtwrap/interface_parser/type.py b/gtwrap/interface_parser/type.py index e56a2f015..02fe8868b 100644 --- a/gtwrap/interface_parser/type.py +++ b/gtwrap/interface_parser/type.py @@ -63,9 +63,6 @@ class Typename: else: self.instantiations = [] - if self.name in ["Matrix", "Vector"] and not self.namespaces: - self.namespaces = ["gtsam"] - @staticmethod def from_parse_result(parse_result: Union[str, list]): """Unpack the parsed result to get the Typename instance.""" @@ -220,7 +217,6 @@ class Type: Generate the C++ code for wrapping. Treat all pointers as "const shared_ptr&" - Treat Matrix and Vector as "const Matrix&" and "const Vector&" resp. """ if self.is_shared_ptr: @@ -228,17 +224,14 @@ class Type: typename=self.typename.to_cpp()) elif self.is_ptr: typename = "{typename}*".format(typename=self.typename.to_cpp()) - elif self.is_ref or self.typename.name in ["Matrix", "Vector"]: + elif self.is_ref: typename = typename = "{typename}&".format( typename=self.typename.to_cpp()) else: typename = self.typename.to_cpp() return ("{const}{typename}".format( - const="const " if - (self.is_const - or self.typename.name in ["Matrix", "Vector"]) else "", - typename=typename)) + const="const " if self.is_const else "", typename=typename)) def get_typename(self): """Convenience method to get the typename of this type.""" @@ -305,13 +298,10 @@ class TemplatedType: typename = f"std::shared_ptr<{typename}>" elif self.is_ptr: typename = "{typename}*".format(typename=typename) - elif self.is_ref or self.typename.name in ["Matrix", "Vector"]: + elif self.is_ref: typename = typename = "{typename}&".format(typename=typename) else: pass return ("{const}{typename}".format( - const="const " if - (self.is_const - or self.typename.name in ["Matrix", "Vector"]) else "", - typename=typename)) + const="const " if self.is_const else "", typename=typename)) diff --git a/pybind11/.appveyor.yml b/pybind11/.appveyor.yml index 360760ac8..391cf1071 100644 --- a/pybind11/.appveyor.yml +++ b/pybind11/.appveyor.yml @@ -9,7 +9,7 @@ platform: - x86 environment: matrix: - - PYTHON: 36 + - PYTHON: 38 CONFIG: Debug install: - ps: | diff --git a/pybind11/.github/CONTRIBUTING.md b/pybind11/.github/CONTRIBUTING.md index ad7974395..f5a08e2d7 100644 --- a/pybind11/.github/CONTRIBUTING.md +++ b/pybind11/.github/CONTRIBUTING.md @@ -135,7 +135,7 @@ The valid options are: * Use `-G` and the name of a generator to use something different. `cmake --help` lists the generators available. - On Unix, setting `CMAKE_GENERATER=Ninja` in your environment will give - you automatic mulithreading on all your CMake projects! + you automatic multithreading on all your CMake projects! * Open the `CMakeLists.txt` with QtCreator to generate for that IDE. * You can use `-DCMAKE_EXPORT_COMPILE_COMMANDS=ON` to generate the `.json` file that some tools expect. diff --git a/pybind11/.github/dependabot.yml b/pybind11/.github/dependabot.yml index 2c7d17083..22c34bd74 100644 --- a/pybind11/.github/dependabot.yml +++ b/pybind11/.github/dependabot.yml @@ -4,4 +4,12 @@ updates: - package-ecosystem: "github-actions" directory: "/" schedule: - interval: "daily" + interval: "weekly" + groups: + actions: + patterns: + - "*" + ignore: + - dependency-name: actions/checkout + versions: + - "<5" diff --git a/pybind11/.github/labeler.yml b/pybind11/.github/labeler.yml index abb0d05aa..e5a8de751 100644 --- a/pybind11/.github/labeler.yml +++ b/pybind11/.github/labeler.yml @@ -1,8 +1,13 @@ docs: -- any: - - 'docs/**/*.rst' - - '!docs/changelog.rst' - - '!docs/upgrade.rst' + all: + - changed-files: + - all-globs-to-all-files: + - '!docs/changelog.rst' + - '!docs/upgrade.rst' + - base-branch: "^(?!dependabot).*" + - base-branch: "^(?!pre-commit-ci).*" ci: -- '.github/workflows/*.yml' + - changed-files: + - any-glob-to-any-file: + - '.github/workflows/*.yml' diff --git a/pybind11/.github/labeler_merged.yml b/pybind11/.github/labeler_merged.yml index 2374ad42e..cf6c1f293 100644 --- a/pybind11/.github/labeler_merged.yml +++ b/pybind11/.github/labeler_merged.yml @@ -1,3 +1,8 @@ +# Add 'needs changelog` label to any change to code files as long as the `CHANGELOG` hasn't changed +# Skip dependabot and pre-commit-ci PRs needs changelog: -- all: - - '!docs/changelog.rst' + - all: + - changed-files: + - all-globs-to-all-files: "!docs/changelog.rst" + - base-branch: "^(?!dependabot).*" + - base-branch: "^(?!pre-commit-ci).*" diff --git a/pybind11/.github/workflows/ci.yml b/pybind11/.github/workflows/ci.yml index 48f7c5e93..3054d842a 100644 --- a/pybind11/.github/workflows/ci.yml +++ b/pybind11/.github/workflows/ci.yml @@ -30,13 +30,12 @@ jobs: strategy: fail-fast: false matrix: - runs-on: [ubuntu-20.04, windows-2022, macos-latest] + runs-on: [ubuntu-20.04, windows-2022, macos-13] python: - - '3.6' + - '3.8' - '3.9' - - '3.10' - - '3.11' - '3.12' + - '3.13' - 'pypy-3.8' - 'pypy-3.9' - 'pypy-3.10' @@ -49,37 +48,42 @@ jobs: include: # Just add a key - runs-on: ubuntu-20.04 - python: '3.6' + python: '3.8' args: > -DPYBIND11_FINDPYTHON=ON -DCMAKE_CXX_FLAGS="-D_=1" + exercise_D_: 1 - runs-on: ubuntu-20.04 python: 'pypy-3.8' args: > -DPYBIND11_FINDPYTHON=ON - runs-on: windows-2019 - python: '3.6' + python: '3.8' args: > -DPYBIND11_FINDPYTHON=ON # Inject a couple Windows 2019 runs - runs-on: windows-2019 python: '3.9' + # Extra ubuntu latest job + - runs-on: ubuntu-latest + python: '3.11' + name: "🐍 ${{ matrix.python }} • ${{ matrix.runs-on }} • x64 ${{ matrix.args }}" runs-on: ${{ matrix.runs-on }} steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup Python ${{ matrix.python }} - uses: actions/setup-python@v4 + uses: actions/setup-python@v5 with: python-version: ${{ matrix.python }} allow-prereleases: true - name: Setup Boost (Linux) # Can't use boost + define _ - if: runner.os == 'Linux' && matrix.python != '3.6' + if: runner.os == 'Linux' && matrix.exercise_D_ != 1 run: sudo apt-get install libboost-dev - name: Setup Boost (macOS) @@ -87,11 +91,11 @@ jobs: run: brew install boost - name: Update CMake - uses: jwlawson/actions-setup-cmake@v1.14 + uses: jwlawson/actions-setup-cmake@v2.0 - name: Cache wheels if: runner.os == 'macOS' - uses: actions/cache@v3 + uses: actions/cache@v4 with: # This path is specific to macOS - we really only need it for PyPy NumPy wheels # See https://github.com/actions/cache/blob/master/examples.md#python---pip @@ -109,12 +113,15 @@ jobs: run: python -m pip install pytest-github-actions-annotate-failures # First build - C++11 mode and inplace - # More-or-less randomly adding -DPYBIND11_SIMPLE_GIL_MANAGEMENT=ON here. + # More-or-less randomly adding -DPYBIND11_SIMPLE_GIL_MANAGEMENT=ON here + # (same for PYBIND11_NUMPY_1_ONLY, but requires a NumPy 1.x at runtime). - name: Configure C++11 ${{ matrix.args }} run: > cmake -S . -B . -DPYBIND11_WERROR=ON + -DPYBIND11_DISABLE_HANDLE_TYPE_NAME_DEFAULT_IMPLEMENTATION=ON -DPYBIND11_SIMPLE_GIL_MANAGEMENT=ON + -DPYBIND11_NUMPY_1_ONLY=ON -DDOWNLOAD_CATCH=ON -DDOWNLOAD_EIGEN=ON -DCMAKE_CXX_STANDARD=11 @@ -127,9 +134,7 @@ jobs: run: cmake --build . --target pytest -j 2 - name: C++11 tests - # TODO: Figure out how to load the DLL on Python 3.8+ - if: "!(runner.os == 'Windows' && (matrix.python == 3.8 || matrix.python == 3.9 || matrix.python == '3.10' || matrix.python == '3.11' || matrix.python == 'pypy-3.8'))" - run: cmake --build . --target cpptest -j 2 + run: cmake --build . --target cpptest -j 2 - name: Interface test C++11 run: cmake --build . --target test_cmake_build @@ -139,11 +144,13 @@ jobs: # Second build - C++17 mode and in a build directory # More-or-less randomly adding -DPYBIND11_SIMPLE_GIL_MANAGEMENT=OFF here. + # (same for PYBIND11_NUMPY_1_ONLY, but requires a NumPy 1.x at runtime). - name: Configure C++17 run: > cmake -S . -B build2 -DPYBIND11_WERROR=ON -DPYBIND11_SIMPLE_GIL_MANAGEMENT=OFF + -DPYBIND11_NUMPY_1_ONLY=ON -DDOWNLOAD_CATCH=ON -DDOWNLOAD_EIGEN=ON -DCMAKE_CXX_STANDARD=17 @@ -156,8 +163,6 @@ jobs: run: cmake --build build2 --target pytest - name: C++ tests - # TODO: Figure out how to load the DLL on Python 3.8+ - if: "!(runner.os == 'Windows' && (matrix.python == 3.8 || matrix.python == 3.9 || matrix.python == '3.10' || matrix.python == '3.11' || matrix.python == 'pypy-3.8'))" run: cmake --build build2 --target cpptest # Third build - C++17 mode with unstable ABI @@ -188,6 +193,35 @@ jobs: pytest tests/extra_setuptools if: "!(matrix.runs-on == 'windows-2022')" + manylinux: + name: Manylinux on 🐍 3.13t • GIL + runs-on: ubuntu-latest + timeout-minutes: 40 + container: quay.io/pypa/musllinux_1_2_x86_64:latest + steps: + - uses: actions/checkout@v4 + with: + fetch-depth: 0 + + - name: Prepare venv + run: python3.13t -m venv .venv + + - name: Install Python deps + run: .venv/bin/pip install -r tests/requirements.txt + + - name: Configure C++11 + run: > + cmake -S. -Bbuild + -DPYBIND11_WERROR=ON + -DDOWNLOAD_CATCH=ON + -DDOWNLOAD_EIGEN=ON + -DPython_ROOT_DIR=.venv + + - name: Build C++11 + run: cmake --build build -j2 + + - name: Python tests C++11 + run: cmake --build build --target pytest -j2 deadsnakes: strategy: @@ -195,9 +229,10 @@ jobs: matrix: include: # TODO: Fails on 3.10, investigate - - python-version: "3.9" - python-debug: true - valgrind: true + # JOB DISABLED (NEEDS WORK): https://github.com/pybind/pybind11/issues/4889 + # - python-version: "3.9" + # python-debug: true + # valgrind: true - python-version: "3.11" python-debug: false @@ -205,20 +240,20 @@ jobs: runs-on: ubuntu-20.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup Python ${{ matrix.python-version }} (deadsnakes) - uses: deadsnakes/action@v3.0.1 + uses: deadsnakes/action@v3.1.0 with: python-version: ${{ matrix.python-version }} debug: ${{ matrix.python-debug }} - name: Update CMake - uses: jwlawson/actions-setup-cmake@v1.14 + uses: jwlawson/actions-setup-cmake@v2.0 - name: Valgrind cache if: matrix.valgrind - uses: actions/cache@v3 + uses: actions/cache@v4 id: cache-valgrind with: path: valgrind @@ -302,12 +337,15 @@ jobs: - clang: 15 std: 20 container_suffix: "-bullseye" + - clang: 16 + std: 20 + container_suffix: "-bullseye" name: "🐍 3 • Clang ${{ matrix.clang }} • C++${{ matrix.std }} • x64" container: "silkeh/clang:${{ matrix.clang }}${{ matrix.container_suffix }}" steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Add wget and python3 run: apt-get update && apt-get install -y python3-dev python3-numpy python3-pytest libeigen3-dev @@ -341,7 +379,7 @@ jobs: container: nvidia/cuda:12.2.0-devel-ubuntu22.04 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 # tzdata will try to ask for the timezone, so set the DEBIAN_FRONTEND - name: Install 🐍 3 @@ -365,7 +403,7 @@ jobs: # container: centos:8 # # steps: -# - uses: actions/checkout@v3 +# - uses: actions/checkout@v4 # # - name: Add Python 3 and a few requirements # run: yum update -y && yum install -y git python3-devel python3-numpy python3-pytest make environment-modules @@ -401,54 +439,55 @@ jobs: # run: cmake --build build --target test_cmake_build - # Testing on CentOS 7 + PGI compilers, which seems to require more workarounds - centos-nvhpc7: - if: ${{ false }} # JOB DISABLED (NEEDS WORK): https://github.com/pybind/pybind11/issues/4690 - runs-on: ubuntu-latest - name: "🐍 3 • CentOS7 / PGI 22.9 • x64" - container: centos:7 + # Testing on Ubuntu + NVHPC (previous PGI) compilers, which seems to require more workarounds + ubuntu-nvhpc7: + runs-on: ubuntu-20.04 + name: "🐍 3 • NVHPC 23.5 • C++17 • x64" + env: + # tzdata will try to ask for the timezone, so set the DEBIAN_FRONTEND + DEBIAN_FRONTEND: 'noninteractive' steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - - name: Add Python 3 and a few requirements - run: yum update -y && yum install -y epel-release && yum install -y git python3-devel make environment-modules cmake3 yum-utils + - name: Add NVHPC Repo + run: | + echo 'deb [trusted=yes] https://developer.download.nvidia.com/hpc-sdk/ubuntu/amd64 /' | \ + sudo tee /etc/apt/sources.list.d/nvhpc.list - - name: Install NVidia HPC SDK - run: yum-config-manager --add-repo https://developer.download.nvidia.com/hpc-sdk/rhel/nvhpc.repo && yum -y install nvhpc-22.9 + - name: Install 🐍 3 & NVHPC + run: | + sudo apt-get update -y && \ + sudo apt-get install -y cmake environment-modules git python3-dev python3-pip python3-numpy && \ + sudo apt-get install -y --no-install-recommends nvhpc-23-5 && \ + sudo rm -rf /var/lib/apt/lists/* + python3 -m pip install --upgrade pip + python3 -m pip install --upgrade pytest - # On CentOS 7, we have to filter a few tests (compiler internal error) - # and allow deeper template recursion (not needed on CentOS 8 with a newer - # standard library). On some systems, you many need further workarounds: + # On some systems, you many need further workarounds: # https://github.com/pybind/pybind11/pull/2475 - name: Configure shell: bash run: | source /etc/profile.d/modules.sh - module load /opt/nvidia/hpc_sdk/modulefiles/nvhpc/22.9 - cmake3 -S . -B build -DDOWNLOAD_CATCH=ON \ - -DCMAKE_CXX_STANDARD=11 \ + module load /opt/nvidia/hpc_sdk/modulefiles/nvhpc/23.5 + cmake -S . -B build -DDOWNLOAD_CATCH=ON \ + -DCMAKE_CXX_STANDARD=17 \ -DPYTHON_EXECUTABLE=$(python3 -c "import sys; print(sys.executable)") \ -DCMAKE_CXX_FLAGS="-Wc,--pending_instantiations=0" \ -DPYBIND11_TEST_FILTER="test_smart_ptr.cpp" - # Building before installing Pip should produce a warning but not an error - name: Build - run: cmake3 --build build -j 2 --verbose - - - name: Install CMake with pip - run: | - python3 -m pip install --upgrade pip - python3 -m pip install pytest + run: cmake --build build -j 2 --verbose - name: Python tests - run: cmake3 --build build --target pytest + run: cmake --build build --target pytest - name: C++ tests - run: cmake3 --build build --target cpptest + run: cmake --build build --target cpptest - name: Interface test - run: cmake3 --build build --target test_cmake_build + run: cmake --build build --target test_cmake_build # Testing on GCC using the GCC docker images (only recent images supported) @@ -465,12 +504,13 @@ jobs: - { gcc: 10, std: 17 } - { gcc: 11, std: 20 } - { gcc: 12, std: 20 } + - { gcc: 13, std: 20 } name: "🐍 3 • GCC ${{ matrix.gcc }} • C++${{ matrix.std }}• x64" container: "gcc:${{ matrix.gcc }}" steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Add Python 3 run: apt-get update; apt-get install -y python3-dev python3-numpy python3-pytest python3-pip libeigen3-dev @@ -479,7 +519,7 @@ jobs: run: python3 -m pip install --upgrade pip - name: Update CMake - uses: jwlawson/actions-setup-cmake@v1.14 + uses: jwlawson/actions-setup-cmake@v2.0 - name: Configure shell: bash @@ -524,13 +564,11 @@ jobs: # Testing on ICC using the oneAPI apt repo icc: runs-on: ubuntu-20.04 - strategy: - fail-fast: false name: "🐍 3 • ICC latest • x64" steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Add apt repo run: | @@ -618,15 +656,13 @@ jobs: cmake --build build-17 --target test_cmake_build - # Testing on CentOS (manylinux uses a centos base, and this is an easy way - # to get GCC 4.8, which is the manylinux1 compiler). + # Testing on CentOS (manylinux uses a centos base). centos: runs-on: ubuntu-latest strategy: fail-fast: false matrix: container: - - "centos:7" # GCC 4.8 - "almalinux:8" - "almalinux:9" @@ -634,14 +670,15 @@ jobs: container: "${{ matrix.container }}" steps: - - uses: actions/checkout@v3 + - name: Latest actions/checkout + uses: actions/checkout@v4 - - name: Add Python 3 (RHEL 7) - if: matrix.container == 'centos:7' - run: yum update -y && yum install -y python3-devel gcc-c++ make git + - name: Add Python 3.8 + if: matrix.container == 'almalinux:8' + run: dnf update -y && dnf install -y python38-devel gcc-c++ make git - - name: Add Python 3 (RHEL 8+) - if: matrix.container != 'centos:7' + - name: Add Python 3 (default) + if: matrix.container != 'almalinux:8' run: dnf update -y && dnf install -y python3-devel gcc-c++ make git - name: Update pip @@ -651,6 +688,11 @@ jobs: run: | python3 -m pip install cmake -r tests/requirements.txt + - name: Ensure NumPy 2 is used (required Python >= 3.9) + if: matrix.container == 'almalinux:9' + run: | + python3 -m pip install 'numpy>=2.0.0b1' 'scipy>=1.13.0rc1' + - name: Configure shell: bash run: > @@ -682,7 +724,7 @@ jobs: container: i386/debian:buster steps: - - uses: actions/checkout@v1 # Required to run inside docker + - uses: actions/checkout@v1 # v1 is required to run inside docker - name: Install requirements run: | @@ -725,9 +767,9 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - - uses: actions/setup-python@v4 + - uses: actions/setup-python@v5 with: python-version: "3.x" @@ -759,17 +801,25 @@ jobs: fail-fast: false matrix: python: - - 3.6 - - 3.7 - - 3.8 - - 3.9 + - '3.7' + - '3.8' + - '3.9' + - '3.10' + - '3.11' + - '3.12' include: - - python: 3.9 + - python: '3.12' args: -DCMAKE_CXX_STANDARD=20 - - python: 3.8 + - python: '3.11' + args: -DCMAKE_CXX_STANDARD=20 + - python: '3.10' + args: -DCMAKE_CXX_STANDARD=20 + - python: '3.9' + args: -DCMAKE_CXX_STANDARD=20 + - python: '3.8' args: -DCMAKE_CXX_STANDARD=17 - - python: 3.7 + - python: '3.7' args: -DCMAKE_CXX_STANDARD=14 @@ -777,19 +827,19 @@ jobs: runs-on: windows-2019 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup Python ${{ matrix.python }} - uses: actions/setup-python@v4 + uses: actions/setup-python@v5 with: python-version: ${{ matrix.python }} architecture: x86 - name: Update CMake - uses: jwlawson/actions-setup-cmake@v1.14 + uses: jwlawson/actions-setup-cmake@v2.0 - name: Prepare MSVC - uses: ilammy/msvc-dev-cmd@v1.12.1 + uses: ilammy/msvc-dev-cmd@v1.13.0 with: arch: x86 @@ -830,19 +880,19 @@ jobs: runs-on: windows-2019 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup Python ${{ matrix.python }} - uses: actions/setup-python@v4 + uses: actions/setup-python@v5 with: python-version: ${{ matrix.python }} architecture: x86 - name: Update CMake - uses: jwlawson/actions-setup-cmake@v1.14 + uses: jwlawson/actions-setup-cmake@v2.0 - name: Prepare MSVC - uses: ilammy/msvc-dev-cmd@v1.12.1 + uses: ilammy/msvc-dev-cmd@v1.13.0 with: arch: x86 @@ -878,19 +928,21 @@ jobs: runs-on: windows-2022 steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup Python ${{ matrix.python }} - uses: actions/setup-python@v4 + uses: actions/setup-python@v5 with: python-version: ${{ matrix.python }} - name: Prepare env + # Ensure use of NumPy 2 (via NumPy nightlies but can be changed soon) run: | python3 -m pip install -r tests/requirements.txt + python3 -m pip install 'numpy>=2.0.0b1' 'scipy>=1.13.0rc1' - name: Update CMake - uses: jwlawson/actions-setup-cmake@v1.14 + uses: jwlawson/actions-setup-cmake@v2.0 - name: Configure C++20 run: > @@ -948,20 +1000,30 @@ jobs: mingw-w64-${{matrix.env}}-gcc mingw-w64-${{matrix.env}}-python-pip mingw-w64-${{matrix.env}}-python-numpy - mingw-w64-${{matrix.env}}-python-scipy mingw-w64-${{matrix.env}}-cmake mingw-w64-${{matrix.env}}-make mingw-w64-${{matrix.env}}-python-pytest - mingw-w64-${{matrix.env}}-eigen3 mingw-w64-${{matrix.env}}-boost mingw-w64-${{matrix.env}}-catch - - uses: actions/checkout@v3 + - uses: msys2/setup-msys2@v2 + if: matrix.sys == 'mingw64' + with: + msystem: ${{matrix.sys}} + install: >- + git + mingw-w64-${{matrix.env}}-python-scipy + mingw-w64-${{matrix.env}}-eigen3 + + - uses: actions/checkout@v4 - name: Configure C++11 # LTO leads to many undefined reference like # `pybind11::detail::function_call::function_call(pybind11::detail::function_call&&) - run: cmake -G "MinGW Makefiles" -DCMAKE_CXX_STANDARD=11 -DPYBIND11_WERROR=ON -DDOWNLOAD_CATCH=ON -S . -B build + run: >- + cmake -G "MinGW Makefiles" -DCMAKE_CXX_STANDARD=11 -DPYBIND11_WERROR=ON -DDOWNLOAD_CATCH=ON + -DPYTHON_EXECUTABLE=$(python -c "import sys; print(sys.executable)") + -S . -B build - name: Build C++11 run: cmake --build build -j 2 @@ -979,7 +1041,10 @@ jobs: run: git clean -fdx - name: Configure C++14 - run: cmake -G "MinGW Makefiles" -DCMAKE_CXX_STANDARD=14 -DPYBIND11_WERROR=ON -DDOWNLOAD_CATCH=ON -S . -B build2 + run: >- + cmake -G "MinGW Makefiles" -DCMAKE_CXX_STANDARD=14 -DPYBIND11_WERROR=ON -DDOWNLOAD_CATCH=ON + -DPYTHON_EXECUTABLE=$(python -c "import sys; print(sys.executable)") + -S . -B build2 - name: Build C++14 run: cmake --build build2 -j 2 @@ -997,7 +1062,10 @@ jobs: run: git clean -fdx - name: Configure C++17 - run: cmake -G "MinGW Makefiles" -DCMAKE_CXX_STANDARD=17 -DPYBIND11_WERROR=ON -DDOWNLOAD_CATCH=ON -S . -B build3 + run: >- + cmake -G "MinGW Makefiles" -DCMAKE_CXX_STANDARD=17 -DPYBIND11_WERROR=ON -DDOWNLOAD_CATCH=ON + -DPYTHON_EXECUTABLE=$(python -c "import sys; print(sys.executable)") + -S . -B build3 - name: Build C++17 run: cmake --build build3 -j 2 @@ -1027,21 +1095,21 @@ jobs: run: env - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Set up Clang uses: egor-tensin/setup-clang@v1 - name: Setup Python ${{ matrix.python }} - uses: actions/setup-python@v4 + uses: actions/setup-python@v5 with: python-version: ${{ matrix.python }} - name: Update CMake - uses: jwlawson/actions-setup-cmake@v1.14 + uses: jwlawson/actions-setup-cmake@v2.0 - name: Install ninja-build tool - uses: seanmiddleditch/gha-setup-ninja@v3 + uses: seanmiddleditch/gha-setup-ninja@v5 - name: Run pip installs run: | @@ -1081,8 +1149,8 @@ jobs: run: git clean -fdx macos_brew_install_llvm: - name: "macos-latest • brew install llvm" - runs-on: macos-latest + name: "macos-13 • brew install llvm" + runs-on: macos-13 env: # https://apple.stackexchange.com/questions/227026/how-to-install-recent-clang-with-homebrew @@ -1096,7 +1164,7 @@ jobs: run: env - name: Checkout - uses: actions/checkout@v3 + uses: actions/checkout@v4 - name: Show Clang++ version before brew install llvm run: clang++ --version @@ -1108,7 +1176,7 @@ jobs: run: clang++ --version - name: Update CMake - uses: jwlawson/actions-setup-cmake@v1.14 + uses: jwlawson/actions-setup-cmake@v2.0 - name: Run pip installs run: | diff --git a/pybind11/.github/workflows/configure.yml b/pybind11/.github/workflows/configure.yml index ec7cd612d..0e55a0795 100644 --- a/pybind11/.github/workflows/configure.yml +++ b/pybind11/.github/workflows/configure.yml @@ -24,7 +24,7 @@ jobs: strategy: fail-fast: false matrix: - runs-on: [ubuntu-20.04, macos-latest, windows-latest] + runs-on: [ubuntu-20.04, macos-13, windows-latest] arch: [x64] cmake: ["3.26"] @@ -35,9 +35,9 @@ jobs: - runs-on: ubuntu-20.04 arch: x64 - cmake: "3.27" + cmake: "3.29" - - runs-on: macos-latest + - runs-on: macos-13 arch: x64 cmake: "3.7" @@ -49,10 +49,10 @@ jobs: runs-on: ${{ matrix.runs-on }} steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup Python 3.7 - uses: actions/setup-python@v4 + uses: actions/setup-python@v5 with: python-version: 3.7 architecture: ${{ matrix.arch }} @@ -63,7 +63,7 @@ jobs: # An action for adding a specific version of CMake: # https://github.com/jwlawson/actions-setup-cmake - name: Setup CMake ${{ matrix.cmake }} - uses: jwlawson/actions-setup-cmake@v1.14 + uses: jwlawson/actions-setup-cmake@v2.0 with: cmake-version: ${{ matrix.cmake }} diff --git a/pybind11/.github/workflows/format.yml b/pybind11/.github/workflows/format.yml index b8242ee52..1eaa56e1c 100644 --- a/pybind11/.github/workflows/format.yml +++ b/pybind11/.github/workflows/format.yml @@ -25,13 +25,13 @@ jobs: name: Format runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 - - uses: actions/setup-python@v4 + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 with: python-version: "3.x" - name: Add matchers run: echo "::add-matcher::$GITHUB_WORKSPACE/.github/matchers/pylint.json" - - uses: pre-commit/action@v3.0.0 + - uses: pre-commit/action@v3.0.1 with: # Slow hooks are marked with manual - slow is okay here, run them too extra_args: --hook-stage manual --all-files @@ -43,7 +43,7 @@ jobs: runs-on: ubuntu-latest container: silkeh/clang:15-bullseye steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Install requirements run: apt-get update && apt-get install -y git python3-dev python3-pytest diff --git a/pybind11/.github/workflows/labeler.yml b/pybind11/.github/workflows/labeler.yml index 858a4a0e2..2152abbcf 100644 --- a/pybind11/.github/workflows/labeler.yml +++ b/pybind11/.github/workflows/labeler.yml @@ -14,7 +14,7 @@ jobs: pull-requests: write steps: - - uses: actions/labeler@main + - uses: actions/labeler@v5 if: > github.event.pull_request.merged == true && !startsWith(github.event.pull_request.title, 'chore(deps):') && diff --git a/pybind11/.github/workflows/pip.yml b/pybind11/.github/workflows/pip.yml index d6687b441..a054ce695 100644 --- a/pybind11/.github/workflows/pip.yml +++ b/pybind11/.github/workflows/pip.yml @@ -21,19 +21,18 @@ env: jobs: # This builds the sdists and wheels and makes sure the files are exactly as - # expected. Using Windows and Python 3.6, since that is often the most - # challenging matrix element. + # expected. test-packaging: - name: 🐍 3.6 • 📦 tests • windows-latest + name: 🐍 3.8 • 📦 tests • windows-latest runs-on: windows-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - - name: Setup 🐍 3.6 - uses: actions/setup-python@v4 + - name: Setup 🐍 3.8 + uses: actions/setup-python@v5 with: - python-version: 3.6 + python-version: 3.8 - name: Prepare env run: | @@ -50,10 +49,10 @@ jobs: runs-on: ubuntu-latest steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - name: Setup 🐍 3.8 - uses: actions/setup-python@v4 + uses: actions/setup-python@v5 with: python-version: 3.8 @@ -73,13 +72,13 @@ jobs: run: twine check dist/* - name: Save standard package - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@v4 with: name: standard path: dist/pybind11-* - name: Save global package - uses: actions/upload-artifact@v3 + uses: actions/upload-artifact@v4 with: name: global path: dist/pybind11_global-* @@ -92,23 +91,27 @@ jobs: runs-on: ubuntu-latest if: github.event_name == 'release' && github.event.action == 'published' needs: [packaging] + environment: pypi + permissions: + id-token: write + attestations: write + contents: read steps: - - uses: actions/setup-python@v4 - with: - python-version: "3.x" - # Downloads all to directories matching the artifact names - - uses: actions/download-artifact@v3 + - uses: actions/download-artifact@v4 + + - name: Generate artifact attestation for sdist and wheel + uses: actions/attest-build-provenance@173725a1209d09b31f9d30a3890cf2757ebbff0d # v1.1.2 + with: + subject-path: "*/pybind11*" - name: Publish standard package uses: pypa/gh-action-pypi-publish@release/v1 with: - password: ${{ secrets.pypi_password }} packages-dir: standard/ - name: Publish global package uses: pypa/gh-action-pypi-publish@release/v1 with: - password: ${{ secrets.pypi_password_global }} packages-dir: global/ diff --git a/pybind11/.github/workflows/upstream.yml b/pybind11/.github/workflows/upstream.yml index dd8a1c960..389260038 100644 --- a/pybind11/.github/workflows/upstream.yml +++ b/pybind11/.github/workflows/upstream.yml @@ -13,30 +13,30 @@ concurrency: env: PIP_BREAK_SYSTEM_PACKAGES: 1 - PIP_ONLY_BINARY: ":all:" # For cmake: VERBOSE: 1 jobs: standard: - name: "🐍 3.12 latest • ubuntu-latest • x64" + name: "🐍 3.13 latest • ubuntu-latest • x64" runs-on: ubuntu-latest # Only runs when the 'python dev' label is selected if: "contains(github.event.pull_request.labels.*.name, 'python dev')" steps: - - uses: actions/checkout@v3 + - uses: actions/checkout@v4 - - name: Setup Python 3.12 - uses: actions/setup-python@v4 + - name: Setup Python 3.13 + uses: actions/setup-python@v5 with: - python-version: "3.12-dev" + python-version: "3.13" + allow-prereleases: true - name: Setup Boost run: sudo apt-get install libboost-dev - name: Update CMake - uses: jwlawson/actions-setup-cmake@v1.14 + uses: jwlawson/actions-setup-cmake@v2.0 - name: Run pip installs run: | diff --git a/pybind11/.pre-commit-config.yaml b/pybind11/.pre-commit-config.yaml index 86ac965d9..3cec1ebe0 100644 --- a/pybind11/.pre-commit-config.yaml +++ b/pybind11/.pre-commit-config.yaml @@ -25,33 +25,28 @@ repos: # Clang format the codebase automatically - repo: https://github.com/pre-commit/mirrors-clang-format - rev: "v16.0.6" + rev: "v18.1.5" hooks: - id: clang-format types_or: [c++, c, cuda] -# Black, the code formatter, natively supports pre-commit -- repo: https://github.com/psf/black - rev: "23.3.0" # Keep in sync with blacken-docs - hooks: - - id: black - -# Ruff, the Python auto-correcting linter written in Rust +# Ruff, the Python auto-correcting linter/formatter written in Rust - repo: https://github.com/astral-sh/ruff-pre-commit - rev: v0.0.276 + rev: v0.4.7 hooks: - id: ruff args: ["--fix", "--show-fixes"] + - id: ruff-format # Check static types with mypy - repo: https://github.com/pre-commit/mirrors-mypy - rev: "v1.4.1" + rev: "v1.10.0" hooks: - id: mypy args: [] exclude: ^(tests|docs)/ additional_dependencies: - - markdown-it-py<3 # Drop this together with dropping Python 3.7 support. + - markdown-it-py - nox - rich - types-setuptools @@ -67,7 +62,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: "v4.4.0" + rev: "v4.6.0" hooks: - id: check-added-large-files - id: check-case-conflict @@ -83,22 +78,22 @@ repos: - id: trailing-whitespace # Also code format the docs -- repo: https://github.com/asottile/blacken-docs - rev: "1.14.0" +- repo: https://github.com/adamchainz/blacken-docs + rev: "1.16.0" hooks: - id: blacken-docs additional_dependencies: - - black==23.3.0 # keep in sync with black hook + - black==23.* # Changes tabs to spaces - repo: https://github.com/Lucas-C/pre-commit-hooks - rev: "v1.5.1" + rev: "v1.5.5" hooks: - id: remove-tabs # Avoid directional quotes - repo: https://github.com/sirosen/texthooks - rev: "0.5.0" + rev: "0.6.6" hooks: - id: fix-ligatures - id: fix-smartquotes @@ -124,15 +119,15 @@ repos: # Use tools/codespell_ignore_lines_from_errors.py # to rebuild .codespell-ignore-lines - repo: https://github.com/codespell-project/codespell - rev: "v2.2.5" + rev: "v2.3.0" hooks: - id: codespell exclude: ".supp$" - args: ["-x.codespell-ignore-lines", "-Lccompiler"] + args: ["-x.codespell-ignore-lines", "-Lccompiler,intstruct"] # Check for common shell mistakes - repo: https://github.com/shellcheck-py/shellcheck-py - rev: "v0.9.0.5" + rev: "v0.10.0.1" hooks: - id: shellcheck @@ -142,12 +137,20 @@ repos: - id: disallow-caps name: Disallow improper capitalization language: pygrep - entry: PyBind|Numpy|Cmake|CCache|PyTest + entry: PyBind|\bNumpy\b|Cmake|CCache|PyTest exclude: ^\.pre-commit-config.yaml$ # PyLint has native support - not always usable, but works for us - repo: https://github.com/PyCQA/pylint - rev: "v3.0.0a6" + rev: "v3.2.2" hooks: - id: pylint files: ^pybind11 + +# Check schemas on some of our YAML files +- repo: https://github.com/python-jsonschema/check-jsonschema + rev: 0.28.4 + hooks: + - id: check-readthedocs + - id: check-github-workflows + - id: check-dependabot diff --git a/pybind11/.readthedocs.yml b/pybind11/.readthedocs.yml index c9c61617c..a2b802f73 100644 --- a/pybind11/.readthedocs.yml +++ b/pybind11/.readthedocs.yml @@ -1,3 +1,20 @@ +# https://blog.readthedocs.com/migrate-configuration-v2/ + +version: 2 + +build: + os: ubuntu-22.04 + apt_packages: + - librsvg2-bin + tools: + python: "3.11" + +sphinx: + configuration: docs/conf.py + python: - version: 3 -requirements_file: docs/requirements.txt + install: + - requirements: docs/requirements.txt + +formats: + - pdf diff --git a/pybind11/CMakeLists.txt b/pybind11/CMakeLists.txt index 87ec10346..3526a1a66 100644 --- a/pybind11/CMakeLists.txt +++ b/pybind11/CMakeLists.txt @@ -5,15 +5,25 @@ # All rights reserved. Use of this source code is governed by a # BSD-style license that can be found in the LICENSE file. +# Propagate this policy (FindPythonInterp removal) so it can be detected later +if(NOT CMAKE_VERSION VERSION_LESS "3.27") + cmake_policy(GET CMP0148 _pybind11_cmp0148) +endif() + cmake_minimum_required(VERSION 3.5) -# The `cmake_minimum_required(VERSION 3.5...3.26)` syntax does not work with +# The `cmake_minimum_required(VERSION 3.5...3.29)` syntax does not work with # some versions of VS that have a patched CMake 3.11. This forces us to emulate # the behavior using the following workaround: -if(${CMAKE_VERSION} VERSION_LESS 3.26) +if(${CMAKE_VERSION} VERSION_LESS 3.29) cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}) else() - cmake_policy(VERSION 3.26) + cmake_policy(VERSION 3.29) +endif() + +if(_pybind11_cmp0148) + cmake_policy(SET CMP0148 ${_pybind11_cmp0148}) + unset(_pybind11_cmp0148) endif() # Avoid infinite recursion if tests include this as a subdirectory @@ -82,33 +92,59 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR) set(pybind11_system "") set_property(GLOBAL PROPERTY USE_FOLDERS ON) + if(CMAKE_VERSION VERSION_LESS "3.18") + set(_pybind11_findpython_default OFF) + else() + set(_pybind11_findpython_default ON) + endif() else() set(PYBIND11_MASTER_PROJECT OFF) set(pybind11_system SYSTEM) + set(_pybind11_findpython_default OFF) endif() # Options option(PYBIND11_INSTALL "Install pybind11 header files?" ${PYBIND11_MASTER_PROJECT}) option(PYBIND11_TEST "Build pybind11 test suite?" ${PYBIND11_MASTER_PROJECT}) option(PYBIND11_NOPYTHON "Disable search for Python" OFF) +option(PYBIND11_DISABLE_HANDLE_TYPE_NAME_DEFAULT_IMPLEMENTATION + "To enforce that a handle_type_name<> specialization exists" OFF) option(PYBIND11_SIMPLE_GIL_MANAGEMENT "Use simpler GIL management logic that does not support disassociation" OFF) +option(PYBIND11_NUMPY_1_ONLY + "Disable NumPy 2 support to avoid changes to previous pybind11 versions." OFF) set(PYBIND11_INTERNALS_VERSION "" CACHE STRING "Override the ABI version, may be used to enable the unstable ABI.") +option(PYBIND11_USE_CROSSCOMPILING "Respect CMAKE_CROSSCOMPILING" OFF) +if(PYBIND11_DISABLE_HANDLE_TYPE_NAME_DEFAULT_IMPLEMENTATION) + add_compile_definitions(PYBIND11_DISABLE_HANDLE_TYPE_NAME_DEFAULT_IMPLEMENTATION) +endif() if(PYBIND11_SIMPLE_GIL_MANAGEMENT) add_compile_definitions(PYBIND11_SIMPLE_GIL_MANAGEMENT) endif() +if(PYBIND11_NUMPY_1_ONLY) + add_compile_definitions(PYBIND11_NUMPY_1_ONLY) +endif() cmake_dependent_option( USE_PYTHON_INCLUDE_DIR "Install pybind11 headers in Python include directory instead of default installation prefix" OFF "PYBIND11_INSTALL" OFF) -cmake_dependent_option(PYBIND11_FINDPYTHON "Force new FindPython" OFF +cmake_dependent_option(PYBIND11_FINDPYTHON "Force new FindPython" ${_pybind11_findpython_default} "NOT CMAKE_VERSION VERSION_LESS 3.12" OFF) +# Allow PYTHON_EXECUTABLE if in FINDPYTHON mode and building pybind11's tests +# (makes transition easier while we support both modes). +if(PYBIND11_MASTER_PROJECT + AND PYBIND11_FINDPYTHON + AND DEFINED PYTHON_EXECUTABLE + AND NOT DEFINED Python_EXECUTABLE) + set(Python_EXECUTABLE "${PYTHON_EXECUTABLE}") +endif() + # NB: when adding a header don't forget to also add it to setup.py set(PYBIND11_HEADERS include/pybind11/detail/class.h @@ -132,6 +168,7 @@ set(PYBIND11_HEADERS include/pybind11/embed.h include/pybind11/eval.h include/pybind11/gil.h + include/pybind11/gil_safe_call_once.h include/pybind11/iostream.h include/pybind11/functional.h include/pybind11/numpy.h @@ -141,7 +178,8 @@ set(PYBIND11_HEADERS include/pybind11/stl.h include/pybind11/stl_bind.h include/pybind11/stl/filesystem.h - include/pybind11/type_caster_pyobject_ptr.h) + include/pybind11/type_caster_pyobject_ptr.h + include/pybind11/typing.h) # Compare with grep and warn if mismatched if(PYBIND11_MASTER_PROJECT AND NOT CMAKE_VERSION VERSION_LESS 3.12) @@ -262,6 +300,7 @@ if(PYBIND11_INSTALL) tools/pybind11Common.cmake tools/pybind11Tools.cmake tools/pybind11NewTools.cmake + tools/pybind11GuessPythonExtSuffix.cmake DESTINATION ${PYBIND11_CMAKECONFIG_INSTALL_DIR}) if(NOT PYBIND11_EXPORT_NAME) @@ -277,7 +316,21 @@ if(PYBIND11_INSTALL) # pkg-config support if(NOT prefix_for_pc_file) - set(prefix_for_pc_file "${CMAKE_INSTALL_PREFIX}") + if(IS_ABSOLUTE "${CMAKE_INSTALL_DATAROOTDIR}") + set(prefix_for_pc_file "${CMAKE_INSTALL_PREFIX}") + else() + set(pc_datarootdir "${CMAKE_INSTALL_DATAROOTDIR}") + if(CMAKE_VERSION VERSION_LESS 3.20) + set(prefix_for_pc_file "\${pcfiledir}/..") + while(pc_datarootdir) + get_filename_component(pc_datarootdir "${pc_datarootdir}" DIRECTORY) + string(APPEND prefix_for_pc_file "/..") + endwhile() + else() + cmake_path(RELATIVE_PATH CMAKE_INSTALL_PREFIX BASE_DIRECTORY CMAKE_INSTALL_DATAROOTDIR + OUTPUT_VARIABLE prefix_for_pc_file) + endif() + endif() endif() join_paths(includedir_for_pc_file "\${prefix}" "${CMAKE_INSTALL_INCLUDEDIR}") configure_file("${CMAKE_CURRENT_SOURCE_DIR}/tools/pybind11.pc.in" diff --git a/pybind11/README.rst b/pybind11/README.rst index 80213a406..0d1e1d291 100644 --- a/pybind11/README.rst +++ b/pybind11/README.rst @@ -34,12 +34,12 @@ dependency. Think of this library as a tiny self-contained version of Boost.Python with everything stripped away that isn't relevant for binding generation. Without comments, the core header files only require ~4K -lines of code and depend on Python (3.6+, or PyPy) and the C++ +lines of code and depend on Python (3.7+, or PyPy) and the C++ standard library. This compact implementation was possible thanks to -some of the new C++11 language features (specifically: tuples, lambda -functions and variadic templates). Since its creation, this library has -grown beyond Boost.Python in many ways, leading to dramatically simpler -binding code in many common situations. +some C++11 language features (specifically: tuples, lambda functions and +variadic templates). Since its creation, this library has grown beyond +Boost.Python in many ways, leading to dramatically simpler binding code in many +common situations. Tutorial and reference documentation is provided at `pybind11.readthedocs.io `_. @@ -71,6 +71,7 @@ pybind11 can map the following core C++ features to Python: - Internal references with correct reference counting - C++ classes with virtual (and pure virtual) methods can be extended in Python +- Integrated NumPy support (NumPy 2 requires pybind11 2.12+) Goodies ------- @@ -78,7 +79,7 @@ Goodies In addition to the core functionality, pybind11 provides some extra goodies: -- Python 3.6+, and PyPy3 7.3 are supported with an implementation-agnostic +- Python 3.7+, and PyPy3 7.3 are supported with an implementation-agnostic interface (pybind11 2.9 was the last version to support Python 2 and 3.5). - It is possible to bind C++11 lambda functions with captured diff --git a/pybind11/docs/advanced/embedding.rst b/pybind11/docs/advanced/embedding.rst index e6a1686f8..cbed82158 100644 --- a/pybind11/docs/advanced/embedding.rst +++ b/pybind11/docs/advanced/embedding.rst @@ -18,7 +18,7 @@ information, see :doc:`/compiling`. .. code-block:: cmake - cmake_minimum_required(VERSION 3.5...3.26) + cmake_minimum_required(VERSION 3.5...3.29) project(example) find_package(pybind11 REQUIRED) # or `add_subdirectory(pybind11)` diff --git a/pybind11/docs/advanced/exceptions.rst b/pybind11/docs/advanced/exceptions.rst index 53981dc08..e20f42b5f 100644 --- a/pybind11/docs/advanced/exceptions.rst +++ b/pybind11/docs/advanced/exceptions.rst @@ -127,8 +127,7 @@ before a global translator is tried. Inside the translator, ``std::rethrow_exception`` should be used within a try block to re-throw the exception. One or more catch clauses to catch the appropriate exceptions should then be used with each clause using -``PyErr_SetString`` to set a Python exception or ``ex(string)`` to set -the python exception to a custom exception type (see below). +``py::set_error()`` (see below). To declare a custom Python exception type, declare a ``py::exception`` variable and use this in the associated exception translator (note: it is often useful @@ -142,14 +141,16 @@ standard python RuntimeError: .. code-block:: cpp - static py::exception exc(m, "MyCustomError"); + PYBIND11_CONSTINIT static py::gil_safe_call_once_and_store exc_storage; + exc_storage.call_once_and_store_result( + [&]() { return py::exception(m, "MyCustomError"); }); py::register_exception_translator([](std::exception_ptr p) { try { if (p) std::rethrow_exception(p); } catch (const MyCustomException &e) { - exc(e.what()); + py::set_error(exc_storage.get_stored(), e.what()); } catch (const OtherException &e) { - PyErr_SetString(PyExc_RuntimeError, e.what()); + py::set_error(PyExc_RuntimeError, e.what()); } }); @@ -168,8 +169,7 @@ section. .. note:: - Call either ``PyErr_SetString`` or a custom exception's call - operator (``exc(string)``) for every exception caught in a custom exception + Call ``py::set_error()`` for every exception caught in a custom exception translator. Failure to do so will cause Python to crash with ``SystemError: error return without exception set``. @@ -200,7 +200,7 @@ If module1 has the following translator: try { if (p) std::rethrow_exception(p); } catch (const std::invalid_argument &e) { - PyErr_SetString("module1 handled this") + py::set_error(PyExc_ArgumentError, "module1 handled this"); } } @@ -212,7 +212,7 @@ and module2 has the following similar translator: try { if (p) std::rethrow_exception(p); } catch (const std::invalid_argument &e) { - PyErr_SetString("module2 handled this") + py::set_error(PyExc_ArgumentError, "module2 handled this"); } } @@ -312,11 +312,11 @@ error protocol, which is outlined here. After calling the Python C API, if Python returns an error, ``throw py::error_already_set();``, which allows pybind11 to deal with the exception and pass it back to the Python interpreter. This includes calls to -the error setting functions such as ``PyErr_SetString``. +the error setting functions such as ``py::set_error()``. .. code-block:: cpp - PyErr_SetString(PyExc_TypeError, "C API type error demo"); + py::set_error(PyExc_TypeError, "C API type error demo"); throw py::error_already_set(); // But it would be easier to simply... diff --git a/pybind11/docs/advanced/functions.rst b/pybind11/docs/advanced/functions.rst index 69e3d8a1d..372934b09 100644 --- a/pybind11/docs/advanced/functions.rst +++ b/pybind11/docs/advanced/functions.rst @@ -16,7 +16,7 @@ lifetime of objects managed by them. This can lead to issues when creating bindings for functions that return a non-trivial type. Just by looking at the type information, it is not clear whether Python should take charge of the returned value and eventually free its resources, or if this is handled on the -C++ side. For this reason, pybind11 provides a several *return value policy* +C++ side. For this reason, pybind11 provides several *return value policy* annotations that can be passed to the :func:`module_::def` and :func:`class_::def` functions. The default policy is :enum:`return_value_policy::automatic`. diff --git a/pybind11/docs/advanced/misc.rst b/pybind11/docs/advanced/misc.rst index 805ec838f..ddd7f3937 100644 --- a/pybind11/docs/advanced/misc.rst +++ b/pybind11/docs/advanced/misc.rst @@ -398,3 +398,32 @@ before they are used as a parameter or return type of a function: pyFoo.def(py::init()); pyBar.def(py::init()); } + +Setting inner type hints in docstrings +====================================== + +When you use pybind11 wrappers for ``list``, ``dict``, and other generic python +types, the docstring will just display the generic type. You can convey the +inner types in the docstring by using a special 'typed' version of the generic +type. + +.. code-block:: cpp + + PYBIND11_MODULE(example, m) { + m.def("pass_list_of_str", [](py::typing::List arg) { + // arg can be used just like py::list + )); + } + +The resulting docstring will be ``pass_list_of_str(arg0: list[str]) -> None``. + +The following special types are available in ``pybind11/typing.h``: + +* ``py::Tuple`` +* ``py::Dict`` +* ``py::List`` +* ``py::Set`` +* ``py::Callable`` + +.. warning:: Just like in python, these are merely hints. They don't actually + enforce the types of their contents at runtime or compile time. diff --git a/pybind11/docs/advanced/pycpp/numpy.rst b/pybind11/docs/advanced/pycpp/numpy.rst index 07c969305..d09a2cea2 100644 --- a/pybind11/docs/advanced/pycpp/numpy.rst +++ b/pybind11/docs/advanced/pycpp/numpy.rst @@ -378,8 +378,6 @@ uses of ``py::array``: - ``.itemsize()`` returns the size of an item in bytes, i.e. ``sizeof(T)``. -- ``.ndim()`` returns the number of dimensions. - - ``.shape(n)`` returns the size of dimension ``n`` - ``.size()`` returns the total number of elements (i.e. the product of the shapes). diff --git a/pybind11/docs/benchmark.py b/pybind11/docs/benchmark.py index 2150b6ca7..a273674f4 100644 --- a/pybind11/docs/benchmark.py +++ b/pybind11/docs/benchmark.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import datetime as dt import os import random @@ -70,7 +72,7 @@ def generate_dummy_code_boost(nclasses=10): for codegen in [generate_dummy_code_pybind11, generate_dummy_code_boost]: print("{") - for i in range(0, 10): + for i in range(10): nclasses = 2**i with open("test.cpp", "w") as f: f.write(codegen(nclasses)) diff --git a/pybind11/docs/changelog.rst b/pybind11/docs/changelog.rst index add3fd66b..ab6c713c1 100644 --- a/pybind11/docs/changelog.rst +++ b/pybind11/docs/changelog.rst @@ -10,8 +10,303 @@ Changes will be added here periodically from the "Suggested changelog entry" block in pull request descriptions. +IN DEVELOPMENT +-------------- + +Changes will be summarized here periodically. + +Version 2.13.1 (June 26, 2024) +------------------------------ + +New Features: + +* Add support for ``Typing.Callable[..., T]``. + `#5202 `_ + +Bug fixes: + +* Avoid aligned allocation in free-threaded build in order to support macOS + versions before 10.14. + `#5200 `_ + +Version 2.13.0 (June 25, 2024) +------------------------------ + +New Features: + +* Support free-threaded CPython (3.13t). Add ``py::mod_gil_not_used()`` tag to + indicate if a module supports running with the GIL disabled. + `#5148 `_ + +* Support for Python 3.6 was removed. (Official end-of-life: 2021-12-23). + `#5177 `_ + +* ``py::list`` gained a ``.clear()`` method. + `#5153 `_ + + +.. feat(types) + +* Support for ``Union``, ``Optional``, ``type[T]``, ``typing.TypeGuard``, + ``typing.TypeIs``, ``typing.Never``, ``typing.NoReturn`` and + ``typing.Literal`` was added to ``pybind11/typing.h``. + `#5166 `_ + `#5165 `_ + `#5194 `_ + `#5193 `_ + `#5192 `_ + + +.. feat(cmake) + +* In CMake, if ``PYBIND11_USE_CROSSCOMPILING`` is enabled, then + ``CMAKE_CROSSCOMPILING`` will be respected and will keep pybind11 from + accessing the interpreter during configuration. Several CMake variables will + be required in this case, but can be deduced from the environment variable + ``SETUPTOOLS_EXT_SUFFIX``. The default (currently ``OFF``) may be changed in + the future. + `#5083 `_ + + +Bug fixes: + +* A refcount bug (leading to heap-use-after-free) involving trampoline + functions with ``PyObject *`` return type was fixed. + `#5156 `_ + +* Return ``py::ssize_t`` from ``.ref_count()`` instead of ``int``. + `#5139 `_ + +* A subtle bug involving C++ types with unusual ``operator&`` overrides + was fixed. + `#5189 `_ + +* Support Python 3.13 with minor fix, add to CI. + `#5127 `_ + + +.. fix(cmake) + +* Fix mistake affecting old cmake and old boost. + `#5149 `_ + + +Documentation: + +* Build docs updated to feature scikit-build-core and meson-python, and updated + setuptools instructions. + `#5168 `_ + + +Tests: + +* Avoid immortal objects in tests. + `#5150 `_ + + +CI: + +* Compile against Python 3.13t in CI. + +* Use ``macos-13`` (Intel) for CI jobs for now (will drop Python 3.7 soon). + `#5109 `_ + +* Releases now have artifact attestations, visible at + https://github.com/pybind/pybind11/attestations. + `#5196 `_ + +Other: + +* Some cleanup in preparation for 3.13 support. + `#5137 `_ + +* Avoid a warning by ensuring an iterator end check is included in release mode. + `#5129 `_ + +* Bump max cmake to 3.29. + `#5075 `_ + +* Update docs and noxfile. + `#5071 `_ + + +Version 2.12.0 (March 27, 2024) +------------------------------- + +New Features: + +* ``pybind11`` now supports compiling for + `NumPy 2 `_. Most + code shouldn't change (see :ref:`upgrade-guide-2.12` for details). However, + if you experience issues you can define ``PYBIND11_NUMPY_1_ONLY`` to disable + the new support for now, but this will be removed in the future. + `#5050 `_ + +* ``pybind11/gil_safe_call_once.h`` was added (it needs to be included + explicitly). The primary use case is GIL-safe initialization of C++ + ``static`` variables. + `#4877 `_ + +* Support move-only iterators in ``py::make_iterator``, + ``py::make_key_iterator``, ``py::make_value_iterator``. + `#4834 `_ + +* Two simple ``py::set_error()`` functions were added and the documentation was + updated accordingly. In particular, ``py::exception<>::operator()`` was + deprecated (use one of the new functions instead). The documentation for + ``py::exception<>`` was further updated to not suggest code that may result + in undefined behavior. + `#4772 `_ + +Bug fixes: + +* Removes potential for Undefined Behavior during process teardown. + `#4897 `_ + +* Improve compatibility with the nvcc compiler (especially CUDA 12.1/12.2). + `#4893 `_ + +* ``pybind11/numpy.h`` now imports NumPy's ``multiarray`` and ``_internal`` + submodules with paths depending on the installed version of NumPy (for + compatibility with NumPy 2). + `#4857 `_ + +* Builtins collections names in docstrings are now consistently rendered in + lowercase (list, set, dict, tuple), in accordance with PEP 585. + `#4833 `_ + +* Added ``py::typing::Iterator``, ``py::typing::Iterable``. + `#4832 `_ + +* Render ``py::function`` as ``Callable`` in docstring. + `#4829 `_ + +* Also bump ``PYBIND11_INTERNALS_VERSION`` for MSVC, which unlocks two new + features without creating additional incompatibilities. + `#4819 `_ + +* Guard against crashes/corruptions caused by modules built with different MSVC + versions. + `#4779 `_ + +* A long-standing bug in the handling of Python multiple inheritance was fixed. + See PR #4762 for the rather complex details. + `#4762 `_ + +* Fix ``bind_map`` with ``using`` declarations. + `#4952 `_ + +* Qualify ``py::detail::concat`` usage to avoid ADL selecting one from + somewhere else, such as modernjson's concat. + `#4955 `_ + +* Use new PyCode API on Python 3.12+. + `#4916 `_ + +* Minor cleanup from warnings reported by Clazy. + `#4988 `_ + +* Remove typing and duplicate ``class_`` for ``KeysView``/``ValuesView``/``ItemsView``. + `#4985 `_ + +* Use ``PyObject_VisitManagedDict()`` and ``PyObject_ClearManagedDict()`` on Python 3.13 and newer. + `#4973 `_ + +* Update ``make_static_property_type()`` to make it compatible with Python 3.13. + `#4971 `_ + +.. fix(types) + +* Render typed iterators for ``make_iterator``, ``make_key_iterator``, + ``make_value_iterator``. + `#4876 `_ + +* Add several missing type name specializations. + `#5073 `_ + +* Change docstring render for ``py::buffer``, ``py::sequence`` and + ``py::handle`` (to ``Buffer``, ``Sequence``, ``Any``). + `#4831 `_ + +* Fixed ``base_enum.__str__`` docstring. + `#4827 `_ + +* Enforce single line docstring signatures. + `#4735 `_ + +* Special 'typed' wrappers now available in ``typing.h`` to annotate tuple, dict, + list, set, and function. + `#4259 `_ + +* Create ``handle_type_name`` specialization to type-hint variable length tuples. + `#5051 `_ + +.. fix(build) + +* Setting ``PYBIND11_FINDPYTHON`` to OFF will force the old FindPythonLibs mechanism to be used. + `#5042 `_ + +* Skip empty ``PYBIND11_PYTHON_EXECUTABLE_LAST`` for the first cmake run. + `#4856 `_ + +* Fix FindPython mode exports & avoid ``pkg_resources`` if + ``importlib.metadata`` available. + `#4941 `_ + +* ``Python_ADDITIONAL_VERSIONS`` (classic search) now includes 3.12. + `#4909 `_ + +* ``pybind11.pc`` is now relocatable by default as long as install destinations + are not absolute paths. + `#4830 `_ + +* Correctly detect CMake FindPython removal when used as a subdirectory. + `#4806 `_ + +* Don't require the libs component on CMake 3.18+ when using + PYBIND11_FINDPYTHON (fixes manylinux builds). + `#4805 `_ + +* ``pybind11_strip`` is no longer automatically applied when + ``CMAKE_BUILD_TYPE`` is unset. + `#4780 `_ + +* Support ``DEBUG_POSFIX`` correctly for debug builds. + `#4761 `_ + +* Hardcode lto/thin lto for Emscripten cross-compiles. + `#4642 `_ + +* Upgrade maximum supported CMake version to 3.27 to fix CMP0148 warnings. + `#4786 `_ + +Documentation: + +* Small fix to grammar in ``functions.rst``. + `#4791 `_ + +* Remove upper bound in example pyproject.toml for setuptools. + `#4774 `_ + +CI: + +* CI: Update NVHPC to 23.5 and Ubuntu 20.04. + `#4764 `_ + +* Test on PyPy 3.10. + `#4714 `_ + +Other: + +* Use Ruff formatter instead of Black. + `#4912 `_ + +* An ``assert()`` was added to help Coverty avoid generating a false positive. + `#4817 `_ + + Version 2.11.1 (July 17, 2023) ------------------------------ +------------------------------ Changes: @@ -26,7 +321,7 @@ Changes: Version 2.11.0 (July 14, 2023) ------------------------------ +------------------------------ New features: diff --git a/pybind11/docs/compiling.rst b/pybind11/docs/compiling.rst index 1fd098bec..0b7c178b0 100644 --- a/pybind11/docs/compiling.rst +++ b/pybind11/docs/compiling.rst @@ -3,15 +3,123 @@ Build systems ############# +For an overview of Python packaging including compiled packaging with a pybind11 +example, along with a cookiecutter that includes several pybind11 options, see +the `Scientific Python Development Guide`_. + +.. _Scientific Python Development Guide: https://learn.scientific-python.org/development/guides/packaging-compiled/ + +.. scikit-build-core: + +Modules with CMake +================== + +A Python extension module can be created with just a few lines of code: + +.. code-block:: cmake + + cmake_minimum_required(VERSION 3.15...3.29) + project(example LANGUAGES CXX) + + set(PYBIND11_FINDPYTHON ON) + find_package(pybind11 CONFIG REQUIRED) + + pybind11_add_module(example example.cpp) + install(TARGET example DESTINATION .) + +(You use the ``add_subdirectory`` instead, see the example in :ref:`cmake`.) In +this example, the code is located in a file named :file:`example.cpp`. Either +method will import the pybind11 project which provides the +``pybind11_add_module`` function. It will take care of all the details needed +to build a Python extension module on any platform. + +To build with pip, build, cibuildwheel, uv, or other Python tools, you can +add a ``pyproject.toml`` file like this: + +.. code-block:: toml + + [build-system] + requires = ["scikit-build-core", "pybind11"] + build-backend = "scikit_build_core.build" + + [project] + name = "example" + version = "0.1.0" + +You don't need setuptools files like ``MANIFEST.in``, ``setup.py``, or +``setup.cfg``, as this is not setuptools. See `scikit-build-core`_ for details. +For projects you plan to upload to PyPI, be sure to fill out the ``[project]`` +table with other important metadata as well (see `Writing pyproject.toml`_). + +A working sample project can be found in the [scikit_build_example]_ +repository. An older and harder-to-maintain method is in [cmake_example]_. More +details about our cmake support can be found below in :ref:`cmake`. + +.. _scikit-build-core: https://scikit-build-core.readthedocs.io + +.. [scikit_build_example] https://github.com/pybind/scikit_build_example + +.. [cmake_example] https://github.com/pybind/cmake_example + +.. _modules-meson-python: + +Modules with meson-python +========================= + +You can also build a package with `Meson`_ using `meson-python`_, if you prefer +that. Your ``meson.build`` file would look something like this: + +.. _meson-example: + +.. code-block:: meson + + project( + 'example', + 'cpp', + version: '0.1.0', + default_options: [ + 'cpp_std=c++11', + ], + ) + + py = import('python').find_installation(pure: false) + pybind11_dep = dependency('pybind11') + + py.extension_module('example', + 'example.cpp', + install: true, + dependencies : [pybind11_dep], + ) + + +And you would need a ``pyproject.toml`` file like this: + +.. code-block:: toml + + [build-system] + requires = ["meson-python", "pybind11"] + build-backend = "mesonpy" + +Meson-python *requires* your project to be in git (or mercurial) as it uses it +for the SDist creation. For projects you plan to upload to PyPI, be sure to fill out the +``[project]`` table as well (see `Writing pyproject.toml`_). + + +.. _Writing pyproject.toml: https://packaging.python.org/en/latest/guides/writing-pyproject-toml + +.. _meson: https://mesonbuild.com + +.. _meson-python: https://meson-python.readthedocs.io/en/latest + .. _build-setuptools: -Building with setuptools -======================== +Modules with setuptools +======================= -For projects on PyPI, building with setuptools is the way to go. Sylvain Corlay -has kindly provided an example project which shows how to set up everything, -including automatic generation of documentation using Sphinx. Please refer to -the [python_example]_ repository. +For projects on PyPI, a historically popular option is setuptools. Sylvain +Corlay has kindly provided an example project which shows how to set up +everything, including automatic generation of documentation using Sphinx. +Please refer to the [python_example]_ repository. .. [python_example] https://github.com/pybind/python_example @@ -21,11 +129,11 @@ To use pybind11 inside your ``setup.py``, you have to have some system to ensure that ``pybind11`` is installed when you build your package. There are four possible ways to do this, and pybind11 supports all four: You can ask all users to install pybind11 beforehand (bad), you can use -:ref:`setup_helpers-pep518` (good, but very new and requires Pip 10), -:ref:`setup_helpers-setup_requires` (discouraged by Python packagers now that -PEP 518 is available, but it still works everywhere), or you can -:ref:`setup_helpers-copy-manually` (always works but you have to manually sync -your copy to get updates). +:ref:`setup_helpers-pep518` (good), ``setup_requires=`` (discouraged), or you +can :ref:`setup_helpers-copy-manually` (works but you have to manually sync +your copy to get updates). Third party packagers like conda-forge generally +strongly prefer the ``pyproject.toml`` method, as it gives them control over +the ``pybind11`` version, and they may apply patches, etc. An example of a ``setup.py`` using pybind11's helpers: @@ -122,70 +230,41 @@ version number that includes the number of commits since your last tag and a hash for a dirty directory. Another way to force a rebuild is purge your cache or use Pip's ``--no-cache-dir`` option. +You also need a ``MANIFEST.in`` file to include all relevant files so that you +can make an SDist. If you use `pypa-build`_, that will build an SDist then a +wheel from that SDist by default, so you can look inside those files (wheels +are just zip files with a ``.whl`` extension) to make sure you aren't missing +files. `check-manifest`_ (setuptools specific) or `check-sdist`_ (general) are +CLI tools that can compare the SDist contents with your source control. + .. [Ccache] https://ccache.dev .. [setuptools_scm] https://github.com/pypa/setuptools_scm .. _setup_helpers-pep518: -PEP 518 requirements (Pip 10+ required) ---------------------------------------- +Build requirements +------------------ -If you use `PEP 518's `_ -``pyproject.toml`` file, you can ensure that ``pybind11`` is available during -the compilation of your project. When this file exists, Pip will make a new -virtual environment, download just the packages listed here in ``requires=``, -and build a wheel (binary Python package). It will then throw away the -environment, and install your wheel. +With a ``pyproject.toml`` file, you can ensure that ``pybind11`` is available +during the compilation of your project. When this file exists, Pip will make a +new virtual environment, download just the packages listed here in +``requires=``, and build a wheel (binary Python package). It will then throw +away the environment, and install your wheel. Your ``pyproject.toml`` file will likely look something like this: .. code-block:: toml [build-system] - requires = ["setuptools>=42", "wheel", "pybind11~=2.6.1"] + requires = ["setuptools", "pybind11"] build-backend = "setuptools.build_meta" -.. note:: - - The main drawback to this method is that a `PEP 517`_ compliant build tool, - such as Pip 10+, is required for this approach to work; older versions of - Pip completely ignore this file. If you distribute binaries (called wheels - in Python) using something like `cibuildwheel`_, remember that ``setup.py`` - and ``pyproject.toml`` are not even contained in the wheel, so this high - Pip requirement is only for source builds, and will not affect users of - your binary wheels. If you are building SDists and wheels, then - `pypa-build`_ is the recommended official tool. - .. _PEP 517: https://www.python.org/dev/peps/pep-0517/ -.. _cibuildwheel: https://cibuildwheel.readthedocs.io -.. _pypa-build: https://pypa-build.readthedocs.io/en/latest/ - -.. _setup_helpers-setup_requires: - -Classic ``setup_requires`` --------------------------- - -If you want to support old versions of Pip with the classic -``setup_requires=["pybind11"]`` keyword argument to setup, which triggers a -two-phase ``setup.py`` run, then you will need to use something like this to -ensure the first pass works (which has not yet installed the ``setup_requires`` -packages, since it can't install something it does not know about): - -.. code-block:: python - - try: - from pybind11.setup_helpers import Pybind11Extension - except ImportError: - from setuptools import Extension as Pybind11Extension - - -It doesn't matter that the Extension class is not the enhanced subclass for the -first pass run; and the second pass will have the ``setup_requires`` -requirements. - -This is obviously more of a hack than the PEP 518 method, but it supports -ancient versions of Pip. +.. _cibuildwheel: https://cibuildwheel.pypa.io +.. _pypa-build: https://build.pypa.io/en/latest/ +.. _check-manifest: https://pypi.io/project/check-manifest +.. _check-sdist: https://pypi.io/project/check-sdist .. _setup_helpers-copy-manually: @@ -231,32 +310,22 @@ the C++ source file. Python is then able to find the module and load it. .. [cppimport] https://github.com/tbenthompson/cppimport + + .. _cmake: Building with CMake =================== For C++ codebases that have an existing CMake-based build system, a Python -extension module can be created with just a few lines of code: +extension module can be created with just a few lines of code, as seen above in +the module section. Pybind11 currently supports a lower minimum if you don't +use the modern FindPython, though be aware that CMake 3.27 removed the old +mechanism, so pybind11 will automatically switch if the old mechanism is not +available. Please opt into the new mechanism if at all possible. Our default +may change in future versions. This is the minimum required: -.. code-block:: cmake - cmake_minimum_required(VERSION 3.5...3.26) - project(example LANGUAGES CXX) - - add_subdirectory(pybind11) - pybind11_add_module(example example.cpp) - -This assumes that the pybind11 repository is located in a subdirectory named -:file:`pybind11` and that the code is located in a file named :file:`example.cpp`. -The CMake command ``add_subdirectory`` will import the pybind11 project which -provides the ``pybind11_add_module`` function. It will take care of all the -details needed to build a Python extension module on any platform. - -A working sample project, including a way to invoke CMake from :file:`setup.py` for -PyPI integration, can be found in the [cmake_example]_ repository. - -.. [cmake_example] https://github.com/pybind/cmake_example .. versionchanged:: 2.6 CMake 3.4+ is required. @@ -264,6 +333,7 @@ PyPI integration, can be found in the [cmake_example]_ repository. .. versionchanged:: 2.11 CMake 3.5+ is required. + Further information can be found at :doc:`cmake/index`. pybind11_add_module @@ -356,7 +426,7 @@ with ``PYTHON_EXECUTABLE``. For example: .. code-block:: bash - cmake -DPYBIND11_PYTHON_VERSION=3.6 .. + cmake -DPYBIND11_PYTHON_VERSION=3.7 .. # Another method: cmake -DPYTHON_EXECUTABLE=/path/to/python .. @@ -423,7 +493,7 @@ existing targets instead: cmake_minimum_required(VERSION 3.15...3.22) project(example LANGUAGES CXX) - find_package(Python 3.6 COMPONENTS Interpreter Development REQUIRED) + find_package(Python 3.7 COMPONENTS Interpreter Development REQUIRED) find_package(pybind11 CONFIG REQUIRED) # or add_subdirectory(pybind11) @@ -498,7 +568,7 @@ You can use these targets to build complex applications. For example, the .. code-block:: cmake - cmake_minimum_required(VERSION 3.5...3.26) + cmake_minimum_required(VERSION 3.5...3.29) project(example LANGUAGES CXX) find_package(pybind11 REQUIRED) # or add_subdirectory(pybind11) @@ -556,7 +626,7 @@ information about usage in C++, see :doc:`/advanced/embedding`. .. code-block:: cmake - cmake_minimum_required(VERSION 3.5...3.26) + cmake_minimum_required(VERSION 3.5...3.29) project(example LANGUAGES CXX) find_package(pybind11 REQUIRED) # or add_subdirectory(pybind11) @@ -616,6 +686,13 @@ Building with Bazel You can build with the Bazel build system using the `pybind11_bazel `_ repository. +Building with Meson +=================== + +You can use Meson, which has support for ``pybind11`` as a dependency (internally +relying on our ``pkg-config`` support). See the :ref:`module example above `. + + Generating binding code automatically ===================================== @@ -639,3 +716,11 @@ cross-project dependency management. Additionally, it is able to autogenerate customizable pybind11-based wrappers by parsing C++ header files. .. [robotpy-build] https://robotpy-build.readthedocs.io + +[litgen]_ is an automatic python bindings generator with a focus on generating +documented and discoverable bindings: bindings will nicely reproduce the documentation +found in headers. It is is based on srcML (srcml.org), a highly scalable, multi-language +parsing tool with a developer centric approach. The API that you want to expose to python +must be C++14 compatible (but your implementation can use more modern constructs). + +.. [litgen] https://pthom.github.io/litgen diff --git a/pybind11/docs/conf.py b/pybind11/docs/conf.py index 6e24751e9..e5cba0382 100644 --- a/pybind11/docs/conf.py +++ b/pybind11/docs/conf.py @@ -11,6 +11,7 @@ # # All configuration values have a default; values that are commented out # serve to show the default. +from __future__ import annotations import os import re @@ -81,7 +82,7 @@ version = loc["__version__"] # # This is also used if you do content translation via gettext catalogs. # Usually you set "language" from the command line for these cases. -language = None +language = "en" # There are two options for replacing |today|: either, you set today to some # non-false value, then it is used: diff --git a/pybind11/docs/release.rst b/pybind11/docs/release.rst index 4950c3b88..47b5717ca 100644 --- a/pybind11/docs/release.rst +++ b/pybind11/docs/release.rst @@ -15,8 +15,8 @@ For example: For beta, ``PYBIND11_VERSION_PATCH`` should be ``Z.b1``. RC's can be ``Z.rc1``. Always include the dot (even though PEP 440 allows it to be dropped). For a -final release, this must be a simple integer. There is also a HEX version of -the version just below. +final release, this must be a simple integer. There is also +``PYBIND11_VERSION_HEX`` just below that needs to be updated. To release a new version of pybind11: @@ -26,53 +26,93 @@ If you don't have nox, you should either use ``pipx run nox`` instead, or use ``pipx install nox`` or ``brew install nox`` (Unix). - Update the version number - - Update ``PYBIND11_VERSION_MAJOR`` etc. in - ``include/pybind11/detail/common.h``. PATCH should be a simple integer. - - Update the version HEX just below, as well. - - Update ``pybind11/_version.py`` (match above) - - Run ``nox -s tests_packaging`` to ensure this was done correctly. - - Ensure that all the information in ``setup.cfg`` is up-to-date, like - supported Python versions. - - Add release date in ``docs/changelog.rst`` and integrate the output of - ``nox -s make_changelog``. - - Note that the ``make_changelog`` command inspects - `needs changelog `_. - - Manually clear the ``needs changelog`` labels using the GitHub web - interface (very easy: start by clicking the link above). - - ``git add`` and ``git commit``, ``git push``. **Ensure CI passes**. (If it - fails due to a known flake issue, either ignore or restart CI.) -- Add a release branch if this is a new minor version, or update the existing release branch if it is a patch version - - New branch: ``git checkout -b vX.Y``, ``git push -u origin vX.Y`` - - Update branch: ``git checkout vX.Y``, ``git merge ``, ``git push`` + + - Update ``PYBIND11_VERSION_MAJOR`` etc. in + ``include/pybind11/detail/common.h``. PATCH should be a simple integer. + + - Update ``PYBIND11_VERSION_HEX`` just below as well. + + - Update ``pybind11/_version.py`` (match above). + + - Run ``nox -s tests_packaging`` to ensure this was done correctly. + +- Ensure that all the information in ``setup.cfg`` is up-to-date, like + supported Python versions. + +- Add release date in ``docs/changelog.rst`` and integrate the output of + ``nox -s make_changelog``. + + - Note that the ``nox -s make_changelog`` command inspects + `needs changelog `_. + + - Manually clear the ``needs changelog`` labels using the GitHub web + interface (very easy: start by clicking the link above). + +- ``git add`` and ``git commit``, ``git push``. **Ensure CI passes**. (If it + fails due to a known flake issue, either ignore or restart CI.) + +- Add a release branch if this is a new MINOR version, or update the existing + release branch if it is a patch version + + - New branch: ``git checkout -b vX.Y``, ``git push -u origin vX.Y`` + + - Update branch: ``git checkout vX.Y``, ``git merge ``, ``git push`` + - Update tags (optional; if you skip this, the GitHub release makes a - non-annotated tag for you) - - ``git tag -a vX.Y.Z -m 'vX.Y.Z release'``. - - ``git push --tags``. + non-annotated tag for you) + + - ``git tag -a vX.Y.Z -m 'vX.Y.Z release'`` + + - ``grep ^__version__ pybind11/_version.py`` + + - Last-minute consistency check: same as tag? + + - ``git push --tags`` + - Update stable - - ``git checkout stable`` - - ``git merge master`` - - ``git push`` + + - ``git checkout stable`` + + - ``git merge -X theirs vX.Y.Z`` + + - ``git diff vX.Y.Z`` + + - Carefully review and reconcile any diffs. There should be none. + + - ``git push`` + - Make a GitHub release (this shows up in the UI, sends new release notifications to users watching releases, and also uploads PyPI packages). (Note: if you do not use an existing tag, this creates a new lightweight tag for you, so you could skip the above step.) - - GUI method: Under `releases `_ - click "Draft a new release" on the far right, fill in the tag name - (if you didn't tag above, it will be made here), fill in a release name - like "Version X.Y.Z", and copy-and-paste the markdown-formatted (!) changelog - into the description (usually ``cat docs/changelog.rst | pandoc -f rst -t gfm``). - Check "pre-release" if this is a beta/RC. - - CLI method: with ``gh`` installed, run ``gh release create vX.Y.Z -t "Version X.Y.Z"`` - If this is a pre-release, add ``-p``. + + - GUI method: Under `releases `_ + click "Draft a new release" on the far right, fill in the tag name + (if you didn't tag above, it will be made here), fill in a release name + like "Version X.Y.Z", and copy-and-paste the markdown-formatted (!) changelog + into the description. You can use ``cat docs/changelog.rst | pandoc -f rst -t gfm``, + then manually remove line breaks and strip links to PRs and issues, + e.g. to a bare ``#1234``, without the surrounding ``<...>_`` hyperlink markup. + Check "pre-release" if this is a beta/RC. + + - CLI method: with ``gh`` installed, run ``gh release create vX.Y.Z -t "Version X.Y.Z"`` + If this is a pre-release, add ``-p``. - Get back to work - - Make sure you are on master, not somewhere else: ``git checkout master`` - - Update version macros in ``include/pybind11/detail/common.h`` (set PATCH to - ``0.dev1`` and increment MINOR). - - Update ``_version.py`` to match - - Run ``nox -s tests_packaging`` to ensure this was done correctly. - - Add a spot for in-development updates in ``docs/changelog.rst``. - - ``git add``, ``git commit``, ``git push`` + + - Make sure you are on master, not somewhere else: ``git checkout master`` + + - Update version macros in ``include/pybind11/detail/common.h`` (set PATCH to + ``0.dev1`` and increment MINOR). + + - Update ``pybind11/_version.py`` to match. + + - Run ``nox -s tests_packaging`` to ensure this was done correctly. + + - If the release was a new MINOR version, add a new ``IN DEVELOPMENT`` + section in ``docs/changelog.rst``. + + - ``git add``, ``git commit``, ``git push`` If a version branch is updated, remember to set PATCH to ``1.dev1``. @@ -89,7 +129,11 @@ merge it if there are no issues. Manual packaging ^^^^^^^^^^^^^^^^ -If you need to manually upload releases, you can download the releases from the job artifacts and upload them with twine. You can also make the files locally (not recommended in general, as your local directory is more likely to be "dirty" and SDists love picking up random unrelated/hidden files); this is the procedure: +If you need to manually upload releases, you can download the releases from +the job artifacts and upload them with twine. You can also make the files +locally (not recommended in general, as your local directory is more likely +to be "dirty" and SDists love picking up random unrelated/hidden files); +this is the procedure: .. code-block:: bash diff --git a/pybind11/docs/requirements.in b/pybind11/docs/requirements.in new file mode 100644 index 000000000..bec8f707a --- /dev/null +++ b/pybind11/docs/requirements.in @@ -0,0 +1,6 @@ +breathe +furo +sphinx +sphinx-copybutton +sphinxcontrib-moderncmakedomain +sphinxcontrib-svg2pdfconverter diff --git a/pybind11/docs/requirements.txt b/pybind11/docs/requirements.txt index d2a9ae164..293db6a06 100644 --- a/pybind11/docs/requirements.txt +++ b/pybind11/docs/requirements.txt @@ -1,6 +1,275 @@ -breathe==4.34.0 -furo==2022.6.21 -sphinx==5.0.2 -sphinx-copybutton==0.5.0 -sphinxcontrib-moderncmakedomain==3.21.4 -sphinxcontrib-svg2pdfconverter==1.2.0 +# This file was autogenerated by uv via the following command: +# uv pip compile --generate-hashes docs/requirements.in -o docs/requirements.txt +alabaster==0.7.16 \ + --hash=sha256:75a8b99c28a5dad50dd7f8ccdd447a121ddb3892da9e53d1ca5cca3106d58d65 \ + --hash=sha256:b46733c07dce03ae4e150330b975c75737fa60f0a7c591b6c8bf4928a28e2c92 + # via sphinx +babel==2.14.0 \ + --hash=sha256:6919867db036398ba21eb5c7a0f6b28ab8cbc3ae7a73a44ebe34ae74a4e7d363 \ + --hash=sha256:efb1a25b7118e67ce3a259bed20545c29cb68be8ad2c784c83689981b7a57287 + # via sphinx +beautifulsoup4==4.12.3 \ + --hash=sha256:74e3d1928edc070d21748185c46e3fb33490f22f52a3addee9aee0f4f7781051 \ + --hash=sha256:b80878c9f40111313e55da8ba20bdba06d8fa3969fc68304167741bbf9e082ed + # via furo +breathe==4.35.0 \ + --hash=sha256:5165541c3c67b6c7adde8b3ecfe895c6f7844783c4076b6d8d287e4f33d62386 \ + --hash=sha256:52c581f42ca4310737f9e435e3851c3d1f15446205a85fbc272f1f97ed74f5be + # via -r requirements.in +certifi==2024.2.2 \ + --hash=sha256:0569859f95fc761b18b45ef421b1290a0f65f147e92a1e5eb3e635f9a5e4e66f \ + --hash=sha256:dc383c07b76109f368f6106eee2b593b04a011ea4d55f652c6ca24a754d1cdd1 + # via requests +charset-normalizer==3.3.2 \ + --hash=sha256:06435b539f889b1f6f4ac1758871aae42dc3a8c0e24ac9e60c2384973ad73027 \ + --hash=sha256:06a81e93cd441c56a9b65d8e1d043daeb97a3d0856d177d5c90ba85acb3db087 \ + --hash=sha256:0a55554a2fa0d408816b3b5cedf0045f4b8e1a6065aec45849de2d6f3f8e9786 \ + --hash=sha256:0b2b64d2bb6d3fb9112bafa732def486049e63de9618b5843bcdd081d8144cd8 \ + --hash=sha256:10955842570876604d404661fbccbc9c7e684caf432c09c715ec38fbae45ae09 \ + --hash=sha256:122c7fa62b130ed55f8f285bfd56d5f4b4a5b503609d181f9ad85e55c89f4185 \ + --hash=sha256:1ceae2f17a9c33cb48e3263960dc5fc8005351ee19db217e9b1bb15d28c02574 \ + --hash=sha256:1d3193f4a680c64b4b6a9115943538edb896edc190f0b222e73761716519268e \ + --hash=sha256:1f79682fbe303db92bc2b1136016a38a42e835d932bab5b3b1bfcfbf0640e519 \ + --hash=sha256:2127566c664442652f024c837091890cb1942c30937add288223dc895793f898 \ + --hash=sha256:22afcb9f253dac0696b5a4be4a1c0f8762f8239e21b99680099abd9b2b1b2269 \ + --hash=sha256:25baf083bf6f6b341f4121c2f3c548875ee6f5339300e08be3f2b2ba1721cdd3 \ + --hash=sha256:2e81c7b9c8979ce92ed306c249d46894776a909505d8f5a4ba55b14206e3222f \ + --hash=sha256:3287761bc4ee9e33561a7e058c72ac0938c4f57fe49a09eae428fd88aafe7bb6 \ + --hash=sha256:34d1c8da1e78d2e001f363791c98a272bb734000fcef47a491c1e3b0505657a8 \ + --hash=sha256:37e55c8e51c236f95b033f6fb391d7d7970ba5fe7ff453dad675e88cf303377a \ + --hash=sha256:3d47fa203a7bd9c5b6cee4736ee84ca03b8ef23193c0d1ca99b5089f72645c73 \ + --hash=sha256:3e4d1f6587322d2788836a99c69062fbb091331ec940e02d12d179c1d53e25fc \ + --hash=sha256:42cb296636fcc8b0644486d15c12376cb9fa75443e00fb25de0b8602e64c1714 \ + --hash=sha256:45485e01ff4d3630ec0d9617310448a8702f70e9c01906b0d0118bdf9d124cf2 \ + --hash=sha256:4a78b2b446bd7c934f5dcedc588903fb2f5eec172f3d29e52a9096a43722adfc \ + --hash=sha256:4ab2fe47fae9e0f9dee8c04187ce5d09f48eabe611be8259444906793ab7cbce \ + --hash=sha256:4d0d1650369165a14e14e1e47b372cfcb31d6ab44e6e33cb2d4e57265290044d \ + --hash=sha256:549a3a73da901d5bc3ce8d24e0600d1fa85524c10287f6004fbab87672bf3e1e \ + --hash=sha256:55086ee1064215781fff39a1af09518bc9255b50d6333f2e4c74ca09fac6a8f6 \ + --hash=sha256:572c3763a264ba47b3cf708a44ce965d98555f618ca42c926a9c1616d8f34269 \ + --hash=sha256:573f6eac48f4769d667c4442081b1794f52919e7edada77495aaed9236d13a96 \ + --hash=sha256:5b4c145409bef602a690e7cfad0a15a55c13320ff7a3ad7ca59c13bb8ba4d45d \ + --hash=sha256:6463effa3186ea09411d50efc7d85360b38d5f09b870c48e4600f63af490e56a \ + --hash=sha256:65f6f63034100ead094b8744b3b97965785388f308a64cf8d7c34f2f2e5be0c4 \ + --hash=sha256:663946639d296df6a2bb2aa51b60a2454ca1cb29835324c640dafb5ff2131a77 \ + --hash=sha256:6897af51655e3691ff853668779c7bad41579facacf5fd7253b0133308cf000d \ + --hash=sha256:68d1f8a9e9e37c1223b656399be5d6b448dea850bed7d0f87a8311f1ff3dabb0 \ + --hash=sha256:6ac7ffc7ad6d040517be39eb591cac5ff87416c2537df6ba3cba3bae290c0fed \ + --hash=sha256:6b3251890fff30ee142c44144871185dbe13b11bab478a88887a639655be1068 \ + --hash=sha256:6c4caeef8fa63d06bd437cd4bdcf3ffefe6738fb1b25951440d80dc7df8c03ac \ + --hash=sha256:6ef1d82a3af9d3eecdba2321dc1b3c238245d890843e040e41e470ffa64c3e25 \ + --hash=sha256:753f10e867343b4511128c6ed8c82f7bec3bd026875576dfd88483c5c73b2fd8 \ + --hash=sha256:7cd13a2e3ddeed6913a65e66e94b51d80a041145a026c27e6bb76c31a853c6ab \ + --hash=sha256:7ed9e526742851e8d5cc9e6cf41427dfc6068d4f5a3bb03659444b4cabf6bc26 \ + --hash=sha256:7f04c839ed0b6b98b1a7501a002144b76c18fb1c1850c8b98d458ac269e26ed2 \ + --hash=sha256:802fe99cca7457642125a8a88a084cef28ff0cf9407060f7b93dca5aa25480db \ + --hash=sha256:80402cd6ee291dcb72644d6eac93785fe2c8b9cb30893c1af5b8fdd753b9d40f \ + --hash=sha256:8465322196c8b4d7ab6d1e049e4c5cb460d0394da4a27d23cc242fbf0034b6b5 \ + --hash=sha256:86216b5cee4b06df986d214f664305142d9c76df9b6512be2738aa72a2048f99 \ + --hash=sha256:87d1351268731db79e0f8e745d92493ee2841c974128ef629dc518b937d9194c \ + --hash=sha256:8bdb58ff7ba23002a4c5808d608e4e6c687175724f54a5dade5fa8c67b604e4d \ + --hash=sha256:8c622a5fe39a48f78944a87d4fb8a53ee07344641b0562c540d840748571b811 \ + --hash=sha256:8d756e44e94489e49571086ef83b2bb8ce311e730092d2c34ca8f7d925cb20aa \ + --hash=sha256:8f4a014bc36d3c57402e2977dada34f9c12300af536839dc38c0beab8878f38a \ + --hash=sha256:9063e24fdb1e498ab71cb7419e24622516c4a04476b17a2dab57e8baa30d6e03 \ + --hash=sha256:90d558489962fd4918143277a773316e56c72da56ec7aa3dc3dbbe20fdfed15b \ + --hash=sha256:923c0c831b7cfcb071580d3f46c4baf50f174be571576556269530f4bbd79d04 \ + --hash=sha256:95f2a5796329323b8f0512e09dbb7a1860c46a39da62ecb2324f116fa8fdc85c \ + --hash=sha256:96b02a3dc4381e5494fad39be677abcb5e6634bf7b4fa83a6dd3112607547001 \ + --hash=sha256:9f96df6923e21816da7e0ad3fd47dd8f94b2a5ce594e00677c0013018b813458 \ + --hash=sha256:a10af20b82360ab00827f916a6058451b723b4e65030c5a18577c8b2de5b3389 \ + --hash=sha256:a50aebfa173e157099939b17f18600f72f84eed3049e743b68ad15bd69b6bf99 \ + --hash=sha256:a981a536974bbc7a512cf44ed14938cf01030a99e9b3a06dd59578882f06f985 \ + --hash=sha256:a9a8e9031d613fd2009c182b69c7b2c1ef8239a0efb1df3f7c8da66d5dd3d537 \ + --hash=sha256:ae5f4161f18c61806f411a13b0310bea87f987c7d2ecdbdaad0e94eb2e404238 \ + --hash=sha256:aed38f6e4fb3f5d6bf81bfa990a07806be9d83cf7bacef998ab1a9bd660a581f \ + --hash=sha256:b01b88d45a6fcb69667cd6d2f7a9aeb4bf53760d7fc536bf679ec94fe9f3ff3d \ + --hash=sha256:b261ccdec7821281dade748d088bb6e9b69e6d15b30652b74cbbac25e280b796 \ + --hash=sha256:b2b0a0c0517616b6869869f8c581d4eb2dd83a4d79e0ebcb7d373ef9956aeb0a \ + --hash=sha256:b4a23f61ce87adf89be746c8a8974fe1c823c891d8f86eb218bb957c924bb143 \ + --hash=sha256:bd8f7df7d12c2db9fab40bdd87a7c09b1530128315d047a086fa3ae3435cb3a8 \ + --hash=sha256:beb58fe5cdb101e3a055192ac291b7a21e3b7ef4f67fa1d74e331a7f2124341c \ + --hash=sha256:c002b4ffc0be611f0d9da932eb0f704fe2602a9a949d1f738e4c34c75b0863d5 \ + --hash=sha256:c083af607d2515612056a31f0a8d9e0fcb5876b7bfc0abad3ecd275bc4ebc2d5 \ + --hash=sha256:c180f51afb394e165eafe4ac2936a14bee3eb10debc9d9e4db8958fe36afe711 \ + --hash=sha256:c235ebd9baae02f1b77bcea61bce332cb4331dc3617d254df3323aa01ab47bd4 \ + --hash=sha256:cd70574b12bb8a4d2aaa0094515df2463cb429d8536cfb6c7ce983246983e5a6 \ + --hash=sha256:d0eccceffcb53201b5bfebb52600a5fb483a20b61da9dbc885f8b103cbe7598c \ + --hash=sha256:d965bba47ddeec8cd560687584e88cf699fd28f192ceb452d1d7ee807c5597b7 \ + --hash=sha256:db364eca23f876da6f9e16c9da0df51aa4f104a972735574842618b8c6d999d4 \ + --hash=sha256:ddbb2551d7e0102e7252db79ba445cdab71b26640817ab1e3e3648dad515003b \ + --hash=sha256:deb6be0ac38ece9ba87dea880e438f25ca3eddfac8b002a2ec3d9183a454e8ae \ + --hash=sha256:e06ed3eb3218bc64786f7db41917d4e686cc4856944f53d5bdf83a6884432e12 \ + --hash=sha256:e27ad930a842b4c5eb8ac0016b0a54f5aebbe679340c26101df33424142c143c \ + --hash=sha256:e537484df0d8f426ce2afb2d0f8e1c3d0b114b83f8850e5f2fbea0e797bd82ae \ + --hash=sha256:eb00ed941194665c332bf8e078baf037d6c35d7c4f3102ea2d4f16ca94a26dc8 \ + --hash=sha256:eb6904c354526e758fda7167b33005998fb68c46fbc10e013ca97f21ca5c8887 \ + --hash=sha256:eb8821e09e916165e160797a6c17edda0679379a4be5c716c260e836e122f54b \ + --hash=sha256:efcb3f6676480691518c177e3b465bcddf57cea040302f9f4e6e191af91174d4 \ + --hash=sha256:f27273b60488abe721a075bcca6d7f3964f9f6f067c8c4c605743023d7d3944f \ + --hash=sha256:f30c3cb33b24454a82faecaf01b19c18562b1e89558fb6c56de4d9118a032fd5 \ + --hash=sha256:fb69256e180cb6c8a894fee62b3afebae785babc1ee98b81cdf68bbca1987f33 \ + --hash=sha256:fd1abc0d89e30cc4e02e4064dc67fcc51bd941eb395c502aac3ec19fab46b519 \ + --hash=sha256:ff8fa367d09b717b2a17a052544193ad76cd49979c805768879cb63d9ca50561 + # via requests +docutils==0.20.1 \ + --hash=sha256:96f387a2c5562db4476f09f13bbab2192e764cac08ebbf3a34a95d9b1e4a59d6 \ + --hash=sha256:f08a4e276c3a1583a86dce3e34aba3fe04d02bba2dd51ed16106244e8a923e3b + # via + # breathe + # sphinx +furo==2024.1.29 \ + --hash=sha256:3548be2cef45a32f8cdc0272d415fcb3e5fa6a0eb4ddfe21df3ecf1fe45a13cf \ + --hash=sha256:4d6b2fe3f10a6e36eb9cc24c1e7beb38d7a23fc7b3c382867503b7fcac8a1e02 + # via -r requirements.in +idna==3.7 \ + --hash=sha256:028ff3aadf0609c1fd278d8ea3089299412a7a8b9bd005dd08b9f8285bcb5cfc \ + --hash=sha256:82fee1fc78add43492d3a1898bfa6d8a904cc97d8427f683ed8e798d07761aa0 + # via requests +imagesize==1.4.1 \ + --hash=sha256:0d8d18d08f840c19d0ee7ca1fd82490fdc3729b7ac93f49870406ddde8ef8d8b \ + --hash=sha256:69150444affb9cb0d5cc5a92b3676f0b2fb7cd9ae39e947a5e11a36b4497cd4a + # via sphinx +jinja2==3.1.4 \ + --hash=sha256:4a3aee7acbbe7303aede8e9648d13b8bf88a429282aa6122a993f0ac800cb369 \ + --hash=sha256:bc5dd2abb727a5319567b7a813e6a2e7318c39f4f487cfe6c89c6f9c7d25197d + # via sphinx +markupsafe==2.1.5 \ + --hash=sha256:00e046b6dd71aa03a41079792f8473dc494d564611a8f89bbbd7cb93295ebdcf \ + --hash=sha256:075202fa5b72c86ad32dc7d0b56024ebdbcf2048c0ba09f1cde31bfdd57bcfff \ + --hash=sha256:0e397ac966fdf721b2c528cf028494e86172b4feba51d65f81ffd65c63798f3f \ + --hash=sha256:17b950fccb810b3293638215058e432159d2b71005c74371d784862b7e4683f3 \ + --hash=sha256:1f3fbcb7ef1f16e48246f704ab79d79da8a46891e2da03f8783a5b6fa41a9532 \ + --hash=sha256:2174c595a0d73a3080ca3257b40096db99799265e1c27cc5a610743acd86d62f \ + --hash=sha256:2b7c57a4dfc4f16f7142221afe5ba4e093e09e728ca65c51f5620c9aaeb9a617 \ + --hash=sha256:2d2d793e36e230fd32babe143b04cec8a8b3eb8a3122d2aceb4a371e6b09b8df \ + --hash=sha256:30b600cf0a7ac9234b2638fbc0fb6158ba5bdcdf46aeb631ead21248b9affbc4 \ + --hash=sha256:397081c1a0bfb5124355710fe79478cdbeb39626492b15d399526ae53422b906 \ + --hash=sha256:3a57fdd7ce31c7ff06cdfbf31dafa96cc533c21e443d57f5b1ecc6cdc668ec7f \ + --hash=sha256:3c6b973f22eb18a789b1460b4b91bf04ae3f0c4234a0a6aa6b0a92f6f7b951d4 \ + --hash=sha256:3e53af139f8579a6d5f7b76549125f0d94d7e630761a2111bc431fd820e163b8 \ + --hash=sha256:4096e9de5c6fdf43fb4f04c26fb114f61ef0bf2e5604b6ee3019d51b69e8c371 \ + --hash=sha256:4275d846e41ecefa46e2015117a9f491e57a71ddd59bbead77e904dc02b1bed2 \ + --hash=sha256:4c31f53cdae6ecfa91a77820e8b151dba54ab528ba65dfd235c80b086d68a465 \ + --hash=sha256:4f11aa001c540f62c6166c7726f71f7573b52c68c31f014c25cc7901deea0b52 \ + --hash=sha256:5049256f536511ee3f7e1b3f87d1d1209d327e818e6ae1365e8653d7e3abb6a6 \ + --hash=sha256:58c98fee265677f63a4385256a6d7683ab1832f3ddd1e66fe948d5880c21a169 \ + --hash=sha256:598e3276b64aff0e7b3451b72e94fa3c238d452e7ddcd893c3ab324717456bad \ + --hash=sha256:5b7b716f97b52c5a14bffdf688f971b2d5ef4029127f1ad7a513973cfd818df2 \ + --hash=sha256:5dedb4db619ba5a2787a94d877bc8ffc0566f92a01c0ef214865e54ecc9ee5e0 \ + --hash=sha256:619bc166c4f2de5caa5a633b8b7326fbe98e0ccbfacabd87268a2b15ff73a029 \ + --hash=sha256:629ddd2ca402ae6dbedfceeba9c46d5f7b2a61d9749597d4307f943ef198fc1f \ + --hash=sha256:656f7526c69fac7f600bd1f400991cc282b417d17539a1b228617081106feb4a \ + --hash=sha256:6ec585f69cec0aa07d945b20805be741395e28ac1627333b1c5b0105962ffced \ + --hash=sha256:72b6be590cc35924b02c78ef34b467da4ba07e4e0f0454a2c5907f473fc50ce5 \ + --hash=sha256:7502934a33b54030eaf1194c21c692a534196063db72176b0c4028e140f8f32c \ + --hash=sha256:7a68b554d356a91cce1236aa7682dc01df0edba8d043fd1ce607c49dd3c1edcf \ + --hash=sha256:7b2e5a267c855eea6b4283940daa6e88a285f5f2a67f2220203786dfa59b37e9 \ + --hash=sha256:823b65d8706e32ad2df51ed89496147a42a2a6e01c13cfb6ffb8b1e92bc910bb \ + --hash=sha256:8590b4ae07a35970728874632fed7bd57b26b0102df2d2b233b6d9d82f6c62ad \ + --hash=sha256:8dd717634f5a044f860435c1d8c16a270ddf0ef8588d4887037c5028b859b0c3 \ + --hash=sha256:8dec4936e9c3100156f8a2dc89c4b88d5c435175ff03413b443469c7c8c5f4d1 \ + --hash=sha256:97cafb1f3cbcd3fd2b6fbfb99ae11cdb14deea0736fc2b0952ee177f2b813a46 \ + --hash=sha256:a17a92de5231666cfbe003f0e4b9b3a7ae3afb1ec2845aadc2bacc93ff85febc \ + --hash=sha256:a549b9c31bec33820e885335b451286e2969a2d9e24879f83fe904a5ce59d70a \ + --hash=sha256:ac07bad82163452a6884fe8fa0963fb98c2346ba78d779ec06bd7a6262132aee \ + --hash=sha256:ae2ad8ae6ebee9d2d94b17fb62763125f3f374c25618198f40cbb8b525411900 \ + --hash=sha256:b91c037585eba9095565a3556f611e3cbfaa42ca1e865f7b8015fe5c7336d5a5 \ + --hash=sha256:bc1667f8b83f48511b94671e0e441401371dfd0f0a795c7daa4a3cd1dde55bea \ + --hash=sha256:bec0a414d016ac1a18862a519e54b2fd0fc8bbfd6890376898a6c0891dd82e9f \ + --hash=sha256:bf50cd79a75d181c9181df03572cdce0fbb75cc353bc350712073108cba98de5 \ + --hash=sha256:bff1b4290a66b490a2f4719358c0cdcd9bafb6b8f061e45c7a2460866bf50c2e \ + --hash=sha256:c061bb86a71b42465156a3ee7bd58c8c2ceacdbeb95d05a99893e08b8467359a \ + --hash=sha256:c8b29db45f8fe46ad280a7294f5c3ec36dbac9491f2d1c17345be8e69cc5928f \ + --hash=sha256:ce409136744f6521e39fd8e2a24c53fa18ad67aa5bc7c2cf83645cce5b5c4e50 \ + --hash=sha256:d050b3361367a06d752db6ead6e7edeb0009be66bc3bae0ee9d97fb326badc2a \ + --hash=sha256:d283d37a890ba4c1ae73ffadf8046435c76e7bc2247bbb63c00bd1a709c6544b \ + --hash=sha256:d9fad5155d72433c921b782e58892377c44bd6252b5af2f67f16b194987338a4 \ + --hash=sha256:daa4ee5a243f0f20d528d939d06670a298dd39b1ad5f8a72a4275124a7819eff \ + --hash=sha256:db0b55e0f3cc0be60c1f19efdde9a637c32740486004f20d1cff53c3c0ece4d2 \ + --hash=sha256:e61659ba32cf2cf1481e575d0462554625196a1f2fc06a1c777d3f48e8865d46 \ + --hash=sha256:ea3d8a3d18833cf4304cd2fc9cbb1efe188ca9b5efef2bdac7adc20594a0e46b \ + --hash=sha256:ec6a563cff360b50eed26f13adc43e61bc0c04d94b8be985e6fb24b81f6dcfdf \ + --hash=sha256:f5dfb42c4604dddc8e4305050aa6deb084540643ed5804d7455b5df8fe16f5e5 \ + --hash=sha256:fa173ec60341d6bb97a89f5ea19c85c5643c1e7dedebc22f5181eb73573142c5 \ + --hash=sha256:fa9db3f79de01457b03d4f01b34cf91bc0048eb2c3846ff26f66687c2f6d16ab \ + --hash=sha256:fce659a462a1be54d2ffcacea5e3ba2d74daa74f30f5f143fe0c58636e355fdd \ + --hash=sha256:ffee1f21e5ef0d712f9033568f8344d5da8cc2869dbd08d87c84656e6a2d2f68 + # via jinja2 +packaging==24.0 \ + --hash=sha256:2ddfb553fdf02fb784c234c7ba6ccc288296ceabec964ad2eae3777778130bc5 \ + --hash=sha256:eb82c5e3e56209074766e6885bb04b8c38a0c015d0a30036ebe7ece34c9989e9 + # via sphinx +pygments==2.17.2 \ + --hash=sha256:b27c2826c47d0f3219f29554824c30c5e8945175d888647acd804ddd04af846c \ + --hash=sha256:da46cec9fd2de5be3a8a784f434e4c4ab670b4ff54d605c4c2717e9d49c4c367 + # via + # furo + # sphinx +requests==2.32.0 \ + --hash=sha256:f2c3881dddb70d056c5bd7600a4fae312b2a300e39be6a118d30b90bd27262b5 \ + --hash=sha256:fa5490319474c82ef1d2c9bc459d3652e3ae4ef4c4ebdd18a21145a47ca4b6b8 + # via sphinx +snowballstemmer==2.2.0 \ + --hash=sha256:09b16deb8547d3412ad7b590689584cd0fe25ec8db3be37788be3810cbf19cb1 \ + --hash=sha256:c8e1716e83cc398ae16824e5572ae04e0d9fc2c6b985fb0f900f5f0c96ecba1a + # via sphinx +soupsieve==2.5 \ + --hash=sha256:5663d5a7b3bfaeee0bc4372e7fc48f9cff4940b3eec54a6451cc5299f1097690 \ + --hash=sha256:eaa337ff55a1579b6549dc679565eac1e3d000563bcb1c8ab0d0fefbc0c2cdc7 + # via beautifulsoup4 +sphinx==7.2.6 \ + --hash=sha256:1e09160a40b956dc623c910118fa636da93bd3ca0b9876a7b3df90f07d691560 \ + --hash=sha256:9a5160e1ea90688d5963ba09a2dcd8bdd526620edbb65c328728f1b2228d5ab5 + # via + # -r requirements.in + # breathe + # furo + # sphinx-basic-ng + # sphinx-copybutton + # sphinxcontrib-moderncmakedomain + # sphinxcontrib-svg2pdfconverter +sphinx-basic-ng==1.0.0b2 \ + --hash=sha256:9ec55a47c90c8c002b5960c57492ec3021f5193cb26cebc2dc4ea226848651c9 \ + --hash=sha256:eb09aedbabfb650607e9b4b68c9d240b90b1e1be221d6ad71d61c52e29f7932b + # via furo +sphinx-copybutton==0.5.2 \ + --hash=sha256:4cf17c82fb9646d1bc9ca92ac280813a3b605d8c421225fd9913154103ee1fbd \ + --hash=sha256:fb543fd386d917746c9a2c50360c7905b605726b9355cd26e9974857afeae06e + # via -r requirements.in +sphinxcontrib-applehelp==1.0.8 \ + --hash=sha256:c40a4f96f3776c4393d933412053962fac2b84f4c99a7982ba42e09576a70619 \ + --hash=sha256:cb61eb0ec1b61f349e5cc36b2028e9e7ca765be05e49641c97241274753067b4 + # via sphinx +sphinxcontrib-devhelp==1.0.6 \ + --hash=sha256:6485d09629944511c893fa11355bda18b742b83a2b181f9a009f7e500595c90f \ + --hash=sha256:9893fd3f90506bc4b97bdb977ceb8fbd823989f4316b28c3841ec128544372d3 + # via sphinx +sphinxcontrib-htmlhelp==2.0.5 \ + --hash=sha256:0dc87637d5de53dd5eec3a6a01753b1ccf99494bd756aafecd74b4fa9e729015 \ + --hash=sha256:393f04f112b4d2f53d93448d4bce35842f62b307ccdc549ec1585e950bc35e04 + # via sphinx +sphinxcontrib-jsmath==1.0.1 \ + --hash=sha256:2ec2eaebfb78f3f2078e73666b1415417a116cc848b72e5172e596c871103178 \ + --hash=sha256:a9925e4a4587247ed2191a22df5f6970656cb8ca2bd6284309578f2153e0c4b8 + # via sphinx +sphinxcontrib-moderncmakedomain==3.27.0 \ + --hash=sha256:51e259e91f58d17cc0fac9307fd40106aa59d5acaa741887903fc3660361d1a1 \ + --hash=sha256:70a73e0e7cff1b117074e968ccb7f72383ed0f572414df0e216cea06914de988 + # via -r requirements.in +sphinxcontrib-qthelp==1.0.7 \ + --hash=sha256:053dedc38823a80a7209a80860b16b722e9e0209e32fea98c90e4e6624588ed6 \ + --hash=sha256:e2ae3b5c492d58fcbd73281fbd27e34b8393ec34a073c792642cd8e529288182 + # via sphinx +sphinxcontrib-serializinghtml==1.1.10 \ + --hash=sha256:326369b8df80a7d2d8d7f99aa5ac577f51ea51556ed974e7716cfd4fca3f6cb7 \ + --hash=sha256:93f3f5dc458b91b192fe10c397e324f262cf163d79f3282c158e8436a2c4511f + # via sphinx +sphinxcontrib-svg2pdfconverter==1.2.2 \ + --hash=sha256:04ec767b55780a6b18d89cc1a8ada6d900c6efde9d1683abdb98a49b144465ca \ + --hash=sha256:80a55ca61f70eae93efc65f3814f2f177c86ba55934a9f6c5022f1778b62146b + # via -r requirements.in +urllib3==2.2.2 \ + --hash=sha256:a448b2f64d686155468037e1ace9f2d2199776e17f0a46610480d311f73e3472 \ + --hash=sha256:dd505485549a7a552833da5e6063639d0d177c04f23bc3864e41e5dc5f612168 + # via requests diff --git a/pybind11/docs/upgrade.rst b/pybind11/docs/upgrade.rst index b13d21f5e..17c26aaa9 100644 --- a/pybind11/docs/upgrade.rst +++ b/pybind11/docs/upgrade.rst @@ -8,6 +8,34 @@ to a new version. But it goes into more detail. This includes things like deprecated APIs and their replacements, build system changes, general code modernization and other useful information. +.. _upgrade-guide-2.12: + +v2.12 +===== + +NumPy support has been upgraded to support the 2.x series too. The two relevant +changes are that: + +* ``dtype.flags()`` is now a ``uint64`` and ``dtype.alignment()`` an + ``ssize_t`` (and NumPy may return an larger than integer value for + ``itemsize()`` in NumPy 2.x). + +* The long deprecated NumPy function ``PyArray_GetArrayParamsFromObject`` + function is not available anymore. + +Due to NumPy changes, you may experience difficulties updating to NumPy 2. +Please see the [NumPy 2 migration guide](https://numpy.org/devdocs/numpy_2_0_migration_guide.html) for details. +For example, a more direct change could be that the default integer ``"int_"`` +(and ``"uint"``) is now ``ssize_t`` and not ``long`` (affects 64bit windows). + +If you want to only support NumPy 1.x for now and are having problems due to +the two internal changes listed above, you can define +``PYBIND11_NUMPY_1_ONLY`` to disable the new support for now. Make sure you +define this on all pybind11 compile units, since it could be a source of ODR +violations if used inconsistently. This option will be removed in the future, +so adapting your code is highly recommended. + + .. _upgrade-guide-2.11: v2.11 diff --git a/pybind11/include/pybind11/buffer_info.h b/pybind11/include/pybind11/buffer_info.h index b99ee8bef..75aec0ba3 100644 --- a/pybind11/include/pybind11/buffer_info.h +++ b/pybind11/include/pybind11/buffer_info.h @@ -102,22 +102,22 @@ struct buffer_info { template buffer_info(const T *ptr, ssize_t size, bool readonly = true) : buffer_info( - const_cast(ptr), sizeof(T), format_descriptor::format(), size, readonly) {} + const_cast(ptr), sizeof(T), format_descriptor::format(), size, readonly) {} explicit buffer_info(Py_buffer *view, bool ownview = true) : buffer_info( - view->buf, - view->itemsize, - view->format, - view->ndim, - {view->shape, view->shape + view->ndim}, - /* Though buffer::request() requests PyBUF_STRIDES, ctypes objects - * ignore this flag and return a view with NULL strides. - * When strides are NULL, build them manually. */ - view->strides - ? std::vector(view->strides, view->strides + view->ndim) - : detail::c_strides({view->shape, view->shape + view->ndim}, view->itemsize), - (view->readonly != 0)) { + view->buf, + view->itemsize, + view->format, + view->ndim, + {view->shape, view->shape + view->ndim}, + /* Though buffer::request() requests PyBUF_STRIDES, ctypes objects + * ignore this flag and return a view with NULL strides. + * When strides are NULL, build them manually. */ + view->strides + ? std::vector(view->strides, view->strides + view->ndim) + : detail::c_strides({view->shape, view->shape + view->ndim}, view->itemsize), + (view->readonly != 0)) { // NOLINTNEXTLINE(cppcoreguidelines-prefer-member-initializer) this->m_view = view; // NOLINTNEXTLINE(cppcoreguidelines-prefer-member-initializer) @@ -176,7 +176,7 @@ private: detail::any_container &&strides_in, bool readonly) : buffer_info( - ptr, itemsize, format, ndim, std::move(shape_in), std::move(strides_in), readonly) {} + ptr, itemsize, format, ndim, std::move(shape_in), std::move(strides_in), readonly) {} Py_buffer *m_view = nullptr; bool ownview = false; diff --git a/pybind11/include/pybind11/cast.h b/pybind11/include/pybind11/cast.h index db3934118..624b8ebac 100644 --- a/pybind11/include/pybind11/cast.h +++ b/pybind11/include/pybind11/cast.h @@ -42,13 +42,15 @@ using make_caster = type_caster>; // Shortcut for calling a caster's `cast_op_type` cast operator for casting a type_caster to a T template typename make_caster::template cast_op_type cast_op(make_caster &caster) { - return caster.operator typename make_caster::template cast_op_type(); + using result_t = typename make_caster::template cast_op_type; // See PR #4893 + return caster.operator result_t(); } template typename make_caster::template cast_op_type::type> cast_op(make_caster &&caster) { - return std::move(caster).operator typename make_caster:: - template cast_op_type::type>(); + using result_t = typename make_caster::template cast_op_type< + typename std::add_rvalue_reference::type>; // See PR #4893 + return std::move(caster).operator result_t(); } template @@ -325,8 +327,9 @@ public: value = false; return true; } - if (convert || (std::strcmp("numpy.bool_", Py_TYPE(src.ptr())->tp_name) == 0)) { - // (allow non-implicit conversion for numpy booleans) + if (convert || is_numpy_bool(src)) { + // (allow non-implicit conversion for numpy booleans), use strncmp + // since NumPy 1.x had an additional trailing underscore. Py_ssize_t res = -1; if (src.is_none()) { @@ -358,6 +361,15 @@ public: return handle(src ? Py_True : Py_False).inc_ref(); } PYBIND11_TYPE_CASTER(bool, const_name("bool")); + +private: + // Test if an object is a NumPy boolean (without fetching the type). + static inline bool is_numpy_bool(handle object) { + const char *type_name = Py_TYPE(object.ptr())->tp_name; + // Name changed to `numpy.bool` in NumPy 2, `numpy.bool_` is needed for 1.x support + return std::strcmp("numpy.bool", type_name) == 0 + || std::strcmp("numpy.bool_", type_name) == 0; + } }; // Helper class for UTF-{8,16,32} C++ stl strings: @@ -660,8 +672,9 @@ public: return cast(*src, policy, parent); } - static constexpr auto name - = const_name("Tuple[") + concat(make_caster::name...) + const_name("]"); + static constexpr auto name = const_name("tuple[") + + ::pybind11::detail::concat(make_caster::name...) + + const_name("]"); template using cast_op_type = type; @@ -869,10 +882,53 @@ struct is_holder_type template struct is_holder_type> : std::true_type {}; +#ifdef PYBIND11_DISABLE_HANDLE_TYPE_NAME_DEFAULT_IMPLEMENTATION // See PR #4888 + +// This leads to compilation errors if a specialization is missing. +template +struct handle_type_name; + +#else + template struct handle_type_name { static constexpr auto name = const_name(); }; + +#endif + +template <> +struct handle_type_name { + static constexpr auto name = const_name("object"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("list"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("dict"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("Union[set, frozenset]"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("set"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("frozenset"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("str"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("tuple"); +}; template <> struct handle_type_name { static constexpr auto name = const_name("bool"); @@ -882,6 +938,10 @@ struct handle_type_name { static constexpr auto name = const_name(PYBIND11_BYTES_NAME); }; template <> +struct handle_type_name { + static constexpr auto name = const_name("Buffer"); +}; +template <> struct handle_type_name { static constexpr auto name = const_name("int"); }; @@ -898,10 +958,50 @@ struct handle_type_name { static constexpr auto name = const_name("float"); }; template <> +struct handle_type_name { + static constexpr auto name = const_name("Callable"); +}; +template <> +struct handle_type_name { + static constexpr auto name = handle_type_name::name; +}; +template <> struct handle_type_name { static constexpr auto name = const_name("None"); }; template <> +struct handle_type_name { + static constexpr auto name = const_name("Sequence"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("bytearray"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("memoryview"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("slice"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("type"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("capsule"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("ellipsis"); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name("weakref"); +}; +template <> struct handle_type_name { static constexpr auto name = const_name("*args"); }; @@ -909,6 +1009,30 @@ template <> struct handle_type_name { static constexpr auto name = const_name("**kwargs"); }; +template <> +struct handle_type_name { + static constexpr auto name = const_name(); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name(); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name(); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name(); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name(); +}; +template <> +struct handle_type_name { + static constexpr auto name = const_name(); +}; template struct pyobject_caster { @@ -1215,13 +1339,24 @@ enable_if_t::value, T> cast_ref(object &&, // static_assert, even though if it's in dead code, so we provide a "trampoline" to pybind11::cast // that only does anything in cases where pybind11::cast is valid. template -enable_if_t::value, T> cast_safe(object &&) { +enable_if_t::value + && !detail::is_same_ignoring_cvref::value, + T> +cast_safe(object &&) { pybind11_fail("Internal error: cast_safe fallback invoked"); } template enable_if_t::value, void> cast_safe(object &&) {} template -enable_if_t, std::is_void>::value, T> +enable_if_t::value, PyObject *> +cast_safe(object &&o) { + return o.release().ptr(); +} +template +enable_if_t, + detail::is_same_ignoring_cvref, + std::is_void>::value, + T> cast_safe(object &&o) { return pybind11::cast(std::move(o)); } @@ -1377,7 +1512,15 @@ inline namespace literals { /** \rst String literal version of `arg` \endrst */ -constexpr arg operator"" _a(const char *name, size_t) { return arg(name); } +constexpr arg +#if !defined(__clang__) && defined(__GNUC__) && __GNUC__ < 5 +operator"" _a // gcc 4.8.5 insists on having a space (hard error). +#else +operator""_a // clang 17 generates a deprecation warning if there is a space. +#endif + (const char *name, size_t) { + return arg(name); +} } // namespace literals PYBIND11_NAMESPACE_BEGIN(detail) @@ -1438,7 +1581,8 @@ public: static_assert(args_pos == -1 || args_pos == constexpr_first(), "py::args cannot be specified more than once"); - static constexpr auto arg_names = concat(type_descr(make_caster::name)...); + static constexpr auto arg_names + = ::pybind11::detail::concat(type_descr(make_caster::name)...); bool load_args(function_call &call) { return load_impl_sequence(call, indices{}); } diff --git a/pybind11/include/pybind11/detail/class.h b/pybind11/include/pybind11/detail/class.h index bc2b40c50..d30621c88 100644 --- a/pybind11/include/pybind11/detail/class.h +++ b/pybind11/include/pybind11/detail/class.h @@ -86,17 +86,16 @@ inline PyTypeObject *make_static_property_type() { type->tp_descr_get = pybind11_static_get; type->tp_descr_set = pybind11_static_set; - if (PyType_Ready(type) < 0) { - pybind11_fail("make_static_property_type(): failure in PyType_Ready()!"); - } - # if PY_VERSION_HEX >= 0x030C0000 - // PRE 3.12 FEATURE FREEZE. PLEASE REVIEW AFTER FREEZE. // Since Python-3.12 property-derived types are required to // have dynamic attributes (to set `__doc__`) enable_dynamic_attributes(heap_type); # endif + if (PyType_Ready(type) < 0) { + pybind11_fail("make_static_property_type(): failure in PyType_Ready()!"); + } + setattr((PyObject *) type, "__module__", str("pybind11_builtins")); PYBIND11_SET_OLDPY_QUALNAME(type, name_obj); @@ -189,12 +188,10 @@ extern "C" inline PyObject *pybind11_meta_call(PyObject *type, PyObject *args, P return nullptr; } - // This must be a pybind11 instance - auto *instance = reinterpret_cast(self); - // Ensure that the base __init__ function(s) were called - for (const auto &vh : values_and_holders(instance)) { - if (!vh.holder_constructed()) { + values_and_holders vhs(self); + for (const auto &vh : vhs) { + if (!vh.holder_constructed() && !vhs.is_redundant_value_and_holder(vh)) { PyErr_Format(PyExc_TypeError, "%.200s.__init__() must be called when overriding __init__", get_fully_qualified_tp_name(vh.type->type).c_str()); @@ -208,39 +205,40 @@ extern "C" inline PyObject *pybind11_meta_call(PyObject *type, PyObject *args, P /// Cleanup the type-info for a pybind11-registered type. extern "C" inline void pybind11_meta_dealloc(PyObject *obj) { - auto *type = (PyTypeObject *) obj; - auto &internals = get_internals(); + with_internals([obj](internals &internals) { + auto *type = (PyTypeObject *) obj; - // A pybind11-registered type will: - // 1) be found in internals.registered_types_py - // 2) have exactly one associated `detail::type_info` - auto found_type = internals.registered_types_py.find(type); - if (found_type != internals.registered_types_py.end() && found_type->second.size() == 1 - && found_type->second[0]->type == type) { + // A pybind11-registered type will: + // 1) be found in internals.registered_types_py + // 2) have exactly one associated `detail::type_info` + auto found_type = internals.registered_types_py.find(type); + if (found_type != internals.registered_types_py.end() && found_type->second.size() == 1 + && found_type->second[0]->type == type) { - auto *tinfo = found_type->second[0]; - auto tindex = std::type_index(*tinfo->cpptype); - internals.direct_conversions.erase(tindex); + auto *tinfo = found_type->second[0]; + auto tindex = std::type_index(*tinfo->cpptype); + internals.direct_conversions.erase(tindex); - if (tinfo->module_local) { - get_local_internals().registered_types_cpp.erase(tindex); - } else { - internals.registered_types_cpp.erase(tindex); - } - internals.registered_types_py.erase(tinfo->type); - - // Actually just `std::erase_if`, but that's only available in C++20 - auto &cache = internals.inactive_override_cache; - for (auto it = cache.begin(), last = cache.end(); it != last;) { - if (it->first == (PyObject *) tinfo->type) { - it = cache.erase(it); + if (tinfo->module_local) { + get_local_internals().registered_types_cpp.erase(tindex); } else { - ++it; + internals.registered_types_cpp.erase(tindex); } - } + internals.registered_types_py.erase(tinfo->type); - delete tinfo; - } + // Actually just `std::erase_if`, but that's only available in C++20 + auto &cache = internals.inactive_override_cache; + for (auto it = cache.begin(), last = cache.end(); it != last;) { + if (it->first == (PyObject *) tinfo->type) { + it = cache.erase(it); + } else { + ++it; + } + } + + delete tinfo; + } + }); PyType_Type.tp_dealloc(obj); } @@ -313,19 +311,20 @@ inline void traverse_offset_bases(void *valueptr, } inline bool register_instance_impl(void *ptr, instance *self) { - get_internals().registered_instances.emplace(ptr, self); + with_instance_map(ptr, [&](instance_map &instances) { instances.emplace(ptr, self); }); return true; // unused, but gives the same signature as the deregister func } inline bool deregister_instance_impl(void *ptr, instance *self) { - auto ®istered_instances = get_internals().registered_instances; - auto range = registered_instances.equal_range(ptr); - for (auto it = range.first; it != range.second; ++it) { - if (self == it->second) { - registered_instances.erase(it); - return true; + return with_instance_map(ptr, [&](instance_map &instances) { + auto range = instances.equal_range(ptr); + for (auto it = range.first; it != range.second; ++it) { + if (self == it->second) { + instances.erase(it); + return true; + } } - } - return false; + return false; + }); } inline void register_instance(instance *self, void *valptr, const type_info *tinfo) { @@ -375,28 +374,37 @@ extern "C" inline PyObject *pybind11_object_new(PyTypeObject *type, PyObject *, extern "C" inline int pybind11_object_init(PyObject *self, PyObject *, PyObject *) { PyTypeObject *type = Py_TYPE(self); std::string msg = get_fully_qualified_tp_name(type) + ": No constructor defined!"; - PyErr_SetString(PyExc_TypeError, msg.c_str()); + set_error(PyExc_TypeError, msg.c_str()); return -1; } inline void add_patient(PyObject *nurse, PyObject *patient) { - auto &internals = get_internals(); auto *instance = reinterpret_cast(nurse); instance->has_patients = true; Py_INCREF(patient); - internals.patients[nurse].push_back(patient); + + with_internals([&](internals &internals) { internals.patients[nurse].push_back(patient); }); } inline void clear_patients(PyObject *self) { auto *instance = reinterpret_cast(self); - auto &internals = get_internals(); - auto pos = internals.patients.find(self); - assert(pos != internals.patients.end()); - // Clearing the patients can cause more Python code to run, which - // can invalidate the iterator. Extract the vector of patients - // from the unordered_map first. - auto patients = std::move(pos->second); - internals.patients.erase(pos); + std::vector patients; + + with_internals([&](internals &internals) { + auto pos = internals.patients.find(self); + + if (pos == internals.patients.end()) { + pybind11_fail( + "FATAL: Internal consistency check failed: Invalid clear_patients() call."); + } + + // Clearing the patients can cause more Python code to run, which + // can invalidate the iterator. Extract the vector of patients + // from the unordered_map first. + patients = std::move(pos->second); + internals.patients.erase(pos); + }); + instance->has_patients = false; for (PyObject *&patient : patients) { Py_CLEAR(patient); @@ -522,8 +530,12 @@ inline PyObject *make_object_base_type(PyTypeObject *metaclass) { /// dynamic_attr: Allow the garbage collector to traverse the internal instance `__dict__`. extern "C" inline int pybind11_traverse(PyObject *self, visitproc visit, void *arg) { +#if PY_VERSION_HEX >= 0x030D0000 + PyObject_VisitManagedDict(self, visit, arg); +#else PyObject *&dict = *_PyObject_GetDictPtr(self); Py_VISIT(dict); +#endif // https://docs.python.org/3/c-api/typeobj.html#c.PyTypeObject.tp_traverse #if PY_VERSION_HEX >= 0x03090000 Py_VISIT(Py_TYPE(self)); @@ -533,8 +545,12 @@ extern "C" inline int pybind11_traverse(PyObject *self, visitproc visit, void *a /// dynamic_attr: Allow the GC to clear the dictionary. extern "C" inline int pybind11_clear(PyObject *self) { +#if PY_VERSION_HEX >= 0x030D0000 + PyObject_ClearManagedDict(self); +#else PyObject *&dict = *_PyObject_GetDictPtr(self); Py_CLEAR(dict); +#endif return 0; } @@ -551,17 +567,9 @@ inline void enable_dynamic_attributes(PyHeapTypeObject *heap_type) { type->tp_traverse = pybind11_traverse; type->tp_clear = pybind11_clear; - static PyGetSetDef getset[] = {{ -#if PY_VERSION_HEX < 0x03070000 - const_cast("__dict__"), -#else - "__dict__", -#endif - PyObject_GenericGetDict, - PyObject_GenericSetDict, - nullptr, - nullptr}, - {nullptr, nullptr, nullptr, nullptr, nullptr}}; + static PyGetSetDef getset[] + = {{"__dict__", PyObject_GenericGetDict, PyObject_GenericSetDict, nullptr, nullptr}, + {nullptr, nullptr, nullptr, nullptr, nullptr}}; type->tp_getset = getset; } @@ -579,7 +587,7 @@ extern "C" inline int pybind11_getbuffer(PyObject *obj, Py_buffer *view, int fla if (view) { view->obj = nullptr; } - PyErr_SetString(PyExc_BufferError, "pybind11_getbuffer(): Internal error"); + set_error(PyExc_BufferError, "pybind11_getbuffer(): Internal error"); return -1; } std::memset(view, 0, sizeof(Py_buffer)); @@ -587,7 +595,7 @@ extern "C" inline int pybind11_getbuffer(PyObject *obj, Py_buffer *view, int fla if ((flags & PyBUF_WRITABLE) == PyBUF_WRITABLE && info->readonly) { delete info; // view->obj = nullptr; // Was just memset to 0, so not necessary - PyErr_SetString(PyExc_BufferError, "Writable buffer requested for readonly storage"); + set_error(PyExc_BufferError, "Writable buffer requested for readonly storage"); return -1; } view->obj = obj; @@ -653,10 +661,13 @@ inline PyObject *make_new_python_type(const type_record &rec) { char *tp_doc = nullptr; if (rec.doc && options::show_user_defined_docstrings()) { - /* Allocate memory for docstring (using PyObject_MALLOC, since - Python will free this later on) */ + /* Allocate memory for docstring (Python will free this later on) */ size_t size = std::strlen(rec.doc) + 1; +#if PY_VERSION_HEX >= 0x030D0000 + tp_doc = (char *) PyMem_MALLOC(size); +#else tp_doc = (char *) PyObject_MALLOC(size); +#endif std::memcpy((void *) tp_doc, rec.doc, size); } diff --git a/pybind11/include/pybind11/detail/common.h b/pybind11/include/pybind11/detail/common.h index 31a54c773..e37152a9a 100644 --- a/pybind11/include/pybind11/detail/common.h +++ b/pybind11/include/pybind11/detail/common.h @@ -10,12 +10,12 @@ #pragma once #define PYBIND11_VERSION_MAJOR 2 -#define PYBIND11_VERSION_MINOR 11 +#define PYBIND11_VERSION_MINOR 13 #define PYBIND11_VERSION_PATCH 1 // Similar to Python's convention: https://docs.python.org/3/c-api/apiabiversion.html // Additional convention: 0xD = dev -#define PYBIND11_VERSION_HEX 0x020B0100 +#define PYBIND11_VERSION_HEX 0x020D0100 // Define some generic pybind11 helper macros for warning management. // @@ -118,6 +118,14 @@ # endif #endif +#if defined(PYBIND11_CPP20) +# define PYBIND11_CONSTINIT constinit +# define PYBIND11_DTOR_CONSTEXPR constexpr +#else +# define PYBIND11_CONSTINIT +# define PYBIND11_DTOR_CONSTEXPR +#endif + // Compiler version assertions #if defined(__INTEL_COMPILER) # if __INTEL_COMPILER < 1800 @@ -264,9 +272,8 @@ PYBIND11_WARNING_DISABLE_MSVC(4505) #endif #include -// Reminder: WITH_THREAD is always defined if PY_VERSION_HEX >= 0x03070000 -#if PY_VERSION_HEX < 0x03060000 -# error "PYTHON < 3.6 IS UNSUPPORTED. pybind11 v2.9 was the last to support Python 2 and 3.5." +#if PY_VERSION_HEX < 0x03070000 +# error "PYTHON < 3.7 IS UNSUPPORTED. pybind11 v2.12 was the last to support Python 3.6." #endif #include #include @@ -288,6 +295,10 @@ PYBIND11_WARNING_DISABLE_MSVC(4505) # undef copysign #endif +#if defined(PYBIND11_NUMPY_1_ONLY) +# define PYBIND11_INTERNAL_NUMPY_1_ONLY_DETECTED +#endif + #if defined(PYPY_VERSION) && !defined(PYBIND11_SIMPLE_GIL_MANAGEMENT) # define PYBIND11_SIMPLE_GIL_MANAGEMENT #endif @@ -399,7 +410,7 @@ PYBIND11_WARNING_POP return nullptr; \ } \ catch (const std::exception &e) { \ - PyErr_SetString(PyExc_ImportError, e.what()); \ + ::pybind11::set_error(PyExc_ImportError, e.what()); \ return nullptr; \ } @@ -452,7 +463,7 @@ PYBIND11_WARNING_POP }); } \endrst */ -#define PYBIND11_MODULE(name, variable) \ +#define PYBIND11_MODULE(name, variable, ...) \ static ::pybind11::module_::module_def PYBIND11_CONCAT(pybind11_module_def_, name) \ PYBIND11_MAYBE_UNUSED; \ PYBIND11_MAYBE_UNUSED \ @@ -461,7 +472,10 @@ PYBIND11_WARNING_POP PYBIND11_CHECK_PYTHON_VERSION \ PYBIND11_ENSURE_INTERNALS_READY \ auto m = ::pybind11::module_::create_extension_module( \ - PYBIND11_TOSTRING(name), nullptr, &PYBIND11_CONCAT(pybind11_module_def_, name)); \ + PYBIND11_TOSTRING(name), \ + nullptr, \ + &PYBIND11_CONCAT(pybind11_module_def_, name), \ + ##__VA_ARGS__); \ try { \ PYBIND11_CONCAT(pybind11_init_, name)(m); \ return m.ptr(); \ @@ -913,8 +927,7 @@ using is_template_base_of = decltype(is_template_base_of_impl::check((intrinsic_t *) nullptr)); #else struct is_template_base_of - : decltype(is_template_base_of_impl::check((intrinsic_t *) nullptr)) { -}; + : decltype(is_template_base_of_impl::check((intrinsic_t *) nullptr)){}; #endif /// Check if T is an instantiation of the template `Class`. For example: @@ -1096,14 +1109,14 @@ struct overload_cast_impl { } template - constexpr auto operator()(Return (Class::*pmf)(Args...), std::false_type = {}) const noexcept - -> decltype(pmf) { + constexpr auto operator()(Return (Class::*pmf)(Args...), + std::false_type = {}) const noexcept -> decltype(pmf) { return pmf; } template - constexpr auto operator()(Return (Class::*pmf)(Args...) const, std::true_type) const noexcept - -> decltype(pmf) { + constexpr auto operator()(Return (Class::*pmf)(Args...) const, + std::true_type) const noexcept -> decltype(pmf) { return pmf; } }; diff --git a/pybind11/include/pybind11/detail/descr.h b/pybind11/include/pybind11/detail/descr.h index 635614b0d..7d546311e 100644 --- a/pybind11/include/pybind11/detail/descr.h +++ b/pybind11/include/pybind11/detail/descr.h @@ -156,8 +156,9 @@ constexpr auto concat(const descr &d, const Args &...args) { } #else template -constexpr auto concat(const descr &d, const Args &...args) - -> decltype(std::declval>() + concat(args...)) { +constexpr auto concat(const descr &d, + const Args &...args) -> decltype(std::declval>() + + concat(args...)) { return d + const_name(", ") + concat(args...); } #endif diff --git a/pybind11/include/pybind11/detail/init.h b/pybind11/include/pybind11/detail/init.h index e21171688..4509bd131 100644 --- a/pybind11/include/pybind11/detail/init.h +++ b/pybind11/include/pybind11/detail/init.h @@ -65,7 +65,7 @@ constexpr bool is_alias(void *) { } // Constructs and returns a new object; if the given arguments don't map to a constructor, we fall -// back to brace aggregate initiailization so that for aggregate initialization can be used with +// back to brace aggregate initialization so that for aggregate initialization can be used with // py::init, e.g. `py::init` to initialize a `struct T { int a; int b; }`. For // non-aggregate types, we need to use an ordinary T(...) constructor (invoking as `T{...}` usually // works, but will not do the expected thing when `T` has an `initializer_list` constructor). diff --git a/pybind11/include/pybind11/detail/internals.h b/pybind11/include/pybind11/detail/internals.h index aaa7f8686..e61c1687f 100644 --- a/pybind11/include/pybind11/detail/internals.h +++ b/pybind11/include/pybind11/detail/internals.h @@ -11,13 +11,15 @@ #include "common.h" -#if defined(WITH_THREAD) && defined(PYBIND11_SIMPLE_GIL_MANAGEMENT) +#if defined(PYBIND11_SIMPLE_GIL_MANAGEMENT) # include "../gil.h" #endif #include "../pytypes.h" #include +#include +#include /// Tracks the `internals` and `type_info` ABI version independent of the main library version. /// @@ -34,8 +36,9 @@ /// further ABI-incompatible changes may be made before the ABI is officially /// changed to the new version. #ifndef PYBIND11_INTERNALS_VERSION -# if PY_VERSION_HEX >= 0x030C0000 +# if PY_VERSION_HEX >= 0x030C0000 || defined(_MSC_VER) // Version bump for Python 3.12+, before first 3.12 beta release. +// Version bump for MSVC piggy-backed on PR #4779. See comments there. # define PYBIND11_INTERNALS_VERSION 5 # else # define PYBIND11_INTERNALS_VERSION 4 @@ -61,60 +64,41 @@ inline PyObject *make_object_base_type(PyTypeObject *metaclass); // The old Python Thread Local Storage (TLS) API is deprecated in Python 3.7 in favor of the new // Thread Specific Storage (TSS) API. -#if PY_VERSION_HEX >= 0x03070000 // Avoid unnecessary allocation of `Py_tss_t`, since we cannot use // `Py_LIMITED_API` anyway. -# if PYBIND11_INTERNALS_VERSION > 4 -# define PYBIND11_TLS_KEY_REF Py_tss_t & -# if defined(__GNUC__) && !defined(__INTEL_COMPILER) -// Clang on macOS warns due to `Py_tss_NEEDS_INIT` not specifying an initializer -// for every field. -# define PYBIND11_TLS_KEY_INIT(var) \ - _Pragma("GCC diagnostic push") /**/ \ - _Pragma("GCC diagnostic ignored \"-Wmissing-field-initializers\"") /**/ \ - Py_tss_t var \ - = Py_tss_NEEDS_INIT; \ - _Pragma("GCC diagnostic pop") -# else -# define PYBIND11_TLS_KEY_INIT(var) Py_tss_t var = Py_tss_NEEDS_INIT; -# endif -# define PYBIND11_TLS_KEY_CREATE(var) (PyThread_tss_create(&(var)) == 0) -# define PYBIND11_TLS_GET_VALUE(key) PyThread_tss_get(&(key)) -# define PYBIND11_TLS_REPLACE_VALUE(key, value) PyThread_tss_set(&(key), (value)) -# define PYBIND11_TLS_DELETE_VALUE(key) PyThread_tss_set(&(key), nullptr) -# define PYBIND11_TLS_FREE(key) PyThread_tss_delete(&(key)) +#if PYBIND11_INTERNALS_VERSION > 4 +# define PYBIND11_TLS_KEY_REF Py_tss_t & +# if defined(__clang__) +# define PYBIND11_TLS_KEY_INIT(var) \ + _Pragma("clang diagnostic push") /**/ \ + _Pragma("clang diagnostic ignored \"-Wmissing-field-initializers\"") /**/ \ + Py_tss_t var \ + = Py_tss_NEEDS_INIT; \ + _Pragma("clang diagnostic pop") +# elif defined(__GNUC__) && !defined(__INTEL_COMPILER) +# define PYBIND11_TLS_KEY_INIT(var) \ + _Pragma("GCC diagnostic push") /**/ \ + _Pragma("GCC diagnostic ignored \"-Wmissing-field-initializers\"") /**/ \ + Py_tss_t var \ + = Py_tss_NEEDS_INIT; \ + _Pragma("GCC diagnostic pop") # else -# define PYBIND11_TLS_KEY_REF Py_tss_t * -# define PYBIND11_TLS_KEY_INIT(var) Py_tss_t *var = nullptr; -# define PYBIND11_TLS_KEY_CREATE(var) \ - (((var) = PyThread_tss_alloc()) != nullptr && (PyThread_tss_create((var)) == 0)) -# define PYBIND11_TLS_GET_VALUE(key) PyThread_tss_get((key)) -# define PYBIND11_TLS_REPLACE_VALUE(key, value) PyThread_tss_set((key), (value)) -# define PYBIND11_TLS_DELETE_VALUE(key) PyThread_tss_set((key), nullptr) -# define PYBIND11_TLS_FREE(key) PyThread_tss_free(key) +# define PYBIND11_TLS_KEY_INIT(var) Py_tss_t var = Py_tss_NEEDS_INIT; # endif +# define PYBIND11_TLS_KEY_CREATE(var) (PyThread_tss_create(&(var)) == 0) +# define PYBIND11_TLS_GET_VALUE(key) PyThread_tss_get(&(key)) +# define PYBIND11_TLS_REPLACE_VALUE(key, value) PyThread_tss_set(&(key), (value)) +# define PYBIND11_TLS_DELETE_VALUE(key) PyThread_tss_set(&(key), nullptr) +# define PYBIND11_TLS_FREE(key) PyThread_tss_delete(&(key)) #else -// Usually an int but a long on Cygwin64 with Python 3.x -# define PYBIND11_TLS_KEY_REF decltype(PyThread_create_key()) -# define PYBIND11_TLS_KEY_INIT(var) PYBIND11_TLS_KEY_REF var = 0; -# define PYBIND11_TLS_KEY_CREATE(var) (((var) = PyThread_create_key()) != -1) -# define PYBIND11_TLS_GET_VALUE(key) PyThread_get_key_value((key)) -# if defined(PYPY_VERSION) -// On CPython < 3.4 and on PyPy, `PyThread_set_key_value` strangely does not set -// the value if it has already been set. Instead, it must first be deleted and -// then set again. -inline void tls_replace_value(PYBIND11_TLS_KEY_REF key, void *value) { - PyThread_delete_key_value(key); - PyThread_set_key_value(key, value); -} -# define PYBIND11_TLS_DELETE_VALUE(key) PyThread_delete_key_value(key) -# define PYBIND11_TLS_REPLACE_VALUE(key, value) \ - ::pybind11::detail::tls_replace_value((key), (value)) -# else -# define PYBIND11_TLS_DELETE_VALUE(key) PyThread_set_key_value((key), nullptr) -# define PYBIND11_TLS_REPLACE_VALUE(key, value) PyThread_set_key_value((key), (value)) -# endif -# define PYBIND11_TLS_FREE(key) (void) key +# define PYBIND11_TLS_KEY_REF Py_tss_t * +# define PYBIND11_TLS_KEY_INIT(var) Py_tss_t *var = nullptr; +# define PYBIND11_TLS_KEY_CREATE(var) \ + (((var) = PyThread_tss_alloc()) != nullptr && (PyThread_tss_create((var)) == 0)) +# define PYBIND11_TLS_GET_VALUE(key) PyThread_tss_get((key)) +# define PYBIND11_TLS_REPLACE_VALUE(key, value) PyThread_tss_set((key), (value)) +# define PYBIND11_TLS_DELETE_VALUE(key) PyThread_tss_set((key), nullptr) +# define PYBIND11_TLS_FREE(key) PyThread_tss_free(key) #endif // Python loads modules by default with dlopen with the RTLD_LOCAL flag; under libc++ and possibly @@ -162,15 +146,33 @@ struct override_hash { } }; +using instance_map = std::unordered_multimap; + +// Instance map shards are used to reduce mutex contention in free-threaded Python. +struct instance_map_shard { + std::mutex mutex; + instance_map registered_instances; + // alignas(64) would be better, but causes compile errors in macOS before 10.14 (see #5200) + char padding[64 - (sizeof(std::mutex) + sizeof(instance_map)) % 64]; +}; + /// Internal data structure used to track registered instances and types. /// Whenever binary incompatible changes are made to this structure, /// `PYBIND11_INTERNALS_VERSION` must be incremented. struct internals { +#ifdef Py_GIL_DISABLED + std::mutex mutex; +#endif // std::type_index -> pybind11's type information type_map registered_types_cpp; // PyTypeObject* -> base type_info(s) std::unordered_map> registered_types_py; - std::unordered_multimap registered_instances; // void * -> instance* +#ifdef Py_GIL_DISABLED + std::unique_ptr instance_shards; // void * -> instance* + size_t instance_shards_mask; +#else + instance_map registered_instances; // void * -> instance* +#endif std::unordered_set, override_hash> inactive_override_cache; type_map> direct_conversions; @@ -186,28 +188,27 @@ struct internals { PyTypeObject *static_property_type; PyTypeObject *default_metaclass; PyObject *instance_base; -#if defined(WITH_THREAD) // Unused if PYBIND11_SIMPLE_GIL_MANAGEMENT is defined: PYBIND11_TLS_KEY_INIT(tstate) -# if PYBIND11_INTERNALS_VERSION > 4 +#if PYBIND11_INTERNALS_VERSION > 4 PYBIND11_TLS_KEY_INIT(loader_life_support_tls_key) -# endif // PYBIND11_INTERNALS_VERSION > 4 +#endif // PYBIND11_INTERNALS_VERSION > 4 // Unused if PYBIND11_SIMPLE_GIL_MANAGEMENT is defined: PyInterpreterState *istate = nullptr; -# if PYBIND11_INTERNALS_VERSION > 4 +#if PYBIND11_INTERNALS_VERSION > 4 // Note that we have to use a std::string to allocate memory to ensure a unique address // We want unique addresses since we use pointer equality to compare function records std::string function_record_capsule_name = internals_function_record_capsule_name; -# endif +#endif internals() = default; internals(const internals &other) = delete; internals &operator=(const internals &other) = delete; ~internals() { -# if PYBIND11_INTERNALS_VERSION > 4 +#if PYBIND11_INTERNALS_VERSION > 4 PYBIND11_TLS_FREE(loader_life_support_tls_key); -# endif // PYBIND11_INTERNALS_VERSION > 4 +#endif // PYBIND11_INTERNALS_VERSION > 4 // This destructor is called *after* Py_Finalize() in finalize_interpreter(). // That *SHOULD BE* fine. The following details what happens when PyThread_tss_free is @@ -218,7 +219,6 @@ struct internals { // that the `tstate` be allocated with the CPython allocator. PYBIND11_TLS_FREE(tstate); } -#endif }; /// Additional type information which does not fit into the PyTypeObject. @@ -291,31 +291,30 @@ struct type_info { #endif /// On Linux/OSX, changes in __GXX_ABI_VERSION__ indicate ABI incompatibility. +/// On MSVC, changes in _MSC_VER may indicate ABI incompatibility (#2898). #ifndef PYBIND11_BUILD_ABI # if defined(__GXX_ABI_VERSION) # define PYBIND11_BUILD_ABI "_cxxabi" PYBIND11_TOSTRING(__GXX_ABI_VERSION) +# elif defined(_MSC_VER) +# define PYBIND11_BUILD_ABI "_mscver" PYBIND11_TOSTRING(_MSC_VER) # else # define PYBIND11_BUILD_ABI "" # endif #endif #ifndef PYBIND11_INTERNALS_KIND -# if defined(WITH_THREAD) -# define PYBIND11_INTERNALS_KIND "" -# else -# define PYBIND11_INTERNALS_KIND "_without_thread" -# endif +# define PYBIND11_INTERNALS_KIND "" #endif #define PYBIND11_INTERNALS_ID \ "__pybind11_internals_v" PYBIND11_TOSTRING(PYBIND11_INTERNALS_VERSION) \ - PYBIND11_INTERNALS_KIND PYBIND11_COMPILER_TYPE PYBIND11_STDLIB PYBIND11_BUILD_ABI \ - PYBIND11_BUILD_TYPE "__" + PYBIND11_INTERNALS_KIND PYBIND11_COMPILER_TYPE PYBIND11_STDLIB \ + PYBIND11_BUILD_ABI PYBIND11_BUILD_TYPE "__" #define PYBIND11_MODULE_LOCAL_ID \ "__pybind11_module_local_v" PYBIND11_TOSTRING(PYBIND11_INTERNALS_VERSION) \ - PYBIND11_INTERNALS_KIND PYBIND11_COMPILER_TYPE PYBIND11_STDLIB PYBIND11_BUILD_ABI \ - PYBIND11_BUILD_TYPE "__" + PYBIND11_INTERNALS_KIND PYBIND11_COMPILER_TYPE PYBIND11_STDLIB \ + PYBIND11_BUILD_ABI PYBIND11_BUILD_TYPE "__" /// Each module locally stores a pointer to the `internals` data. The data /// itself is shared among modules with the same `PYBIND11_INTERNALS_ID`. @@ -352,7 +351,7 @@ inline bool raise_err(PyObject *exc_type, const char *msg) { raise_from(exc_type, msg); return true; } - PyErr_SetString(exc_type, msg); + set_error(exc_type, msg); return false; } @@ -447,22 +446,39 @@ inline object get_python_state_dict() { #endif if (!state_dict) { raise_from(PyExc_SystemError, "pybind11::detail::get_python_state_dict() FAILED"); + throw error_already_set(); } return state_dict; } inline object get_internals_obj_from_state_dict(handle state_dict) { - return reinterpret_borrow(dict_getitemstring(state_dict.ptr(), PYBIND11_INTERNALS_ID)); + return reinterpret_steal( + dict_getitemstringref(state_dict.ptr(), PYBIND11_INTERNALS_ID)); } inline internals **get_internals_pp_from_capsule(handle obj) { void *raw_ptr = PyCapsule_GetPointer(obj.ptr(), /*name=*/nullptr); if (raw_ptr == nullptr) { raise_from(PyExc_SystemError, "pybind11::detail::get_internals_pp_from_capsule() FAILED"); + throw error_already_set(); } return static_cast(raw_ptr); } +inline uint64_t round_up_to_next_pow2(uint64_t x) { + // Round-up to the next power of two. + // See https://graphics.stanford.edu/~seander/bithacks.html#RoundUpPowerOf2 + x--; + x |= (x >> 1); + x |= (x >> 2); + x |= (x >> 4); + x |= (x >> 8); + x |= (x >> 16); + x |= (x >> 32); + x++; + return x; +} + /// Return a reference to the current `internals` data PYBIND11_NOINLINE internals &get_internals() { auto **&internals_pp = get_internals_pp(); @@ -470,10 +486,9 @@ PYBIND11_NOINLINE internals &get_internals() { return **internals_pp; } -#if defined(WITH_THREAD) -# if defined(PYBIND11_SIMPLE_GIL_MANAGEMENT) +#if defined(PYBIND11_SIMPLE_GIL_MANAGEMENT) gil_scoped_acquire gil; -# else +#else // Ensure that the GIL is held since we will need to make Python calls. // Cannot use py::gil_scoped_acquire here since that constructor calls get_internals. struct gil_scoped_acquire_local { @@ -483,7 +498,6 @@ PYBIND11_NOINLINE internals &get_internals() { ~gil_scoped_acquire_local() { PyGILState_Release(state); } const PyGILState_STATE state; } gil; -# endif #endif error_scope err_scope; @@ -508,7 +522,6 @@ PYBIND11_NOINLINE internals &get_internals() { } auto *&internals_ptr = *internals_pp; internals_ptr = new internals(); -#if defined(WITH_THREAD) PyThreadState *tstate = PyThreadState_Get(); // NOLINTNEXTLINE(bugprone-assignment-in-if-condition) @@ -517,20 +530,29 @@ PYBIND11_NOINLINE internals &get_internals() { } PYBIND11_TLS_REPLACE_VALUE(internals_ptr->tstate, tstate); -# if PYBIND11_INTERNALS_VERSION > 4 +#if PYBIND11_INTERNALS_VERSION > 4 // NOLINTNEXTLINE(bugprone-assignment-in-if-condition) if (!PYBIND11_TLS_KEY_CREATE(internals_ptr->loader_life_support_tls_key)) { pybind11_fail("get_internals: could not successfully initialize the " "loader_life_support TSS key!"); } -# endif - internals_ptr->istate = tstate->interp; #endif + internals_ptr->istate = tstate->interp; state_dict[PYBIND11_INTERNALS_ID] = capsule(internals_pp); internals_ptr->registered_exception_translators.push_front(&translate_exception); internals_ptr->static_property_type = make_static_property_type(); internals_ptr->default_metaclass = make_default_metaclass(); internals_ptr->instance_base = make_object_base_type(internals_ptr->default_metaclass); +#ifdef Py_GIL_DISABLED + // Scale proportional to the number of cores. 2x is a heuristic to reduce contention. + auto num_shards + = static_cast(round_up_to_next_pow2(2 * std::thread::hardware_concurrency())); + if (num_shards == 0) { + num_shards = 1; + } + internals_ptr->instance_shards.reset(new instance_map_shard[num_shards]); + internals_ptr->instance_shards_mask = num_shards - 1; +#endif // Py_GIL_DISABLED } return **internals_pp; } @@ -544,7 +566,7 @@ PYBIND11_NOINLINE internals &get_internals() { struct local_internals { type_map registered_types_cpp; std::forward_list registered_exception_translators; -#if defined(WITH_THREAD) && PYBIND11_INTERNALS_VERSION == 4 +#if PYBIND11_INTERNALS_VERSION == 4 // For ABI compatibility, we can't store the loader_life_support TLS key in // the `internals` struct directly. Instead, we store it in `shared_data` and @@ -577,7 +599,7 @@ struct local_internals { loader_life_support_tls_key = static_cast(ptr)->loader_life_support_tls_key; } -#endif // defined(WITH_THREAD) && PYBIND11_INTERNALS_VERSION == 4 +#endif // PYBIND11_INTERNALS_VERSION == 4 }; /// Works like `get_internals`, but for things which are locally registered. @@ -591,13 +613,80 @@ inline local_internals &get_local_internals() { return *locals; } +#ifdef Py_GIL_DISABLED +# define PYBIND11_LOCK_INTERNALS(internals) std::unique_lock lock((internals).mutex) +#else +# define PYBIND11_LOCK_INTERNALS(internals) +#endif + +template +inline auto with_internals(const F &cb) -> decltype(cb(get_internals())) { + auto &internals = get_internals(); + PYBIND11_LOCK_INTERNALS(internals); + return cb(internals); +} + +inline std::uint64_t mix64(std::uint64_t z) { + // David Stafford's variant 13 of the MurmurHash3 finalizer popularized + // by the SplitMix PRNG. + // https://zimbry.blogspot.com/2011/09/better-bit-mixing-improving-on.html + z = (z ^ (z >> 30)) * 0xbf58476d1ce4e5b9; + z = (z ^ (z >> 27)) * 0x94d049bb133111eb; + return z ^ (z >> 31); +} + +template +inline auto with_instance_map(const void *ptr, + const F &cb) -> decltype(cb(std::declval())) { + auto &internals = get_internals(); + +#ifdef Py_GIL_DISABLED + // Hash address to compute shard, but ignore low bits. We'd like allocations + // from the same thread/core to map to the same shard and allocations from + // other threads/cores to map to other shards. Using the high bits is a good + // heuristic because memory allocators often have a per-thread + // arena/superblock/segment from which smaller allocations are served. + auto addr = reinterpret_cast(ptr); + auto hash = mix64(static_cast(addr >> 20)); + auto idx = static_cast(hash & internals.instance_shards_mask); + + auto &shard = internals.instance_shards[idx]; + std::unique_lock lock(shard.mutex); + return cb(shard.registered_instances); +#else + (void) ptr; + return cb(internals.registered_instances); +#endif +} + +// Returns the number of registered instances for testing purposes. The result may not be +// consistent if other threads are registering or unregistering instances concurrently. +inline size_t num_registered_instances() { + auto &internals = get_internals(); +#ifdef Py_GIL_DISABLED + size_t count = 0; + for (size_t i = 0; i <= internals.instance_shards_mask; ++i) { + auto &shard = internals.instance_shards[i]; + std::unique_lock lock(shard.mutex); + count += shard.registered_instances.size(); + } + return count; +#else + return internals.registered_instances.size(); +#endif +} + /// Constructs a std::string with the given arguments, stores it in `internals`, and returns its /// `c_str()`. Such strings objects have a long storage duration -- the internal strings are only /// cleared when the program exits or after interpreter shutdown (when embedding), and so are /// suitable for c-style strings needed by Python internals (such as PyTypeObject's tp_name). template const char *c_str(Args &&...args) { - auto &strings = get_internals().static_strings; + // GCC 4.8 doesn't like parameter unpack within lambda capture, so use + // PYBIND11_LOCK_INTERNALS. + auto &internals = get_internals(); + PYBIND11_LOCK_INTERNALS(internals); + auto &strings = internals.static_strings; strings.emplace_front(std::forward(args)...); return strings.front().c_str(); } @@ -627,15 +716,18 @@ PYBIND11_NAMESPACE_END(detail) /// pybind11 version) running in the current interpreter. Names starting with underscores /// are reserved for internal usage. Returns `nullptr` if no matching entry was found. PYBIND11_NOINLINE void *get_shared_data(const std::string &name) { - auto &internals = detail::get_internals(); - auto it = internals.shared_data.find(name); - return it != internals.shared_data.end() ? it->second : nullptr; + return detail::with_internals([&](detail::internals &internals) { + auto it = internals.shared_data.find(name); + return it != internals.shared_data.end() ? it->second : nullptr; + }); } /// Set the shared data that can be later recovered by `get_shared_data()`. PYBIND11_NOINLINE void *set_shared_data(const std::string &name, void *data) { - detail::get_internals().shared_data[name] = data; - return data; + return detail::with_internals([&](detail::internals &internals) { + internals.shared_data[name] = data; + return data; + }); } /// Returns a typed reference to a shared data entry (by using `get_shared_data()`) if @@ -643,14 +735,15 @@ PYBIND11_NOINLINE void *set_shared_data(const std::string &name, void *data) { /// added to the shared data under the given name and a reference to it is returned. template T &get_or_create_shared_data(const std::string &name) { - auto &internals = detail::get_internals(); - auto it = internals.shared_data.find(name); - T *ptr = (T *) (it != internals.shared_data.end() ? it->second : nullptr); - if (!ptr) { - ptr = new T(); - internals.shared_data[name] = ptr; - } - return *ptr; + return *detail::with_internals([&](detail::internals &internals) { + auto it = internals.shared_data.find(name); + T *ptr = (T *) (it != internals.shared_data.end() ? it->second : nullptr); + if (!ptr) { + ptr = new T(); + internals.shared_data[name] = ptr; + } + return ptr; + }); } PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE) diff --git a/pybind11/include/pybind11/detail/type_caster_base.h b/pybind11/include/pybind11/detail/type_caster_base.h index 16387506c..fd8c81b9a 100644 --- a/pybind11/include/pybind11/detail/type_caster_base.h +++ b/pybind11/include/pybind11/detail/type_caster_base.h @@ -36,14 +36,13 @@ private: loader_life_support *parent = nullptr; std::unordered_set keep_alive; -#if defined(WITH_THREAD) // Store stack pointer in thread-local storage. static PYBIND11_TLS_KEY_REF get_stack_tls_key() { -# if PYBIND11_INTERNALS_VERSION == 4 +#if PYBIND11_INTERNALS_VERSION == 4 return get_local_internals().loader_life_support_tls_key; -# else +#else return get_internals().loader_life_support_tls_key; -# endif +#endif } static loader_life_support *get_stack_top() { return static_cast(PYBIND11_TLS_GET_VALUE(get_stack_tls_key())); @@ -51,15 +50,6 @@ private: static void set_stack_top(loader_life_support *value) { PYBIND11_TLS_REPLACE_VALUE(get_stack_tls_key(), value); } -#else - // Use single global variable for stack. - static loader_life_support **get_stack_pp() { - static loader_life_support *global_stack = nullptr; - return global_stack; - } - static loader_life_support *get_stack_top() { return *get_stack_pp(); } - static void set_stack_top(loader_life_support *value) { *get_stack_pp() = value; } -#endif public: /// A new patient frame is created when a function is entered @@ -102,8 +92,22 @@ public: inline std::pair all_type_info_get_cache(PyTypeObject *type); +// Band-aid workaround to fix a subtle but serious bug in a minimalistic fashion. See PR #4762. +inline void all_type_info_add_base_most_derived_first(std::vector &bases, + type_info *addl_base) { + for (auto it = bases.begin(); it != bases.end(); it++) { + type_info *existing_base = *it; + if (PyType_IsSubtype(addl_base->type, existing_base->type) != 0) { + bases.insert(it, addl_base); + return; + } + } + bases.push_back(addl_base); +} + // Populates a just-created cache entry. PYBIND11_NOINLINE void all_type_info_populate(PyTypeObject *t, std::vector &bases) { + assert(bases.empty()); std::vector check; for (handle parent : reinterpret_borrow(t->tp_bases)) { check.push_back((PyTypeObject *) parent.ptr()); @@ -136,7 +140,7 @@ PYBIND11_NOINLINE void all_type_info_populate(PyTypeObject *t, std::vectortp_bases) { @@ -203,12 +207,15 @@ inline detail::type_info *get_local_type_info(const std::type_index &tp) { } inline detail::type_info *get_global_type_info(const std::type_index &tp) { - auto &types = get_internals().registered_types_cpp; - auto it = types.find(tp); - if (it != types.end()) { - return it->second; - } - return nullptr; + return with_internals([&](internals &internals) { + detail::type_info *type_info = nullptr; + auto &types = internals.registered_types_cpp; + auto it = types.find(tp); + if (it != types.end()) { + type_info = it->second; + } + return type_info; + }); } /// Return the type info for a given C++ type; on lookup failure can either throw or return @@ -239,15 +246,17 @@ PYBIND11_NOINLINE handle get_type_handle(const std::type_info &tp, bool throw_if // Searches the inheritance graph for a registered Python instance, using all_type_info(). PYBIND11_NOINLINE handle find_registered_python_instance(void *src, const detail::type_info *tinfo) { - auto it_instances = get_internals().registered_instances.equal_range(src); - for (auto it_i = it_instances.first; it_i != it_instances.second; ++it_i) { - for (auto *instance_type : detail::all_type_info(Py_TYPE(it_i->second))) { - if (instance_type && same_type(*instance_type->cpptype, *tinfo->cpptype)) { - return handle((PyObject *) it_i->second).inc_ref(); + return with_instance_map(src, [&](instance_map &instances) { + auto it_instances = instances.equal_range(src); + for (auto it_i = it_instances.first; it_i != it_instances.second; ++it_i) { + for (auto *instance_type : detail::all_type_info(Py_TYPE(it_i->second))) { + if (instance_type && same_type(*instance_type->cpptype, *tinfo->cpptype)) { + return handle((PyObject *) it_i->second).inc_ref(); + } } } - } - return handle(); + return handle(); + }); } struct value_and_holder { @@ -322,18 +331,29 @@ public: explicit values_and_holders(instance *inst) : inst{inst}, tinfo(all_type_info(Py_TYPE(inst))) {} + explicit values_and_holders(PyObject *obj) + : inst{nullptr}, tinfo(all_type_info(Py_TYPE(obj))) { + if (!tinfo.empty()) { + inst = reinterpret_cast(obj); + } + } + struct iterator { private: instance *inst = nullptr; const type_vec *types = nullptr; value_and_holder curr; friend struct values_and_holders; - iterator(instance *inst, const type_vec *tinfo) - : inst{inst}, types{tinfo}, - curr(inst /* instance */, - types->empty() ? nullptr : (*types)[0] /* type info */, - 0, /* vpos: (non-simple types only): the first vptr comes first */ - 0 /* index */) {} + iterator(instance *inst, const type_vec *tinfo) : inst{inst}, types{tinfo} { + if (inst != nullptr) { + assert(!types->empty()); + curr = value_and_holder( + inst /* instance */, + (*types)[0] /* type info */, + 0, /* vpos: (non-simple types only): the first vptr comes first */ + 0 /* index */); + } + } // Past-the-end iterator: explicit iterator(size_t end) : curr(end) {} @@ -364,6 +384,16 @@ public: } size_t size() { return tinfo.size(); } + + // Band-aid workaround to fix a subtle but serious bug in a minimalistic fashion. See PR #4762. + bool is_redundant_value_and_holder(const value_and_holder &vh) { + for (size_t i = 0; i < vh.index; i++) { + if (PyType_IsSubtype(tinfo[i]->type, tinfo[vh.index]->type) != 0) { + return true; + } + } + return false; + } }; /** @@ -471,23 +501,26 @@ PYBIND11_NOINLINE bool isinstance_generic(handle obj, const std::type_info &tp) } PYBIND11_NOINLINE handle get_object_handle(const void *ptr, const detail::type_info *type) { - auto &instances = get_internals().registered_instances; - auto range = instances.equal_range(ptr); - for (auto it = range.first; it != range.second; ++it) { - for (const auto &vh : values_and_holders(it->second)) { - if (vh.type == type) { - return handle((PyObject *) it->second); + return with_instance_map(ptr, [&](instance_map &instances) { + auto range = instances.equal_range(ptr); + for (auto it = range.first; it != range.second; ++it) { + for (const auto &vh : values_and_holders(it->second)) { + if (vh.type == type) { + return handle((PyObject *) it->second); + } } } - } - return handle(); + return handle(); + }); } inline PyThreadState *get_thread_state_unchecked() { #if defined(PYPY_VERSION) return PyThreadState_GET(); -#else +#elif PY_VERSION_HEX < 0x030D0000 return _PyThreadState_UncheckedGet(); +#else + return PyThreadState_GetUnchecked(); #endif } @@ -786,7 +819,7 @@ public: std::string tname = rtti_type ? rtti_type->name() : cast_type.name(); detail::clean_type_id(tname); std::string msg = "Unregistered type : " + tname; - PyErr_SetString(PyExc_TypeError, msg.c_str()); + set_error(PyExc_TypeError, msg.c_str()); return {nullptr, nullptr}; } @@ -1074,11 +1107,11 @@ public: || policy == return_value_policy::automatic_reference) { policy = return_value_policy::copy; } - return cast(&src, policy, parent); + return cast(std::addressof(src), policy, parent); } static handle cast(itype &&src, return_value_policy, handle parent) { - return cast(&src, return_value_policy::move, parent); + return cast(std::addressof(src), return_value_policy::move, parent); } // Returns a (pointer, type_info) pair taking care of necessary type lookup for a @@ -1147,14 +1180,14 @@ protected: does not have a private operator new implementation. A comma operator is used in the decltype argument to apply SFINAE to the public copy/move constructors.*/ template ::value>> - static auto make_copy_constructor(const T *) - -> decltype(new T(std::declval()), Constructor{}) { + static auto make_copy_constructor(const T *) -> decltype(new T(std::declval()), + Constructor{}) { return [](const void *arg) -> void * { return new T(*reinterpret_cast(arg)); }; } template ::value>> - static auto make_move_constructor(const T *) - -> decltype(new T(std::declval()), Constructor{}) { + static auto make_move_constructor(const T *) -> decltype(new T(std::declval()), + Constructor{}) { return [](const void *arg) -> void * { return new T(std::move(*const_cast(reinterpret_cast(arg)))); }; @@ -1164,13 +1197,17 @@ protected: static Constructor make_move_constructor(...) { return nullptr; } }; +inline std::string quote_cpp_type_name(const std::string &cpp_type_name) { + return cpp_type_name; // No-op for now. See PR #4888 +} + PYBIND11_NOINLINE std::string type_info_description(const std::type_info &ti) { if (auto *type_data = get_type_info(ti)) { handle th((PyObject *) type_data->type); return th.attr("__module__").cast() + '.' + th.attr("__qualname__").cast(); } - return clean_type_id(ti.name()); + return quote_cpp_type_name(clean_type_id(ti.name())); } PYBIND11_NAMESPACE_END(detail) diff --git a/pybind11/include/pybind11/eigen/tensor.h b/pybind11/include/pybind11/eigen/tensor.h index 25d12baca..d4ed6c0ca 100644 --- a/pybind11/include/pybind11/eigen/tensor.h +++ b/pybind11/include/pybind11/eigen/tensor.h @@ -70,7 +70,7 @@ struct eigen_tensor_helper struct helper> { - static constexpr auto value = concat(const_name(((void) Is, "?"))...); + static constexpr auto value = ::pybind11::detail::concat(const_name(((void) Is, "?"))...); }; static constexpr auto dimensions_descriptor @@ -104,7 +104,8 @@ struct eigen_tensor_helper< return get_shape() == shape; } - static constexpr auto dimensions_descriptor = concat(const_name()...); + static constexpr auto dimensions_descriptor + = ::pybind11::detail::concat(const_name()...); template static Type *alloc(Args &&...args) { diff --git a/pybind11/include/pybind11/embed.h b/pybind11/include/pybind11/embed.h index caa14f4a0..9d29eb824 100644 --- a/pybind11/include/pybind11/embed.h +++ b/pybind11/include/pybind11/embed.h @@ -103,9 +103,6 @@ inline void initialize_interpreter_pre_pyconfig(bool init_signal_handlers, bool add_program_dir_to_path) { detail::precheck_interpreter(); Py_InitializeEx(init_signal_handlers ? 1 : 0); -# if defined(WITH_THREAD) && PY_VERSION_HEX < 0x03070000 - PyEval_InitThreads(); -# endif // Before it was special-cased in python 3.8, passing an empty or null argv // caused a segfault, so we have to reimplement the special case ourselves. diff --git a/pybind11/include/pybind11/functional.h b/pybind11/include/pybind11/functional.h index 87ec4d10c..6856119cd 100644 --- a/pybind11/include/pybind11/functional.h +++ b/pybind11/include/pybind11/functional.h @@ -128,7 +128,8 @@ public: } PYBIND11_TYPE_CASTER(type, - const_name("Callable[[") + concat(make_caster::name...) + const_name("Callable[[") + + ::pybind11::detail::concat(make_caster::name...) + const_name("], ") + make_caster::name + const_name("]")); }; diff --git a/pybind11/include/pybind11/gil.h b/pybind11/include/pybind11/gil.h index 570a5581d..6b0edaee4 100644 --- a/pybind11/include/pybind11/gil.h +++ b/pybind11/include/pybind11/gil.h @@ -11,7 +11,9 @@ #include "detail/common.h" -#if defined(WITH_THREAD) && !defined(PYBIND11_SIMPLE_GIL_MANAGEMENT) +#include + +#if !defined(PYBIND11_SIMPLE_GIL_MANAGEMENT) # include "detail/internals.h" #endif @@ -24,9 +26,7 @@ PyThreadState *get_thread_state_unchecked(); PYBIND11_NAMESPACE_END(detail) -#if defined(WITH_THREAD) - -# if !defined(PYBIND11_SIMPLE_GIL_MANAGEMENT) +#if !defined(PYBIND11_SIMPLE_GIL_MANAGEMENT) /* The functions below essentially reproduce the PyGILState_* API using a RAII * pattern, but there are a few important differences: @@ -67,11 +67,11 @@ public: if (!tstate) { tstate = PyThreadState_New(internals.istate); -# if defined(PYBIND11_DETAILED_ERROR_MESSAGES) +# if defined(PYBIND11_DETAILED_ERROR_MESSAGES) if (!tstate) { pybind11_fail("scoped_acquire: could not create thread state!"); } -# endif +# endif tstate->gilstate_counter = 0; PYBIND11_TLS_REPLACE_VALUE(internals.tstate, tstate); } else { @@ -92,20 +92,20 @@ public: PYBIND11_NOINLINE void dec_ref() { --tstate->gilstate_counter; -# if defined(PYBIND11_DETAILED_ERROR_MESSAGES) +# if defined(PYBIND11_DETAILED_ERROR_MESSAGES) if (detail::get_thread_state_unchecked() != tstate) { pybind11_fail("scoped_acquire::dec_ref(): thread state must be current!"); } if (tstate->gilstate_counter < 0) { pybind11_fail("scoped_acquire::dec_ref(): reference count underflow!"); } -# endif +# endif if (tstate->gilstate_counter == 0) { -# if defined(PYBIND11_DETAILED_ERROR_MESSAGES) +# if defined(PYBIND11_DETAILED_ERROR_MESSAGES) if (!release) { pybind11_fail("scoped_acquire::dec_ref(): internal error!"); } -# endif +# endif PyThreadState_Clear(tstate); if (active) { PyThreadState_DeleteCurrent(); @@ -137,7 +137,9 @@ private: class gil_scoped_release { public: + // PRECONDITION: The GIL must be held when this constructor is called. explicit gil_scoped_release(bool disassoc = false) : disassoc(disassoc) { + assert(PyGILState_Check()); // `get_internals()` must be called here unconditionally in order to initialize // `internals.tstate` for subsequent `gil_scoped_acquire` calls. Otherwise, an // initialization race could occur as multiple threads try `gil_scoped_acquire`. @@ -184,7 +186,7 @@ private: bool active = true; }; -# else // PYBIND11_SIMPLE_GIL_MANAGEMENT +#else // PYBIND11_SIMPLE_GIL_MANAGEMENT class gil_scoped_acquire { PyGILState_STATE state; @@ -201,39 +203,17 @@ class gil_scoped_release { PyThreadState *state; public: - gil_scoped_release() : state{PyEval_SaveThread()} {} + // PRECONDITION: The GIL must be held when this constructor is called. + gil_scoped_release() { + assert(PyGILState_Check()); + state = PyEval_SaveThread(); + } gil_scoped_release(const gil_scoped_release &) = delete; gil_scoped_release &operator=(const gil_scoped_release &) = delete; ~gil_scoped_release() { PyEval_RestoreThread(state); } void disarm() {} }; -# endif // PYBIND11_SIMPLE_GIL_MANAGEMENT - -#else // WITH_THREAD - -class gil_scoped_acquire { -public: - gil_scoped_acquire() { - // Trick to suppress `unused variable` error messages (at call sites). - (void) (this != (this + 1)); - } - gil_scoped_acquire(const gil_scoped_acquire &) = delete; - gil_scoped_acquire &operator=(const gil_scoped_acquire &) = delete; - void disarm() {} -}; - -class gil_scoped_release { -public: - gil_scoped_release() { - // Trick to suppress `unused variable` error messages (at call sites). - (void) (this != (this + 1)); - } - gil_scoped_release(const gil_scoped_release &) = delete; - gil_scoped_release &operator=(const gil_scoped_release &) = delete; - void disarm() {} -}; - -#endif // WITH_THREAD +#endif // PYBIND11_SIMPLE_GIL_MANAGEMENT PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE) diff --git a/pybind11/include/pybind11/gil_safe_call_once.h b/pybind11/include/pybind11/gil_safe_call_once.h new file mode 100644 index 000000000..eaf84d16e --- /dev/null +++ b/pybind11/include/pybind11/gil_safe_call_once.h @@ -0,0 +1,91 @@ +// Copyright (c) 2023 The pybind Community. + +#pragma once + +#include "detail/common.h" +#include "gil.h" + +#include +#include + +PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE) + +// Use the `gil_safe_call_once_and_store` class below instead of the naive +// +// static auto imported_obj = py::module_::import("module_name"); // BAD, DO NOT USE! +// +// which has two serious issues: +// +// 1. Py_DECREF() calls potentially after the Python interpreter was finalized already, and +// 2. deadlocks in multi-threaded processes (because of missing lock ordering). +// +// The following alternative avoids both problems: +// +// PYBIND11_CONSTINIT static py::gil_safe_call_once_and_store storage; +// auto &imported_obj = storage // Do NOT make this `static`! +// .call_once_and_store_result([]() { +// return py::module_::import("module_name"); +// }) +// .get_stored(); +// +// The parameter of `call_once_and_store_result()` must be callable. It can make +// CPython API calls, and in particular, it can temporarily release the GIL. +// +// `T` can be any C++ type, it does not have to involve CPython API types. +// +// The behavior with regard to signals, e.g. `SIGINT` (`KeyboardInterrupt`), +// is not ideal. If the main thread is the one to actually run the `Callable`, +// then a `KeyboardInterrupt` will interrupt it if it is running normal Python +// code. The situation is different if a non-main thread runs the +// `Callable`, and then the main thread starts waiting for it to complete: +// a `KeyboardInterrupt` will not interrupt the non-main thread, but it will +// get processed only when it is the main thread's turn again and it is running +// normal Python code. However, this will be unnoticeable for quick call-once +// functions, which is usually the case. +template +class gil_safe_call_once_and_store { +public: + // PRECONDITION: The GIL must be held when `call_once_and_store_result()` is called. + template + gil_safe_call_once_and_store &call_once_and_store_result(Callable &&fn) { + if (!is_initialized_) { // This read is guarded by the GIL. + // Multiple threads may enter here, because the GIL is released in the next line and + // CPython API calls in the `fn()` call below may release and reacquire the GIL. + gil_scoped_release gil_rel; // Needed to establish lock ordering. + std::call_once(once_flag_, [&] { + // Only one thread will ever enter here. + gil_scoped_acquire gil_acq; + ::new (storage_) T(fn()); // fn may release, but will reacquire, the GIL. + is_initialized_ = true; // This write is guarded by the GIL. + }); + // All threads will observe `is_initialized_` as true here. + } + // Intentionally not returning `T &` to ensure the calling code is self-documenting. + return *this; + } + + // This must only be called after `call_once_and_store_result()` was called. + T &get_stored() { + assert(is_initialized_); + PYBIND11_WARNING_PUSH +#if !defined(__clang__) && defined(__GNUC__) && __GNUC__ < 5 + // Needed for gcc 4.8.5 + PYBIND11_WARNING_DISABLE_GCC("-Wstrict-aliasing") +#endif + return *reinterpret_cast(storage_); + PYBIND11_WARNING_POP + } + + constexpr gil_safe_call_once_and_store() = default; + PYBIND11_DTOR_CONSTEXPR ~gil_safe_call_once_and_store() = default; + +private: + alignas(T) char storage_[sizeof(T)] = {}; + std::once_flag once_flag_ = {}; + bool is_initialized_ = false; + // The `is_initialized_`-`storage_` pair is very similar to `std::optional`, + // but the latter does not have the triviality properties of former, + // therefore `std::optional` is not a viable alternative here. +}; + +PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE) diff --git a/pybind11/include/pybind11/numpy.h b/pybind11/include/pybind11/numpy.h index 36077ec04..05ef3918b 100644 --- a/pybind11/include/pybind11/numpy.h +++ b/pybind11/include/pybind11/numpy.h @@ -10,7 +10,10 @@ #pragma once #include "pybind11.h" +#include "detail/common.h" #include "complex.h" +#include "gil_safe_call_once.h" +#include "pytypes.h" #include #include @@ -26,10 +29,15 @@ #include #include +#if defined(PYBIND11_NUMPY_1_ONLY) && !defined(PYBIND11_INTERNAL_NUMPY_1_ONLY_DETECTED) +# error PYBIND11_NUMPY_1_ONLY must be defined before any pybind11 header is included. +#endif + /* This will be true on all flat address space platforms and allows us to reduce the whole npy_intp / ssize_t / Py_intptr_t business down to just ssize_t for all size and dimension types (e.g. shape, strides, indexing), instead of inflicting this - upon the library user. */ + upon the library user. + Note that NumPy 2 now uses ssize_t for `npy_intp` to simplify this. */ static_assert(sizeof(::pybind11::ssize_t) == sizeof(Py_intptr_t), "ssize_t != Py_intptr_t"); static_assert(std::is_signed::value, "Py_intptr_t must be signed"); // We now can reinterpret_cast between py::ssize_t and Py_intptr_t (MSVC + PyPy cares) @@ -38,10 +46,16 @@ PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE) PYBIND11_WARNING_DISABLE_MSVC(4127) +class dtype; // Forward declaration class array; // Forward declaration PYBIND11_NAMESPACE_BEGIN(detail) +template <> +struct handle_type_name { + static constexpr auto name = const_name("numpy.dtype"); +}; + template <> struct handle_type_name { static constexpr auto name = const_name("numpy.ndarray"); @@ -50,7 +64,8 @@ struct handle_type_name { template struct npy_format_descriptor; -struct PyArrayDescr_Proxy { +/* NumPy 1 proxy (always includes legacy fields) */ +struct PyArrayDescr1_Proxy { PyObject_HEAD PyObject *typeobj; char kind; @@ -65,6 +80,43 @@ struct PyArrayDescr_Proxy { PyObject *names; }; +#ifndef PYBIND11_NUMPY_1_ONLY +struct PyArrayDescr_Proxy { + PyObject_HEAD + PyObject *typeobj; + char kind; + char type; + char byteorder; + char _former_flags; + int type_num; + /* Additional fields are NumPy version specific. */ +}; +#else +/* NumPy 1.x only, we can expose all fields */ +using PyArrayDescr_Proxy = PyArrayDescr1_Proxy; +#endif + +/* NumPy 2 proxy, including legacy fields */ +struct PyArrayDescr2_Proxy { + PyObject_HEAD + PyObject *typeobj; + char kind; + char type; + char byteorder; + char _former_flags; + int type_num; + std::uint64_t flags; + ssize_t elsize; + ssize_t alignment; + PyObject *metadata; + Py_hash_t hash; + void *reserved_null[2]; + /* The following fields only exist if 0 <= type_num < 2056 */ + char *subarray; + PyObject *fields; + PyObject *names; +}; + struct PyArray_Proxy { PyObject_HEAD char *data; @@ -120,6 +172,28 @@ inline numpy_internals &get_numpy_internals() { return *ptr; } +PYBIND11_NOINLINE module_ import_numpy_core_submodule(const char *submodule_name) { + module_ numpy = module_::import("numpy"); + str version_string = numpy.attr("__version__"); + + module_ numpy_lib = module_::import("numpy.lib"); + object numpy_version = numpy_lib.attr("NumpyVersion")(version_string); + int major_version = numpy_version.attr("major").cast(); + +#ifdef PYBIND11_NUMPY_1_ONLY + if (major_version >= 2) { + throw std::runtime_error( + "This extension was built with PYBIND11_NUMPY_1_ONLY defined, " + "but NumPy 2 is used in this process. For NumPy2 compatibility, " + "this extension needs to be rebuilt without the PYBIND11_NUMPY_1_ONLY define."); + } +#endif + /* `numpy.core` was renamed to `numpy._core` in NumPy 2.0 as it officially + became a private module. */ + std::string numpy_core_path = major_version >= 2 ? "numpy._core" : "numpy.core"; + return module_::import((numpy_core_path + "." + submodule_name).c_str()); +} + template struct same_size { template @@ -186,14 +260,16 @@ struct npy_api { NPY_ULONG_, NPY_ULONGLONG_, NPY_UINT_), }; + unsigned int PyArray_RUNTIME_VERSION_; + struct PyArray_Dims { Py_intptr_t *ptr; int len; }; static npy_api &get() { - static npy_api api = lookup(); - return api; + PYBIND11_CONSTINIT static gil_safe_call_once_and_store storage; + return storage.call_once_and_store_result(lookup).get_stored(); } bool PyArray_Check_(PyObject *obj) const { @@ -224,6 +300,7 @@ struct npy_api { PyObject *(*PyArray_FromAny_)(PyObject *, PyObject *, int, int, int, PyObject *); int (*PyArray_DescrConverter_)(PyObject *, PyObject **); bool (*PyArray_EquivTypes_)(PyObject *, PyObject *); +#ifdef PYBIND11_NUMPY_1_ONLY int (*PyArray_GetArrayParamsFromObject_)(PyObject *, PyObject *, unsigned char, @@ -232,6 +309,7 @@ struct npy_api { Py_intptr_t *, PyObject **, PyObject *); +#endif PyObject *(*PyArray_Squeeze_)(PyObject *); // Unused. Not removed because that affects ABI of the class. int (*PyArray_SetBaseObject_)(PyObject *, PyObject *); @@ -249,7 +327,8 @@ private: API_PyArray_DescrFromScalar = 57, API_PyArray_FromAny = 69, API_PyArray_Resize = 80, - API_PyArray_CopyInto = 82, + // CopyInto was slot 82 and 50 was effectively an alias. NumPy 2 removed 82. + API_PyArray_CopyInto = 50, API_PyArray_NewCopy = 85, API_PyArray_NewFromDescr = 94, API_PyArray_DescrNewFromType = 96, @@ -258,18 +337,25 @@ private: API_PyArray_View = 137, API_PyArray_DescrConverter = 174, API_PyArray_EquivTypes = 182, +#ifdef PYBIND11_NUMPY_1_ONLY API_PyArray_GetArrayParamsFromObject = 278, +#endif API_PyArray_SetBaseObject = 282 }; static npy_api lookup() { - module_ m = module_::import("numpy.core.multiarray"); + module_ m = detail::import_numpy_core_submodule("multiarray"); auto c = m.attr("_ARRAY_API"); void **api_ptr = (void **) PyCapsule_GetPointer(c.ptr(), nullptr); + if (api_ptr == nullptr) { + raise_from(PyExc_SystemError, "FAILURE obtaining numpy _ARRAY_API pointer."); + throw error_already_set(); + } npy_api api; #define DECL_NPY_API(Func) api.Func##_ = (decltype(api.Func##_)) api_ptr[API_##Func]; DECL_NPY_API(PyArray_GetNDArrayCFeatureVersion); - if (api.PyArray_GetNDArrayCFeatureVersion_() < 0x7) { + api.PyArray_RUNTIME_VERSION_ = api.PyArray_GetNDArrayCFeatureVersion_(); + if (api.PyArray_RUNTIME_VERSION_ < 0x7) { pybind11_fail("pybind11 numpy support requires numpy >= 1.7.0"); } DECL_NPY_API(PyArray_Type); @@ -288,7 +374,9 @@ private: DECL_NPY_API(PyArray_View); DECL_NPY_API(PyArray_DescrConverter); DECL_NPY_API(PyArray_EquivTypes); +#ifdef PYBIND11_NUMPY_1_ONLY DECL_NPY_API(PyArray_GetArrayParamsFromObject); +#endif DECL_NPY_API(PyArray_SetBaseObject); #undef DECL_NPY_API @@ -310,6 +398,14 @@ inline const PyArrayDescr_Proxy *array_descriptor_proxy(const PyObject *ptr) { return reinterpret_cast(ptr); } +inline const PyArrayDescr1_Proxy *array_descriptor1_proxy(const PyObject *ptr) { + return reinterpret_cast(ptr); +} + +inline const PyArrayDescr2_Proxy *array_descriptor2_proxy(const PyObject *ptr) { + return reinterpret_cast(ptr); +} + inline bool check_flags(const void *ptr, int flag) { return (flag == (array_proxy(ptr)->flags & flag)); } @@ -350,7 +446,7 @@ struct array_info> { } static constexpr auto extents = const_name::is_array>( - concat(const_name(), array_info::extents), const_name()); + ::pybind11::detail::concat(const_name(), array_info::extents), const_name()); }; // For numpy we have special handling for arrays of characters, so we don't include // the size in the array extents. @@ -589,10 +685,32 @@ public: } /// Size of the data type in bytes. +#ifdef PYBIND11_NUMPY_1_ONLY ssize_t itemsize() const { return detail::array_descriptor_proxy(m_ptr)->elsize; } +#else + ssize_t itemsize() const { + if (detail::npy_api::get().PyArray_RUNTIME_VERSION_ < 0x12) { + return detail::array_descriptor1_proxy(m_ptr)->elsize; + } + return detail::array_descriptor2_proxy(m_ptr)->elsize; + } +#endif /// Returns true for structured data types. +#ifdef PYBIND11_NUMPY_1_ONLY bool has_fields() const { return detail::array_descriptor_proxy(m_ptr)->names != nullptr; } +#else + bool has_fields() const { + if (detail::npy_api::get().PyArray_RUNTIME_VERSION_ < 0x12) { + return detail::array_descriptor1_proxy(m_ptr)->names != nullptr; + } + const auto *proxy = detail::array_descriptor2_proxy(m_ptr); + if (proxy->type_num < 0 || proxy->type_num >= 2056) { + return false; + } + return proxy->names != nullptr; + } +#endif /// Single-character code for dtype's kind. /// For example, floating point types are 'f' and integral types are 'i'. @@ -618,20 +736,39 @@ public: /// Single character for byteorder char byteorder() const { return detail::array_descriptor_proxy(m_ptr)->byteorder; } - /// Alignment of the data type +/// Alignment of the data type +#ifdef PYBIND11_NUMPY_1_ONLY int alignment() const { return detail::array_descriptor_proxy(m_ptr)->alignment; } +#else + ssize_t alignment() const { + if (detail::npy_api::get().PyArray_RUNTIME_VERSION_ < 0x12) { + return detail::array_descriptor1_proxy(m_ptr)->alignment; + } + return detail::array_descriptor2_proxy(m_ptr)->alignment; + } +#endif - /// Flags for the array descriptor +/// Flags for the array descriptor +#ifdef PYBIND11_NUMPY_1_ONLY char flags() const { return detail::array_descriptor_proxy(m_ptr)->flags; } +#else + std::uint64_t flags() const { + if (detail::npy_api::get().PyArray_RUNTIME_VERSION_ < 0x12) { + return (unsigned char) detail::array_descriptor1_proxy(m_ptr)->flags; + } + return detail::array_descriptor2_proxy(m_ptr)->flags; + } +#endif private: - static object _dtype_from_pep3118() { - static PyObject *obj = module_::import("numpy.core._internal") - .attr("_dtype_from_pep3118") - .cast() - .release() - .ptr(); - return reinterpret_borrow(obj); + static object &_dtype_from_pep3118() { + PYBIND11_CONSTINIT static gil_safe_call_once_and_store storage; + return storage + .call_once_and_store_result([]() { + return detail::import_numpy_core_submodule("_internal") + .attr("_dtype_from_pep3118"); + }) + .get_stored(); } dtype strip_padding(ssize_t itemsize) { @@ -788,9 +925,7 @@ public: } /// Byte size of a single element - ssize_t itemsize() const { - return detail::array_descriptor_proxy(detail::array_proxy(m_ptr)->descr)->elsize; - } + ssize_t itemsize() const { return dtype().itemsize(); } /// Total number of bytes ssize_t nbytes() const { return size() * itemsize(); } @@ -1008,7 +1143,7 @@ protected: /// Create array from any object -- always returns a new reference static PyObject *raw_array(PyObject *ptr, int ExtraFlags = 0) { if (ptr == nullptr) { - PyErr_SetString(PyExc_ValueError, "cannot create a pybind11::array from a nullptr"); + set_error(PyExc_ValueError, "cannot create a pybind11::array from a nullptr"); return nullptr; } return detail::npy_api::get().PyArray_FromAny_( @@ -1155,7 +1290,7 @@ protected: /// Create array from any object -- always returns a new reference static PyObject *raw_array_t(PyObject *ptr) { if (ptr == nullptr) { - PyErr_SetString(PyExc_ValueError, "cannot create a pybind11::array_t from a nullptr"); + set_error(PyExc_ValueError, "cannot create a pybind11::array_t from a nullptr"); return nullptr; } return detail::npy_api::get().PyArray_FromAny_(ptr, @@ -1418,7 +1553,9 @@ PYBIND11_NOINLINE void register_structured_dtype(any_container auto tindex = std::type_index(tinfo); numpy_internals.registered_dtypes[tindex] = {dtype_ptr, std::move(format_str)}; - get_internals().direct_conversions[tindex].push_back(direct_converter); + with_internals([tindex, &direct_converter](internals &internals) { + internals.direct_conversions[tindex].push_back(direct_converter); + }); } template diff --git a/pybind11/include/pybind11/pybind11.h b/pybind11/include/pybind11/pybind11.h index 3bce1a01b..74919a7d5 100644 --- a/pybind11/include/pybind11/pybind11.h +++ b/pybind11/include/pybind11/pybind11.h @@ -14,7 +14,9 @@ #include "detail/init.h" #include "attr.h" #include "gil.h" +#include "gil_safe_call_once.h" #include "options.h" +#include "typing.h" #include #include @@ -52,6 +54,47 @@ PYBIND11_WARNING_DISABLE_MSVC(4127) PYBIND11_NAMESPACE_BEGIN(detail) +inline std::string replace_newlines_and_squash(const char *text) { + const char *whitespaces = " \t\n\r\f\v"; + std::string result(text); + bool previous_is_whitespace = false; + + if (result.size() >= 2) { + // Do not modify string representations + char first_char = result[0]; + char last_char = result[result.size() - 1]; + if (first_char == last_char && first_char == '\'') { + return result; + } + } + result.clear(); + + // Replace characters in whitespaces array with spaces and squash consecutive spaces + while (*text != '\0') { + if (std::strchr(whitespaces, *text)) { + if (!previous_is_whitespace) { + result += ' '; + previous_is_whitespace = true; + } + } else { + result += *text; + previous_is_whitespace = false; + } + ++text; + } + + // Strip leading and trailing whitespaces + const size_t str_begin = result.find_first_not_of(whitespaces); + if (str_begin == std::string::npos) { + return ""; + } + + const size_t str_end = result.find_last_not_of(whitespaces); + const size_t str_range = str_end - str_begin + 1; + + return result.substr(str_begin, str_range); +} + // Apply all the extensions translators from a list // Return true if one of the translators completed without raising an exception // itself. Return of false indicates that if there are other translators @@ -424,7 +467,7 @@ protected: // Write default value if available. if (!is_starred && arg_index < rec->args.size() && rec->args[arg_index].descr) { signature += " = "; - signature += rec->args[arg_index].descr; + signature += detail::replace_newlines_and_squash(rec->args[arg_index].descr); } // Separator for positional-only arguments (placed after the // argument, rather than before like * @@ -449,9 +492,7 @@ protected: signature += rec->scope.attr("__module__").cast() + "." + rec->scope.attr("__qualname__").cast(); } else { - std::string tname(t->name()); - detail::clean_type_id(tname); - signature += tname; + signature += detail::quote_cpp_type_name(detail::clean_type_id(t->name())); } } else { signature += c; @@ -680,7 +721,7 @@ protected: /* Iterator over the list of potentially admissible overloads */ const function_record *overloads = reinterpret_cast( PyCapsule_GetPointer(self, get_function_record_capsule_name())), - *it = overloads; + *current_overload = overloads; assert(overloads != nullptr); /* Need to know how many arguments + keyword arguments there are to pick the right @@ -694,9 +735,8 @@ protected: if (overloads->is_constructor) { if (!parent || !PyObject_TypeCheck(parent.ptr(), (PyTypeObject *) overloads->scope.ptr())) { - PyErr_SetString( - PyExc_TypeError, - "__init__(self, ...) called with invalid or missing `self` argument"); + set_error(PyExc_TypeError, + "__init__(self, ...) called with invalid or missing `self` argument"); return nullptr; } @@ -719,9 +759,10 @@ protected: std::vector second_pass; // However, if there are no overloads, we can just skip the no-convert pass entirely - const bool overloaded = it != nullptr && it->next != nullptr; + const bool overloaded + = current_overload != nullptr && current_overload->next != nullptr; - for (; it != nullptr; it = it->next) { + for (; current_overload != nullptr; current_overload = current_overload->next) { /* For each overload: 1. Copy all positional arguments we were given, also checking to make sure that @@ -742,7 +783,7 @@ protected: a result other than PYBIND11_TRY_NEXT_OVERLOAD. */ - const function_record &func = *it; + const function_record &func = *current_overload; size_t num_args = func.nargs; // Number of positional arguments that we need if (func.has_args) { --num_args; // (but don't count py::args @@ -980,10 +1021,10 @@ protected: } if (result.ptr() != PYBIND11_TRY_NEXT_OVERLOAD) { - // The error reporting logic below expects 'it' to be valid, as it would be - // if we'd encountered this failure in the first-pass loop. + // The error reporting logic below expects 'current_overload' to be valid, + // as it would be if we'd encountered this failure in the first-pass loop. if (!result) { - it = &call.func; + current_overload = &call.func; } break; } @@ -1007,24 +1048,30 @@ protected: A translator may choose to do one of the following: - - catch the exception and call PyErr_SetString or PyErr_SetObject + - catch the exception and call py::set_error() to set a standard (or custom) Python exception, or - do nothing and let the exception fall through to the next translator, or - delegate translation to the next translator by throwing a new type of exception. */ - auto &local_exception_translators - = get_local_internals().registered_exception_translators; - if (detail::apply_exception_translators(local_exception_translators)) { - return nullptr; - } - auto &exception_translators = get_internals().registered_exception_translators; - if (detail::apply_exception_translators(exception_translators)) { + bool handled = with_internals([&](internals &internals) { + auto &local_exception_translators + = get_local_internals().registered_exception_translators; + if (detail::apply_exception_translators(local_exception_translators)) { + return true; + } + auto &exception_translators = internals.registered_exception_translators; + if (detail::apply_exception_translators(exception_translators)) { + return true; + } + return false; + }); + + if (handled) { return nullptr; } - PyErr_SetString(PyExc_SystemError, - "Exception escaped from default exception translator!"); + set_error(PyExc_SystemError, "Exception escaped from default exception translator!"); return nullptr; } @@ -1102,7 +1149,7 @@ protected: } msg += "kwargs: "; bool first = true; - for (auto kwarg : kwargs) { + for (const auto &kwarg : kwargs) { if (first) { first = false; } else { @@ -1125,20 +1172,21 @@ protected: raise_from(PyExc_TypeError, msg.c_str()); return nullptr; } - PyErr_SetString(PyExc_TypeError, msg.c_str()); + set_error(PyExc_TypeError, msg.c_str()); return nullptr; } if (!result) { std::string msg = "Unable to convert function return value to a " "Python type! The signature was\n\t"; - msg += it->signature; + assert(current_overload != nullptr); + msg += current_overload->signature; append_note_if_missing_header_is_suspected(msg); // Attach additional error info to the exception if supported if (PyErr_Occurred()) { raise_from(PyExc_TypeError, msg.c_str()); return nullptr; } - PyErr_SetString(PyExc_TypeError, msg.c_str()); + set_error(PyExc_TypeError, msg.c_str()); return nullptr; } if (overloads->is_constructor && !self_value_and_holder.holder_constructed()) { @@ -1149,6 +1197,25 @@ protected: } }; +PYBIND11_NAMESPACE_BEGIN(detail) + +template <> +struct handle_type_name { + static constexpr auto name = const_name("Callable"); +}; + +PYBIND11_NAMESPACE_END(detail) + +// Use to activate Py_MOD_GIL_NOT_USED. +class mod_gil_not_used { +public: + explicit mod_gil_not_used(bool flag = true) : flag_(flag) {} + bool flag() const { return flag_; } + +private: + bool flag_; +}; + /// Wrapper for Python extension modules class module_ : public object { public: @@ -1249,7 +1316,11 @@ public: ``def`` should point to a statically allocated module_def. \endrst */ - static module_ create_extension_module(const char *name, const char *doc, module_def *def) { + static module_ create_extension_module(const char *name, + const char *doc, + module_def *def, + mod_gil_not_used gil_not_used + = mod_gil_not_used(false)) { // module_def is PyModuleDef // Placement new (not an allocation). def = new (def) @@ -1269,6 +1340,11 @@ public: } pybind11_fail("Internal error in module_::create_extension_module()"); } + if (gil_not_used.flag()) { +#ifdef Py_GIL_DISABLED + PyUnstable_Module_SetGIL(m, Py_MOD_GIL_NOT_USED); +#endif + } // TODO: Should be reinterpret_steal for Python 3, but Python also steals it again when // returned from PyInit_... // For Python 2, reinterpret_borrow was correct. @@ -1276,6 +1352,15 @@ public: } }; +PYBIND11_NAMESPACE_BEGIN(detail) + +template <> +struct handle_type_name { + static constexpr auto name = const_name("module"); +}; + +PYBIND11_NAMESPACE_END(detail) + // When inside a namespace (or anywhere as long as it's not the first item on a line), // C++20 allows "module" to be used. This is provided for backward compatibility, and for // simplicity, if someone wants to use py::module for example, that is perfectly safe. @@ -1285,8 +1370,14 @@ using module = module_; /// Return a dictionary representing the global variables in the current execution frame, /// or ``__main__.__dict__`` if there is no frame (usually when the interpreter is embedded). inline dict globals() { +#if PY_VERSION_HEX >= 0x030d0000 + PyObject *p = PyEval_GetFrameGlobals(); + return p ? reinterpret_steal(p) + : reinterpret_borrow(module_::import("__main__").attr("__dict__").ptr()); +#else PyObject *p = PyEval_GetGlobals(); return reinterpret_borrow(p ? p : module_::import("__main__").attr("__dict__").ptr()); +#endif } template ()>> @@ -1332,15 +1423,16 @@ protected: tinfo->default_holder = rec.default_holder; tinfo->module_local = rec.module_local; - auto &internals = get_internals(); - auto tindex = std::type_index(*rec.type); - tinfo->direct_conversions = &internals.direct_conversions[tindex]; - if (rec.module_local) { - get_local_internals().registered_types_cpp[tindex] = tinfo; - } else { - internals.registered_types_cpp[tindex] = tinfo; - } - internals.registered_types_py[(PyTypeObject *) m_ptr] = {tinfo}; + with_internals([&](internals &internals) { + auto tindex = std::type_index(*rec.type); + tinfo->direct_conversions = &internals.direct_conversions[tindex]; + if (rec.module_local) { + get_local_internals().registered_types_cpp[tindex] = tinfo; + } else { + internals.registered_types_cpp[tindex] = tinfo; + } + internals.registered_types_py[(PyTypeObject *) m_ptr] = {tinfo}; + }); if (rec.bases.size() > 1 || rec.multiple_inheritance) { mark_parents_nonsimple(tinfo->type); @@ -1553,10 +1645,12 @@ public: generic_type::initialize(record); if (has_alias) { - auto &instances = record.module_local ? get_local_internals().registered_types_cpp - : get_internals().registered_types_cpp; - instances[std::type_index(typeid(type_alias))] - = instances[std::type_index(typeid(type))]; + with_internals([&](internals &internals) { + auto &instances = record.module_local ? get_local_internals().registered_types_cpp + : internals.registered_types_cpp; + instances[std::type_index(typeid(type_alias))] + = instances[std::type_index(typeid(type))]; + }); } } @@ -1977,7 +2071,7 @@ struct enum_base { object type_name = type::handle_of(arg).attr("__name__"); return pybind11::str("{}.{}").format(std::move(type_name), enum_name(arg)); }, - name("name"), + name("__str__"), is_method(m_base)); if (options::show_enum_members_docstring()) { @@ -2271,28 +2365,32 @@ keep_alive_impl(size_t Nurse, size_t Patient, function_call &call, handle ret) { inline std::pair all_type_info_get_cache(PyTypeObject *type) { - auto res = get_internals() - .registered_types_py + auto res = with_internals([type](internals &internals) { + return internals + .registered_types_py #ifdef __cpp_lib_unordered_map_try_emplace - .try_emplace(type); + .try_emplace(type); #else - .emplace(type, std::vector()); + .emplace(type, std::vector()); #endif + }); if (res.second) { // New cache entry created; set up a weak reference to automatically remove it if the type // gets destroyed: weakref((PyObject *) type, cpp_function([type](handle wr) { - get_internals().registered_types_py.erase(type); + with_internals([type](internals &internals) { + internals.registered_types_py.erase(type); - // TODO consolidate the erasure code in pybind11_meta_dealloc() in class.h - auto &cache = get_internals().inactive_override_cache; - for (auto it = cache.begin(), last = cache.end(); it != last;) { - if (it->first == reinterpret_cast(type)) { - it = cache.erase(it); - } else { - ++it; + // TODO consolidate the erasure code in pybind11_meta_dealloc() in class.h + auto &cache = internals.inactive_override_cache; + for (auto it = cache.begin(), last = cache.end(); it != last;) { + if (it->first == reinterpret_cast(type)) { + it = cache.erase(it); + } else { + ++it; + } } - } + }); wr.dec_ref(); })) @@ -2395,7 +2493,7 @@ iterator make_iterator_impl(Iterator first, Sentinel last, Extra &&...extra) { Policy); } - return cast(state{first, last, true}); + return cast(state{std::forward(first), std::forward(last), true}); } PYBIND11_NAMESPACE_END(detail) @@ -2406,13 +2504,15 @@ template ::result_type, typename... Extra> -iterator make_iterator(Iterator first, Sentinel last, Extra &&...extra) { +typing::Iterator make_iterator(Iterator first, Sentinel last, Extra &&...extra) { return detail::make_iterator_impl, Policy, Iterator, Sentinel, ValueType, - Extra...>(first, last, std::forward(extra)...); + Extra...>(std::forward(first), + std::forward(last), + std::forward(extra)...); } /// Makes a python iterator over the keys (`.first`) of a iterator over pairs from a @@ -2422,13 +2522,15 @@ template ::result_type, typename... Extra> -iterator make_key_iterator(Iterator first, Sentinel last, Extra &&...extra) { +typing::Iterator make_key_iterator(Iterator first, Sentinel last, Extra &&...extra) { return detail::make_iterator_impl, Policy, Iterator, Sentinel, KeyType, - Extra...>(first, last, std::forward(extra)...); + Extra...>(std::forward(first), + std::forward(last), + std::forward(extra)...); } /// Makes a python iterator over the values (`.second`) of a iterator over pairs from a @@ -2438,21 +2540,25 @@ template ::result_type, typename... Extra> -iterator make_value_iterator(Iterator first, Sentinel last, Extra &&...extra) { +typing::Iterator make_value_iterator(Iterator first, Sentinel last, Extra &&...extra) { return detail::make_iterator_impl, Policy, Iterator, Sentinel, ValueType, - Extra...>(first, last, std::forward(extra)...); + Extra...>(std::forward(first), + std::forward(last), + std::forward(extra)...); } /// Makes an iterator over values of an stl container or other container supporting /// `std::begin()`/`std::end()` template ()))>::result_type, typename... Extra> -iterator make_iterator(Type &value, Extra &&...extra) { +typing::Iterator make_iterator(Type &value, Extra &&...extra) { return make_iterator( std::begin(value), std::end(value), std::forward(extra)...); } @@ -2461,8 +2567,10 @@ iterator make_iterator(Type &value, Extra &&...extra) { /// `std::begin()`/`std::end()` template ()))>::result_type, typename... Extra> -iterator make_key_iterator(Type &value, Extra &&...extra) { +typing::Iterator make_key_iterator(Type &value, Extra &&...extra) { return make_key_iterator( std::begin(value), std::end(value), std::forward(extra)...); } @@ -2471,8 +2579,10 @@ iterator make_key_iterator(Type &value, Extra &&...extra) { /// `std::begin()`/`std::end()` template ()))>::result_type, typename... Extra> -iterator make_value_iterator(Type &value, Extra &&...extra) { +typing::Iterator make_value_iterator(Type &value, Extra &&...extra) { return make_value_iterator( std::begin(value), std::end(value), std::forward(extra)...); } @@ -2485,7 +2595,11 @@ void implicitly_convertible() { ~set_flag() { flag = false; } }; auto implicit_caster = [](PyObject *obj, PyTypeObject *type) -> PyObject * { +#ifdef Py_GIL_DISABLED + thread_local bool currently_used = false; +#else static bool currently_used = false; +#endif if (currently_used) { // implicit conversions are non-reentrant return nullptr; } @@ -2510,8 +2624,10 @@ void implicitly_convertible() { } inline void register_exception_translator(ExceptionTranslator &&translator) { - detail::get_internals().registered_exception_translators.push_front( - std::forward(translator)); + detail::with_internals([&](detail::internals &internals) { + internals.registered_exception_translators.push_front( + std::forward(translator)); + }); } /** @@ -2521,14 +2637,17 @@ inline void register_exception_translator(ExceptionTranslator &&translator) { * the exception. */ inline void register_local_exception_translator(ExceptionTranslator &&translator) { - detail::get_local_internals().registered_exception_translators.push_front( - std::forward(translator)); + detail::with_internals([&](detail::internals &internals) { + (void) internals; + detail::get_local_internals().registered_exception_translators.push_front( + std::forward(translator)); + }); } /** * Wrapper to generate a new Python exception type. * - * This should only be used with PyErr_SetString for now. + * This should only be used with py::set_error() for now. * It is not (yet) possible to use as a py::base. * Template type argument is reserved for future use. */ @@ -2549,27 +2668,25 @@ public: } // Sets the current python exception to this exception object with the given message - void operator()(const char *message) { PyErr_SetString(m_ptr, message); } + PYBIND11_DEPRECATED("Please use py::set_error() instead " + "(https://github.com/pybind/pybind11/pull/4772)") + void operator()(const char *message) const { set_error(*this, message); } }; PYBIND11_NAMESPACE_BEGIN(detail) -// Returns a reference to a function-local static exception object used in the simple -// register_exception approach below. (It would be simpler to have the static local variable -// directly in register_exception, but that makes clang <3.5 segfault - issue #1349). -template -exception &get_exception_object() { - static exception ex; - return ex; -} + +template <> +struct handle_type_name> { + static constexpr auto name = const_name("Exception"); +}; // Helper function for register_exception and register_local_exception template exception & register_exception_impl(handle scope, const char *name, handle base, bool isLocal) { - auto &ex = detail::get_exception_object(); - if (!ex) { - ex = exception(scope, name, base); - } + PYBIND11_CONSTINIT static gil_safe_call_once_and_store> exc_storage; + exc_storage.call_once_and_store_result( + [&]() { return exception(scope, name, base); }); auto register_func = isLocal ? ®ister_local_exception_translator : ®ister_exception_translator; @@ -2581,10 +2698,10 @@ register_exception_impl(handle scope, const char *name, handle base, bool isLoca try { std::rethrow_exception(p); } catch (const CppException &e) { - detail::get_exception_object()(e.what()); + set_error(exc_storage.get_stored(), e.what()); } }); - return ex; + return exc_storage.get_stored(); } PYBIND11_NAMESPACE_END(detail) @@ -2681,14 +2798,19 @@ get_type_override(const void *this_ptr, const type_info *this_type, const char * /* Cache functions that aren't overridden in Python to avoid many costly Python dictionary lookups below */ - auto &cache = get_internals().inactive_override_cache; - if (cache.find(key) != cache.end()) { + bool not_overridden = with_internals([&key](internals &internals) { + auto &cache = internals.inactive_override_cache; + return cache.find(key) != cache.end(); + }); + if (not_overridden) { return function(); } function override = getattr(self, name, function()); if (override.is_cpp_function()) { - cache.insert(std::move(key)); + with_internals([&](internals &internals) { + internals.inactive_override_cache.insert(std::move(key)); + }); return function(); } @@ -2701,12 +2823,22 @@ get_type_override(const void *this_ptr, const type_info *this_type, const char * PyCodeObject *f_code = PyFrame_GetCode(frame); // f_code is guaranteed to not be NULL if ((std::string) str(f_code->co_name) == name && f_code->co_argcount > 0) { +# if PY_VERSION_HEX >= 0x030d0000 + PyObject *locals = PyEval_GetFrameLocals(); +# else PyObject *locals = PyEval_GetLocals(); + Py_XINCREF(locals); +# endif if (locals != nullptr) { +# if PY_VERSION_HEX >= 0x030b0000 + PyObject *co_varnames = PyCode_GetVarnames(f_code); +# else PyObject *co_varnames = PyObject_GetAttrString((PyObject *) f_code, "co_varnames"); +# endif PyObject *self_arg = PyTuple_GET_ITEM(co_varnames, 0); Py_DECREF(co_varnames); PyObject *self_caller = dict_getitem(locals, self_arg); + Py_DECREF(locals); if (self_caller == self.ptr()) { Py_DECREF(f_code); Py_DECREF(frame); @@ -2783,10 +2915,14 @@ function get_override(const T *this_ptr, const char *name) { = pybind11::get_override(static_cast(this), name); \ if (override) { \ auto o = override(__VA_ARGS__); \ - if (pybind11::detail::cast_is_temporary_value_reference::value) { \ + PYBIND11_WARNING_PUSH \ + PYBIND11_WARNING_DISABLE_MSVC(4127) \ + if (pybind11::detail::cast_is_temporary_value_reference::value \ + && !pybind11::detail::is_same_ignoring_cvref::value) { \ static pybind11::detail::override_caster_t caster; \ return pybind11::detail::cast_ref(std::move(o), caster); \ } \ + PYBIND11_WARNING_POP \ return pybind11::detail::cast_safe(std::move(o)); \ } \ } while (false) diff --git a/pybind11/include/pybind11/pytypes.h b/pybind11/include/pybind11/pytypes.h index 64aad6347..f26c307a8 100644 --- a/pybind11/include/pybind11/pytypes.h +++ b/pybind11/include/pybind11/pytypes.h @@ -59,6 +59,7 @@ struct sequence_item; struct list_item; struct tuple_item; } // namespace accessor_policies +// PLEASE KEEP handle_type_name SPECIALIZATIONS IN SYNC. using obj_attr_accessor = accessor; using str_attr_accessor = accessor; using item_accessor = accessor; @@ -182,7 +183,15 @@ public: str_attr_accessor doc() const; /// Return the object's current reference count - int ref_count() const { return static_cast(Py_REFCNT(derived().ptr())); } + ssize_t ref_count() const { +#ifdef PYPY_VERSION + // PyPy uses the top few bits for REFCNT_FROM_PYPY & REFCNT_FROM_PYPY_LIGHT + // Following pybind11 2.12.1 and older behavior and removing this part + return static_cast(static_cast(Py_REFCNT(derived().ptr()))); +#else + return Py_REFCNT(derived().ptr()); +#endif + } // TODO PYBIND11_DEPRECATED( // "Call py::type::handle_of(h) or py::type::of(h) instead of h.get_type()") @@ -305,19 +314,19 @@ private: "https://pybind11.readthedocs.io/en/stable/advanced/" "misc.html#common-sources-of-global-interpreter-lock-errors for debugging advice.\n" "If you are convinced there is no bug in your code, you can #define " - "PYBIND11_NO_ASSERT_GIL_HELD_INCREF_DECREF" + "PYBIND11_NO_ASSERT_GIL_HELD_INCREF_DECREF " "to disable this check. In that case you have to ensure this #define is consistently " "used for all translation units linked into a given pybind11 extension, otherwise " "there will be ODR violations.", function_name.c_str()); - fflush(stderr); if (Py_TYPE(m_ptr)->tp_name != nullptr) { fprintf(stderr, - "The failing %s call was triggered on a %s object.\n", + " The failing %s call was triggered on a %s object.", function_name.c_str(), Py_TYPE(m_ptr)->tp_name); - fflush(stderr); } + fprintf(stderr, "\n"); + fflush(stderr); throw std::runtime_error(function_name + " PyGILState_Check() failure."); } #endif @@ -334,6 +343,14 @@ public: #endif }; +inline void set_error(const handle &type, const char *message) { + PyErr_SetString(type.ptr(), message); +} + +inline void set_error(const handle &type, const handle &value) { + PyErr_SetObject(type.ptr(), value.ptr()); +} + /** \rst Holds a reference to a Python object (with reference counting) @@ -963,6 +980,23 @@ inline PyObject *dict_getitem(PyObject *v, PyObject *key) { return rv; } +inline PyObject *dict_getitemstringref(PyObject *v, const char *key) { +#if PY_VERSION_HEX >= 0x030D0000 + PyObject *rv; + if (PyDict_GetItemStringRef(v, key, &rv) < 0) { + throw error_already_set(); + } + return rv; +#else + PyObject *rv = dict_getitemstring(v, key); + if (rv == nullptr && PyErr_Occurred()) { + throw error_already_set(); + } + Py_XINCREF(rv); + return rv; +#endif +} + // Helper aliases/functions to support implicit casting of values given to python // accessors/methods. When given a pyobject, this simply returns the pyobject as-is; for other C++ // type, the value goes through pybind11::cast(obj) to convert it to an `object`. @@ -1612,7 +1646,15 @@ inline namespace literals { /** \rst String literal version of `str` \endrst */ -inline str operator"" _s(const char *s, size_t size) { return {s, size}; } +inline str +#if !defined(__clang__) && defined(__GNUC__) && __GNUC__ < 5 +operator"" _s // gcc 4.8.5 insists on having a space (hard error). +#else +operator""_s // clang 17 generates a deprecation warning if there is a space. +#endif + (const char *s, size_t size) { + return {s, size}; +} } // namespace literals /// \addtogroup pytypes @@ -2158,6 +2200,11 @@ public: throw error_already_set(); } } + void clear() /* py-non-const */ { + if (PyList_SetSlice(m_ptr, 0, PyList_Size(m_ptr), nullptr) == -1) { + throw error_already_set(); + } + } }; class args : public tuple { diff --git a/pybind11/include/pybind11/stl.h b/pybind11/include/pybind11/stl.h index f39f44f7c..71bc5902e 100644 --- a/pybind11/include/pybind11/stl.h +++ b/pybind11/include/pybind11/stl.h @@ -100,7 +100,7 @@ public: return s.release(); } - PYBIND11_TYPE_CASTER(type, const_name("Set[") + key_conv::name + const_name("]")); + PYBIND11_TYPE_CASTER(type, const_name("set[") + key_conv::name + const_name("]")); }; template @@ -157,7 +157,7 @@ public: } PYBIND11_TYPE_CASTER(Type, - const_name("Dict[") + key_conv::name + const_name(", ") + value_conv::name + const_name("dict[") + key_conv::name + const_name(", ") + value_conv::name + const_name("]")); }; @@ -172,7 +172,7 @@ struct list_caster { auto s = reinterpret_borrow(src); value.clear(); reserve_maybe(s, &value); - for (auto it : s) { + for (const auto &it : s) { value_conv conv; if (!conv.load(it, convert)) { return false; @@ -208,7 +208,7 @@ public: return l.release(); } - PYBIND11_TYPE_CASTER(Type, const_name("List[") + value_conv::name + const_name("]")); + PYBIND11_TYPE_CASTER(Type, const_name("list[") + value_conv::name + const_name("]")); }; template @@ -247,7 +247,7 @@ public: return false; } size_t ctr = 0; - for (auto it : l) { + for (const auto &it : l) { value_conv conv; if (!conv.load(it, convert)) { return false; @@ -274,7 +274,7 @@ public: PYBIND11_TYPE_CASTER(ArrayType, const_name(const_name(""), const_name("Annotated[")) - + const_name("List[") + value_conv::name + const_name("]") + + const_name("list[") + value_conv::name + const_name("]") + const_name(const_name(""), const_name(", FixedSize(") + const_name() + const_name(")]"))); @@ -421,7 +421,8 @@ struct variant_caster> { using Type = V; PYBIND11_TYPE_CASTER(Type, - const_name("Union[") + detail::concat(make_caster::name...) + const_name("Union[") + + ::pybind11::detail::concat(make_caster::name...) + const_name("]")); }; diff --git a/pybind11/include/pybind11/stl/filesystem.h b/pybind11/include/pybind11/stl/filesystem.h index e26f42177..85c131efe 100644 --- a/pybind11/include/pybind11/stl/filesystem.h +++ b/pybind11/include/pybind11/stl/filesystem.h @@ -14,8 +14,7 @@ #ifdef __has_include # if defined(PYBIND11_CPP17) -# if __has_include() && \ - PY_VERSION_HEX >= 0x03060000 +# if __has_include() # include # define PYBIND11_HAS_FILESYSTEM 1 # elif __has_include() diff --git a/pybind11/include/pybind11/stl_bind.h b/pybind11/include/pybind11/stl_bind.h index 49f1b7782..66c452ea7 100644 --- a/pybind11/include/pybind11/stl_bind.h +++ b/pybind11/include/pybind11/stl_bind.h @@ -158,8 +158,7 @@ void vector_modifiers( return v.release(); })); - cl.def( - "clear", [](Vector &v) { v.clear(); }, "Clear the contents"); + cl.def("clear", [](Vector &v) { v.clear(); }, "Clear the contents"); cl.def( "extend", @@ -525,7 +524,7 @@ class_ bind_vector(handle scope, std::string const &name, A [](const Vector &v) -> bool { return !v.empty(); }, "Check whether the list is nonempty"); - cl.def("__len__", &Vector::size); + cl.def("__len__", [](const Vector &vec) { return vec.size(); }); #if 0 // C++ style functions deprecated, leaving it here as an example @@ -645,49 +644,50 @@ auto map_if_insertion_operator(Class_ &cl, std::string const &name) "Return the canonical string representation of this map."); } -template struct keys_view { virtual size_t len() = 0; virtual iterator iter() = 0; - virtual bool contains(const KeyType &k) = 0; - virtual bool contains(const object &k) = 0; + virtual bool contains(const handle &k) = 0; virtual ~keys_view() = default; }; -template struct values_view { virtual size_t len() = 0; virtual iterator iter() = 0; virtual ~values_view() = default; }; -template struct items_view { virtual size_t len() = 0; virtual iterator iter() = 0; virtual ~items_view() = default; }; -template -struct KeysViewImpl : public KeysView { +template +struct KeysViewImpl : public detail::keys_view { explicit KeysViewImpl(Map &map) : map(map) {} size_t len() override { return map.size(); } iterator iter() override { return make_key_iterator(map.begin(), map.end()); } - bool contains(const typename Map::key_type &k) override { return map.find(k) != map.end(); } - bool contains(const object &) override { return false; } + bool contains(const handle &k) override { + try { + return map.find(k.template cast()) != map.end(); + } catch (const cast_error &) { + return false; + } + } Map ↦ }; -template -struct ValuesViewImpl : public ValuesView { +template +struct ValuesViewImpl : public detail::values_view { explicit ValuesViewImpl(Map &map) : map(map) {} size_t len() override { return map.size(); } iterator iter() override { return make_value_iterator(map.begin(), map.end()); } Map ↦ }; -template -struct ItemsViewImpl : public ItemsView { +template +struct ItemsViewImpl : public detail::items_view { explicit ItemsViewImpl(Map &map) : map(map) {} size_t len() override { return map.size(); } iterator iter() override { return make_iterator(map.begin(), map.end()); } @@ -700,11 +700,9 @@ template , typename... class_ bind_map(handle scope, const std::string &name, Args &&...args) { using KeyType = typename Map::key_type; using MappedType = typename Map::mapped_type; - using StrippedKeyType = detail::remove_cvref_t; - using StrippedMappedType = detail::remove_cvref_t; - using KeysView = detail::keys_view; - using ValuesView = detail::values_view; - using ItemsView = detail::items_view; + using KeysView = detail::keys_view; + using ValuesView = detail::values_view; + using ItemsView = detail::items_view; using Class_ = class_; // If either type is a non-module-local bound type then make the map binding non-local as well; @@ -718,39 +716,20 @@ class_ bind_map(handle scope, const std::string &name, Args && } Class_ cl(scope, name.c_str(), pybind11::module_local(local), std::forward(args)...); - static constexpr auto key_type_descr = detail::make_caster::name; - static constexpr auto mapped_type_descr = detail::make_caster::name; - std::string key_type_name(key_type_descr.text), mapped_type_name(mapped_type_descr.text); - // If key type isn't properly wrapped, fall back to C++ names - if (key_type_name == "%") { - key_type_name = detail::type_info_description(typeid(KeyType)); - } - // Similarly for value type: - if (mapped_type_name == "%") { - mapped_type_name = detail::type_info_description(typeid(MappedType)); - } - - // Wrap KeysView[KeyType] if it wasn't already wrapped + // Wrap KeysView if it wasn't already wrapped if (!detail::get_type_info(typeid(KeysView))) { - class_ keys_view( - scope, ("KeysView[" + key_type_name + "]").c_str(), pybind11::module_local(local)); + class_ keys_view(scope, "KeysView", pybind11::module_local(local)); keys_view.def("__len__", &KeysView::len); keys_view.def("__iter__", &KeysView::iter, keep_alive<0, 1>() /* Essential: keep view alive while iterator exists */ ); - keys_view.def("__contains__", - static_cast(&KeysView::contains)); - // Fallback for when the object is not of the key type - keys_view.def("__contains__", - static_cast(&KeysView::contains)); + keys_view.def("__contains__", &KeysView::contains); } // Similarly for ValuesView: if (!detail::get_type_info(typeid(ValuesView))) { - class_ values_view(scope, - ("ValuesView[" + mapped_type_name + "]").c_str(), - pybind11::module_local(local)); + class_ values_view(scope, "ValuesView", pybind11::module_local(local)); values_view.def("__len__", &ValuesView::len); values_view.def("__iter__", &ValuesView::iter, @@ -759,10 +738,7 @@ class_ bind_map(handle scope, const std::string &name, Args && } // Similarly for ItemsView: if (!detail::get_type_info(typeid(ItemsView))) { - class_ items_view( - scope, - ("ItemsView[" + key_type_name + ", ").append(mapped_type_name + "]").c_str(), - pybind11::module_local(local)); + class_ items_view(scope, "ItemsView", pybind11::module_local(local)); items_view.def("__len__", &ItemsView::len); items_view.def("__iter__", &ItemsView::iter, @@ -788,25 +764,19 @@ class_ bind_map(handle scope, const std::string &name, Args && cl.def( "keys", - [](Map &m) { - return std::unique_ptr(new detail::KeysViewImpl(m)); - }, + [](Map &m) { return std::unique_ptr(new detail::KeysViewImpl(m)); }, keep_alive<0, 1>() /* Essential: keep map alive while view exists */ ); cl.def( "values", - [](Map &m) { - return std::unique_ptr(new detail::ValuesViewImpl(m)); - }, + [](Map &m) { return std::unique_ptr(new detail::ValuesViewImpl(m)); }, keep_alive<0, 1>() /* Essential: keep map alive while view exists */ ); cl.def( "items", - [](Map &m) { - return std::unique_ptr(new detail::ItemsViewImpl(m)); - }, + [](Map &m) { return std::unique_ptr(new detail::ItemsViewImpl(m)); }, keep_alive<0, 1>() /* Essential: keep map alive while view exists */ ); @@ -843,7 +813,8 @@ class_ bind_map(handle scope, const std::string &name, Args && m.erase(it); }); - cl.def("__len__", &Map::size); + // Always use a lambda in case of `using` declaration + cl.def("__len__", [](const Map &m) { return m.size(); }); return cl; } diff --git a/pybind11/include/pybind11/typing.h b/pybind11/include/pybind11/typing.h new file mode 100644 index 000000000..1442cdc7f --- /dev/null +++ b/pybind11/include/pybind11/typing.h @@ -0,0 +1,239 @@ +/* + pybind11/typing.h: Convenience wrapper classes for basic Python types + with more explicit annotations. + + Copyright (c) 2023 Dustin Spicuzza + + All rights reserved. Use of this source code is governed by a + BSD-style license that can be found in the LICENSE file. +*/ + +#pragma once + +#include "detail/common.h" +#include "cast.h" +#include "pytypes.h" + +PYBIND11_NAMESPACE_BEGIN(PYBIND11_NAMESPACE) +PYBIND11_NAMESPACE_BEGIN(typing) + +/* + The following types can be used to direct pybind11-generated docstrings + to have have more explicit types (e.g., `list[str]` instead of `list`). + Just use these in place of existing types. + + There is no additional enforcement of types at runtime. +*/ + +template +class Tuple : public tuple { + using tuple::tuple; +}; + +template +class Dict : public dict { + using dict::dict; +}; + +template +class List : public list { + using list::list; +}; + +template +class Set : public set { + using set::set; +}; + +template +class Iterable : public iterable { + using iterable::iterable; +}; + +template +class Iterator : public iterator { + using iterator::iterator; +}; + +template +class Callable; + +template +class Callable : public function { + using function::function; +}; + +template +class Type : public type { + using type::type; +}; + +template +class Union : public object { + PYBIND11_OBJECT_DEFAULT(Union, object, PyObject_Type) + using object::object; +}; + +template +class Optional : public object { + PYBIND11_OBJECT_DEFAULT(Optional, object, PyObject_Type) + using object::object; +}; + +template +class TypeGuard : public bool_ { + using bool_::bool_; +}; + +template +class TypeIs : public bool_ { + using bool_::bool_; +}; + +class NoReturn : public none { + using none::none; +}; + +class Never : public none { + using none::none; +}; + +#if defined(__cpp_nontype_template_parameter_class) +template +struct StringLiteral { + constexpr StringLiteral(const char (&str)[N]) { std::copy_n(str, N, name); } + char name[N]; +}; + +template +class Literal : public object { + PYBIND11_OBJECT_DEFAULT(Literal, object, PyObject_Type) +}; + +// Example syntax for creating a TypeVar. +// typedef typing::TypeVar<"T"> TypeVarT; +template +class TypeVar : public object { + PYBIND11_OBJECT_DEFAULT(TypeVar, object, PyObject_Type) + using object::object; +}; +#endif + +PYBIND11_NAMESPACE_END(typing) + +PYBIND11_NAMESPACE_BEGIN(detail) + +template +struct handle_type_name> { + static constexpr auto name = const_name("tuple[") + + ::pybind11::detail::concat(make_caster::name...) + + const_name("]"); +}; + +template <> +struct handle_type_name> { + // PEP 484 specifies this syntax for an empty tuple + static constexpr auto name = const_name("tuple[()]"); +}; + +template +struct handle_type_name> { + // PEP 484 specifies this syntax for a variable-length tuple + static constexpr auto name + = const_name("tuple[") + make_caster::name + const_name(", ...]"); +}; + +template +struct handle_type_name> { + static constexpr auto name = const_name("dict[") + make_caster::name + const_name(", ") + + make_caster::name + const_name("]"); +}; + +template +struct handle_type_name> { + static constexpr auto name = const_name("list[") + make_caster::name + const_name("]"); +}; + +template +struct handle_type_name> { + static constexpr auto name = const_name("set[") + make_caster::name + const_name("]"); +}; + +template +struct handle_type_name> { + static constexpr auto name = const_name("Iterable[") + make_caster::name + const_name("]"); +}; + +template +struct handle_type_name> { + static constexpr auto name = const_name("Iterator[") + make_caster::name + const_name("]"); +}; + +template +struct handle_type_name> { + using retval_type = conditional_t::value, void_type, Return>; + static constexpr auto name + = const_name("Callable[[") + ::pybind11::detail::concat(make_caster::name...) + + const_name("], ") + make_caster::name + const_name("]"); +}; + +template +struct handle_type_name> { + // PEP 484 specifies this syntax for defining only return types of callables + using retval_type = conditional_t::value, void_type, Return>; + static constexpr auto name + = const_name("Callable[..., ") + make_caster::name + const_name("]"); +}; + +template +struct handle_type_name> { + static constexpr auto name = const_name("type[") + make_caster::name + const_name("]"); +}; + +template +struct handle_type_name> { + static constexpr auto name = const_name("Union[") + + ::pybind11::detail::concat(make_caster::name...) + + const_name("]"); +}; + +template +struct handle_type_name> { + static constexpr auto name = const_name("Optional[") + make_caster::name + const_name("]"); +}; + +template +struct handle_type_name> { + static constexpr auto name = const_name("TypeGuard[") + make_caster::name + const_name("]"); +}; + +template +struct handle_type_name> { + static constexpr auto name = const_name("TypeIs[") + make_caster::name + const_name("]"); +}; + +template <> +struct handle_type_name { + static constexpr auto name = const_name("NoReturn"); +}; + +template <> +struct handle_type_name { + static constexpr auto name = const_name("Never"); +}; + +#if defined(__cpp_nontype_template_parameter_class) +template +struct handle_type_name> { + static constexpr auto name = const_name("Literal[") + + pybind11::detail::concat(const_name(Literals.name)...) + + const_name("]"); +}; +template +struct handle_type_name> { + static constexpr auto name = const_name(StrLit.name); +}; +#endif + +PYBIND11_NAMESPACE_END(detail) +PYBIND11_NAMESPACE_END(PYBIND11_NAMESPACE) diff --git a/pybind11/noxfile.py b/pybind11/noxfile.py index 021ced245..e9a2fa8fe 100644 --- a/pybind11/noxfile.py +++ b/pybind11/noxfile.py @@ -1,24 +1,12 @@ -import os +from __future__ import annotations + +import argparse import nox -nox.needs_version = ">=2022.1.7" +nox.needs_version = ">=2024.3.2" nox.options.sessions = ["lint", "tests", "tests_packaging"] - -PYTHON_VERSIONS = [ - "3.6", - "3.7", - "3.8", - "3.9", - "3.10", - "3.11", - "pypy3.7", - "pypy3.8", - "pypy3.9", -] - -if os.environ.get("CI", None): - nox.options.error_on_missing_interpreters = True +nox.options.default_venv_backend = "uv|virtualenv" @nox.session(reuse_venv=True) @@ -30,7 +18,7 @@ def lint(session: nox.Session) -> None: session.run("pre-commit", "run", "-a", *session.posargs) -@nox.session(python=PYTHON_VERSIONS) +@nox.session def tests(session: nox.Session) -> None: """ Run the tests (requires a compiler). @@ -57,30 +45,42 @@ def tests_packaging(session: nox.Session) -> None: Run the packaging tests. """ - session.install("-r", "tests/requirements.txt", "--prefer-binary") + session.install("-r", "tests/requirements.txt", "pip") session.run("pytest", "tests/extra_python_package", *session.posargs) @nox.session(reuse_venv=True) def docs(session: nox.Session) -> None: """ - Build the docs. Pass "serve" to serve. + Build the docs. Pass --non-interactive to avoid serving. """ - session.install("-r", "docs/requirements.txt") + parser = argparse.ArgumentParser() + parser.add_argument( + "-b", dest="builder", default="html", help="Build target (default: html)" + ) + args, posargs = parser.parse_known_args(session.posargs) + serve = args.builder == "html" and session.interactive + + extra_installs = ["sphinx-autobuild"] if serve else [] + session.install("-r", "docs/requirements.txt", *extra_installs) session.chdir("docs") - if "pdf" in session.posargs: - session.run("sphinx-build", "-M", "latexpdf", ".", "_build") - return + shared_args = ( + "-n", # nitpicky mode + "-T", # full tracebacks + f"-b={args.builder}", + ".", + f"_build/{args.builder}", + *posargs, + ) - session.run("sphinx-build", "-M", "html", ".", "_build") - - if "serve" in session.posargs: - session.log("Launching docs at http://localhost:8000/ - use Ctrl-C to quit") - session.run("python", "-m", "http.server", "8000", "-d", "_build/html") - elif session.posargs: - session.error("Unsupported argument to docs") + if serve: + session.run( + "sphinx-autobuild", "--open-browser", "--ignore=.build", *shared_args + ) + else: + session.run("sphinx-build", "--keep-going", *shared_args) @nox.session(reuse_venv=True) diff --git a/pybind11/pybind11/__init__.py b/pybind11/pybind11/__init__.py index 7c10b3057..b14660cae 100644 --- a/pybind11/pybind11/__init__.py +++ b/pybind11/pybind11/__init__.py @@ -1,7 +1,9 @@ +from __future__ import annotations + import sys -if sys.version_info < (3, 6): # noqa: UP036 - msg = "pybind11 does not support Python < 3.6. 2.9 was the last release supporting Python 2.7 and 3.5." +if sys.version_info < (3, 7): # noqa: UP036 + msg = "pybind11 does not support Python < 3.7. v2.12 was the last release supporting Python 3.6." raise ImportError(msg) diff --git a/pybind11/pybind11/__main__.py b/pybind11/pybind11/__main__.py index 180665c23..b656ce6fe 100644 --- a/pybind11/pybind11/__main__.py +++ b/pybind11/pybind11/__main__.py @@ -1,4 +1,5 @@ # pylint: disable=missing-function-docstring +from __future__ import annotations import argparse import sys diff --git a/pybind11/pybind11/_version.py b/pybind11/pybind11/_version.py index 9280fa054..18b72c07c 100644 --- a/pybind11/pybind11/_version.py +++ b/pybind11/pybind11/_version.py @@ -1,12 +1,12 @@ -from typing import Union +from __future__ import annotations -def _to_int(s: str) -> Union[int, str]: +def _to_int(s: str) -> int | str: try: return int(s) except ValueError: return s -__version__ = "2.11.1" +__version__ = "2.13.1" version_info = tuple(_to_int(s) for s in __version__.split(".")) diff --git a/pybind11/pybind11/commands.py b/pybind11/pybind11/commands.py index b11690f46..d535b6cca 100644 --- a/pybind11/pybind11/commands.py +++ b/pybind11/pybind11/commands.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import os DIR = os.path.abspath(os.path.dirname(__file__)) diff --git a/pybind11/pybind11/setup_helpers.py b/pybind11/pybind11/setup_helpers.py index aeeee9dcf..ced506f8c 100644 --- a/pybind11/pybind11/setup_helpers.py +++ b/pybind11/pybind11/setup_helpers.py @@ -36,6 +36,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. # # If you copy this file in, you don't # need the .pyi file; it's just an interface file for static type checkers. +from __future__ import annotations import contextlib import os @@ -52,7 +53,6 @@ from pathlib import Path from typing import ( Any, Callable, - Dict, Iterable, Iterator, List, @@ -66,7 +66,9 @@ try: from setuptools import Extension as _Extension from setuptools.command.build_ext import build_ext as _build_ext except ImportError: - from distutils.command.build_ext import build_ext as _build_ext # type: ignore[assignment] + from distutils.command.build_ext import ( # type: ignore[assignment] + build_ext as _build_ext, + ) from distutils.extension import Extension as _Extension # type: ignore[assignment] import distutils.ccompiler @@ -111,10 +113,10 @@ class Pybind11Extension(_Extension): # flags are prepended, so that they can be further overridden, e.g. by # ``extra_compile_args=["-g"]``. - def _add_cflags(self, flags: List[str]) -> None: + def _add_cflags(self, flags: list[str]) -> None: self.extra_compile_args[:0] = flags - def _add_ldflags(self, flags: List[str]) -> None: + def _add_ldflags(self, flags: list[str]) -> None: self.extra_link_args[:0] = flags def __init__(self, *args: Any, **kwargs: Any) -> None: @@ -248,7 +250,7 @@ cpp_flag_cache = None @lru_cache() -def auto_cpp_level(compiler: Any) -> Union[str, int]: +def auto_cpp_level(compiler: Any) -> str | int: """ Return the max supported C++ std level (17, 14, or 11). Returns latest on Windows. """ @@ -286,8 +288,8 @@ class build_ext(_build_ext): # noqa: N801 def intree_extensions( - paths: Iterable[str], package_dir: Optional[Dict[str, str]] = None -) -> List[Pybind11Extension]: + paths: Iterable[str], package_dir: dict[str, str] | None = None +) -> list[Pybind11Extension]: """ Generate Pybind11Extensions from source files directly located in a Python source tree. @@ -351,7 +353,7 @@ CCompilerMethod = Callable[ distutils.ccompiler.CCompiler, List[str], Optional[str], - Optional[Union[Tuple[str], Tuple[str, Optional[str]]]], + Optional[List[Union[Tuple[str], Tuple[str, Optional[str]]]]], Optional[List[str]], bool, Optional[List[str]], @@ -407,7 +409,7 @@ class ParallelCompile: def __init__( self, - envvar: Optional[str] = None, + envvar: str | None = None, default: int = 0, max: int = 0, # pylint: disable=redefined-builtin needs_recompile: Callable[[str, str], bool] = no_recompile, @@ -416,7 +418,7 @@ class ParallelCompile: self.default = default self.max = max self.needs_recompile = needs_recompile - self._old: List[CCompilerMethod] = [] + self._old: list[CCompilerMethod] = [] def function(self) -> CCompilerMethod: """ @@ -425,14 +427,14 @@ class ParallelCompile: def compile_function( compiler: distutils.ccompiler.CCompiler, - sources: List[str], - output_dir: Optional[str] = None, - macros: Optional[Union[Tuple[str], Tuple[str, Optional[str]]]] = None, - include_dirs: Optional[List[str]] = None, + sources: list[str], + output_dir: str | None = None, + macros: list[tuple[str] | tuple[str, str | None]] | None = None, + include_dirs: list[str] | None = None, debug: bool = False, - extra_preargs: Optional[List[str]] = None, - extra_postargs: Optional[List[str]] = None, - depends: Optional[List[str]] = None, + extra_preargs: list[str] | None = None, + extra_postargs: list[str] | None = None, + depends: list[str] | None = None, ) -> Any: # These lines are directly from distutils.ccompiler.CCompiler macros, objects, extra_postargs, pp_opts, build = compiler._setup_compile( # type: ignore[attr-defined] diff --git a/pybind11/pyproject.toml b/pybind11/pyproject.toml index 59c15ea63..71e7f5617 100644 --- a/pybind11/pyproject.toml +++ b/pybind11/pyproject.toml @@ -19,9 +19,8 @@ ignore = [ [tool.mypy] files = ["pybind11"] -python_version = "3.6" +python_version = "3.8" strict = true -show_error_codes = true enable_error_code = ["ignore-without-code", "redundant-expr", "truthy-bool"] warn_unreachable = true @@ -30,20 +29,8 @@ module = ["ghapi.*"] ignore_missing_imports = true -[tool.pytest.ini_options] -minversion = "6.0" -addopts = ["-ra", "--showlocals", "--strict-markers", "--strict-config"] -xfail_strict = true -filterwarnings = ["error"] -log_cli_level = "info" -testpaths = [ - "tests", -] -timeout=300 - - [tool.pylint] -master.py-version = "3.6" +master.py-version = "3.7" reports.output-format = "colorized" messages_control.disable = [ "design", @@ -57,10 +44,12 @@ messages_control.disable = [ "unused-argument", # covered by Ruff ARG ] - [tool.ruff] -select = [ - "E", "F", "W", # flake8 +target-version = "py37" +src = ["src"] + +[tool.ruff.lint] +extend-select = [ "B", # flake8-bugbear "I", # isort "N", # pep8-naming @@ -68,7 +57,6 @@ select = [ "C4", # flake8-comprehensions "EM", # flake8-errmsg "ICN", # flake8-import-conventions - "ISC", # flake8-implicit-str-concat "PGH", # pygrep-hooks "PIE", # flake8-pie "PL", # pylint @@ -86,13 +74,14 @@ ignore = [ "PT004", # Fixture that doesn't return needs underscore (no, it is fine) "SIM118", # iter(x) is not always the same as iter(x.keys()) ] -target-version = "py37" -src = ["src"] unfixable = ["T20"] -exclude = [] -line-length = 120 isort.known-first-party = ["env", "pybind11_cross_module_tests", "pybind11_tests"] +isort.required-imports = ["from __future__ import annotations"] -[tool.ruff.per-file-ignores] -"tests/**" = ["EM", "N"] + +[tool.ruff.lint.per-file-ignores] +"tests/**" = ["EM", "N", "E721"] "tests/test_call_policies.py" = ["PLC1901"] + +[tool.repo-review] +ignore = ["PP"] diff --git a/pybind11/setup.cfg b/pybind11/setup.cfg index 92e6c953a..2beca780f 100644 --- a/pybind11/setup.cfg +++ b/pybind11/setup.cfg @@ -14,13 +14,13 @@ classifiers = Topic :: Utilities Programming Language :: C++ Programming Language :: Python :: 3 :: Only - Programming Language :: Python :: 3.6 Programming Language :: Python :: 3.7 Programming Language :: Python :: 3.8 Programming Language :: Python :: 3.9 Programming Language :: Python :: 3.10 Programming Language :: Python :: 3.11 Programming Language :: Python :: 3.12 + Programming Language :: Python :: 3.13 License :: OSI Approved :: BSD License Programming Language :: Python :: Implementation :: PyPy Programming Language :: Python :: Implementation :: CPython @@ -39,5 +39,5 @@ project_urls = Chat = https://gitter.im/pybind/Lobby [options] -python_requires = >=3.6 +python_requires = >=3.7 zip_safe = False diff --git a/pybind11/setup.py b/pybind11/setup.py index 9fea7d35c..96563c1a5 100644 --- a/pybind11/setup.py +++ b/pybind11/setup.py @@ -1,6 +1,7 @@ #!/usr/bin/env python3 # Setup script for PyPI; use CMakeFile.txt to build extension modules +from __future__ import annotations import contextlib import os @@ -9,9 +10,9 @@ import shutil import string import subprocess import sys +from collections.abc import Generator from pathlib import Path from tempfile import TemporaryDirectory -from typing import Dict, Iterator, List, Union import setuptools.command.sdist @@ -23,7 +24,7 @@ VERSION_FILE = Path("pybind11/_version.py") COMMON_FILE = Path("include/pybind11/detail/common.h") -def build_expected_version_hex(matches: Dict[str, str]) -> str: +def build_expected_version_hex(matches: dict[str, str]) -> str: patch_level_serial = matches["PATCH"] serial = None major = int(matches["MAJOR"]) @@ -64,7 +65,7 @@ to_src = ( # Read the listed version -loc: Dict[str, str] = {} +loc: dict[str, str] = {} code = compile(VERSION_FILE.read_text(encoding="utf-8"), "pybind11/_version.py", "exec") exec(code, loc) version = loc["__version__"] @@ -84,9 +85,7 @@ if version_hex != exp_version_hex: # TODO: use literals & overload (typing extensions or Python 3.8) -def get_and_replace( - filename: Path, binary: bool = False, **opts: str -) -> Union[bytes, str]: +def get_and_replace(filename: Path, binary: bool = False, **opts: str) -> bytes | str: if binary: contents = filename.read_bytes() return string.Template(contents.decode()).substitute(opts).encode() @@ -97,7 +96,7 @@ def get_and_replace( # Use our input files instead when making the SDist (and anything that depends # on it, like a wheel) class SDist(setuptools.command.sdist.sdist): - def make_release_tree(self, base_dir: str, files: List[str]) -> None: + def make_release_tree(self, base_dir: str, files: list[str]) -> None: super().make_release_tree(base_dir, files) for to, src in to_src: @@ -112,7 +111,7 @@ class SDist(setuptools.command.sdist.sdist): # Remove the CMake install directory when done @contextlib.contextmanager -def remove_output(*sources: str) -> Iterator[None]: +def remove_output(*sources: str) -> Generator[None, None, None]: try: yield finally: diff --git a/pybind11/tests/CMakeLists.txt b/pybind11/tests/CMakeLists.txt index 80ee9c1f1..f182e2499 100644 --- a/pybind11/tests/CMakeLists.txt +++ b/pybind11/tests/CMakeLists.txt @@ -7,13 +7,13 @@ cmake_minimum_required(VERSION 3.5) -# The `cmake_minimum_required(VERSION 3.5...3.26)` syntax does not work with +# The `cmake_minimum_required(VERSION 3.5...3.29)` syntax does not work with # some versions of VS that have a patched CMake 3.11. This forces us to emulate # the behavior using the following workaround: -if(${CMAKE_VERSION} VERSION_LESS 3.26) +if(${CMAKE_VERSION} VERSION_LESS 3.29) cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}) else() - cmake_policy(VERSION 3.26) + cmake_policy(VERSION 3.29) endif() # Filter out items; print an optional message if any items filtered. This ignores extensions. @@ -144,6 +144,7 @@ set(PYBIND11_TEST_FILES test_opaque_types test_operator_overloading test_pickling + test_python_multiple_inheritance test_pytypes test_sequences_and_iterators test_smart_ptr @@ -329,7 +330,7 @@ if(Boost_FOUND) add_library(Boost::headers IMPORTED INTERFACE) if(TARGET Boost::boost) # Classic FindBoost - set_property(TARGET Boost::boost PROPERTY INTERFACE_LINK_LIBRARIES Boost::boost) + set_property(TARGET Boost::headers PROPERTY INTERFACE_LINK_LIBRARIES Boost::boost) else() # Very old FindBoost, or newer Boost than CMake in older CMakes set_property(TARGET Boost::headers PROPERTY INTERFACE_INCLUDE_DIRECTORIES @@ -519,11 +520,15 @@ set(PYBIND11_TEST_PREFIX_COMMAND "" CACHE STRING "Put this before pytest, use for checkers and such") +set(PYBIND11_PYTEST_ARGS + "" + CACHE STRING "Extra arguments for pytest") + # A single command to compile and run the tests add_custom_target( pytest COMMAND ${PYBIND11_TEST_PREFIX_COMMAND} ${PYTHON_EXECUTABLE} -m pytest - ${PYBIND11_ABS_PYTEST_FILES} + ${PYBIND11_ABS_PYTEST_FILES} ${PYBIND11_PYTEST_ARGS} DEPENDS ${test_targets} WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" USES_TERMINAL) diff --git a/pybind11/tests/conftest.py b/pybind11/tests/conftest.py index ad5b47b4b..7de6c2ace 100644 --- a/pybind11/tests/conftest.py +++ b/pybind11/tests/conftest.py @@ -4,6 +4,8 @@ Extends output capture as needed by pybind11: ignore constructors, optional unor Adds docstring and exceptions message sanitizers. """ +from __future__ import annotations + import contextlib import difflib import gc @@ -218,4 +220,5 @@ def pytest_report_header(config): f" {pybind11_tests.cpp_std}" f" {pybind11_tests.PYBIND11_INTERNALS_ID}" f" PYBIND11_SIMPLE_GIL_MANAGEMENT={pybind11_tests.PYBIND11_SIMPLE_GIL_MANAGEMENT}" + f" PYBIND11_NUMPY_1_ONLY={pybind11_tests.PYBIND11_NUMPY_1_ONLY}" ) diff --git a/pybind11/tests/cross_module_gil_utils.cpp b/pybind11/tests/cross_module_gil_utils.cpp index 7c20849dd..39112668a 100644 --- a/pybind11/tests/cross_module_gil_utils.cpp +++ b/pybind11/tests/cross_module_gil_utils.cpp @@ -92,6 +92,9 @@ extern "C" PYBIND11_EXPORT PyObject *PyInit_cross_module_gil_utils() { if (m != nullptr) { static_assert(sizeof(&gil_acquire) == sizeof(void *), "Function pointer must have the same size as void*"); +#ifdef Py_GIL_DISABLED + PyUnstable_Module_SetGIL(m, Py_MOD_GIL_NOT_USED); +#endif ADD_FUNCTION("gil_acquire_funcaddr", gil_acquire) ADD_FUNCTION("gil_multi_acquire_release_funcaddr", gil_multi_acquire_release) ADD_FUNCTION("gil_acquire_inner_custom_funcaddr", diff --git a/pybind11/tests/cross_module_interleaved_error_already_set.cpp b/pybind11/tests/cross_module_interleaved_error_already_set.cpp index fdd9939e4..65a4463b8 100644 --- a/pybind11/tests/cross_module_interleaved_error_already_set.cpp +++ b/pybind11/tests/cross_module_interleaved_error_already_set.cpp @@ -16,12 +16,12 @@ namespace { namespace py = pybind11; void interleaved_error_already_set() { - PyErr_SetString(PyExc_RuntimeError, "1st error."); + py::set_error(PyExc_RuntimeError, "1st error."); try { throw py::error_already_set(); } catch (const py::error_already_set &) { // The 2nd error could be conditional in a real application. - PyErr_SetString(PyExc_RuntimeError, "2nd error."); + py::set_error(PyExc_RuntimeError, "2nd error."); } // Here the 1st error is destroyed before the 2nd error is fetched. // The error_already_set dtor triggers a pybind11::detail::get_internals() // call via pybind11::gil_scoped_acquire. @@ -42,6 +42,9 @@ extern "C" PYBIND11_EXPORT PyObject *PyInit_cross_module_interleaved_error_alrea if (m != nullptr) { static_assert(sizeof(&interleaved_error_already_set) == sizeof(void *), "Function pointer must have the same size as void *"); +#ifdef Py_GIL_DISABLED + PyUnstable_Module_SetGIL(m, Py_MOD_GIL_NOT_USED); +#endif PyModule_AddObject( m, "funcaddr", diff --git a/pybind11/tests/eigen_tensor_avoid_stl_array.cpp b/pybind11/tests/eigen_tensor_avoid_stl_array.cpp index eacc9e9bd..5aca66c5e 100644 --- a/pybind11/tests/eigen_tensor_avoid_stl_array.cpp +++ b/pybind11/tests/eigen_tensor_avoid_stl_array.cpp @@ -11,4 +11,6 @@ #include "test_eigen_tensor.inl" -PYBIND11_MODULE(eigen_tensor_avoid_stl_array, m) { eigen_tensor_test::test_module(m); } +PYBIND11_MODULE(eigen_tensor_avoid_stl_array, m, pybind11::mod_gil_not_used()) { + eigen_tensor_test::test_module(m); +} diff --git a/pybind11/tests/env.py b/pybind11/tests/env.py index 7eea5a3b3..9f5347f2e 100644 --- a/pybind11/tests/env.py +++ b/pybind11/tests/env.py @@ -1,5 +1,8 @@ +from __future__ import annotations + import platform import sys +import sysconfig import pytest @@ -9,6 +12,7 @@ WIN = sys.platform.startswith("win32") or sys.platform.startswith("cygwin") CPYTHON = platform.python_implementation() == "CPython" PYPY = platform.python_implementation() == "PyPy" +PY_GIL_DISABLED = bool(sysconfig.get_config_var("Py_GIL_DISABLED")) def deprecated_call(): diff --git a/pybind11/tests/extra_python_package/test_files.py b/pybind11/tests/extra_python_package/test_files.py index 57387dd8b..5a3f779a7 100644 --- a/pybind11/tests/extra_python_package/test_files.py +++ b/pybind11/tests/extra_python_package/test_files.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import contextlib import os import string @@ -35,6 +37,7 @@ main_headers = { "include/pybind11/eval.h", "include/pybind11/functional.h", "include/pybind11/gil.h", + "include/pybind11/gil_safe_call_once.h", "include/pybind11/iostream.h", "include/pybind11/numpy.h", "include/pybind11/operators.h", @@ -44,6 +47,7 @@ main_headers = { "include/pybind11/stl.h", "include/pybind11/stl_bind.h", "include/pybind11/type_caster_pyobject_ptr.h", + "include/pybind11/typing.h", } detail_headers = { @@ -71,6 +75,7 @@ cmake_files = { "share/cmake/pybind11/pybind11Common.cmake", "share/cmake/pybind11/pybind11Config.cmake", "share/cmake/pybind11/pybind11ConfigVersion.cmake", + "share/cmake/pybind11/pybind11GuessPythonExtSuffix.cmake", "share/cmake/pybind11/pybind11NewTools.cmake", "share/cmake/pybind11/pybind11Targets.cmake", "share/cmake/pybind11/pybind11Tools.cmake", diff --git a/pybind11/tests/extra_setuptools/test_setuphelper.py b/pybind11/tests/extra_setuptools/test_setuphelper.py index d5d3093bf..2c069adff 100644 --- a/pybind11/tests/extra_setuptools/test_setuphelper.py +++ b/pybind11/tests/extra_setuptools/test_setuphelper.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import os import subprocess import sys diff --git a/pybind11/tests/pybind11_cross_module_tests.cpp b/pybind11/tests/pybind11_cross_module_tests.cpp index 9379f3f25..76f40bfa9 100644 --- a/pybind11/tests/pybind11_cross_module_tests.cpp +++ b/pybind11/tests/pybind11_cross_module_tests.cpp @@ -16,7 +16,7 @@ #include #include -PYBIND11_MODULE(pybind11_cross_module_tests, m) { +PYBIND11_MODULE(pybind11_cross_module_tests, m, py::mod_gil_not_used()) { m.doc() = "pybind11 cross-module test module"; // test_local_bindings.py tests: @@ -31,11 +31,11 @@ PYBIND11_MODULE(pybind11_cross_module_tests, m) { // test_exceptions.py py::register_local_exception(m, "LocalSimpleException"); m.def("raise_runtime_error", []() { - PyErr_SetString(PyExc_RuntimeError, "My runtime error"); + py::set_error(PyExc_RuntimeError, "My runtime error"); throw py::error_already_set(); }); m.def("raise_value_error", []() { - PyErr_SetString(PyExc_ValueError, "My value error"); + py::set_error(PyExc_ValueError, "My value error"); throw py::error_already_set(); }); m.def("throw_pybind_value_error", []() { throw py::value_error("pybind11 value error"); }); @@ -49,7 +49,7 @@ PYBIND11_MODULE(pybind11_cross_module_tests, m) { std::rethrow_exception(p); } } catch (const shared_exception &e) { - PyErr_SetString(PyExc_KeyError, e.what()); + py::set_error(PyExc_KeyError, e.what()); } }); @@ -60,7 +60,7 @@ PYBIND11_MODULE(pybind11_cross_module_tests, m) { std::rethrow_exception(p); } } catch (const LocalException &e) { - PyErr_SetString(PyExc_KeyError, e.what()); + py::set_error(PyExc_KeyError, e.what()); } }); diff --git a/pybind11/tests/pybind11_tests.cpp b/pybind11/tests/pybind11_tests.cpp index 624034648..3d2d84e77 100644 --- a/pybind11/tests/pybind11_tests.cpp +++ b/pybind11/tests/pybind11_tests.cpp @@ -58,7 +58,7 @@ void bind_ConstructorStats(py::module_ &m) { // registered instances to allow instance cleanup checks (invokes a GC first) .def_static("detail_reg_inst", []() { ConstructorStats::gc(); - return py::detail::get_internals().registered_instances.size(); + return py::detail::num_registered_instances(); }); } @@ -75,26 +75,34 @@ const char *cpp_std() { #endif } -PYBIND11_MODULE(pybind11_tests, m) { +PYBIND11_MODULE(pybind11_tests, m, py::mod_gil_not_used()) { m.doc() = "pybind11 test module"; // Intentionally kept minimal to not create a maintenance chore // ("just enough" to be conclusive). -#if defined(_MSC_FULL_VER) - m.attr("compiler_info") = "MSVC " PYBIND11_TOSTRING(_MSC_FULL_VER); -#elif defined(__VERSION__) +#if defined(__VERSION__) m.attr("compiler_info") = __VERSION__; +#elif defined(_MSC_FULL_VER) + m.attr("compiler_info") = "MSVC " PYBIND11_TOSTRING(_MSC_FULL_VER); #else m.attr("compiler_info") = py::none(); #endif m.attr("cpp_std") = cpp_std(); m.attr("PYBIND11_INTERNALS_ID") = PYBIND11_INTERNALS_ID; + // Free threaded Python uses UINT32_MAX for immortal objects. + m.attr("PYBIND11_REFCNT_IMMORTAL") = UINT32_MAX; m.attr("PYBIND11_SIMPLE_GIL_MANAGEMENT") = #if defined(PYBIND11_SIMPLE_GIL_MANAGEMENT) true; #else false; #endif + m.attr("PYBIND11_NUMPY_1_ONLY") = +#if defined(PYBIND11_NUMPY_1_ONLY) + true; +#else + false; +#endif bind_ConstructorStats(m); diff --git a/pybind11/tests/pybind11_tests.h b/pybind11/tests/pybind11_tests.h index a7c00c2f9..7be58feb6 100644 --- a/pybind11/tests/pybind11_tests.h +++ b/pybind11/tests/pybind11_tests.h @@ -3,6 +3,8 @@ #include #include +#include + namespace py = pybind11; using namespace pybind11::literals; @@ -52,6 +54,17 @@ union IntFloat { float f; }; +class UnusualOpRef { +public: + using NonTrivialType = std::shared_ptr; // Almost any non-trivial type will do. + // Overriding operator& should not break pybind11. + NonTrivialType operator&() { return non_trivial_member; } + NonTrivialType operator&() const { return non_trivial_member; } + +private: + NonTrivialType non_trivial_member; +}; + /// Custom cast-only type that casts to a string "rvalue" or "lvalue" depending on the cast /// context. Used to test recursive casters (e.g. std::tuple, stl containers). struct RValueCaster {}; diff --git a/pybind11/tests/pytest.ini b/pybind11/tests/pytest.ini index 792ba361f..42c3c40c6 100644 --- a/pybind11/tests/pytest.ini +++ b/pybind11/tests/pytest.ini @@ -20,3 +20,4 @@ filterwarnings = # bogus numpy ABI warning (see numpy/#432) ignore:.*numpy.dtype size changed.*:RuntimeWarning ignore:.*numpy.ufunc size changed.*:RuntimeWarning + default:The global interpreter lock:RuntimeWarning diff --git a/pybind11/tests/requirements.txt b/pybind11/tests/requirements.txt index 4ba101119..337897bd2 100644 --- a/pybind11/tests/requirements.txt +++ b/pybind11/tests/requirements.txt @@ -1,9 +1,13 @@ -build==0.8.0 -numpy==1.21.5; platform_python_implementation=="PyPy" and sys_platform=="linux" and python_version=="3.7" -numpy==1.19.3; platform_python_implementation!="PyPy" and python_version=="3.6" -numpy==1.21.5; platform_python_implementation!="PyPy" and python_version>="3.7" and python_version<"3.10" -numpy==1.22.2; platform_python_implementation!="PyPy" and python_version>="3.10" and python_version<"3.11" -pytest==7.0.0 +--only-binary=:all: +build~=1.0; python_version>="3.7" +numpy~=1.20.0; python_version=="3.7" and platform_python_implementation=="PyPy" +numpy~=1.23.0; python_version=="3.8" and platform_python_implementation=="PyPy" +numpy~=1.25.0; python_version=="3.9" and platform_python_implementation=='PyPy' +numpy~=1.21.5; platform_python_implementation!="PyPy" and python_version>="3.7" and python_version<"3.10" +numpy~=1.22.2; platform_python_implementation!="PyPy" and python_version=="3.10" +numpy~=1.26.0; platform_python_implementation!="PyPy" and python_version>="3.11" and python_version<"3.13" +pytest~=7.0 pytest-timeout -scipy==1.5.4; platform_python_implementation!="PyPy" and python_version<"3.10" -scipy==1.10.0; platform_python_implementation!="PyPy" and python_version=="3.10" +scipy~=1.5.4; platform_python_implementation!="PyPy" and python_version<"3.10" +scipy~=1.8.0; platform_python_implementation!="PyPy" and python_version=="3.10" and sys_platform!='win32' +scipy~=1.11.1; platform_python_implementation!="PyPy" and python_version>="3.11" and python_version<"3.13" and sys_platform!='win32' diff --git a/pybind11/tests/test_async.py b/pybind11/tests/test_async.py index 83a4c5036..4d33ba65f 100644 --- a/pybind11/tests/test_async.py +++ b/pybind11/tests/test_async.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest asyncio = pytest.importorskip("asyncio") diff --git a/pybind11/tests/test_buffers.py b/pybind11/tests/test_buffers.py index 63d9d869f..84a301e25 100644 --- a/pybind11/tests/test_buffers.py +++ b/pybind11/tests/test_buffers.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import ctypes import io import struct @@ -219,3 +221,10 @@ def test_ctypes_from_buffer(): assert cinfo.shape == pyinfo.shape assert cinfo.strides == pyinfo.strides assert not cinfo.readonly + + +def test_buffer_docstring(): + assert ( + m.get_buffer_info.__doc__.strip() + == "get_buffer_info(arg0: Buffer) -> pybind11_tests.buffers.buffer_info" + ) diff --git a/pybind11/tests/test_builtin_casters.cpp b/pybind11/tests/test_builtin_casters.cpp index 0623b85dc..292304a63 100644 --- a/pybind11/tests/test_builtin_casters.cpp +++ b/pybind11/tests/test_builtin_casters.cpp @@ -95,7 +95,7 @@ TEST_SUBMODULE(builtin_casters, m) { } // 𝐀, utf16 else { wstr.push_back((wchar_t) mathbfA32); - } // 𝐀, utf32 + } // 𝐀, utf32 wstr.push_back(0x7a); // z m.def("good_utf8_string", []() { @@ -104,10 +104,9 @@ TEST_SUBMODULE(builtin_casters, m) { m.def("good_utf16_string", [=]() { return std::u16string({b16, ib16, cake16_1, cake16_2, mathbfA16_1, mathbfA16_2, z16}); }); // b‽🎂𝐀z - m.def("good_utf32_string", [=]() { - return std::u32string({a32, mathbfA32, cake32, ib32, z32}); - }); // a𝐀🎂‽z - m.def("good_wchar_string", [=]() { return wstr; }); // a‽𝐀z + m.def("good_utf32_string", + [=]() { return std::u32string({a32, mathbfA32, cake32, ib32, z32}); }); // a𝐀🎂‽z + m.def("good_wchar_string", [=]() { return wstr; }); // a‽𝐀z m.def("bad_utf8_string", []() { return std::string("abc\xd0" "def"); @@ -117,9 +116,8 @@ TEST_SUBMODULE(builtin_casters, m) { // UnicodeDecodeError m.def("bad_utf32_string", [=]() { return std::u32string({a32, char32_t(0xd800), z32}); }); if (sizeof(wchar_t) == 2) { - m.def("bad_wchar_string", [=]() { - return std::wstring({wchar_t(0x61), wchar_t(0xd800)}); - }); + m.def("bad_wchar_string", + [=]() { return std::wstring({wchar_t(0x61), wchar_t(0xd800)}); }); } m.def("u8_Z", []() -> char { return 'Z'; }); m.def("u8_eacute", []() -> char { return '\xe9'; }); @@ -236,8 +234,7 @@ TEST_SUBMODULE(builtin_casters, m) { // test_int_convert m.def("int_passthrough", [](int arg) { return arg; }); - m.def( - "int_passthrough_noconvert", [](int arg) { return arg; }, py::arg{}.noconvert()); + m.def("int_passthrough_noconvert", [](int arg) { return arg; }, py::arg{}.noconvert()); // test_tuple m.def( @@ -302,8 +299,7 @@ TEST_SUBMODULE(builtin_casters, m) { // test_bool_caster m.def("bool_passthrough", [](bool arg) { return arg; }); - m.def( - "bool_passthrough_noconvert", [](bool arg) { return arg; }, py::arg{}.noconvert()); + m.def("bool_passthrough_noconvert", [](bool arg) { return arg; }, py::arg{}.noconvert()); // TODO: This should be disabled and fixed in future Intel compilers #if !defined(__INTEL_COMPILER) @@ -311,8 +307,7 @@ TEST_SUBMODULE(builtin_casters, m) { // When compiled with the Intel compiler, this results in segmentation faults when importing // the module. Tested with icc (ICC) 2021.1 Beta 20200827, this should be tested again when // a newer version of icc is available. - m.def( - "bool_passthrough_noconvert2", [](bool arg) { return arg; }, py::arg().noconvert()); + m.def("bool_passthrough_noconvert2", [](bool arg) { return arg; }, py::arg().noconvert()); #endif // test_reference_wrapper diff --git a/pybind11/tests/test_builtin_casters.py b/pybind11/tests/test_builtin_casters.py index b1f57bdd9..9aa5926e9 100644 --- a/pybind11/tests/test_builtin_casters.py +++ b/pybind11/tests/test_builtin_casters.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import sys import pytest @@ -352,7 +354,7 @@ def test_tuple(doc): assert ( doc(m.pair_passthrough) == """ - pair_passthrough(arg0: Tuple[bool, str]) -> Tuple[str, bool] + pair_passthrough(arg0: tuple[bool, str]) -> tuple[str, bool] Return a pair in reversed order """ @@ -360,7 +362,7 @@ def test_tuple(doc): assert ( doc(m.tuple_passthrough) == """ - tuple_passthrough(arg0: Tuple[bool, str, int]) -> Tuple[int, str, bool] + tuple_passthrough(arg0: tuple[bool, str, int]) -> tuple[int, str, bool] Return a triple in reversed order """ diff --git a/pybind11/tests/test_call_policies.cpp b/pybind11/tests/test_call_policies.cpp index d177008cf..92924cb45 100644 --- a/pybind11/tests/test_call_policies.cpp +++ b/pybind11/tests/test_call_policies.cpp @@ -63,10 +63,8 @@ TEST_SUBMODULE(call_policies, m) { .def("returnNullChildKeepAliveParent", &Parent::returnNullChild, py::keep_alive<0, 1>()) .def_static("staticFunction", &Parent::staticFunction, py::keep_alive<1, 0>()); - m.def( - "free_function", [](Parent *, Child *) {}, py::keep_alive<1, 2>()); - m.def( - "invalid_arg_index", [] {}, py::keep_alive<0, 1>()); + m.def("free_function", [](Parent *, Child *) {}, py::keep_alive<1, 2>()); + m.def("invalid_arg_index", [] {}, py::keep_alive<0, 1>()); #if !defined(PYPY_VERSION) // test_alive_gc @@ -97,7 +95,7 @@ TEST_SUBMODULE(call_policies, m) { }, py::call_guard()); -#if defined(WITH_THREAD) && !defined(PYPY_VERSION) +#if !defined(PYPY_VERSION) // `py::call_guard()` should work in PyPy as well, // but it's unclear how to test it without `PyGILState_GetThisThreadState`. auto report_gil_status = []() { diff --git a/pybind11/tests/test_call_policies.py b/pybind11/tests/test_call_policies.py index 616056412..91670deb3 100644 --- a/pybind11/tests/test_call_policies.py +++ b/pybind11/tests/test_call_policies.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest import env # noqa: F401 diff --git a/pybind11/tests/test_callbacks.py b/pybind11/tests/test_callbacks.py index 4a652f53e..ce2a6d254 100644 --- a/pybind11/tests/test_callbacks.py +++ b/pybind11/tests/test_callbacks.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import time from threading import Thread @@ -216,3 +218,10 @@ def test_custom_func(): def test_custom_func2(): assert m.custom_function2(3) == 27 assert m.roundtrip(m.custom_function2)(3) == 27 + + +def test_callback_docstring(): + assert ( + m.test_tuple_unpacking.__doc__.strip() + == "test_tuple_unpacking(arg0: Callable) -> object" + ) diff --git a/pybind11/tests/test_chrono.py b/pybind11/tests/test_chrono.py index a29316c38..ed889fbd6 100644 --- a/pybind11/tests/test_chrono.py +++ b/pybind11/tests/test_chrono.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import datetime import pytest diff --git a/pybind11/tests/test_class.cpp b/pybind11/tests/test_class.cpp index 7241bc881..9001d86b1 100644 --- a/pybind11/tests/test_class.cpp +++ b/pybind11/tests/test_class.cpp @@ -403,7 +403,7 @@ TEST_SUBMODULE(class_, m) { // [workaround(intel)] = default does not work here // Removing or defaulting this destructor results in linking errors with the Intel compiler // (in Debug builds only, tested with icpc (ICC) 2021.1 Beta 20200827) - ~PublicistB() override{}; // NOLINT(modernize-use-equals-default) + ~PublicistB() override {}; // NOLINT(modernize-use-equals-default) using ProtectedB::foo; using ProtectedB::get_self; using ProtectedB::void_foo; @@ -461,8 +461,7 @@ TEST_SUBMODULE(class_, m) { py::class_(base, "Nested") .def(py::init<>()) .def("fn", [](Nested &, int, NestBase &, Nested &) {}) - .def( - "fa", [](Nested &, int, NestBase &, Nested &) {}, "a"_a, "b"_a, "c"_a); + .def("fa", [](Nested &, int, NestBase &, Nested &) {}, "a"_a, "b"_a, "c"_a); base.def("g", [](NestBase &, Nested &) {}); base.def("h", []() { return NestBase(); }); diff --git a/pybind11/tests/test_class.py b/pybind11/tests/test_class.py index ee7467cf8..9b2b1d834 100644 --- a/pybind11/tests/test_class.py +++ b/pybind11/tests/test_class.py @@ -1,7 +1,11 @@ +from __future__ import annotations + +from unittest import mock + import pytest import env -from pybind11_tests import ConstructorStats, UserType +from pybind11_tests import PYBIND11_REFCNT_IMMORTAL, ConstructorStats, UserType from pybind11_tests import class_ as m @@ -203,6 +207,18 @@ def test_inheritance_init(msg): assert msg(exc_info.value) == expected +@pytest.mark.parametrize( + "mock_return_value", [None, (1, 2, 3), m.Pet("Polly", "parrot"), m.Dog("Molly")] +) +def test_mock_new(mock_return_value): + with mock.patch.object( + m.Pet, "__new__", return_value=mock_return_value + ) as mock_new: + obj = m.Pet("Noname", "Nospecies") + assert obj is mock_return_value + mock_new.assert_called_once_with(m.Pet, "Noname", "Nospecies") + + def test_automatic_upcasting(): assert type(m.return_class_1()).__name__ == "DerivedClass1" assert type(m.return_class_2()).__name__ == "DerivedClass2" @@ -363,7 +379,9 @@ def test_class_refcount(): refcount_3 = getrefcount(cls) assert refcount_1 == refcount_3 - assert refcount_2 > refcount_1 + assert (refcount_2 > refcount_1) or ( + refcount_2 == refcount_1 == PYBIND11_REFCNT_IMMORTAL + ) def test_reentrant_implicit_conversion_failure(msg): diff --git a/pybind11/tests/test_cmake_build/CMakeLists.txt b/pybind11/tests/test_cmake_build/CMakeLists.txt index e5aa975cf..f28bde08e 100644 --- a/pybind11/tests/test_cmake_build/CMakeLists.txt +++ b/pybind11/tests/test_cmake_build/CMakeLists.txt @@ -5,9 +5,8 @@ function(pybind11_add_build_test name) set(build_options "-DCMAKE_CXX_COMPILER=${CMAKE_CXX_COMPILER}") + list(APPEND build_options "-DPYBIND11_FINDPYTHON=${PYBIND11_FINDPYTHON}") if(PYBIND11_FINDPYTHON) - list(APPEND build_options "-DPYBIND11_FINDPYTHON=${PYBIND11_FINDPYTHON}") - if(DEFINED Python_ROOT_DIR) list(APPEND build_options "-DPython_ROOT_DIR=${Python_ROOT_DIR}") endif() diff --git a/pybind11/tests/test_cmake_build/installed_embed/CMakeLists.txt b/pybind11/tests/test_cmake_build/installed_embed/CMakeLists.txt index d9dcb45e4..2be0aa659 100644 --- a/pybind11/tests/test_cmake_build/installed_embed/CMakeLists.txt +++ b/pybind11/tests/test_cmake_build/installed_embed/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.5) -# The `cmake_minimum_required(VERSION 3.5...3.26)` syntax does not work with +# The `cmake_minimum_required(VERSION 3.5...3.29)` syntax does not work with # some versions of VS that have a patched CMake 3.11. This forces us to emulate # the behavior using the following workaround: -if(${CMAKE_VERSION} VERSION_LESS 3.26) +if(${CMAKE_VERSION} VERSION_LESS 3.29) cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}) else() - cmake_policy(VERSION 3.26) + cmake_policy(VERSION 3.29) endif() project(test_installed_embed CXX) diff --git a/pybind11/tests/test_cmake_build/installed_function/CMakeLists.txt b/pybind11/tests/test_cmake_build/installed_function/CMakeLists.txt index 2f4f64275..fa7795e1e 100644 --- a/pybind11/tests/test_cmake_build/installed_function/CMakeLists.txt +++ b/pybind11/tests/test_cmake_build/installed_function/CMakeLists.txt @@ -1,13 +1,13 @@ cmake_minimum_required(VERSION 3.5) project(test_installed_module CXX) -# The `cmake_minimum_required(VERSION 3.5...3.26)` syntax does not work with +# The `cmake_minimum_required(VERSION 3.5...3.29)` syntax does not work with # some versions of VS that have a patched CMake 3.11. This forces us to emulate # the behavior using the following workaround: -if(${CMAKE_VERSION} VERSION_LESS 3.26) +if(${CMAKE_VERSION} VERSION_LESS 3.29) cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}) else() - cmake_policy(VERSION 3.26) + cmake_policy(VERSION 3.29) endif() project(test_installed_function CXX) diff --git a/pybind11/tests/test_cmake_build/installed_target/CMakeLists.txt b/pybind11/tests/test_cmake_build/installed_target/CMakeLists.txt index a981e236f..7e73f4243 100644 --- a/pybind11/tests/test_cmake_build/installed_target/CMakeLists.txt +++ b/pybind11/tests/test_cmake_build/installed_target/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.5) -# The `cmake_minimum_required(VERSION 3.5...3.26)` syntax does not work with +# The `cmake_minimum_required(VERSION 3.5...3.29)` syntax does not work with # some versions of VS that have a patched CMake 3.11. This forces us to emulate # the behavior using the following workaround: -if(${CMAKE_VERSION} VERSION_LESS 3.26) +if(${CMAKE_VERSION} VERSION_LESS 3.29) cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}) else() - cmake_policy(VERSION 3.26) + cmake_policy(VERSION 3.29) endif() project(test_installed_target CXX) diff --git a/pybind11/tests/test_cmake_build/main.cpp b/pybind11/tests/test_cmake_build/main.cpp index e30f2c4b9..640449c31 100644 --- a/pybind11/tests/test_cmake_build/main.cpp +++ b/pybind11/tests/test_cmake_build/main.cpp @@ -1,6 +1,6 @@ #include namespace py = pybind11; -PYBIND11_MODULE(test_cmake_build, m) { +PYBIND11_MODULE(test_cmake_build, m, py::mod_gil_not_used()) { m.def("add", [](int i, int j) { return i + j; }); } diff --git a/pybind11/tests/test_cmake_build/subdirectory_embed/CMakeLists.txt b/pybind11/tests/test_cmake_build/subdirectory_embed/CMakeLists.txt index f286746b9..33a366472 100644 --- a/pybind11/tests/test_cmake_build/subdirectory_embed/CMakeLists.txt +++ b/pybind11/tests/test_cmake_build/subdirectory_embed/CMakeLists.txt @@ -1,12 +1,12 @@ cmake_minimum_required(VERSION 3.5) -# The `cmake_minimum_required(VERSION 3.5...3.26)` syntax does not work with +# The `cmake_minimum_required(VERSION 3.5...3.29)` syntax does not work with # some versions of VS that have a patched CMake 3.11. This forces us to emulate # the behavior using the following workaround: -if(${CMAKE_VERSION} VERSION_LESS 3.26) +if(${CMAKE_VERSION} VERSION_LESS 3.29) cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}) else() - cmake_policy(VERSION 3.26) + cmake_policy(VERSION 3.29) endif() project(test_subdirectory_embed CXX) @@ -16,6 +16,12 @@ set(PYBIND11_INSTALL CACHE BOOL "") set(PYBIND11_EXPORT_NAME test_export) +# Allow PYTHON_EXECUTABLE if in FINDPYTHON mode and building pybind11's tests +# (makes transition easier while we support both modes). +if(DEFINED PYTHON_EXECUTABLE AND NOT DEFINED Python_EXECUTABLE) + set(Python_EXECUTABLE "${PYTHON_EXECUTABLE}") +endif() + add_subdirectory("${pybind11_SOURCE_DIR}" pybind11) # Test basic target functionality diff --git a/pybind11/tests/test_cmake_build/subdirectory_function/CMakeLists.txt b/pybind11/tests/test_cmake_build/subdirectory_function/CMakeLists.txt index 275a75c0b..76418a71f 100644 --- a/pybind11/tests/test_cmake_build/subdirectory_function/CMakeLists.txt +++ b/pybind11/tests/test_cmake_build/subdirectory_function/CMakeLists.txt @@ -1,16 +1,22 @@ cmake_minimum_required(VERSION 3.5) -# The `cmake_minimum_required(VERSION 3.5...3.26)` syntax does not work with +# The `cmake_minimum_required(VERSION 3.5...3.29)` syntax does not work with # some versions of VS that have a patched CMake 3.11. This forces us to emulate # the behavior using the following workaround: -if(${CMAKE_VERSION} VERSION_LESS 3.26) +if(${CMAKE_VERSION} VERSION_LESS 3.29) cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}) else() - cmake_policy(VERSION 3.26) + cmake_policy(VERSION 3.29) endif() project(test_subdirectory_function CXX) +# Allow PYTHON_EXECUTABLE if in FINDPYTHON mode and building pybind11's tests +# (makes transition easier while we support both modes). +if(DEFINED PYTHON_EXECUTABLE AND NOT DEFINED Python_EXECUTABLE) + set(Python_EXECUTABLE "${PYTHON_EXECUTABLE}") +endif() + add_subdirectory("${pybind11_SOURCE_DIR}" pybind11) pybind11_add_module(test_subdirectory_function ../main.cpp) set_target_properties(test_subdirectory_function PROPERTIES OUTPUT_NAME test_cmake_build) diff --git a/pybind11/tests/test_cmake_build/subdirectory_target/CMakeLists.txt b/pybind11/tests/test_cmake_build/subdirectory_target/CMakeLists.txt index 37bb2c56e..28e903187 100644 --- a/pybind11/tests/test_cmake_build/subdirectory_target/CMakeLists.txt +++ b/pybind11/tests/test_cmake_build/subdirectory_target/CMakeLists.txt @@ -1,16 +1,22 @@ cmake_minimum_required(VERSION 3.5) -# The `cmake_minimum_required(VERSION 3.5...3.26)` syntax does not work with +# The `cmake_minimum_required(VERSION 3.5...3.29)` syntax does not work with # some versions of VS that have a patched CMake 3.11. This forces us to emulate # the behavior using the following workaround: -if(${CMAKE_VERSION} VERSION_LESS 3.26) +if(${CMAKE_VERSION} VERSION_LESS 3.29) cmake_policy(VERSION ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION}) else() - cmake_policy(VERSION 3.26) + cmake_policy(VERSION 3.29) endif() project(test_subdirectory_target CXX) +# Allow PYTHON_EXECUTABLE if in FINDPYTHON mode and building pybind11's tests +# (makes transition easier while we support both modes). +if(DEFINED PYTHON_EXECUTABLE AND NOT DEFINED Python_EXECUTABLE) + set(Python_EXECUTABLE "${PYTHON_EXECUTABLE}") +endif() + add_subdirectory("${pybind11_SOURCE_DIR}" pybind11) add_library(test_subdirectory_target MODULE ../main.cpp) diff --git a/pybind11/tests/test_cmake_build/test.py b/pybind11/tests/test_cmake_build/test.py index 807fd43b4..bb4c20e0c 100644 --- a/pybind11/tests/test_cmake_build/test.py +++ b/pybind11/tests/test_cmake_build/test.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import sys import test_cmake_build diff --git a/pybind11/tests/test_const_name.py b/pybind11/tests/test_const_name.py index a145f0bbb..f52024945 100644 --- a/pybind11/tests/test_const_name.py +++ b/pybind11/tests/test_const_name.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest from pybind11_tests import const_name as m diff --git a/pybind11/tests/test_constants_and_functions.cpp b/pybind11/tests/test_constants_and_functions.cpp index 312edca9e..bf42b4155 100644 --- a/pybind11/tests/test_constants_and_functions.cpp +++ b/pybind11/tests/test_constants_and_functions.cpp @@ -54,7 +54,11 @@ int f2(int x) noexcept(true) { return x + 2; } int f3(int x) noexcept(false) { return x + 3; } PYBIND11_WARNING_PUSH PYBIND11_WARNING_DISABLE_GCC("-Wdeprecated") +#if defined(__clang_major__) && __clang_major__ >= 5 +PYBIND11_WARNING_DISABLE_CLANG("-Wdeprecated-dynamic-exception-spec") +#else PYBIND11_WARNING_DISABLE_CLANG("-Wdeprecated") +#endif // NOLINTNEXTLINE(modernize-use-noexcept) int f4(int x) throw() { return x + 4; } // Deprecated equivalent to noexcept(true) PYBIND11_WARNING_POP diff --git a/pybind11/tests/test_constants_and_functions.py b/pybind11/tests/test_constants_and_functions.py index a1142461c..63004e1b6 100644 --- a/pybind11/tests/test_constants_and_functions.py +++ b/pybind11/tests/test_constants_and_functions.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest m = pytest.importorskip("pybind11_tests.constants_and_functions") diff --git a/pybind11/tests/test_copy_move.cpp b/pybind11/tests/test_copy_move.cpp index f54733550..ddffbe3a9 100644 --- a/pybind11/tests/test_copy_move.cpp +++ b/pybind11/tests/test_copy_move.cpp @@ -157,6 +157,13 @@ public: PYBIND11_NAMESPACE_END(detail) PYBIND11_NAMESPACE_END(pybind11) +namespace { + +py::object CastUnusualOpRefConstRef(const UnusualOpRef &cref) { return py::cast(cref); } +py::object CastUnusualOpRefMovable(UnusualOpRef &&mvbl) { return py::cast(std::move(mvbl)); } + +} // namespace + TEST_SUBMODULE(copy_move_policies, m) { // test_lacking_copy_ctor py::class_(m, "lacking_copy_ctor") @@ -289,11 +296,15 @@ TEST_SUBMODULE(copy_move_policies, m) { "get_moveissue1", [](int i) { return std::unique_ptr(new MoveIssue1(i)); }, py::return_value_policy::move); - m.def( - "get_moveissue2", [](int i) { return MoveIssue2(i); }, py::return_value_policy::move); + m.def("get_moveissue2", [](int i) { return MoveIssue2(i); }, py::return_value_policy::move); // Make sure that cast from pytype rvalue to other pytype works m.def("get_pytype_rvalue_castissue", [](double i) { return py::float_(i).cast(); }); + + py::class_(m, "UnusualOpRef"); + m.def("CallCastUnusualOpRefConstRef", + []() { return CastUnusualOpRefConstRef(UnusualOpRef()); }); + m.def("CallCastUnusualOpRefMovable", []() { return CastUnusualOpRefMovable(UnusualOpRef()); }); } /* diff --git a/pybind11/tests/test_copy_move.py b/pybind11/tests/test_copy_move.py index 9fef08933..ee046305f 100644 --- a/pybind11/tests/test_copy_move.py +++ b/pybind11/tests/test_copy_move.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest from pybind11_tests import copy_move_policies as m @@ -130,3 +132,9 @@ def test_pytype_rvalue_cast(): value = m.get_pytype_rvalue_castissue(1.0) assert value == 1 + + +def test_unusual_op_ref(): + # Merely to test that this still exists and built successfully. + assert m.CallCastUnusualOpRefConstRef().__class__.__name__ == "UnusualOpRef" + assert m.CallCastUnusualOpRefMovable().__class__.__name__ == "UnusualOpRef" diff --git a/pybind11/tests/test_custom_type_casters.cpp b/pybind11/tests/test_custom_type_casters.cpp index b4af02a45..0ca2d2541 100644 --- a/pybind11/tests/test_custom_type_casters.cpp +++ b/pybind11/tests/test_custom_type_casters.cpp @@ -134,6 +134,16 @@ struct type_caster : public other_lib::my_caster {}; } // namespace detail } // namespace PYBIND11_NAMESPACE +// This simply is required to compile +namespace ADL_issue { +template +OutStringType concat(Args &&...) { + return OutStringType(); +} + +struct test {}; +} // namespace ADL_issue + TEST_SUBMODULE(custom_type_casters, m) { // test_custom_type_casters @@ -175,14 +185,10 @@ TEST_SUBMODULE(custom_type_casters, m) { py::arg_v(nullptr, ArgInspector1()).noconvert(true), py::arg() = ArgAlwaysConverts()); - m.def( - "floats_preferred", [](double f) { return 0.5 * f; }, "f"_a); - m.def( - "floats_only", [](double f) { return 0.5 * f; }, "f"_a.noconvert()); - m.def( - "ints_preferred", [](int i) { return i / 2; }, "i"_a); - m.def( - "ints_only", [](int i) { return i / 2; }, "i"_a.noconvert()); + m.def("floats_preferred", [](double f) { return 0.5 * f; }, "f"_a); + m.def("floats_only", [](double f) { return 0.5 * f; }, "f"_a.noconvert()); + m.def("ints_preferred", [](int i) { return i / 2; }, "i"_a); + m.def("ints_only", [](int i) { return i / 2; }, "i"_a.noconvert()); // test_custom_caster_destruction // Test that `take_ownership` works on types with a custom type caster when given a pointer @@ -206,4 +212,6 @@ TEST_SUBMODULE(custom_type_casters, m) { py::return_value_policy::reference); m.def("other_lib_type", [](other_lib::MyType x) { return x; }); + + m.def("_adl_issue", [](const ADL_issue::test &) {}); } diff --git a/pybind11/tests/test_custom_type_casters.py b/pybind11/tests/test_custom_type_casters.py index 3a00ea964..689b1e9cb 100644 --- a/pybind11/tests/test_custom_type_casters.py +++ b/pybind11/tests/test_custom_type_casters.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest from pybind11_tests import custom_type_casters as m diff --git a/pybind11/tests/test_custom_type_setup.py b/pybind11/tests/test_custom_type_setup.py index e63ff5758..56922c6dd 100644 --- a/pybind11/tests/test_custom_type_setup.py +++ b/pybind11/tests/test_custom_type_setup.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import gc import weakref diff --git a/pybind11/tests/test_docstring_options.cpp b/pybind11/tests/test_docstring_options.cpp index dda1cf6e4..de045a7ca 100644 --- a/pybind11/tests/test_docstring_options.cpp +++ b/pybind11/tests/test_docstring_options.cpp @@ -15,37 +15,26 @@ TEST_SUBMODULE(docstring_options, m) { py::options options; options.disable_function_signatures(); - m.def( - "test_function1", [](int, int) {}, py::arg("a"), py::arg("b")); - m.def( - "test_function2", [](int, int) {}, py::arg("a"), py::arg("b"), "A custom docstring"); + m.def("test_function1", [](int, int) {}, py::arg("a"), py::arg("b")); + m.def("test_function2", [](int, int) {}, py::arg("a"), py::arg("b"), "A custom docstring"); - m.def( - "test_overloaded1", [](int) {}, py::arg("i"), "Overload docstring"); - m.def( - "test_overloaded1", [](double) {}, py::arg("d")); + m.def("test_overloaded1", [](int) {}, py::arg("i"), "Overload docstring"); + m.def("test_overloaded1", [](double) {}, py::arg("d")); - m.def( - "test_overloaded2", [](int) {}, py::arg("i"), "overload docstring 1"); - m.def( - "test_overloaded2", [](double) {}, py::arg("d"), "overload docstring 2"); + m.def("test_overloaded2", [](int) {}, py::arg("i"), "overload docstring 1"); + m.def("test_overloaded2", [](double) {}, py::arg("d"), "overload docstring 2"); - m.def( - "test_overloaded3", [](int) {}, py::arg("i")); - m.def( - "test_overloaded3", [](double) {}, py::arg("d"), "Overload docstr"); + m.def("test_overloaded3", [](int) {}, py::arg("i")); + m.def("test_overloaded3", [](double) {}, py::arg("d"), "Overload docstr"); options.enable_function_signatures(); - m.def( - "test_function3", [](int, int) {}, py::arg("a"), py::arg("b")); - m.def( - "test_function4", [](int, int) {}, py::arg("a"), py::arg("b"), "A custom docstring"); + m.def("test_function3", [](int, int) {}, py::arg("a"), py::arg("b")); + m.def("test_function4", [](int, int) {}, py::arg("a"), py::arg("b"), "A custom docstring"); options.disable_function_signatures().disable_user_defined_docstrings(); - m.def( - "test_function5", [](int, int) {}, py::arg("a"), py::arg("b"), "A custom docstring"); + m.def("test_function5", [](int, int) {}, py::arg("a"), py::arg("b"), "A custom docstring"); { py::options nested_options; @@ -59,8 +48,7 @@ TEST_SUBMODULE(docstring_options, m) { } } - m.def( - "test_function7", [](int, int) {}, py::arg("a"), py::arg("b"), "A custom docstring"); + m.def("test_function7", [](int, int) {}, py::arg("a"), py::arg("b"), "A custom docstring"); { py::options options; diff --git a/pybind11/tests/test_docstring_options.py b/pybind11/tests/test_docstring_options.py index e6f5a9d98..09fc8ac25 100644 --- a/pybind11/tests/test_docstring_options.py +++ b/pybind11/tests/test_docstring_options.py @@ -1,3 +1,5 @@ +from __future__ import annotations + from pybind11_tests import docstring_options as m diff --git a/pybind11/tests/test_eigen_matrix.cpp b/pybind11/tests/test_eigen_matrix.cpp index 554cc4d7f..21261bfc2 100644 --- a/pybind11/tests/test_eigen_matrix.cpp +++ b/pybind11/tests/test_eigen_matrix.cpp @@ -330,6 +330,21 @@ TEST_SUBMODULE(eigen_matrix, m) { m.def("dense_c", [mat]() -> DenseMatrixC { return DenseMatrixC(mat); }); m.def("dense_copy_r", [](const DenseMatrixR &m) -> DenseMatrixR { return m; }); m.def("dense_copy_c", [](const DenseMatrixC &m) -> DenseMatrixC { return m; }); + // test_defaults + bool have_numpy = true; + try { + py::module_::import("numpy"); + } catch (const py::error_already_set &) { + have_numpy = false; + } + if (have_numpy) { + py::module_::import("numpy"); + Eigen::Matrix defaultMatrix = Eigen::Matrix3d::Identity(); + m.def("defaults_mat", [](const Eigen::Matrix3d &) {}, py::arg("mat") = defaultMatrix); + + Eigen::VectorXd defaultVector = Eigen::VectorXd::Ones(32); + m.def("defaults_vec", [](const Eigen::VectorXd &) {}, py::arg("vec") = defaultMatrix); + } // test_sparse, test_sparse_signature m.def("sparse_r", [mat]() -> SparseMatrixR { // NOLINTNEXTLINE(clang-analyzer-core.uninitialized.UndefReturn) diff --git a/pybind11/tests/test_eigen_matrix.py b/pybind11/tests/test_eigen_matrix.py index b2e76740b..e1d7433f1 100644 --- a/pybind11/tests/test_eigen_matrix.py +++ b/pybind11/tests/test_eigen_matrix.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest from pybind11_tests import ConstructorStats @@ -608,7 +610,9 @@ def test_both_ref_mutators(): def test_nocopy_wrapper(): # get_elem requires a column-contiguous matrix reference, but should be # callable with other types of matrix (via copying): - int_matrix_colmajor = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]], order="F") + int_matrix_colmajor = np.array( + [[1, 2, 3], [4, 5, 6], [7, 8, 9]], dtype="l", order="F" + ) dbl_matrix_colmajor = np.array( int_matrix_colmajor, dtype="double", order="F", copy=True ) @@ -716,6 +720,11 @@ def test_dense_signature(doc): ) +def test_defaults(doc): + assert "\n" not in str(doc(m.defaults_mat)) + assert "\n" not in str(doc(m.defaults_vec)) + + def test_named_arguments(): a = np.array([[1.0, 2], [3, 4], [5, 6]]) b = np.ones((2, 1)) diff --git a/pybind11/tests/test_eigen_tensor.inl b/pybind11/tests/test_eigen_tensor.inl index d864ce737..25cf29f15 100644 --- a/pybind11/tests/test_eigen_tensor.inl +++ b/pybind11/tests/test_eigen_tensor.inl @@ -121,8 +121,7 @@ void init_tensor_module(pybind11::module &m) { []() { return &get_fixed_tensor(); }, py::return_value_policy::copy); - m.def( - "copy_tensor", []() { return &get_tensor(); }, py::return_value_policy::copy); + m.def("copy_tensor", []() { return &get_tensor(); }, py::return_value_policy::copy); m.def( "copy_const_tensor", diff --git a/pybind11/tests/test_eigen_tensor.py b/pybind11/tests/test_eigen_tensor.py index 3e7ee6b7f..a2b99d9d7 100644 --- a/pybind11/tests/test_eigen_tensor.py +++ b/pybind11/tests/test_eigen_tensor.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import sys import pytest diff --git a/pybind11/tests/test_embed/CMakeLists.txt b/pybind11/tests/test_embed/CMakeLists.txt index 09a369399..9b539cd42 100644 --- a/pybind11/tests/test_embed/CMakeLists.txt +++ b/pybind11/tests/test_embed/CMakeLists.txt @@ -7,6 +7,13 @@ if("${PYTHON_MODULE_EXTENSION}" MATCHES "pypy" OR "${Python_INTERPRETER_ID}" STR return() endif() +if(TARGET Python::Module AND NOT TARGET Python::Python) + message(STATUS "Skipping embed test since no embed libs found") + add_custom_target(cpptest) # Dummy target since embedding is not supported. + set(_suppress_unused_variable_warning "${DOWNLOAD_CATCH}") + return() +endif() + find_package(Catch 2.13.9) if(CATCH_FOUND) diff --git a/pybind11/tests/test_embed/external_module.cpp b/pybind11/tests/test_embed/external_module.cpp index 5c482fe06..6564ecbef 100644 --- a/pybind11/tests/test_embed/external_module.cpp +++ b/pybind11/tests/test_embed/external_module.cpp @@ -6,7 +6,7 @@ namespace py = pybind11; * modules aren't preserved over a finalize/initialize. */ -PYBIND11_MODULE(external_module, m) { +PYBIND11_MODULE(external_module, m, py::mod_gil_not_used()) { class A { public: explicit A(int value) : v{value} {}; diff --git a/pybind11/tests/test_embed/test_interpreter.py b/pybind11/tests/test_embed/test_interpreter.py index f27944972..424d36dea 100644 --- a/pybind11/tests/test_embed/test_interpreter.py +++ b/pybind11/tests/test_embed/test_interpreter.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import sys from widget_module import Widget diff --git a/pybind11/tests/test_embed/test_trampoline.py b/pybind11/tests/test_embed/test_trampoline.py index 8e14e8ef0..b8ff3eba2 100644 --- a/pybind11/tests/test_embed/test_trampoline.py +++ b/pybind11/tests/test_embed/test_trampoline.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import trampoline_module diff --git a/pybind11/tests/test_enum.py b/pybind11/tests/test_enum.py index 4e85d29c3..9914b9001 100644 --- a/pybind11/tests/test_enum.py +++ b/pybind11/tests/test_enum.py @@ -1,4 +1,5 @@ # ruff: noqa: SIM201 SIM300 SIM202 +from __future__ import annotations import pytest @@ -60,9 +61,7 @@ Members: ETwo : Docstring for ETwo - EThree : Docstring for EThree""".split( - "\n" - ): + EThree : Docstring for EThree""".split("\n"): assert docstring_line in m.UnscopedEnum.__doc__ # Unscoped enums will accept ==/!= int comparisons @@ -264,3 +263,8 @@ def test_docstring_signatures(): for attr in enum_type.__dict__.values(): # Issue #2623/PR #2637: Add argument names to enum_ methods assert "arg0" not in (attr.__doc__ or "") + + +def test_str_signature(): + for enum_type in [m.ScopedEnum, m.UnscopedEnum]: + assert enum_type.__str__.__doc__.startswith("__str__") diff --git a/pybind11/tests/test_eval.py b/pybind11/tests/test_eval.py index 51b6b796b..45b68ece7 100644 --- a/pybind11/tests/test_eval.py +++ b/pybind11/tests/test_eval.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import os import pytest diff --git a/pybind11/tests/test_eval_call.py b/pybind11/tests/test_eval_call.py index fd1da2a5c..a677249d4 100644 --- a/pybind11/tests/test_eval_call.py +++ b/pybind11/tests/test_eval_call.py @@ -1,4 +1,5 @@ # This file is called from 'test_eval.py' +from __future__ import annotations if "call_test2" in locals(): call_test2(y) # noqa: F821 undefined name diff --git a/pybind11/tests/test_exceptions.cpp b/pybind11/tests/test_exceptions.cpp index 854c7e6f7..c1d05bb24 100644 --- a/pybind11/tests/test_exceptions.cpp +++ b/pybind11/tests/test_exceptions.cpp @@ -6,6 +6,8 @@ All rights reserved. Use of this source code is governed by a BSD-style license that can be found in the LICENSE file. */ +#include + #include "test_exceptions.h" #include "local_bindings.h" @@ -25,6 +27,10 @@ private: std::string message = ""; }; +class MyExceptionUseDeprecatedOperatorCall : public MyException { + using MyException::MyException; +}; + // A type that should be translated to a standard Python exception class MyException2 : public std::exception { public: @@ -109,8 +115,10 @@ TEST_SUBMODULE(exceptions, m) { m.def("throw_std_exception", []() { throw std::runtime_error("This exception was intentionally thrown."); }); - // make a new custom exception and use it as a translation target - static py::exception ex(m, "MyException"); + // PLEASE KEEP IN SYNC with docs/advanced/exceptions.rst + PYBIND11_CONSTINIT static py::gil_safe_call_once_and_store ex_storage; + ex_storage.call_once_and_store_result( + [&]() { return py::exception(m, "MyException"); }); py::register_exception_translator([](std::exception_ptr p) { try { if (p) { @@ -118,7 +126,32 @@ TEST_SUBMODULE(exceptions, m) { } } catch (const MyException &e) { // Set MyException as the active python error - ex(e.what()); + py::set_error(ex_storage.get_stored(), e.what()); + } + }); + + // Same as above, but using the deprecated `py::exception<>::operator()` + // We want to be sure it still works, until it's removed. + static const auto *const exd = new py::exception( + m, "MyExceptionUseDeprecatedOperatorCall"); + py::register_exception_translator([](std::exception_ptr p) { + try { + if (p) { + std::rethrow_exception(p); + } + } catch (const MyExceptionUseDeprecatedOperatorCall &e) { +#if defined(__INTEL_COMPILER) || defined(__NVCOMPILER) + // It is not worth the trouble dealing with warning suppressions for these compilers. + // Falling back to the recommended approach to keep the test code simple. + py::set_error(*exd, e.what()); +#else + PYBIND11_WARNING_PUSH + PYBIND11_WARNING_DISABLE_CLANG("-Wdeprecated-declarations") + PYBIND11_WARNING_DISABLE_GCC("-Wdeprecated-declarations") + PYBIND11_WARNING_DISABLE_MSVC(4996) + (*exd)(e.what()); + PYBIND11_WARNING_POP +#endif } }); @@ -132,7 +165,7 @@ TEST_SUBMODULE(exceptions, m) { } } catch (const MyException2 &e) { // Translate this exception to a standard RuntimeError - PyErr_SetString(PyExc_RuntimeError, e.what()); + py::set_error(PyExc_RuntimeError, e.what()); } }); @@ -162,11 +195,16 @@ TEST_SUBMODULE(exceptions, m) { std::rethrow_exception(p); } } catch (const MyException6 &e) { - PyErr_SetString(PyExc_RuntimeError, e.what()); + py::set_error(PyExc_RuntimeError, e.what()); } }); - m.def("throws1", []() { throw MyException("this error should go to a custom type"); }); + m.def("throws1", + []() { throw MyException("this error should go to py::exception"); }); + m.def("throws1d", []() { + throw MyExceptionUseDeprecatedOperatorCall( + "this error should go to py::exception"); + }); m.def("throws2", []() { throw MyException2("this error should go to a standard Python exception"); }); m.def("throws3", []() { throw MyException3("this error cannot be translated"); }); @@ -222,7 +260,7 @@ TEST_SUBMODULE(exceptions, m) { m.def("throw_already_set", [](bool err) { if (err) { - PyErr_SetString(PyExc_ValueError, "foo"); + py::set_error(PyExc_ValueError, "foo"); } try { throw py::error_already_set(); @@ -238,7 +276,7 @@ TEST_SUBMODULE(exceptions, m) { } PyErr_Clear(); if (err) { - PyErr_SetString(PyExc_ValueError, "foo"); + py::set_error(PyExc_ValueError, "foo"); } throw py::error_already_set(); }); @@ -247,7 +285,7 @@ TEST_SUBMODULE(exceptions, m) { bool retval = false; try { PythonCallInDestructor set_dict_in_destructor(d); - PyErr_SetString(PyExc_ValueError, "foo"); + py::set_error(PyExc_ValueError, "foo"); throw py::error_already_set(); } catch (const py::error_already_set &) { retval = true; @@ -282,14 +320,14 @@ TEST_SUBMODULE(exceptions, m) { m.def("throw_should_be_translated_to_key_error", []() { throw shared_exception(); }); m.def("raise_from", []() { - PyErr_SetString(PyExc_ValueError, "inner"); + py::set_error(PyExc_ValueError, "inner"); py::raise_from(PyExc_ValueError, "outer"); throw py::error_already_set(); }); m.def("raise_from_already_set", []() { try { - PyErr_SetString(PyExc_ValueError, "inner"); + py::set_error(PyExc_ValueError, "inner"); throw py::error_already_set(); } catch (py::error_already_set &e) { py::raise_from(e, PyExc_ValueError, "outer"); @@ -306,7 +344,7 @@ TEST_SUBMODULE(exceptions, m) { }); m.def("error_already_set_what", [](const py::object &exc_type, const py::object &exc_value) { - PyErr_SetObject(exc_type.ptr(), exc_value.ptr()); + py::set_error(exc_type, exc_value); std::string what = py::error_already_set().what(); bool py_err_set_after_what = (PyErr_Occurred() != nullptr); PyErr_Clear(); @@ -321,7 +359,7 @@ TEST_SUBMODULE(exceptions, m) { }); m.def("test_error_already_set_double_restore", [](bool dry_run) { - PyErr_SetString(PyExc_ValueError, "Random error."); + py::set_error(PyExc_ValueError, "Random error."); py::error_already_set e; e.restore(); PyErr_Clear(); @@ -344,4 +382,7 @@ TEST_SUBMODULE(exceptions, m) { // function returns None instead of int, should give a useful error message fn().cast(); }); + + // m.def("pass_exception_void", [](const py::exception&) {}); // Does not compile. + m.def("return_exception_void", []() { return py::exception(); }); } diff --git a/pybind11/tests/test_exceptions.h b/pybind11/tests/test_exceptions.h index 03684b89f..2eaa3d3d1 100644 --- a/pybind11/tests/test_exceptions.h +++ b/pybind11/tests/test_exceptions.h @@ -9,5 +9,5 @@ class PYBIND11_EXPORT_EXCEPTION shared_exception : public pybind11::builtin_exce public: using builtin_exception::builtin_exception; explicit shared_exception() : shared_exception("") {} - void set_error() const override { PyErr_SetString(PyExc_RuntimeError, what()); } + void set_error() const override { py::set_error(PyExc_RuntimeError, what()); } }; diff --git a/pybind11/tests/test_exceptions.py b/pybind11/tests/test_exceptions.py index ccac4536d..b33997eee 100644 --- a/pybind11/tests/test_exceptions.py +++ b/pybind11/tests/test_exceptions.py @@ -1,10 +1,12 @@ +from __future__ import annotations + import sys import pytest import env import pybind11_cross_module_tests as cm -import pybind11_tests # noqa: F401 +import pybind11_tests from pybind11_tests import exceptions as m @@ -139,7 +141,15 @@ def test_custom(msg): # Can we catch a MyException? with pytest.raises(m.MyException) as excinfo: m.throws1() - assert msg(excinfo.value) == "this error should go to a custom type" + assert msg(excinfo.value) == "this error should go to py::exception" + + # Can we catch a MyExceptionUseDeprecatedOperatorCall? + with pytest.raises(m.MyExceptionUseDeprecatedOperatorCall) as excinfo: + m.throws1d() + assert ( + msg(excinfo.value) + == "this error should go to py::exception" + ) # Can we translate to standard Python exceptions? with pytest.raises(RuntimeError) as excinfo: @@ -240,6 +250,11 @@ def test_nested_throws(capture): assert str(excinfo.value) == "this is a helper-defined translated exception" +# TODO: Investigate this crash, see pybind/pybind11#5062 for background +@pytest.mark.skipif( + sys.platform.startswith("win32") and "Clang" in pybind11_tests.compiler_info, + reason="Started segfaulting February 2024", +) def test_throw_nested_exception(): with pytest.raises(RuntimeError) as excinfo: m.throw_nested_exception() @@ -411,3 +426,9 @@ def test_fn_cast_int_exception(): assert str(excinfo.value).startswith( "Unable to cast Python instance of type to C++ type" ) + + +def test_return_exception_void(): + with pytest.raises(TypeError) as excinfo: + m.return_exception_void() + assert "Exception" in str(excinfo.value) diff --git a/pybind11/tests/test_factory_constructors.py b/pybind11/tests/test_factory_constructors.py index 04df80260..0ddad5e32 100644 --- a/pybind11/tests/test_factory_constructors.py +++ b/pybind11/tests/test_factory_constructors.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import re import pytest @@ -77,7 +79,7 @@ def test_init_factory_signature(msg): 1. m.factory_constructors.TestFactory1(arg0: m.factory_constructors.tag.unique_ptr_tag, arg1: int) 2. m.factory_constructors.TestFactory1(arg0: str) 3. m.factory_constructors.TestFactory1(arg0: m.factory_constructors.tag.pointer_tag) - 4. m.factory_constructors.TestFactory1(arg0: handle, arg1: int, arg2: handle) + 4. m.factory_constructors.TestFactory1(arg0: object, arg1: int, arg2: object) Invoked with: 'invalid', 'constructor', 'arguments' """ @@ -95,7 +97,7 @@ def test_init_factory_signature(msg): 3. __init__(self: m.factory_constructors.TestFactory1, arg0: m.factory_constructors.tag.pointer_tag) -> None - 4. __init__(self: m.factory_constructors.TestFactory1, arg0: handle, arg1: int, arg2: handle) -> None + 4. __init__(self: m.factory_constructors.TestFactory1, arg0: object, arg1: int, arg2: object) -> None """ ) diff --git a/pybind11/tests/test_gil_scoped.py b/pybind11/tests/test_gil_scoped.py index fc8af9b77..a18387684 100644 --- a/pybind11/tests/test_gil_scoped.py +++ b/pybind11/tests/test_gil_scoped.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import multiprocessing import sys import threading diff --git a/pybind11/tests/test_iostream.py b/pybind11/tests/test_iostream.py index d283eb152..f7eeff502 100644 --- a/pybind11/tests/test_iostream.py +++ b/pybind11/tests/test_iostream.py @@ -1,3 +1,5 @@ +from __future__ import annotations + from contextlib import redirect_stderr, redirect_stdout from io import StringIO @@ -34,7 +36,7 @@ def test_captured_large_string(capsys): def test_captured_utf8_2byte_offset0(capsys): - msg = "\u07FF" + msg = "\u07ff" msg = "" + msg * (1024 // len(msg) + 1) m.captured_output_default(msg) @@ -44,7 +46,7 @@ def test_captured_utf8_2byte_offset0(capsys): def test_captured_utf8_2byte_offset1(capsys): - msg = "\u07FF" + msg = "\u07ff" msg = "1" + msg * (1024 // len(msg) + 1) m.captured_output_default(msg) @@ -54,7 +56,7 @@ def test_captured_utf8_2byte_offset1(capsys): def test_captured_utf8_3byte_offset0(capsys): - msg = "\uFFFF" + msg = "\uffff" msg = "" + msg * (1024 // len(msg) + 1) m.captured_output_default(msg) @@ -64,7 +66,7 @@ def test_captured_utf8_3byte_offset0(capsys): def test_captured_utf8_3byte_offset1(capsys): - msg = "\uFFFF" + msg = "\uffff" msg = "1" + msg * (1024 // len(msg) + 1) m.captured_output_default(msg) @@ -74,7 +76,7 @@ def test_captured_utf8_3byte_offset1(capsys): def test_captured_utf8_3byte_offset2(capsys): - msg = "\uFFFF" + msg = "\uffff" msg = "12" + msg * (1024 // len(msg) + 1) m.captured_output_default(msg) @@ -84,7 +86,7 @@ def test_captured_utf8_3byte_offset2(capsys): def test_captured_utf8_4byte_offset0(capsys): - msg = "\U0010FFFF" + msg = "\U0010ffff" msg = "" + msg * (1024 // len(msg) + 1) m.captured_output_default(msg) @@ -94,7 +96,7 @@ def test_captured_utf8_4byte_offset0(capsys): def test_captured_utf8_4byte_offset1(capsys): - msg = "\U0010FFFF" + msg = "\U0010ffff" msg = "1" + msg * (1024 // len(msg) + 1) m.captured_output_default(msg) @@ -104,7 +106,7 @@ def test_captured_utf8_4byte_offset1(capsys): def test_captured_utf8_4byte_offset2(capsys): - msg = "\U0010FFFF" + msg = "\U0010ffff" msg = "12" + msg * (1024 // len(msg) + 1) m.captured_output_default(msg) @@ -114,7 +116,7 @@ def test_captured_utf8_4byte_offset2(capsys): def test_captured_utf8_4byte_offset3(capsys): - msg = "\U0010FFFF" + msg = "\U0010ffff" msg = "123" + msg * (1024 // len(msg) + 1) m.captured_output_default(msg) diff --git a/pybind11/tests/test_kwargs_and_defaults.cpp b/pybind11/tests/test_kwargs_and_defaults.cpp index 77e72c0c7..bc76ec7c2 100644 --- a/pybind11/tests/test_kwargs_and_defaults.cpp +++ b/pybind11/tests/test_kwargs_and_defaults.cpp @@ -22,8 +22,7 @@ TEST_SUBMODULE(kwargs_and_defaults, m) { m.def("kw_func0", kw_func); m.def("kw_func1", kw_func, py::arg("x"), py::arg("y")); m.def("kw_func2", kw_func, py::arg("x") = 100, py::arg("y") = 200); - m.def( - "kw_func3", [](const char *) {}, py::arg("data") = std::string("Hello world!")); + m.def("kw_func3", [](const char *) {}, py::arg("data") = std::string("Hello world!")); /* A fancier default argument */ std::vector list{{13, 17}}; @@ -42,6 +41,50 @@ TEST_SUBMODULE(kwargs_and_defaults, m) { m.def("kw_func_udl", kw_func, "x"_a, "y"_a = 300); m.def("kw_func_udl_z", kw_func, "x"_a, "y"_a = 0); + // test line breaks in default argument representation + struct CustomRepr { + std::string repr_string; + + explicit CustomRepr(const std::string &repr) : repr_string(repr) {} + + std::string __repr__() const { return repr_string; } + }; + + py::class_(m, "CustomRepr") + .def(py::init()) + .def("__repr__", &CustomRepr::__repr__); + + m.def( + "kw_lb_func0", + [](const CustomRepr &) {}, + py::arg("custom") = CustomRepr(" array([[A, B], [C, D]]) ")); + m.def( + "kw_lb_func1", + [](const CustomRepr &) {}, + py::arg("custom") = CustomRepr(" array([[A, B],\n[C, D]]) ")); + m.def( + "kw_lb_func2", + [](const CustomRepr &) {}, + py::arg("custom") = CustomRepr("\v\n array([[A, B], [C, D]])")); + m.def( + "kw_lb_func3", + [](const CustomRepr &) {}, + py::arg("custom") = CustomRepr("array([[A, B], [C, D]]) \f\n")); + m.def( + "kw_lb_func4", + [](const CustomRepr &) {}, + py::arg("custom") = CustomRepr("array([[A, B],\n\f\n[C, D]])")); + m.def( + "kw_lb_func5", + [](const CustomRepr &) {}, + py::arg("custom") = CustomRepr("array([[A, B],\r [C, D]])")); + m.def("kw_lb_func6", [](const CustomRepr &) {}, py::arg("custom") = CustomRepr(" \v\t ")); + m.def( + "kw_lb_func7", + [](const std::string &) {}, + py::arg("str_arg") = "First line.\n Second line."); + m.def("kw_lb_func8", [](const CustomRepr &) {}, py::arg("custom") = CustomRepr("")); + // test_args_and_kwargs m.def("args_function", [](py::args args) -> py::tuple { PYBIND11_WARNING_PUSH @@ -107,10 +150,13 @@ TEST_SUBMODULE(kwargs_and_defaults, m) { // test_args_refcount // PyPy needs a garbage collection to get the reference count values to match CPython's behaviour +// PyPy uses the top few bits for REFCNT_FROM_PYPY & REFCNT_FROM_PYPY_LIGHT, so truncate #ifdef PYPY_VERSION # define GC_IF_NEEDED ConstructorStats::gc() +# define REFCNT(x) (int) Py_REFCNT(x) #else # define GC_IF_NEEDED +# define REFCNT(x) Py_REFCNT(x) #endif m.def("arg_refcount_h", [](py::handle h) { GC_IF_NEEDED; @@ -129,7 +175,7 @@ TEST_SUBMODULE(kwargs_and_defaults, m) { py::tuple t(a.size()); for (size_t i = 0; i < a.size(); i++) { // Use raw Python API here to avoid an extra, intermediate incref on the tuple item: - t[i] = (int) Py_REFCNT(PyTuple_GET_ITEM(a.ptr(), static_cast(i))); + t[i] = REFCNT(PyTuple_GET_ITEM(a.ptr(), static_cast(i))); } return t; }); @@ -139,7 +185,7 @@ TEST_SUBMODULE(kwargs_and_defaults, m) { t[0] = o.ref_count(); for (size_t i = 0; i < a.size(); i++) { // Use raw Python API here to avoid an extra, intermediate incref on the tuple item: - t[i + 1] = (int) Py_REFCNT(PyTuple_GET_ITEM(a.ptr(), static_cast(i))); + t[i + 1] = REFCNT(PyTuple_GET_ITEM(a.ptr(), static_cast(i))); } return t; }); @@ -233,11 +279,9 @@ TEST_SUBMODULE(kwargs_and_defaults, m) { // These should fail to compile: #ifdef PYBIND11_NEVER_DEFINED_EVER // argument annotations are required when using kw_only - m.def( - "bad_kw_only1", [](int) {}, py::kw_only()); + m.def("bad_kw_only1", [](int) {}, py::kw_only()); // can't specify both `py::kw_only` and a `py::args` argument - m.def( - "bad_kw_only2", [](int i, py::args) {}, py::kw_only(), "i"_a); + m.def("bad_kw_only2", [](int i, py::args) {}, py::kw_only(), "i"_a); #endif // test_function_signatures (along with most of the above) diff --git a/pybind11/tests/test_kwargs_and_defaults.py b/pybind11/tests/test_kwargs_and_defaults.py index 7174726fc..b9b1a7ea8 100644 --- a/pybind11/tests/test_kwargs_and_defaults.py +++ b/pybind11/tests/test_kwargs_and_defaults.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest from pybind11_tests import kwargs_and_defaults as m @@ -8,7 +10,7 @@ def test_function_signatures(doc): assert doc(m.kw_func1) == "kw_func1(x: int, y: int) -> str" assert doc(m.kw_func2) == "kw_func2(x: int = 100, y: int = 200) -> str" assert doc(m.kw_func3) == "kw_func3(data: str = 'Hello world!') -> None" - assert doc(m.kw_func4) == "kw_func4(myList: List[int] = [13, 17]) -> str" + assert doc(m.kw_func4) == "kw_func4(myList: list[int] = [13, 17]) -> str" assert doc(m.kw_func_udl) == "kw_func_udl(x: int, y: int = 300) -> str" assert doc(m.kw_func_udl_z) == "kw_func_udl_z(x: int, y: int = 0) -> str" assert doc(m.args_function) == "args_function(*args) -> tuple" @@ -23,6 +25,42 @@ def test_function_signatures(doc): doc(m.KWClass.foo1) == "foo1(self: m.kwargs_and_defaults.KWClass, x: int, y: float) -> None" ) + assert ( + doc(m.kw_lb_func0) + == "kw_lb_func0(custom: m.kwargs_and_defaults.CustomRepr = array([[A, B], [C, D]])) -> None" + ) + assert ( + doc(m.kw_lb_func1) + == "kw_lb_func1(custom: m.kwargs_and_defaults.CustomRepr = array([[A, B], [C, D]])) -> None" + ) + assert ( + doc(m.kw_lb_func2) + == "kw_lb_func2(custom: m.kwargs_and_defaults.CustomRepr = array([[A, B], [C, D]])) -> None" + ) + assert ( + doc(m.kw_lb_func3) + == "kw_lb_func3(custom: m.kwargs_and_defaults.CustomRepr = array([[A, B], [C, D]])) -> None" + ) + assert ( + doc(m.kw_lb_func4) + == "kw_lb_func4(custom: m.kwargs_and_defaults.CustomRepr = array([[A, B], [C, D]])) -> None" + ) + assert ( + doc(m.kw_lb_func5) + == "kw_lb_func5(custom: m.kwargs_and_defaults.CustomRepr = array([[A, B], [C, D]])) -> None" + ) + assert ( + doc(m.kw_lb_func6) + == "kw_lb_func6(custom: m.kwargs_and_defaults.CustomRepr = ) -> None" + ) + assert ( + doc(m.kw_lb_func7) + == "kw_lb_func7(str_arg: str = 'First line.\\n Second line.') -> None" + ) + assert ( + doc(m.kw_lb_func8) + == "kw_lb_func8(custom: m.kwargs_and_defaults.CustomRepr = ) -> None" + ) def test_named_arguments(): @@ -345,7 +383,7 @@ def test_args_refcount(): arguments""" refcount = m.arg_refcount_h - myval = 54321 + myval = object() expected = refcount(myval) assert m.arg_refcount_h(myval) == expected assert m.arg_refcount_o(myval) == expected + 1 @@ -384,6 +422,7 @@ def test_args_refcount(): # for the `py::args`; in the previous case, we could simply inc_ref and pass on Python's input # tuple without having to inc_ref the individual elements, but here we can't, hence the extra # refs. - assert m.mixed_args_refcount(myval, myval, myval) == (exp3 + 3, exp3 + 3, exp3 + 3) + exp3_3 = exp3 + 3 + assert m.mixed_args_refcount(myval, myval, myval) == (exp3_3, exp3_3, exp3_3) assert m.class_default_argument() == "" diff --git a/pybind11/tests/test_local_bindings.py b/pybind11/tests/test_local_bindings.py index d64187739..1e83164c7 100644 --- a/pybind11/tests/test_local_bindings.py +++ b/pybind11/tests/test_local_bindings.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest import env # noqa: F401 diff --git a/pybind11/tests/test_methods_and_attributes.cpp b/pybind11/tests/test_methods_and_attributes.cpp index 31d46eb7e..f433847c7 100644 --- a/pybind11/tests/test_methods_and_attributes.cpp +++ b/pybind11/tests/test_methods_and_attributes.cpp @@ -374,8 +374,7 @@ TEST_SUBMODULE(methods_and_attributes, m) { m.def("overload_order", [](const std::string &) { return 1; }); m.def("overload_order", [](const std::string &) { return 2; }); m.def("overload_order", [](int) { return 3; }); - m.def( - "overload_order", [](int) { return 4; }, py::prepend{}); + m.def("overload_order", [](int) { return 4; }, py::prepend{}); #if !defined(PYPY_VERSION) // test_dynamic_attributes diff --git a/pybind11/tests/test_methods_and_attributes.py b/pybind11/tests/test_methods_and_attributes.py index 955a85f67..dfa31f546 100644 --- a/pybind11/tests/test_methods_and_attributes.py +++ b/pybind11/tests/test_methods_and_attributes.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import sys import pytest @@ -232,25 +234,29 @@ def test_no_mixed_overloads(): with pytest.raises(RuntimeError) as excinfo: m.ExampleMandA.add_mixed_overloads1() - assert str( - excinfo.value - ) == "overloading a method with both static and instance methods is not supported; " + ( - "#define PYBIND11_DETAILED_ERROR_MESSAGES or compile in debug mode for more details" - if not detailed_error_messages_enabled - else "error while attempting to bind static method ExampleMandA.overload_mixed1" - "(arg0: float) -> str" + assert ( + str(excinfo.value) + == "overloading a method with both static and instance methods is not supported; " + + ( + "#define PYBIND11_DETAILED_ERROR_MESSAGES or compile in debug mode for more details" + if not detailed_error_messages_enabled + else "error while attempting to bind static method ExampleMandA.overload_mixed1" + "(arg0: float) -> str" + ) ) with pytest.raises(RuntimeError) as excinfo: m.ExampleMandA.add_mixed_overloads2() - assert str( - excinfo.value - ) == "overloading a method with both static and instance methods is not supported; " + ( - "#define PYBIND11_DETAILED_ERROR_MESSAGES or compile in debug mode for more details" - if not detailed_error_messages_enabled - else "error while attempting to bind instance method ExampleMandA.overload_mixed2" - "(self: pybind11_tests.methods_and_attributes.ExampleMandA, arg0: int, arg1: int)" - " -> str" + assert ( + str(excinfo.value) + == "overloading a method with both static and instance methods is not supported; " + + ( + "#define PYBIND11_DETAILED_ERROR_MESSAGES or compile in debug mode for more details" + if not detailed_error_messages_enabled + else "error while attempting to bind instance method ExampleMandA.overload_mixed2" + "(self: pybind11_tests.methods_and_attributes.ExampleMandA, arg0: int, arg1: int)" + " -> str" + ) ) diff --git a/pybind11/tests/test_modules.py b/pybind11/tests/test_modules.py index 2f6d825b7..95835e14e 100644 --- a/pybind11/tests/test_modules.py +++ b/pybind11/tests/test_modules.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import builtins import pytest diff --git a/pybind11/tests/test_multiple_inheritance.py b/pybind11/tests/test_multiple_inheritance.py index 3a1d88d71..d445824b5 100644 --- a/pybind11/tests/test_multiple_inheritance.py +++ b/pybind11/tests/test_multiple_inheritance.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest import env # noqa: F401 diff --git a/pybind11/tests/test_numpy_array.cpp b/pybind11/tests/test_numpy_array.cpp index 8c122a865..c2f754208 100644 --- a/pybind11/tests/test_numpy_array.cpp +++ b/pybind11/tests/test_numpy_array.cpp @@ -287,8 +287,7 @@ TEST_SUBMODULE(numpy_array, sm) { // [workaround(intel)] ICC 20/21 breaks with py::arg().stuff, using py::arg{}.stuff works. // Only accept the exact types: - sm.def( - "overloaded3", [](const py::array_t &) { return "int"; }, py::arg{}.noconvert()); + sm.def("overloaded3", [](const py::array_t &) { return "int"; }, py::arg{}.noconvert()); sm.def( "overloaded3", [](const py::array_t &) { return "double"; }, @@ -444,9 +443,8 @@ TEST_SUBMODULE(numpy_array, sm) { }); // resize to 3D array with each dimension = N - sm.def("array_resize3", [](py::array_t a, size_t N, bool refcheck) { - a.resize({N, N, N}, refcheck); - }); + sm.def("array_resize3", + [](py::array_t a, size_t N, bool refcheck) { a.resize({N, N, N}, refcheck); }); // test_array_create_and_resize // return 2D array with Nrows = Ncols = N @@ -460,9 +458,8 @@ TEST_SUBMODULE(numpy_array, sm) { sm.def("array_view", [](py::array_t a, const std::string &dtype) { return a.view(dtype); }); - sm.def("reshape_initializer_list", [](py::array_t a, size_t N, size_t M, size_t O) { - return a.reshape({N, M, O}); - }); + sm.def("reshape_initializer_list", + [](py::array_t a, size_t N, size_t M, size_t O) { return a.reshape({N, M, O}); }); sm.def("reshape_tuple", [](py::array_t a, const std::vector &new_shape) { return a.reshape(new_shape); }); @@ -471,8 +468,7 @@ TEST_SUBMODULE(numpy_array, sm) { [](const py::array &a) { return a[py::make_tuple(0, py::ellipsis(), 0)]; }); // test_argument_conversions - sm.def( - "accept_double", [](const py::array_t &) {}, py::arg("a")); + sm.def("accept_double", [](const py::array_t &) {}, py::arg("a")); sm.def( "accept_double_forcecast", [](const py::array_t &) {}, @@ -493,8 +489,7 @@ TEST_SUBMODULE(numpy_array, sm) { "accept_double_f_style_forcecast", [](const py::array_t &) {}, py::arg("a")); - sm.def( - "accept_double_noconvert", [](const py::array_t &) {}, "a"_a.noconvert()); + sm.def("accept_double_noconvert", [](const py::array_t &) {}, "a"_a.noconvert()); sm.def( "accept_double_forcecast_noconvert", [](const py::array_t &) {}, diff --git a/pybind11/tests/test_numpy_array.py b/pybind11/tests/test_numpy_array.py index 12e7d17d1..4726a8e73 100644 --- a/pybind11/tests/test_numpy_array.py +++ b/pybind11/tests/test_numpy_array.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest import env # noqa: F401 @@ -198,11 +200,7 @@ def test_wrap(): assert a.flags.f_contiguous == b.flags.f_contiguous assert a.flags.writeable == b.flags.writeable assert a.flags.aligned == b.flags.aligned - # 1.13 supported Python 3.6 - if tuple(int(x) for x in np.__version__.split(".")[:2]) >= (1, 14): - assert a.flags.writebackifcopy == b.flags.writebackifcopy - else: - assert a.flags.updateifcopy == b.flags.updateifcopy + assert a.flags.writebackifcopy == b.flags.writebackifcopy assert np.all(a == b) assert not b.flags.owndata assert b.base is base @@ -298,7 +296,7 @@ def test_constructors(): results = m.converting_constructors([1, 2, 3]) for a in results.values(): np.testing.assert_array_equal(a, [1, 2, 3]) - assert results["array"].dtype == np.int_ + assert results["array"].dtype == np.dtype(int) assert results["array_t"].dtype == np.int32 assert results["array_t"].dtype == np.float64 @@ -536,7 +534,12 @@ def test_format_descriptors_for_floating_point_types(test_func): @pytest.mark.parametrize("contiguity", [None, "C", "F"]) @pytest.mark.parametrize("noconvert", [False, True]) @pytest.mark.filterwarnings( - "ignore:Casting complex values to real discards the imaginary part:numpy.ComplexWarning" + "ignore:Casting complex values to real discards the imaginary part:" + + ( + "numpy.exceptions.ComplexWarning" + if hasattr(np, "exceptions") + else "numpy.ComplexWarning" + ) ) def test_argument_conversions(forcecast, contiguity, noconvert): function_name = "accept_double" @@ -583,7 +586,8 @@ def test_argument_conversions(forcecast, contiguity, noconvert): def test_dtype_refcount_leak(): from sys import getrefcount - dtype = np.dtype(np.float_) + # Was np.float_ but that alias for float64 was removed in NumPy 2. + dtype = np.dtype(np.float64) a = np.array([1], dtype=dtype) before = getrefcount(dtype) m.ndim(a) diff --git a/pybind11/tests/test_numpy_dtypes.cpp b/pybind11/tests/test_numpy_dtypes.cpp index 6654f9ed8..ed77ec024 100644 --- a/pybind11/tests/test_numpy_dtypes.cpp +++ b/pybind11/tests/test_numpy_dtypes.cpp @@ -157,7 +157,7 @@ py::array mkarray_via_buffer(size_t n) { do { \ (s).bool_ = (i) % 2 != 0; \ (s).uint_ = (uint32_t) (i); \ - (s).float_ = (float) (i) *1.5f; \ + (s).float_ = (float) (i) * 1.5f; \ (s).ldbl_ = (long double) (i) * -2.5L; \ } while (0) @@ -348,7 +348,7 @@ TEST_SUBMODULE(numpy_dtypes, m) { // is not a POD type struct NotPOD { std::string v; - NotPOD() : v("hi"){}; + NotPOD() : v("hi") {}; }; PYBIND11_NUMPY_DTYPE(NotPOD, v); #endif @@ -405,10 +405,35 @@ TEST_SUBMODULE(numpy_dtypes, m) { }); // test_dtype + // Below we use `L` for unsigned long as unfortunately the only name that + // works reliably on Both NumPy 2.x and old NumPy 1.x. std::vector dtype_names{ - "byte", "short", "intc", "int_", "longlong", "ubyte", "ushort", - "uintc", "uint", "ulonglong", "half", "single", "double", "longdouble", - "csingle", "cdouble", "clongdouble", "bool_", "datetime64", "timedelta64", "object_"}; + "byte", + "short", + "intc", + "long", + "longlong", + "ubyte", + "ushort", + "uintc", + "L", + "ulonglong", + "half", + "single", + "double", + "longdouble", + "csingle", + "cdouble", + "clongdouble", + "bool_", + "datetime64", + "timedelta64", + "object_", + // platform dependent aliases (int_ and uint are also NumPy version dependent on windows) + "int_", + "uint", + "intp", + "uintp"}; m.def("print_dtypes", []() { py::list l; diff --git a/pybind11/tests/test_numpy_dtypes.py b/pybind11/tests/test_numpy_dtypes.py index d10457eeb..8ae239ed8 100644 --- a/pybind11/tests/test_numpy_dtypes.py +++ b/pybind11/tests/test_numpy_dtypes.py @@ -1,8 +1,11 @@ +from __future__ import annotations + import re import pytest import env # noqa: F401 +from pybind11_tests import PYBIND11_NUMPY_1_ONLY from pybind11_tests import numpy_dtypes as m np = pytest.importorskip("numpy") @@ -76,9 +79,7 @@ def partial_nested_fmt(): partial_size = partial_ld_off + ld.itemsize partial_end_padding = partial_size % np.dtype("uint64").alignment partial_nested_size = partial_nested_off * 2 + partial_size + partial_end_padding - return "{{'names':['a'],'formats':[{}],'offsets':[{}],'itemsize':{}}}".format( - partial_dtype_fmt(), partial_nested_off, partial_nested_size - ) + return f"{{'names':['a'],'formats':[{partial_dtype_fmt()}],'offsets':[{partial_nested_off}],'itemsize':{partial_nested_size}}}" def assert_equal(actual, expected_data, expected_dtype): @@ -172,13 +173,20 @@ def test_dtype(simple_dtype): np.zeros(1, m.trailing_padding_dtype()) ) - expected_chars = "bhilqBHILQefdgFDG?MmO" - assert m.test_dtype_kind() == list("iiiiiuuuuuffffcccbMmO") + expected_chars = list("bhilqBHILQefdgFDG?MmO") + # Note that int_ and uint size and mapping is NumPy version dependent: + expected_chars += [np.dtype(_).char for _ in ("int_", "uint", "intp", "uintp")] + assert m.test_dtype_kind() == list("iiiiiuuuuuffffcccbMmOiuiu") assert m.test_dtype_char_() == list(expected_chars) assert m.test_dtype_num() == [np.dtype(ch).num for ch in expected_chars] assert m.test_dtype_byteorder() == [np.dtype(ch).byteorder for ch in expected_chars] assert m.test_dtype_alignment() == [np.dtype(ch).alignment for ch in expected_chars] - assert m.test_dtype_flags() == [chr(np.dtype(ch).flags) for ch in expected_chars] + if not PYBIND11_NUMPY_1_ONLY: + assert m.test_dtype_flags() == [np.dtype(ch).flags for ch in expected_chars] + else: + assert m.test_dtype_flags() == [ + chr(np.dtype(ch).flags) for ch in expected_chars + ] def test_recarray(simple_dtype, packed_dtype): diff --git a/pybind11/tests/test_numpy_vectorize.py b/pybind11/tests/test_numpy_vectorize.py index f1e8b6254..ce38d72d9 100644 --- a/pybind11/tests/test_numpy_vectorize.py +++ b/pybind11/tests/test_numpy_vectorize.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest from pybind11_tests import numpy_vectorize as m diff --git a/pybind11/tests/test_opaque_types.py b/pybind11/tests/test_opaque_types.py index 5d4f2a1bf..342086436 100644 --- a/pybind11/tests/test_opaque_types.py +++ b/pybind11/tests/test_opaque_types.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest from pybind11_tests import ConstructorStats, UserType diff --git a/pybind11/tests/test_operator_overloading.py b/pybind11/tests/test_operator_overloading.py index 9fde305a0..b6760902d 100644 --- a/pybind11/tests/test_operator_overloading.py +++ b/pybind11/tests/test_operator_overloading.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest from pybind11_tests import ConstructorStats diff --git a/pybind11/tests/test_pickling.py b/pybind11/tests/test_pickling.py index 12361a661..ad67a1df9 100644 --- a/pybind11/tests/test_pickling.py +++ b/pybind11/tests/test_pickling.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pickle import re diff --git a/pybind11/tests/test_python_multiple_inheritance.cpp b/pybind11/tests/test_python_multiple_inheritance.cpp new file mode 100644 index 000000000..689917158 --- /dev/null +++ b/pybind11/tests/test_python_multiple_inheritance.cpp @@ -0,0 +1,45 @@ +#include "pybind11_tests.h" + +namespace test_python_multiple_inheritance { + +// Copied from: +// https://github.com/google/clif/blob/5718e4d0807fd3b6a8187dde140069120b81ecef/clif/testing/python_multiple_inheritance.h + +struct CppBase { + explicit CppBase(int value) : base_value(value) {} + int get_base_value() const { return base_value; } + void reset_base_value(int new_value) { base_value = new_value; } + +private: + int base_value; +}; + +struct CppDrvd : CppBase { + explicit CppDrvd(int value) : CppBase(value), drvd_value(value * 3) {} + int get_drvd_value() const { return drvd_value; } + void reset_drvd_value(int new_value) { drvd_value = new_value; } + + int get_base_value_from_drvd() const { return get_base_value(); } + void reset_base_value_from_drvd(int new_value) { reset_base_value(new_value); } + +private: + int drvd_value; +}; + +} // namespace test_python_multiple_inheritance + +TEST_SUBMODULE(python_multiple_inheritance, m) { + using namespace test_python_multiple_inheritance; + + py::class_(m, "CppBase") + .def(py::init()) + .def("get_base_value", &CppBase::get_base_value) + .def("reset_base_value", &CppBase::reset_base_value); + + py::class_(m, "CppDrvd") + .def(py::init()) + .def("get_drvd_value", &CppDrvd::get_drvd_value) + .def("reset_drvd_value", &CppDrvd::reset_drvd_value) + .def("get_base_value_from_drvd", &CppDrvd::get_base_value_from_drvd) + .def("reset_base_value_from_drvd", &CppDrvd::reset_base_value_from_drvd); +} diff --git a/pybind11/tests/test_python_multiple_inheritance.py b/pybind11/tests/test_python_multiple_inheritance.py new file mode 100644 index 000000000..12216283a --- /dev/null +++ b/pybind11/tests/test_python_multiple_inheritance.py @@ -0,0 +1,36 @@ +# Adapted from: +# https://github.com/google/clif/blob/5718e4d0807fd3b6a8187dde140069120b81ecef/clif/testing/python/python_multiple_inheritance_test.py +from __future__ import annotations + +from pybind11_tests import python_multiple_inheritance as m + + +class PC(m.CppBase): + pass + + +class PPCC(PC, m.CppDrvd): + pass + + +def test_PC(): + d = PC(11) + assert d.get_base_value() == 11 + d.reset_base_value(13) + assert d.get_base_value() == 13 + + +def test_PPCC(): + d = PPCC(11) + assert d.get_drvd_value() == 33 + d.reset_drvd_value(55) + assert d.get_drvd_value() == 55 + + assert d.get_base_value() == 11 + assert d.get_base_value_from_drvd() == 11 + d.reset_base_value(20) + assert d.get_base_value() == 20 + assert d.get_base_value_from_drvd() == 20 + d.reset_base_value_from_drvd(30) + assert d.get_base_value() == 30 + assert d.get_base_value_from_drvd() == 30 diff --git a/pybind11/tests/test_pytypes.cpp b/pybind11/tests/test_pytypes.cpp index b4ee64289..7c30978ce 100644 --- a/pybind11/tests/test_pytypes.cpp +++ b/pybind11/tests/test_pytypes.cpp @@ -7,6 +7,8 @@ BSD-style license that can be found in the LICENSE file. */ +#include + #include "pybind11_tests.h" #include @@ -23,7 +25,7 @@ PyObject *conv(PyObject *o) { ret = PyFloat_FromDouble(v); } } else { - PyErr_SetString(PyExc_TypeError, "Unexpected type"); + py::set_error(PyExc_TypeError, "Unexpected type"); } return ret; } @@ -39,6 +41,15 @@ class float_ : public py::object { }; } // namespace external +namespace pybind11 { +namespace detail { +template <> +struct handle_type_name { + static constexpr auto name = const_name("float"); +}; +} // namespace detail +} // namespace pybind11 + namespace implicit_conversion_from_0_to_handle { // Uncomment to trigger compiler error. Note: Before PR #4008 this used to compile successfully. // void expected_to_trigger_compiler_error() { py::handle(0); } @@ -98,6 +109,26 @@ void m_defs(py::module_ &m) { } // namespace handle_from_move_only_type_with_operator_PyObject +#if defined(__cpp_nontype_template_parameter_class) +namespace literals { +enum Color { RED = 0, BLUE = 1 }; + +typedef py::typing::Literal<"26", + "0x1A", + "\"hello world\"", + "b\"hello world\"", + "u\"hello world\"", + "True", + "Color.RED", + "None"> + LiteralFoo; +} // namespace literals +namespace typevar { +typedef py::typing::TypeVar<"T"> TypeVarT; +typedef py::typing::TypeVar<"V"> TypeVarV; +} // namespace typevar +#endif + TEST_SUBMODULE(pytypes, m) { m.def("obj_class_name", [](py::handle obj) { return py::detail::obj_class_name(obj.ptr()); }); @@ -124,6 +155,7 @@ TEST_SUBMODULE(pytypes, m) { m.def("list_size_t", []() { return py::list{(py::size_t) 0}; }); m.def("list_insert_ssize_t", [](py::list *l) { return l->insert((py::ssize_t) 1, 83); }); m.def("list_insert_size_t", [](py::list *l) { return l->insert((py::size_t) 3, 57); }); + m.def("list_clear", [](py::list *l) { l->clear(); }); m.def("get_list", []() { py::list list; list.append("value"); @@ -660,8 +692,8 @@ TEST_SUBMODULE(pytypes, m) { // This is "most correct" and enforced on these platforms. # define PYBIND11_AUTO_IT auto it #else -// This works on many platforms and is (unfortunately) reflective of existing user code. -// NOLINTNEXTLINE(bugprone-macro-parentheses) + // This works on many platforms and is (unfortunately) reflective of existing user code. + // NOLINTNEXTLINE(bugprone-macro-parentheses) # define PYBIND11_AUTO_IT auto &it #endif @@ -820,4 +852,75 @@ TEST_SUBMODULE(pytypes, m) { a >>= b; return a; }); + + m.def("annotate_tuple_float_str", [](const py::typing::Tuple &) {}); + m.def("annotate_tuple_empty", [](const py::typing::Tuple<> &) {}); + m.def("annotate_tuple_variable_length", + [](const py::typing::Tuple &) {}); + m.def("annotate_dict_str_int", [](const py::typing::Dict &) {}); + m.def("annotate_list_int", [](const py::typing::List &) {}); + m.def("annotate_set_str", [](const py::typing::Set &) {}); + m.def("annotate_iterable_str", [](const py::typing::Iterable &) {}); + m.def("annotate_iterator_int", [](const py::typing::Iterator &) {}); + m.def("annotate_fn", + [](const py::typing::Callable, py::str)> &) {}); + + m.def("annotate_fn_only_return", [](const py::typing::Callable &) {}); + m.def("annotate_type", [](const py::typing::Type &t) -> py::type { return t; }); + + m.def("annotate_union", + [](py::typing::List> l, + py::str a, + py::int_ b, + py::object c) -> py::typing::List> { + l.append(a); + l.append(b); + l.append(c); + return l; + }); + + m.def("union_typing_only", + [](py::typing::List> &l) + -> py::typing::List> { return l; }); + + m.def("annotate_union_to_object", + [](py::typing::Union &o) -> py::object { return o; }); + + m.def("annotate_optional", + [](py::list &list) -> py::typing::List> { + list.append(py::str("hi")); + list.append(py::none()); + return list; + }); + + m.def("annotate_type_guard", [](py::object &o) -> py::typing::TypeGuard { + return py::isinstance(o); + }); + m.def("annotate_type_is", + [](py::object &o) -> py::typing::TypeIs { return py::isinstance(o); }); + + m.def("annotate_no_return", []() -> py::typing::NoReturn { throw 0; }); + m.def("annotate_never", []() -> py::typing::Never { throw 0; }); + + m.def("annotate_optional_to_object", + [](py::typing::Optional &o) -> py::object { return o; }); + +#if defined(__cpp_nontype_template_parameter_class) + py::enum_(m, "Color") + .value("RED", literals::Color::RED) + .value("BLUE", literals::Color::BLUE); + + m.def("annotate_literal", [](literals::LiteralFoo &o) -> py::object { return o; }); + m.def("annotate_generic_containers", + [](const py::typing::List &l) -> py::typing::List { + return l; + }); + + m.def("annotate_listT_to_T", + [](const py::typing::List &l) -> typevar::TypeVarT { return l[0]; }); + m.def("annotate_object_to_T", [](const py::object &o) -> typevar::TypeVarT { return o; }); + m.attr("if_defined__cpp_nontype_template_parameter_class") = true; +#else + m.attr("if_defined__cpp_nontype_template_parameter_class") = false; +#endif } diff --git a/pybind11/tests/test_pytypes.py b/pybind11/tests/test_pytypes.py index eda7a20a9..30931e0b9 100644 --- a/pybind11/tests/test_pytypes.py +++ b/pybind11/tests/test_pytypes.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import contextlib import sys import types @@ -65,6 +67,8 @@ def test_list(capture, doc): assert lins == [1, 83, 2] m.list_insert_size_t(lins) assert lins == [1, 83, 2, 57] + m.list_clear(lins) + assert lins == [] with capture: lst = m.get_list() @@ -121,7 +125,7 @@ def test_set(capture, doc): assert m.anyset_contains({"foo"}, "foo") assert doc(m.get_set) == "get_set() -> set" - assert doc(m.print_anyset) == "print_anyset(arg0: anyset) -> None" + assert doc(m.print_anyset) == "print_anyset(arg0: Union[set, frozenset]) -> None" def test_frozenset(capture, doc): @@ -631,7 +635,8 @@ def test_memoryview(method, args, fmt, expected_view): ], ) def test_memoryview_refcount(method): - buf = b"\x0a\x0b\x0c\x0d" + # Avoiding a literal to avoid an immortal object in free-threaded builds + buf = "\x0a\x0b\x0c\x0d".encode("ascii") ref_before = sys.getrefcount(buf) view = method(buf) ref_after = sys.getrefcount(buf) @@ -896,3 +901,150 @@ def test_inplace_lshift(a, b): def test_inplace_rshift(a, b): expected = a >> b assert m.inplace_rshift(a, b) == expected + + +def test_tuple_nonempty_annotations(doc): + assert ( + doc(m.annotate_tuple_float_str) + == "annotate_tuple_float_str(arg0: tuple[float, str]) -> None" + ) + + +def test_tuple_empty_annotations(doc): + assert ( + doc(m.annotate_tuple_empty) == "annotate_tuple_empty(arg0: tuple[()]) -> None" + ) + + +def test_tuple_variable_length_annotations(doc): + assert ( + doc(m.annotate_tuple_variable_length) + == "annotate_tuple_variable_length(arg0: tuple[float, ...]) -> None" + ) + + +def test_dict_annotations(doc): + assert ( + doc(m.annotate_dict_str_int) + == "annotate_dict_str_int(arg0: dict[str, int]) -> None" + ) + + +def test_list_annotations(doc): + assert doc(m.annotate_list_int) == "annotate_list_int(arg0: list[int]) -> None" + + +def test_set_annotations(doc): + assert doc(m.annotate_set_str) == "annotate_set_str(arg0: set[str]) -> None" + + +def test_iterable_annotations(doc): + assert ( + doc(m.annotate_iterable_str) + == "annotate_iterable_str(arg0: Iterable[str]) -> None" + ) + + +def test_iterator_annotations(doc): + assert ( + doc(m.annotate_iterator_int) + == "annotate_iterator_int(arg0: Iterator[int]) -> None" + ) + + +def test_fn_annotations(doc): + assert ( + doc(m.annotate_fn) + == "annotate_fn(arg0: Callable[[list[str], str], int]) -> None" + ) + + +def test_fn_return_only(doc): + assert ( + doc(m.annotate_fn_only_return) + == "annotate_fn_only_return(arg0: Callable[..., int]) -> None" + ) + + +def test_type_annotation(doc): + assert doc(m.annotate_type) == "annotate_type(arg0: type[int]) -> type" + + +def test_union_annotations(doc): + assert ( + doc(m.annotate_union) + == "annotate_union(arg0: list[Union[str, int, object]], arg1: str, arg2: int, arg3: object) -> list[Union[str, int, object]]" + ) + + +def test_union_typing_only(doc): + assert ( + doc(m.union_typing_only) + == "union_typing_only(arg0: list[Union[str]]) -> list[Union[int]]" + ) + + +def test_union_object_annotations(doc): + assert ( + doc(m.annotate_union_to_object) + == "annotate_union_to_object(arg0: Union[int, str]) -> object" + ) + + +def test_optional_annotations(doc): + assert ( + doc(m.annotate_optional) + == "annotate_optional(arg0: list) -> list[Optional[str]]" + ) + + +def test_type_guard_annotations(doc): + assert ( + doc(m.annotate_type_guard) + == "annotate_type_guard(arg0: object) -> TypeGuard[str]" + ) + + +def test_type_is_annotations(doc): + assert doc(m.annotate_type_is) == "annotate_type_is(arg0: object) -> TypeIs[str]" + + +def test_no_return_annotation(doc): + assert doc(m.annotate_no_return) == "annotate_no_return() -> NoReturn" + + +def test_never_annotation(doc): + assert doc(m.annotate_never) == "annotate_never() -> Never" + + +def test_optional_object_annotations(doc): + assert ( + doc(m.annotate_optional_to_object) + == "annotate_optional_to_object(arg0: Optional[int]) -> object" + ) + + +@pytest.mark.skipif( + not m.if_defined__cpp_nontype_template_parameter_class, + reason="C++20 feature not available.", +) +def test_literal(doc): + assert ( + doc(m.annotate_literal) + == 'annotate_literal(arg0: Literal[26, 0x1A, "hello world", b"hello world", u"hello world", True, Color.RED, None]) -> object' + ) + + +@pytest.mark.skipif( + not m.if_defined__cpp_nontype_template_parameter_class, + reason="C++20 feature not available.", +) +def test_typevar(doc): + assert ( + doc(m.annotate_generic_containers) + == "annotate_generic_containers(arg0: list[T]) -> list[V]" + ) + + assert doc(m.annotate_listT_to_T) == "annotate_listT_to_T(arg0: list[T]) -> T" + + assert doc(m.annotate_object_to_T) == "annotate_object_to_T(arg0: object) -> T" diff --git a/pybind11/tests/test_sequences_and_iterators.cpp b/pybind11/tests/test_sequences_and_iterators.cpp index 1de65edbf..4a1d37f4d 100644 --- a/pybind11/tests/test_sequences_and_iterators.cpp +++ b/pybind11/tests/test_sequences_and_iterators.cpp @@ -28,6 +28,13 @@ class NonZeroIterator { public: explicit NonZeroIterator(const T *ptr) : ptr_(ptr) {} + + // Make the iterator non-copyable and movable + NonZeroIterator(const NonZeroIterator &) = delete; + NonZeroIterator(NonZeroIterator &&) noexcept = default; + NonZeroIterator &operator=(const NonZeroIterator &) = delete; + NonZeroIterator &operator=(NonZeroIterator &&) noexcept = default; + const T &operator*() const { return *ptr_; } NonZeroIterator &operator++() { ++ptr_; @@ -78,6 +85,7 @@ private: int value_; }; using NonCopyableIntPair = std::pair; + PYBIND11_MAKE_OPAQUE(std::vector); PYBIND11_MAKE_OPAQUE(std::vector); @@ -375,6 +383,17 @@ TEST_SUBMODULE(sequences_and_iterators, m) { private: std::vector> data_; }; + + { + // #4383 : Make sure `py::make_*iterator` functions work with move-only iterators + using iterator_t = NonZeroIterator>; + + static_assert(std::is_move_assignable::value, ""); + static_assert(std::is_move_constructible::value, ""); + static_assert(!std::is_copy_assignable::value, ""); + static_assert(!std::is_copy_constructible::value, ""); + } + py::class_(m, "IntPairs") .def(py::init>>()) .def( diff --git a/pybind11/tests/test_sequences_and_iterators.py b/pybind11/tests/test_sequences_and_iterators.py index dc129f2bf..f609f553d 100644 --- a/pybind11/tests/test_sequences_and_iterators.py +++ b/pybind11/tests/test_sequences_and_iterators.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest from pytest import approx # noqa: PT013 @@ -58,6 +60,15 @@ def test_generalized_iterators_simple(): assert list(m.IntPairs([(1, 2), (3, 4), (0, 5)]).simple_values()) == [2, 4, 5] +def test_iterator_doc_annotations(): + assert m.IntPairs.nonref.__doc__.endswith("-> Iterator[tuple[int, int]]\n") + assert m.IntPairs.nonref_keys.__doc__.endswith("-> Iterator[int]\n") + assert m.IntPairs.nonref_values.__doc__.endswith("-> Iterator[int]\n") + assert m.IntPairs.simple_iterator.__doc__.endswith("-> Iterator[tuple[int, int]]\n") + assert m.IntPairs.simple_keys.__doc__.endswith("-> Iterator[int]\n") + assert m.IntPairs.simple_values.__doc__.endswith("-> Iterator[int]\n") + + def test_iterator_referencing(): """Test that iterators reference rather than copy their referents.""" vec = m.VectorNonCopyableInt() @@ -171,6 +182,10 @@ def test_sequence_length(): assert m.sequence_length("hello") == 5 +def test_sequence_doc(): + assert m.sequence_length.__doc__.strip() == "sequence_length(arg0: Sequence) -> int" + + def test_map_iterator(): sm = m.StringMap({"hi": "bye", "black": "white"}) assert sm["hi"] == "bye" diff --git a/pybind11/tests/test_smart_ptr.cpp b/pybind11/tests/test_smart_ptr.cpp index 6d9efcedc..496073b3c 100644 --- a/pybind11/tests/test_smart_ptr.cpp +++ b/pybind11/tests/test_smart_ptr.cpp @@ -103,21 +103,26 @@ private: int value; }; +template +std::unordered_set &pointer_set() { + // https://google.github.io/styleguide/cppguide.html#Static_and_Global_Variables + static auto singleton = new std::unordered_set(); + return *singleton; +} + // test_unique_nodelete // Object with a private destructor -class MyObject4; -std::unordered_set myobject4_instances; class MyObject4 { public: explicit MyObject4(int value) : value{value} { print_created(this); - myobject4_instances.insert(this); + pointer_set().insert(this); } int value; static void cleanupAllInstances() { - auto tmp = std::move(myobject4_instances); - myobject4_instances.clear(); + auto tmp = std::move(pointer_set()); + pointer_set().clear(); for (auto *o : tmp) { delete o; } @@ -125,7 +130,7 @@ public: private: ~MyObject4() { - myobject4_instances.erase(this); + pointer_set().erase(this); print_destroyed(this); } }; @@ -133,19 +138,17 @@ private: // test_unique_deleter // Object with std::unique_ptr where D is not matching the base class // Object with a protected destructor -class MyObject4a; -std::unordered_set myobject4a_instances; class MyObject4a { public: explicit MyObject4a(int i) : value{i} { print_created(this); - myobject4a_instances.insert(this); + pointer_set().insert(this); }; int value; static void cleanupAllInstances() { - auto tmp = std::move(myobject4a_instances); - myobject4a_instances.clear(); + auto tmp = std::move(pointer_set()); + pointer_set().clear(); for (auto *o : tmp) { delete o; } @@ -153,7 +156,7 @@ public: protected: virtual ~MyObject4a() { - myobject4a_instances.erase(this); + pointer_set().erase(this); print_destroyed(this); } }; diff --git a/pybind11/tests/test_smart_ptr.py b/pybind11/tests/test_smart_ptr.py index 2f204e01b..bf0ae4aeb 100644 --- a/pybind11/tests/test_smart_ptr.py +++ b/pybind11/tests/test_smart_ptr.py @@ -1,3 +1,5 @@ +from __future__ import annotations + import pytest m = pytest.importorskip("pybind11_tests.smart_ptr") diff --git a/pybind11/tests/test_stl.cpp b/pybind11/tests/test_stl.cpp index d45465d68..48c907ff3 100644 --- a/pybind11/tests/test_stl.cpp +++ b/pybind11/tests/test_stl.cpp @@ -78,7 +78,7 @@ struct hash { template