Add testCovariance helper function

Author: Enrique Fernandez

diff_drive_controller/include/diff_drive_controller/covariance.h

@@ -41,6 +41,8 @@
 
 #include <Eigen/Dense>
 
+#include <boost/array.hpp>
+
 #include <limits>
 
 namespace diff_drive_controller

diff_drive_controller/include/diff_drive_controller/rigid_body_motion.h

@@ -78,6 +78,8 @@ static void integrate_motion(double& x, double &y, double &yaw,
  * \param[in] v_y   Velocity/Twist y   component
  * \param[in] v_yaw Velocity/Twist yaw component
  * \param[in] dt Time step
+ * \param[out] J_pose Jacobian wrt the pose
+ * \param[out] J_twist Jacobian wrt the velocity/twist
  */
 static void integrate_motion(double& x, double &y, double &yaw,
     const double v_x, const double v_y, const double v_yaw,

diff_drive_controller/test/diff_drive_test.cpp

@@ -114,40 +114,17 @@ TEST_F(DiffDriveControllerTest, testNoMove)
 
   // check we have a positive definite covariance matrix
   // isCovariance = isSymmetric + isPositiveDefinite
-  const Eigen::IOFormat HeavyFmt(
-      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
-
-  typedef Eigen::SelfAdjointEigenSolver<PoseCovariance> PoseEigenSolver;
-  typedef Eigen::SelfAdjointEigenSolver<TwistCovariance> TwistEigenSolver;
-
-  PoseEigenSolver pose_eigensolver(pose_covariance);
-  TwistEigenSolver twist_eigensolver(twist_covariance);
-
-  EXPECT_TRUE(isSymmetric(pose_covariance))
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt);
-  EXPECT_TRUE(isPositiveDefinite(pose_covariance, no_tag()))
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-  EXPECT_LT(conditionNumber(pose_covariance),
-      POSE_COVARIANCE_MAX_CONDITION_NUMBER)
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-
-  EXPECT_TRUE(isSymmetric(twist_covariance))
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt);
-  EXPECT_TRUE(isPositiveDefinite(twist_covariance, no_tag()))
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-  EXPECT_LT(conditionNumber(pose_covariance),
-      TWIST_COVARIANCE_MAX_CONDITION_NUMBER)
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt);
+  testCovariance(pose_covariance, POSE_COVARIANCE_MAX_CONDITION_NUMBER);
+  testCovariance(twist_covariance, TWIST_COVARIANCE_MAX_CONDITION_NUMBER);
 
   // when the robot doesn't move the twist covariance should be exactly the
   // minimum twist covariance, which is defined as a diagonal covariance matrix
   // like this:
   //   Odometry::DEFAULT_MINIMUM_TWIST_COVARIANCE * TwistCovariance::Identity()
   // where Odometry::DEFAULT_MINIMUM_TWIST_COVARIANCE == 1e-9
+  const Eigen::IOFormat HeavyFmt(
+      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
+
   TwistCovariance minimum_twist_covariance = 1e-9 * TwistCovariance::Identity();
   EXPECT_TRUE(((twist_covariance - minimum_twist_covariance).array() == 0).all())
     << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
@@ -306,34 +283,8 @@ TEST_F(DiffDriveControllerTest, testForward)
 
   // check we have a positive definite covariance matrix
   // isCovariance = isSymmetric + isPositiveDefinite
-  const Eigen::IOFormat HeavyFmt(
-      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
-
-  typedef Eigen::SelfAdjointEigenSolver<PoseCovariance> PoseEigenSolver;
-  typedef Eigen::SelfAdjointEigenSolver<TwistCovariance> TwistEigenSolver;
-
-  PoseEigenSolver pose_eigensolver(pose_covariance);
-  TwistEigenSolver twist_eigensolver(twist_covariance);
-
-  EXPECT_TRUE(isSymmetric(pose_covariance))
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt);
-  EXPECT_TRUE(isPositiveDefinite(pose_covariance, no_tag()))
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-  EXPECT_LT(conditionNumber(pose_covariance),
-      POSE_COVARIANCE_MAX_CONDITION_NUMBER)
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-
-  EXPECT_TRUE(isSymmetric(twist_covariance))
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt);
-  EXPECT_TRUE(isPositiveDefinite(twist_covariance, no_tag()))
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-  EXPECT_LT(conditionNumber(pose_covariance),
-      TWIST_COVARIANCE_MAX_CONDITION_NUMBER)
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt);
+  testCovariance(pose_covariance, POSE_COVARIANCE_MAX_CONDITION_NUMBER);
+  testCovariance(twist_covariance, TWIST_COVARIANCE_MAX_CONDITION_NUMBER);
 
   // Take the last two odometry messages:
   std::vector<nav_msgs::Odometry> odoms = getLastOdoms();
@@ -406,6 +357,9 @@ TEST_F(DiffDriveControllerTest, testForward)
   EXPECT_NEAR(yaw_0, yaw, std::numeric_limits<double>::epsilon());
 
   // Check new pose covariance is equal to the expected one:
+  const Eigen::IOFormat HeavyFmt(
+      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
+
   EXPECT_TRUE(((pose_covariance_0_expected - pose_covariance_0).array().abs() < 1e-5).all())
     << "Pose covariance actual =\n" << pose_covariance_0.format(HeavyFmt)
     << "\nPose covariance expected =\n" << pose_covariance_0_expected.format(HeavyFmt);
@@ -493,37 +447,8 @@ TEST_F(DiffDriveControllerTest, testTurn)
 
   // check we have a positive definite covariance matrix
   // isCovariance = isSymmetric + isPositiveDefinite
-  const Eigen::IOFormat HeavyFmt(
-      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
-
-  typedef Eigen::SelfAdjointEigenSolver<PoseCovariance> PoseEigenSolver;
-  typedef Eigen::SelfAdjointEigenSolver<TwistCovariance> TwistEigenSolver;
-
-  PoseEigenSolver pose_eigensolver(pose_covariance);
-  TwistEigenSolver twist_eigensolver(twist_covariance);
-
-  EXPECT_TRUE(pose_eigensolver.info() == Eigen::Success);
-  EXPECT_TRUE(twist_eigensolver.info() == Eigen::Success);
-
-  EXPECT_TRUE(isSymmetric(pose_covariance))
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt);
-  EXPECT_TRUE(isPositiveDefinite(pose_covariance, no_tag()))
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-  EXPECT_LT(conditionNumber(pose_covariance),
-      POSE_COVARIANCE_MAX_CONDITION_NUMBER)
-    << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-
-  EXPECT_TRUE(isSymmetric(twist_covariance))
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt);
-  EXPECT_TRUE(isPositiveDefinite(twist_covariance, no_tag()))
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt);
-  EXPECT_LT(conditionNumber(pose_covariance),
-      TWIST_COVARIANCE_MAX_CONDITION_NUMBER)
-    << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n"
-    << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt);
+  testCovariance(pose_covariance, POSE_COVARIANCE_MAX_CONDITION_NUMBER);
+  testCovariance(twist_covariance, TWIST_COVARIANCE_MAX_CONDITION_NUMBER);
 
   // Take the last two odometry messages:
   std::vector<nav_msgs::Odometry> odoms = getLastOdoms();
@@ -596,6 +521,9 @@ TEST_F(DiffDriveControllerTest, testTurn)
   EXPECT_NEAR(yaw_0, yaw, ORIENTATION_TOLERANCE);
 
   // Check new pose covariance is equal to the expected one:
+  const Eigen::IOFormat HeavyFmt(
+      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
+
   EXPECT_TRUE(((pose_covariance_0_expected - pose_covariance_0).array().abs() < 1e-5).all())
     << "Pose covariance actual =\n" << pose_covariance_0.format(HeavyFmt)
     << "\nPose covariance expected =\n" << pose_covariance_0_expected.format(HeavyFmt);

diff_drive_controller/test/gtest_common.h

@@ -0,0 +1,61 @@
+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+//
+// 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 PAL Robotics, Inc. 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 OWNER 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.
+///////////////////////////////////////////////////////////////////////////////
+
+/// \author Enrique Fernandez
+
+#include <diff_drive_controller/covariance.h>
+
+#include <gtest/gtest.h>
+
+#include <Eigen/Dense>
+
+template <typename T, int N>
+void testCovariance(const Eigen::Matrix<T, N, N>& covariance,
+    const double max_condition_number = 1e3)
+{
+  using namespace diff_drive_controller;
+
+  static const Eigen::IOFormat HeavyFmt(
+      Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
+
+  typedef Eigen::SelfAdjointEigenSolver< Eigen::Matrix<T, N, N> > EigenSolver;
+
+  EigenSolver eigensolver(covariance);
+
+  EXPECT_TRUE(eigensolver.info() == Eigen::Success)
+    << "Covariance =\n" << covariance.format(HeavyFmt);
+
+  EXPECT_TRUE(isSymmetric(covariance))
+    << "Covariance =\n" << covariance.format(HeavyFmt);
+  EXPECT_TRUE(isPositiveDefinite(covariance, no_tag()))
+    << "Covariance =\n" << covariance.format(HeavyFmt) << "\n"
+    << "Eigenvalues = " << eigensolver.eigenvalues().transpose().format(HeavyFmt);
+  EXPECT_LT(conditionNumber(covariance), max_condition_number)
+    << "Covariance =\n" << covariance.format(HeavyFmt) << "\n"
+    << "Condition number = " << conditionNumber(covariance) << "\n"
+    << "Eigenvalues = " << eigensolver.eigenvalues().transpose().format(HeavyFmt);
+}