From dfa06af3c2ec1ae70989a7c5f2f4c012d9f0ad37 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Fri, 7 Feb 2025 14:37:57 +0900 Subject: [PATCH] feat(ceres_intrinsic_camera_calibrator): soften coeffs residuals (#226) * feat(ceres_intrinsic_camera_calibrator): set threshold for clamping residuals Signed-off-by: Amadeusz Szymko * feat(ceres_intrinsic_camera_calibrator): remove threshold & use log function for residuals Signed-off-by: Amadeusz Szymko * feat(intrinsic_camera_calibrator): logging Signed-off-by: Amadeusz Szymko * feat(ceres_intrinsic_camera_calibrator): ignore regularization for 0.0 weight Signed-off-by: Amadeusz Szymko --------- Signed-off-by: Amadeusz Szymko --- .../distortion_coefficients_residual.hpp | 27 +++++++++++++------ .../src/ceres_camera_intrinsics_optimizer.cpp | 20 +++++++------- .../camera_calibrator.py | 2 ++ 3 files changed, 32 insertions(+), 17 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp index 78636653..213e728c 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/distortion_coefficients_residual.hpp +++ b/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 + 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/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp index ea953d85..d6118165 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp +++ b/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 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/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py index e19693f9..87d537a1 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_calibrator.py +++ b/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)