From 4befe7a01fa4d22b23a4f7c9408479923ae9006f Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Sat, 7 Mar 2020 22:40:23 -0500 Subject: [PATCH 1/5] improved floating point comparison for better numerical stability --- gtsam/base/Matrix.h | 5 ++--- gtsam/base/Vector.cpp | 32 ++++++++++++++++++++++++++------ gtsam/base/Vector.h | 12 ++++++++++++ 3 files changed, 40 insertions(+), 9 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 2910ce74c..04bddb111 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -88,10 +88,9 @@ bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBas for(size_t i=0; i tol) + if(!fp_isequal(A(i,j), B(i,j), tol)) { return false; + } } return true; } diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index ac03c0f53..f00f9fddf 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -14,6 +14,7 @@ * @brief typedef and functions to augment Eigen's Vectors * @author Kai Ni * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -33,6 +34,29 @@ using namespace std; namespace gtsam { +/* ************************************************************************* */ +bool fp_isequal(double a, double b, double epsilon) { + double DOUBLE_MIN_NORMAL = std::numeric_limits::min() + 1.0; + + // handle NaNs + if(std::isnan(a) ^ std::isnan(b)) { + return false; + } + // handle inf + else if(std::isinf(a) ^ std::isinf(b)) { + return false; + } + // If the two values are zero or both are extremely close to it + // relative error is less meaningful here + else if(a == 0 || b == 0 || (std::abs(a) + std::abs(b)) < DOUBLE_MIN_NORMAL) { + return std::abs(a-b) < epsilon * DOUBLE_MIN_NORMAL; + } + // Use relative error + else { + return std::abs(a-b) < epsilon * std::min((std::abs(a) + std::abs(b)), std::numeric_limits::max()); + } +} + /* ************************************************************************* */ //3 argument call void print(const Vector& v, const string& s, ostream& stream) { @@ -82,9 +106,7 @@ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) { if (vec1.size()!=vec2.size()) return false; size_t m = vec1.size(); for(size_t i=0; i tol) + if (!fp_isequal(vec1[i], vec2[i], tol)) return false; } return true; @@ -95,9 +117,7 @@ bool equal_with_abs_tol(const SubVector& vec1, const SubVector& vec2, double tol if (vec1.size()!=vec2.size()) return false; size_t m = vec1.size(); for(size_t i=0; i tol) + if (!fp_isequal(vec1[i], vec2[i], tol)) return false; } return true; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 99c5a4d42..1b51a1b99 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -15,6 +15,7 @@ * @author Kai Ni * @author Frank Dellaert * @author Alex Hagiopol + * @author Varun Agrawal */ // \callgraph @@ -24,6 +25,8 @@ #define MKL_BLAS MKL_DOMAIN_BLAS #endif +#include +#include #include #include #include @@ -75,6 +78,15 @@ static_assert( "Error: GTSAM was built against a different version of Eigen"); #endif +/** + * Numerically stable function for comparing if floating point values are equal + * within epsilon tolerance. + * Used for vector and matrix comparison with C++11 compatible functions. + * Reference : https://floating-point-gui.de/errors/comparison/ + * Return true if two numbers are close wrt epsilon. + */ +GTSAM_EXPORT bool fp_isequal(double a, double b, double epsilon); + /** * print without optional string, must specify cout yourself */ From 670cc7a74ee10774fd793317b65f6d958073ae4b Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Mar 2020 10:50:12 -0400 Subject: [PATCH 2/5] correct handling of NaNs and Infs, camelCase, and better namespace handling --- gtsam/base/Matrix.h | 2 +- gtsam/base/Vector.cpp | 36 ++++++++++++++++++++++++------------ gtsam/base/Vector.h | 10 ++++++---- 3 files changed, 31 insertions(+), 17 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 04bddb111..1ee1a6c4a 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -88,7 +88,7 @@ bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBas for(size_t i=0; i::min() + 1.0; +bool fpEqual(double a, double b, double tol) { + using std::abs; + using std::isnan; + using std::isinf; + + double DOUBLE_MIN_NORMAL = numeric_limits::min() + 1.0; + double larger = (abs(b) > abs(a)) ? abs(b) : abs(a); // handle NaNs - if(std::isnan(a) ^ std::isnan(b)) { - return false; + if(std::isnan(a) || isnan(b)) { + return isnan(a) && isnan(b); } // handle inf - else if(std::isinf(a) ^ std::isinf(b)) { - return false; + else if(isinf(a) || isinf(b)) { + return isinf(a) && isinf(b); } // If the two values are zero or both are extremely close to it // relative error is less meaningful here - else if(a == 0 || b == 0 || (std::abs(a) + std::abs(b)) < DOUBLE_MIN_NORMAL) { - return std::abs(a-b) < epsilon * DOUBLE_MIN_NORMAL; + else if(a == 0 || b == 0 || (abs(a) + abs(b)) < DOUBLE_MIN_NORMAL) { + return abs(a-b) <= tol * DOUBLE_MIN_NORMAL; + } + // Check if the numbers are really close + // Needed when comparing numbers near zero or tol is in vicinity + else if(abs(a-b) <= tol) { + return true; } // Use relative error - else { - return std::abs(a-b) < epsilon * std::min((std::abs(a) + std::abs(b)), std::numeric_limits::max()); + else if(abs(a-b) <= tol * min(larger, std::numeric_limits::max())) { + return true; } + + return false; } /* ************************************************************************* */ @@ -106,7 +118,7 @@ bool equal_with_abs_tol(const Vector& vec1, const Vector& vec2, double tol) { if (vec1.size()!=vec2.size()) return false; size_t m = vec1.size(); for(size_t i=0; i -#include #include #include +#include #include +#include #include namespace gtsam { @@ -82,10 +82,12 @@ static_assert( * Numerically stable function for comparing if floating point values are equal * within epsilon tolerance. * Used for vector and matrix comparison with C++11 compatible functions. - * Reference : https://floating-point-gui.de/errors/comparison/ + * References: + * 1. https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ + * 2. https://floating-point-gui.de/errors/comparison/ * Return true if two numbers are close wrt epsilon. */ -GTSAM_EXPORT bool fp_isequal(double a, double b, double epsilon); +GTSAM_EXPORT bool fpEqual(double a, double b, double epsilon); /** * print without optional string, must specify cout yourself From 8b00840169e8c481376f03051327847ae74f842a Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Mon, 9 Mar 2020 16:33:56 -0400 Subject: [PATCH 3/5] removed header bloat and added reference comment for fpEqual implementation --- gtsam/base/Matrix.h | 1 + gtsam/base/Vector.cpp | 6 +++++- gtsam/base/Vector.h | 2 -- 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 1ee1a6c4a..fa70e5b00 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -17,6 +17,7 @@ * @author Frank Dellaert * @author Alex Cunningham * @author Alex Hagiopol + * @author Varun Agrawal */ // \callgraph diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp index 8c8b15416..beec030ba 100644 --- a/gtsam/base/Vector.cpp +++ b/gtsam/base/Vector.cpp @@ -34,7 +34,11 @@ using namespace std; namespace gtsam { -/* ************************************************************************* */ +/* ************************************************************************* + * References: + * 1. https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ + * 2. https://floating-point-gui.de/errors/comparison/ + * ************************************************************************* */ bool fpEqual(double a, double b, double tol) { using std::abs; using std::isnan; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 035a6fa87..576717a0c 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -27,9 +27,7 @@ #include #include -#include #include -#include #include namespace gtsam { From 0a91c081fb18301549ca8c82a3fe518420e4b081 Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Tue, 10 Mar 2020 14:18:07 -0400 Subject: [PATCH 4/5] moved references for implementation from header to cpp --- gtsam/base/Vector.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 576717a0c..bd72a0473 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -80,9 +80,6 @@ static_assert( * Numerically stable function for comparing if floating point values are equal * within epsilon tolerance. * Used for vector and matrix comparison with C++11 compatible functions. - * References: - * 1. https://randomascii.wordpress.com/2012/02/25/comparing-floating-point-numbers-2012-edition/ - * 2. https://floating-point-gui.de/errors/comparison/ * Return true if two numbers are close wrt epsilon. */ GTSAM_EXPORT bool fpEqual(double a, double b, double epsilon); From b5e975f7e4969718e9ed835da6457aa59b32f8cd Mon Sep 17 00:00:00 2001 From: Varun Agrawal Date: Fri, 13 Mar 2020 19:42:14 -0400 Subject: [PATCH 5/5] added documentation to fpEqual on behavior for NaNs/Infs --- gtsam/base/Vector.h | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index bd72a0473..81be36c0a 100644 --- a/gtsam/base/Vector.h +++ b/gtsam/base/Vector.h @@ -80,9 +80,14 @@ static_assert( * Numerically stable function for comparing if floating point values are equal * within epsilon tolerance. * Used for vector and matrix comparison with C++11 compatible functions. - * Return true if two numbers are close wrt epsilon. + * + * If either value is NaN or Inf, we check for both values to be NaN or Inf + * respectively for the comparison to be true. + * If one is NaN/Inf and the other is not, returns false. + * + * Return true if two numbers are close wrt tol. */ -GTSAM_EXPORT bool fpEqual(double a, double b, double epsilon); +GTSAM_EXPORT bool fpEqual(double a, double b, double tol); /** * print without optional string, must specify cout yourself