From 96425c4437c211dd8acce83bba45cef3b4c4644e Mon Sep 17 00:00:00 2001 From: Kenzo Lobos-Tsunekawa Date: Wed, 1 Nov 2023 16:29:03 +0900 Subject: [PATCH] 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; +}