From 98e7cde29ccd45ea0095b637532f6068ad79bf08 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Fri, 17 Jan 2025 18:31:15 +0900 Subject: [PATCH 01/15] feat(ceres_intrinsic_camera_calibrator): add FOV regularization Signed-off-by: Amadeusz Szymko --- .../ceres_camera_intrinsics_optimizer.hpp | 22 +- .../fov_residual.hpp | 309 ++++++++++++++++++ .../src/ceres_camera_intrinsics_optimizer.cpp | 44 ++- .../ceres_intrinsic_camera_calibrator_py.cpp | 23 +- .../src/main.cpp | 21 +- .../config/ceres_intrinsics_calibrator.yaml | 3 +- .../calibrators/ceres_calibrator.py | 8 +- .../camera_models/ceres_camera_model.py | 14 +- 8 files changed, 415 insertions(+), 29 deletions(-) create mode 100644 calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp index 099922b0..adec6c0f 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp @@ -70,9 +70,22 @@ class CeresCameraIntrinsicsOptimizer /*! * Sets the regularization weight for the distortion coefficients - * @param[in] regularization_weight the regularization weight + * @param[in] coeffs_regularization_weight the regularization weight */ - void setRegularizationWeight(double regularization_weight); + void setCoeffsRegularizationWeight(double coeffs_regularization_weight); + + /*! + * Sets the regularization weight for the field of view + * @param[in] fov_regularization_weight the regularization weight + */ + void setFovRegularizationWeight(double fov_regularization_weight); + + /*! + * Sets the source dimensions + * @param[in] width the width of the source image + * @param[in] height the height of the source image + */ + void setSourceDimensions(int width, int height); /*! * Sets the verbose mode @@ -132,7 +145,10 @@ class CeresCameraIntrinsicsOptimizer int radial_distortion_coefficients_; bool use_tangential_distortion_; int rational_distortion_coefficients_; - double regularization_weight_; + double coeffs_regularization_weight_; + double fov_regularization_weight_; + int width_; + int height_; bool verbose_; cv::Mat_ camera_matrix_; cv::Mat_ distortion_coeffs_; diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp new file mode 100644 index 00000000..c8407c79 --- /dev/null +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp @@ -0,0 +1,309 @@ +// 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__FOV_RESIDUAL_HPP_ +#define CERES_INTRINSIC_CAMERA_CALIBRATOR__FOV_RESIDUAL_HPP_ + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +struct FOVResidual +{ + static constexpr int INTRINSICS_CX_INDEX = 0; + static constexpr int INTRINSICS_CY_INDEX = 1; + static constexpr int INTRINSICS_FX_INDEX = 2; + static constexpr int INTRINSICS_FY_INDEX = 3; + + static constexpr int RESIDUAL_DIM = 8; + + static constexpr int UNDIST_ITERS = 100; + + FOVResidual( + int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs, + int width, int height) + { + radial_distortion_coeffs_ = radial_distortion_coeffs; + use_tangential_distortion_ = use_tangential_distortion; + rational_distortion_coeffs_ = rational_distortion_coeffs; + width_ = width; + height_ = height; + } + + /*! + * 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 + bool operator()(const T * const camera_intrinsics, T * residuals) const + { + const T null_value = T(0.0); + const T depth = T(1.0); + std::vector shifts = {T(0.01), T(0.03), T(0.05), T(0.1), T(0.3), T(0.5), T(1.0), T(3.0)}; + + const T width_t = static_cast(width_); + const T height_t = static_cast(height_); + int distortion_index = 4; + const T & cx = camera_intrinsics[INTRINSICS_CX_INDEX]; + const T & cy = camera_intrinsics[INTRINSICS_CY_INDEX]; + const T & fx = camera_intrinsics[INTRINSICS_FX_INDEX]; + const T & fy = camera_intrinsics[INTRINSICS_FY_INDEX]; + 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; + + auto apply_residual = + [this, residuals, shifts, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth]( + const int & idx, const T & u, const T & v, const T & sign_shift_x, const T & sign_shift_y) { + residuals[idx] = T(0.0); + for (const auto & shift : shifts) { + auto [x, y] = imageToCamera(u, v, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6); + auto [u_shifted, v_shifted] = cameraToImage( + x + shift * sign_shift_x, y + shift * sign_shift_y, cx, cy, fx, fy, k1, k2, k3, p1, p2, + k4, k5, k6, depth); + residuals[idx] += getFovResidual(u_shifted, v_shifted); + } + }; + + // Middle top + apply_residual(0, width_t / T(2.0), T(0.0), T(0.0), T(-1.0)); + + // Middle left + apply_residual(1, T(0.0), height_t / T(2.0), T(-1.0), T(0.0)); + + // Middle bottom + apply_residual(2, width_t / T(2.0), height_t - T(1.0), T(0.0), T(1.0)); + + // Middle right + apply_residual(3, width_t - T(1.0), height_t / T(2.0), T(1.0), T(0.0)); + + // Top left + apply_residual(4, T(0.0), T(0.0), T(-1.0), T(-1.0)); + + // Top right + apply_residual(5, width_t - T(1.0), T(0.0), T(1.0), T(-1.0)); + + // Bottom left + apply_residual(6, T(0.0), height_t - T(1.0), T(-1.0), T(1.0)); + + // Bottom right + apply_residual(7, width_t - T(1.0), height_t - T(1.0), T(1.0), T(1.0)); + + return true; + } + + /*! + * Calculates FOV residual (closest border) for given pixel in image coordinate system + * @param[in] u The pixel x coordinate + * @param[in] v The pixel y coordinate + * @returns The residual + */ + template + T getFovResidual(const T u, const T v) const + { + T width_t = static_cast(width_); + T height_t = static_cast(height_); + + if (u >= T(0.0) && u <= width_t - T(1.0) && v >= T(0.0) && v <= height_t - T(1.0)) { + T closest_u = std::min(u, width_t - u - T(1.0)) / (std::max(height_t, width_t) - T(1.0)); + T closest_v = std::min(v, height_t - v - T(1.0)) / (std::max(height_t, width_t) - T(1.0)); + return std::min(closest_u, closest_v); + } + return T(0.0); + } + + /*! + * Converts a real world coordinate to a pixel (distorted) coordinate + * @param[in] x The x coordinate in camera frame + * @param[in] y The y coordinate in camera frame + * @param[in] cx The camera center x coordinate + * @param[in] cy The camera center y coordinate + * @param[in] fx The focal length x + * @param[in] fy The focal length y + * @param[in] k1 The radial distortion coefficient k1 + * @param[in] k2 The radial distortion coefficient k2 + * @param[in] k3 The radial distortion coefficient k3 + * @param[in] p1 The tangential distortion coefficient p1 + * @param[in] p2 The tangential distortion coefficient p2 + * @param[in] k4 The rational distortion coefficient k4 + * @param[in] k5 The rational distortion coefficient k5 + * @param[in] k6 The rational distortion coefficient k6 + * @param[in] depth The depth of the point + * @returns The pixel x and y coordinates + */ + template + std::pair cameraToImage( + const T x, const T y, const T cx, const T cy, const T fx, const T fy, const T k1, const T k2, + const T k3, const T p1, const T p2, const T k4, const T k5, const T k6, T depth = T(1.0)) const + { + const T xp = x / depth; + const T yp = y / depth; + const T r2 = xp * xp + yp * yp; + const T dn = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + const T dd = 1.0 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2; + const T d = dn / dd; + const T xy = xp * yp; + const T tdx = 2.0 * p1 * xy + p2 * (r2 + 2.0 * xp * xp); + const T tdy = 2.0 * p2 * xy + p1 * (r2 + 2.0 * yp * yp); + + const T u = cx + fx * (xp * d + tdx); + const T v = cy + fy * (yp * d + tdy); + + return std::make_pair(u, v); + } + + /*! + * Converts a pixel coordinate to a point (undistorted) in camera coordinates system + * @param[in] u The x coordinate of pixel in image coordinate system + * @param[in] v The y coordinate of pixel in image coordinate system + * @param[in] cx The camera center x coordinate + * @param[in] cy The camera center y coordinate + * @param[in] fx The focal length x + * @param[in] fy The focal length y + * @param[in] k1 The radial distortion coefficient k1 + * @param[in] k2 The radial distortion coefficient k2 + * @param[in] k3 The radial distortion coefficient k3 + * @param[in] p1 The tangential distortion coefficient p1 + * @param[in] p2 The tangential distortion coefficient p2 + * @param[in] k4 The rational distortion coefficient k4 + * @param[in] k5 The rational distortion coefficient k5 + * @param[in] k6 The rational distortion coefficient k6 + * @param[in] depth The depth of the point + * @returns The real world x and y coordinates in camera frame + */ + template + std::pair imageToCamera( + const T u, const T v, const T cx, const T cy, const T fx, const T fy, const T k1, const T k2, + const T k3, const T p1, const T p2, const T k4, const T k5, const T k6, T depth = T(1.0), + const T tol = T(1e-6)) const + { + T xp = (u - cx) / fx; + T yp = (v - cy) / fy; + + for (int i = 0; i < UNDIST_ITERS; i++) { + const T r2 = xp * xp + yp * yp; + const T dn = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + const T dd = 1.0 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2; + const T d = dn / dd; + const T xy = xp * yp; + const T tdx = 2.0 * p1 * xy + p2 * (r2 + 2.0 * xp * xp); + const T tdy = 2.0 * p2 * xy + p1 * (r2 + 2.0 * yp * yp); + + const T predicted_x = xp * d + tdx; + const T predicted_y = yp * d + tdy; + + const T delta_x = (u - cx) / fx - predicted_x; + const T delta_y = (v - cy) / fy - predicted_y; + + if (delta_x * delta_x + delta_y * delta_y < tol) { + break; + } + + xp += delta_x; + yp += delta_y; + } + + const T x = xp * depth; + const T y = yp * depth; + + return std::make_pair(x, y); + } + + /*! + * 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 + * @param[in] width the source image width + * @param[in] height the source image height + * @returns the ceres residual + */ + static ceres::CostFunction * createResidual( + int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs, + int width, int height) + { + auto f = new FOVResidual( + radial_distortion_coeffs, use_tangential_distortion, rational_distortion_coeffs, width, + height); + + int distortion_coefficients = radial_distortion_coeffs + + 2 * static_cast(use_tangential_distortion) + + rational_distortion_coeffs; + ceres::CostFunction * cost_function = nullptr; + + switch (distortion_coefficients) { + case 0: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 1: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 2: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 3: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 4: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 5: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 6: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 7: + cost_function = new ceres::AutoDiffCostFunction(f); + break; + case 8: + cost_function = new ceres::AutoDiffCostFunction(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 width_; + int height_; +}; + +#endif // CERES_INTRINSIC_CAMERA_CALIBRATOR__FOV_RESIDUAL_HPP_ 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..fefefa66 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 @@ -15,6 +15,7 @@ #include #include #include +#include #include #include #include @@ -51,9 +52,21 @@ void CeresCameraIntrinsicsOptimizer::setRationalDistortionCoefficients( rational_distortion_coefficients_ = rational_distortion_coefficients; } -void CeresCameraIntrinsicsOptimizer::setRegularizationWeight(double regularization_weight) +void CeresCameraIntrinsicsOptimizer::setCoeffsRegularizationWeight( + double coeffs_regularization_weight) { - regularization_weight_ = regularization_weight; + coeffs_regularization_weight_ = coeffs_regularization_weight; +} + +void CeresCameraIntrinsicsOptimizer::setFovRegularizationWeight(double fov_regularization_weight) +{ + fov_regularization_weight_ = fov_regularization_weight; +} + +void CeresCameraIntrinsicsOptimizer::setSourceDimensions(int width, int height) +{ + width_ = width; + height_ = height; } void CeresCameraIntrinsicsOptimizer::setVerbose(bool verbose) { verbose_ = verbose; } @@ -360,13 +373,26 @@ 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 (coeffs_regularization_weight_ > 0.0) { + problem.AddResidualBlock( + DistortionCoefficientsResidual::createResidual( + radial_distortion_coefficients_, use_tangential_distortion_, + rational_distortion_coefficients_), + new ceres::ScaledLoss( + nullptr, coeffs_regularization_weight_ * object_points_.size(), + ceres::TAKE_OWNERSHIP), // L2 + intrinsics_placeholder_.data()); + } + + if (fov_regularization_weight_ > 0.0) { + problem.AddResidualBlock( + FOVResidual::createResidual( + radial_distortion_coefficients_, use_tangential_distortion_, + rational_distortion_coefficients_, width_, height_), + new ceres::ScaledLoss( + nullptr, fov_regularization_weight_ * object_points_.size(), ceres::TAKE_OWNERSHIP), // L2 + intrinsics_placeholder_.data()); + } double initial_cost = 0.0; std::vector residuals; diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp index d4993508..67746a65 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp @@ -45,7 +45,8 @@ calibrate( const std::vector & 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, double regularization_weight, bool verbose) + bool use_tangential_distortion, double coeffs_regularization_weight, + double fov_regularization_weight, int width, int height, bool verbose) { Eigen::Index num_dist_coeffs = initial_dist_coeffs_eigen.rows() * initial_dist_coeffs_eigen.cols(); @@ -54,7 +55,8 @@ 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 || regularization_weight < 0.0) { + num_dist_coeffs != expected_dist_coeffs || coeffs_regularization_weight < 0.0 || + fov_regularization_weight < 0.0 || width <= 0 || height <= 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,7 +65,10 @@ 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; + std::cout << "\t coeffs_regularization_weight: " << coeffs_regularization_weight << std::endl; + std::cout << "\t fov_regularization_weight: " << fov_regularization_weight << std::endl; + std::cout << "\t width: " << width << std::endl; + std::cout << "\t height: " << height << std::endl; return std::tuple< double, Eigen::MatrixXd, Eigen::MatrixXd, std::vector, std::vector>(); @@ -122,7 +127,9 @@ calibrate( optimizer.setRadialDistortionCoefficients(num_radial_coeffs); optimizer.setTangentialDistortion(use_tangential_distortion); optimizer.setRationalDistortionCoefficients(num_rational_coeffs); - optimizer.setRegularizationWeight(regularization_weight); + optimizer.setCoeffsRegularizationWeight(coeffs_regularization_weight); + optimizer.setFovRegularizationWeight(fov_regularization_weight); + optimizer.setSourceDimensions(width, height); optimizer.setVerbose(verbose); optimizer.setData( initial_camera_matrix_cv, initial_dist_coeffs_cv, object_points_list_cv, image_points_list_cv, @@ -183,7 +190,10 @@ 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 (double): The regularization weight for distortion coefficients + coeffs_regularization_weight (double): The regularization weight for distortion coefficients + fov_regularization_weight (double): The regularization weight for the field of view + width (int): The width of the source images + height (int): The height of the source images verbose (bool): Whether we should print debug information Returns: @@ -191,7 +201,8 @@ PYBIND11_MODULE(ceres_intrinsic_camera_calibrator_py, m) )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("regularization_weight"), + py::arg("use_tangential_distortion"), py::arg("coeffs_regularization_weight"), + py::arg("fov_regularization_weight"), py::arg("width"), py::arg("height"), py::arg("verbose") = false); #ifdef VERSION_INFO diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp index 77700deb..410a116e 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp @@ -30,10 +30,10 @@ int main(int argc, char ** argv) { - if (argc != 6) { + if (argc != 7) { std::cout << "Usage: " << argv[0] << " " - " " + " " << std::endl; return 1; } @@ -46,7 +46,10 @@ 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]); + double coeffs_regularization_weight = atof(argv[5]); + double fov_regularization_weight = atof(argv[6]); + int width = 0; + int height = 0; // Placeholders std::vector> all_object_points; @@ -100,6 +103,14 @@ int main(int argc, char ** argv) cv::Mat grayscale_img = cv::imread(image_paths[i], cv::IMREAD_GRAYSCALE | cv::IMREAD_IGNORE_ORIENTATION); + if (width == 0 || height == 0) { + width = grayscale_img.cols; + height = grayscale_img.rows; + } else { + assert(width == grayscale_img.cols); + assert(height == grayscale_img.rows); + } + assert(size.height == -1 || size.height == grayscale_img.rows); assert(size.width == -1 || size.width == grayscale_img.cols); size = grayscale_img.size(); @@ -249,7 +260,9 @@ 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.setCoeffsRegularizationWeight(coeffs_regularization_weight); + optimizer.setFovRegularizationWeight(fov_regularization_weight); + optimizer.setSourceDimensions(width, height); optimizer.setVerbose(true); optimizer.setData( mini_opencv_camera_matrix, mini_opencv_dist_coeffs, calibration_object_points, diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/ceres_intrinsics_calibrator.yaml b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/ceres_intrinsics_calibrator.yaml index 0188281d..ba4fea82 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/ceres_intrinsics_calibrator.yaml +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/config/ceres_intrinsics_calibrator.yaml @@ -28,7 +28,8 @@ calibration_parameters: rational_distortion_coefficients: 3 use_tangential_distortion: true pre_calibration_num_samples: 40 - regularization_weight: 0.2 + coeffs_regularization_weight: 0.2 + fov_regularization_weight: 0.0 calibrator_type: ceres diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py index 124eb5a9..2ebb49ce 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/calibrators/ceres_calibrator.py @@ -37,7 +37,10 @@ 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=40, min_value=1, max_value=100) - self.regularization_weight = Parameter(float, value=0.001, min_value=0.0, max_value=1.0) + self.coeffs_regularization_weight = Parameter( + float, value=0.001, min_value=0.0, max_value=1.0 + ) + self.fov_regularization_weight = Parameter(float, value=0.0, min_value=0.0, max_value=1.0) self.set_parameters(**cfg) @@ -57,7 +60,8 @@ 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, + coeffs_regularization_weight=self.coeffs_regularization_weight.value, + fov_regularization_weight=self.fov_regularization_weight.value, ) camera_model.calibrate( height=height, diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py index a36e3b94..b1cbc0c4 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/camera_models/ceres_camera_model.py @@ -39,7 +39,8 @@ def __init__( self.rational_distortion_coefficients: Optional[int] = None self.use_tangential_distortion: Optional[bool] = None self.pre_calibration_num_samples: Optional[int] = None - self.regularization_weight: Optional[float] = None + self.coeffs_regularization_weight: Optional[float] = None + self.fov_regularization_weight: Optional[float] = None self.verbose = ( True if os.getenv("GLOG_minloglevel") == "0" else False ) # cSpell:ignore minloglevel @@ -93,7 +94,10 @@ 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, + coeffs_regularization_weight=self.coeffs_regularization_weight, + fov_regularization_weight=self.fov_regularization_weight, + width=self.width, + height=self.height, verbose=self.verbose, ) @@ -106,7 +110,8 @@ def _update_config_impl( rational_distortion_coefficients: int, use_tangential_distortion: bool, pre_calibration_num_samples: int, - regularization_weight: float, + coeffs_regularization_weight: float, + fov_regularization_weight: float, **kwargs ): """Update parameters.""" @@ -114,4 +119,5 @@ def _update_config_impl( 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 + self.coeffs_regularization_weight = coeffs_regularization_weight + self.fov_regularization_weight = fov_regularization_weight From 1dc1c3a853368a6f4e4c44aa585f7630311637b7 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Mon, 27 Jan 2025 18:27:00 +0900 Subject: [PATCH 02/15] feat(ceres_intrinsic_camera_calibrator): use approximation for undistortion and distortion Signed-off-by: Amadeusz Szymko --- .../fov_residual.hpp | 99 ++++++++++++++----- 1 file changed, 72 insertions(+), 27 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp index c8407c79..69bb916f 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp @@ -60,7 +60,8 @@ struct FOVResidual { const T null_value = T(0.0); const T depth = T(1.0); - std::vector shifts = {T(0.01), T(0.03), T(0.05), T(0.1), T(0.3), T(0.5), T(1.0), T(3.0)}; + const std::vector shifts = {T(0.01), T(0.03), T(0.05), T(0.1), + T(0.3), T(0.5), T(1.0), T(3.0)}; const T width_t = static_cast(width_); const T height_t = static_cast(height_); @@ -84,42 +85,53 @@ struct FOVResidual const T & k6 = rational_distortion_coeffs_ > 2 ? camera_intrinsics[distortion_index++] : null_value; - auto apply_residual = - [this, residuals, shifts, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth]( - const int & idx, const T & u, const T & v, const T & sign_shift_x, const T & sign_shift_y) { - residuals[idx] = T(0.0); - for (const auto & shift : shifts) { - auto [x, y] = imageToCamera(u, v, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6); - auto [u_shifted, v_shifted] = cameraToImage( - x + shift * sign_shift_x, y + shift * sign_shift_y, cx, cy, fx, fy, k1, k2, k3, p1, p2, - k4, k5, k6, depth); - residuals[idx] += getFovResidual(u_shifted, v_shifted); - } - }; + auto apply_residual = [this, residuals, shifts, width_t, height_t, cx, cy, fx, fy, k1, k2, k3, + p1, p2, k4, k5, k6, depth]( + const int & idx, const T & u, const T & v, + const T & backprojection_err_thr = T(10.0)) -> void { + residuals[idx] = T(0.0); + auto [x, y] = imageToCamera(u, v, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth); + auto [u_bpr, v_bpr] = + cameraToImage(x, y, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth); + auto backprojection_err = ceres::sqrt(ceres::pow(u - u_bpr, 2) + ceres::pow(v - v_bpr, 2)); + if (ceres::IsNaN(backprojection_err) || backprojection_err > backprojection_err_thr) { + return; + } + auto sign_shift_x = u <= T(0.0) ? T(-1.0) : u >= width_t - T(1.0) ? T(1.0) : T(0.0); + auto sign_shift_y = v <= T(0.0) ? T(-1.0) : v >= height_t - T(1.0) ? T(1.0) : T(0.0); + for (const auto & shift : shifts) { + auto [u_shifted, v_shifted] = cameraToImage( + x + shift * sign_shift_x, y + shift * sign_shift_y, cx, cy, fx, fy, k1, k2, k3, p1, p2, + k4, k5, k6, depth); + auto residual = getFovResidual(u_shifted, v_shifted); + // Weigh the residuals by the backprojection error + residuals[idx] += residual * (T(1.0) / (backprojection_err + T(1.0))); + } + }; // Middle top - apply_residual(0, width_t / T(2.0), T(0.0), T(0.0), T(-1.0)); + apply_residual(0, width_t / T(2.0) - T(1.0), T(0.0)); // Middle left - apply_residual(1, T(0.0), height_t / T(2.0), T(-1.0), T(0.0)); + apply_residual(1, T(0.0), height_t / T(2.0) - T(1.0)); // Middle bottom - apply_residual(2, width_t / T(2.0), height_t - T(1.0), T(0.0), T(1.0)); + apply_residual(2, width_t / T(2.0) - T(1.0), height_t - T(1.0)); // Middle right - apply_residual(3, width_t - T(1.0), height_t / T(2.0), T(1.0), T(0.0)); + apply_residual(3, width_t - T(1.0), height_t / T(2.0) - T(1.0)); // Top left - apply_residual(4, T(0.0), T(0.0), T(-1.0), T(-1.0)); + apply_residual(4, T(0.0), T(0.0)); // Top right - apply_residual(5, width_t - T(1.0), T(0.0), T(1.0), T(-1.0)); + apply_residual(5, width_t - T(1.0), T(0.0)); // Bottom left - apply_residual(6, T(0.0), height_t - T(1.0), T(-1.0), T(1.0)); + apply_residual(6, T(0.0), height_t - T(1.0)); // Bottom right - apply_residual(7, width_t - T(1.0), height_t - T(1.0), T(1.0), T(1.0)); + apply_residual(7, width_t - T(1.0), height_t - T(1.0)); return true; } @@ -171,9 +183,7 @@ struct FOVResidual const T xp = x / depth; const T yp = y / depth; const T r2 = xp * xp + yp * yp; - const T dn = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; - const T dd = 1.0 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2; - const T d = dn / dd; + const T d = getRadialDist(xp, yp, k1, k2, k3, k4, k5, k6); const T xy = xp * yp; const T tdx = 2.0 * p1 * xy + p2 * (r2 + 2.0 * xp * xp); const T tdy = 2.0 * p2 * xy + p1 * (r2 + 2.0 * yp * yp); @@ -214,9 +224,7 @@ struct FOVResidual for (int i = 0; i < UNDIST_ITERS; i++) { const T r2 = xp * xp + yp * yp; - const T dn = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; - const T dd = 1.0 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2; - const T d = dn / dd; + const T d = getRadialDist(xp, yp, k1, k2, k3, k4, k5, k6); const T xy = xp * yp; const T tdx = 2.0 * p1 * xy + p2 * (r2 + 2.0 * xp * xp); const T tdy = 2.0 * p2 * xy + p1 * (r2 + 2.0 * yp * yp); @@ -241,6 +249,43 @@ struct FOVResidual return std::make_pair(x, y); } + /*! + * Calculates radial distortion + * + * Approximation is applied if any rational distortion coefficient is negative. This approximation + * follows Taylor series expansion of a function around x0=0, also known as a Maclaurin series. + * In given context, only constant term of P''(x0) is not equal to zero, which sipmlifies the + * polynomial to a 2nd degree polynomial. + * + * @param[in] x Normalized input coordinate along x-axis + * @param[in] y Normalized input coordinate along y-axis + * @param[in] k1 The radial distortion coefficient k1 + * @param[in] k2 The radial distortion coefficient k2 + * @param[in] k3 The radial distortion coefficient k3 + * @param[in] k4 The rational distortion coefficient k4 + * @param[in] k5 The rational distortion coefficient k5 + * @param[in] k6 The rational distortion coefficient k6 + * @returns The 3rd degree polynomial coefficients + */ + template + T getRadialDist( + const T x, const T y, const T k1, const T k2, const T k3, const T k4, const T k5, + const T k6) const + { + const T r2 = x * x + y * y; + + // Use approximation if any rational distortion coefficient is negative + if (k4 < T(0.0) || k5 < T(0.0) || k6 < T(0.0)) { + const T dn = 1.0 + k1 * r2; + const T dd = 1.0 + k4 * r2; + return dn / dd; + } + + const T dn = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + const T dd = 1.0 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2; + return dn / dd; + } + /*! * Residual factory method * @param[in] object_point The object point From c8c032a33affa8c5773e419e1449d2ee616ffb3b Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Mon, 27 Jan 2025 19:02:19 +0900 Subject: [PATCH 03/15] style(ceres_intrinsic_camera_calibrator): cSpell Signed-off-by: Amadeusz Szymko --- .../ceres_intrinsic_camera_calibrator/fov_residual.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp index 69bb916f..ec3370bb 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp @@ -36,7 +36,7 @@ struct FOVResidual static constexpr int RESIDUAL_DIM = 8; - static constexpr int UNDIST_ITERS = 100; + static constexpr int UNDIST_ITERS = 100; // cSpell:ignore UNDIST FOVResidual( int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs, @@ -85,6 +85,7 @@ struct FOVResidual const T & k6 = rational_distortion_coeffs_ > 2 ? camera_intrinsics[distortion_index++] : null_value; + // cSpell:ignore backprojection auto apply_residual = [this, residuals, shifts, width_t, height_t, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth]( const int & idx, const T & u, const T & v, @@ -249,12 +250,13 @@ struct FOVResidual return std::make_pair(x, y); } + // cSpell:ignore Maclaurin /*! * Calculates radial distortion * * Approximation is applied if any rational distortion coefficient is negative. This approximation * follows Taylor series expansion of a function around x0=0, also known as a Maclaurin series. - * In given context, only constant term of P''(x0) is not equal to zero, which sipmlifies the + * In given context, only constant term of P''(x0) is not equal to zero, which simplifies the * polynomial to a 2nd degree polynomial. * * @param[in] x Normalized input coordinate along x-axis From 43ef5b77103e0f61c7dd5e7ee01b0f3ca4db5b9a Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Tue, 28 Jan 2025 10:26:29 +0900 Subject: [PATCH 04/15] style(ceres_intrinsic_camera_calibrator): sync with Gregorian calendar Signed-off-by: Amadeusz Szymko --- .../include/ceres_intrinsic_camera_calibrator/fov_residual.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp index ec3370bb..8950d82e 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 Tier IV, Inc. +// Copyright 2025 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. From 2ea83e3a0cf8d4df049527fb63e75aa011625453 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Fri, 31 Jan 2025 18:36:27 +0900 Subject: [PATCH 05/15] fix(ceres_intrinsic_camera_calibrator): remove unused headers Signed-off-by: Amadeusz Szymko --- .../include/ceres_intrinsic_camera_calibrator/fov_residual.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp index 8950d82e..b7b09405 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp @@ -16,10 +16,7 @@ #define CERES_INTRINSIC_CAMERA_CALIBRATOR__FOV_RESIDUAL_HPP_ #include -#include -#include -#include #include #include From 3c41206694894cad3c3c246296278fcdebdf2f41 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Fri, 31 Jan 2025 18:38:24 +0900 Subject: [PATCH 06/15] fix(ceres_intrinsic_camera_calibrator): remove cond statement for coeffs regularization block Signed-off-by: Amadeusz Szymko --- .../src/ceres_camera_intrinsics_optimizer.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) 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 fefefa66..620a9922 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 @@ -373,16 +373,14 @@ void CeresCameraIntrinsicsOptimizer::solve() } } - if (coeffs_regularization_weight_ > 0.0) { - problem.AddResidualBlock( - DistortionCoefficientsResidual::createResidual( - radial_distortion_coefficients_, use_tangential_distortion_, - rational_distortion_coefficients_), - new ceres::ScaledLoss( - nullptr, coeffs_regularization_weight_ * object_points_.size(), - ceres::TAKE_OWNERSHIP), // L2 - intrinsics_placeholder_.data()); - } + problem.AddResidualBlock( + DistortionCoefficientsResidual::createResidual( + radial_distortion_coefficients_, use_tangential_distortion_, + rational_distortion_coefficients_), + new ceres::ScaledLoss( + nullptr, coeffs_regularization_weight_ * object_points_.size(), + ceres::TAKE_OWNERSHIP), // L2 + intrinsics_placeholder_.data()); if (fov_regularization_weight_ > 0.0) { problem.AddResidualBlock( From f9036329f5acf6f95edeef848f7f6eb1ce662f9b Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Fri, 31 Jan 2025 18:41:58 +0900 Subject: [PATCH 07/15] fix(ceres_intrinsic_camera_calibrator): casting consistency for T Signed-off-by: Amadeusz Szymko --- .../ceres_intrinsic_camera_calibrator/fov_residual.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp index b7b09405..2317061c 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp @@ -60,8 +60,8 @@ struct FOVResidual const std::vector shifts = {T(0.01), T(0.03), T(0.05), T(0.1), T(0.3), T(0.5), T(1.0), T(3.0)}; - const T width_t = static_cast(width_); - const T height_t = static_cast(height_); + const T width_t = T(width_); + const T height_t = T(height_); int distortion_index = 4; const T & cx = camera_intrinsics[INTRINSICS_CX_INDEX]; const T & cy = camera_intrinsics[INTRINSICS_CY_INDEX]; @@ -143,8 +143,8 @@ struct FOVResidual template T getFovResidual(const T u, const T v) const { - T width_t = static_cast(width_); - T height_t = static_cast(height_); + T width_t = T(width_); + T height_t = T(height_); if (u >= T(0.0) && u <= width_t - T(1.0) && v >= T(0.0) && v <= height_t - T(1.0)) { T closest_u = std::min(u, width_t - u - T(1.0)) / (std::max(height_t, width_t) - T(1.0)); From b705e584f202f61023b07670be9800f7f250497b Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Mon, 3 Feb 2025 10:13:30 +0900 Subject: [PATCH 08/15] feat(ceres_intrinsic_camera_calibrator): apply FOV regularization out of main optimization graph Signed-off-by: Amadeusz Szymko --- .../ceres_camera_intrinsics_optimizer.hpp | 5 ++ .../fov_residual.hpp | 72 ++++++------------- .../src/ceres_camera_intrinsics_optimizer.cpp | 46 +++++++++--- .../ceres_intrinsic_camera_calibrator_py.cpp | 3 + .../src/main.cpp | 3 + 5 files changed, 69 insertions(+), 60 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp index adec6c0f..73b735e3 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp @@ -141,6 +141,11 @@ class CeresCameraIntrinsicsOptimizer */ void solve(); + /*! + * Applies the optimization for the field of view + */ + void solveFov(); + protected: int radial_distortion_coefficients_; bool use_tangential_distortion_; diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp index 2317061c..f03e277c 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp @@ -21,6 +21,7 @@ #include #include +#include #include #include @@ -86,14 +87,14 @@ struct FOVResidual auto apply_residual = [this, residuals, shifts, width_t, height_t, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth]( const int & idx, const T & u, const T & v, - const T & backprojection_err_thr = T(10.0)) -> void { + const T & backprojection_err_thr = T(10.0)) -> bool { residuals[idx] = T(0.0); auto [x, y] = imageToCamera(u, v, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth); auto [u_bpr, v_bpr] = cameraToImage(x, y, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth); auto backprojection_err = ceres::sqrt(ceres::pow(u - u_bpr, 2) + ceres::pow(v - v_bpr, 2)); if (ceres::IsNaN(backprojection_err) || backprojection_err > backprojection_err_thr) { - return; + return false; } auto sign_shift_x = u <= T(0.0) ? T(-1.0) : u >= width_t - T(1.0) ? T(1.0) : T(0.0); auto sign_shift_y = v <= T(0.0) ? T(-1.0) : v >= height_t - T(1.0) ? T(1.0) : T(0.0); @@ -105,31 +106,36 @@ struct FOVResidual // Weigh the residuals by the backprojection error residuals[idx] += residual * (T(1.0) / (backprojection_err + T(1.0))); } + return true; }; + std::size_t valid_residuals = 0; + // Middle top - apply_residual(0, width_t / T(2.0) - T(1.0), T(0.0)); + valid_residuals += apply_residual(0, width_t / T(2.0) - T(1.0), T(0.0)); // Middle left - apply_residual(1, T(0.0), height_t / T(2.0) - T(1.0)); + valid_residuals += apply_residual(1, T(0.0), height_t / T(2.0) - T(1.0)); // Middle bottom - apply_residual(2, width_t / T(2.0) - T(1.0), height_t - T(1.0)); + valid_residuals += apply_residual(2, width_t / T(2.0) - T(1.0), height_t - T(1.0)); // Middle right - apply_residual(3, width_t - T(1.0), height_t / T(2.0) - T(1.0)); + valid_residuals += apply_residual(3, width_t - T(1.0), height_t / T(2.0) - T(1.0)); // Top left - apply_residual(4, T(0.0), T(0.0)); + valid_residuals += apply_residual(4, T(0.0), T(0.0)); // Top right - apply_residual(5, width_t - T(1.0), T(0.0)); + valid_residuals += apply_residual(5, width_t - T(1.0), T(0.0)); // Bottom left - apply_residual(6, T(0.0), height_t - T(1.0)); + valid_residuals += apply_residual(6, T(0.0), height_t - T(1.0)); // Bottom right - apply_residual(7, width_t - T(1.0), height_t - T(1.0)); + valid_residuals += apply_residual(7, width_t - T(1.0), height_t - T(1.0)); + + LOG(INFO) << "Valid FOV residuals: " << valid_residuals << " / 8" << std::endl; return true; } @@ -181,7 +187,9 @@ struct FOVResidual const T xp = x / depth; const T yp = y / depth; const T r2 = xp * xp + yp * yp; - const T d = getRadialDist(xp, yp, k1, k2, k3, k4, k5, k6); + const T dn = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + const T dd = 1.0 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2; + const T d = dn / dd; const T xy = xp * yp; const T tdx = 2.0 * p1 * xy + p2 * (r2 + 2.0 * xp * xp); const T tdy = 2.0 * p2 * xy + p1 * (r2 + 2.0 * yp * yp); @@ -222,7 +230,9 @@ struct FOVResidual for (int i = 0; i < UNDIST_ITERS; i++) { const T r2 = xp * xp + yp * yp; - const T d = getRadialDist(xp, yp, k1, k2, k3, k4, k5, k6); + const T dn = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + const T dd = 1.0 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2; + const T d = dn / dd; const T xy = xp * yp; const T tdx = 2.0 * p1 * xy + p2 * (r2 + 2.0 * xp * xp); const T tdy = 2.0 * p2 * xy + p1 * (r2 + 2.0 * yp * yp); @@ -247,44 +257,6 @@ struct FOVResidual return std::make_pair(x, y); } - // cSpell:ignore Maclaurin - /*! - * Calculates radial distortion - * - * Approximation is applied if any rational distortion coefficient is negative. This approximation - * follows Taylor series expansion of a function around x0=0, also known as a Maclaurin series. - * In given context, only constant term of P''(x0) is not equal to zero, which simplifies the - * polynomial to a 2nd degree polynomial. - * - * @param[in] x Normalized input coordinate along x-axis - * @param[in] y Normalized input coordinate along y-axis - * @param[in] k1 The radial distortion coefficient k1 - * @param[in] k2 The radial distortion coefficient k2 - * @param[in] k3 The radial distortion coefficient k3 - * @param[in] k4 The rational distortion coefficient k4 - * @param[in] k5 The rational distortion coefficient k5 - * @param[in] k6 The rational distortion coefficient k6 - * @returns The 3rd degree polynomial coefficients - */ - template - T getRadialDist( - const T x, const T y, const T k1, const T k2, const T k3, const T k4, const T k5, - const T k6) const - { - const T r2 = x * x + y * y; - - // Use approximation if any rational distortion coefficient is negative - if (k4 < T(0.0) || k5 < T(0.0) || k6 < T(0.0)) { - const T dn = 1.0 + k1 * r2; - const T dd = 1.0 + k4 * r2; - return dn / dd; - } - - const T dn = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; - const T dd = 1.0 + k4 * r2 + k5 * r2 * r2 + k6 * r2 * r2 * r2; - return dn / dd; - } - /*! * Residual factory method * @param[in] object_point The object point 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 620a9922..411bf6c1 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 @@ -382,16 +382,6 @@ void CeresCameraIntrinsicsOptimizer::solve() ceres::TAKE_OWNERSHIP), // L2 intrinsics_placeholder_.data()); - if (fov_regularization_weight_ > 0.0) { - problem.AddResidualBlock( - FOVResidual::createResidual( - radial_distortion_coefficients_, use_tangential_distortion_, - rational_distortion_coefficients_, width_, height_), - new ceres::ScaledLoss( - nullptr, fov_regularization_weight_ * object_points_.size(), ceres::TAKE_OWNERSHIP), // L2 - intrinsics_placeholder_.data()); - } - double initial_cost = 0.0; std::vector residuals; ceres::Problem::EvaluateOptions eval_opt; @@ -419,3 +409,39 @@ void CeresCameraIntrinsicsOptimizer::solve() std::cout << "Report: " << summary.FullReport(); } } + +void CeresCameraIntrinsicsOptimizer::solveFov() +{ + ceres::Problem problem; + + problem.AddResidualBlock( + FOVResidual::createResidual( + radial_distortion_coefficients_, use_tangential_distortion_, + rational_distortion_coefficients_, width_, height_), + new ceres::ScaledLoss( + nullptr, fov_regularization_weight_ * object_points_.size(), ceres::TAKE_OWNERSHIP), // L2 + intrinsics_placeholder_.data()); + + double initial_cost = 0.0; + std::vector residuals; + ceres::Problem::EvaluateOptions eval_opt; + eval_opt.num_threads = 1; + problem.GetResidualBlocks(&eval_opt.residual_blocks); + problem.Evaluate(eval_opt, &initial_cost, &residuals, nullptr, nullptr); + + if (verbose_) { + std::cout << "[FOV] Initial cost: " << initial_cost << std::endl; + } + + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_SCHUR; // cSpell:ignore SCHUR + options.minimizer_progress_to_stdout = verbose_; + options.max_num_iterations = 500; + options.function_tolerance = 1e-10; + options.gradient_tolerance = 1e-14; + options.num_threads = 8; + options.max_num_consecutive_invalid_steps = 1000; + options.use_inner_iterations = false; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); +} diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp index 67746a65..9eb66a1e 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp @@ -137,6 +137,9 @@ calibrate( optimizer.dataToPlaceholders(); optimizer.evaluate(); optimizer.solve(); + if (fov_regularization_weight > 0.0) { + optimizer.solveFov(); + } optimizer.placeholdersToData(); optimizer.evaluate(); double rms_error = optimizer.getSolution(camera_matrix_cv, dist_coeffs_cv, rvecs_cv, tvecs_cv); diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp index 410a116e..a13e4c33 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp @@ -270,6 +270,9 @@ int main(int argc, char ** argv) optimizer.dataToPlaceholders(); optimizer.evaluate(); optimizer.solve(); + if (fov_regularization_weight > 0.0) { + optimizer.solveFov(); + } optimizer.placeholdersToData(); optimizer.evaluate(); [[maybe_unused]] double rms_error = optimizer.getSolution( From db4286b3c90ea5af1a7dee309d9a7cf2cb9497af Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Wed, 5 Feb 2025 17:30:16 +0900 Subject: [PATCH 09/15] feat(ceres_intrinsic_camera_calibrator): apply FOV regularization only if anomaly detected Signed-off-by: Amadeusz Szymko --- .../ceres_camera_intrinsics_optimizer.hpp | 26 ++- .../fov_residual.hpp | 176 ++++++++++++------ .../src/ceres_camera_intrinsics_optimizer.cpp | 139 +++++++------- .../ceres_intrinsic_camera_calibrator_py.cpp | 40 +++- .../src/main.cpp | 40 +++- 5 files changed, 289 insertions(+), 132 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp index 73b735e3..16735ce1 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp @@ -48,6 +48,10 @@ class CeresCameraIntrinsicsOptimizer static constexpr int RESIDUAL_DIM = 2; + static constexpr int SOLVE_MAX_ATTEMPTS = 5; + static constexpr double REPR_THR = 0.1; + static constexpr double FOV_THR = 1e-6; + /*! * Sets the number of radial distortion coefficients * @param[in] radial_distortion_coefficients number of radial distortion coefficients @@ -120,6 +124,18 @@ class CeresCameraIntrinsicsOptimizer cv::Mat_ & camera_matrix, cv::Mat_ & distortion_coeffs, std::vector & rvecs, std::vector & tvecs); + /*! + * Calculates the ceres total reprojection error + * @return the ceres total reprojection error + */ + double getTotalReprojectionError(); + + /*! + * Calculates the average total reprojection error + * @return the ceres average reprojection error + */ + double getAvgReprojectionError(); + /*! * Formats the input data into optimization placeholders */ @@ -137,14 +153,16 @@ class CeresCameraIntrinsicsOptimizer void evaluate(); /*! - * Formulates and solves the optimization problem + * Evaluates the current optimization variables with the ceres cost function for the field of view + * @return the Ceres total field of view error */ - void solve(); + double evaluateFov(); /*! - * Applies the optimization for the field of view + * Formulates and solves the optimization problem + * @param[in] use_fov_block whether or not to use the field of view regularization */ - void solveFov(); + void solve(bool use_fov_block = false); protected: int radial_distortion_coefficients_; diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp index f03e277c..f7cf591f 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp @@ -25,6 +25,17 @@ #include #include +template +struct CameraPoint +{ + T x; + T y; + T depth; + T sign_shift_x; + T sign_shift_y; + T backprojection_err; // cSpell:ignore backprojection +}; + struct FOVResidual { static constexpr int INTRINSICS_CX_INDEX = 0; @@ -38,13 +49,14 @@ struct FOVResidual FOVResidual( int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs, - int width, int height) + int width, int height, const std::vector> & camera_points) { radial_distortion_coeffs_ = radial_distortion_coeffs; use_tangential_distortion_ = use_tangential_distortion; rational_distortion_coeffs_ = rational_distortion_coeffs; width_ = width; height_ = height; + camera_points_ = camera_points; } /*! @@ -60,9 +72,6 @@ struct FOVResidual const T depth = T(1.0); const std::vector shifts = {T(0.01), T(0.03), T(0.05), T(0.1), T(0.3), T(0.5), T(1.0), T(3.0)}; - - const T width_t = T(width_); - const T height_t = T(height_); int distortion_index = 4; const T & cx = camera_intrinsics[INTRINSICS_CX_INDEX]; const T & cy = camera_intrinsics[INTRINSICS_CY_INDEX]; @@ -83,59 +92,23 @@ struct FOVResidual const T & k6 = rational_distortion_coeffs_ > 2 ? camera_intrinsics[distortion_index++] : null_value; - // cSpell:ignore backprojection - auto apply_residual = [this, residuals, shifts, width_t, height_t, cx, cy, fx, fy, k1, k2, k3, - p1, p2, k4, k5, k6, depth]( - const int & idx, const T & u, const T & v, - const T & backprojection_err_thr = T(10.0)) -> bool { - residuals[idx] = T(0.0); - auto [x, y] = imageToCamera(u, v, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth); - auto [u_bpr, v_bpr] = - cameraToImage(x, y, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth); - auto backprojection_err = ceres::sqrt(ceres::pow(u - u_bpr, 2) + ceres::pow(v - v_bpr, 2)); - if (ceres::IsNaN(backprojection_err) || backprojection_err > backprojection_err_thr) { - return false; - } - auto sign_shift_x = u <= T(0.0) ? T(-1.0) : u >= width_t - T(1.0) ? T(1.0) : T(0.0); - auto sign_shift_y = v <= T(0.0) ? T(-1.0) : v >= height_t - T(1.0) ? T(1.0) : T(0.0); - for (const auto & shift : shifts) { + if (RESIDUAL_DIM != shifts.size()) { + throw std::runtime_error("The number of residuals should match the number of shifts"); + } + std::fill(residuals, residuals + RESIDUAL_DIM, T(0.0)); + for (const auto & cp : camera_points_) { + for (std::size_t i = 0; i < shifts.size(); i++) { auto [u_shifted, v_shifted] = cameraToImage( - x + shift * sign_shift_x, y + shift * sign_shift_y, cx, cy, fx, fy, k1, k2, k3, p1, p2, - k4, k5, k6, depth); + cp.x + shifts[i] * cp.sign_shift_x, cp.y + shifts[i] * cp.sign_shift_y, cx, cy, fx, fy, + k1, k2, k3, p1, p2, k4, k5, k6, depth); auto residual = getFovResidual(u_shifted, v_shifted); // Weigh the residuals by the backprojection error - residuals[idx] += residual * (T(1.0) / (backprojection_err + T(1.0))); + auto residual_weighted = residual / (cp.backprojection_err + T(1.0)); + // Increase residual magnitude & soften + residual_weighted = ceres::log(T(1.0) + residual_weighted); + residuals[i] += residual_weighted; } - return true; - }; - - std::size_t valid_residuals = 0; - - // Middle top - valid_residuals += apply_residual(0, width_t / T(2.0) - T(1.0), T(0.0)); - - // Middle left - valid_residuals += apply_residual(1, T(0.0), height_t / T(2.0) - T(1.0)); - - // Middle bottom - valid_residuals += apply_residual(2, width_t / T(2.0) - T(1.0), height_t - T(1.0)); - - // Middle right - valid_residuals += apply_residual(3, width_t - T(1.0), height_t / T(2.0) - T(1.0)); - - // Top left - valid_residuals += apply_residual(4, T(0.0), T(0.0)); - - // Top right - valid_residuals += apply_residual(5, width_t - T(1.0), T(0.0)); - - // Bottom left - valid_residuals += apply_residual(6, T(0.0), height_t - T(1.0)); - - // Bottom right - valid_residuals += apply_residual(7, width_t - T(1.0), height_t - T(1.0)); - - LOG(INFO) << "Valid FOV residuals: " << valid_residuals << " / 8" << std::endl; + } return true; } @@ -180,9 +153,9 @@ struct FOVResidual * @returns The pixel x and y coordinates */ template - std::pair cameraToImage( + static std::pair cameraToImage( const T x, const T y, const T cx, const T cy, const T fx, const T fy, const T k1, const T k2, - const T k3, const T p1, const T p2, const T k4, const T k5, const T k6, T depth = T(1.0)) const + const T k3, const T p1, const T p2, const T k4, const T k5, const T k6, T depth = T(1.0)) { const T xp = x / depth; const T yp = y / depth; @@ -220,10 +193,10 @@ struct FOVResidual * @returns The real world x and y coordinates in camera frame */ template - std::pair imageToCamera( + static std::pair imageToCamera( const T u, const T v, const T cx, const T cy, const T fx, const T fy, const T k1, const T k2, const T k3, const T p1, const T p2, const T k4, const T k5, const T k6, T depth = T(1.0), - const T tol = T(1e-6)) const + const T tol = T(1e-6)) { T xp = (u - cx) / fx; T yp = (v - cy) / fy; @@ -269,11 +242,11 @@ struct FOVResidual */ static ceres::CostFunction * createResidual( int radial_distortion_coeffs, bool use_tangential_distortion, int rational_distortion_coeffs, - int width, int height) + int width, int height, const std::vector> & camera_points) { auto f = new FOVResidual( radial_distortion_coeffs, use_tangential_distortion, rational_distortion_coeffs, width, - height); + height, camera_points); int distortion_coefficients = radial_distortion_coeffs + 2 * static_cast(use_tangential_distortion) + @@ -320,6 +293,91 @@ struct FOVResidual int rational_distortion_coeffs_; int width_; int height_; + std::vector> camera_points_; }; +/*! + * Get points around image border represented in camera frame + * @param[in] camera_intrinsics The camera intrinsics + * @param[in] radial_distortion_coeffs The number of radial distortion coefficients + * @param[in] use_tangential_distortion Whether to use or not tangential distortion + * @param[in] rational_distortion_coeffs The number of rational distortion coefficients + * @param[in] width the source image width + * @param[in] height the source image height + * @returns The points in camera frame + */ +template +std::vector> getCameraPoints( + const T * const camera_intrinsics, int radial_distortion_coeffs, int use_tangential_distortion, + int rational_distortion_coeffs, const T width, const T height) +{ + const T null_value = T(0.0); + const T depth = T(1.0); + + int distortion_index = 0; + const T & cx = camera_intrinsics[distortion_index++]; + const T & cy = camera_intrinsics[distortion_index++]; + const T & fx = camera_intrinsics[distortion_index++]; + const T & fy = camera_intrinsics[distortion_index++]; + 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; + + auto getPoints = [width, height, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth]( + const T u, const T v, std::vector> & camera_points, + const T backprojection_err_thr = T(10.0)) -> bool { + auto [x, y] = + FOVResidual::imageToCamera(u, v, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth); + auto [u_bpr, v_bpr] = + FOVResidual::cameraToImage(x, y, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth); + auto backprojection_err = ceres::sqrt(ceres::pow(u - u_bpr, 2) + ceres::pow(v - v_bpr, 2)); + if (ceres::IsNaN(backprojection_err) || backprojection_err > backprojection_err_thr) { + return false; + } + auto sign_shift_x = u <= T(0.0) ? T(-1.0) : u >= width - T(1.0) ? T(1.0) : T(0.0); + auto sign_shift_y = v <= T(0.0) ? T(-1.0) : v >= height - T(1.0) ? T(1.0) : T(0.0); + camera_points.push_back({x, y, depth, sign_shift_x, sign_shift_y, backprojection_err}); + return true; + }; + + std::vector> camera_points; + std::size_t valid_points = 0; + + // Middle top + valid_points += getPoints(width / T(2.0) - T(1.0), T(0.0), camera_points); + + // Middle left + valid_points += getPoints(T(0.0), height / T(2.0) - T(1.0), camera_points); + + // Middle bottom + valid_points += getPoints(width / T(2.0) - T(1.0), height - T(1.0), camera_points); + + // Middle right + valid_points += getPoints(width - T(1.0), height / T(2.0) - T(1.0), camera_points); + + // Top left + valid_points += getPoints(T(0.0), T(0.0), camera_points); + + // Top right + valid_points += getPoints(width - T(1.0), T(0.0), camera_points); + + // Bottom left + valid_points += getPoints(T(0.0), height - T(1.0), camera_points); + + // Bottom right + valid_points += getPoints(width - T(1.0), height - T(1.0), camera_points); + + LOG(INFO) << "Valid camera points: " << valid_points << " / 8" << std::endl; + + return camera_points; +} + #endif // CERES_INTRINSIC_CAMERA_CALIBRATOR__FOV_RESIDUAL_HPP_ 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 411bf6c1..87873b1c 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 @@ -301,6 +301,35 @@ void CeresCameraIntrinsicsOptimizer::placeholdersToData() } } +double CeresCameraIntrinsicsOptimizer::getTotalReprojectionError() +{ + double total_ceres_error = 0; + + for (std::size_t view_index = 0; view_index < object_points_.size(); view_index++) { + const auto & view_object_points = object_points_[view_index]; + const auto & view_image_points = image_points_[view_index]; + auto & pose_placeholder = pose_placeholders_[view_index]; + + for (std::size_t point_index = 0; point_index < view_object_points.size(); point_index++) { + auto f = ReprojectionResidual( + view_object_points[point_index], view_image_points[point_index], + radial_distortion_coefficients_, use_tangential_distortion_, + rational_distortion_coefficients_); + std::array residuals; + f(intrinsics_placeholder_.data(), pose_placeholder.data(), residuals.data()); + total_ceres_error += residuals[0] * residuals[0] + residuals[1] * residuals[1]; + } + } + + return total_ceres_error; +} + +double CeresCameraIntrinsicsOptimizer::getAvgReprojectionError() +{ + double total_ceres_error = getTotalReprojectionError(); + return total_ceres_error / object_points_.size(); +} + void CeresCameraIntrinsicsOptimizer::evaluate() { // Start developing the ceres optimizer @@ -330,30 +359,36 @@ void CeresCameraIntrinsicsOptimizer::evaluate() printf("summary | calibration_error=%.3f\n", total_calibration_error / object_points_.size()); } - double total_ceres_error = 0; - - for (std::size_t view_index = 0; view_index < object_points_.size(); view_index++) { - const auto & view_object_points = object_points_[view_index]; - const auto & view_image_points = image_points_[view_index]; - auto & pose_placeholder = pose_placeholders_[view_index]; + double total_ceres_error = getTotalReprojectionError(); - for (std::size_t point_index = 0; point_index < view_object_points.size(); point_index++) { - auto f = ReprojectionResidual( - view_object_points[point_index], view_image_points[point_index], - radial_distortion_coefficients_, use_tangential_distortion_, - rational_distortion_coefficients_); - std::array residuals; - f(intrinsics_placeholder_.data(), pose_placeholder.data(), residuals.data()); - total_ceres_error += residuals[0] * residuals[0] + residuals[1] * residuals[1]; - } + if (verbose_) { + std::cout << "total_ceres_error: " << 0.5 * total_ceres_error << std::endl; } +} + +double CeresCameraIntrinsicsOptimizer::evaluateFov() +{ + double total_ceres_fov_error = 0; + auto camera_points = getCameraPoints( + intrinsics_placeholder_.data(), radial_distortion_coefficients_, use_tangential_distortion_, + rational_distortion_coefficients_, width_, height_); + + auto f = FOVResidual( + radial_distortion_coefficients_, use_tangential_distortion_, rational_distortion_coefficients_, + width_, height_, camera_points); + + std::array residuals; + f(intrinsics_placeholder_.data(), residuals.data()); + total_ceres_fov_error = std::accumulate(residuals.begin(), residuals.end(), 0.0); if (verbose_) { - std::cout << "total_ceres_error: " << 0.5 * total_ceres_error << std::endl; + std::cout << "total_ceres_fov_error: " << total_ceres_fov_error << std::endl; } + + return total_ceres_fov_error; } -void CeresCameraIntrinsicsOptimizer::solve() +void CeresCameraIntrinsicsOptimizer::solve(bool use_fov_block) { ceres::Problem problem; @@ -373,14 +408,28 @@ void CeresCameraIntrinsicsOptimizer::solve() } } - problem.AddResidualBlock( - DistortionCoefficientsResidual::createResidual( - radial_distortion_coefficients_, use_tangential_distortion_, - rational_distortion_coefficients_), - new ceres::ScaledLoss( - nullptr, coeffs_regularization_weight_ * object_points_.size(), - ceres::TAKE_OWNERSHIP), // L2 - intrinsics_placeholder_.data()); + if (use_fov_block) { + auto camera_points = getCameraPoints( + intrinsics_placeholder_.data(), radial_distortion_coefficients_, use_tangential_distortion_, + rational_distortion_coefficients_, width_, height_); + problem.AddResidualBlock( + FOVResidual::createResidual( + radial_distortion_coefficients_, use_tangential_distortion_, + rational_distortion_coefficients_, width_, height_, camera_points), + new ceres::ScaledLoss( + nullptr, fov_regularization_weight_ * 1e6 * object_points_.size(), + ceres::TAKE_OWNERSHIP), // L2 + intrinsics_placeholder_.data()); + } else { + problem.AddResidualBlock( + DistortionCoefficientsResidual::createResidual( + radial_distortion_coefficients_, use_tangential_distortion_, + rational_distortion_coefficients_), + new ceres::ScaledLoss( + nullptr, coeffs_regularization_weight_ * object_points_.size(), + ceres::TAKE_OWNERSHIP), // L2 + intrinsics_placeholder_.data()); + } double initial_cost = 0.0; std::vector residuals; @@ -390,7 +439,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; @@ -406,42 +455,6 @@ void CeresCameraIntrinsicsOptimizer::solve() ceres::Solve(options, &problem, &summary); if (verbose_) { - std::cout << "Report: " << summary.FullReport(); - } -} - -void CeresCameraIntrinsicsOptimizer::solveFov() -{ - ceres::Problem problem; - - problem.AddResidualBlock( - FOVResidual::createResidual( - radial_distortion_coefficients_, use_tangential_distortion_, - rational_distortion_coefficients_, width_, height_), - new ceres::ScaledLoss( - nullptr, fov_regularization_weight_ * object_points_.size(), ceres::TAKE_OWNERSHIP), // L2 - intrinsics_placeholder_.data()); - - double initial_cost = 0.0; - std::vector residuals; - ceres::Problem::EvaluateOptions eval_opt; - eval_opt.num_threads = 1; - problem.GetResidualBlocks(&eval_opt.residual_blocks); - problem.Evaluate(eval_opt, &initial_cost, &residuals, nullptr, nullptr); - - if (verbose_) { - std::cout << "[FOV] Initial cost: " << initial_cost << std::endl; + std::cout << "Report: " << summary.FullReport() << std::endl; } - - ceres::Solver::Options options; - options.linear_solver_type = ceres::DENSE_SCHUR; // cSpell:ignore SCHUR - options.minimizer_progress_to_stdout = verbose_; - options.max_num_iterations = 500; - options.function_tolerance = 1e-10; - options.gradient_tolerance = 1e-14; - options.num_threads = 8; - options.max_num_consecutive_invalid_steps = 1000; - options.use_inner_iterations = false; - ceres::Solver::Summary summary; - ceres::Solve(options, &problem, &summary); } diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp index 9eb66a1e..cccafa1f 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp @@ -137,11 +137,45 @@ calibrate( optimizer.dataToPlaceholders(); optimizer.evaluate(); optimizer.solve(); + optimizer.placeholdersToData(); + if (fov_regularization_weight > 0.0) { - optimizer.solveFov(); + auto init_avg_repr_error = optimizer.getAvgReprojectionError(); + auto best_fov_eval = optimizer.evaluateFov(); + + if (best_fov_eval > CeresCameraIntrinsicsOptimizer::FOV_THR) { + std::cout << "FOV anomaly detected..." << std::endl; + int ex_solve_attempt = 0; + while (true) { + if (ex_solve_attempt >= CeresCameraIntrinsicsOptimizer::SOLVE_MAX_ATTEMPTS) { + std::cout << "Max solve attempts reached. Failed to converge with FOV regularization." + << std::endl; + break; + } + if (ex_solve_attempt > 0) { + optimizer.solve(); + } + ex_solve_attempt++; + std::cout << "Retrying with FOV regularization, attempt " << ex_solve_attempt << "..." + << std::endl; + optimizer.solve(true); + auto adjustment = init_avg_repr_error - optimizer.getAvgReprojectionError(); + std::cout << "Ceres error adjustment: " << adjustment << std::endl; + auto fov_eval = optimizer.evaluateFov(); + + if (fov_eval < best_fov_eval && adjustment >= -CeresCameraIntrinsicsOptimizer::REPR_THR) { + std::cout << "Found better solution!" << std::endl; + best_fov_eval = fov_eval; + optimizer.placeholdersToData(); + if (best_fov_eval <= CeresCameraIntrinsicsOptimizer::FOV_THR) { + std::cout << "Converged!" << std::endl; + break; + } + } + } + } } - optimizer.placeholdersToData(); - optimizer.evaluate(); + double rms_error = optimizer.getSolution(camera_matrix_cv, dist_coeffs_cv, rvecs_cv, tvecs_cv); // Extract the results diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp index a13e4c33..b063ae63 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp @@ -270,11 +270,45 @@ int main(int argc, char ** argv) optimizer.dataToPlaceholders(); optimizer.evaluate(); optimizer.solve(); + optimizer.placeholdersToData(); + if (fov_regularization_weight > 0.0) { - optimizer.solveFov(); + auto init_avg_repr_error = optimizer.getAvgReprojectionError(); + auto best_fov_eval = optimizer.evaluateFov(); + + if (best_fov_eval > CeresCameraIntrinsicsOptimizer::FOV_THR) { + std::cout << "FOV anomaly detected..." << std::endl; + int ex_solve_attempt = 0; + while (true) { + if (ex_solve_attempt >= CeresCameraIntrinsicsOptimizer::SOLVE_MAX_ATTEMPTS) { + std::cout << "Max solve attempts reached. Failed to converge with FOV regularization." + << std::endl; + break; + } + if (ex_solve_attempt > 0) { + optimizer.solve(); + } + ex_solve_attempt++; + std::cout << "Retrying with FOV regularization, attempt " << ex_solve_attempt << "..." + << std::endl; + optimizer.solve(true); + auto adjustment = init_avg_repr_error - optimizer.getAvgReprojectionError(); + std::cout << "Ceres error adjustment: " << adjustment << std::endl; + auto fov_eval = optimizer.evaluateFov(); + + if (fov_eval < best_fov_eval && adjustment >= -CeresCameraIntrinsicsOptimizer::REPR_THR) { + std::cout << "Found better solution!" << std::endl; + best_fov_eval = fov_eval; + optimizer.placeholdersToData(); + if (best_fov_eval <= CeresCameraIntrinsicsOptimizer::FOV_THR) { + std::cout << "Converged!" << std::endl; + break; + } + } + } + } } - optimizer.placeholdersToData(); - optimizer.evaluate(); + [[maybe_unused]] double rms_error = optimizer.getSolution( ceres_camera_matrix, ceres_dist_coeffs, ceres_calibration_rvecs, ceres_calibration_tvecs); From 641011860db580367ebb9bb3836ba20376b26e06 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Fri, 7 Feb 2025 15:49:46 +0900 Subject: [PATCH 10/15] fix(ceres_intrinsic_camera_calibrator): function naming Signed-off-by: Amadeusz Szymko --- .../ceres_camera_intrinsics_optimizer.hpp | 12 ++++++------ .../src/ceres_camera_intrinsics_optimizer.cpp | 8 ++++---- .../src/ceres_intrinsic_camera_calibrator_py.cpp | 4 ++-- .../ceres_intrinsic_camera_calibrator/src/main.cpp | 4 ++-- 4 files changed, 14 insertions(+), 14 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp index 16735ce1..5265d6ab 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp @@ -125,16 +125,16 @@ class CeresCameraIntrinsicsOptimizer std::vector & rvecs, std::vector & tvecs); /*! - * Calculates the ceres total reprojection error - * @return the ceres total reprojection error + * Calculates the ceres total error + * @return the ceres total error */ - double getTotalReprojectionError(); + double getTotalCeresError(); /*! - * Calculates the average total reprojection error - * @return the ceres average reprojection error + * Calculates the average ceres error + * @return the ceres average error */ - double getAvgReprojectionError(); + double getAvgCeresError(); /*! * Formats the input data into optimization placeholders 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 c438e45d..9a8cff7b 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 @@ -301,7 +301,7 @@ void CeresCameraIntrinsicsOptimizer::placeholdersToData() } } -double CeresCameraIntrinsicsOptimizer::getTotalReprojectionError() +double CeresCameraIntrinsicsOptimizer::getTotalCeresError() { double total_ceres_error = 0; @@ -324,9 +324,9 @@ double CeresCameraIntrinsicsOptimizer::getTotalReprojectionError() return total_ceres_error; } -double CeresCameraIntrinsicsOptimizer::getAvgReprojectionError() +double CeresCameraIntrinsicsOptimizer::getAvgCeresError() { - double total_ceres_error = getTotalReprojectionError(); + double total_ceres_error = getTotalCeresError(); return total_ceres_error / object_points_.size(); } @@ -359,7 +359,7 @@ void CeresCameraIntrinsicsOptimizer::evaluate() printf("summary | calibration_error=%.3f\n", total_calibration_error / object_points_.size()); } - double total_ceres_error = getTotalReprojectionError(); + double total_ceres_error = getTotalCeresError(); if (verbose_) { std::cout << "total_ceres_error: " << 0.5 * total_ceres_error << std::endl; diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp index cccafa1f..1c98fc2d 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp @@ -140,7 +140,7 @@ calibrate( optimizer.placeholdersToData(); if (fov_regularization_weight > 0.0) { - auto init_avg_repr_error = optimizer.getAvgReprojectionError(); + auto init_avg_ceres_error = optimizer.getAvgCeresError(); auto best_fov_eval = optimizer.evaluateFov(); if (best_fov_eval > CeresCameraIntrinsicsOptimizer::FOV_THR) { @@ -159,7 +159,7 @@ calibrate( std::cout << "Retrying with FOV regularization, attempt " << ex_solve_attempt << "..." << std::endl; optimizer.solve(true); - auto adjustment = init_avg_repr_error - optimizer.getAvgReprojectionError(); + auto adjustment = init_avg_ceres_error - optimizer.getAvgCeresError(); std::cout << "Ceres error adjustment: " << adjustment << std::endl; auto fov_eval = optimizer.evaluateFov(); diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp index b063ae63..803e7d11 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp @@ -273,7 +273,7 @@ int main(int argc, char ** argv) optimizer.placeholdersToData(); if (fov_regularization_weight > 0.0) { - auto init_avg_repr_error = optimizer.getAvgReprojectionError(); + auto init_avg_ceres_error = optimizer.getAvgCeresError(); auto best_fov_eval = optimizer.evaluateFov(); if (best_fov_eval > CeresCameraIntrinsicsOptimizer::FOV_THR) { @@ -292,7 +292,7 @@ int main(int argc, char ** argv) std::cout << "Retrying with FOV regularization, attempt " << ex_solve_attempt << "..." << std::endl; optimizer.solve(true); - auto adjustment = init_avg_repr_error - optimizer.getAvgReprojectionError(); + auto adjustment = init_avg_ceres_error - optimizer.getAvgCeresError(); std::cout << "Ceres error adjustment: " << adjustment << std::endl; auto fov_eval = optimizer.evaluateFov(); From 59461cd085f653048a2017a41e0d620fa23b1ac2 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Fri, 7 Feb 2025 15:55:32 +0900 Subject: [PATCH 11/15] feat(ceres_intrinsic_camera_calibrator): improve logging Signed-off-by: Amadeusz Szymko --- .../src/ceres_intrinsic_camera_calibrator_py.cpp | 10 +++++++--- .../ceres_intrinsic_camera_calibrator/src/main.cpp | 10 +++++++--- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp index 1c98fc2d..d75e9a71 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp @@ -159,10 +159,14 @@ calibrate( std::cout << "Retrying with FOV regularization, attempt " << ex_solve_attempt << "..." << std::endl; optimizer.solve(true); - auto adjustment = init_avg_ceres_error - optimizer.getAvgCeresError(); - std::cout << "Ceres error adjustment: " << adjustment << std::endl; + auto avg_ceres_error = optimizer.getAvgCeresError(); + auto adjustment = init_avg_ceres_error - avg_ceres_error; + std::cout << "Average ceres error [init / updated | adjustment]: " << init_avg_ceres_error + << " / " << avg_ceres_error << " | " << adjustment << std::endl; auto fov_eval = optimizer.evaluateFov(); - + std::cout << "Ceres total field of view error [best / updated | adjustment]: " + << best_fov_eval << " / " << fov_eval << " | " << best_fov_eval - fov_eval + << std::endl; if (fov_eval < best_fov_eval && adjustment >= -CeresCameraIntrinsicsOptimizer::REPR_THR) { std::cout << "Found better solution!" << std::endl; best_fov_eval = fov_eval; diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp index 803e7d11..ed8d8d05 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp @@ -292,10 +292,14 @@ int main(int argc, char ** argv) std::cout << "Retrying with FOV regularization, attempt " << ex_solve_attempt << "..." << std::endl; optimizer.solve(true); - auto adjustment = init_avg_ceres_error - optimizer.getAvgCeresError(); - std::cout << "Ceres error adjustment: " << adjustment << std::endl; + auto avg_ceres_error = optimizer.getAvgCeresError(); + auto adjustment = init_avg_ceres_error - avg_ceres_error; + std::cout << "Average ceres error [init / updated | adjustment]: " << init_avg_ceres_error + << " / " << avg_ceres_error << " | " << adjustment << std::endl; auto fov_eval = optimizer.evaluateFov(); - + std::cout << "Ceres total field of view error [best / updated | adjustment]: " + << best_fov_eval << " / " << fov_eval << " | " << best_fov_eval - fov_eval + << std::endl; if (fov_eval < best_fov_eval && adjustment >= -CeresCameraIntrinsicsOptimizer::REPR_THR) { std::cout << "Found better solution!" << std::endl; best_fov_eval = fov_eval; From 48111de65401152f48dacbf7f4d1c73275622e69 Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Fri, 7 Feb 2025 16:08:53 +0900 Subject: [PATCH 12/15] style(ceres_intrinsic_camera_calibrator): pass args Signed-off-by: Amadeusz Szymko --- .../src/ceres_intrinsic_camera_calibrator_py.cpp | 4 ++-- .../ceres_intrinsic_camera_calibrator/src/main.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp index d75e9a71..7b15a953 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp @@ -136,7 +136,7 @@ calibrate( initial_rvecs_cv, initial_tvecs_cv); optimizer.dataToPlaceholders(); optimizer.evaluate(); - optimizer.solve(); + optimizer.solve(false); optimizer.placeholdersToData(); if (fov_regularization_weight > 0.0) { @@ -153,7 +153,7 @@ calibrate( break; } if (ex_solve_attempt > 0) { - optimizer.solve(); + optimizer.solve(false); } ex_solve_attempt++; std::cout << "Retrying with FOV regularization, attempt " << ex_solve_attempt << "..." diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp index ed8d8d05..4f91eda1 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp @@ -269,7 +269,7 @@ int main(int argc, char ** argv) calibration_image_points, mini_opencv_calibration_rvecs, mini_opencv_calibration_tvecs); optimizer.dataToPlaceholders(); optimizer.evaluate(); - optimizer.solve(); + optimizer.solve(false); optimizer.placeholdersToData(); if (fov_regularization_weight > 0.0) { @@ -286,7 +286,7 @@ int main(int argc, char ** argv) break; } if (ex_solve_attempt > 0) { - optimizer.solve(); + optimizer.solve(false); } ex_solve_attempt++; std::cout << "Retrying with FOV regularization, attempt " << ex_solve_attempt << "..." From cd41b4c03d7d114708452b9c5c2a459171dd425a Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Thu, 13 Feb 2025 16:31:34 +0900 Subject: [PATCH 13/15] fix(ceres_intrinsic_camera_calibrator): use ceres min and max Signed-off-by: Amadeusz Szymko --- .../ceres_intrinsic_camera_calibrator/fov_residual.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp index f7cf591f..26b38413 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp @@ -126,9 +126,9 @@ struct FOVResidual T height_t = T(height_); if (u >= T(0.0) && u <= width_t - T(1.0) && v >= T(0.0) && v <= height_t - T(1.0)) { - T closest_u = std::min(u, width_t - u - T(1.0)) / (std::max(height_t, width_t) - T(1.0)); - T closest_v = std::min(v, height_t - v - T(1.0)) / (std::max(height_t, width_t) - T(1.0)); - return std::min(closest_u, closest_v); + T closest_u = ceres::fmin(u, width_t - u - T(1.0)) / (ceres::fmax(height_t, width_t) - T(1.0)); + T closest_v = ceres::fmin(v, height_t - v - T(1.0)) / (ceres::fmax(height_t, width_t) - T(1.0)); + return ceres::fmin(closest_u, closest_v); } return T(0.0); } From 79a6f232b9e13a2aedc9dfd15527d49670c28346 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 13 Feb 2025 07:33:22 +0000 Subject: [PATCH 14/15] ci(pre-commit): autofix --- .../ceres_intrinsic_camera_calibrator/fov_residual.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp index 26b38413..c1a3885c 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp @@ -126,8 +126,10 @@ struct FOVResidual T height_t = T(height_); if (u >= T(0.0) && u <= width_t - T(1.0) && v >= T(0.0) && v <= height_t - T(1.0)) { - T closest_u = ceres::fmin(u, width_t - u - T(1.0)) / (ceres::fmax(height_t, width_t) - T(1.0)); - T closest_v = ceres::fmin(v, height_t - v - T(1.0)) / (ceres::fmax(height_t, width_t) - T(1.0)); + T closest_u = + ceres::fmin(u, width_t - u - T(1.0)) / (ceres::fmax(height_t, width_t) - T(1.0)); + T closest_v = + ceres::fmin(v, height_t - v - T(1.0)) / (ceres::fmax(height_t, width_t) - T(1.0)); return ceres::fmin(closest_u, closest_v); } return T(0.0); From 844ae1997ac4f44985f577907da97265a7792e1b Mon Sep 17 00:00:00 2001 From: Amadeusz Szymko Date: Thu, 13 Feb 2025 16:39:26 +0900 Subject: [PATCH 15/15] style(ceres_intrinsic_camera_calibrator): var naming for array indices Signed-off-by: Amadeusz Szymko --- .../fov_residual.hpp | 58 +++++++++---------- 1 file changed, 26 insertions(+), 32 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp index c1a3885c..9c8b98ff 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/fov_residual.hpp @@ -38,13 +38,7 @@ struct CameraPoint struct FOVResidual { - static constexpr int INTRINSICS_CX_INDEX = 0; - static constexpr int INTRINSICS_CY_INDEX = 1; - static constexpr int INTRINSICS_FX_INDEX = 2; - static constexpr int INTRINSICS_FY_INDEX = 3; - static constexpr int RESIDUAL_DIM = 8; - static constexpr int UNDIST_ITERS = 100; // cSpell:ignore UNDIST FOVResidual( @@ -72,25 +66,25 @@ struct FOVResidual const T depth = T(1.0); const std::vector shifts = {T(0.01), T(0.03), T(0.05), T(0.1), T(0.3), T(0.5), T(1.0), T(3.0)}; - int distortion_index = 4; - const T & cx = camera_intrinsics[INTRINSICS_CX_INDEX]; - const T & cy = camera_intrinsics[INTRINSICS_CY_INDEX]; - const T & fx = camera_intrinsics[INTRINSICS_FX_INDEX]; - const T & fy = camera_intrinsics[INTRINSICS_FY_INDEX]; + int intrinsics_index = 0; + const T & cx = camera_intrinsics[intrinsics_index++]; + const T & cy = camera_intrinsics[intrinsics_index++]; + const T & fx = camera_intrinsics[intrinsics_index++]; + const T & fy = camera_intrinsics[intrinsics_index++]; const T & k1 = - radial_distortion_coeffs_ > 0 ? camera_intrinsics[distortion_index++] : null_value; + radial_distortion_coeffs_ > 0 ? camera_intrinsics[intrinsics_index++] : null_value; const T & k2 = - radial_distortion_coeffs_ > 1 ? camera_intrinsics[distortion_index++] : null_value; + radial_distortion_coeffs_ > 1 ? camera_intrinsics[intrinsics_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; + radial_distortion_coeffs_ > 2 ? camera_intrinsics[intrinsics_index++] : null_value; + const T & p1 = use_tangential_distortion_ ? camera_intrinsics[intrinsics_index++] : null_value; + const T & p2 = use_tangential_distortion_ ? camera_intrinsics[intrinsics_index++] : null_value; const T & k4 = - rational_distortion_coeffs_ > 0 ? camera_intrinsics[distortion_index++] : null_value; + rational_distortion_coeffs_ > 0 ? camera_intrinsics[intrinsics_index++] : null_value; const T & k5 = - rational_distortion_coeffs_ > 1 ? camera_intrinsics[distortion_index++] : null_value; + rational_distortion_coeffs_ > 1 ? camera_intrinsics[intrinsics_index++] : null_value; const T & k6 = - rational_distortion_coeffs_ > 2 ? camera_intrinsics[distortion_index++] : null_value; + rational_distortion_coeffs_ > 2 ? camera_intrinsics[intrinsics_index++] : null_value; if (RESIDUAL_DIM != shifts.size()) { throw std::runtime_error("The number of residuals should match the number of shifts"); @@ -316,22 +310,22 @@ std::vector> getCameraPoints( const T null_value = T(0.0); const T depth = T(1.0); - int distortion_index = 0; - const T & cx = camera_intrinsics[distortion_index++]; - const T & cy = camera_intrinsics[distortion_index++]; - const T & fx = camera_intrinsics[distortion_index++]; - const T & fy = camera_intrinsics[distortion_index++]; - 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; + int intrinsics_index = 0; + const T & cx = camera_intrinsics[intrinsics_index++]; + const T & cy = camera_intrinsics[intrinsics_index++]; + const T & fx = camera_intrinsics[intrinsics_index++]; + const T & fy = camera_intrinsics[intrinsics_index++]; + const T & k1 = radial_distortion_coeffs > 0 ? camera_intrinsics[intrinsics_index++] : null_value; + const T & k2 = radial_distortion_coeffs > 1 ? camera_intrinsics[intrinsics_index++] : null_value; + const T & k3 = radial_distortion_coeffs > 2 ? camera_intrinsics[intrinsics_index++] : null_value; + const T & p1 = use_tangential_distortion ? camera_intrinsics[intrinsics_index++] : null_value; + const T & p2 = use_tangential_distortion ? camera_intrinsics[intrinsics_index++] : null_value; const T & k4 = - rational_distortion_coeffs > 0 ? camera_intrinsics[distortion_index++] : null_value; + rational_distortion_coeffs > 0 ? camera_intrinsics[intrinsics_index++] : null_value; const T & k5 = - rational_distortion_coeffs > 1 ? camera_intrinsics[distortion_index++] : null_value; + rational_distortion_coeffs > 1 ? camera_intrinsics[intrinsics_index++] : null_value; const T & k6 = - rational_distortion_coeffs > 2 ? camera_intrinsics[distortion_index++] : null_value; + rational_distortion_coeffs > 2 ? camera_intrinsics[intrinsics_index++] : null_value; auto getPoints = [width, height, cx, cy, fx, fy, k1, k2, k3, p1, p2, k4, k5, k6, depth]( const T u, const T v, std::vector> & camera_points,