Add gtest equal

CMakeLists.txt

@@ -6,30 +6,30 @@ catkin_simple()
 
 add_definitions(--std=c++0x -Wall -Wextra -pedantic)
 
-cs_add_library(${PROJECT_NAME} src/eigen_checks.cc)
+cs_add_library(${PROJECT_NAME} src/eigen-checks.cc)
 
 catkin_add_gtest(test_gtest_near
-	test/test_gtest_near.cc)
+	test/test_gtest-near.cc)
 target_link_libraries(test_gtest_near ${PROJECT_NAME})
 
 catkin_add_gtest(test_gtest_equal
-  test/test_gtest_equal.cc)
+  test/test_gtest-equal.cc)
 target_link_libraries(test_gtest_equal ${PROJECT_NAME})
 
 catkin_add_gtest(test_gtest_equal_double
-  test/test_gtest_equal_double.cc)
+  test/test_gtest-equal-double.cc)
 target_link_libraries(test_gtest_equal_double ${PROJECT_NAME})
 
 catkin_add_gtest(test_glog_near
-  test/test_glog_near.cc)
+  test/test_glog-near.cc)
 target_link_libraries(test_glog_near ${PROJECT_NAME})
 
 catkin_add_gtest(test_glog_equal
-  test/test_glog_equal.cc)
+  test/test_glog-equal.cc)
 target_link_libraries(test_glog_equal ${PROJECT_NAME})
 
 catkin_add_gtest(test_glog_equal_double
-  test/test_glog_equal_double.cc)
+  test/test_glog-equal-double.cc)
 target_link_libraries(test_glog_equal_double ${PROJECT_NAME})
 
 cs_install()

include/eigen-checks/gtest.h

@@ -0,0 +1,17 @@
+#ifndef EIGEN_CHECKS_GTEST_H_
+#define EIGEN_CHECKS_GTEST_H_
+#include <eigen-checks/internal/gtest-equal.h>
+
+#define EIGEN_MATRIX_EQUAL(MatrixA, MatrixB) \
+  eigen_checks::internal::matricesNear(MatrixA, #MatrixA, MatrixB, #MatrixB,\
+      static_cast<decltype(MatrixA)::Scalar>(0.0));
+
+#define EIGEN_MATRIX_EQUAL_DOUBLE(MatrixA, MatrixB) \
+  eigen_checks::internal::matricesNear(MatrixA, #MatrixA, MatrixB, #MatrixB, \
+      eigen_checks::internal::kDefaultPrecision);
+
+#define EIGEN_MATRIX_NEAR(MatrixA, MatrixB, Precision) \
+  eigen_checks::internal::matricesNear(MatrixA, #MatrixA, MatrixB, #MatrixB, \
+                                       Precision);
+
+#endif  // EIGEN_CHECKS_GTEST_H_

include/eigen-checks/internal/gtest-equal.h

@@ -0,0 +1,63 @@
+#ifndef EIGEN_CHECKS_INTERNAL_GTEST_H_
+#define EIGEN_CHECKS_INTERNAL_GTEST_H_
+
+#include <Eigen/Dense>
+#include <gtest/gtest.h>
+
+namespace eigen_checks {
+namespace internal {
+constexpr double kDefaultPrecision = 1e-20;
+
+template<typename LHSMatrix, typename RHSMatrix>
+::testing::AssertionResult matricesEqual(
+    const Eigen::MatrixBase<LHSMatrix>& lhs,
+    const std::string& name_lhs,
+    const Eigen::MatrixBase<RHSMatrix>& rhs,
+    const std::string& name_rhs,
+    typename Eigen::MatrixBase<LHSMatrix>::Scalar tolerance) {
+  if (lhs.rows() != rhs.rows()) {
+    ::testing::AssertionResult failure_reason(false);
+    failure_reason << "Matrices have a different number of rows: "
+        << name_lhs << " has " << lhs.rows() << " rows while " << name_rhs
+        << " has " << rhs.rows() << " rows." << std::endl;
+    return failure_reason;
+  }
+  if (lhs.cols() != rhs.cols()) {
+    ::testing::AssertionResult failure_reason(false);
+    failure_reason << "Matrices have a different number of cols: "
+        << name_lhs << " has " << lhs.cols() << " cols while " << name_rhs
+        << " cols " << rhs.cols() << " cols." << std::endl;
+    return failure_reason;
+  }
+
+  if (lhs.isApprox(rhs, tolerance)) {
+    return ::testing::AssertionSuccess();
+  } else {
+    ::testing::AssertionResult failure_reason(false);
+    for (int i = 0; i < lhs.rows(); ++i) {
+      for (int j = 0; j < lhs.cols(); ++j) {
+        const double& lij = lhs(i, j);
+        const double& rij = rhs(i, j);
+        if (!std::isfinite(lij) ||
+            !std::isfinite(rij) ||
+            !Eigen::internal::isApprox(lij, rij, tolerance)) {
+          if (lhs.rows() == 1) {
+            failure_reason <<
+                "\nMismatch at position " << j << ": " << lij << " != " << lij;
+          } else if (lhs.cols() == 1) {
+            failure_reason <<
+                "\nMismatch at position " << i << ": " << lij << " != " << rij;
+          } else {
+            failure_reason << "\nMismatch at "
+                << i << "," << j << ": " << lij << " != " << rij;
+          }
+        }
+      }
+    }
+    failure_reason << "\n";
+    return failure_reason;
+  }
+}
+}  // namespace internal
+}  // namespace eigen_checks
+#endif  // EIGEN_CHECKS_INTERNAL_GTEST_H_

test/test_gtest_near.cc → test/test_gtest-equal.cc

@@ -1,4 +1,5 @@
 #include <eigen-checks/entrypoint.h>
+#include <eigen-checks/gtest.h>