Merge pull request #80 from ros-planning/pr-support-rational-polynomi…

…al-distortion Support rational polynomial distortion
moveit · Apr 16, 2024 · d226508 · d226508
2 parents ff6918e + 828eaae
commit d226508
Showing 1 changed file with 17 additions and 4 deletions.
diff --git a/...andeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h b/...andeye_calibration_target/include/moveit/handeye_calibration_target/handeye_target_base.h
@@ -113,7 +113,10 @@ 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 = { { "none", 0 },
+                                                                                          { "plumb_bob", 5 },
+                                                                                          { "rational_polynomial",
+                                                                                            8 } };
 
   virtual ~HandEyeTargetBase() = default;
   HandEyeTargetBase()
@@ -225,10 +228,19 @@ class HandEyeTargetBase
       return false;
     }
 
-    if (msg->D.size() != CAMERA_DISTORTION_VECTOR_DIMENSION)
+    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 +256,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];
     }