feat(ceres_intrinsic_camera_calibrator): soften coeffs residuals (#226)

* feat(ceres_intrinsic_camera_calibrator): set threshold for clamping residuals Signed-off-by: Amadeusz Szymko <[email protected]> * feat(ceres_intrinsic_camera_calibrator): remove threshold & use log function for residuals Signed-off-by: Amadeusz Szymko <[email protected]> * feat(intrinsic_camera_calibrator): logging Signed-off-by: Amadeusz Szymko <[email protected]> * feat(ceres_intrinsic_camera_calibrator): ignore regularization for 0.0 weight Signed-off-by: Amadeusz Szymko <[email protected]> --------- Signed-off-by: Amadeusz Szymko <[email protected]>
tier4 · Feb 7, 2025 · dfa06af · dfa06af
1 parent a30037d
commit dfa06af
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 17 deletions.
diff --git a/...calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp b/...calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp
@@ -56,18 +56,29 @@ struct DistortionCoefficientsResidual
     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;
+    residuals[0] = getResidual(k1);
+    residuals[1] = getResidual(k2);
+    residuals[2] = getResidual(k3);
+    residuals[3] = getResidual(p1);
+    residuals[4] = getResidual(p2);
+    residuals[5] = getResidual(k4);
+    residuals[6] = getResidual(k5);
+    residuals[7] = getResidual(k6);
 
     return true;
   }
 
+  /*!
+   * Calculates the residual for a given distortion coefficient
+   * @param[in] value The distortion coefficient
+   * @returns The residual
+   */
+  template <typename T>
+  T getResidual(const T & value) const
+  {
+    return ceres::log(ceres::abs(value) + T(1.0));
+  }
+
   /*!
    * Residual factory method
    * @param[in] radial_distortion_coeffs The number of radial distortion coefficients

diff --git a/...ra_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp b/...ra_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp
@@ -360,13 +360,15 @@ 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());
+  if (regularization_weight_ > 0.0) {
+    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;
@@ -376,7 +378,7 @@ void CeresCameraIntrinsicsOptimizer::solve()
   problem.Evaluate(eval_opt, &initial_cost, &residuals, nullptr, nullptr);
 
   if (verbose_) {
-    std::cout << "Initial cost: " << initial_cost;
+    std::cout << "Initial cost: " << initial_cost << std::endl;
   }
 
   ceres::Solver::Options options;
@@ -392,6 +394,6 @@ void CeresCameraIntrinsicsOptimizer::solve()
   ceres::Solve(options, &problem, &summary);
 
   if (verbose_) {
-    std::cout << "Report: " << summary.FullReport();
+    std::cout << "Report: " << std::endl << summary.FullReport() << std::endl;
   }
 }
diff --git a/...a_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/...a_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py
@@ -811,6 +811,8 @@ def process_calibration_results(
         self.calibration_evaluation_inlier_rms_label.setText(
             f"\trms error (inliers): {evaluation_inlier_rms_error:.3f}"  # noqa E231
         )
+        logging.info(f"Camera matrix:\n{calibrated_model.k}")
+        logging.info(f"Distortion coefficients:\n{calibrated_model.d}")
 
         self.calibrator_type_combobox.setEnabled(True)
         self.calibration_parameters_button.setEnabled(True)