feat(intrinsic_camera_calibrator): add coefficients regularization in…

… Ceres

Signed-off-by: amadeuszsz <amadeusz.szymko.2@tier4.jp>

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/coefficients_residual.hpp

@@ -0,0 +1,142 @@
+// 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__COEFFICIENTS_RESIDUAL_HPP_
+#define CERES_INTRINSIC_CAMERA_CALIBRATOR__COEFFICIENTS_RESIDUAL_HPP_
+
+#include <Eigen/Core>
+#include <opencv2/core.hpp>
+
+#include <ceres/autodiff_cost_function.h>
+#include <ceres/ceres.h>
+
+struct CoefficientsResidual
+{
+  static constexpr int RESIDUAL_DIM = 8;
+
+  CoefficientsResidual(
+    int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs,
+    int num_samples_factor, double regularization_weight)
+  {
+    radial_distortion_coeffs_ = radial_distortion_coeffs;
+    use_tangential_distortion_ = use_tangential_distortion;
+    rational_distortion_coeffs_ = rational_distortion_coeffs;
+    num_samples_factor_ = num_samples_factor;
+    regularization_weight_ = regularization_weight;
+  }
+
+  /*!
+   * 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] = num_samples_factor_ * regularization_weight_ * pow(k1, 2);
+    residuals[1] = num_samples_factor_ * regularization_weight_ * pow(k2, 2);
+    residuals[2] = num_samples_factor_ * regularization_weight_ * pow(k3, 2);
+    residuals[3] = num_samples_factor_ * regularization_weight_ * pow(p1, 2);
+    residuals[4] = num_samples_factor_ * regularization_weight_ * pow(p2, 2);
+    residuals[5] = num_samples_factor_ * regularization_weight_ * pow(k4, 2);
+    residuals[6] = num_samples_factor_ * regularization_weight_ * pow(k5, 2);
+    residuals[7] = num_samples_factor_ * regularization_weight_ * pow(k6, 2);
+
+    return true;
+  }
+
+  /*!
+   * Residual factory method
+   * @param[in] object_point The object point
+   * @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
+   * @returns the ceres residual
+   */
+  static ceres::CostFunction * createResidual(
+    int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs,
+    int num_samples_factor, double regularization_weight)
+  {
+    auto f = new CoefficientsResidual(
+      radial_distortion_coeffs, use_tangential_distortion, rational_distortion_coeffs,
+      num_samples_factor, regularization_weight);
+
+    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<CoefficientsResidual, RESIDUAL_DIM, 4>(f);
+        break;
+      case 1:
+        cost_function = new ceres::AutoDiffCostFunction<CoefficientsResidual, RESIDUAL_DIM, 5>(f);
+        break;
+      case 2:
+        cost_function = new ceres::AutoDiffCostFunction<CoefficientsResidual, RESIDUAL_DIM, 6>(f);
+        break;
+      case 3:
+        cost_function = new ceres::AutoDiffCostFunction<CoefficientsResidual, RESIDUAL_DIM, 7>(f);
+        break;
+      case 4:
+        cost_function = new ceres::AutoDiffCostFunction<CoefficientsResidual, RESIDUAL_DIM, 8>(f);
+        break;
+      case 5:
+        cost_function = new ceres::AutoDiffCostFunction<CoefficientsResidual, RESIDUAL_DIM, 9>(f);
+        break;
+      case 6:
+        cost_function = new ceres::AutoDiffCostFunction<CoefficientsResidual, RESIDUAL_DIM, 10>(f);
+        break;
+      case 7:
+        cost_function = new ceres::AutoDiffCostFunction<CoefficientsResidual, RESIDUAL_DIM, 11>(f);
+        break;
+      case 8:
+        cost_function = new ceres::AutoDiffCostFunction<CoefficientsResidual, RESIDUAL_DIM, 12>(f);
+        break;
+      default:
+        throw std::runtime_error("Invalid number of distortion coefficients");
+    }
+
+    return cost_function;
+  }
+
+  Eigen::Vector3d object_point_;
+  Eigen::Vector2d image_point_;
+  int radial_distortion_coeffs_;
+  bool use_tangential_distortion_;
+  int rational_distortion_coeffs_;
+  int num_samples_factor_;
+  double regularization_weight_;
+};
+
+#endif  // CERES_INTRINSIC_CAMERA_CALIBRATOR__COEFFICIENTS_RESIDUAL_HPP_

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/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,13 @@ void CeresCameraIntrinsicsOptimizer::solve()
     }
   }
 
+  problem.AddResidualBlock(
+    CoefficientsResidual::createResidual(
+      radial_distortion_coefficients_, use_tangential_distortion_,
+      rational_distortion_coefficients_, object_points_.size(), regularization_weight_),
+    nullptr,  // 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,
@@ -182,14 +184,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 (float): 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,

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py

@@ -37,6 +37,7 @@ def __init__(self, lock: threading.RLock, cfg: Dict = {}):
             bool, value=True, min_value=False, max_value=True
         )
         self.pre_calibration_num_samples = Parameter(int, value=6, min_value=0, max_value=20)
+        self.regularization_weight = Parameter(float, value=0.001, min_value=0.0, max_value=1.0)
 
         self.set_parameters(**cfg)
 
@@ -54,6 +55,7 @@ def _calibration_impl(self, detections: List[BoardDetection]) -> CeresCameraMode
             rational_distortion_coefficients=self.rational_distortion_coefficients.value,
             use_tangential_distortion=self.use_tangential_distortion.value,
             pre_calibration_num_samples=self.pre_calibration_num_samples.value,
+            regularization_weight=self.regularization_weight.value,
         )
         camera_model.calibrate(
             height=height,

calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py

@@ -43,6 +43,7 @@ def __init__(
         self.rational_distortion_coefficients = 3
         self.use_tangential_distortion = True
         self.pre_calibration_num_samples = 6
+        self.regularization_weight = 0.001
 
     def init_calibrate(
         self, object_points_list: List[np.array], image_points_list: List[np.array]
@@ -93,6 +94,7 @@ def _calibrate_impl(
             num_radial_coeffs=self.radial_distortion_coefficients,
             num_rational_coeffs=self.rational_distortion_coefficients,
             use_tangential_distortion=self.use_tangential_distortion,
+            regularization_weight=self.regularization_weight,
             verbose=False,
         )
 
@@ -105,10 +107,12 @@ def _update_config_impl(
         rational_distortion_coefficients: int,
         use_tangential_distortion: bool,
         pre_calibration_num_samples: int,
+        regularization_weight: float,
         **kwargs
     ):
         """Update parameters."""
         self.radial_distortion_coefficients = radial_distortion_coefficients
         self.rational_distortion_coefficients = rational_distortion_coefficients
         self.use_tangential_distortion = use_tangential_distortion
         self.pre_calibration_num_samples = pre_calibration_num_samples
+        self.regularization_weight = regularization_weight