3D reprojection analysis (#107)

* Added functions for projecting 2D image features onto 3D plane

* Added functions for computing 3D reprojection error statistics for hand-eye calibration tools

* Updated CI

.github/workflows/clang_format.yml

@@ -5,8 +5,6 @@ on:
     branches:
       - master
   pull_request:
-  schedule:
-    - cron: '0 5 * * *'
 
 jobs:
   clang_format:

.github/workflows/cmake_format.yml

@@ -5,8 +5,6 @@ on:
     branches:
       - master
   pull_request:
-  schedule:
-    - cron: '0 5 * * *'
 
 jobs:
   cmake_lang:

.github/workflows/main.yml

@@ -1,6 +1,12 @@
 name: CI
 
-on: [push, pull_request]
+on:
+  push:
+    branches:
+      - master
+  pull_request:
+  schedule:
+    - cron: '0 5 * * *'
 
 jobs:
   ci:

rct_examples/src/tools/camera_on_wrist_extrinsic.cpp

@@ -22,7 +22,8 @@ using namespace rct_image_tools;
 using namespace rct_ros_tools;
 using namespace rct_common;
 
-const std::string WINDOW = "window";
+static const std::string WINDOW = "window";
+static const unsigned RANDOM_SEED = 1;
 
 template <typename T>
 T get(const ros::NodeHandle& nh, const std::string& key)
@@ -104,8 +105,8 @@ int main(int argc, char** argv)
 
         //// 3. Check that a homography matrix can accurately reproject the observed points onto the expected target
         /// points within a defined threshold
-        rct_optimizations::RandomCorrespondenceSampler random_sampler(obs.correspondence_set.size(),
-                                                                      obs.correspondence_set.size() / 3);
+        rct_optimizations::RandomCorrespondenceSampler random_sampler(
+            obs.correspondence_set.size(), obs.correspondence_set.size() / 3, RANDOM_SEED);
         Eigen::VectorXd homography_error =
             rct_optimizations::calculateHomographyError(obs.correspondence_set, random_sampler);
         if (homography_error.array().mean() > homography_threshold)
@@ -136,6 +137,11 @@ int main(int argc, char** argv)
     printOptResults(opt_result.converged, opt_result.initial_cost_per_obs, opt_result.final_cost_per_obs);
     printNewLine();
 
+    // Let's analyze the reprojection errors in 3D by projecting the 2D image features onto the calibrated target plane
+    // and measuring their difference from the known 3D target features
+    analyze3DProjectionError(problem, opt_result);
+    printNewLine();
+
     Eigen::Isometry3d c = opt_result.camera_mount_to_camera;
     printTransform(c, "Wrist", "Camera", "WRIST TO CAMERA");
     printNewLine();

rct_examples/src/tools/hand_eye_calibration_analysis.h

@@ -2,12 +2,13 @@
 
 #include <rct_optimizations/pnp.h>
 #include <rct_optimizations/extrinsic_hand_eye.h>
+#include <rct_optimizations/validation/projection.h>
 #include <rct_image_tools/image_utils.h>
 
 #include <boost/accumulators/accumulators.hpp>
 #include <boost/accumulators/statistics.hpp>
+#include <iostream>
 #include <opencv2/highgui.hpp>
-#include <ros/console.h>
 
 using namespace rct_optimizations;
 using namespace rct_image_tools;
@@ -95,9 +96,22 @@ void analyzeResults(const ExtrinsicHandEyeProblem2D3D& problem,
                      .angularDistance(Eigen::Quaterniond(camera_to_target_pnp.linear())));
   }
 
-  ROS_INFO_STREAM("Difference in camera to target transform between extrinsic calibration and PnP optimization");
-  ROS_INFO_STREAM("Position:\n\tMean (m): " << ba::mean(pos_diff_acc)
-                                            << "\n\tStd. Dev. (m): " << std::sqrt(ba::variance(pos_diff_acc)));
-  ROS_INFO_STREAM("Orientation:\n\tMean (deg): " << ba::mean(ori_diff_acc) * 180.0 / M_PI << "\n\tStd. Dev. (deg): "
-                                                 << std::sqrt(ba::variance(ori_diff_acc)) * 180.0 / M_PI);
+  std::cout << "Difference in camera to target transform between extrinsic calibration and PnP optimization"
+            << "\nPosition:"
+            << "\n\tMean (m): " << ba::mean(pos_diff_acc)
+            << "\n\tStd. Dev. (m): " << std::sqrt(ba::variance(pos_diff_acc)) << "\nOrientation:"
+            << "\n\tMean (deg): " << ba::mean(ori_diff_acc) * 180.0 / M_PI
+            << "\n\tStd. Dev. (deg): " << std::sqrt(ba::variance(ori_diff_acc)) * 180.0 / M_PI << std::endl;
+}
+
+void analyze3DProjectionError(const ExtrinsicHandEyeProblem2D3D& problem, const ExtrinsicHandEyeResult& opt_result)
+{
+  Eigen::ArrayXd error = compute3DProjectionError(
+      problem.observations, problem.intr, opt_result.camera_mount_to_camera, opt_result.target_mount_to_target);
+
+  // Compute stats
+  double std_dev = std::sqrt((error - error.mean()).square().sum() / (error.size() - 1));
+  std::cout << "3D reprojection error statistics:"
+            << "\n\tMean +/- Std. Dev. (m): " << error.mean() << " +/- " << std_dev
+            << "\n\tMin (m): " << error.minCoeff() << "\n\tMax (m): " << error.maxCoeff() << std::endl;
 }

rct_examples/src/tools/static_camera_extrinsic.cpp

@@ -20,7 +20,8 @@ using namespace rct_image_tools;
 using namespace rct_ros_tools;
 using namespace rct_common;
 
-std::string WINDOW = "window";
+static const std::string WINDOW = "window";
+static const unsigned RANDOM_SEED = 1;
 
 template <typename T>
 T get(const ros::NodeHandle& nh, const std::string& key)
@@ -98,8 +99,8 @@ int main(int argc, char** argv)
 
         // Check that a homography matrix can accurately reproject the observed points onto the expected target points
         // within a defined threshold
-        rct_optimizations::RandomCorrespondenceSampler random_sampler(obs.correspondence_set.size(),
-                                                                      obs.correspondence_set.size() / 3);
+        rct_optimizations::RandomCorrespondenceSampler random_sampler(
+            obs.correspondence_set.size(), obs.correspondence_set.size() / 3, RANDOM_SEED);
         Eigen::VectorXd homography_error =
             rct_optimizations::calculateHomographyError(obs.correspondence_set, random_sampler);
         if (homography_error.array().mean() > homography_threshold)
@@ -130,6 +131,11 @@ int main(int argc, char** argv)
     printOptResults(opt_result.converged, opt_result.initial_cost_per_obs, opt_result.final_cost_per_obs);
     printNewLine();
 
+    // Let's analyze the reprojection errors in 3D by projecting the 2D image features onto the calibrated target plane
+    // and measuring their difference from the known 3D target features
+    analyze3DProjectionError(problem, opt_result);
+    printNewLine();
+
     Eigen::Isometry3d c = opt_result.camera_mount_to_camera;
     printTransform(c, "Base", "Camera", "BASE TO CAMERA");
     printNewLine();

rct_optimizations/CMakeLists.txt

@@ -35,6 +35,8 @@ add_library(
   src/${PROJECT_NAME}/validation/noise_qualification.cpp
   # Target Homography
   src/${PROJECT_NAME}/validation/homography_validation.cpp
+  # Projection
+  src/${PROJECT_NAME}/validation/projection.cpp
   # DH Chain Kinematic Calibration
   src/${PROJECT_NAME}/dh_chain.cpp
   src/${PROJECT_NAME}/dh_chain_kinematic_calibration.cpp)

rct_optimizations/include/rct_optimizations/validation/projection.h

@@ -0,0 +1,55 @@
+#pragma once
+
+#include <rct_optimizations/types.h>
+
+#include <Eigen/Dense>
+
+namespace rct_optimizations
+{
+/**
+ * @brief Computes the vector from an input point on a line to the point where the line intersects with the input plane
+ * See <a href=https://en.wikipedia.org/wiki/Line%E2%80%93plane_intersection>this link</a> for more information
+ * @param plane_normal Unit normal vector defining the plane
+ * @param plane_pt Point on the plane
+ * @param line Unit vector defining the line
+ * @param line_pt Point on the line
+ * @param epsilon
+ */
+Eigen::Vector3d computeLinePlaneIntersection(const Eigen::Vector3d& plane_normal,
+                                             const Eigen::Vector3d& plane_pt,
+                                             const Eigen::Vector3d& line,
+                                             const Eigen::Vector3d& line_pt,
+                                             const double epsilon = 1.0e-6);
+
+/**
+ * @brief Projects a set of 2D source points (e.g., from an image observation) onto a 3D plane (e.g., a flat calibration
+ * target)
+ * @param source Set of 2D points
+ * @param source_to_plane Matrix that transforms the source points into the frame of the plane (e.g., camera to target
+ * transform)
+ * @param k 3x3 camera projection matrix
+ * @return
+ */
+Eigen::MatrixX3d project3D(const Eigen::MatrixX2d& source,
+                           const Eigen::Isometry3d& source_to_plane,
+                           const Eigen::Matrix3d& k);
+
+/**
+ * @brief Projects the 2D target features from an observation onto the 3D plane of the calibrated target and computes
+ * the difference between the projections and the known target features
+ */
+Eigen::ArrayXd compute3DProjectionError(const Observation2D3D& obs,
+                                        const CameraIntrinsics& intr,
+                                        const Eigen::Isometry3d& camera_mount_to_camera,
+                                        const Eigen::Isometry3d& target_mount_to_target);
+
+/**
+ * @brief Projects the 2D target features from a set of observations onto the 3D plane of the calibrated target and
+ * computes the difference between the projections and the known target features
+ */
+Eigen::ArrayXd compute3DProjectionError(const Observation2D3D::Set& obs,
+                                        const CameraIntrinsics& intr,
+                                        const Eigen::Isometry3d& camera_mount_to_camera,
+                                        const Eigen::Isometry3d& target_mount_to_target);
+
+}  // namespace rct_optimizations

rct_optimizations/src/rct_optimizations/validation/projection.cpp

@@ -0,0 +1,111 @@
+#include <rct_optimizations/validation/projection.h>
+
+namespace rct_optimizations
+{
+Eigen::Vector3d computeLinePlaneIntersection(const Eigen::Vector3d& plane_normal,
+                                             const Eigen::Vector3d& plane_pt,
+                                             const Eigen::Vector3d& line,
+                                             const Eigen::Vector3d& line_pt,
+                                             const double epsilon)
+{
+  double dp = line.dot(plane_normal);
+  if (std::abs(dp) < epsilon)
+    throw std::runtime_error("No intersection between line and plane because they are parallel");
+
+  // Solve for the distance from line_pt to the plane: d = ((plane_pt - line_pt) * plane_normal) / (line * plane_normal)
+  double d = (plane_pt - line_pt).dot(plane_normal) / dp;
+
+  // Compute a new point at distance d along line from line_pt
+  return line_pt + line * d;
+}
+
+Eigen::MatrixX3d project3D(const Eigen::MatrixX2d& source,
+                           const Eigen::Isometry3d& source_to_plane,
+                           const Eigen::Matrix3d& k)
+{
+  const Eigen::Index n = source.rows();
+
+  // Convert 2D source points to 3D homogeneous coordinates
+  Eigen::MatrixX3d source_3d(n, 3);
+  source_3d.block(0, 0, n, 2) = source;
+  source_3d.col(2) = Eigen::VectorXd::Ones(n);
+
+  // Using camera as reference frame
+  Eigen::Vector3d plane_normal = source_to_plane.matrix().col(2).head<3>();  // Plane z-axis in source frame
+  Eigen::Vector3d plane_pt = source_to_plane.translation();                  // Plane origin in source frame
+  Eigen::Vector3d camera_origin = Eigen::Vector3d::Zero();
+
+  // Create 3D unit vector rays for each 3D coorindate in the camera frame
+  Eigen::MatrixX3d rays_in_camera = (k.inverse() * source_3d.transpose()).transpose();
+  rays_in_camera.rowwise().normalize();
+
+  Eigen::MatrixX3d projected_source_3d(n, 3);
+  for (Eigen::Index i = 0; i < n; ++i)
+    projected_source_3d.row(i) =
+        computeLinePlaneIntersection(plane_normal, plane_pt, rays_in_camera.row(i), camera_origin);
+
+  // Transform the projected source points into the plane frame
+  //   First convert 3D projected source points into 4D homogeneous coordinates
+  Eigen::MatrixX4d projected_source_4d(n, 4);
+  projected_source_4d.block(0, 0, n, 3) = projected_source_3d;
+  projected_source_4d.col(3) = Eigen::VectorXd::Ones(n);
+  //   Apply the transform
+  Eigen::MatrixX4d projected_source_4d_in_plane =
+      (source_to_plane.inverse() * projected_source_4d.transpose()).transpose();
+
+  // Return only the 3D points in matrix form
+  return projected_source_4d_in_plane.block(0, 0, n, 3);
+}
+
+Eigen::ArrayXd compute3DProjectionError(const Observation2D3D& obs,
+                                        const CameraIntrinsics& intr,
+                                        const Eigen::Isometry3d& camera_mount_to_camera,
+                                        const Eigen::Isometry3d& target_mount_to_target)
+{
+  Eigen::Matrix3d camera_matrix;
+  camera_matrix << intr.fx(), 0.0, intr.cx(), 0.0, intr.fy(), intr.cy(), 0.0, 0.0, 1.0;
+
+  // Calculate the optimized transform from the camera to the target for the ith observation
+  Eigen::Isometry3d camera_to_target =
+      camera_mount_to_camera.inverse() * obs.to_camera_mount.inverse() * obs.to_target_mount * target_mount_to_target;
+
+  // Convert the image and target features to matrices
+  const Eigen::Index n = static_cast<Eigen::Index>(obs.correspondence_set.size());
+  Eigen::MatrixX2d image_features(n, 2);
+  Eigen::MatrixX3d target_features(n, 3);
+  for (Eigen::Index i = 0; i < n; ++i)
+  {
+    image_features.row(i) = obs.correspondence_set[i].in_image;
+    target_features.row(i) = obs.correspondence_set[i].in_target;
+  }
+
+  // Project the image features onto the 3D target plane
+  Eigen::MatrixX3d projected_image_features_in_target = project3D(image_features, camera_to_target, camera_matrix);
+
+  // Compute the error
+  Eigen::ArrayXd error = (target_features - projected_image_features_in_target).rowwise().norm().array();
+
+  return error;
+}
+
+Eigen::ArrayXd compute3DProjectionError(const Observation2D3D::Set& observations,
+                                        const CameraIntrinsics& intr,
+                                        const Eigen::Isometry3d& camera_mount_to_camera,
+                                        const Eigen::Isometry3d& target_mount_to_target)
+{
+  Eigen::ArrayXd error;
+
+  // Iterate over all of the images in which an observation of the target was made
+  for (const Observation2D3D& obs : observations)
+  {
+    Eigen::Index n = static_cast<Eigen::Index>(obs.correspondence_set.size());
+
+    // Append the projection error
+    error.conservativeResize(error.rows() + n);
+    error.tail(n) = compute3DProjectionError(obs, intr, camera_mount_to_camera, target_mount_to_target);
+  }
+
+  return error;
+}
+
+}  // namespace rct_optimizations