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]>

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
+#)

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 <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 = 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_<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_;
+  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_

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 <Eigen/Core>
+#include <opencv2/core.hpp>
+
+#include <ceres/autodiff_cost_function.h>
+#include <ceres/ceres.h>
+#include <ceres/rotation.h>
+
+struct ReprojectionResidual
+{
+  template <class T>
+  using Vector3 = Eigen::Matrix<T, 3, 1>;
+
+  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 <typename T>
+  bool operator()(
+    const T * const camera_intrinsics, const T * const board_pose, T * residuals) const
+  {
+    const Eigen::Map<const Vector3<T>> board_translation(&board_pose[TRANSLATION_X_INDEX]);
+
+    Eigen::Quaternion<T> 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<T> 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<int>(use_tangential_distortion);
+    ceres::CostFunction * cost_function = nullptr;
+
+    switch (distortion_coefficients) {
+      case 0:
+        cost_function =
+          new ceres::AutoDiffCostFunction<ReprojectionResidual, RESIDUAL_DIM, 4, POSE_OPT_DIM>(f);
+        break;
+      case 1:
+        cost_function =
+          new ceres::AutoDiffCostFunction<ReprojectionResidual, RESIDUAL_DIM, 5, POSE_OPT_DIM>(f);
+        break;
+      case 2:
+        cost_function =
+          new ceres::AutoDiffCostFunction<ReprojectionResidual, RESIDUAL_DIM, 6, POSE_OPT_DIM>(f);
+        break;
+      case 3:
+        cost_function =
+          new ceres::AutoDiffCostFunction<ReprojectionResidual, RESIDUAL_DIM, 7, POSE_OPT_DIM>(f);
+        break;
+      case 4:
+        cost_function =
+          new ceres::AutoDiffCostFunction<ReprojectionResidual, RESIDUAL_DIM, 8, POSE_OPT_DIM>(f);
+        break;
+      case 5:
+        cost_function =
+          new ceres::AutoDiffCostFunction<ReprojectionResidual, RESIDUAL_DIM, 9, POSE_OPT_DIM>(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_