feat: backported the rational model implementation and other changes …

…(was lost in an experimental branch)

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp

@@ -31,7 +31,7 @@ class CeresCameraIntrinsicsOptimizer
 {
 public:
   static constexpr int POSE_OPT_DIM = 7;
-  static constexpr int INTRINSICS_DIM = 9;
+  static constexpr int INTRINSICS_DIM = 12;
 
   static constexpr int ROTATION_W_INDEX = 0;
   static constexpr int ROTATION_X_INDEX = 1;
@@ -61,6 +61,13 @@ class CeresCameraIntrinsicsOptimizer
    */
   void setTangentialDistortion(bool use_tangential_distortion);
 
+  /*!
+   * Sets the number of rational distortion coefficients
+   * @param[in] rational_distortion_coefficients number of radial distortion coefficients
+   * optimized
+   */
+  void setRationalDistortionCoefficients(int rational_distortion_coefficients);
+
   /*!
    * Sets the verbose mode
    * @param[in] verbose whether or not to use tangential distortion
@@ -118,6 +125,7 @@ class CeresCameraIntrinsicsOptimizer
 protected:
   int radial_distortion_coefficients_;
   bool use_tangential_distortion_;
+  int rational_distortion_coefficients_;
   bool verbose_;
   cv::Mat_<double> camera_matrix_;
   cv::Mat_<double> distortion_coeffs_;

calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp

@@ -46,12 +46,13 @@ struct ReprojectionResidual
 
   ReprojectionResidual(
     const cv::Point3f & object_point, const cv::Point2f & image_point, int radial_distortion_coeffs,
-    bool use_tangential_distortion)
+    bool use_tangential_distortion, int rational_distortion_coeffs)
   {
     object_point_ = Eigen::Vector3d(object_point.x, object_point.y, object_point.z);
     image_point_ = Eigen::Vector2d(image_point.x, image_point.y);
     radial_distortion_coeffs_ = radial_distortion_coeffs;
     use_tangential_distortion_ = use_tangential_distortion;
+    rational_distortion_coeffs_ = rational_distortion_coeffs;
   }
 
   /*!
@@ -76,22 +77,32 @@ struct ReprojectionResidual
     Vector3<T> object_point_ccs = board_quaternion * (T(1.0) * object_point_) + board_translation;
 
     const T null_value = T(0.0);
+    int distortion_index = 4;
     const T & cx = camera_intrinsics[INTRINSICS_CX_INDEX];
     const T & cy = camera_intrinsics[INTRINSICS_CY_INDEX];
     const T & fx = camera_intrinsics[INTRINSICS_FX_INDEX];
     const T & fy = camera_intrinsics[INTRINSICS_FY_INDEX];
-    const T & k1 = radial_distortion_coeffs_ > 0 ? camera_intrinsics[4] : null_value;
-    const T & k2 = radial_distortion_coeffs_ > 1 ? camera_intrinsics[5] : null_value;
-    const T & k3 = radial_distortion_coeffs_ > 2 ? camera_intrinsics[6] : null_value;
-    const T & p1 =
-      use_tangential_distortion_ ? camera_intrinsics[4 + radial_distortion_coeffs_] : null_value;
-    const T & p2 =
-      use_tangential_distortion_ ? camera_intrinsics[5 + radial_distortion_coeffs_] : null_value;
+    const T & k1 =
+      radial_distortion_coeffs_ > 0 ? camera_intrinsics[distortion_index++] : null_value;
+    const T & k2 =
+      radial_distortion_coeffs_ > 1 ? camera_intrinsics[distortion_index++] : null_value;
+    const T & k3 =
+      radial_distortion_coeffs_ > 2 ? camera_intrinsics[distortion_index++] : null_value;
+    const T & p1 = use_tangential_distortion_ ? camera_intrinsics[distortion_index++] : null_value;
+    const T & p2 = use_tangential_distortion_ ? camera_intrinsics[distortion_index++] : null_value;
+    const T & k4 =
+      rational_distortion_coeffs_ > 0 ? camera_intrinsics[distortion_index++] : null_value;
+    const T & k5 =
+      rational_distortion_coeffs_ > 1 ? camera_intrinsics[distortion_index++] : null_value;
+    const T & k6 =
+      rational_distortion_coeffs_ > 2 ? camera_intrinsics[distortion_index++] : null_value;
 
     const T xp = object_point_ccs.x() / object_point_ccs.z();
     const T yp = object_point_ccs.y() / object_point_ccs.z();
     const T r2 = xp * xp + yp * yp;
-    const T d = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2;
+    const T dn = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2;
+    const T dd = 1.0 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2;
+    const T d = dn / dd;
     const T xy = xp * yp;
     const T tdx = 2.0 * p1 * xy + p2 * (r2 + 2.0 * xp * xp);
     const T tdy = 2.0 * p2 * xy + p1 * (r2 + 2.0 * yp * yp);
@@ -115,13 +126,15 @@ struct ReprojectionResidual
    */
   static ceres::CostFunction * createResidual(
     const cv::Point3f & object_point, const cv::Point2f & image_point, int radial_distortion_coeffs,
-    bool use_tangential_distortion)
+    bool use_tangential_distortion, int rational_distortion_coeffs)
   {
     auto f = new ReprojectionResidual(
-      object_point, image_point, radial_distortion_coeffs, use_tangential_distortion);
+      object_point, image_point, radial_distortion_coeffs, use_tangential_distortion,
+      rational_distortion_coeffs);
 
-    int distortion_coefficients =
-      radial_distortion_coeffs + 2 * static_cast<int>(use_tangential_distortion);
+    int distortion_coefficients = radial_distortion_coeffs +
+                                  2 * static_cast<int>(use_tangential_distortion) +
+                                  rational_distortion_coeffs;
     ceres::CostFunction * cost_function = nullptr;
 
     switch (distortion_coefficients) {
@@ -149,6 +162,20 @@ struct ReprojectionResidual
         cost_function =
           new ceres::AutoDiffCostFunction<ReprojectionResidual, RESIDUAL_DIM, 9, POSE_OPT_DIM>(f);
         break;
+      case 6:
+        cost_function =
+          new ceres::AutoDiffCostFunction<ReprojectionResidual, RESIDUAL_DIM, 10, POSE_OPT_DIM>(f);
+        break;
+      case 7:
+        cost_function =
+          new ceres::AutoDiffCostFunction<ReprojectionResidual, RESIDUAL_DIM, 11, POSE_OPT_DIM>(f);
+        break;
+      case 8:
+        cost_function =
+          new ceres::AutoDiffCostFunction<ReprojectionResidual, RESIDUAL_DIM, 12, POSE_OPT_DIM>(f);
+        break;
+      default:
+        throw std::runtime_error("Invalid number of distortion coefficients");
     }
 
     return cost_function;
@@ -158,6 +185,7 @@ struct ReprojectionResidual
   Eigen::Vector2d image_point_;
   int radial_distortion_coeffs_;
   bool use_tangential_distortion_;
+  int rational_distortion_coeffs_;
 };
 
 #endif  // CERES_INTRINSIC_CAMERA_CALIBRATOR__REPROJECTION_RESIDUAL_HPP_

calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp

@@ -45,6 +45,12 @@ void CeresCameraIntrinsicsOptimizer::setTangentialDistortion(bool use_tangential
   use_tangential_distortion_ = use_tangential_distortion;
 }
 
+void CeresCameraIntrinsicsOptimizer::setRationalDistortionCoefficients(
+  int rational_distortion_coefficients)
+{
+  rational_distortion_coefficients_ = rational_distortion_coefficients;
+}
+
 void CeresCameraIntrinsicsOptimizer::setVerbose(bool verbose) { verbose_ = verbose; }
 
 void CeresCameraIntrinsicsOptimizer::setData(
@@ -53,6 +59,40 @@ void CeresCameraIntrinsicsOptimizer::setData(
   const std::vector<std::vector<cv::Point2f>> & image_points, const std::vector<cv::Mat> & rvecs,
   const std::vector<cv::Mat> & tvecs)
 {
+  if (camera_matrix.cols != 3 || camera_matrix.rows != 3) {
+    throw std::runtime_error("The camera matrix should be a 3x3 matrix");
+  }
+
+  if (
+    rational_distortion_coefficients_ > 0 && distortion_coeffs.cols * distortion_coeffs.rows != 8) {
+    throw std::runtime_error(
+      "8 distortion coefficients are expected when the rational model is used");
+  }
+
+  if (
+    rational_distortion_coefficients_ == 0 &&
+    distortion_coeffs.cols * distortion_coeffs.rows != 5) {
+    throw std::runtime_error(
+      "5 distortion coefficients are expected when the polynomial model is used");
+  }
+
+  if (object_points.size() != image_points.size()) {
+    throw std::runtime_error(
+      "The number of object points should be equal to the number of image points");
+  }
+
+  if (rvecs.size() != tvecs.size()) {
+    throw std::runtime_error("The number of rvecs should be equal to the number of tvecs");
+  }
+
+  if (rvecs.size() != object_points.size()) {
+    throw std::runtime_error("The number of rvecs should be equal to the number of object points");
+  }
+
+  if (object_points.size() == 0) {
+    throw std::runtime_error("There should be at least one calibration sample");
+  }
+
   camera_matrix_ = camera_matrix.clone();
   distortion_coeffs_ = distortion_coeffs.clone();
   object_points_ = object_points;
@@ -97,17 +137,24 @@ double CeresCameraIntrinsicsOptimizer::getSolution(
 void CeresCameraIntrinsicsOptimizer::dataToPlaceholders()
 {
   // Convert the intrinsics
+  int num_dist_coefficients = distortion_coeffs_.size().area();
   intrinsics_placeholder_[INTRINSICS_CX_INDEX] = camera_matrix_(0, 2);
   intrinsics_placeholder_[INTRINSICS_CY_INDEX] = camera_matrix_(1, 2);
   intrinsics_placeholder_[INTRINSICS_FX_INDEX] = camera_matrix_(0, 0);
   intrinsics_placeholder_[INTRINSICS_FY_INDEX] = camera_matrix_(1, 1);
 
+  // plumb bob
   double k1 = distortion_coeffs_(0);
   double k2 = distortion_coeffs_(1);
   double p1 = distortion_coeffs_(2);
   double p2 = distortion_coeffs_(3);
   double k3 = distortion_coeffs_(4);
 
+  // rational
+  double k4 = num_dist_coefficients >= 6 ? distortion_coeffs_(5) : 0.0;
+  double k5 = num_dist_coefficients >= 7 ? distortion_coeffs_(6) : 0.0;
+  double k6 = num_dist_coefficients >= 8 ? distortion_coeffs_(7) : 0.0;
+
   int index = 4;
 
   if (radial_distortion_coefficients_ > 0) {
@@ -124,6 +171,16 @@ void CeresCameraIntrinsicsOptimizer::dataToPlaceholders()
     intrinsics_placeholder_[index++] = p2;
   }
 
+  if (rational_distortion_coefficients_ > 0) {
+    intrinsics_placeholder_[index++] = k4;
+  }
+  if (rational_distortion_coefficients_ > 1) {
+    intrinsics_placeholder_[index++] = k5;
+  }
+  if (rational_distortion_coefficients_ > 2) {
+    intrinsics_placeholder_[index++] = k6;
+  }
+
   // Convert the revcs, tvecs into the placeholders
   pose_placeholders_.resize(object_points_.size());
 
@@ -156,12 +213,14 @@ void CeresCameraIntrinsicsOptimizer::placeholdersToData()
   camera_matrix_(0, 0) = intrinsics_placeholder_[INTRINSICS_FX_INDEX];
   camera_matrix_(1, 1) = intrinsics_placeholder_[INTRINSICS_FY_INDEX];
 
-  distortion_coeffs_ = cv::Mat_<double>::zeros(5, 1);
   double & k1 = distortion_coeffs_(0);
   double & k2 = distortion_coeffs_(1);
   double & p1 = distortion_coeffs_(2);
   double & p2 = distortion_coeffs_(3);
   double & k3 = distortion_coeffs_(4);
+  double & k4 = distortion_coeffs_(5);
+  double & k5 = distortion_coeffs_(6);
+  double & k6 = distortion_coeffs_(7);
 
   int index = 4;
 
@@ -179,6 +238,16 @@ void CeresCameraIntrinsicsOptimizer::placeholdersToData()
     p2 = intrinsics_placeholder_[index++];
   }
 
+  if (rational_distortion_coefficients_ > 0) {
+    k4 = intrinsics_placeholder_[index++];
+  }
+  if (rational_distortion_coefficients_ > 1) {
+    k5 = intrinsics_placeholder_[index++];
+  }
+  if (rational_distortion_coefficients_ > 2) {
+    k6 = intrinsics_placeholder_[index++];
+  }
+
   // Convert the revcs, tvecs into the placeholders
   rvecs_.resize(pose_placeholders_.size());
   tvecs_.resize(pose_placeholders_.size());
@@ -253,7 +322,8 @@ void CeresCameraIntrinsicsOptimizer::evaluate()
     for (std::size_t point_index = 0; point_index < view_object_points.size(); point_index++) {
       auto f = ReprojectionResidual(
         view_object_points[point_index], view_image_points[point_index],
-        radial_distortion_coefficients_, use_tangential_distortion_);
+        radial_distortion_coefficients_, use_tangential_distortion_,
+        rational_distortion_coefficients_);
       std::array<double, 2> residuals;
       f(intrinsics_placeholder_.data(), pose_placeholder.data(), residuals.data());
       total_ceres_error += residuals[0] * residuals[0] + residuals[1] * residuals[1];
@@ -278,7 +348,8 @@ void CeresCameraIntrinsicsOptimizer::solve()
       problem.AddResidualBlock(
         ReprojectionResidual::createResidual(
           view_object_points[point_index], view_image_points[point_index],
-          radial_distortion_coefficients_, use_tangential_distortion_),
+          radial_distortion_coefficients_, use_tangential_distortion_,
+          rational_distortion_coefficients_),
         nullptr,  // L2
         intrinsics_placeholder_.data(), pose_placeholder.data());
     }
@@ -302,6 +373,7 @@ void CeresCameraIntrinsicsOptimizer::solve()
   options.function_tolerance = 1e-10;
   options.gradient_tolerance = 1e-14;
   options.num_threads = 8;
+  options.max_num_consecutive_invalid_steps = 1000;
   options.use_inner_iterations = true;
   ceres::Solver::Summary summary;
   ceres::Solve(options, &problem, &summary);

calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp

@@ -45,19 +45,22 @@ calibrate(
   const std::vector<Eigen::MatrixXd> & image_points_eigen_list,
   const Eigen::MatrixXd & initial_camera_matrix_eigen,
   const Eigen::MatrixXd & initial_dist_coeffs_eigen, int num_radial_coeffs,
-  bool use_tangential_distortion, bool verbose)
+  bool use_tangential_distortion, int num_rational_coeffs, bool verbose)
 {
-  (void)num_radial_coeffs;
-  (void)use_tangential_distortion;
-  (void)verbose;
-
   if (
     initial_camera_matrix_eigen.cols() != 3 || initial_camera_matrix_eigen.rows() != 3 ||
     object_points_eigen_list.size() != image_points_eigen_list.size() || num_radial_coeffs < 0 ||
-    num_radial_coeffs > 3 ||
-    std::min<std::size_t>(initial_dist_coeffs_eigen.rows(), initial_dist_coeffs_eigen.cols() > 1) ||
+    num_radial_coeffs > 3 || num_rational_coeffs < 0 || num_rational_coeffs > 3 ||
+    std::min<std::size_t>(initial_dist_coeffs_eigen.rows(), initial_dist_coeffs_eigen.cols()) > 1 ||
     (initial_dist_coeffs_eigen.rows() * initial_dist_coeffs_eigen.cols()) != 5) {
     std::cout << "Invalid parameters" << std::endl;
+    std::cout << "\t object_points_list.size(): " << object_points_eigen_list.size() << std::endl;
+    std::cout << "\t image_points_list.size(): " << image_points_eigen_list.size() << std::endl;
+    std::cout << "\t initial_camera_matrix:\n" << initial_camera_matrix_eigen << std::endl;
+    std::cout << "\t initial_dist_coeffs:\n" << initial_dist_coeffs_eigen << std::endl;
+    std::cout << "\t num_radial_coeffs: " << num_radial_coeffs << std::endl;
+    std::cout << "\t num_rational_coeffs: " << num_rational_coeffs << std::endl;
+    std::cout << "\t use_tangential_distortion: " << use_tangential_distortion << std::endl;
     return std::tuple<
       double, Eigen::MatrixXd, Eigen::MatrixXd, std::vector<Eigen::Vector3d>,
       std::vector<Eigen::Vector3d>>();
@@ -115,6 +118,7 @@ calibrate(
   CeresCameraIntrinsicsOptimizer optimizer;
   optimizer.setRadialDistortionCoefficients(num_radial_coeffs);
   optimizer.setTangentialDistortion(use_tangential_distortion);
+  optimizer.setRationalDistortionCoefficients(num_rational_coeffs);
   optimizer.setVerbose(verbose);
   optimizer.setData(
     initial_camera_matrix_cv, initial_dist_coeffs_cv, object_points_list_cv, image_points_list_cv,
@@ -179,7 +183,7 @@ PYBIND11_MODULE(ceres_intrinsic_camera_calibrator_py, m)
             The RMS reprojection error, the optimized camera intrinsics, and the board extrinsics
       )pbdoc",
     py::arg("object_points_list"), py::arg("image_points_list"), py::arg("initial_camera_matrix"),
-    py::arg("initial_dist_coeffs"), py::arg("num_radial_coeffs"),
+    py::arg("initial_dist_coeffs"), py::arg("num_radial_coeffs"), py::arg("num_rational_coeffs"),
     py::arg("use_tangential_distortion"), py::arg("verbose") = false);
 
 #ifdef VERSION_INFO