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

@@ -0,0 +1,140 @@
+// 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;
+  }
+
+  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/board_detections/board_detection.py

@@ -20,7 +20,7 @@
 from typing import Tuple
 
 import cv2
-from intrinsic_camera_calibrator.camera_model import CameraModel
+from intrinsic_camera_calibrator.camera_models.camera_model import CameraModel
 import numpy as np
 
 
@@ -54,9 +54,8 @@ def __init__(
         self._cached_pose = None
         self._cached_flattened_3d_points = None
 
-    def _precompute_single_shot_model(self):
+    def _precompute_single_shot_model(self, model: CameraModel):
         """Compute and caches a camera model calibrated with the current detection."""
-        model = CameraModel()
         model.calibrate(
             height=self.height,
             width=self.width,
@@ -65,10 +64,10 @@ def _precompute_single_shot_model(self):
         )
         self._cached_camera_model = model
 
-    def _get_cached_model(self) -> CameraModel:
+    def _get_cached_model(self, model: CameraModel) -> CameraModel:
         """Return the single shot camera model and computes it is has not been pre-computed yet."""
         if self._cached_camera_model is None:
-            self._precompute_single_shot_model()
+            self._precompute_single_shot_model(model)
 
         return self._cached_camera_model
 
@@ -108,33 +107,26 @@ def get_center_2d(self) -> np.array:
         self._cached_center_2d = self.get_flattened_image_points().mean(axis=0)
         return self._cached_center_2d
 
-    def get_reprojection_errors(self, model: Optional[CameraModel] = None) -> np.array:
+    def get_reprojection_errors(self, model: CameraModel) -> np.array:
         """Return the error of projecting the object points into the image and comparing them with the detections."""
-        if model is None:
-            model = self._get_cached_model()
-
-        if (
-            self._cached_camera_model is not None
-            and model == self._cached_camera_model
-            and self._cached_reprojection_errors is not None
-        ):
+        if self._cached_camera_model is None:
+            self._precompute_single_shot_model(model)
+
+        if model == self._cached_camera_model and self._cached_reprojection_errors is not None:
             return self._cached_reprojection_errors
 
         self._cached_camera_model = model
         self._cached_reprojection_errors = model.get_reprojection_errors(self)
 
         return self._cached_reprojection_errors
 
-    def get_tilt(self, model: Optional[CameraModel] = None) -> float:
+    def get_tilt(self, model: CameraModel) -> float:
         """Return the angle difference between the detection and the camera. Specifically, the pose of the detection points considers +z pointing towards the camera and the camera itself uses +z pointing towards the scene."""
-        if model is None:
-            model = self._get_cached_model()
-
         if model == self._cached_camera_model and self._cached_tilt is not None:
             return self._cached_tilt
 
         # cSpell:enableCompoundWords
-        rvec, _ = self.get_pose()
+        rvec, _ = self.get_pose(model)
         rotmat, _ = cv2.Rodrigues(rvec)
         rotmat[:2, :] *= -1
 
@@ -145,15 +137,12 @@ def get_tilt(self, model: Optional[CameraModel] = None) -> float:
 
         return self._cached_tilt
 
-    def get_rotation_angles(self, model: Optional[CameraModel] = None) -> Tuple[float, float]:
+    def get_rotation_angles(self, model: CameraModel) -> Tuple[float, float]:
         """Return the angle difference between the detection and the camera with respect to the x and y axes of the camera."""
-        if model is None:
-            model = self._get_cached_model()
-
         if model == self._cached_camera_model and self._cached_rotation_angles is not None:
             return self._cached_rotation_angles
 
-        rvec, _ = self.get_pose()
+        rvec, _ = self.get_pose(model)
         rotmat, _ = cv2.Rodrigues(rvec)
         rotmat[:2, :] *= -1
 
@@ -166,11 +155,8 @@ def get_rotation_angles(self, model: Optional[CameraModel] = None) -> Tuple[floa
 
         return self._cached_rotation_angles
 
-    def get_pose(self, model: Optional[CameraModel] = None) -> Tuple[np.array, np.array]:
+    def get_pose(self, model: CameraModel) -> Tuple[np.array, np.array]:
         """Return the pose of the detection in rodrigues tuple formulation. If a model is not provided, whe single-shot version is used, which produced only a rough estimation in most cases, and a complete incorrect one in some."""
-        if model is None:
-            model = self._get_cached_model()
-
         if model == self._cached_camera_model and self._cached_pose is not None:
             return self._cached_pose
 
@@ -179,11 +165,8 @@ def get_pose(self, model: Optional[CameraModel] = None) -> Tuple[np.array, np.ar
 
         return self._cached_pose
 
-    def get_flattened_3d_points(self, model: Optional[CameraModel] = None) -> np.array:
+    def get_flattened_3d_points(self, model: CameraModel) -> np.array:
         """Get the image points reprojected into camera coordinates in the 3d space as a (M, 3) array."""
-        if model is None:
-            model = self._get_cached_model()
-
         if model == self._cached_camera_model and self._cached_flattened_3d_points is not None:
             return self._cached_pose