diff --git a/include/ceres/internal/integer_sequence_algorithm.h b/include/ceres/internal/integer_sequence_algorithm.h index 8c0f3bc8a..777c119a7 100644 --- a/include/ceres/internal/integer_sequence_algorithm.h +++ b/include/ceres/internal/integer_sequence_algorithm.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2018 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -27,6 +27,7 @@ // POSSIBILITY OF SUCH DAMAGE. // // Author: jodebo_beck@gmx.de (Johannes Beck) +// sergiu.deitsch@gmail.com (Sergiu Deitsch) // // Algorithms to be used together with integer_sequence, like computing the sum // or the exclusive scan (sometimes called exclusive prefix sum) at compile @@ -37,6 +38,8 @@ #include +#include "ceres/jet_fwd.h" + namespace ceres { namespace internal { @@ -164,6 +167,124 @@ class ExclusiveScanT { template using ExclusiveScan = typename ExclusiveScanT::Type; +// Removes all elements from a integer sequence corresponding to specified +// ValueToRemove. +// +// This type should not be used directly but instead RemoveValue. +template +struct RemoveValueImpl; + +// Final filtered sequence +template +struct RemoveValueImpl, + std::integer_sequence> { + using type = std::integer_sequence; +}; + +// Found a matching value +template +struct RemoveValueImpl, + std::integer_sequence> + : RemoveValueImpl, + std::integer_sequence> {}; + +// Move one element from the tail to the head +template +struct RemoveValueImpl, + std::integer_sequence> + : RemoveValueImpl, + std::integer_sequence> {}; + +// Start recursion by splitting the integer sequence into two separate ones +template +struct RemoveValueImpl> + : RemoveValueImpl, + std::integer_sequence> {}; + +// RemoveValue takes an integer Sequence of arbitrary type and removes all +// elements matching ValueToRemove. +// +// In contrast to RemoveValueImpl, this implementation deduces the value type +// eliminating the need to specify it explicitly. +// +// As an example, RemoveValue, 4>::type will +// not transform the type of the original sequence. However, +// RemoveValue, 2>::type will generate a new +// sequence of type std::integer_sequence by removing the value 2. +template +struct RemoveValue + : RemoveValueImpl { +}; + +// Convenience template alias for RemoveValue. +template +using RemoveValue_t = typename RemoveValue::type; + +// Determines whether the values of an integer sequence are all the same. +// +// The integer sequence must contain at least one value. The predicate is +// undefined for empty sequences. The evaluation result of the predicate for a +// sequence containing only one value is defined to be true. +template +struct AreAllEqual; + +// The predicate result for a sequence containing one element is defined to be +// true. +template +struct AreAllEqual> : std::true_type {}; + +// Recursion end. +template +struct AreAllEqual> + : std::integral_constant {}; + +// Recursion for sequences containing at least two elements. +template +// clang-format off +struct AreAllEqual > + : std::integral_constant +< + bool, + AreAllEqual >::value && + AreAllEqual >::value +> +// clang-format on +{}; + +// Convenience variable template for AreAllEqual. +template +constexpr bool AreAllEqual_v = AreAllEqual::value; + +// Predicate determining whether an integer sequence is either empty or all +// values are equal. +template +struct IsEmptyOrAreAllEqual; + +// Empty case. +template +struct IsEmptyOrAreAllEqual> : std::true_type {}; + +// General case for sequences containing at least one value. +template +struct IsEmptyOrAreAllEqual> + : AreAllEqual> {}; + +// Convenience variable template for IsEmptyOrAreAllEqual. +template +constexpr bool IsEmptyOrAreAllEqual_v = IsEmptyOrAreAllEqual::value; + } // namespace internal } // namespace ceres diff --git a/include/ceres/internal/jet_traits.h b/include/ceres/internal/jet_traits.h new file mode 100644 index 000000000..2a38c05b7 --- /dev/null +++ b/include/ceres/internal/jet_traits.h @@ -0,0 +1,223 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * 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. +// * Neither the name of Google Inc. nor the names of its contributors 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 OWNER 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. +// +// Author: sergiu.deitsch@gmail.com (Sergiu Deitsch) +// + +#ifndef CERES_PUBLIC_INTERNAL_JET_TRAITS_H_ +#define CERES_PUBLIC_INTERNAL_JET_TRAITS_H_ + +#include +#include +#include + +#include "ceres/internal/integer_sequence_algorithm.h" +#include "ceres/jet_fwd.h" + +namespace ceres { +namespace internal { + +// Predicate that determines whether T is a Jet. +template +struct IsJet : std::false_type {}; + +template +struct IsJet> : std::true_type {}; + +// Convenience variable template for IsJet. +template +constexpr bool IsJet_v = IsJet::value; + +// Predicate that determines whether any of the Types is a Jet. +template +struct AreAnyJet : std::false_type {}; + +template +struct AreAnyJet : AreAnyJet {}; + +template +struct AreAnyJet, Types...> : std::true_type {}; + +// Convenience variable template for AreAnyJet. +template +constexpr bool AreAnyJet_v = AreAnyJet::value; + +// Extracts the underlying floating-point from a type T. +template +struct UnderlyingScalar { + using type = T; +}; + +template +struct UnderlyingScalar> : UnderlyingScalar {}; + +// Convenience template alias for UnderlyingScalar type trait. +template +using UnderlyingScalar_t = typename UnderlyingScalar::type; + +// Predicate determining whether all Types in the pack are the same. +// +// Specifically, the predicate applies std::is_same recursively to pairs of +// Types in the pack. +// +// The predicate is defined only for template packs containing at least two +// types. +template +// clang-format off +struct AreAllSame : std::integral_constant +< + bool, + AreAllSame::value && + AreAllSame::value +> +// clang-format on +{}; + +// AreAllSame pairwise test. +template +struct AreAllSame : std::is_same {}; + +// Convenience variable template for AreAllSame. +template +constexpr bool AreAllSame_v = AreAllSame::value; + +// Determines the rank of a type. This allows to ensure that types passed as +// arguments are compatible to each other. The rank of Jet is determined by the +// dimensions of the dual part. The rank of a scalar is always 0. +// Non-specialized types default to a rank of -1. +template +struct Rank : std::integral_constant {}; + +// The rank of a scalar is 0. +template +struct Rank::value>> + : std::integral_constant {}; + +// The rank of a Jet is given by its dimensionality. +template +struct Rank> : std::integral_constant {}; + +// Convenience variable template for Rank. +template +constexpr int Rank_v = Rank::value; + +// Constructs an integer sequence of ranks for each of the Types in the pack. +template +using Ranks_t = std::integer_sequence...>; + +// Returns the scalar part of a type. This overload acts as an identity. +template +constexpr decltype(auto) AsScalar(T&& value) noexcept { + return std::forward(value); +} + +// Recursively unwraps the scalar part of a Jet until a non-Jet scalar type is +// encountered. +template +constexpr decltype(auto) AsScalar(const Jet& value) noexcept( + noexcept(AsScalar(value.a))) { + return AsScalar(value.a); +} + +} // namespace internal + +// Type trait ensuring at least one of the types is a Jet, +// the underlying scalar types are the same and Jet dimensions match. +// +// The type trait can be further specialized if necessary. +// +// This trait is a candidate for a concept definition once C++20 features can +// be used. +template +// clang-format off +struct CompatibleJetOperands : std::integral_constant +< + bool, + // At least one of the types is a Jet + internal::AreAnyJet_v && + // The underlying floating-point types are exactly the same + internal::AreAllSame_v...> && + // Non-zero ranks of types are equal + internal::IsEmptyOrAreAllEqual_v, 0>> +> +// clang-format on +{}; + +// Single Jet operand is always compatible. +template +struct CompatibleJetOperands> : std::true_type {}; + +// Single non-Jet operand is always incompatible. +template +struct CompatibleJetOperands : std::false_type {}; + +// Empty operands are always incompatible. +template <> +struct CompatibleJetOperands<> : std::false_type {}; + +// Convenience variable template ensuring at least one of the types is a Jet, +// the underlying scalar types are the same and Jet dimensions match. +// +// This trait is a candidate for a concept definition once C++20 features can +// be used. +template +constexpr bool CompatibleJetOperands_v = CompatibleJetOperands::value; + +// Type trait ensuring at least one of the types is a Jet, +// the underlying scalar types are compatible among each other and Jet +// dimensions match. +// +// The type trait can be further specialized if necessary. +// +// This trait is a candidate for a concept definition once C++20 features can +// be used. +template +// clang-format off +struct PromotableJetOperands : std::integral_constant +< + bool, + // Types can be compatible among each other + internal::AreAnyJet_v && + // Non-zero ranks of types are equal + internal::IsEmptyOrAreAllEqual_v, 0>> +> +// clang-format on +{}; + +// Convenience variable template ensuring at least one of the types is a Jet, +// the underlying scalar types are compatible among each other and Jet +// dimensions match. +// +// This trait is a candidate for a concept definition once C++20 features can +// be used. +template +constexpr bool PromotableJetOperands_v = PromotableJetOperands::value; + +} // namespace ceres + +#endif // CERES_PUBLIC_INTERNAL_JET_TRAITS_H_ diff --git a/include/ceres/jet.h b/include/ceres/jet.h index 3048cdce2..3a5b5546d 100644 --- a/include/ceres/jet.h +++ b/include/ceres/jet.h @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2019 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -167,14 +167,9 @@ #include #include "Eigen/Core" +#include "ceres/internal/jet_traits.h" #include "ceres/internal/port.h" - -namespace ceres { -// Jet foward declaration necessary for the following partial specialization of -// std::common_type. -template -struct Jet; -} // namespace ceres +#include "ceres/jet_fwd.h" // Here we provide partial specializations of std::common_type for the Jet class // to allow determining a Jet type with a common underlying arithmetic type. @@ -396,32 +391,21 @@ inline Jet operator/(const Jet& f, T s) { return Jet(f.a * s_inverse, f.v * s_inverse); } -// Binary comparison operators for both scalars and jets. std::common_type_t is -// used as an SFINAE constraint to selectively enable compatible operand types. -// This allows comparison, for instance, against int literals without implicit -// conversion. In case the Jet arithmetic type is a Jet itself, a recursive -// expansion of Jet value is performed. -#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ - template \ - constexpr bool operator op( \ - const Jet& f, const Jet& g) noexcept(noexcept(f.a op g.a)) { \ - return f.a op g.a; \ - } \ - template * = nullptr> \ - constexpr bool operator op( \ - const U& s, const Jet& g) noexcept(noexcept(s op g.a)) { \ - return s op g.a; \ - } \ - template * = nullptr> \ - constexpr bool operator op(const Jet& f, \ - const U& s) noexcept(noexcept(f.a op s)) { \ - return f.a op s; \ +// Binary comparison operators for both scalars and jets. At least one of the +// operands must be a Jet. Promotable scalars (e.g., int, float, double etc.) +// can appear on either side of the operator. std::common_type_t is used as an +// SFINAE constraint to selectively enable compatible operand types. This allows +// comparison, for instance, against int literals without implicit conversion. +// In case the Jet arithmetic type is a Jet itself, a recursive expansion of Jet +// value is performed. +#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \ + template >* = nullptr> \ + constexpr bool operator op(const Lhs& f, const Rhs& g) noexcept( \ + noexcept(internal::AsScalar(f) op internal::AsScalar(g))) { \ + using internal::AsScalar; \ + return AsScalar(f) op AsScalar(g); \ } CERES_DEFINE_JET_COMPARISON_OPERATOR(<) // NOLINT CERES_DEFINE_JET_COMPARISON_OPERATOR(<=) // NOLINT @@ -451,21 +435,30 @@ using std::erfc; using std::exp; using std::exp2; using std::expm1; +using std::fdim; using std::floor; using std::fma; using std::fmax; using std::fmin; +using std::fpclassify; using std::hypot; using std::isfinite; +using std::isgreater; +using std::isgreaterequal; using std::isinf; +using std::isless; +using std::islessequal; +using std::islessgreater; using std::isnan; using std::isnormal; +using std::isunordered; using std::log; using std::log10; using std::log1p; using std::log2; using std::norm; using std::pow; +using std::signbit; using std::sin; using std::sinh; using std::sqrt; @@ -479,9 +472,13 @@ using std::midpoint; // Legacy names from pre-C++11 days. // clang-format off +[[deprecated("ceres::IsFinite will be removed in a future Ceres Solver release. Please use ceres::isfinite.")]] inline bool IsFinite(double x) { return std::isfinite(x); } +[[deprecated("ceres::IsInfinite will be removed in a future Ceres Solver release. Please use ceres::isinf.")]] inline bool IsInfinite(double x) { return std::isinf(x); } +[[deprecated("ceres::IsNaN will be removed in a future Ceres Solver release. Please use ceres::isnan.")]] inline bool IsNaN(double x) { return std::isnan(x); } +[[deprecated("ceres::IsNormal will be removed in a future Ceres Solver release. Please use ceres::isnormal.")]] inline bool IsNormal(double x) { return std::isnormal(x); } // clang-format on @@ -518,7 +515,7 @@ template inline Jet copysign(const Jet& f, const Jet g) { // The Dirac delta function δ(b) is undefined at b=0 (here it's // infinite) and 0 everywhere else. - T d = g.a == T(0) ? std::numeric_limits::infinity() : T(0); + T d = fpclassify(g) == FP_ZERO ? std::numeric_limits::infinity() : T(0); T sa = copysign(T(1), f.a); // sgn(a) T sb = copysign(T(1), g.a); // sgn(b) // The second part of the infinitesimal is 2|a|δ(b) which is either infinity @@ -725,36 +722,44 @@ inline Jet fma(const Jet& x, return Jet(fma(x.a, y.a, z.a), y.a * x.v + x.a * y.v + z.v); } -template -inline Jet fmax(const Jet& x, const Jet& y) { - using std::isgreater; - return isnan(y.a) || isgreater(x.a, y.a) ? x : y; +// Returns the larger of the two arguments. NaNs are treated as missing data. +// +// NOTE: This function is NOT subject to any of the error conditions specified +// in `math_errhandling`. +template >* = nullptr> +inline decltype(auto) fmax(const Lhs& f, const Rhs& g) { + using J = std::common_type_t; + return (isnan(g) || isgreater(f, g)) ? J{f} : J{g}; } -template -inline Jet fmax(const Jet& x, const T& y) { - return fmax(x, Jet{y}); +// Returns the smaller of the two arguments. NaNs are treated as missing data. +// +// NOTE: This function is NOT subject to any of the error conditions specified +// in `math_errhandling`. +template >* = nullptr> +inline decltype(auto) fmin(const Lhs& f, const Rhs& g) { + using J = std::common_type_t; + return (isnan(f) || isless(g, f)) ? J{g} : J{f}; } -template -inline Jet fmax(const T& x, const Jet& y) { - return fmax(Jet{x}, y); -} - -template -inline Jet fmin(const Jet& x, const Jet& y) { - using std::isless; - return isnan(x.a) || isless(y.a, x.a) ? y : x; -} - -template -inline Jet fmin(const Jet& x, const T& y) { - return fmin(x, Jet{y}); -} - -template -inline Jet fmin(const T& x, const Jet& y) { - return fmin(Jet{x}, y); +// Returns the positive difference (f - g) of two arguments and zero if f <= g. +// If at least one argument is NaN, a NaN is return. +// +// NOTE At least one of the argument types must be a Jet, the other one can be a +// scalar. In case both arguments are Jets, their dimensionality must match. +template >* = nullptr> +inline decltype(auto) fdim(const Lhs& f, const Rhs& g) { + using J = std::common_type_t; + if (isnan(f) || isnan(g)) { + return std::numeric_limits::quiet_NaN(); + } + return isgreater(f, g) ? J{f - g} : J{}; } // erf is defined as an integral that cannot be expressed analytically @@ -839,77 +844,162 @@ inline Jet BesselJn(int n, const Jet& f) { T(0.5) * (BesselJn(n - 1, f.a) - BesselJn(n + 1, f.a)) * f.v); } -// Jet Classification. It is not clear what the appropriate semantics are for -// these classifications. This picks that std::isfinite and std::isnormal are -// "all" operations, i.e. all elements of the jet must be finite for the jet -// itself to be finite (or normal). For IsNaN and IsInfinite, the answer is less -// clear. This takes a "any" approach for IsNaN and IsInfinite such that if any -// part of a jet is nan or inf, then the entire jet is nan or inf. This leads -// to strange situations like a jet can be both IsInfinite and IsNaN, but in -// practice the "any" semantics are the most useful for e.g. checking that -// derivatives are sane. +// Classification and comparison functionality referencing only the scalar part +// of a Jet. To classify the derivatives (e.g., for sanity checks), the dual +// part should be referenced explicitly. For instance, to check whether the +// derivatives of a Jet 'f' are reasonable, one can use +// +// isfinite(f.v.array()).all() +// !isnan(f.v.array()).any() +// +// etc., depending on the desired semantics. +// +// NOTE: Floating-point classification and comparison functions and operators +// should be used with care as no derivatives can be propagated by such +// functions directly but only by expressions resulting from corresponding +// conditional statements. At the same time, conditional statements can possibly +// introduce a discontinuity in the cost function making it impossible to +// evaluate its derivative and thus the optimization problem intractable. -// The jet is finite if all parts of the jet are finite. +// Determines whether the scalar part of the Jet is finite. template inline bool isfinite(const Jet& f) { - // Branchless implementation. This is more efficient for the false-case and - // works with the codegen system. - auto result = isfinite(f.a); - for (int i = 0; i < N; ++i) { - result = result & isfinite(f.v[i]); - } - return result; + return isfinite(f.a); } -// The jet is infinite if any part of the Jet is infinite. +// Determines whether the scalar part of the Jet is infinite. template inline bool isinf(const Jet& f) { - auto result = isinf(f.a); - for (int i = 0; i < N; ++i) { - result = result | isinf(f.v[i]); - } - return result; + return isinf(f.a); } -// The jet is NaN if any part of the jet is NaN. +// Determines whether the scalar part of the Jet is NaN. template inline bool isnan(const Jet& f) { - auto result = isnan(f.a); - for (int i = 0; i < N; ++i) { - result = result | isnan(f.v[i]); - } - return result; + return isnan(f.a); } -// The jet is normal if all parts of the jet are normal. +// Determines whether the scalar part of the Jet is neither zero, subnormal, +// infinite, nor NaN. template inline bool isnormal(const Jet& f) { - auto result = isnormal(f.a); - for (int i = 0; i < N; ++i) { - result = result & isnormal(f.v[i]); - } - return result; + return isnormal(f.a); +} + +// Determines whether the scalar part of the Jet f is less than the scalar +// part of g. +// +// NOTE: This function does NOT set any floating-point exceptions. +template >* = nullptr> +inline bool isless(const Lhs& f, const Rhs& g) { + using internal::AsScalar; + return isless(AsScalar(f), AsScalar(g)); +} + +// Determines whether the scalar part of the Jet f is greater than the scalar +// part of g. +// +// NOTE: This function does NOT set any floating-point exceptions. +template >* = nullptr> +inline bool isgreater(const Lhs& f, const Rhs& g) { + using internal::AsScalar; + return isgreater(AsScalar(f), AsScalar(g)); +} + +// Determines whether the scalar part of the Jet f is less than or equal to the +// scalar part of g. +// +// NOTE: This function does NOT set any floating-point exceptions. +template >* = nullptr> +inline bool islessequal(const Lhs& f, const Rhs& g) { + using internal::AsScalar; + return islessequal(AsScalar(f), AsScalar(g)); +} + +// Determines whether the scalar part of the Jet f is less than or greater than +// (f < g || f > g) the scalar part of g. +// +// NOTE: This function does NOT set any floating-point exceptions. +template >* = nullptr> +inline bool islessgreater(const Lhs& f, const Rhs& g) { + using internal::AsScalar; + return islessgreater(AsScalar(f), AsScalar(g)); +} + +// Determines whether the scalar part of the Jet f is greater than or equal to +// the scalar part of g. +// +// NOTE: This function does NOT set any floating-point exceptions. +template >* = nullptr> +inline bool isgreaterequal(const Lhs& f, const Rhs& g) { + using internal::AsScalar; + return isgreaterequal(AsScalar(f), AsScalar(g)); +} + +// Determines if either of the scalar parts of the arguments are NaN and +// thus cannot be ordered with respect to each other. +template >* = nullptr> +inline bool isunordered(const Lhs& f, const Rhs& g) { + using internal::AsScalar; + return isunordered(AsScalar(f), AsScalar(g)); +} + +// Categorize scalar part as zero, subnormal, normal, infinite, NaN, or +// implementation-defined. +template +inline int fpclassify(const Jet& f) { + return fpclassify(f.a); +} + +// Determines whether the scalar part of the argument is negative. +template +inline bool signbit(const Jet& f) { + return signbit(f.a); } // Legacy functions from the pre-C++11 days. template -inline bool IsFinite(const Jet& f) { +[[deprecated( + "ceres::IsFinite will be removed in a future Ceres Solver release. Please " + "use ceres::isfinite.")]] inline bool +IsFinite(const Jet& f) { return isfinite(f); } template -inline bool IsNaN(const Jet& f) { +[[deprecated( + "ceres::IsNaN will be removed in a future Ceres Solver release. Please use " + "ceres::isnan.")]] inline bool +IsNaN(const Jet& f) { return isnan(f); } template -inline bool IsNormal(const Jet& f) { +[[deprecated( + "ceres::IsNormal will be removed in a future Ceres Solver release. Please " + "use ceres::isnormal.")]] inline bool +IsNormal(const Jet& f) { return isnormal(f); } // The jet is infinite if any part of the jet is infinite. template -inline bool IsInfinite(const Jet& f) { +[[deprecated( + "ceres::IsInfinite will be removed in a future Ceres Solver release. " + "Please use ceres::isinf.")]] inline bool +IsInfinite(const Jet& f) { return isinf(f); } @@ -976,9 +1066,9 @@ inline Jet atan2(const Jet& g, const Jet& f) { // Computes the square x^2 of a real number x (not the Euclidean L^2 norm as // the name might suggest). // -// NOTE While std::norm is primarly intended for computing the squared magnitude -// of a std::complex<> number, the current Jet implementation does not support -// mixing a scalar T in its real part and std::complex and in the +// NOTE: While std::norm is primarily intended for computing the squared +// magnitude of a std::complex<> number, the current Jet implementation does not +// support mixing a scalar T in its real part and std::complex and in the // infinitesimal. Mixed Jet support is necessary for the type decay from // std::complex to T (the squared magnitude of a complex number is always // real) performed by std::norm. @@ -1012,14 +1102,14 @@ template inline Jet pow(T f, const Jet& g) { Jet result; - if (f == T(0) && g.a > T(0)) { + if (fpclassify(f) == FP_ZERO && g > 0) { // Handle case 2. result = Jet(T(0.0)); } else { - if (f < 0 && g.a == floor(g.a)) { // Handle case 3. + if (f < 0 && g == floor(g.a)) { // Handle case 3. result = Jet(pow(f, g.a)); for (int i = 0; i < N; i++) { - if (g.v[i] != T(0.0)) { + if (fpclassify(g.v[i]) != FP_ZERO) { // Return a NaN when g.v != 0. result.v[i] = std::numeric_limits::quiet_NaN(); } @@ -1074,21 +1164,21 @@ template inline Jet pow(const Jet& f, const Jet& g) { Jet result; - if (f.a == T(0) && g.a >= T(1)) { + if (fpclassify(f) == FP_ZERO && g >= 1) { // Handle cases 2 and 3. - if (g.a > T(1)) { + if (g > 1) { result = Jet(T(0.0)); } else { result = f; } } else { - if (f.a < T(0) && g.a == floor(g.a)) { + if (f < 0 && g == floor(g.a)) { // Handle cases 7 and 8. T const tmp = g.a * pow(f.a, g.a - T(1.0)); result = Jet(pow(f.a, g.a), tmp * f.v); for (int i = 0; i < N; i++) { - if (g.v[i] != T(0.0)) { + if (fpclassify(g.v[i]) != FP_ZERO) { // Return a NaN when g.v != 0. result.v[i] = T(std::numeric_limits::quiet_NaN()); } diff --git a/include/ceres/jet_fwd.h b/include/ceres/jet_fwd.h new file mode 100644 index 000000000..fbb628695 --- /dev/null +++ b/include/ceres/jet_fwd.h @@ -0,0 +1,44 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * 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. +// * Neither the name of Google Inc. nor the names of its contributors 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 OWNER 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. +// +// Author: sergiu.deitsch@gmail.com (Sergiu Deitsch) +// + +#ifndef CERES_PUBLIC_JET_FWD_H_ +#define CERES_PUBLIC_JET_FWD_H_ + +namespace ceres { + +// Jet forward declaration necessary for the following partial specialization of +// std::common_type and type traits. +template +struct Jet; + +} // namespace ceres + +#endif // CERES_PUBLIC_JET_FWD_H_ diff --git a/internal/ceres/CMakeLists.txt b/internal/ceres/CMakeLists.txt index 957d476ea..80703a1c7 100644 --- a/internal/ceres/CMakeLists.txt +++ b/internal/ceres/CMakeLists.txt @@ -482,6 +482,7 @@ if (BUILD_TESTING AND GFLAGS) ceres_test(iterative_refiner) ceres_test(iterative_schur_complement_solver) ceres_test(jet) + ceres_test(jet_traits) ceres_test(levenberg_marquardt_strategy) ceres_test(line_search_minimizer) ceres_test(line_search_preprocessor) diff --git a/internal/ceres/integer_sequence_algorithm_test.cc b/internal/ceres/integer_sequence_algorithm_test.cc index af42a9134..7e041486c 100644 --- a/internal/ceres/integer_sequence_algorithm_test.cc +++ b/internal/ceres/integer_sequence_algorithm_test.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2018 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -27,12 +27,15 @@ // POSSIBILITY OF SUCH DAMAGE. // // Author: jodebo_beck@gmx.de (Johannes Beck) +// sergiu.deitsch@gmail.com (Sergiu Deitsch) #include "ceres/internal/integer_sequence_algorithm.h" #include #include +#include "ceres/internal/jet_traits.h" + namespace ceres { namespace internal { @@ -68,5 +71,84 @@ static_assert(std::is_same>, "Unit test of calculating the exclusive scan of an integer " "sequence failed."); +using Ranks001 = Ranks_t, double, Jet>; +using Ranks1 = Ranks_t>; +using Ranks110 = Ranks_t, Jet, double>; +using Ranks023 = Ranks_t, Jet>; +using EmptyRanks = Ranks_t<>; + +// Remove zero from the ranks integer sequence +using NonZeroRanks001 = RemoveValue_t; +using NonZeroRanks1 = RemoveValue_t; +using NonZeroRanks110 = RemoveValue_t; +using NonZeroRanks023 = RemoveValue_t; + +static_assert(std::is_same, + std::integer_sequence>::value, + "filtered sequence does not match an empty one"); +static_assert(std::is_same, 2>, + std::integer_sequence>::value, + "filtered sequence does not match an empty one"); +static_assert( + std::is_same, 2>, + std::integer_sequence>::value, + "filtered sequence does not match the expected one"); +static_assert( + std::is_same, 7>, + std::make_integer_sequence>::value, + "sequence not containing the element to remove must not be transformed"); +static_assert( + std::is_same>::value, + "sequences do not match"); +static_assert(std::is_same>::value, + "sequences do not match"); +static_assert( + std::is_same>::value, + "sequences do not match"); +static_assert( + std::is_same>::value, + "sequences do not match"); +static_assert(std::is_same, -1>, + std::integer_sequence>::value, + "sequences do not match"); +static_assert( + std::is_same, -1>, + std::integer_sequence>::value, + "sequences do not match"); + +using J = Jet; +template +using J0 = Jet; +using J0d = J0; + +// Ensure all types match +static_assert(AreAllSame_v, "types must be the same"); +static_assert(AreAllSame_v, "types must be the same"); +static_assert(AreAllSame_v, "types must be the same"); +static_assert(!AreAllSame_v, "types must not be the same"); +static_assert(!AreAllSame_v, "types must not be the same"); + +// Ensure all values in the integer sequence match +static_assert(AreAllEqual_v>, + "integer sequence must contain same values"); +static_assert(AreAllEqual_v>, + "integer sequence must contain one value"); +static_assert(!AreAllEqual_v>, + "integer sequence must not contain the same values"); +static_assert(!AreAllEqual_v>, + "integer sequence must not contain the same values"); +static_assert(!AreAllEqual_v>, + "integer sequence must not contain the same values"); + +static_assert(IsEmptyOrAreAllEqual_v>, + "expected empty sequence is not"); +static_assert(IsEmptyOrAreAllEqual_v>, + "expected all equal sequence is not"); +static_assert(IsEmptyOrAreAllEqual_v>, + "expected all equal sequence is not"); +static_assert( + IsEmptyOrAreAllEqual_v>, + "expected all equal sequence is not"); + } // namespace internal } // namespace ceres diff --git a/internal/ceres/jet_test.cc b/internal/ceres/jet_test.cc index fb7ee81e8..544575cce 100644 --- a/internal/ceres/jet_test.cc +++ b/internal/ceres/jet_test.cc @@ -1,5 +1,5 @@ // Ceres Solver - A fast non-linear least squares minimizer -// Copyright 2015 Google Inc. All rights reserved. +// Copyright 2022 Google Inc. All rights reserved. // http://ceres-solver.org/ // // Redistribution and use in source and binary forms, with or without @@ -28,10 +28,15 @@ // // Author: keir@google.com (Keir Mierle) +// The floating-point environment access and modification is only meaningful +// with the following pragma. +#pragma STDC FENV_ACCESS ON + #include "ceres/jet.h" #include #include +#include #include #include "ceres/stringprintf.h" @@ -48,6 +53,11 @@ namespace { constexpr double kE = 2.71828182845904523536; using J = Jet; +// Don't care about the dual part for scalar part categorization and comparison +// tests +template +using J0 = Jet; +using J0d = J0; // Convenient shorthand for making a jet. J MakeJet(double a, double v0, double v1) { @@ -60,6 +70,22 @@ J MakeJet(double a, double v0, double v1) { double const kTolerance = 1e-13; +// Stores the floating-point environment containing active floating-point +// exceptions, rounding mode, etc., and restores it upon destruction. +// +// Useful for avoiding side-effects. +class Fenv { + public: + Fenv() { std::fegetenv(&e); } + ~Fenv() { std::fesetenv(&e); } + + Fenv(const Fenv&) = delete; + Fenv& operator=(const Fenv&) = delete; + + private: + std::fenv_t e; +}; + bool AreAlmostEqual(double x, double y, double max_abs_relative_difference) { if (std::isnan(x) && std::isnan(y)) { return true; @@ -69,16 +95,17 @@ bool AreAlmostEqual(double x, double y, double max_abs_relative_difference) { return (std::signbit(x) == std::signbit(y)); } + Fenv env; // Do not leak floating-point exceptions to the caller double absolute_difference = std::abs(x - y); double relative_difference = absolute_difference / std::max(std::abs(x), std::abs(y)); - if (x == 0 || y == 0) { + if (std::fpclassify(x) == FP_ZERO || std::fpclassify(y) == FP_ZERO) { // If x or y is exactly zero, then relative difference doesn't have any // meaning. Take the absolute difference instead. relative_difference = absolute_difference; } - return relative_difference <= max_abs_relative_difference; + return std::islessequal(relative_difference, max_abs_relative_difference); } MATCHER_P(IsAlmostEqualTo, y, "") { @@ -420,9 +447,9 @@ TEST(Jet, Pow) { J b = MakeJet(i * 0.1, 3, 4); // b = 0.1 ... 0.9 J c = pow(a, b); EXPECT_EQ(c.a, 0.0) << "\na: " << a << "\nb: " << b << "\na^b: " << c; - EXPECT_FALSE(IsFinite(c.v[0])) + EXPECT_FALSE(isfinite(c.v[0])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; - EXPECT_FALSE(IsFinite(c.v[1])) + EXPECT_FALSE(isfinite(c.v[1])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; } @@ -430,11 +457,11 @@ TEST(Jet, Pow) { J a = MakeJet(0, 1, 2); J b = MakeJet(i * 0.1, 3, 4); // b = -1,-0.9 ... -0.1 J c = pow(a, b); - EXPECT_FALSE(IsFinite(c.a)) + EXPECT_FALSE(isfinite(c.a)) << "\na: " << a << "\nb: " << b << "\na^b: " << c; - EXPECT_FALSE(IsFinite(c.v[0])) + EXPECT_FALSE(isfinite(c.v[0])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; - EXPECT_FALSE(IsFinite(c.v[1])) + EXPECT_FALSE(isfinite(c.v[1])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; } @@ -444,9 +471,9 @@ TEST(Jet, Pow) { J b = MakeJet(0, 3, 4); J c = pow(a, b); EXPECT_EQ(c.a, 1.0) << "\na: " << a << "\nb: " << b << "\na^b: " << c; - EXPECT_FALSE(IsFinite(c.v[0])) + EXPECT_FALSE(isfinite(c.v[0])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; - EXPECT_FALSE(IsFinite(c.v[1])) + EXPECT_FALSE(isfinite(c.v[1])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; } } @@ -462,9 +489,9 @@ TEST(Jet, Pow) { EXPECT_TRUE(AreAlmostEqual(c.a, pow(-1.5, i), kTolerance)) << "\na: " << a << "\nb: " << b << "\na^b: " << c; - EXPECT_TRUE(IsFinite(c.v[0])) + EXPECT_TRUE(isfinite(c.v[0])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; - EXPECT_FALSE(IsFinite(c.v[1])) + EXPECT_FALSE(isfinite(c.v[1])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; EXPECT_TRUE( AreAlmostEqual(c.v[0], i * pow(-1.5, i - 1) * 3.0, kTolerance)) @@ -477,11 +504,11 @@ TEST(Jet, Pow) { J a = MakeJet(-1.5, 3, 4); J b = MakeJet(-2.5, 0, 5); J c = pow(a, b); - EXPECT_FALSE(IsFinite(c.a)) + EXPECT_FALSE(isfinite(c.a)) << "\na: " << a << "\nb: " << b << "\na^b: " << c; - EXPECT_FALSE(IsFinite(c.v[0])) + EXPECT_FALSE(isfinite(c.v[0])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; - EXPECT_FALSE(IsFinite(c.v[1])) + EXPECT_FALSE(isfinite(c.v[1])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; } @@ -495,9 +522,9 @@ TEST(Jet, Pow) { J b = MakeJet(i, 3, 0); J c = pow(a, b); ExpectClose(c.a, pow(-1.5, i), kTolerance); - EXPECT_FALSE(IsFinite(c.v[0])) + EXPECT_FALSE(isfinite(c.v[0])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; - EXPECT_TRUE(IsFinite(c.v[1])) + EXPECT_TRUE(isfinite(c.v[1])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; ExpectClose(c.v[1], 0, kTolerance); } @@ -508,11 +535,11 @@ TEST(Jet, Pow) { double a = -1.5; J b = MakeJet(-3.14, 3, 0); J c = pow(a, b); - EXPECT_FALSE(IsFinite(c.a)) + EXPECT_FALSE(isfinite(c.a)) << "\na: " << a << "\nb: " << b << "\na^b: " << c; - EXPECT_FALSE(IsFinite(c.v[0])) + EXPECT_FALSE(isfinite(c.v[0])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; - EXPECT_FALSE(IsFinite(c.v[1])) + EXPECT_FALSE(isfinite(c.v[1])) << "\na: " << a << "\nb: " << b << "\na^b: " << c; } } @@ -652,6 +679,11 @@ TEST(Jet, Fma) { } TEST(Jet, Fmax) { + Fenv env; + // Clear all exceptions to ensure none are set by the following function + // calls. + std::feclearexcept(FE_ALL_EXCEPT); + EXPECT_THAT(fmax(x, y), IsAlmostEqualTo(x)); EXPECT_THAT(fmax(y, x), IsAlmostEqualTo(x)); EXPECT_THAT(fmax(x, y.a), IsAlmostEqualTo(x)); @@ -662,9 +694,16 @@ TEST(Jet, Fmax) { IsAlmostEqualTo(x)); EXPECT_THAT(fmax(std::numeric_limits::quiet_NaN(), x), IsAlmostEqualTo(x)); + + EXPECT_EQ(std::fetestexcept(FE_ALL_EXCEPT & ~FE_INEXACT), 0); } TEST(Jet, Fmin) { + Fenv env; + // Clear all exceptions to ensure none are set by the following function + // calls. + std::feclearexcept(FE_ALL_EXCEPT); + EXPECT_THAT(fmin(x, y), IsAlmostEqualTo(y)); EXPECT_THAT(fmin(y, x), IsAlmostEqualTo(y)); EXPECT_THAT(fmin(x, y.a), IsAlmostEqualTo(J{y.a})); @@ -675,85 +714,112 @@ TEST(Jet, Fmin) { IsAlmostEqualTo(x)); EXPECT_THAT(fmin(std::numeric_limits::quiet_NaN(), x), IsAlmostEqualTo(x)); + + EXPECT_EQ(std::fetestexcept(FE_ALL_EXCEPT & ~FE_INEXACT), 0); +} + +TEST(Jet, Fdim) { + Fenv env; + // Clear all exceptions to ensure none are set by the following function + // calls. + std::feclearexcept(FE_ALL_EXCEPT); + + const J zero{}; + const J diff = x - y; + const J diffx = x - J{y.a}; + const J diffy = J{x.a} - y; + + EXPECT_THAT(fdim(x, y), IsAlmostEqualTo(diff)); + EXPECT_THAT(fdim(y, x), IsAlmostEqualTo(zero)); + EXPECT_THAT(fdim(x, y.a), IsAlmostEqualTo(diffx)); + EXPECT_THAT(fdim(y.a, x), IsAlmostEqualTo(J{zero.a})); + EXPECT_THAT(fdim(x.a, y), IsAlmostEqualTo(diffy)); + EXPECT_THAT(fdim(y, x.a), IsAlmostEqualTo(zero)); + EXPECT_TRUE(isnan(fdim(x, std::numeric_limits::quiet_NaN()))); + EXPECT_TRUE(isnan(fdim(std::numeric_limits::quiet_NaN(), x))); + EXPECT_TRUE(isnan(fdim(x, std::numeric_limits::quiet_NaN()))); + EXPECT_TRUE(isnan(fdim(std::numeric_limits::quiet_NaN(), x))); + + EXPECT_EQ(std::fetestexcept(FE_ALL_EXCEPT & ~FE_INEXACT), 0); } TEST(Jet, CopySign) { { // copysign(x, +1) J z = copysign(x, J{+1}); EXPECT_THAT(z, IsAlmostEqualTo(x)); - EXPECT_TRUE(IsFinite(z.v[0])) << z; - EXPECT_TRUE(IsFinite(z.v[1])) << z; + EXPECT_TRUE(isfinite(z.v[0])) << z; + EXPECT_TRUE(isfinite(z.v[1])) << z; } { // copysign(x, -1) J z = copysign(x, J{-1}); EXPECT_THAT(z, IsAlmostEqualTo(-x)); - EXPECT_TRUE(IsFinite(z.v[0])) << z; - EXPECT_TRUE(IsFinite(z.v[1])) << z; + EXPECT_TRUE(isfinite(z.v[0])) << z; + EXPECT_TRUE(isfinite(z.v[1])) << z; } { // copysign(-x, +1) J z = copysign(-x, J{+1}); EXPECT_THAT(z, IsAlmostEqualTo(x)); - EXPECT_TRUE(IsFinite(z.v[0])) << z; - EXPECT_TRUE(IsFinite(z.v[1])) << z; + EXPECT_TRUE(isfinite(z.v[0])) << z; + EXPECT_TRUE(isfinite(z.v[1])) << z; } { // copysign(-x, -1) J z = copysign(-x, J{-1}); EXPECT_THAT(z, IsAlmostEqualTo(-x)); - EXPECT_TRUE(IsFinite(z.v[0])) << z; - EXPECT_TRUE(IsFinite(z.v[1])) << z; + EXPECT_TRUE(isfinite(z.v[0])) << z; + EXPECT_TRUE(isfinite(z.v[1])) << z; } { // copysign(-0, +1) J z = copysign(MakeJet(-0, 1, 2), J{+1}); EXPECT_THAT(z, IsAlmostEqualTo(MakeJet(+0, 1, 2))); EXPECT_FALSE(std::signbit(z.a)) << z; - EXPECT_TRUE(IsFinite(z.v[0])) << z; - EXPECT_TRUE(IsFinite(z.v[1])) << z; + EXPECT_TRUE(isfinite(z.v[0])) << z; + EXPECT_TRUE(isfinite(z.v[1])) << z; } { // copysign(-0, -1) J z = copysign(MakeJet(-0, 1, 2), J{-1}); EXPECT_THAT(z, IsAlmostEqualTo(MakeJet(-0, -1, -2))); EXPECT_TRUE(std::signbit(z.a)) << z; - EXPECT_TRUE(IsFinite(z.v[0])) << z; - EXPECT_TRUE(IsFinite(z.v[1])) << z; + EXPECT_TRUE(isfinite(z.v[0])) << z; + EXPECT_TRUE(isfinite(z.v[1])) << z; } { // copysign(+0, -1) J z = copysign(MakeJet(+0, 1, 2), J{-1}); EXPECT_THAT(z, IsAlmostEqualTo(MakeJet(-0, -1, -2))); EXPECT_TRUE(std::signbit(z.a)) << z; - EXPECT_TRUE(IsFinite(z.v[0])) << z; - EXPECT_TRUE(IsFinite(z.v[1])) << z; + EXPECT_TRUE(isfinite(z.v[0])) << z; + EXPECT_TRUE(isfinite(z.v[1])) << z; } { // copysign(+0, +1) J z = copysign(MakeJet(+0, 1, 2), J{+1}); EXPECT_THAT(z, IsAlmostEqualTo(MakeJet(+0, 1, 2))); EXPECT_FALSE(std::signbit(z.a)) << z; - EXPECT_TRUE(IsFinite(z.v[0])) << z; - EXPECT_TRUE(IsFinite(z.v[1])) << z; + EXPECT_TRUE(isfinite(z.v[0])) << z; + EXPECT_TRUE(isfinite(z.v[1])) << z; } { // copysign(+0, +0) J z = copysign(MakeJet(+0, 1, 2), J{+0}); EXPECT_FALSE(std::signbit(z.a)) << z; - EXPECT_TRUE(IsNaN(z.v[0])) << z; - EXPECT_TRUE(IsNaN(z.v[1])) << z; + EXPECT_TRUE(isnan(z.v[0])) << z; + EXPECT_TRUE(isnan(z.v[1])) << z; } { // copysign(+0, -0) J z = copysign(MakeJet(+0, 1, 2), J{-0}); EXPECT_FALSE(std::signbit(z.a)) << z; - EXPECT_TRUE(IsNaN(z.v[0])) << z; - EXPECT_TRUE(IsNaN(z.v[1])) << z; + EXPECT_TRUE(isnan(z.v[0])) << z; + EXPECT_TRUE(isnan(z.v[1])) << z; } { // copysign(-0, +0) J z = copysign(MakeJet(-0, 1, 2), J{+0}); EXPECT_FALSE(std::signbit(z.a)) << z; - EXPECT_TRUE(IsNaN(z.v[0])) << z; - EXPECT_TRUE(IsNaN(z.v[1])) << z; + EXPECT_TRUE(isnan(z.v[0])) << z; + EXPECT_TRUE(isnan(z.v[1])) << z; } { // copysign(-0, -0) J z = copysign(MakeJet(-0, 1, 2), J{-0}); EXPECT_FALSE(std::signbit(z.a)) << z; - EXPECT_TRUE(IsNaN(z.v[0])) << z; - EXPECT_TRUE(IsNaN(z.v[1])) << z; + EXPECT_TRUE(isnan(z.v[0])) << z; + EXPECT_TRUE(isnan(z.v[1])) << z; } { // copysign(1, -nan) J z = copysign(MakeJet(1, 2, 3), @@ -761,8 +827,8 @@ TEST(Jet, CopySign) { EXPECT_TRUE(std::signbit(z.a)) << z; EXPECT_TRUE(std::signbit(z.v[0])) << z; EXPECT_TRUE(std::signbit(z.v[1])) << z; - EXPECT_FALSE(IsNaN(z.v[0])) << z; - EXPECT_FALSE(IsNaN(z.v[1])) << z; + EXPECT_FALSE(isnan(z.v[0])) << z; + EXPECT_FALSE(isnan(z.v[1])) << z; } { // copysign(1, +nan) J z = copysign(MakeJet(1, 2, 3), @@ -770,8 +836,8 @@ TEST(Jet, CopySign) { EXPECT_FALSE(std::signbit(z.a)) << z; EXPECT_FALSE(std::signbit(z.v[0])) << z; EXPECT_FALSE(std::signbit(z.v[1])) << z; - EXPECT_FALSE(IsNaN(z.v[0])) << z; - EXPECT_FALSE(IsNaN(z.v[1])) << z; + EXPECT_FALSE(isnan(z.v[0])) << z; + EXPECT_FALSE(isnan(z.v[1])) << z; } } @@ -795,48 +861,180 @@ TEST(Jet, JetsInEigenMatrices) { EXPECT_THAT(r1(1), IsAlmostEqualTo(r2(1))); } -TEST(JetTraitsTest, ClassificationMixed) { - Jet a(5.5, 0); - a.v[0] = std::numeric_limits::quiet_NaN(); - a.v[1] = std::numeric_limits::infinity(); - a.v[2] = -std::numeric_limits::infinity(); - EXPECT_FALSE(IsFinite(a)); - EXPECT_FALSE(IsNormal(a)); - EXPECT_TRUE(IsInfinite(a)); - EXPECT_TRUE(IsNaN(a)); +TEST(Jet, ScalarComparison) { + Jet zero{0.0}; + zero.v << std::numeric_limits::infinity(); + + Jet one{1.0}; + one.v << std::numeric_limits::quiet_NaN(); + + Jet two{2.0}; + two.v << std::numeric_limits::min() / 2; + + Jet three{3.0}; + + auto inf = std::numeric_limits>::infinity(); + auto nan = std::numeric_limits>::quiet_NaN(); + inf.v << 1.2; + nan.v << 3.4; + + std::feclearexcept(FE_ALL_EXCEPT); + + EXPECT_FALSE(islessgreater(zero, zero)); + EXPECT_FALSE(islessgreater(zero, zero.a)); + EXPECT_FALSE(islessgreater(zero.a, zero)); + + EXPECT_TRUE(isgreaterequal(three, three)); + EXPECT_TRUE(isgreaterequal(three, three.a)); + EXPECT_TRUE(isgreaterequal(three.a, three)); + + EXPECT_TRUE(isgreater(three, two)); + EXPECT_TRUE(isgreater(three, two.a)); + EXPECT_TRUE(isgreater(three.a, two)); + + EXPECT_TRUE(islessequal(one, one)); + EXPECT_TRUE(islessequal(one, one.a)); + EXPECT_TRUE(islessequal(one.a, one)); + + EXPECT_TRUE(isless(one, two)); + EXPECT_TRUE(isless(one, two.a)); + EXPECT_TRUE(isless(one.a, two)); + + EXPECT_FALSE(isunordered(inf, one)); + EXPECT_FALSE(isunordered(inf, one.a)); + EXPECT_FALSE(isunordered(inf.a, one)); + + EXPECT_TRUE(isunordered(nan, two)); + EXPECT_TRUE(isunordered(nan, two.a)); + EXPECT_TRUE(isunordered(nan.a, two)); + + EXPECT_TRUE(isunordered(inf, nan)); + EXPECT_TRUE(isunordered(inf, nan.a)); + EXPECT_TRUE(isunordered(inf.a, nan.a)); + + EXPECT_EQ(std::fetestexcept(FE_ALL_EXCEPT & ~FE_INEXACT), 0); +} + +TEST(Jet, Nested2XScalarComparison) { + Jet zero{J0d{0.0}}; + zero.v << std::numeric_limits::infinity(); + + Jet one{J0d{1.0}}; + one.v << std::numeric_limits::quiet_NaN(); + + Jet two{J0d{2.0}}; + two.v << std::numeric_limits::min() / J0d{2}; + + Jet three{J0d{3.0}}; + + auto inf = std::numeric_limits>::infinity(); + auto nan = std::numeric_limits>::quiet_NaN(); + inf.v << J0d{1.2}; + nan.v << J0d{3.4}; + + std::feclearexcept(FE_ALL_EXCEPT); + + EXPECT_FALSE(islessgreater(zero, zero)); + EXPECT_FALSE(islessgreater(zero, zero.a)); + EXPECT_FALSE(islessgreater(zero.a, zero)); + EXPECT_FALSE(islessgreater(zero, zero.a.a)); + EXPECT_FALSE(islessgreater(zero.a.a, zero)); + + EXPECT_TRUE(isgreaterequal(three, three)); + EXPECT_TRUE(isgreaterequal(three, three.a)); + EXPECT_TRUE(isgreaterequal(three.a, three)); + EXPECT_TRUE(isgreaterequal(three, three.a.a)); + EXPECT_TRUE(isgreaterequal(three.a.a, three)); + + EXPECT_TRUE(isgreater(three, two)); + EXPECT_TRUE(isgreater(three, two.a)); + EXPECT_TRUE(isgreater(three.a, two)); + EXPECT_TRUE(isgreater(three, two.a.a)); + EXPECT_TRUE(isgreater(three.a.a, two)); + + EXPECT_TRUE(islessequal(one, one)); + EXPECT_TRUE(islessequal(one, one.a)); + EXPECT_TRUE(islessequal(one.a, one)); + EXPECT_TRUE(islessequal(one, one.a.a)); + EXPECT_TRUE(islessequal(one.a.a, one)); + + EXPECT_TRUE(isless(one, two)); + EXPECT_TRUE(isless(one, two.a)); + EXPECT_TRUE(isless(one.a, two)); + EXPECT_TRUE(isless(one, two.a.a)); + EXPECT_TRUE(isless(one.a.a, two)); + + EXPECT_FALSE(isunordered(inf, one)); + EXPECT_FALSE(isunordered(inf, one.a)); + EXPECT_FALSE(isunordered(inf.a, one)); + EXPECT_FALSE(isunordered(inf, one.a.a)); + EXPECT_FALSE(isunordered(inf.a.a, one)); + + EXPECT_TRUE(isunordered(nan, two)); + EXPECT_TRUE(isunordered(nan, two.a)); + EXPECT_TRUE(isunordered(nan.a, two)); + EXPECT_TRUE(isunordered(nan, two.a.a)); + EXPECT_TRUE(isunordered(nan.a.a, two)); + + EXPECT_TRUE(isunordered(inf, nan)); + EXPECT_TRUE(isunordered(inf, nan.a)); + EXPECT_TRUE(isunordered(inf.a, nan)); + EXPECT_TRUE(isunordered(inf, nan.a.a)); + EXPECT_TRUE(isunordered(inf.a.a, nan)); + + EXPECT_EQ(std::fetestexcept(FE_ALL_EXCEPT & ~FE_INEXACT), 0); } TEST(JetTraitsTest, ClassificationNaN) { - Jet a(5.5, 0); - a.v[0] = std::numeric_limits::quiet_NaN(); - a.v[1] = 0.0; - a.v[2] = 0.0; - EXPECT_FALSE(IsFinite(a)); - EXPECT_FALSE(IsNormal(a)); - EXPECT_FALSE(IsInfinite(a)); - EXPECT_TRUE(IsNaN(a)); + Jet a(std::numeric_limits::quiet_NaN()); + a.v << std::numeric_limits::infinity(); + EXPECT_EQ(fpclassify(a), FP_NAN); + EXPECT_FALSE(isfinite(a)); + EXPECT_FALSE(isinf(a)); + EXPECT_FALSE(isnormal(a)); + EXPECT_FALSE(signbit(a)); + EXPECT_TRUE(isnan(a)); } TEST(JetTraitsTest, ClassificationInf) { - Jet a(5.5, 0); - a.v[0] = std::numeric_limits::infinity(); - a.v[1] = 0.0; - a.v[2] = 0.0; - EXPECT_FALSE(IsFinite(a)); - EXPECT_FALSE(IsNormal(a)); - EXPECT_TRUE(IsInfinite(a)); - EXPECT_FALSE(IsNaN(a)); + Jet a(-std::numeric_limits::infinity()); + a.v << std::numeric_limits::quiet_NaN(); + EXPECT_EQ(fpclassify(a), FP_INFINITE); + EXPECT_FALSE(isfinite(a)); + EXPECT_FALSE(isnan(a)); + EXPECT_FALSE(isnormal(a)); + EXPECT_TRUE(signbit(a)); + EXPECT_TRUE(isinf(a)); } TEST(JetTraitsTest, ClassificationFinite) { - Jet a(5.5, 0); - a.v[0] = 100.0; - a.v[1] = 1.0; - a.v[2] = 3.14159; - EXPECT_TRUE(IsFinite(a)); - EXPECT_TRUE(IsNormal(a)); - EXPECT_FALSE(IsInfinite(a)); - EXPECT_FALSE(IsNaN(a)); + Jet a(-5.5); + a.v << std::numeric_limits::quiet_NaN(); + EXPECT_EQ(fpclassify(a), FP_NORMAL); + EXPECT_FALSE(isinf(a)); + EXPECT_FALSE(isnan(a)); + EXPECT_TRUE(signbit(a)); + EXPECT_TRUE(isfinite(a)); + EXPECT_TRUE(isnormal(a)); +} + +TEST(JetTraitsTest, ClassificationScalar) { + EXPECT_EQ(fpclassify(J0d{+0.0}), FP_ZERO); + EXPECT_EQ(fpclassify(J0d{-0.0}), FP_ZERO); + EXPECT_EQ(fpclassify(J0d{1.234}), FP_NORMAL); + EXPECT_EQ(fpclassify(J0d{std::numeric_limits::min() / 2}), + FP_SUBNORMAL); + EXPECT_EQ(fpclassify(J0d{std::numeric_limits::quiet_NaN()}), FP_NAN); +} + +TEST(JetTraitsTest, Nested2XClassificationScalar) { + EXPECT_EQ(fpclassify(J0{J0d{+0.0}}), FP_ZERO); + EXPECT_EQ(fpclassify(J0{J0d{-0.0}}), FP_ZERO); + EXPECT_EQ(fpclassify(J0{J0d{1.234}}), FP_NORMAL); + EXPECT_EQ(fpclassify(J0{J0d{std::numeric_limits::min() / 2}}), + FP_SUBNORMAL); + EXPECT_EQ(fpclassify(J0{J0d{std::numeric_limits::quiet_NaN()}}), + FP_NAN); } // The following test ensures that Jets have all the appropriate Eigen @@ -1013,11 +1211,6 @@ class JetTest : public testing::Test {}; TYPED_TEST_SUITE(JetTest, Types); -// Don't care about the dual part for comparison tests -template -using J0 = Jet; -using J0d = J0; - TYPED_TEST(JetTest, Comparison) { using Scalar = TypeParam; diff --git a/internal/ceres/jet_traits_test.cc b/internal/ceres/jet_traits_test.cc new file mode 100644 index 000000000..ee38f4712 --- /dev/null +++ b/internal/ceres/jet_traits_test.cc @@ -0,0 +1,122 @@ +// Ceres Solver - A fast non-linear least squares minimizer +// Copyright 2022 Google Inc. All rights reserved. +// http://ceres-solver.org/ +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * 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. +// * Neither the name of Google Inc. nor the names of its contributors 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 OWNER 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. +// +// Author: sergiu.deitsch@gmail.com (Sergiu Deitsch) + +#include "ceres/internal/jet_traits.h" + +#include +#include +#include + +namespace ceres { +namespace internal { + +using J = Jet; +// Don't care about the dual part for scalar part categorization and comparison +// tests +template +using J0 = Jet; +using J0d = J0; + +struct NotAJet {}; + +static_assert(IsJet_v, "Jet is not identified as one"); +static_assert(IsJet_v>, "Jet is not identified as one"); +static_assert(IsJet_v>, "nested Jet is not identified as one"); +static_assert(IsJet_v>>, "nested Jet is not identified as one"); + +static_assert(!IsJet_v, "double must not be a Jet"); +static_assert(!IsJet_v, "Eigen::VectorXd must not be a Jet"); +static_assert(!IsJet_v() * + std::declval())>, + "product of Eigen::MatrixXd must not be a Jet"); +static_assert(!IsJet_v, "NotAJet must not be a Jet"); + +// Extract the ranks of given types +using Ranks001 = Ranks_t, double, Jet>; +using Ranks1 = Ranks_t>; +using Ranks110 = Ranks_t, Jet, double>; +using Ranks023 = Ranks_t, Jet>; +using EmptyRanks = Ranks_t<>; + +// Ensure extracted ranks match the expected integer sequence +static_assert( + std::is_same>::value, + "ranks do not match"); +static_assert(std::is_same>::value, + "ranks do not match"); +static_assert( + std::is_same>::value, + "ranks do not match"); +static_assert( + std::is_same>::value, + "ranks do not match"); +static_assert(std::is_same>::value, + "ranks sequence is not empty"); + +// Extract the underlying floating-point type +static_assert(std::is_same, double>::value, + "underlying type is not a double"); +static_assert(std::is_same, double>::value, + "underlying type is not a double"); +static_assert(std::is_same>, double>::value, + "underlying type is not a double"); +static_assert(std::is_same>>, double>::value, + "underlying type is not a double"); + +static_assert(CompatibleJetOperands_v, Jet>, + "Jets must be compatible"); +static_assert(CompatibleJetOperands_v, double>, + "Jet and scalar must be compatible"); +static_assert(CompatibleJetOperands_v>, + "single Jet must be compatible"); +static_assert(!CompatibleJetOperands_v, double, Jet>, + "Jets and scalar must not be compatible"); +static_assert(!CompatibleJetOperands_v, + "scalars must not be compatible"); +static_assert(!CompatibleJetOperands_v, + "single scalar must not be compatible"); +static_assert(!CompatibleJetOperands_v<>, + "empty arguments must not be compatible"); + +static_assert(!PromotableJetOperands_v, + "single scalar must not be Jet promotable"); +static_assert(!PromotableJetOperands_v, + "multiple scalars must not be Jet promotable"); +static_assert(PromotableJetOperands_v, + "Jet and several scalars must be promotable"); +static_assert(PromotableJetOperands_v, float, int>, + "nested Jet and several scalars must be promotable"); +static_assert(!PromotableJetOperands_v, float, int>, + "Eigen::Array must not be Jet promotable"); +static_assert(!PromotableJetOperands_v, float, int>, + "Eigen::Matrix must not be Jet promotable"); + +} // namespace internal +} // namespace ceres diff --git a/internal/ceres/local_parameterization_test.cc b/internal/ceres/local_parameterization_test.cc index 2847737f1..2321ce7bd 100644 --- a/internal/ceres/local_parameterization_test.cc +++ b/internal/ceres/local_parameterization_test.cc @@ -293,7 +293,7 @@ void QuaternionParameterizationTestHelper(const double* x, double jacobian[12]; parameterization.ComputeJacobian(x, jacobian); for (int i = 0; i < 12; ++i) { - EXPECT_TRUE(IsFinite(jacobian[i])); + EXPECT_TRUE(isfinite(jacobian[i])); EXPECT_NEAR(jacobian[i], jacobian_ref[i], kTolerance) << "Jacobian mismatch: i = " << i << "\n Expected \n" << ConstMatrixRef(jacobian_ref, kGlobalSize, kLocalSize) @@ -524,7 +524,7 @@ static void HomogeneousVectorParameterizationHelper(const double* x, autodiff_jacobian.ComputeJacobian(x, jacobian_autodiff); for (int i = 0; i < 12; ++i) { - EXPECT_TRUE(ceres::IsFinite(jacobian_analytic[i])); + EXPECT_TRUE(ceres::isfinite(jacobian_analytic[i])); EXPECT_NEAR(jacobian_analytic[i], jacobian_autodiff[i], kTolerance) << "Jacobian mismatch: i = " << i << ", " << jacobian_analytic[i] << " " << jacobian_autodiff[i]; diff --git a/internal/ceres/rotation_test.cc b/internal/ceres/rotation_test.cc index c2f5e83e6..e28185c6d 100644 --- a/internal/ceres/rotation_test.cc +++ b/internal/ceres/rotation_test.cc @@ -685,8 +685,8 @@ J4 MakeJ4(double a, double v0, double v1, double v2, double v3) { } bool IsClose(double x, double y) { - EXPECT_FALSE(IsNaN(x)); - EXPECT_FALSE(IsNaN(y)); + EXPECT_FALSE(isnan(x)); + EXPECT_FALSE(isnan(y)); return internal::IsClose(x, y, kTolerance, NULL, NULL); }