feat(ceres_intrinsic_camera_calibrator): ceres-based intrinsic calibr…

…ator (#141) * 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 <[email protected]> * Removed unused file Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * refactor(ceres_intrinsic_camera_calibrator): new directory structure Signed-off-by: amadeuszsz <[email protected]> * chore(ceres_intrinsic_camera_calibrator): update dependencies Signed-off-by: amadeuszsz <[email protected]> * chore(ceres_intrinsic_camera_calibrator): install targets Signed-off-by: amadeuszsz <[email protected]> * chore: updated headers, addressed pre-commit concerns, and fixed some data conditions Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * feat: backported the rational model implementation and other changes (was lost in an experimental branch) Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: ci/cd fixes Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: last ci/cd fixes Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: last-last fix Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * fix(intrinsic_camera_calibrator): empty points array Signed-off-by: amadeuszsz <[email protected]> * fix(ceres_intrinsic_camera_calibrator): missing args & args order Signed-off-by: amadeuszsz <[email protected]> * feat(ceres_intrinsic_camera_calibrator): enable rational coeffs Signed-off-by: amadeuszsz <[email protected]> * ci(pre-commit): autofix * chore: updated parameter check Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> * chore: type fix Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> --------- Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]> Signed-off-by: amadeuszsz <[email protected]> Co-authored-by: amadeuszsz <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
tier4 · Oct 23, 2024 · 1bb940c · 1bb940c
1 parent 84b1ab6
commit 1bb940c
Show file tree

Hide file tree

Showing 10 changed files with 1,384 additions and 1 deletion.
diff --git a/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt b/calibrators/intrinsic_camera_calibrator/ceres_intrinsic_camera_calibrator/CMakeLists.txt
@@ -0,0 +1,77 @@
+
+cmake_minimum_required(VERSION 3.5)
+project(ceres_intrinsic_camera_calibrator)
+
+find_package(rclcpp REQUIRED)
+find_package(rclpy 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)
+
+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}
+  )
+
+# 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})
+
+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}
+)
+
+target_compile_definitions(${PROJECT_NAME}_py
+                           PRIVATE VERSION_INFO=${EXAMPLE_VERSION_INFO})
+install(
+  TARGETS ${PROJECT_NAME}_py
+  DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
+)
+
+ament_export_dependencies(ament_cmake_python)
+
+ament_auto_package()
diff --git a/...or/ceres_intrinsic_camera_calibrator/.tmp → ...s_intrinsic_camera_calibrator/__init__.py b/...or/ceres_intrinsic_camera_calibrator/.tmp → ...s_intrinsic_camera_calibrator/__init__.py
diff --git a/...alibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp b/...alibrator/include/ceres_intrinsic_camera_calibrator/ceres_camera_intrinsics_optimizer.hpp
@@ -0,0 +1,142 @@
+// Copyright 2024 Tier IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef CERES_INTRINSIC_CAMERA_CALIBRATOR__CERES_CAMERA_INTRINSICS_OPTIMIZER_HPP_
+#define CERES_INTRINSIC_CAMERA_CALIBRATOR__CERES_CAMERA_INTRINSICS_OPTIMIZER_HPP_
+
+#include <Eigen/Dense>
+#include <opencv2/core/affine.hpp>
+#include <opencv2/core/core.hpp>
+#include <opencv2/core/mat.hpp>
+
+#include <array>
+#include <map>
+#include <memory>
+#include <set>
+#include <string>
+#include <vector>
+
+class CeresCameraIntrinsicsOptimizer
+{
+public:
+  static constexpr int POSE_OPT_DIM = 7;
+  static constexpr int INTRINSICS_DIM = 12;
+
+  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 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
+   */
+  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_<double> & camera_matrix, const cv::Mat_<double> & distortion_coeffs,
+    const std::vector<std::vector<cv::Point3f>> & object_points,
+    const std::vector<std::vector<cv::Point2f>> & image_points, const std::vector<cv::Mat> & rvecs,
+    const std::vector<cv::Mat> & 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_<double> & camera_matrix, cv::Mat_<double> & distortion_coeffs,
+    std::vector<cv::Mat> & rvecs, std::vector<cv::Mat> & 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_;
+  int rational_distortion_coefficients_;
+  bool verbose_;
+  cv::Mat_<double> camera_matrix_;
+  cv::Mat_<double> distortion_coeffs_;
+
+  std::vector<std::vector<cv::Point3f>> object_points_;
+  std::vector<std::vector<cv::Point2f>> image_points_;
+  std::vector<cv::Mat> rvecs_, tvecs_;
+
+  // Optimization placeholders
+  std::vector<std::array<double, POSE_OPT_DIM>> pose_placeholders_;
+  std::array<double, INTRINSICS_DIM> intrinsics_placeholder_;
+};
+
+#endif  // CERES_INTRINSIC_CAMERA_CALIBRATOR__CERES_CAMERA_INTRINSICS_OPTIMIZER_HPP_