From 4befe7a01fa4d22b23a4f7c9408479923ae9006f Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Sat, 7 Mar 2020 22:40:23 -0500
Subject: [PATCH] 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<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)))
-        return false;
-      else if(std::abs(A(i,j) - B(i,j)) > 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 <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]))
-      return false;
-    if(std::abs(vec1[i] - vec2[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<m; ++i) {
-    if(std::isnan(vec1[i]) ^ std::isnan(vec2[i]))
-      return false;
-    if(std::abs(vec1[i] - vec2[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 <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
  */