improved floating point comparison for better numerical stability

gtsam/base/Matrix.h

@@ -88,10 +88,9 @@ bool equal_with_abs_tol(const Eigen::DenseBase<MATRIX>& A, const Eigen::DenseBas
 
   for(size_t i=0; i<m1; i++)
     for(size_t j=0; j<n1; j++) {
-      if(boost::math::isnan(A(i,j)) ^ boost::math::isnan(B(i,j)))
+      if(!fp_isequal(A(i,j), B(i,j), tol)) {
-        return false;
-      else if(std::abs(A(i,j) - B(i,j)) > tol)
         return false;
+      }
     }
   return true;
 }

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 <gtsam/base/Vector.h>
@@ -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<double>::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<double>::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<m; ++i) {
-    if(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
+    if (!fp_isequal(vec1[i], vec2[i], tol))
-      return false;
-    if(std::abs(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<m; ++i) {
-    if(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
+    if (!fp_isequal(vec1[i], vec2[i], tol))
-      return false;
-    if(std::abs(vec1[i] - vec2[i]) > tol)
       return false;
   }
   return true;

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 <cmath>
+#include <limits>
 #include <gtsam/global_includes.h>
 #include <Eigen/Core>
 #include <iosfwd>
@@ -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
  */