From 25e396efd82868a8ac0f1acac4371a62e2167600 Mon Sep 17 00:00:00 2001
From: John Stechschulte <john@picknik.ai>
Date: Wed, 31 Mar 2021 11:01:50 -0600
Subject: [PATCH 1/3] silently accept the rational-polynomial model, convert to
 plumb-bob

---
 .../moveit/handeye_calibration_target/handeye_target_base.h     | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h
index a613bda..b352630 100644
--- a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h
+++ b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h
@@ -225,7 +225,7 @@ class HandEyeTargetBase
       return false;
     }
 
-    if (msg->D.size() != CAMERA_DISTORTION_VECTOR_DIMENSION)
+    if (msg->D.size() != CAMERA_DISTORTION_VECTOR_DIMENSION && msg->D.size() != 8)
     {
       ROS_ERROR_NAMED(LOGNAME, "Invalid distortion parameters dimension, current is %ld, required is %zu.",
                       msg->D.size(), CAMERA_DISTORTION_VECTOR_DIMENSION);

From bcd7fa7195a9034c24f88663873f330def5774fe Mon Sep 17 00:00:00 2001
From: John Stechschulte <john@picknik.ai>
Date: Wed, 31 Mar 2021 22:03:27 -0600
Subject: [PATCH 2/3] should work

---
 .../handeye_target_base.h                     | 20 +++++++++++++++----
 1 file changed, 16 insertions(+), 4 deletions(-)

diff --git a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h
index b352630..90d63f6 100644
--- a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h
+++ b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h
@@ -113,7 +113,9 @@ class HandEyeTargetBase
   const std::size_t CAMERA_MATRIX_VECTOR_DIMENSION = 9;  // 3x3 camera intrinsic matrix
   const std::size_t CAMERA_MATRIX_WIDTH = 3;
   const std::size_t CAMERA_MATRIX_HEIGHT = 3;
-  const std::size_t CAMERA_DISTORTION_VECTOR_DIMENSION = 5;  // distortion parameters (k1, k2, t1, t2, k3)
+  const std::map<std::string, std::size_t> CAMERA_DISTORTION_MODELS_VECTOR_DIMENSIONS = { { "plumb_bob", 5 },
+                                                                                          { "rational_polynomial",
+                                                                                            8 } };
 
   virtual ~HandEyeTargetBase() = default;
   HandEyeTargetBase()
@@ -225,10 +227,19 @@ class HandEyeTargetBase
       return false;
     }
 
-    if (msg->D.size() != CAMERA_DISTORTION_VECTOR_DIMENSION && msg->D.size() != 8)
+    if (0 == CAMERA_DISTORTION_MODELS_VECTOR_DIMENSIONS.count(msg->distortion_model))
+    {
+      ROS_ERROR_NAMED(LOGNAME, "Invalid camera distortion model, '%s'.", msg->distortion_model.c_str());
+      return false;
+    }
+
+    const size_t camera_distortion_vector_dimension =
+        CAMERA_DISTORTION_MODELS_VECTOR_DIMENSIONS.at(msg->distortion_model);
+
+    if (msg->D.size() != camera_distortion_vector_dimension)
     {
       ROS_ERROR_NAMED(LOGNAME, "Invalid distortion parameters dimension, current is %ld, required is %zu.",
-                      msg->D.size(), CAMERA_DISTORTION_VECTOR_DIMENSION);
+                      msg->D.size(), camera_distortion_vector_dimension);
       return false;
     }
 
@@ -244,7 +255,8 @@ class HandEyeTargetBase
     }
 
     // Store camera distortion info
-    for (size_t i = 0; i < CAMERA_DISTORTION_VECTOR_DIMENSION; i++)
+    distortion_coeffs_ = cv::Mat::zeros(camera_distortion_vector_dimension, 1, CV_64F);
+    for (size_t i = 0; i < camera_distortion_vector_dimension; i++)
     {
       distortion_coeffs_.at<double>(i, 0) = msg->D[i];
     }

From 828eaaeb797f341a68a98ebc68ce1d21875f3039 Mon Sep 17 00:00:00 2001
From: John Stechschulte <john@picknik.ai>
Date: Mon, 19 Apr 2021 21:27:18 -0600
Subject: [PATCH 3/3] none too

---
 .../moveit/handeye_calibration_target/handeye_target_base.h    | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h
index 90d63f6..2d2fbbd 100644
--- a/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h
+++ b/moveit_calibration_plugins/handeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h
@@ -113,7 +113,8 @@ class HandEyeTargetBase
   const std::size_t CAMERA_MATRIX_VECTOR_DIMENSION = 9;  // 3x3 camera intrinsic matrix
   const std::size_t CAMERA_MATRIX_WIDTH = 3;
   const std::size_t CAMERA_MATRIX_HEIGHT = 3;
-  const std::map<std::string, std::size_t> CAMERA_DISTORTION_MODELS_VECTOR_DIMENSIONS = { { "plumb_bob", 5 },
+  const std::map<std::string, std::size_t> CAMERA_DISTORTION_MODELS_VECTOR_DIMENSIONS = { { "none", 0 },
+                                                                                          { "plumb_bob", 5 },
                                                                                           { "rational_polynomial",
                                                                                             8 } };