feat(ceres_intrinsic_camera_calibrator): soften coeffs residuals (tier4#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]>

Author: amadeuszsz

calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_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

calibrators/intrinsic_camera_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;
   }
 }

calibrators/intrinsic_camera_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)