diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 2910ce74c..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 @@ -88,10 +89,9 @@ bool equal_with_abs_tol(const Eigen::DenseBase& A, const Eigen::DenseBas for(size_t i=0; i tol) + if(!fpEqual(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..beec030ba 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,45 @@ 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; + 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) || isnan(b)) { + return isnan(a) && isnan(b); + } + // handle inf + 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 || (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 if(abs(a-b) <= tol * min(larger, std::numeric_limits::max())) { + return true; + } + + return false; +} + /* ************************************************************************* */ //3 argument call void print(const Vector& v, const string& s, ostream& stream) { @@ -82,9 +122,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 (!fpEqual(vec1[i], vec2[i], tol)) return false; } return true; @@ -95,9 +133,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 (!fpEqual(vec1[i], vec2[i], tol)) return false; } return true; diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h index 99c5a4d42..81be36c0a 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 @@ -75,6 +76,19 @@ 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. + * + * 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 tol); + /** * print without optional string, must specify cout yourself */