Merge pull request #580 from borglab/fix/numerical-error

Flag for relative error

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, false)) {
         return false;
       }
     }

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 check_relative_also) {
   using std::abs;
   using std::isnan;
   using std::isinf;
@@ -48,7 +48,7 @@ bool fpEqual(double a, double b, double tol) {
   double larger = (abs(b) > abs(a)) ? abs(b) : abs(a);
 
   // handle NaNs
-  if(std::isnan(a) || isnan(b)) {
+  if(isnan(a) || isnan(b)) {
     return isnan(a) && isnan(b);
   }
   // handle inf
@@ -60,13 +60,15 @@ bool fpEqual(double a, double b, double tol) {
   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
+  // Check if the numbers are really close.
-  // Needed when comparing numbers near zero or tol is in vicinity
+  // Needed when comparing numbers near zero or tol is in vicinity.
   else if (abs(a - b) <= tol) {
     return true;
   }
-  // Use relative error
+  // Check for 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()) &&
+           check_relative_also) {
     return true;
   }
 

gtsam/base/Vector.h

@@ -85,9 +85,15 @@ static_assert(
  * respectively for the comparison to be true.
  * If one is NaN/Inf and the other is not, returns false.
  *
+ * @param check_relative_also is a flag which toggles additional checking for
+ * relative error. This means that if either the absolute error or the relative
+ * error is within the tolerance, the result will be true.
+ * By default, the flag is true.
+ *
  * 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 check_relative_also = true);
 
 /**
  * print without optional string, must specify cout yourself

gtsam/base/tests/testMatrix.cpp

@@ -1163,6 +1163,19 @@ TEST(Matrix , IsVectorSpace) {
   BOOST_CONCEPT_ASSERT((IsVectorSpace<Vector5>));
 }
 
+TEST(Matrix, AbsoluteError) {
+  double a = 2000, b = 1997, tol = 1e-1;
+  bool isEqual;
+
+  // Test only absolute error
+  isEqual = fpEqual(a, b, tol, false);
+  EXPECT(!isEqual);
+
+  // Test relative error as well
+  isEqual = fpEqual(a, b, tol);
+  EXPECT(isEqual);
+}
+
 /* ************************************************************************* */
 int main() {
   TestResult tr;

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, M_PI / 2 - 1e-1, 0}, 1e-7});
-  test_xyz.push_back(VecAndErr{{0, 3 * M_PI / 2 + 1e-1, 0}, 1e-8});
+  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});