From 85153ac7ddbbee85c95d574b771be24b413612f1 Mon Sep 17 00:00:00 2001 From: mripperger Date: Thu, 25 Jun 2020 16:43:07 -0500 Subject: [PATCH] Revised PnP unit test with test fixture --- rct_optimizations/test/pnp_utest.cpp | 95 +++++++++++----------------- 1 file changed, 38 insertions(+), 57 deletions(-) diff --git a/rct_optimizations/test/pnp_utest.cpp b/rct_optimizations/test/pnp_utest.cpp index 8276e795..92d378fb 100644 --- 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(TARGET_ROWS - 1) * SPACING / 2.0; + double y = static_cast(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(TARGET_ROWS - 1) * SPACING / 2.0; - double y = static_cast(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(TARGET_ROWS) * SPACING / 2.0; - double y = static_cast(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(TARGET_ROWS) * SPACING / 2.0; - double y = static_cast(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;