From 51b6fd0b43852bf14148b39c14d2563f2d87c3d4 Mon Sep 17 00:00:00 2001
From: Varun Agrawal <varagrawal@gmail.com>
Date: Tue, 3 Nov 2020 12:11:17 -0500
Subject: [PATCH] Added flag for absolute error

---
 gtsam/base/Matrix.h               | 2 +-
 gtsam/base/Vector.cpp             | 6 ++++--
 gtsam/base/Vector.h               | 3 ++-
 gtsam/geometry/tests/testRot3.cpp | 6 +++---
 4 files changed, 10 insertions(+), 7 deletions(-)

diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h
index 071c33fca..9adf4e1c1 100644
--- a/gtsam/base/Matrix.h
+++ b/gtsam/base/Matrix.h
@@ -90,7 +90,7 @@ 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(!fpEqual(A(i,j), B(i,j), tol)) {
+      if(!fpEqual(A(i,j), B(i,j), tol, true)) {
         return false;
       }
     }
diff --git a/gtsam/base/Vector.cpp b/gtsam/base/Vector.cpp
index beec030ba..ae4d75455 100644
--- a/gtsam/base/Vector.cpp
+++ b/gtsam/base/Vector.cpp
@@ -39,7 +39,7 @@ namespace gtsam {
  * 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) {
+bool fpEqual(double a, double b, double tol, bool absolute) {
   using std::abs;
   using std::isnan;
   using std::isinf;
@@ -66,7 +66,9 @@ bool fpEqual(double a, double b, double tol) {
     return true;
   }
   // Use relative error
-  else if(abs(a-b) <= tol * min(larger, std::numeric_limits<double>::max())) {
+  else if (abs(a - b) <=
+               tol * min(larger, std::numeric_limits<double>::max()) &&
+           !absolute) {
     return true;
   }
 
diff --git a/gtsam/base/Vector.h b/gtsam/base/Vector.h
index 319ad6ee6..b0fc74f26 100644
--- a/gtsam/base/Vector.h
+++ b/gtsam/base/Vector.h
@@ -87,7 +87,8 @@ static_assert(
  *
  * Return true if two numbers are close wrt tol.
  */
-GTSAM_EXPORT bool fpEqual(double a, double b, double tol);
+GTSAM_EXPORT bool fpEqual(double a, double b, double tol,
+                          bool absolute = false);
 
 /**
  * print without optional string, must specify cout yourself
diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp
index 7b792f8bd..889f68580 100644
--- a/gtsam/geometry/tests/testRot3.cpp
+++ b/gtsam/geometry/tests/testRot3.cpp
@@ -807,15 +807,15 @@ TEST(Rot3, RQ_derivative) {
   test_xyz.push_back(VecAndErr{{0, 0, 0}, error});
   test_xyz.push_back(VecAndErr{{0, 0.5, -0.5}, error});
   test_xyz.push_back(VecAndErr{{0.3, 0, 0.2}, error});
-  test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, error});
+  test_xyz.push_back(VecAndErr{{-0.6, 1.3, 0}, 1e-8});
   test_xyz.push_back(VecAndErr{{1.0, 0.7, 0.8}, error});
   test_xyz.push_back(VecAndErr{{3.0, 0.7, -0.6}, error});
   test_xyz.push_back(VecAndErr{{M_PI / 2, 0, 0}, error});
   test_xyz.push_back(VecAndErr{{0, 0, M_PI / 2}, error});
 
   // Test close to singularity
-  test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-8});
-  test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-8});
+  test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1e-1, 0}, 1e-7});
+  test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-7});
   test_xyz.push_back(VecAndErr{{0, M_PI / 2 - 1.1e-2, 0}, 1e-4});
   test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1.1e-2, 0}, 1e-4});