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 <colin.lewis@swri.org>

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)

rct_optimizations/include/rct_optimizations/validation/homography_validation.h

@@ -0,0 +1,71 @@
+#pragma once
+#include <rct_optimizations/types.h>
+
+namespace rct_optimizations
+{
+
+/**
+ * @brief A simple structure for choosing which correspondence indices to use for generating the homography matrix
+ */
+struct CorrespondenceSampler
+{
+  virtual std::vector<std::size_t> 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<std::size_t> 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<std::size_t> 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

rct_optimizations/src/rct_optimizations/validation/homography_validation.cpp

@@ -0,0 +1,109 @@
+#include <rct_optimizations/validation/homography_validation.h>
+#include <Eigen/Dense>
+#include <rct_optimizations/types.h>
+
+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<std::size_t> 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<double, 3, 3, Eigen::RowMajor> H = Eigen::Matrix3d::Ones();
+
+  // Map the elements of the H matrix into a column vector and solve for the first 8
+  {
+    Eigen::Map<Eigen::VectorXd> 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
+

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

rct_optimizations/test/homography_utest.cpp

@@ -0,0 +1,113 @@
+#include <gtest/gtest.h>
+#include <random>
+#include <rct_optimizations/validation/homography_validation.h>
+#include <rct_optimizations_tests/observation_creator.h>
+#include <rct_optimizations_tests/utilities.h>
+
+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<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.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<std::size_t> 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<double> 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();
+}