Added functions for projecting 2D image features onto 3D plane and co…

…mputing 3D reprojection error statistics

rct_examples/src/tools/camera_on_wrist_extrinsic.cpp

@@ -134,6 +134,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
+    analyzeReprojectionError3D(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

@@ -6,8 +6,8 @@
 
 #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;
@@ -89,9 +89,127 @@ void analyzeResults(const ExtrinsicHandEyeProblem2D3D& problem, const ExtrinsicH
                      .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;
+}
+
+/**
+ * @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 linePlaneIntersection(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)
+{
+  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;
+}
+
+/**
+ * @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)
+{
+  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) = linePlaneIntersection(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);
+}
+
+/**
+ * @brief Analyzes the results of the hand eye calibration by projecting the observed 2D target features onto the 3D plane of the calibrated target
+ * and measuring the difference between the projections and the known target features
+ * @param problem
+ * @param opt_result
+ */
+void analyzeReprojectionError3D(const ExtrinsicHandEyeProblem2D3D& problem,
+                                const ExtrinsicHandEyeResult& opt_result)
+{
+  Eigen::Matrix3d camera_matrix;
+  camera_matrix << problem.intr.fx(), 0.0, problem.intr.cx(), 0.0, problem.intr.fy(), problem.intr.cy(), 0.0, 0.0, 1.0;
+
+  Eigen::ArrayXd error;
+
+  // Iterate over all of the images in which an observation of the target was made
+  for (const Observation2D3D& obs : problem.observations)
+  {
+    // Calculate the optimized transform from the camera to the target for the ith observation
+    Eigen::Isometry3d camera_to_target = opt_result.camera_mount_to_camera.inverse() * obs.to_camera_mount.inverse() *
+                                         obs.to_target_mount * opt_result.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);
+
+    // Append the projection error
+    error.conservativeResize(error.rows() + n);
+    error.tail(projected_image_features_in_target.rows()) = (target_features - projected_image_features_in_target).rowwise().norm().array();
+  }
+
+  // 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

@@ -128,6 +128,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
+    analyzeReprojectionError3D(problem, opt_result);
+    printNewLine();
+
     Eigen::Isometry3d c = opt_result.camera_mount_to_camera;
     printTransform(c, "Base", "Camera", "BASE TO CAMERA");
     printNewLine();