Merge pull request #208 from amadeuszsz/feature/ceres_intrinsic_calib…

…rator-integration

feat(intrinsic_camera_calibrator): multiple camera models & ceres integration

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

@@ -68,6 +68,12 @@ class CeresCameraIntrinsicsOptimizer
    */
   void setRationalDistortionCoefficients(int rational_distortion_coefficients);
 
+  /*!
+   * Sets the regularization weight for the distortion coefficients
+   * @param[in] regularization_weight the regularization weight
+   */
+  void setRegularizationWeight(double regularization_weight);
+
   /*!
    * Sets the verbose mode
    * @param[in] verbose whether or not to use tangential distortion
@@ -126,6 +132,7 @@ class CeresCameraIntrinsicsOptimizer
   int radial_distortion_coefficients_;
   bool use_tangential_distortion_;
   int rational_distortion_coefficients_;
+  double regularization_weight_;
   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/distortion_coefficients_residual.hpp

@@ -0,0 +1,138 @@
+// Copyright 2024 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CERES_INTRINSIC_CAMERA_CALIBRATOR__DISTORTION_COEFFICIENTS_RESIDUAL_HPP_
+#define CERES_INTRINSIC_CAMERA_CALIBRATOR__DISTORTION_COEFFICIENTS_RESIDUAL_HPP_
+
+#include <ceres/autodiff_cost_function.h>
+#include <ceres/ceres.h>
+
+struct DistortionCoefficientsResidual
+{
+  static constexpr int RESIDUAL_DIM = 8;
+
+  DistortionCoefficientsResidual(
+    int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs)
+  {
+    radial_distortion_coeffs_ = radial_distortion_coeffs;
+    use_tangential_distortion_ = use_tangential_distortion;
+    rational_distortion_coeffs_ = rational_distortion_coeffs;
+  }
+
+  /*!
+   * The cost function representing the reprojection error
+   * @param[in] camera_intrinsics The camera intrinsics
+   * @param[in] residuals The residual error of projecting the tag into the camera
+   * @returns success status
+   */
+  template <typename T>
+  bool operator()(const T * const camera_intrinsics, T * residuals) const
+  {
+    const T null_value = T(0.0);
+    int distortion_index = 4;
+    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;
+
+    residuals[0] = k1;
+    residuals[1] = k2;
+    residuals[2] = k3;
+    residuals[3] = p1;
+    residuals[4] = p2;
+    residuals[5] = k4;
+    residuals[6] = k5;
+    residuals[7] = k6;
+
+    return true;
+  }
+
+  /*!
+   * Residual factory method
+   * @param[in] radial_distortion_coeffs The number of radial distortion coefficients
+   * @param[in] use_tangential_distortion Whether to use or not tangential distortion
+   * @param[in] rational_distortion_coeffs The number of rational distortion coefficients
+   * @returns the ceres residual
+   */
+  static ceres::CostFunction * createResidual(
+    int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs)
+  {
+    auto f = new DistortionCoefficientsResidual(
+      radial_distortion_coeffs, use_tangential_distortion, rational_distortion_coeffs);
+
+    int distortion_coefficients = radial_distortion_coeffs +
+                                  2 * static_cast<int>(use_tangential_distortion) +
+                                  rational_distortion_coeffs;
+    ceres::CostFunction * cost_function = nullptr;
+
+    switch (distortion_coefficients) {
+      case 0:
+        cost_function =
+          new ceres::AutoDiffCostFunction<DistortionCoefficientsResidual, RESIDUAL_DIM, 4>(f);
+        break;
+      case 1:
+        cost_function =
+          new ceres::AutoDiffCostFunction<DistortionCoefficientsResidual, RESIDUAL_DIM, 5>(f);
+        break;
+      case 2:
+        cost_function =
+          new ceres::AutoDiffCostFunction<DistortionCoefficientsResidual, RESIDUAL_DIM, 6>(f);
+        break;
+      case 3:
+        cost_function =
+          new ceres::AutoDiffCostFunction<DistortionCoefficientsResidual, RESIDUAL_DIM, 7>(f);
+        break;
+      case 4:
+        cost_function =
+          new ceres::AutoDiffCostFunction<DistortionCoefficientsResidual, RESIDUAL_DIM, 8>(f);
+        break;
+      case 5:
+        cost_function =
+          new ceres::AutoDiffCostFunction<DistortionCoefficientsResidual, RESIDUAL_DIM, 9>(f);
+        break;
+      case 6:
+        cost_function =
+          new ceres::AutoDiffCostFunction<DistortionCoefficientsResidual, RESIDUAL_DIM, 10>(f);
+        break;
+      case 7:
+        cost_function =
+          new ceres::AutoDiffCostFunction<DistortionCoefficientsResidual, RESIDUAL_DIM, 11>(f);
+        break;
+      case 8:
+        cost_function =
+          new ceres::AutoDiffCostFunction<DistortionCoefficientsResidual, RESIDUAL_DIM, 12>(f);
+        break;
+      default:
+        throw std::runtime_error("Invalid number of distortion coefficients");
+    }
+
+    return cost_function;
+  }
+
+  int radial_distortion_coeffs_;
+  bool use_tangential_distortion_;
+  int rational_distortion_coeffs_;
+};
+
+#endif  // CERES_INTRINSIC_CAMERA_CALIBRATOR__DISTORTION_COEFFICIENTS_RESIDUAL_HPP_

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

@@ -122,6 +122,7 @@ struct ReprojectionResidual
    * @param[in] image_point The image point
    * @param[in] radial_distortion_coeffs The number of radial distortion coefficients
    * @param[in] use_tangential_distortion Whether to use or not tangential distortion
+   * @param[in] rational_distortion_coeffs The number of rational distortion coefficients
    * @returns the ceres residual
    */
   static ceres::CostFunction * createResidual(

calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp

@@ -14,6 +14,7 @@
 
 #include <Eigen/Core>
 #include <ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp>
+#include <ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp>
 #include <ceres_intrinsic_camera_calibrator/reprojection_residual.hpp>
 #include <opencv2/calib3d/calib3d.hpp>
 #include <opencv2/core.hpp>
@@ -51,6 +52,11 @@ void CeresCameraIntrinsicsOptimizer::setRationalDistortionCoefficients(
   rational_distortion_coefficients_ = rational_distortion_coefficients;
 }
 
+void CeresCameraIntrinsicsOptimizer::setRegularizationWeight(double regularization_weight)
+{
+  regularization_weight_ = regularization_weight;
+}
+
 void CeresCameraIntrinsicsOptimizer::setVerbose(bool verbose) { verbose_ = verbose; }
 
 void CeresCameraIntrinsicsOptimizer::setData(
@@ -355,6 +361,14 @@ void CeresCameraIntrinsicsOptimizer::solve()
     }
   }
 
+  problem.AddResidualBlock(
+    DistortionCoefficientsResidual::createResidual(
+      radial_distortion_coefficients_, use_tangential_distortion_,
+      rational_distortion_coefficients_),
+    new ceres::ScaledLoss(
+      nullptr, regularization_weight_ * object_points_.size(), ceres::TAKE_OWNERSHIP),  // L2
+    intrinsics_placeholder_.data());
+
   double initial_cost = 0.0;
   std::vector<double> residuals;
   ceres::Problem::EvaluateOptions eval_opt;

calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp

@@ -45,7 +45,7 @@ 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, int num_rational_coeffs,
-  bool use_tangential_distortion, bool verbose)
+  bool use_tangential_distortion, double regularization_weight, bool verbose)
 {
   Eigen::Index num_dist_coeffs =
     initial_dist_coeffs_eigen.rows() * initial_dist_coeffs_eigen.cols();
@@ -54,7 +54,7 @@ calibrate(
     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 || num_rational_coeffs < 0 || num_rational_coeffs > 3 ||
-    num_dist_coeffs != expected_dist_coeffs) {
+    num_dist_coeffs != expected_dist_coeffs || regularization_weight < 0.0) {
     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;
@@ -63,6 +63,7 @@ calibrate(
     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;
+    std::cout << "\t regularization_weight: " << regularization_weight << std::endl;
     return std::tuple<
       double, Eigen::MatrixXd, Eigen::MatrixXd, std::vector<Eigen::Vector3d>,
       std::vector<Eigen::Vector3d>>();
@@ -121,6 +122,7 @@ calibrate(
   optimizer.setRadialDistortionCoefficients(num_radial_coeffs);
   optimizer.setTangentialDistortion(use_tangential_distortion);
   optimizer.setRationalDistortionCoefficients(num_rational_coeffs);
+  optimizer.setRegularizationWeight(regularization_weight);
   optimizer.setVerbose(verbose);
   optimizer.setData(
     initial_camera_matrix_cv, initial_dist_coeffs_cv, object_points_list_cv, image_points_list_cv,
@@ -181,14 +183,16 @@ PYBIND11_MODULE(ceres_intrinsic_camera_calibrator_py, m)
             num_radial_coeffs (int): The number of radial distortion coefficients used during calibration
             num_rational_coeffs (int): The number of rational distortion coefficients used during calibration
             use_tangential_distortion (bool): Whether we should use tangential distortion during calibration
+            regularization_weight (double): The regularization weight for distortion coefficients
             verbose (bool): Whether we should print debug information
 
         Returns:
             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("num_rational_coeffs"),
-    py::arg("use_tangential_distortion"), py::arg("verbose") = false);
+    py::arg("use_tangential_distortion"), py::arg("regularization_weight"),
+    py::arg("verbose") = false);
 
 #ifdef VERSION_INFO
   m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO);

calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp

@@ -30,11 +30,11 @@
 
 int main(int argc, char ** argv)
 {
-  if (argc != 5) {
-    std::cout
-      << "Usage: " << argv[0]
-      << " <data_path> <num_radial_coeffs> <use_tangential_distortion> <num_rational_coeffs>"
-      << std::endl;
+  if (argc != 6) {
+    std::cout << "Usage: " << argv[0]
+              << " <data_path> <num_radial_coeffs> <use_tangential_distortion> "
+                 "<num_rational_coeffs> <regularization_weight>"
+              << std::endl;
     return 1;
   }
 
@@ -46,6 +46,7 @@ int main(int argc, char ** argv)
   int num_radial_distortion_coeffs = atoi(argv[2]);
   bool use_tangent_distortion = atoi(argv[3]);
   int num_rational_distortion_coeffs = atoi(argv[4]);
+  double regularization_weight = atof(argv[5]);
 
   // Placeholders
   std::vector<std::vector<cv::Point3f>> all_object_points;
@@ -248,6 +249,7 @@ int main(int argc, char ** argv)
   optimizer.setRadialDistortionCoefficients(num_radial_distortion_coeffs);
   optimizer.setTangentialDistortion(use_tangent_distortion);
   optimizer.setRationalDistortionCoefficients(num_rational_distortion_coeffs);
+  optimizer.setRegularizationWeight(regularization_weight);
   optimizer.setVerbose(true);
   optimizer.setData(
     mini_opencv_camera_matrix, mini_opencv_dist_coeffs, calibration_object_points,