From 96425c4437c211dd8acce83bba45cef3b4c4644e Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 1 Nov 2023 16:29:03 +0900 Subject: [PATCH 01/16] Implemented a ceres-based camera intrinsics calibrator. Numerical results seem to be the same as opencv but it is way faster when using a large number of calibration images Signed-off-by: Kenzo Lobos-Tsunekawa --- .../CMakeLists.txt | 99 ++++++ .../__init__.py | 0 .../ceres_camera_intrinsics_optimizer.hpp | 134 ++++++++ .../reprojection_residual.hpp | 163 +++++++++ .../package.xml | 44 +++ .../src/ceres_camera_intrinsics_optimizer.cpp | 309 +++++++++++++++++ .../ceres_intrinsic_camera_calibrator_py.cpp | 188 +++++++++++ .../src/main.cpp | 314 ++++++++++++++++++ 8 files changed, 1251 insertions(+) create mode 100644 sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt create mode 100644 sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/__init__.py create mode 100644 sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp create mode 100644 sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp create mode 100644 sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/package.xml create mode 100644 sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp create mode 100644 sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp create mode 100644 sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt new file mode 100644 index 00000000..5da4a003 --- /dev/null +++ b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt @@ -0,0 +1,99 @@ + +cmake_minimum_required(VERSION 3.5) +project(ceres_intrinsic_camera_calibrator) + +find_package(rclcpp REQUIRED) +find_package(rclpy REQUIRED) +find_package(apriltag REQUIRED) +find_package(autoware_cmake REQUIRED) +find_package(OpenCV REQUIRED) +find_package(Ceres REQUIRED) + +# Find python before pybind11 +find_package(Python3 REQUIRED COMPONENTS Interpreter Development) + +find_package(pybind11_vendor REQUIRED) +find_package(pybind11 REQUIRED) + +#add_subdirectory(pybind11) + +autoware_package() + +# These need to be called after autoware_package to avoid being overwritten +find_package(Boost REQUIRED COMPONENTS system serialization filesystem) + +# Optimizer as a library +ament_auto_add_library(${PROJECT_NAME} SHARED + src/ceres_camera_intrinsics_optimizer.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${Boost_LIBRARIES} + ${OpenCV_LIBS} + ${CERES_LIBRARIES} +) + +target_include_directories(${PROJECT_NAME} + PUBLIC + include + ${OpenCV_INCLUDE_DIRS}) + +ament_export_include_directories( + include + ${OpenCV_INCLUDE_DIRS} + ) + +# Test + +# COMPILE THE SOURCE +#======================================================================== +ament_auto_add_executable(${PROJECT_NAME}_test + src/main.cpp +) + +target_link_libraries(${PROJECT_NAME}_test + ${Boost_LIBRARIES} + ${OpenCV_LIBS} + ${CERES_LIBRARIES} + ${PROJECT_NAME} +) + +ament_python_install_package(${PROJECT_NAME}) + +#ament_python_install_package(${PROJECT_NAME}_py) + +pybind11_add_module(${PROJECT_NAME}_py src/ceres_intrinsic_camera_calibrator_py.cpp) + +target_include_directories(${PROJECT_NAME}_py PRIVATE include ${OpenCV_INCLUDE_DIRS}) + +target_link_libraries(${PROJECT_NAME}_py PRIVATE + ${OpenCV_LIBS} + ${CERES_LIBRARIES} + ${PROJECT_NAME} +) + +# EXAMPLE_VERSION_INFO is defined by setup.py and passed into the C++ code as a +# define (VERSION_INFO) here. +target_compile_definitions(${PROJECT_NAME}_py + PRIVATE VERSION_INFO=${EXAMPLE_VERSION_INFO}) + + +message("====PYTHON_INSTALL_DIR=${PYTHON_INSTALL_DIR}") + +install( + TARGETS ${PROJECT_NAME}_py + #DESTINATION "${PYTHON_INSTALL_DIR}" + DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" +) + +#install(PROGRAMS +# scripts/calibrator_ui_node.py +# DESTINATION lib/${PROJECT_NAME} +#) + +ament_export_dependencies(ament_cmake_python) + +#ament_auto_package( +# INSTALL_TO_SHARE +# launch +#) diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/__init__.py b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp new file mode 100644 index 00000000..1fc2268e --- /dev/null +++ b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp @@ -0,0 +1,134 @@ +// Copyright 2023 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__CERES_CAMERA_INTRINSICS_OPTIMIZER_HPP_ +#define CERES_INTRINSIC_CAMERA_CALIBRATOR__CERES_CAMERA_INTRINSICS_OPTIMIZER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +class CeresCameraIntrinsicsOptimizer +{ +public: + static constexpr int POSE_OPT_DIM = 7; + static constexpr int INTRINSICS_DIM = 9; + + static constexpr int ROTATION_W_INDEX = 0; + static constexpr int ROTATION_X_INDEX = 1; + static constexpr int ROTATION_Y_INDEX = 2; + static constexpr int ROTATION_Z_INDEX = 3; + static constexpr int TRANSLATION_X_INDEX = 4; + static constexpr int TRANSLATION_Y_INDEX = 5; + static constexpr int TRANSLATION_Z_INDEX = 6; + + 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 = 2; + + /*! + * Sets the number of radial distortion coefficients + * @param[in] radial_distortion_coefficients number of radial distortion coefficients + * optimized + */ + void setRadialDistortionCoefficients(int radial_distortion_coefficients); + + /*! + * Sets the use of tangential distortion + * @param[in] use_tangential_distortion whether or not to use tangential distortion + */ + void setTangentialDistortion(bool use_tangential_distortion); + + /*! + * Sets the verbose mode + * @param[in] verbose whether or not to use tangential distortion + */ + void setVerbose(bool verbose); + + /*! + * Sets the calibration data + * @param[in] camera_matrix the initial 3x3 camera matrix + * @param[in] distortion_coeffs the initial 5d distortion coefficients in the opencv formulation + * @param[in] object_points the calibration object points + * @param[in] image_points the calibration image points + * @param[in] rvecs the calibration boards initial poses + * @param[in] tvecs the calibration boards initial poses + */ + void setData( + const cv::Mat_ & camera_matrix, const cv::Mat_ & distortion_coeffs, + const std::vector> & object_points, + const std::vector> & image_points, const std::vector & rvecs, + const std::vector & tvecs); + + /*! + * Extract the solution from the optimization + * @param[in] camera_matrix the optimized 3x3 camera matrix + * @param[in] distortion_coeffs the optimized 5d distortion coefficients in the opencv formulation + * @param[in] rvecs the calibration boards optimized poses + * @param[in] tvecs the calibration boards optimized poses + * @return the reprojection RMS error as in the opencv formulation + */ + double getSolution( + cv::Mat_ & camera_matrix, cv::Mat_ & distortion_coeffs, + std::vector & rvecs, std::vector & tvecs); + + /*! + * Formats the input data into optimization placeholders + */ + void dataToPlaceholders(); + + /*! + * Extracts the information from the optimization placeholders and formats it into the calibration + * data structure + */ + void placeholdersToData(); + + /*! + * Evaluates the current optimization variables with the ceres cost function + */ + void evaluate(); + + /*! + * Formulates and solves the optimization problem + */ + void solve(); + +protected: + int radial_distortion_coefficients_; + bool use_tangential_distortion_; + bool verbose_; + cv::Mat_ camera_matrix_; + cv::Mat_ distortion_coeffs_; + + std::vector> object_points_; + std::vector> image_points_; + std::vector rvecs_, tvecs_; + + // Optimization placeholders + std::vector> pose_placeholders_; + std::array intrinsics_placeholder_; +}; + +#endif // CERES_INTRINSIC_CAMERA_CALIBRATOR__CERES_CAMERA_INTRINSICS_OPTIMIZER_HPP_ diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp new file mode 100644 index 00000000..9f2226cf --- /dev/null +++ b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp @@ -0,0 +1,163 @@ +// Copyright 2023 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__REPROJECTION_RESIDUAL_HPP_ +#define CERES_INTRINSIC_CAMERA_CALIBRATOR__REPROJECTION_RESIDUAL_HPP_ + +#include +#include + +#include +#include +#include + +struct ReprojectionResidual +{ + template + using Vector3 = Eigen::Matrix; + + static constexpr int POSE_OPT_DIM = 7; + + static constexpr int ROTATION_W_INDEX = 0; + static constexpr int ROTATION_X_INDEX = 1; + static constexpr int ROTATION_Y_INDEX = 2; + static constexpr int ROTATION_Z_INDEX = 3; + static constexpr int TRANSLATION_X_INDEX = 4; + static constexpr int TRANSLATION_Y_INDEX = 5; + static constexpr int TRANSLATION_Z_INDEX = 6; + + 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 = 2; + + ReprojectionResidual( + const cv::Point3f & object_point, const cv::Point2f & image_point, int radial_distortion_coeffs, + bool use_tangential_distortion) + { + object_point_ = Eigen::Vector3d(object_point.x, object_point.y, object_point.z); + image_point_ = Eigen::Vector2d(image_point.x, image_point.y); + radial_distortion_coeffs_ = radial_distortion_coeffs; + use_tangential_distortion_ = use_tangential_distortion; + } + + /*! + * The cost function representing the reprojection error + * @param[in] camera_intrinsics The camera intrinsics + * @param[in] board_pose The pose from ground plane to the tag (only used when using ground tags) + * @param[in] residuals The residual error of projecting the tag into the camera + * @returns success status + */ + template + bool operator()( + const T * const camera_intrinsics, const T * const board_pose, T * residuals) const + { + const Eigen::Map> board_translation(&board_pose[TRANSLATION_X_INDEX]); + + Eigen::Quaternion board_quaternion = { + board_pose[ROTATION_W_INDEX], board_pose[ROTATION_X_INDEX], board_pose[ROTATION_Y_INDEX], + board_pose[ROTATION_Z_INDEX]}; + + board_quaternion = board_quaternion.normalized(); + + Vector3 object_point_ccs = board_quaternion * (T(1.0) * object_point_) + board_translation; + + const T null_value = T(0.0); + 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[4] : null_value; + const T & k2 = radial_distortion_coeffs_ > 1 ? camera_intrinsics[5] : null_value; + const T & k3 = radial_distortion_coeffs_ > 2 ? camera_intrinsics[6] : null_value; + const T & p1 = + use_tangential_distortion_ ? camera_intrinsics[4 + radial_distortion_coeffs_] : null_value; + const T & p2 = + use_tangential_distortion_ ? camera_intrinsics[5 + radial_distortion_coeffs_] : null_value; + + const T xp = object_point_ccs.x() / object_point_ccs.z(); + const T yp = object_point_ccs.y() / object_point_ccs.z(); + const T r2 = xp * xp + yp * yp; + const T d = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + 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_ics_x = cx + fx * (xp * d + tdx); + const T predicted_ics_y = cy + fy * (yp * d + tdy); + + residuals[0] = predicted_ics_x - image_point_.x(); + residuals[1] = predicted_ics_y - image_point_.y(); + + return true; + } + + /*! + * Residual factory method + * @param[in] object_point The object point + * @param[in] image_point The image point + * @param[in] radial_distortion_coeffs The number of radial distortion coefficients + * @param[in] use_tangential_distortion Whether to use or not tangenrial distortion + * @returns the ceres residual + */ + static ceres::CostFunction * createResidual( + const cv::Point3f & object_point, const cv::Point2f & image_point, int radial_distortion_coeffs, + bool use_tangential_distortion) + { + auto f = new ReprojectionResidual( + object_point, image_point, radial_distortion_coeffs, use_tangential_distortion); + + int distortion_coefficients = + radial_distortion_coeffs + 2 * static_cast(use_tangential_distortion); + 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; + } + + return cost_function; + } + + Eigen::Vector3d object_point_; + Eigen::Vector2d image_point_; + int radial_distortion_coeffs_; + bool use_tangential_distortion_; +}; + +#endif // CERES_INTRINSIC_CAMERA_CALIBRATOR__REPROJECTION_RESIDUAL_HPP_ diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/package.xml b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/package.xml new file mode 100644 index 00000000..27d0a9bd --- /dev/null +++ b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/package.xml @@ -0,0 +1,44 @@ + + + + ceres_intrinsic_camera_calibrator + 0.0.1 + The ceres_intrinsic_camera_calibrator package + Kenzo Lobos Tsunekawa + + BSD + + ament_cmake_auto + ament_cmake_python + + autoware_cmake + + cv_bridge + eigen + geometry_msgs + image_geometry + image_transport + libboost-filesystem + libboost-serialization + libceres-dev + libgoogle-glog-dev + lidartag_msgs + pybind11-dev + rclcpp + sensor_msgs + std_msgs + std_srvs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + tier4_autoware_utils + tier4_calibration_msgs + tier4_tag_utils + visualization_msgs + + + + ament_cmake + + diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp new file mode 100644 index 00000000..fb3e6a0f --- /dev/null +++ b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp @@ -0,0 +1,309 @@ +// Copyright 2023 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. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +void CeresCameraIntrinsicsOptimizer::setRadialDistortionCoefficients( + int radial_distortion_coefficients) +{ + radial_distortion_coefficients_ = radial_distortion_coefficients; +} + +void CeresCameraIntrinsicsOptimizer::setTangentialDistortion(bool use_tangential_distortion) +{ + use_tangential_distortion_ = use_tangential_distortion; +} + +void CeresCameraIntrinsicsOptimizer::setVerbose(bool verbose) { verbose_ = verbose; } + +void CeresCameraIntrinsicsOptimizer::setData( + const cv::Mat_ & camera_matrix, const cv::Mat_ & distortion_coeffs, + const std::vector> & object_points, + const std::vector> & image_points, const std::vector & rvecs, + const std::vector & tvecs) +{ + camera_matrix_ = camera_matrix.clone(); + distortion_coeffs_ = distortion_coeffs.clone(); + object_points_ = object_points; + image_points_ = image_points; + rvecs_ = rvecs; + tvecs_ = tvecs; +} + +double CeresCameraIntrinsicsOptimizer::getSolution( + cv::Mat_ & camera_matrix, cv::Mat_ & distortion_coeffs, + std::vector & rvecs, std::vector & tvecs) +{ + camera_matrix = camera_matrix_; + distortion_coeffs = distortion_coeffs_; + rvecs = rvecs_; + tvecs = tvecs_; + + double total_error = 0.0; + std::size_t total_points = 0; + + auto get_reprojection_error = [](auto & image_points, auto & projected_points) -> double { + cv::Mat x = cv::Mat(2 * image_points.size(), 1, CV_32F, image_points.data()); + cv::Mat y = cv::Mat(2 * projected_points.size(), 1, CV_32F, projected_points.data()); + double total_error = cv::norm(x - y, cv::NORM_L2SQR); + return total_error; + }; + + for (std::size_t i = 0; i < object_points_.size(); i++) { + std::vector projected_points; + total_points += object_points_[i].size(); + + cv::projectPoints( + object_points_[i], rvecs[i], tvecs[i], camera_matrix, distortion_coeffs, projected_points); + + total_error += get_reprojection_error(projected_points, image_points_[i]); + } + + double rms_error = std::sqrt(total_error / total_points); + return rms_error; +} + +void CeresCameraIntrinsicsOptimizer::dataToPlaceholders() +{ + // Convert the intrinsics + intrinsics_placeholder_[INTRINSICS_CX_INDEX] = camera_matrix_(0, 2); + intrinsics_placeholder_[INTRINSICS_CY_INDEX] = camera_matrix_(1, 2); + intrinsics_placeholder_[INTRINSICS_FX_INDEX] = camera_matrix_(0, 0); + intrinsics_placeholder_[INTRINSICS_FY_INDEX] = camera_matrix_(1, 1); + + double k1 = distortion_coeffs_(0); + double k2 = distortion_coeffs_(1); + double p1 = distortion_coeffs_(2); + double p2 = distortion_coeffs_(3); + double k3 = distortion_coeffs_(4); + + int index = 4; + + if (radial_distortion_coefficients_ > 0) { + intrinsics_placeholder_[index++] = k1; + } + if (radial_distortion_coefficients_ > 1) { + intrinsics_placeholder_[index++] = k2; + } + if (radial_distortion_coefficients_ > 2) { + intrinsics_placeholder_[index++] = k3; + } + if (use_tangential_distortion_) { + intrinsics_placeholder_[index++] = p1; + intrinsics_placeholder_[index++] = p2; + } + + // Convert the revcs, tvecs into the placeholders + pose_placeholders_.resize(object_points_.size()); + + for (std::size_t i = 0; i < object_points_.size(); i++) { + cv::Mat rmat; + cv::Rodrigues(rvecs_[i], rmat); + + Eigen::Vector3d translation; + Eigen::Matrix3d rotation; + cv::cv2eigen(tvecs_[i], translation); + cv::cv2eigen(rmat, rotation); + Eigen::Quaterniond quat(rotation); + + std::array & placeholder = pose_placeholders_[i]; + placeholder[ROTATION_W_INDEX] = quat.w(); + placeholder[ROTATION_X_INDEX] = quat.x(); + placeholder[ROTATION_Y_INDEX] = quat.y(); + placeholder[ROTATION_Z_INDEX] = quat.z(); + placeholder[TRANSLATION_X_INDEX] = translation.x(); + placeholder[TRANSLATION_Y_INDEX] = translation.y(); + placeholder[TRANSLATION_Z_INDEX] = translation.z(); + } +} + +void CeresCameraIntrinsicsOptimizer::placeholdersToData() +{ + // Convert the intrinsics + camera_matrix_(0, 2) = intrinsics_placeholder_[INTRINSICS_CX_INDEX]; + camera_matrix_(1, 2) = intrinsics_placeholder_[INTRINSICS_CY_INDEX]; + camera_matrix_(0, 0) = intrinsics_placeholder_[INTRINSICS_FX_INDEX]; + camera_matrix_(1, 1) = intrinsics_placeholder_[INTRINSICS_FY_INDEX]; + + distortion_coeffs_ = cv::Mat_::zeros(5, 1); + double & k1 = distortion_coeffs_(0); + double & k2 = distortion_coeffs_(1); + double & p1 = distortion_coeffs_(2); + double & p2 = distortion_coeffs_(3); + double & k3 = distortion_coeffs_(4); + + int index = 4; + + if (radial_distortion_coefficients_ > 0) { + k1 = intrinsics_placeholder_[index++]; + } + if (radial_distortion_coefficients_ > 1) { + k2 = intrinsics_placeholder_[index++]; + } + if (radial_distortion_coefficients_ > 2) { + k3 = intrinsics_placeholder_[index++]; + } + if (use_tangential_distortion_) { + p1 = intrinsics_placeholder_[index++]; + p2 = intrinsics_placeholder_[index++]; + } + + // Convert the revcs, tvecs into the placeholders + rvecs_.resize(pose_placeholders_.size()); + tvecs_.resize(pose_placeholders_.size()); + + for (std::size_t i = 0; i < pose_placeholders_.size(); i++) { + const auto & placeholder = pose_placeholders_[i]; + const double scale = 1.0 / std::sqrt( + placeholder[0] * placeholder[0] + placeholder[1] * placeholder[1] + + placeholder[2] * placeholder[2] + placeholder[3] * placeholder[3]); + + Eigen::Quaterniond quat = Eigen::Quaterniond( + scale * placeholder[ROTATION_W_INDEX], scale * placeholder[ROTATION_X_INDEX], + scale * placeholder[ROTATION_Y_INDEX], scale * placeholder[ROTATION_Z_INDEX]); + + Eigen::Vector3d translation = Eigen::Vector3d( + placeholder[TRANSLATION_X_INDEX], placeholder[TRANSLATION_Y_INDEX], + placeholder[TRANSLATION_Z_INDEX]); + + Eigen::Matrix3d rotation = quat.toRotationMatrix(); + + cv::Matx33d cv_rot; + cv::Matx31d cv_transl; + cv::eigen2cv(translation, cv_transl); + cv::eigen2cv(rotation, cv_rot); + + cv::Mat rvec, tvec; + tvec = cv::Mat(cv_transl); + + cv::Rodrigues(cv_rot, rvec); + + rvecs_[i] = rvec; + tvecs_[i] = tvec; + } +} + +void CeresCameraIntrinsicsOptimizer::evaluate() +{ + // Start developing the ceres optimizer + double total_calibration_error = 0.0; + auto get_reprojection_error = + [](const auto & image_points, const auto & projected_points) -> double { + double total_error = 0; + for (size_t i = 0; i < projected_points.size(); i++) { + double error = cv::norm(image_points[i] - projected_points[i]); + total_error += error * error; + } + return std::sqrt(total_error / projected_points.size()); + }; + + for (std::size_t i = 0; i < object_points_.size(); i++) { + std::vector projected_points; + cv::projectPoints( + object_points_[i], rvecs_[i], tvecs_[i], camera_matrix_, distortion_coeffs_, + projected_points); + + double calibration_error = get_reprojection_error(projected_points, image_points_[i]); + + total_calibration_error += calibration_error; + } + + if (verbose_) { + 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]; + + 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_); + 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; + } +} + +void CeresCameraIntrinsicsOptimizer::solve() +{ + ceres::Problem problem; + + 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++) { + problem.AddResidualBlock( + ReprojectionResidual::createResidual( + view_object_points[point_index], view_image_points[point_index], + radial_distortion_coefficients_, use_tangential_distortion_), + nullptr, // L2 + intrinsics_placeholder_.data(), pose_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 << "Initial cost: " << initial_cost; + } + + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_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.use_inner_iterations = true; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + if (verbose_) { + std::cout << "Report: " << summary.FullReport(); + } +} diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp new file mode 100644 index 00000000..24a3c023 --- /dev/null +++ b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp @@ -0,0 +1,188 @@ +// Copyright 2023 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. + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#define STRINGIFY(x) #x +#define MACRO_STRINGIFY(x) STRINGIFY(x) + +int add(int i, int j) { return i + j; } + +std::tuple test(const Eigen::MatrixXd & matrix) +{ + return std::make_tuple(matrix.determinant(), matrix.sum()); +} + +std::tuple< + double, Eigen::MatrixXd, Eigen::MatrixXd, std::vector, + std::vector> +calibrate( + const std::vector & object_points_eigen_list, + 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, + bool use_tangential_distortion, bool verbose) +{ + (void)num_radial_coeffs; + (void)use_tangential_distortion; + (void)verbose; + + if ( + 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 || + std::min(initial_dist_coeffs_eigen.rows(), initial_dist_coeffs_eigen.cols() > 1) || + (initial_dist_coeffs_eigen.rows() * initial_dist_coeffs_eigen.cols()) != 5) { + std::cout << "Invalid parameters" << std::endl; + return std::tuple< + double, Eigen::MatrixXd, Eigen::MatrixXd, std::vector, + std::vector>(); + } + + // Convert all the data to formats we are used to + cv::Mat_ initial_camera_matrix_cv = cv::Mat_::zeros(3, 3); + cv::Mat_ initial_dist_coeffs_cv = cv::Mat_::zeros(5, 1); + cv::Mat_ camera_matrix_cv = cv::Mat_::zeros(3, 3); + cv::Mat_ dist_coeffs_cv = cv::Mat_::zeros(5, 1); + std::vector initial_rvecs_cv, rvecs_cv; + std::vector initial_tvecs_cv, tvecs_cv; + + cv::eigen2cv(initial_camera_matrix_eigen, initial_camera_matrix_cv); + cv::eigen2cv(initial_dist_coeffs_eigen, initial_dist_coeffs_cv); + + std::vector> object_points_list_cv; + std::vector> image_points_list_cv; + + for (std::size_t view_id = 0; view_id < object_points_eigen_list.size(); view_id++) { + std::vector object_points; + std::vector image_points; + + const auto & input_object_points = object_points_eigen_list[view_id]; + const auto & input_image_points = image_points_eigen_list[view_id]; + + object_points.reserve(input_object_points.rows()); + image_points.reserve(input_image_points.rows()); + + for (int i = 0; i < input_image_points.rows(); i++) { + object_points.emplace_back( + input_object_points(i, 0), input_object_points(i, 1), input_object_points(i, 2)); + image_points.emplace_back(input_image_points(i, 0), input_image_points(i, 1)); + } + + object_points_list_cv.push_back(object_points); + image_points_list_cv.push_back(image_points); + } + + // Use PnP to get the initial board poses + for (std::size_t view_id = 0; view_id < object_points_list_cv.size(); view_id++) { + std::vector calibration_projected_points; + std::vector pnp_projected_points; + + cv::Mat rvec, tvec; + bool status = cv::solvePnP( + object_points_list_cv[view_id], image_points_list_cv[view_id], initial_camera_matrix_cv, + initial_dist_coeffs_cv, rvec, tvec); + CV_UNUSED(status); + initial_rvecs_cv.push_back(rvec); + initial_tvecs_cv.push_back(tvec); + } + + // Calibrate + CeresCameraIntrinsicsOptimizer optimizer; + optimizer.setRadialDistortionCoefficients(num_radial_coeffs); + optimizer.setTangentialDistortion(use_tangential_distortion); + optimizer.setVerbose(verbose); + optimizer.setData( + initial_camera_matrix_cv, initial_dist_coeffs_cv, object_points_list_cv, image_points_list_cv, + initial_rvecs_cv, initial_tvecs_cv); + optimizer.dataToPlaceholders(); + optimizer.evaluate(); + optimizer.solve(); + optimizer.placeholdersToData(); + optimizer.evaluate(); + double rms_error = optimizer.getSolution(camera_matrix_cv, dist_coeffs_cv, rvecs_cv, tvecs_cv); + + // Extract the results + Eigen::MatrixXd camera_matrix_eigen, dist_coeffs_eigen; + std::vector rvecs_eigen, tvecs_eigen; + + cv::cv2eigen(camera_matrix_cv, camera_matrix_eigen); + cv::cv2eigen(dist_coeffs_cv, dist_coeffs_eigen); + + rvecs_eigen.resize(rvecs_cv.size()); + tvecs_eigen.resize(tvecs_cv.size()); + + for (std::size_t view_id = object_points_list_cv.size(); view_id < object_points_list_cv.size(); + view_id++) { + cv::cv2eigen(rvecs_cv[view_id], rvecs_eigen[view_id]); + cv::cv2eigen(tvecs_cv[view_id], tvecs_eigen[view_id]); + } + + return std::make_tuple( + rms_error, camera_matrix_eigen, dist_coeffs_eigen, rvecs_eigen, tvecs_eigen); +} + +namespace py = pybind11; + +PYBIND11_MODULE(ceres_intrinsic_camera_calibrator_py, m) +{ + m.doc() = R"pbdoc( + Ceres-based camera intrinsics calibrator module + ----------------------- + + .. currentmodule:: ceres_intrinsic_camera_calibrator_py + + .. autosummary:: + :toctree: _generate + + calibrate + )pbdoc"; + + m.def( + "calibrate", calibrate, + R"pbdoc( + Calibrate camera intrinsics + + Args: + object_points_list (List[np.array]): The object points from different views + image_points_list (List[np.array]): The image points from different views + initial_camera_matrix (np.array): The initial camera matrix + initial_dist_coeffs (np.array): The initial distortion coefficients + num_radial_coeffs (int): The number of radial distortion coefficients used during calibration + use_tangential_distortion (bool): Whether we should use tangential distortion durin calibration + + Returns: + The RMS reprojection error, the optimized camera intrinsics, and the board extrinsics + )pbdoc", + py::arg("object_points_list"), py::arg("image_points_list"), py::arg("initial_camera_matrix"), + py::arg("initial_dist_coeffs"), py::arg("num_radial_coeffs"), + py::arg("use_tangential_distortion"), py::arg("verbose") = false); + +#ifdef VERSION_INFO + m.attr("__version__") = MACRO_STRINGIFY(VERSION_INFO); +#else + m.attr("__version__") = "dev"; +#endif +} diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp new file mode 100644 index 00000000..c4a2ca13 --- /dev/null +++ b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp @@ -0,0 +1,314 @@ +// Copyright 2023 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. + +// #include +// #include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +int main(int argc, char ** argv) +{ + CV_UNUSED(argc); + CV_UNUSED(argv); + + // Global config + int cols = 6; + int rows = 8; + std::size_t max_samples = 50; + std::size_t mini_calibration_samples = 20; + bool use_tangent_distortion = true; + int num_radial_distortion_coeffs = 3; + + // Placeholders + std::vector> all_object_points; + std::vector> all_image_points; + + std::vector> mini_calibration_object_points; + std::vector> mini_calibration_image_points; + std::vector> calibration_object_points; + std::vector> calibration_image_points; + std::vector mini_opencv_calibration_rvecs; + std::vector mini_opencv_calibration_tvecs; + std::vector opencv_calibration_rvecs; + std::vector opencv_calibration_tvecs; + std::vector ceres_calibration_rvecs; + std::vector ceres_calibration_tvecs; + + std::string data_path = std::string(argv[1]); + std::vector image_paths; + + // write code to iterate through a folder + const std::filesystem::path fs_data_path{data_path}; + + for (const auto & entry : std::filesystem::directory_iterator(fs_data_path)) { + const auto file_name = entry.path().string(); + if (entry.is_regular_file()) { + image_paths.push_back(file_name); + } + } + + std::cout << "Image files: " << image_paths.size() << std::endl; + + double min_area_percentage = 0.01; + double max_area_percentage = 1.2; + double min_dist_between_blobs_percentage = 1.0; + + cv::SimpleBlobDetector::Params params; + params.filterByArea = true; + + cv::Ptr blobDetector = cv::SimpleBlobDetector::create(params); + + std::vector template_points; + for (int j = 0; j < rows; j++) { + for (int i = 0; i < cols; i++) { + template_points.emplace_back(i, j, 0.0); + } + } + + cv::Size size(-1, -1); + + for (std::size_t i = 0; i < image_paths.size(); ++i) { + cv::Mat grayscale_img = + cv::imread(image_paths[i], cv::IMREAD_GRAYSCALE | cv::IMREAD_IGNORE_ORIENTATION); + + assert(size.height == -1 || size.height == grayscale_img.rows); + assert(size.width == -1 || size.width == grayscale_img.cols); + size = grayscale_img.size(); + + params.minArea = min_area_percentage * size.height * size.width / 100.0; + params.maxArea = max_area_percentage * size.height * size.width / 100.0; + params.minDistBetweenBlobs = + (min_dist_between_blobs_percentage * std::max(size.height, size.width) / 100.0); + + cv::Size pattern(cols, rows); // w x h format + std::vector centers; // this will be filled by the detected centers + + bool found = cv::findCirclesGrid( + grayscale_img, pattern, centers, cv::CALIB_CB_SYMMETRIC_GRID | cv::CALIB_CB_CLUSTERING, + blobDetector); + + if (found) { + all_object_points.push_back(template_points); + all_image_points.push_back(centers); + } + + if (all_object_points.size() >= max_samples) { + break; + } + + std::cout << "|" << std::flush; + } + + std::cout << "Board detections: " << all_object_points.size() << std::endl; + + // Fill the calibration points + calibration_object_points.insert( + calibration_object_points.end(), all_object_points.begin(), + all_object_points.begin() + max_samples); + calibration_image_points.insert( + calibration_image_points.end(), all_image_points.begin(), + all_image_points.begin() + max_samples); + + mini_calibration_object_points.insert( + mini_calibration_object_points.end(), all_object_points.begin(), + all_object_points.begin() + mini_calibration_samples); + mini_calibration_image_points.insert( + mini_calibration_image_points.end(), all_image_points.begin(), + all_image_points.begin() + mini_calibration_samples); + + cv::Mat_ mini_opencv_camera_matrix = cv::Mat_::zeros(3, 3); + cv::Mat_ mini_opencv_dist_coeffs = cv::Mat_::zeros(5, 1); + + cv::Mat_ opencv_camera_matrix = cv::Mat_::zeros(3, 3); + cv::Mat_ opencv_dist_coeffs = cv::Mat_::zeros(5, 1); + cv::Mat_ undistorted_camera_matrix = cv::Mat_::zeros(3, 3); + + cv::Mat_ ceres_camera_matrix = cv::Mat_::zeros(3, 3); + cv::Mat_ ceres_dist_coeffs = cv::Mat_::zeros(5, 1); + + int flags = 0; + + if (!use_tangent_distortion) { + flags |= cv::CALIB_ZERO_TANGENT_DIST; + } + + if (num_radial_distortion_coeffs < 3) { + flags |= cv::CALIB_FIX_K3; + } + + if (num_radial_distortion_coeffs < 2) { + flags |= cv::CALIB_FIX_K2; + } + + if (num_radial_distortion_coeffs < 1) { + flags |= cv::CALIB_FIX_K1; + } + + auto mini_opencv_start = std::chrono::high_resolution_clock::now(); + double mini_reproj_error = cv::calibrateCamera( + mini_calibration_object_points, mini_calibration_image_points, size, mini_opencv_camera_matrix, + mini_opencv_dist_coeffs, mini_opencv_calibration_rvecs, mini_opencv_calibration_tvecs, flags); + + auto mini_opencv_stop = std::chrono::high_resolution_clock::now(); + + std::cout << "Mini opencv calibration results" << std::endl; + std::cout << "\tcamera_matrix: \n" << mini_opencv_camera_matrix << std::endl; + std::cout << "\tdist_coeffs: \n" << mini_opencv_dist_coeffs << std::endl; + std::cout << "\tmini_opencv_calibration_rvecs[0]: \n" + << mini_opencv_calibration_rvecs[0] << std::endl; + std::cout << "\tmini_opencv_calibration_rvecs[0]: \n" + << mini_opencv_calibration_tvecs[0] << std::endl; + + auto opencv_start = std::chrono::high_resolution_clock::now(); + + double reproj_error = cv::calibrateCamera( + calibration_object_points, calibration_image_points, size, opencv_camera_matrix, + opencv_dist_coeffs, opencv_calibration_rvecs, opencv_calibration_tvecs, flags); + + auto opencv_stop = std::chrono::high_resolution_clock::now(); + + std::cout << "Opencv calibration results" << std::endl; + std::cout << "\tcamera_matrix: \n" << opencv_camera_matrix << std::endl; + std::cout << "\tdist_coeffs: \n" << opencv_dist_coeffs << std::endl; + + std::cout << "Mini OpenCV calibration error (reported by the calibrator)=" << mini_reproj_error + << std::endl; + std::cout << "OpenCV calibration error (reported by the calibrator)=" << reproj_error + << std::endl; + + // Need to compute the whole rvecs, tvecs for the whole calibration set + auto ceres_start = std::chrono::high_resolution_clock::now(); + + for (std::size_t i = mini_calibration_object_points.size(); i < calibration_object_points.size(); + i++) { + std::vector calibration_projected_points; + std::vector pnp_projected_points; + + cv::Mat rvec, tvec; + bool status = cv::solvePnP( + calibration_object_points[i], calibration_image_points[i], mini_opencv_camera_matrix, + mini_opencv_dist_coeffs, rvec, tvec); + CV_UNUSED(status); + mini_opencv_calibration_rvecs.push_back(rvec); + mini_opencv_calibration_tvecs.push_back(tvec); + } + + CeresCameraIntrinsicsOptimizer optimizer; + optimizer.setRadialDistortionCoefficients(num_radial_distortion_coeffs); + optimizer.setTangentialDistortion(use_tangent_distortion); + optimizer.setVerbose(true); + optimizer.setData( + mini_opencv_camera_matrix, mini_opencv_dist_coeffs, calibration_object_points, + calibration_image_points, mini_opencv_calibration_rvecs, mini_opencv_calibration_tvecs); + optimizer.dataToPlaceholders(); + optimizer.evaluate(); + optimizer.solve(); + optimizer.placeholdersToData(); + optimizer.evaluate(); + double rms_rror = optimizer.getSolution( + ceres_camera_matrix, ceres_dist_coeffs, ceres_calibration_rvecs, ceres_calibration_tvecs); + (void)rms_rror; + + auto ceres_stop = std::chrono::high_resolution_clock::now(); + + std::cout << "Ceres calibration results" << std::endl; + std::cout << "\tcamera_matrix: \n" << ceres_camera_matrix << std::endl; + std::cout << "\tdist_coeffs: \n" << ceres_dist_coeffs << std::endl; + + std::cout << "Mini opencv calibration results" << std::endl; + std::cout << "\tcamera_matrix: \n" << mini_opencv_camera_matrix << std::endl; + std::cout << "\tdist_coeffs: \n" << mini_opencv_dist_coeffs << std::endl; + std::cout << "\tmini_opencv_calibration_rvecs[0]: \n" + << mini_opencv_calibration_rvecs[0] << std::endl; + std::cout << "\tmini_opencv_calibration_rvecs[0]: \n" + << mini_opencv_calibration_tvecs[0] << std::endl; + + // Start developing the ceres optimizer + double total_mini_opencv_calibration_error = 0.0; + double total_opencv_calibration_error = 0.0; + double total_ceres_calibration_error = 0.0; + auto get_reprojection_error = [](auto & image_points, auto & projected_points) -> double { + cv::Mat x = cv::Mat(2 * image_points.size(), 1, CV_32F, image_points.data()); + cv::Mat y = cv::Mat(2 * projected_points.size(), 1, CV_32F, projected_points.data()); + double total_error = cv::norm(x - y, cv::NORM_L2SQR); + return total_error; + }; + + for (std::size_t i = 0; i < calibration_object_points.size(); i++) { + std::vector mini_opencv_projected_points; + std::vector opencv_projected_points; + std::vector ceres_projected_points; + + cv::projectPoints( + calibration_object_points[i], mini_opencv_calibration_rvecs[i], + mini_opencv_calibration_tvecs[i], mini_opencv_camera_matrix, mini_opencv_dist_coeffs, + mini_opencv_projected_points); + + cv::projectPoints( + calibration_object_points[i], opencv_calibration_rvecs[i], opencv_calibration_tvecs[i], + opencv_camera_matrix, opencv_dist_coeffs, opencv_projected_points); + + cv::projectPoints( + calibration_object_points[i], ceres_calibration_rvecs[i], ceres_calibration_tvecs[i], + ceres_camera_matrix, ceres_dist_coeffs, ceres_projected_points); + + double mini_opencv_error = + get_reprojection_error(mini_opencv_projected_points, calibration_image_points[i]); + double opencv_error = + get_reprojection_error(opencv_projected_points, calibration_image_points[i]); + double ceres_error = + get_reprojection_error(ceres_projected_points, calibration_image_points[i]); + + total_mini_opencv_calibration_error += mini_opencv_error; + total_opencv_calibration_error += opencv_error; + total_ceres_calibration_error += ceres_error; + + printf( + "id=%lu | mini_opencv_error=%.4f | opencv_error=%.4f | ceres_error=%.4f\n", i, + mini_opencv_error, opencv_error, ceres_error); + } + + double rms_mini_opencv_calibration_error = std::sqrt( + total_mini_opencv_calibration_error / (rows * cols * calibration_object_points.size())); + double rms_opencv_calibration_error = + std::sqrt(total_opencv_calibration_error / (rows * cols * calibration_object_points.size())); + double rms_ceres_calibration_error = + std::sqrt(total_ceres_calibration_error / (rows * cols * calibration_object_points.size())); + + printf( + "summary | mini_opencv_error=%.4f | opencv_error=%.4f | ceres_error=%.4f\n", + rms_mini_opencv_calibration_error, rms_opencv_calibration_error, rms_ceres_calibration_error); + + auto mini_opencv_duration = + std::chrono::duration_cast(mini_opencv_stop - mini_opencv_start); + auto opencv_duration = + std::chrono::duration_cast(opencv_stop - opencv_start); + auto ceres_duration = std::chrono::duration_cast(ceres_stop - ceres_start); + + std::cout << "Mini opencv time: " << mini_opencv_duration.count() << " s" << std::endl; + std::cout << "Opencv time: " << opencv_duration.count() << " s" << std::endl; + std::cout << "Ceres time: " << ceres_duration.count() << " s" << std::endl; + + return 0; +} From a0edbffcb192448c8d65fdff6a395498ffaf619d Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 1 Nov 2023 17:19:31 +0900 Subject: [PATCH 02/16] Removed unused file Signed-off-by: Kenzo Lobos-Tsunekawa --- .../ceres_intrinsic_camera_calibrator/CMakeLists.txt | 1 - 1 file changed, 1 deletion(-) diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt index 5da4a003..0c1f6bea 100644 --- a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt +++ b/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt @@ -4,7 +4,6 @@ project(ceres_intrinsic_camera_calibrator) find_package(rclcpp REQUIRED) find_package(rclpy REQUIRED) -find_package(apriltag REQUIRED) find_package(autoware_cmake REQUIRED) find_package(OpenCV REQUIRED) find_package(Ceres REQUIRED) From 1de05a986ed12cd208c432d7fddd9048e27d4691 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Mon, 21 Oct 2024 09:36:27 +0900 Subject: [PATCH 03/16] refactor(ceres_intrinsic_camera_calibrator): new directory structure Signed-off-by: amadeuszsz --- .../ceres_intrinsic_camera_calibrator/.tmp | 0 .../ceres_intrinsic_camera_calibrator/CMakeLists.txt | 0 .../ceres_intrinsic_camera_calibrator/__init__.py | 0 .../ceres_camera_intrinsics_optimizer.hpp | 0 .../ceres_intrinsic_camera_calibrator/reprojection_residual.hpp | 0 .../ceres_intrinsic_camera_calibrator/package.xml | 0 .../src/ceres_camera_intrinsics_optimizer.cpp | 0 .../src/ceres_intrinsic_camera_calibrator_py.cpp | 0 .../ceres_intrinsic_camera_calibrator/src/main.cpp | 0 9 files changed, 0 insertions(+), 0 deletions(-) delete mode 100644 calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/.tmp rename {sensor => calibrators}/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt (100%) rename {sensor => calibrators}/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/__init__.py (100%) rename {sensor => calibrators}/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp (100%) rename {sensor => calibrators}/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp (100%) rename {sensor => calibrators}/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/package.xml (100%) rename {sensor => calibrators}/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp (100%) rename {sensor => calibrators}/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp (100%) rename {sensor => calibrators}/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp (100%) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/.tmp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/.tmp deleted file mode 100644 index e69de29b..00000000 diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt similarity index 100% rename from sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt rename to calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/__init__.py b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/__init__.py similarity index 100% rename from sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/__init__.py rename to calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/__init__.py diff --git a/sensor/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 similarity index 100% rename from sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp rename to calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp similarity index 100% rename from sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp rename to calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/package.xml b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/package.xml similarity index 100% rename from sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/package.xml rename to calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/package.xml diff --git a/sensor/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 similarity index 100% rename from sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp rename to calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_camera_intrinsics_optimizer.cpp diff --git a/sensor/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 similarity index 100% rename from sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp rename to calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/ceres_intrinsic_camera_calibrator_py.cpp diff --git a/sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp similarity index 100% rename from sensor/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp rename to calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/src/main.cpp From d047e7d89ef864664f07d40e868be281914fb61a Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Mon, 21 Oct 2024 09:47:14 +0900 Subject: [PATCH 04/16] chore(ceres_intrinsic_camera_calibrator): update dependencies Signed-off-by: amadeuszsz --- .../ceres_intrinsic_camera_calibrator/package.xml | 2 +- sensor_calibration_tools/package.xml | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/package.xml b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/package.xml index 27d0a9bd..2c960333 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/package.xml +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/package.xml @@ -13,6 +13,7 @@ autoware_cmake + autoware_universe_utils cv_bridge eigen geometry_msgs @@ -32,7 +33,6 @@ tf2_eigen tf2_geometry_msgs tf2_ros - tier4_autoware_utils tier4_calibration_msgs tier4_tag_utils visualization_msgs diff --git a/sensor_calibration_tools/package.xml b/sensor_calibration_tools/package.xml index 22706c06..455ff7b1 100644 --- a/sensor_calibration_tools/package.xml +++ b/sensor_calibration_tools/package.xml @@ -8,6 +8,7 @@ ament_cmake + ceres_intrinsic_camera_calibrator ground_plane_calibrator interactive_camera_lidar_calibrator intrinsic_camera_calibrator From d67455a49e58d0457f031588cc4d44fc0e2e4c59 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Mon, 21 Oct 2024 15:26:37 +0900 Subject: [PATCH 05/16] chore(ceres_intrinsic_camera_calibrator): install targets Signed-off-by: amadeuszsz --- .../ceres_intrinsic_camera_calibrator/CMakeLists.txt | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt index 0c1f6bea..d3e8b322 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt @@ -92,7 +92,4 @@ install( ament_export_dependencies(ament_cmake_python) -#ament_auto_package( -# INSTALL_TO_SHARE -# launch -#) +ament_auto_package() From 0e9fc0318c46abb545b91e732fb8bc75491b45d6 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 21 Oct 2024 16:25:03 +0900 Subject: [PATCH 06/16] chore: updated headers, addressed pre-commit concerns, and fixed some data conditions Signed-off-by: Kenzo Lobos-Tsunekawa --- .../CMakeLists.txt | 18 ------------------ .../ceres_camera_intrinsics_optimizer.hpp | 2 +- .../reprojection_residual.hpp | 2 +- .../src/ceres_camera_intrinsics_optimizer.cpp | 5 ++++- .../ceres_intrinsic_camera_calibrator_py.cpp | 4 +++- .../src/main.cpp | 11 +++++++++-- 6 files changed, 18 insertions(+), 24 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt index d3e8b322..d6296082 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt @@ -14,8 +14,6 @@ find_package(Python3 REQUIRED COMPONENTS Interpreter Development) find_package(pybind11_vendor REQUIRED) find_package(pybind11 REQUIRED) -#add_subdirectory(pybind11) - autoware_package() # These need to be called after autoware_package to avoid being overwritten @@ -42,8 +40,6 @@ ament_export_include_directories( ${OpenCV_INCLUDE_DIRS} ) -# Test - # COMPILE THE SOURCE #======================================================================== ament_auto_add_executable(${PROJECT_NAME}_test @@ -59,8 +55,6 @@ target_link_libraries(${PROJECT_NAME}_test ament_python_install_package(${PROJECT_NAME}) -#ament_python_install_package(${PROJECT_NAME}_py) - pybind11_add_module(${PROJECT_NAME}_py src/ceres_intrinsic_camera_calibrator_py.cpp) target_include_directories(${PROJECT_NAME}_py PRIVATE include ${OpenCV_INCLUDE_DIRS}) @@ -71,25 +65,13 @@ target_link_libraries(${PROJECT_NAME}_py PRIVATE ${PROJECT_NAME} ) -# EXAMPLE_VERSION_INFO is defined by setup.py and passed into the C++ code as a -# define (VERSION_INFO) here. target_compile_definitions(${PROJECT_NAME}_py PRIVATE VERSION_INFO=${EXAMPLE_VERSION_INFO}) - - -message("====PYTHON_INSTALL_DIR=${PYTHON_INSTALL_DIR}") - install( TARGETS ${PROJECT_NAME}_py - #DESTINATION "${PYTHON_INSTALL_DIR}" DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}" ) -#install(PROGRAMS -# scripts/calibrator_ui_node.py -# DESTINATION lib/${PROJECT_NAME} -#) - ament_export_dependencies(ament_cmake_python) ament_auto_package() 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 1fc2268e..8350abe3 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 @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp index 9f2226cf..8750d1e3 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. 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 fb3e6a0f..7372bf58 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 @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -29,7 +29,10 @@ #include #include +#include +#include #include +#include void CeresCameraIntrinsicsOptimizer::setRadialDistortionCoefficients( int radial_distortion_coefficients) 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 24a3c023..29c05119 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 @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -22,8 +22,10 @@ #include #include +#include #include #include +#include #define STRINGIFY(x) #x #define MACRO_STRINGIFY(x) STRINGIFY(x) 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 c4a2ca13..ecb44e3c 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 @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -20,8 +20,9 @@ #include #include +#include #include -#include +#include // NOLINT #include #include #include @@ -122,7 +123,13 @@ int main(int argc, char ** argv) std::cout << "Board detections: " << all_object_points.size() << std::endl; + if (all_object_points.size() == 0) { + std::cout << "Calibration aborted due to a lack of observations" << std::endl; + } + // Fill the calibration points + max_samples = std::min(max_samples, all_object_points.size()); + mini_calibration_samples = std::min(mini_calibration_samples, all_object_points.size()); calibration_object_points.insert( calibration_object_points.end(), all_object_points.begin(), all_object_points.begin() + max_samples); From a9d435c246876cff3fbdc9b1325f4e56a5c0e256 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 21 Oct 2024 19:06:07 +0900 Subject: [PATCH 07/16] feat: backported the rational model implementation and other changes (was lost in an experimental branch) Signed-off-by: Kenzo Lobos-Tsunekawa --- .../ceres_camera_intrinsics_optimizer.hpp | 10 ++- .../reprojection_residual.hpp | 54 ++++++++++--- .../src/ceres_camera_intrinsics_optimizer.cpp | 78 +++++++++++++++++- .../ceres_intrinsic_camera_calibrator_py.cpp | 20 +++-- .../src/main.cpp | 79 +++++++++++++++---- 5 files changed, 201 insertions(+), 40 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 8350abe3..72e69ce0 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 @@ -31,7 +31,7 @@ class CeresCameraIntrinsicsOptimizer { public: static constexpr int POSE_OPT_DIM = 7; - static constexpr int INTRINSICS_DIM = 9; + static constexpr int INTRINSICS_DIM = 12; static constexpr int ROTATION_W_INDEX = 0; static constexpr int ROTATION_X_INDEX = 1; @@ -61,6 +61,13 @@ class CeresCameraIntrinsicsOptimizer */ void setTangentialDistortion(bool use_tangential_distortion); + /*! + * Sets the number of rational distortion coefficients + * @param[in] rational_distortion_coefficients number of radial distortion coefficients + * optimized + */ + void setRationalDistortionCoefficients(int rational_distortion_coefficients); + /*! * Sets the verbose mode * @param[in] verbose whether or not to use tangential distortion @@ -118,6 +125,7 @@ class CeresCameraIntrinsicsOptimizer protected: int radial_distortion_coefficients_; bool use_tangential_distortion_; + int rational_distortion_coefficients_; 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/reprojection_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp index 8750d1e3..099416f4 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp @@ -46,12 +46,13 @@ struct ReprojectionResidual ReprojectionResidual( const cv::Point3f & object_point, const cv::Point2f & image_point, int radial_distortion_coeffs, - bool use_tangential_distortion) + bool use_tangential_distortion, int rational_distortion_coeffs) { object_point_ = Eigen::Vector3d(object_point.x, object_point.y, object_point.z); image_point_ = Eigen::Vector2d(image_point.x, image_point.y); radial_distortion_coeffs_ = radial_distortion_coeffs; use_tangential_distortion_ = use_tangential_distortion; + rational_distortion_coeffs_ = rational_distortion_coeffs; } /*! @@ -76,22 +77,32 @@ struct ReprojectionResidual Vector3 object_point_ccs = board_quaternion * (T(1.0) * object_point_) + board_translation; const T null_value = T(0.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]; - const T & k1 = radial_distortion_coeffs_ > 0 ? camera_intrinsics[4] : null_value; - const T & k2 = radial_distortion_coeffs_ > 1 ? camera_intrinsics[5] : null_value; - const T & k3 = radial_distortion_coeffs_ > 2 ? camera_intrinsics[6] : null_value; - const T & p1 = - use_tangential_distortion_ ? camera_intrinsics[4 + radial_distortion_coeffs_] : null_value; - const T & p2 = - use_tangential_distortion_ ? camera_intrinsics[5 + radial_distortion_coeffs_] : null_value; + 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; const T xp = object_point_ccs.x() / object_point_ccs.z(); const T yp = object_point_ccs.y() / object_point_ccs.z(); const T r2 = xp * xp + yp * yp; - const T d = 1.0 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2; + 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); @@ -115,13 +126,15 @@ struct ReprojectionResidual */ static ceres::CostFunction * createResidual( const cv::Point3f & object_point, const cv::Point2f & image_point, int radial_distortion_coeffs, - bool use_tangential_distortion) + bool use_tangential_distortion, int rational_distortion_coeffs) { auto f = new ReprojectionResidual( - object_point, image_point, radial_distortion_coeffs, use_tangential_distortion); + object_point, image_point, radial_distortion_coeffs, use_tangential_distortion, + rational_distortion_coeffs); - int distortion_coefficients = - radial_distortion_coeffs + 2 * static_cast(use_tangential_distortion); + int distortion_coefficients = radial_distortion_coeffs + + 2 * static_cast(use_tangential_distortion) + + rational_distortion_coeffs; ceres::CostFunction * cost_function = nullptr; switch (distortion_coefficients) { @@ -149,6 +162,20 @@ struct ReprojectionResidual 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; @@ -158,6 +185,7 @@ struct ReprojectionResidual Eigen::Vector2d image_point_; int radial_distortion_coeffs_; bool use_tangential_distortion_; + int rational_distortion_coeffs_; }; #endif // CERES_INTRINSIC_CAMERA_CALIBRATOR__REPROJECTION_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 7372bf58..a27e4ea2 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 @@ -45,6 +45,12 @@ void CeresCameraIntrinsicsOptimizer::setTangentialDistortion(bool use_tangential use_tangential_distortion_ = use_tangential_distortion; } +void CeresCameraIntrinsicsOptimizer::setRationalDistortionCoefficients( + int rational_distortion_coefficients) +{ + rational_distortion_coefficients_ = rational_distortion_coefficients; +} + void CeresCameraIntrinsicsOptimizer::setVerbose(bool verbose) { verbose_ = verbose; } void CeresCameraIntrinsicsOptimizer::setData( @@ -53,6 +59,40 @@ void CeresCameraIntrinsicsOptimizer::setData( const std::vector> & image_points, const std::vector & rvecs, const std::vector & tvecs) { + if (camera_matrix.cols != 3 || camera_matrix.rows != 3) { + throw std::runtime_error("The camera matrix should be a 3x3 matrix"); + } + + if ( + rational_distortion_coefficients_ > 0 && distortion_coeffs.cols * distortion_coeffs.rows != 8) { + throw std::runtime_error( + "8 distortion coefficients are expected when the rational model is used"); + } + + if ( + rational_distortion_coefficients_ == 0 && + distortion_coeffs.cols * distortion_coeffs.rows != 5) { + throw std::runtime_error( + "5 distortion coefficients are expected when the polynomial model is used"); + } + + if (object_points.size() != image_points.size()) { + throw std::runtime_error( + "The number of object points should be equal to the number of image points"); + } + + if (rvecs.size() != tvecs.size()) { + throw std::runtime_error("The number of rvecs should be equal to the number of tvecs"); + } + + if (rvecs.size() != object_points.size()) { + throw std::runtime_error("The number of rvecs should be equal to the number of object points"); + } + + if (object_points.size() == 0) { + throw std::runtime_error("There should be at least one calibration sample"); + } + camera_matrix_ = camera_matrix.clone(); distortion_coeffs_ = distortion_coeffs.clone(); object_points_ = object_points; @@ -97,17 +137,24 @@ double CeresCameraIntrinsicsOptimizer::getSolution( void CeresCameraIntrinsicsOptimizer::dataToPlaceholders() { // Convert the intrinsics + int num_dist_coefficients = distortion_coeffs_.size().area(); intrinsics_placeholder_[INTRINSICS_CX_INDEX] = camera_matrix_(0, 2); intrinsics_placeholder_[INTRINSICS_CY_INDEX] = camera_matrix_(1, 2); intrinsics_placeholder_[INTRINSICS_FX_INDEX] = camera_matrix_(0, 0); intrinsics_placeholder_[INTRINSICS_FY_INDEX] = camera_matrix_(1, 1); + // plumb bob double k1 = distortion_coeffs_(0); double k2 = distortion_coeffs_(1); double p1 = distortion_coeffs_(2); double p2 = distortion_coeffs_(3); double k3 = distortion_coeffs_(4); + // rational + double k4 = num_dist_coefficients >= 6 ? distortion_coeffs_(5) : 0.0; + double k5 = num_dist_coefficients >= 7 ? distortion_coeffs_(6) : 0.0; + double k6 = num_dist_coefficients >= 8 ? distortion_coeffs_(7) : 0.0; + int index = 4; if (radial_distortion_coefficients_ > 0) { @@ -124,6 +171,16 @@ void CeresCameraIntrinsicsOptimizer::dataToPlaceholders() intrinsics_placeholder_[index++] = p2; } + if (rational_distortion_coefficients_ > 0) { + intrinsics_placeholder_[index++] = k4; + } + if (rational_distortion_coefficients_ > 1) { + intrinsics_placeholder_[index++] = k5; + } + if (rational_distortion_coefficients_ > 2) { + intrinsics_placeholder_[index++] = k6; + } + // Convert the revcs, tvecs into the placeholders pose_placeholders_.resize(object_points_.size()); @@ -156,12 +213,14 @@ void CeresCameraIntrinsicsOptimizer::placeholdersToData() camera_matrix_(0, 0) = intrinsics_placeholder_[INTRINSICS_FX_INDEX]; camera_matrix_(1, 1) = intrinsics_placeholder_[INTRINSICS_FY_INDEX]; - distortion_coeffs_ = cv::Mat_::zeros(5, 1); double & k1 = distortion_coeffs_(0); double & k2 = distortion_coeffs_(1); double & p1 = distortion_coeffs_(2); double & p2 = distortion_coeffs_(3); double & k3 = distortion_coeffs_(4); + double & k4 = distortion_coeffs_(5); + double & k5 = distortion_coeffs_(6); + double & k6 = distortion_coeffs_(7); int index = 4; @@ -179,6 +238,16 @@ void CeresCameraIntrinsicsOptimizer::placeholdersToData() p2 = intrinsics_placeholder_[index++]; } + if (rational_distortion_coefficients_ > 0) { + k4 = intrinsics_placeholder_[index++]; + } + if (rational_distortion_coefficients_ > 1) { + k5 = intrinsics_placeholder_[index++]; + } + if (rational_distortion_coefficients_ > 2) { + k6 = intrinsics_placeholder_[index++]; + } + // Convert the revcs, tvecs into the placeholders rvecs_.resize(pose_placeholders_.size()); tvecs_.resize(pose_placeholders_.size()); @@ -253,7 +322,8 @@ void CeresCameraIntrinsicsOptimizer::evaluate() 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_); + 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]; @@ -278,7 +348,8 @@ void CeresCameraIntrinsicsOptimizer::solve() problem.AddResidualBlock( ReprojectionResidual::createResidual( view_object_points[point_index], view_image_points[point_index], - radial_distortion_coefficients_, use_tangential_distortion_), + radial_distortion_coefficients_, use_tangential_distortion_, + rational_distortion_coefficients_), nullptr, // L2 intrinsics_placeholder_.data(), pose_placeholder.data()); } @@ -302,6 +373,7 @@ void CeresCameraIntrinsicsOptimizer::solve() 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 = true; 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 29c05119..926d7f8c 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,19 +45,22 @@ 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, - bool use_tangential_distortion, bool verbose) + bool use_tangential_distortion, int num_rational_coeffs, bool verbose) { - (void)num_radial_coeffs; - (void)use_tangential_distortion; - (void)verbose; - if ( 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 || - std::min(initial_dist_coeffs_eigen.rows(), initial_dist_coeffs_eigen.cols() > 1) || + num_radial_coeffs > 3 || num_rational_coeffs < 0 || num_rational_coeffs > 3 || + std::min(initial_dist_coeffs_eigen.rows(), initial_dist_coeffs_eigen.cols()) > 1 || (initial_dist_coeffs_eigen.rows() * initial_dist_coeffs_eigen.cols()) != 5) { 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; + std::cout << "\t initial_camera_matrix:\n" << initial_camera_matrix_eigen << std::endl; + std::cout << "\t initial_dist_coeffs:\n" << initial_dist_coeffs_eigen << std::endl; + 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; return std::tuple< double, Eigen::MatrixXd, Eigen::MatrixXd, std::vector, std::vector>(); @@ -115,6 +118,7 @@ calibrate( CeresCameraIntrinsicsOptimizer optimizer; optimizer.setRadialDistortionCoefficients(num_radial_coeffs); optimizer.setTangentialDistortion(use_tangential_distortion); + optimizer.setRationalDistortionCoefficients(num_rational_coeffs); optimizer.setVerbose(verbose); optimizer.setData( initial_camera_matrix_cv, initial_dist_coeffs_cv, object_points_list_cv, image_points_list_cv, @@ -179,7 +183,7 @@ PYBIND11_MODULE(ceres_intrinsic_camera_calibrator_py, m) The RMS reprojection error, the optimized camera intrinsics, and the board extrinsics )pbdoc", py::arg("object_points_list"), py::arg("image_points_list"), py::arg("initial_camera_matrix"), - py::arg("initial_dist_coeffs"), py::arg("num_radial_coeffs"), + py::arg("initial_dist_coeffs"), py::arg("num_radial_coeffs"), py::arg("num_rational_coeffs"), py::arg("use_tangential_distortion"), 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 ecb44e3c..e991832b 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,16 +30,22 @@ int main(int argc, char ** argv) { - CV_UNUSED(argc); - CV_UNUSED(argv); + if (argc != 5) { + std::cout + << "Usage: " << argv[0] + << " " + << std::endl; + return 1; + } // Global config int cols = 6; int rows = 8; std::size_t max_samples = 50; std::size_t mini_calibration_samples = 20; - bool use_tangent_distortion = true; - int num_radial_distortion_coeffs = 3; + int num_radial_distortion_coeffs = atoi(argv[2]); + bool use_tangent_distortion = atoi(argv[3]); + int num_rational_distortion_coeffs = atoi(argv[4]); // Placeholders std::vector> all_object_points; @@ -102,16 +108,29 @@ int main(int argc, char ** argv) params.minDistBetweenBlobs = (min_dist_between_blobs_percentage * std::max(size.height, size.width) / 100.0); - cv::Size pattern(cols, rows); // w x h format - std::vector centers; // this will be filled by the detected centers + cv::Size pattern(cols, rows); // w x h format + std::vector circle_centers, + chessboard_centers; // this will be filled by the detected centers - bool found = cv::findCirclesGrid( - grayscale_img, pattern, centers, cv::CALIB_CB_SYMMETRIC_GRID | cv::CALIB_CB_CLUSTERING, + bool found_circles = cv::findCirclesGrid( + grayscale_img, pattern, circle_centers, cv::CALIB_CB_SYMMETRIC_GRID | cv::CALIB_CB_CLUSTERING, blobDetector); - if (found) { + bool found_chessboard = findChessboardCorners( + grayscale_img, pattern, chessboard_centers, + cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE + cv::CALIB_CB_FAST_CHECK); + + if (found_chessboard) + cornerSubPix( + grayscale_img, chessboard_centers, cv::Size(11, 11), cv::Size(-1, -1), + cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::MAX_ITER, 30, 0.1)); + + if (found_circles) { + all_object_points.push_back(template_points); + all_image_points.push_back(circle_centers); + } else if (found_chessboard) { all_object_points.push_back(template_points); - all_image_points.push_back(centers); + all_image_points.push_back(chessboard_centers); } if (all_object_points.size() >= max_samples) { @@ -145,18 +164,24 @@ int main(int argc, char ** argv) all_image_points.begin() + mini_calibration_samples); cv::Mat_ mini_opencv_camera_matrix = cv::Mat_::zeros(3, 3); - cv::Mat_ mini_opencv_dist_coeffs = cv::Mat_::zeros(5, 1); + cv::Mat_ mini_opencv_dist_coeffs = num_rational_distortion_coeffs == 0 + ? cv::Mat_::zeros(5, 1) + : cv::Mat_::zeros(8, 1); cv::Mat_ opencv_camera_matrix = cv::Mat_::zeros(3, 3); - cv::Mat_ opencv_dist_coeffs = cv::Mat_::zeros(5, 1); + cv::Mat_ opencv_dist_coeffs = num_rational_distortion_coeffs == 0 + ? cv::Mat_::zeros(5, 1) + : cv::Mat_::zeros(8, 1); cv::Mat_ undistorted_camera_matrix = cv::Mat_::zeros(3, 3); cv::Mat_ ceres_camera_matrix = cv::Mat_::zeros(3, 3); - cv::Mat_ ceres_dist_coeffs = cv::Mat_::zeros(5, 1); + cv::Mat_ ceres_dist_coeffs = num_rational_distortion_coeffs == 0 + ? cv::Mat_::zeros(5, 1) + : cv::Mat_::zeros(8, 1); int flags = 0; - if (!use_tangent_distortion) { + /* if (!use_tangent_distortion) { flags |= cv::CALIB_ZERO_TANGENT_DIST; } @@ -170,13 +195,36 @@ int main(int argc, char ** argv) if (num_radial_distortion_coeffs < 1) { flags |= cv::CALIB_FIX_K1; - } + } */ + + /* flags |= cv2.CALIB_ZERO_TANGENT_DIST if not self.use_tangential_distortion.value else 0 + flags |= cv2.CALIB_RATIONAL_MODEL if self.radial_distortion_coefficients.value > 3 else 0 + flags |= cv2.CALIB_FIX_K6 if self.radial_distortion_coefficients.value < 6 else 0 + flags |= cv2.CALIB_FIX_K5 if self.radial_distortion_coefficients.value < 5 else 0 + flags |= cv2.CALIB_FIX_K4 if self.radial_distortion_coefficients.value < 4 else 0 + flags |= cv2.CALIB_FIX_K3 if self.radial_distortion_coefficients.value < 3 else 0 + flags |= cv2.CALIB_FIX_K2 if self.radial_distortion_coefficients.value < 2 else 0 + flags |= cv2.CALIB_FIX_K1 if self.radial_distortion_coefficients.value < 1 else 0 */ + + flags |= use_tangent_distortion ? 0 : cv::CALIB_ZERO_TANGENT_DIST; + flags |= num_rational_distortion_coeffs > 0 ? cv::CALIB_RATIONAL_MODEL : 0; + flags |= num_rational_distortion_coeffs < 3 ? cv::CALIB_FIX_K6 : 0; + flags |= num_rational_distortion_coeffs < 2 ? cv::CALIB_FIX_K5 : 0; + flags |= num_rational_distortion_coeffs < 1 ? cv::CALIB_FIX_K4 : 0; + flags |= num_radial_distortion_coeffs < 3 ? cv::CALIB_FIX_K3 : 0; + flags |= num_radial_distortion_coeffs < 2 ? cv::CALIB_FIX_K2 : 0; + flags |= num_radial_distortion_coeffs < 1 ? cv::CALIB_FIX_K1 : 0; auto mini_opencv_start = std::chrono::high_resolution_clock::now(); double mini_reproj_error = cv::calibrateCamera( mini_calibration_object_points, mini_calibration_image_points, size, mini_opencv_camera_matrix, mini_opencv_dist_coeffs, mini_opencv_calibration_rvecs, mini_opencv_calibration_tvecs, flags); + // When the rational mode is enabled in opencv, it just created a vector of the maximum dimension + if (mini_opencv_dist_coeffs.rows * mini_opencv_dist_coeffs.cols > 8) { + mini_opencv_dist_coeffs = mini_opencv_dist_coeffs.rowRange(0, 8); + } + auto mini_opencv_stop = std::chrono::high_resolution_clock::now(); std::cout << "Mini opencv calibration results" << std::endl; @@ -224,6 +272,7 @@ int main(int argc, char ** argv) CeresCameraIntrinsicsOptimizer optimizer; optimizer.setRadialDistortionCoefficients(num_radial_distortion_coeffs); optimizer.setTangentialDistortion(use_tangent_distortion); + optimizer.setRationalDistortionCoefficients(num_rational_distortion_coeffs); optimizer.setVerbose(true); optimizer.setData( mini_opencv_camera_matrix, mini_opencv_dist_coeffs, calibration_object_points, From 5b5a1486899f6dc2389dbd6ee335d5d19cb94b2a Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 21 Oct 2024 19:16:21 +0900 Subject: [PATCH 08/16] chore: ci/cd fixes Signed-off-by: Kenzo Lobos-Tsunekawa --- .../src/ceres_camera_intrinsics_optimizer.cpp | 10 ++--- .../ceres_intrinsic_camera_calibrator_py.cpp | 3 +- .../src/main.cpp | 38 +++---------------- 3 files changed, 13 insertions(+), 38 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 a27e4ea2..99a4c287 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 @@ -181,17 +181,17 @@ void CeresCameraIntrinsicsOptimizer::dataToPlaceholders() intrinsics_placeholder_[index++] = k6; } - // Convert the revcs, tvecs into the placeholders + // Convert the revcs, tvecs into the placeholders // cSpell:ignore rvecs,tvecs pose_placeholders_.resize(object_points_.size()); for (std::size_t i = 0; i < object_points_.size(); i++) { - cv::Mat rmat; - cv::Rodrigues(rvecs_[i], rmat); + cv::Mat rotation_cv; + cv::Rodrigues(rvecs_[i], rotation_cv); Eigen::Vector3d translation; Eigen::Matrix3d rotation; cv::cv2eigen(tvecs_[i], translation); - cv::cv2eigen(rmat, rotation); + cv::cv2eigen(rotation_cv, rotation); Eigen::Quaterniond quat(rotation); std::array & placeholder = pose_placeholders_[i]; @@ -367,7 +367,7 @@ void CeresCameraIntrinsicsOptimizer::solve() } ceres::Solver::Options options; - options.linear_solver_type = ceres::DENSE_SCHUR; + 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; 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 926d7f8c..071b8214 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 @@ -154,6 +154,7 @@ namespace py = pybind11; PYBIND11_MODULE(ceres_intrinsic_camera_calibrator_py, m) { + // cSpell:ignore pbdoc,currentmodule,autosummary,toctree m.doc() = R"pbdoc( Ceres-based camera intrinsics calibrator module ----------------------- @@ -177,7 +178,7 @@ PYBIND11_MODULE(ceres_intrinsic_camera_calibrator_py, m) initial_camera_matrix (np.array): The initial camera matrix initial_dist_coeffs (np.array): The initial distortion coefficients num_radial_coeffs (int): The number of radial distortion coefficients used during calibration - use_tangential_distortion (bool): Whether we should use tangential distortion durin calibration + use_tangential_distortion (bool): Whether we should use tangential distortion during calibration Returns: The RMS reprojection error, the optimized camera intrinsics, and the board extrinsics 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 e991832b..3e9379ef 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 @@ -181,31 +181,6 @@ int main(int argc, char ** argv) int flags = 0; - /* if (!use_tangent_distortion) { - flags |= cv::CALIB_ZERO_TANGENT_DIST; - } - - if (num_radial_distortion_coeffs < 3) { - flags |= cv::CALIB_FIX_K3; - } - - if (num_radial_distortion_coeffs < 2) { - flags |= cv::CALIB_FIX_K2; - } - - if (num_radial_distortion_coeffs < 1) { - flags |= cv::CALIB_FIX_K1; - } */ - - /* flags |= cv2.CALIB_ZERO_TANGENT_DIST if not self.use_tangential_distortion.value else 0 - flags |= cv2.CALIB_RATIONAL_MODEL if self.radial_distortion_coefficients.value > 3 else 0 - flags |= cv2.CALIB_FIX_K6 if self.radial_distortion_coefficients.value < 6 else 0 - flags |= cv2.CALIB_FIX_K5 if self.radial_distortion_coefficients.value < 5 else 0 - flags |= cv2.CALIB_FIX_K4 if self.radial_distortion_coefficients.value < 4 else 0 - flags |= cv2.CALIB_FIX_K3 if self.radial_distortion_coefficients.value < 3 else 0 - flags |= cv2.CALIB_FIX_K2 if self.radial_distortion_coefficients.value < 2 else 0 - flags |= cv2.CALIB_FIX_K1 if self.radial_distortion_coefficients.value < 1 else 0 */ - flags |= use_tangent_distortion ? 0 : cv::CALIB_ZERO_TANGENT_DIST; flags |= num_rational_distortion_coeffs > 0 ? cv::CALIB_RATIONAL_MODEL : 0; flags |= num_rational_distortion_coeffs < 3 ? cv::CALIB_FIX_K6 : 0; @@ -216,7 +191,7 @@ int main(int argc, char ** argv) flags |= num_radial_distortion_coeffs < 1 ? cv::CALIB_FIX_K1 : 0; auto mini_opencv_start = std::chrono::high_resolution_clock::now(); - double mini_reproj_error = cv::calibrateCamera( + double mini_reprojection_error = cv::calibrateCamera( mini_calibration_object_points, mini_calibration_image_points, size, mini_opencv_camera_matrix, mini_opencv_dist_coeffs, mini_opencv_calibration_rvecs, mini_opencv_calibration_tvecs, flags); @@ -237,7 +212,7 @@ int main(int argc, char ** argv) auto opencv_start = std::chrono::high_resolution_clock::now(); - double reproj_error = cv::calibrateCamera( + double reprojection_error = cv::calibrateCamera( calibration_object_points, calibration_image_points, size, opencv_camera_matrix, opencv_dist_coeffs, opencv_calibration_rvecs, opencv_calibration_tvecs, flags); @@ -247,9 +222,9 @@ int main(int argc, char ** argv) std::cout << "\tcamera_matrix: \n" << opencv_camera_matrix << std::endl; std::cout << "\tdist_coeffs: \n" << opencv_dist_coeffs << std::endl; - std::cout << "Mini OpenCV calibration error (reported by the calibrator)=" << mini_reproj_error - << std::endl; - std::cout << "OpenCV calibration error (reported by the calibrator)=" << reproj_error + std::cout << "Mini OpenCV calibration error (reported by the calibrator)=" + << mini_reprojection_error << std::endl; + std::cout << "OpenCV calibration error (reported by the calibrator)=" << reprojection_error << std::endl; // Need to compute the whole rvecs, tvecs for the whole calibration set @@ -282,9 +257,8 @@ int main(int argc, char ** argv) optimizer.solve(); optimizer.placeholdersToData(); optimizer.evaluate(); - double rms_rror = optimizer.getSolution( + [[maybe_unused]] double rms_error = optimizer.getSolution( ceres_camera_matrix, ceres_dist_coeffs, ceres_calibration_rvecs, ceres_calibration_tvecs); - (void)rms_rror; auto ceres_stop = std::chrono::high_resolution_clock::now(); From 088e9950faa669992d63273e524e91553e34cf2f Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 21 Oct 2024 19:18:21 +0900 Subject: [PATCH 09/16] chore: last ci/cd fixes Signed-off-by: Kenzo Lobos-Tsunekawa --- .../ceres_intrinsic_camera_calibrator/reprojection_residual.hpp | 2 +- .../src/ceres_camera_intrinsics_optimizer.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp index 099416f4..82049fa1 100644 --- a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp +++ b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/include/ceres_intrinsic_camera_calibrator/reprojection_residual.hpp @@ -121,7 +121,7 @@ struct ReprojectionResidual * @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 tangenrial distortion + * @param[in] use_tangential_distortion Whether to use or not tangential distortion * @returns the ceres residual */ static ceres::CostFunction * createResidual( 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 99a4c287..8a283e42 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 @@ -248,7 +248,7 @@ void CeresCameraIntrinsicsOptimizer::placeholdersToData() k6 = intrinsics_placeholder_[index++]; } - // Convert the revcs, tvecs into the placeholders + // Convert the revcs, tvecs into the placeholders // cSpell:ignore rvecs,tvecs rvecs_.resize(pose_placeholders_.size()); tvecs_.resize(pose_placeholders_.size()); From 22db8b802382ebbeedac9d8c9aa357ab75907987 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Mon, 21 Oct 2024 19:19:43 +0900 Subject: [PATCH 10/16] chore: last-last fix Signed-off-by: Kenzo Lobos-Tsunekawa --- .../src/ceres_camera_intrinsics_optimizer.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 8a283e42..673211a2 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 @@ -181,7 +181,7 @@ void CeresCameraIntrinsicsOptimizer::dataToPlaceholders() intrinsics_placeholder_[index++] = k6; } - // Convert the revcs, tvecs into the placeholders // cSpell:ignore rvecs,tvecs + // Convert the rvecs, tvecs into the placeholders // cSpell:ignore rvecs,tvecs pose_placeholders_.resize(object_points_.size()); for (std::size_t i = 0; i < object_points_.size(); i++) { @@ -248,7 +248,7 @@ void CeresCameraIntrinsicsOptimizer::placeholdersToData() k6 = intrinsics_placeholder_[index++]; } - // Convert the revcs, tvecs into the placeholders // cSpell:ignore rvecs,tvecs + // Convert the rvecs, tvecs into the placeholders // cSpell:ignore rvecs,tvecs rvecs_.resize(pose_placeholders_.size()); tvecs_.resize(pose_placeholders_.size()); From ca6db27c095688bdf08d887f5802fae9b696c471 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Tue, 22 Oct 2024 10:28:19 +0900 Subject: [PATCH 11/16] fix(intrinsic_camera_calibrator): empty points array Signed-off-by: amadeuszsz --- .../intrinsic_camera_calibrator/views/image_view.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py index 2223e37a..73d64641 100644 --- a/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py +++ b/calibrators/intrinsic_camera_calibrator/intrinsic_camera_calibrator/intrinsic_camera_calibrator/views/image_view.py @@ -292,7 +292,8 @@ def draw_points(self, painter: QPainter, points: np.array): pen = QPen(QColor(255, 0.0, 255, int(255 * self.rendering_alpha))) painter.setPen(pen) - points = points * self.image_to_view_factor + if points.size > 0: + points = points * self.image_to_view_factor size = 3 rect_list = [ QRectF(point[0] - 0.5 * size, point[1] - 0.5 * size, size, size) for point in points From e3a809de7dff43e4db044d0fc92df792c1862f06 Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 23 Oct 2024 15:45:24 +0900 Subject: [PATCH 12/16] fix(ceres_intrinsic_camera_calibrator): missing args & args order Signed-off-by: amadeuszsz --- .../src/ceres_intrinsic_camera_calibrator_py.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 071b8214..1303a2fa 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,7 @@ 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, - bool use_tangential_distortion, int num_rational_coeffs, bool verbose) + int num_rational_coeffs, bool use_tangential_distortion, bool verbose) { if ( initial_camera_matrix_eigen.cols() != 3 || initial_camera_matrix_eigen.rows() != 3 || @@ -178,7 +178,9 @@ PYBIND11_MODULE(ceres_intrinsic_camera_calibrator_py, m) initial_camera_matrix (np.array): The initial camera matrix initial_dist_coeffs (np.array): The initial distortion coefficients 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 + verbose (bool): Whether we should print debug information Returns: The RMS reprojection error, the optimized camera intrinsics, and the board extrinsics From 28739099ae11815497c1895f30e25118db26b65a Mon Sep 17 00:00:00 2001 From: amadeuszsz Date: Wed, 23 Oct 2024 15:46:41 +0900 Subject: [PATCH 13/16] feat(ceres_intrinsic_camera_calibrator): enable rational coeffs Signed-off-by: amadeuszsz --- .../src/ceres_intrinsic_camera_calibrator_py.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 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 1303a2fa..d28cfa30 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 @@ -51,8 +51,7 @@ calibrate( initial_camera_matrix_eigen.cols() != 3 || initial_camera_matrix_eigen.rows() != 3 || object_points_eigen_list.size() != image_points_eigen_list.size() || num_radial_coeffs < 0 || num_radial_coeffs > 3 || num_rational_coeffs < 0 || num_rational_coeffs > 3 || - std::min(initial_dist_coeffs_eigen.rows(), initial_dist_coeffs_eigen.cols()) > 1 || - (initial_dist_coeffs_eigen.rows() * initial_dist_coeffs_eigen.cols()) != 5) { + std::min(initial_dist_coeffs_eigen.rows(), initial_dist_coeffs_eigen.cols()) > 1) { 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; From 1482deae4fda5c3180d9a798e8dc4f9b310ec68e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 23 Oct 2024 06:47:56 +0000 Subject: [PATCH 14/16] ci(pre-commit): autofix --- .../src/ceres_intrinsic_camera_calibrator_py.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 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 d28cfa30..4d2e44d8 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 @@ -44,8 +44,8 @@ calibrate( const std::vector & object_points_eigen_list, 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, bool verbose) + const Eigen::MatrixXd & initial_dist_coeffs_eigen, int num_radial_coeffs, int num_rational_coeffs, + bool use_tangential_distortion, bool verbose) { if ( initial_camera_matrix_eigen.cols() != 3 || initial_camera_matrix_eigen.rows() != 3 || From 579f676a50ba9c27b8919284613e2c838c99a2d1 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 23 Oct 2024 17:11:33 +0900 Subject: [PATCH 15/16] chore: updated parameter check Signed-off-by: Kenzo Lobos-Tsunekawa --- .../src/ceres_intrinsic_camera_calibrator_py.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 4d2e44d8..491c3128 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 @@ -47,11 +47,13 @@ calibrate( const Eigen::MatrixXd & initial_dist_coeffs_eigen, int num_radial_coeffs, int num_rational_coeffs, bool use_tangential_distortion, bool verbose) { + int num_dist_coeffs = initial_dist_coeffs_eigen.rows() * initial_dist_coeffs_eigen.cols(); + int expected_dist_coeffs = num_rational_coeffs > 0 ? 8 : 5; if ( 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 || - std::min(initial_dist_coeffs_eigen.rows(), initial_dist_coeffs_eigen.cols()) > 1) { + num_dist_coeffs != expected_dist_coeffs) { 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; From e00efb697e9e9cad5a07a9e701557d0f7631e3c8 Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 23 Oct 2024 18:59:21 +0900 Subject: [PATCH 16/16] chore: type fix Signed-off-by: Kenzo Lobos-Tsunekawa --- .../src/ceres_intrinsic_camera_calibrator_py.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 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 491c3128..2a1a16d4 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 @@ -47,8 +47,9 @@ calibrate( const Eigen::MatrixXd & initial_dist_coeffs_eigen, int num_radial_coeffs, int num_rational_coeffs, bool use_tangential_distortion, bool verbose) { - int num_dist_coeffs = initial_dist_coeffs_eigen.rows() * initial_dist_coeffs_eigen.cols(); - int expected_dist_coeffs = num_rational_coeffs > 0 ? 8 : 5; + Eigen::Index num_dist_coeffs = + initial_dist_coeffs_eigen.rows() * initial_dist_coeffs_eigen.cols(); + Eigen::Index expected_dist_coeffs = num_rational_coeffs > 0 ? 8 : 5; if ( 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 ||