Revised PnP unit test with test fixture

Jmeyer1292 · Jun 29, 2020 · 85153ac · 85153ac
1 parent 0fa3016
commit 85153ac
Showing 1 changed file with 38 additions and 57 deletions.
diff --git a/rct_optimizations/test/pnp_utest.cpp b/rct_optimizations/test/pnp_utest.cpp
@@ -10,17 +10,40 @@ static const unsigned TARGET_COLS = 14;
 static const double SPACING = 0.025;
 static const double CORRELATION_COEFFICIENT_THRESHOLD = 0.8;
 
-TEST(PNP_2D, PerfectInitialConditions)
+class PnP2DTest : public ::testing::Test
 {
-  test::Camera camera = test::makeKinectCamera();
-  test::Target target(TARGET_ROWS, TARGET_COLS, SPACING);
+  public:
+  PnP2DTest()
+    : camera(test::makeKinectCamera())
+    , target(TARGET_ROWS, TARGET_COLS, SPACING)
+    , target_to_camera(Eigen::Isometry3d::Identity())
+  {
+    double x = static_cast<double>(TARGET_ROWS - 1) * SPACING / 2.0;
+    double y = static_cast<double>(TARGET_COLS - 1) * SPACING / 2.0;
+    target_to_camera.translate(Eigen::Vector3d(x, y, 0.4));
+    target_to_camera.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
+  }
 
-  Eigen::Isometry3d target_to_camera(Eigen::Isometry3d::Identity());
-  double x = static_cast<double>(TARGET_ROWS - 1) * SPACING / 2.0;
-  double y = static_cast<double>(TARGET_COLS - 1) * SPACING / 2.0;
-  target_to_camera.translate(Eigen::Vector3d(x, y, 1.0));
-  target_to_camera.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
+  void checkCovariance(const Eigen::MatrixXd& cov)
+  {
+    for (Eigen::Index row = 0; row < cov.rows(); ++row)
+    {
+      for (Eigen::Index col = 0; col < cov.cols(); ++col)
+      {
+        // Since the covariance matrix is symmetric, just check the values in the top triangle
+        if(row <= col)
+          EXPECT_LT(std::abs(cov(row, col)), CORRELATION_COEFFICIENT_THRESHOLD);
+      }
+    }
+  }
+
+  test::Camera camera;
+  test::Target target;
+  Eigen::Isometry3d target_to_camera;
+};
 
+TEST_F(PnP2DTest, PerfectInitialConditions)
+{
   PnPProblem problem;
   problem.camera_to_target_guess = target_to_camera.inverse();
   problem.intr = camera.intr;
@@ -32,31 +55,14 @@ TEST(PNP_2D, PerfectInitialConditions)
   EXPECT_TRUE(result.camera_to_target.isApprox(target_to_camera.inverse()));
   EXPECT_LT(result.initial_cost_per_obs, 1.0e-15);
   EXPECT_LT(result.final_cost_per_obs, 1.0e-15);
+  checkCovariance(result.camera_to_target_covariance);
 
-  for (Eigen::Index row = 0; row < result.camera_to_target_covariance.rows(); ++row)
-  {
-    for (Eigen::Index col = 0; col < result.camera_to_target_covariance.cols(); ++col)
-    {
-      if(row != col)
-        EXPECT_LT(std::abs(result.camera_to_target_covariance(row, col)), CORRELATION_COEFFICIENT_THRESHOLD);
-    }
-  }
-
-//  Eigen::IOFormat fmt(4, 0, " | ", "\n", "|", "|");
-//  std::cout << "Covariance:\n" << result.camera_to_target_covariance.format(fmt) << std::endl;
+  Eigen::IOFormat fmt(4, 0, " | ", "\n", "|", "|");
+  std::cout << "Covariance:\n" << result.camera_to_target_covariance.format(fmt) << std::endl;
 }
 
-TEST(PNP_2D, PerturbedInitialCondition)
+TEST_F(PnP2DTest, PerturbedInitialCondition)
 {
-  test::Camera camera = test::makeKinectCamera();
-  test::Target target(TARGET_ROWS, TARGET_COLS, SPACING);
-
-  Eigen::Isometry3d target_to_camera(Eigen::Isometry3d::Identity());
-  double x = static_cast<double>(TARGET_ROWS) * SPACING / 2.0;
-  double y = static_cast<double>(TARGET_COLS) * SPACING / 2.0;
-  target_to_camera.translate(Eigen::Vector3d(x, y, 1.0));
-  target_to_camera.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
-
   PnPProblem problem;
   problem.camera_to_target_guess = test::perturbPose(target_to_camera.inverse(), 0.05, 0.05);
   problem.intr = camera.intr;
@@ -70,31 +76,14 @@ TEST(PNP_2D, PerturbedInitialCondition)
   EXPECT_TRUE(result.converged);
   EXPECT_TRUE(result.camera_to_target.isApprox(target_to_camera.inverse(), 1.0e-8));
   EXPECT_LT(result.final_cost_per_obs, 1.0e-12);
-
-  for (Eigen::Index row = 0; row < result.camera_to_target_covariance.rows(); ++row)
-  {
-    for (Eigen::Index col = 0; col < result.camera_to_target_covariance.cols(); ++col)
-    {
-      if (row != col)
-        EXPECT_LT(std::abs(result.camera_to_target_covariance(row, col)), CORRELATION_COEFFICIENT_THRESHOLD);
-    }
-  }
+  checkCovariance(result.camera_to_target_covariance);
 
 //  Eigen::IOFormat fmt(4, 0, " | ", "\n", "|", "|");
 //  std::cout << "Covariance:\n" << result.camera_to_target_covariance.format(fmt) << std::endl;
 }
 
-TEST(PNP_2D, BadIntrinsicParameters)
+TEST_F(PnP2DTest, BadIntrinsicParameters)
 {
-  test::Camera camera = test::makeKinectCamera();
-  test::Target target(TARGET_ROWS, TARGET_COLS, SPACING);
-
-  Eigen::Isometry3d target_to_camera(Eigen::Isometry3d::Identity());
-  double x = static_cast<double>(TARGET_ROWS) * SPACING / 2.0;
-  double y = static_cast<double>(TARGET_COLS) * SPACING / 2.0;
-  target_to_camera.translate(Eigen::Vector3d(x, y, 1.0));
-  target_to_camera.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX()));
-
   PnPProblem problem;
   problem.camera_to_target_guess = target_to_camera.inverse();
   problem.correspondences = test::getCorrespondences(target_to_camera,
@@ -118,15 +107,7 @@ TEST(PNP_2D, BadIntrinsicParameters)
   EXPECT_TRUE(result.converged);
   EXPECT_FALSE(result.camera_to_target.isApprox(target_to_camera.inverse(), 1.0e-3));
   EXPECT_GT(result.final_cost_per_obs, 1.0e-3);
-
-  for (Eigen::Index row = 0; row < result.camera_to_target_covariance.rows(); ++row)
-  {
-    for (Eigen::Index col = 0; col < result.camera_to_target_covariance.cols(); ++col)
-    {
-      if (row != col)
-        EXPECT_LT(std::abs(result.camera_to_target_covariance(row, col)), CORRELATION_COEFFICIENT_THRESHOLD);
-    }
-  }
+  checkCovariance(result.camera_to_target_covariance);
 
 //  Eigen::IOFormat fmt(4, 0, " | ", "\n", "|", "|");
 //  std::cout << "Covariance:\n" << result.camera_to_target_covariance.format(fmt) << std::endl;