From 089b432b2a397acc13d0be2211477dd6194f76ef Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Fri, 10 Jul 2020 11:05:56 -0500 Subject: [PATCH] Homography Validation Update (#66) * 2d capability. Need to template for 3d. Optional outlier detection partially implemented * added outlier detection option * fixed python syntax * First round pr updates; still in image tools. Squash after move * added sampling assertion, moved to optimizations * linking problem in unit test * Updates to homography error calculation * Updates to homography error unit tests * Renamed files * Tests CMakeLists fixup * Reduced to minimum number of homography samples * Use matrix instead of vector when calculating k Co-authored-by: ctlewis --- rct_optimizations/CMakeLists.txt | 2 + .../validation/homography_validation.h | 71 +++++++++++ .../validation/homography_validation.cpp | 109 +++++++++++++++++ rct_optimizations/test/CMakeLists.txt | 9 ++ rct_optimizations/test/homography_utest.cpp | 113 ++++++++++++++++++ 5 files changed, 304 insertions(+) create mode 100644 rct_optimizations/include/rct_optimizations/validation/homography_validation.h create mode 100644 rct_optimizations/src/rct_optimizations/validation/homography_validation.cpp create mode 100644 rct_optimizations/test/homography_utest.cpp diff --git a/rct_optimizations/CMakeLists.txt b/rct_optimizations/CMakeLists.txt index 291d2756..c9e2ec8f 100644 --- a/rct_optimizations/CMakeLists.txt +++ b/rct_optimizations/CMakeLists.txt @@ -26,6 +26,8 @@ add_library(${PROJECT_NAME} SHARED src/${PROJECT_NAME}/validation/camera_intrinsic_calibration_validation.cpp #Noise Qualification src/${PROJECT_NAME}/validation/noise_qualification.cpp + #Target Homography + src/${PROJECT_NAME}/validation/homography_validation.cpp ) target_compile_options(${PROJECT_NAME} PUBLIC -std=c++14) diff --git a/rct_optimizations/include/rct_optimizations/validation/homography_validation.h b/rct_optimizations/include/rct_optimizations/validation/homography_validation.h new file mode 100644 index 00000000..e16ce203 --- /dev/null +++ b/rct_optimizations/include/rct_optimizations/validation/homography_validation.h @@ -0,0 +1,71 @@ +#pragma once +#include + +namespace rct_optimizations +{ + +/** + * @brief A simple structure for choosing which correspondence indices to use for generating the homography matrix + */ +struct CorrespondenceSampler +{ + virtual std::vector getSampleCorrespondenceIndices() const = 0; +}; + +/** + * @brief A correspondence sampler specifically for modified circle grid targets + */ +struct ModifiedCircleGridCorrespondenceSampler : CorrespondenceSampler +{ + ModifiedCircleGridCorrespondenceSampler(const std::size_t rows_, const std::size_t cols_) + : rows(rows_) + , cols(cols_) + { + } + + virtual std::vector getSampleCorrespondenceIndices() const final override + { + const std::size_t n_samples = 4; + + // Make sure there are at least two times as many points as the number of sample points + if ((rows * cols / 2) < n_samples) + { + std::stringstream ss; + ss << "Number of correspondences does not exceed minimum of " << n_samples * 2 << " (" << rows * cols << " provided)"; + throw std::runtime_error(ss.str()); + } + + std::vector correspondence_indices; + correspondence_indices.reserve(n_samples); + + // For a modified circle target grid, the sample points should be the corners of the grid + correspondence_indices.push_back(0); // upper left point + correspondence_indices.push_back(rows - 1); // upper right point + correspondence_indices.push_back(rows * cols - cols - 1); // lower left point + correspondence_indices.push_back(rows * cols - 1); // lower right point + + return correspondence_indices; + } + + std::size_t rows; + std::size_t cols; +}; + +/** + * @brief Computes the error between a set of corresponding planar data points (points on a planar target and their corresponding locations in the image plane) + * This function creates a homography matrix which transforms data from one plane onto another plane (i.e. from the target plane to the image plane). + * This matrix is created using only a subset of the correspondences. The matrix is then used to estimate the location of all target points in the image plane. + * The error between these estimates and the actual values is a good measurement of how accurately a set of 2D camera measurements represent a calbration target with known geometry + * + * Assumptions: + * - Both sets of points lie on a plane (i.e. points on a planar calibration target and points on the image plane) + * + * @param correspondences - A set of corresponding points + * @param correspondence_sampler - a struct for choosing correspondence indices to generate the homography matrix + * @return A vector of homography errors for each correspondence + * @throws Exception when number of correspondences does not exceed 2x the number of samples + */ +Eigen::VectorXd calculateHomographyError(const Correspondence2D3D::Set &correspondences, + const CorrespondenceSampler &correspondence_sampler); + +} //rct_optimizations diff --git a/rct_optimizations/src/rct_optimizations/validation/homography_validation.cpp b/rct_optimizations/src/rct_optimizations/validation/homography_validation.cpp new file mode 100644 index 00000000..b6a349b1 --- /dev/null +++ b/rct_optimizations/src/rct_optimizations/validation/homography_validation.cpp @@ -0,0 +1,109 @@ +#include +#include +#include + +namespace rct_optimizations +{ +Eigen::VectorXd calculateHomographyError(const Correspondence2D3D::Set &correspondences, + const CorrespondenceSampler &correspondence_sampler) +{ + /* There is a 3x3 homography matrix, H, that can transform a point from one plane onto a different plane + * | u | = k * | H00 H01 H02 | * | x | + * | v | | H10 H11 H12 | | y | + * | 1 | | H20 H12 1 | | 1 | + * + * In our case we have 2 sets of known corresponding planar points: points on the planar target, and points in the image plane + * Therefore, there is some matrix, H, which can transform target points into the image plane. + * If the target points and camera points actually match, we should be able to: + * 1. Calculate H for a subset of corresponding points + * 2. Transform the remaining target points by H to obtain estimates of their locations in the image plane + * 3. Compare the calculated estimations to the actual image points to make sure they are very close. If they are not close, we know that the correspondences are not valid + * + * The matrix H has 8 unique values. + * These 8 values of the homography matrix can be solved for, given a set of (at least) 8 corresponding planar vectors, by rearranging the above equations: + * + * A * H = b, where + * H = inv(A) * b + * - A is matrix (size 2*n x 8), where n is the number of corresponding vectors + * - H is a vector (size 8 x 1) of the unknown elements of the homography matrix + * - B is a vector (size 2*n x 1) representing the elements of one set of planar vectors + * + * A * H = b + * |-x0 -y0 -1 0 0 0 u0*x0 u0*y0 | * | H00 | = | -u0 | + * | 0 0 0 -x0 -y0 -1 v0*x0 v0*y0 | * | H01 | = | -v0 | + * ... + * |-x7 -y7 -1 0 0 0 u7*x7 u7*y7 | * | H20 | = | -u7 | + * | 0 0 0 -x7 -y7 -1 v7*x7 v7*y7 | * | H21 | = | -v7 | + * + */ + + // Select the points that we want to use to create the H matrix + std::vector sample_correspondence_indices = correspondence_sampler + .getSampleCorrespondenceIndices(); + std::size_t n_samples = sample_correspondence_indices.size(); + + // Ensure that there are enough points for testing outside of the sampled set + if (correspondences.size() < 2 * n_samples) + { + std::stringstream ss; + ss << "Correspondences size is not more than 2x sample size (" << correspondences.size() + << " correspondences vs. " << n_samples << ")"; + throw std::runtime_error(ss.str()); + } + + // Create the A and b matrices + Eigen::MatrixXd A(2 * n_samples, 8); + Eigen::MatrixXd b(2 * n_samples, 1); + + // Fill the A and B matrices with data from the selected correspondences + for (std::size_t i = 0; i < n_samples; ++i) + { + std::size_t corr_idx = sample_correspondence_indices.at(i); + const Correspondence2D3D &corr = correspondences.at(corr_idx); + + //assign A row-th row: + const double x = corr.in_target.x(); + const double y = corr.in_target.y(); + const double u = corr.in_image.x(); + const double v = corr.in_image.y(); + A.row(2 * i) << -x, -y, -1.0, 0.0, 0.0, 0.0, u * x, u * y; + A.row(2 * i + 1) << 0.0, 0.0, 0.0, -x, -y, -1.0, v * x, v * y; + + b.block<2, 1>(2 * i, 0) = -1.0 * corr.in_image; + } + + // Create the homography matrix + Eigen::Matrix H = Eigen::Matrix3d::Ones(); + + // Map the elements of the H matrix into a column vector and solve for the first 8 + { + Eigen::Map hv(H.data(), 9); + hv.head<8>() = A.fullPivLu().solve(b); + } + + // Estimate the image locations of all the target observations and compare to the actual image locations + Eigen::VectorXd error(correspondences.size()); + for (std::size_t i = 0; i < correspondences.size(); ++i) + { + const Correspondence2D3D &corr = correspondences[i]; + + // Calculate the scaling factor + double ki = 1.0 / (H(2, 0) * corr.in_target.x() + H(2, 1) * corr.in_target.y() + 1.0); + + // Replace the z-element of the point with 1 + Eigen::Vector3d xy(corr.in_target); + xy(2) = 1.0; + + // Estimate the point in the image plane + Eigen::Vector3d in_image_estimate = ki * H * xy; + + // Calculate the error + Eigen::Vector2d image_error = corr.in_image - in_image_estimate.head<2>(); + error(i) = image_error.norm(); + } + + return error; +} + +} // namespace rct_optimizations + diff --git a/rct_optimizations/test/CMakeLists.txt b/rct_optimizations/test/CMakeLists.txt index e5d0b876..5ca45721 100644 --- a/rct_optimizations/test/CMakeLists.txt +++ b/rct_optimizations/test/CMakeLists.txt @@ -53,6 +53,13 @@ add_executable(${PROJECT_NAME}_covariance_tests covariance_utest.cpp) target_link_libraries(${PROJECT_NAME}_covariance_tests PRIVATE ${PROJECT_NAME} GTest::GTest GTest::Main) add_dependencies(${PROJECT_NAME}_covariance_tests ${PROJECT_NAME}) +# Homography +add_executable(${PROJECT_NAME}_homography_tests homography_utest.cpp) +target_link_libraries(${PROJECT_NAME}_homography_tests PRIVATE ${PROJECT_NAME}_test_support GTest::GTest GTest::Main) +rct_gtest_discover_tests(${PROJECT_NAME}_homography_tests) +add_dependencies(${PROJECT_NAME}_homography_tests ${PROJECT_NAME}) +add_dependencies(run_tests ${PROJECT_NAME}_homography_tests) + # PnP add_executable(${PROJECT_NAME}_pnp_tests pnp_utest.cpp) target_link_libraries(${PROJECT_NAME}_pnp_tests PRIVATE ${PROJECT_NAME}_test_support GTest::GTest GTest::Main) @@ -67,6 +74,7 @@ rct_gtest_discover_tests(${PROJECT_NAME}_camera_intrinsic_validation_tests) add_dependencies(${PROJECT_NAME}_camera_intrinsic_validation_tests ${PROJECT_NAME}) add_dependencies(run_tests ${PROJECT_NAME}_camera_intrinsic_validation_tests) +# Sensor noise add_executable(${PROJECT_NAME}_noise_tests noise_qualification_utest.cpp) target_link_libraries(${PROJECT_NAME}_noise_tests PRIVATE ${PROJECT_NAME}_test_support GTest::GTest GTest::Main) rct_gtest_discover_tests(${PROJECT_NAME}_noise_tests) @@ -82,6 +90,7 @@ install( ${PROJECT_NAME}_extrinsic_hand_eye_tests ${PROJECT_NAME}_dh_parameter_tests ${PROJECT_NAME}_extrinsic_hand_eye_dh_chain_tests + ${PROJECT_NAME}_homography_tests ${PROJECT_NAME}_pnp_tests ${PROJECT_NAME}_camera_intrinsic_validation_tests ${PROJECT_NAME}_noise_tests diff --git a/rct_optimizations/test/homography_utest.cpp b/rct_optimizations/test/homography_utest.cpp new file mode 100644 index 00000000..a2acaca3 --- /dev/null +++ b/rct_optimizations/test/homography_utest.cpp @@ -0,0 +1,113 @@ +#include +#include +#include +#include +#include + +using namespace rct_optimizations; + +const unsigned TARGET_ROWS = 5; +const unsigned TARGET_COLS = 7; +const double SPACING = 0.025; + +class HomographyTest : public ::testing::Test +{ + public: + HomographyTest() + : camera(test::makeKinectCamera()) + , target(TARGET_ROWS, TARGET_COLS, SPACING) + , target_to_camera(Eigen::Isometry3d::Identity()) + , sampler(TARGET_ROWS, TARGET_COLS) + { + 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.5)); + target_to_camera.rotate(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX())); + } + + test::Camera camera; + test::Target target; + Eigen::Isometry3d target_to_camera; + ModifiedCircleGridCorrespondenceSampler sampler; +}; + +TEST_F(HomographyTest, PerfectInitialConditions) +{ + rct_optimizations::Correspondence2D3D::Set correspondence_set; + EXPECT_NO_THROW(correspondence_set = test::getCorrespondences(target_to_camera, + Eigen::Isometry3d::Identity(), + camera, + target, + true)); + + Eigen::VectorXd error = calculateHomographyError(correspondence_set, sampler); + // Expect all of the errors to be extremely small + EXPECT_LT(error.maxCoeff(), 1.0e-12); +} + +TEST_F(HomographyTest, SwapCorrespondencesConditions) +{ + rct_optimizations::Correspondence2D3D::Set correspondence_set; + EXPECT_NO_THROW(correspondence_set = test::getCorrespondences(target_to_camera, + Eigen::Isometry3d::Identity(), + camera, + target, + true)); + + + + + // Swap the image measurements between 2 arbitrary correspondences + Correspondence2D3D &c1 = correspondence_set.at(10); + Eigen::Vector2d in_image_1 = c1.in_image; + Correspondence2D3D &c2 = correspondence_set.at(21); + Eigen::Vector2d in_image_2 = c2.in_image; + c1.in_image = in_image_2; + c2.in_image = in_image_1; + + Eigen::VectorXd error = calculateHomographyError(correspondence_set, sampler); + + // Expect the error of the points used to create the homography matrix to be very small + std::vector sample_indices = sampler.getSampleCorrespondenceIndices(); + for (std::size_t idx : sample_indices) + { + EXPECT_LT(error(idx), 1.0e-12); + } + + // Expect the homography to be off by more than one pixel because of the swap + EXPECT_GT(error.maxCoeff(), 1.0); +} + +TEST_F(HomographyTest, NoisyCorrespondences) +{ + rct_optimizations::Correspondence2D3D::Set correspondence_set; + EXPECT_NO_THROW(correspondence_set = test::getCorrespondences(target_to_camera, + Eigen::Isometry3d::Identity(), + camera, + target, + true)); + + // Add Gaussian noise to the image features + std::mt19937 mt_rand(std::random_device{}()); + const double stdev = 1.0; + std::normal_distribution dist(0.0, stdev); + for (Correspondence2D3D &corr : correspondence_set) + { + Eigen::Vector2d noise; + noise << dist(mt_rand), dist(mt_rand); + corr.in_image += noise; + } + + Eigen::VectorXd error = calculateHomographyError(correspondence_set, sampler); + + // TODO: create an informed expectation for the max or average error. Initial testing indicates that the average error can occasionally be > 4 * stddev + double stdev_mag = stdev * std::sqrt(2.0); + EXPECT_GT(error.maxCoeff(), stdev_mag); + EXPECT_GT(error.mean(), stdev_mag); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}