Added some Eigen GTest functions

fuse_core/include/fuse_core/eigen_gtest.h

@@ -0,0 +1,138 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, Locus Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef FUSE_CORE_EIGEN_GTEST_H
+#define FUSE_CORE_EIGEN_GTEST_H
+
+#include <Eigen/Core>
+#include <gtest/gtest.h>
+
+/**
+ * @file eigen_gtest.h
+ *
+ * @brief Provides ASSERT_MATRIX_EQ()/EXPECT_MATRIX_EQ() and ASSERT_MATRIX_NEAR()/EXPECT_MATRIX_NEAR()
+ *        gtest macros
+ */
+
+namespace testing
+{
+
+/**
+ * @brief Internal helper function for implementing {EXPECT|ASSERT}_MATRIX_NEAR.
+ *
+ * Don't use this in your code.
+ * @param[in] e1  Expected matrix name
+ * @param[in] e2  Actual matrix name
+ * @param[in] v1  Expected matrix
+ * @param[in] v2  Actual matrix
+ * @param[in] tol Tolerance
+ * @return AssertionSuccess or AssertionFailure
+ */
+template <typename Derived>
+AssertionResult AssertMatrixEqualHelper(
+  const char* e1,
+  const char* e2,
+  const Eigen::MatrixBase<Derived>& v1,
+  const Eigen::MatrixBase<Derived>& v2)
+{
+  if (v1 == v2)
+  {
+    return AssertionSuccess();
+  }
+
+  Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]");
+  return AssertionFailure() << e1 << " is:\n" << v1.format(clean) << "\n"
+                            << e2 << " is:\n" << v2.format(clean) << "\n"
+                            << "Difference is:\n" << (v1 - v2).format(clean) << "\n";
+}
+
+/**
+ * @brief Internal helper function for implementing {EXPECT|ASSERT}_MATRIX_NEAR.
+ *
+ * Don't use this in your code.
+ * @param[in] e1  Expected matrix name
+ * @param[in] e2  Actual matrix name
+ * @param[in] v1  Expected matrix
+ * @param[in] v2  Actual matrix
+ * @param[in] tol Tolerance
+ * @return AssertionSuccess or AssertionFailure
+ */
+template <typename Derived>
+AssertionResult AssertMatrixNearHelper(
+  const char* e1,
+  const char* e2,
+  const Eigen::MatrixBase<Derived>& v1,
+  const Eigen::MatrixBase<Derived>& v2,
+  double tol)
+{
+  if (v1.isApprox(v2, tol))
+  {
+    return AssertionSuccess();
+  }
+
+  Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]");
+  return AssertionFailure() << e1 << " is:\n" << v1.format(clean) << "\n"
+                            << e2 << " is:\n" << v2.format(clean) << "\n"
+                            << "Difference is:\n" << (v1 - v2).format(clean) << "\n";
+}
+
+// Internal macro for implementing {EXPECT|ASSERT}_MATRIX_EQUAL.
+// Don't use this in your code.
+#define GTEST_MATRIX_EQUAL_(v1, v2, on_failure)           \
+  GTEST_ASSERT_(::testing::AssertMatrixEqualHelper(#v1,   \
+                                                   #v2,   \
+                                                    v1,   \
+                                                    v2), on_failure)
+
+// Internal macro for implementing {EXPECT|ASSERT}_MATRIX_NEAR.
+// Don't use this in your code.
+#define GTEST_MATRIX_NEAR_(v1, v2, tol, on_failure)      \
+  GTEST_ASSERT_(::testing::AssertMatrixNearHelper(#v1,   \
+                                                  #v2,   \
+                                                   v1,   \
+                                                   v2,   \
+                                                   tol), on_failure)
+
+// Define gtest macros for use with Eigen
+#define EXPECT_MATRIX_EQ(v1, v2) \
+    GTEST_MATRIX_EQUAL_(v1, v2, GTEST_NONFATAL_FAILURE_)
+#define ASSERT_MATRIX_EQ(v1, v2) \
+    GTEST_MATRIX_EQUAL_(v1, v2, GTEST_FATAL_FAILURE_)
+#define EXPECT_MATRIX_NEAR(v1, v2, tol) \
+    GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_NONFATAL_FAILURE_)
+#define ASSERT_MATRIX_NEAR(v1, v2, tol) \
+    GTEST_MATRIX_NEAR_(v1, v2, tol, GTEST_FATAL_FAILURE_)
+
+}  // namespace testing
+
+#endif  // FUSE_CORE_EIGEN_GTEST_H

fuse_core/test/test_local_parameterization.cpp

@@ -33,8 +33,8 @@
  */
 #include <fuse_core/autodiff_local_parameterization.h>
 #include <fuse_core/eigen.h>
+#include <fuse_core/eigen_gtest.h>
 
-#include <Eigen/Core>
 #include <gtest/gtest.h>
 
 
@@ -86,19 +86,16 @@ TEST(LocalParameterization, PlusJacobian)
   TestLocalParameterization parameterization;
 
   double x[3] = {1.0, 2.0, 3.0};
-  fuse_core::MatrixXd actual(2, 3);
+  fuse_core::MatrixXd actual(3, 2);
   bool success = parameterization.ComputeJacobian(x, actual.data());
 
-  fuse_core::MatrixXd expected(2, 3);
+  fuse_core::MatrixXd expected(3, 2);
   expected << 2.0, 0.0,
               0.0, 5.0,
               0.0, 0.0;
 
-  Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]");
   EXPECT_TRUE(success);
-  EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n"
-                                                 << "Actual is:\n" << actual.format(clean) << "\n"
-                                                 << "Difference is:\n" << (expected - actual).format(clean) << "\n";
+  EXPECT_MATRIX_NEAR(expected, actual, 1.0e-5);
 }
 
 TEST(LocalParameterization, Minus)
@@ -120,18 +117,16 @@ TEST(LocalParameterization, MinusJacobian)
   TestLocalParameterization parameterization;
 
   double x[3] = {1.0, 2.0, 3.0};
-  fuse_core::MatrixXd actual(3, 2);
+  fuse_core::MatrixXd actual(2, 3);
   bool success = parameterization.ComputeMinusJacobian(x, actual.data());
 
-  fuse_core::MatrixXd expected(3, 2);
+  fuse_core::MatrixXd expected(2, 3);
   expected << 0.5, 0.0, 0.0,
               0.0, 0.2, 0.0;
 
-  Eigen::IOFormat clean(4, 0, ", ", "\n", "[", "]");
   EXPECT_TRUE(success);
-  EXPECT_TRUE(expected.isApprox(actual, 1.0e-5)) << "Expected is:\n" << expected.format(clean) << "\n"
-                                                 << "Actual is:\n" << actual.format(clean) << "\n"
-                                                 << "Difference is:\n" << (expected - actual).format(clean) << "\n";
+  EXPECT_MATRIX_NEAR(expected, actual, 1.0e-5);
+  EXPECT_MATRIX_EQ(expected, actual);
 }
 
 int main(int argc, char **argv)