From cae74c00305e6824758662828b334aec3ce44c2f Mon Sep 17 00:00:00 2001 From: Michael Ripperger Date: Wed, 14 Feb 2024 09:48:19 -0500 Subject: [PATCH] 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 | 2 - .github/workflows/cmake_format.yml | 2 - .github/workflows/main.yml | 8 +- .../src/tools/camera_on_wrist_extrinsic.cpp | 12 +- .../src/tools/hand_eye_calibration_analysis.h | 26 +++- .../src/tools/static_camera_extrinsic.cpp | 12 +- rct_optimizations/CMakeLists.txt | 2 + .../rct_optimizations/validation/projection.h | 55 +++++++++ .../validation/projection.cpp | 111 ++++++++++++++++++ 9 files changed, 213 insertions(+), 17 deletions(-) create mode 100644 rct_optimizations/include/rct_optimizations/validation/projection.h create mode 100644 rct_optimizations/src/rct_optimizations/validation/projection.cpp diff --git a/.github/workflows/clang_format.yml b/.github/workflows/clang_format.yml index 99cc8f0d..88cc9cf9 100644 --- a/.github/workflows/clang_format.yml +++ b/.github/workflows/clang_format.yml @@ -5,8 +5,6 @@ on: branches: - master pull_request: - schedule: - - cron: '0 5 * * *' jobs: clang_format: diff --git a/.github/workflows/cmake_format.yml b/.github/workflows/cmake_format.yml index 97baa60a..b02d4680 100644 --- a/.github/workflows/cmake_format.yml +++ b/.github/workflows/cmake_format.yml @@ -5,8 +5,6 @@ on: branches: - master pull_request: - schedule: - - cron: '0 5 * * *' jobs: cmake_lang: diff --git a/.github/workflows/main.yml b/.github/workflows/main.yml index 940c12b6..6faa7d32 100644 --- a/.github/workflows/main.yml +++ b/.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: diff --git a/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp b/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp index 74678a18..96ad4004 100644 --- a/rct_examples/src/tools/camera_on_wrist_extrinsic.cpp +++ b/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 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(); diff --git a/rct_examples/src/tools/hand_eye_calibration_analysis.h b/rct_examples/src/tools/hand_eye_calibration_analysis.h index 77547d26..c3831eaf 100644 --- a/rct_examples/src/tools/hand_eye_calibration_analysis.h +++ b/rct_examples/src/tools/hand_eye_calibration_analysis.h @@ -2,12 +2,13 @@ #include #include +#include #include #include #include +#include #include -#include 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; } diff --git a/rct_examples/src/tools/static_camera_extrinsic.cpp b/rct_examples/src/tools/static_camera_extrinsic.cpp index 62a05aa2..630653a3 100644 --- a/rct_examples/src/tools/static_camera_extrinsic.cpp +++ b/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 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(); diff --git a/rct_optimizations/CMakeLists.txt b/rct_optimizations/CMakeLists.txt index cfa6b9a7..e183033d 100644 --- a/rct_optimizations/CMakeLists.txt +++ b/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) diff --git a/rct_optimizations/include/rct_optimizations/validation/projection.h b/rct_optimizations/include/rct_optimizations/validation/projection.h new file mode 100644 index 00000000..a1d970ee --- /dev/null +++ b/rct_optimizations/include/rct_optimizations/validation/projection.h @@ -0,0 +1,55 @@ +#pragma once + +#include + +#include + +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 this link 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 diff --git a/rct_optimizations/src/rct_optimizations/validation/projection.cpp b/rct_optimizations/src/rct_optimizations/validation/projection.cpp new file mode 100644 index 00000000..4d39d506 --- /dev/null +++ b/rct_optimizations/src/rct_optimizations/validation/projection.cpp @@ -0,0 +1,111 @@ +#include + +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(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(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