feat(tag_based_pnp_calibrator): choose between sqpnp and iterative au…

…tomatically (#222)

faet: changed sqpnp to the best between sqpnp and the iterative for pnp based calibration. automatically choose between observations and hypotheses when returning the final calibration

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

calibrators/tag_based_pnp_calibrator/CMakeLists.txt

@@ -18,6 +18,7 @@ ament_auto_add_executable(tag_based_pnp_calibrator
   src/tag_based_pnp_calibrator.cpp
   src/tag_calibrator_visualizer.cpp
   src/main.cpp
+  src/math.cpp
 )
 
 target_link_libraries(tag_based_pnp_calibrator

calibrators/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/calibration_estimator.hpp

@@ -93,7 +93,9 @@ class CalibrationEstimator
   double getNewHypothesisDistance() const;
   double getCalibrationCoveragePercentage() const;
   int getCurrentCalibrationPairsNumber() const;
-  double getCrossValidationReprojectionError() const;
+  double computeCrossValidationReprojectionError(
+    const std::vector<cv::Point3d> & object_points, const std::vector<cv::Point2d> & image_points);
+  std::tuple<bool, double> getCrossValidationReprojectionError();
   int getConvergencePairNumber() const;
 
 private:
@@ -104,14 +106,12 @@ class CalibrationEstimator
     bool use_estimated);
 
   std::tuple<bool, cv::Matx31d, cv::Matx33d> calibrate(
-    const std::vector<cv::Point3d> & object_points, const std::vector<cv::Point2d> & image_points);
+    const std::vector<cv::Point3d> & object_points,
+    const std::vector<cv::Point2d> & image_points) const;
 
   tf2::Transform toTf2(
     const cv::Matx31d & translation_vector, const cv::Matx33d & rotation_matrix) const;
 
-  void computeCrossValidationReprojectionError(
-    const std::vector<cv::Point3d> & object_points, const std::vector<cv::Point2d> & image_points);
-
   // Parameters
 
   // Calibration convergence criteria
@@ -160,7 +160,6 @@ class CalibrationEstimator
   std::vector<std::shared_ptr<tier4_tag_utils::ApriltagHypothesis>> converged_apriltag_hypotheses_;
 
   // Output
-  double crossvalidation_reprojection_error_;
   bool valid_;
   cv::Matx33d hypothesis_rotation_matrix_, observation_rotation_matrix_;
   cv::Matx31d hypothesis_translation_vector_, observation_translation_vector_;

calibrators/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/math.hpp

@@ -0,0 +1,36 @@
+// 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 TAG_BASED_PNP_CALIBRATOR__MATH_HPP_
+#define TAG_BASED_PNP_CALIBRATOR__MATH_HPP_
+
+#include <opencv2/core.hpp>
+#include <opencv2/opencv.hpp>
+
+#include <vector>
+
+double getReprojectionError(
+  const std::vector<cv::Point2d> & points1, const std::vector<cv::Point2d> & points2);
+
+double getReprojectionError(
+  const std::vector<cv::Point3d> & object_points, const std::vector<cv::Point2d> & image_points,
+  const cv::Matx31d & translation_vector, const cv::Matx33d & rotation_matrix,
+  const cv::Matx33d & camera_matrix, const cv::Mat_<double> & distortion_coeffs);
+
+double getReprojectionError(
+  const std::vector<cv::Point3d> & object_points, const std::vector<cv::Point2d> & image_points,
+  const cv::Matx31d & translation_vector, const cv::Matx31d & rotation_vector,
+  const cv::Matx33d & camera_matrix, const cv::Mat_<double> & distortion_coeffs);
+
+#endif  // TAG_BASED_PNP_CALIBRATOR__MATH_HPP_

calibrators/tag_based_pnp_calibrator/include/tag_based_pnp_calibrator/tag_calibrator_visualizer.hpp

@@ -58,7 +58,7 @@ class TagCalibratorVisualizer
   void setBaseLidarTransform(
     const cv::Matx31d & translation_vector, const cv::Matx33d & rotation_matrix);
 
-  void drawCalibrationStatus(const CalibrationEstimator & estimation, rclcpp::Time & stamp);
+  void drawCalibrationStatus(CalibrationEstimator & estimation, rclcpp::Time & stamp);
 
   void drawAprilTagDetections(
     const apriltag_msgs::msg::AprilTagDetectionArray::SharedPtr & apriltag_detection_array);
@@ -86,7 +86,7 @@ class TagCalibratorVisualizer
   void drawCalibrationZone(
     rclcpp::Time & stamp, visualization_msgs::msg::MarkerArray & marker_array);
   void drawCalibrationStatusText(
-    const CalibrationEstimator & estimator, rclcpp::Time & stamp,
+    CalibrationEstimator & estimator, rclcpp::Time & stamp,
     visualization_msgs::msg::MarkerArray & marker_array);
 
   std::vector<cv::Point3d> get3dpoints(apriltag_msgs::msg::AprilTagDetection & detection);

calibrators/tag_based_pnp_calibrator/src/calibration_estimator.cpp

@@ -18,6 +18,7 @@
 #include <opencv2/imgproc.hpp>
 #include <tag_based_pnp_calibrator/brute_force_matcher.hpp>
 #include <tag_based_pnp_calibrator/calibration_estimator.hpp>
+#include <tag_based_pnp_calibrator/math.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
 
 #include <pcl/point_types.h>
@@ -50,7 +51,6 @@ CalibrationEstimator::CalibrationEstimator()
   apriltag_new_hypothesis_translation_(10.0),
   apriltag_process_noise_translation_(0.5),
   apriltag_measurement_noise_translation_(2.0),
-  crossvalidation_reprojection_error_(std::numeric_limits<double>::infinity()),
   valid_(false)
 {
 }
@@ -467,13 +467,12 @@ bool CalibrationEstimator::calibrate()
     hypothesis_rotation_matrix_ = hypothesis_rotation_matrix;
   }
 
-  computeCrossValidationReprojectionError(estimated_object_points, estimated_image_points);
-
   return status;
 }
 
 std::tuple<bool, cv::Matx31d, cv::Matx33d> CalibrationEstimator::calibrate(
-  const std::vector<cv::Point3d> & object_points, const std::vector<cv::Point2d> & image_points)
+  const std::vector<cv::Point3d> & object_points,
+  const std::vector<cv::Point2d> & image_points) const
 {
   cv::Matx31d translation_vector;
   cv::Matx33d rotation_matrix;
@@ -487,19 +486,36 @@ std::tuple<bool, cv::Matx31d, cv::Matx33d> CalibrationEstimator::calibrate(
   auto camera_intrinsics = pinhole_camera_model_.intrinsicMatrix();
   auto distortion_coeffs = pinhole_camera_model_.distortionCoeffs();
 
-  cv::Mat rvec, tvec;
+  cv::Matx31d sq_tvec, iterative_tvec;
+  cv::Matx31d sq_rvec, iterative_rvec;
 
-  bool success = cv::solvePnP(
-    object_points, image_points, camera_intrinsics, distortion_coeffs, rvec, tvec, false,
+  bool sq_success = cv::solvePnP(
+    object_points, image_points, camera_intrinsics, distortion_coeffs, sq_rvec, sq_tvec, false,
     cv::SOLVEPNP_SQPNP);
 
-  if (!success) {
+  bool iterative_success = cv::solvePnP(
+    object_points, image_points, camera_intrinsics, distortion_coeffs, iterative_rvec,
+    iterative_tvec, false, cv::SOLVEPNP_ITERATIVE);
+
+  if (!sq_success && !iterative_success) {
     RCLCPP_ERROR(rclcpp::get_logger("calibration_estimator"), "PNP failed");
     return std::tuple<bool, cv::Matx31d, cv::Matx33d>(false, translation_vector, rotation_matrix);
   }
 
-  translation_vector = tvec;
-  cv::Rodrigues(rvec, rotation_matrix);
+  double sq_error = getReprojectionError(
+    object_points, image_points, sq_tvec, sq_rvec, camera_intrinsics, distortion_coeffs);
+
+  double iterative_error = getReprojectionError(
+    object_points, image_points, iterative_tvec, iterative_rvec, camera_intrinsics,
+    distortion_coeffs);
+
+  if (sq_error < iterative_error) {
+    translation_vector = sq_tvec;
+    cv::Rodrigues(sq_rvec, rotation_matrix);
+  } else {
+    translation_vector = iterative_tvec;
+    cv::Rodrigues(iterative_rvec, rotation_matrix);
+  }
 
   return std::tuple<bool, cv::Matx31d, cv::Matx33d>(true, translation_vector, rotation_matrix);
 }
@@ -519,7 +535,7 @@ tf2::Transform CalibrationEstimator::toTf2(
   return tf2::Transform(tf2_rotation_matrix, tf2_trans);
 }
 
-void CalibrationEstimator::computeCrossValidationReprojectionError(
+double CalibrationEstimator::computeCrossValidationReprojectionError(
   const std::vector<cv::Point3d> & object_points, const std::vector<cv::Point2d> & image_points)
 {
   // Iterate a number of times
@@ -535,9 +551,12 @@ void CalibrationEstimator::computeCrossValidationReprojectionError(
   std::size_t training_size = crossvalidation_training_ratio_ * object_points.size();
 
   if (static_cast<int>(training_size) < min_pnp_pairs_) {
-    return;
+    return std::numeric_limits<double>::infinity();
   }
 
+  const auto & camera_matrix = pinhole_camera_model_.intrinsicMatrix();
+  const auto & distortion_coeffs = pinhole_camera_model_.distortionCoeffs();
+
   double error = 0.0;
 
   for (int trial = 0; trial < trials; trial++) {
@@ -561,25 +580,35 @@ void CalibrationEstimator::computeCrossValidationReprojectionError(
     [[maybe_unused]] auto [status, iter_translation_vector, iter_rotation_matrix] =
       calibrate(training_object_points, training_image_points);
 
+    double reprojection_error = getReprojectionError(
+      test_object_points, test_image_points, iter_translation_vector, iter_rotation_matrix,
+      camera_matrix, distortion_coeffs);
+
     cv::Matx31d iter_rvec;
     cv::Rodrigues(iter_rotation_matrix, iter_rvec);
 
-    cv::projectPoints(
-      test_object_points, iter_rvec, iter_translation_vector,
-      pinhole_camera_model_.intrinsicMatrix(), pinhole_camera_model_.distortionCoeffs(),
-      eval_projected_points);
-
-    double reprojection_error = 0.0;
-    for (std::size_t i = 0; i < test_image_points.size(); i++) {
-      double dist = cv::norm(test_image_points[i] - eval_projected_points[i]);
-      reprojection_error += dist;
-    }
-
-    reprojection_error /= test_image_points.size();
     error += reprojection_error;
   }
 
-  crossvalidation_reprojection_error_ = error / trials;
+  return error / trials;
+}
+
+std::tuple<bool, double> CalibrationEstimator::getCrossValidationReprojectionError()
+{
+  // Return the current or filtered pose depending on which fits their set better
+  auto [current_object_points, current_image_points] = getCalibrationPoints(false);
+  auto [filtered_object_points, filtered_image_points] = getCalibrationPoints(true);
+
+  double current_crossval_reprojection_error =
+    computeCrossValidationReprojectionError(current_object_points, current_image_points);
+  double filtered_crossval_reprojection_error =
+    computeCrossValidationReprojectionError(filtered_object_points, filtered_image_points);
+
+  if (current_crossval_reprojection_error < filtered_crossval_reprojection_error) {
+    return std::tuple<bool, double>(false, current_crossval_reprojection_error);
+  } else {
+    return std::tuple<bool, double>(true, filtered_crossval_reprojection_error);
+  }
 }
 
 bool CalibrationEstimator::converged() const
@@ -770,9 +799,4 @@ int CalibrationEstimator::getCurrentCalibrationPairsNumber() const
   return converged_lidartag_hypotheses_.size();
 }
 
-double CalibrationEstimator::getCrossValidationReprojectionError() const
-{
-  return crossvalidation_reprojection_error_;
-}
-
 int CalibrationEstimator::getConvergencePairNumber() const { return convergence_min_pairs_; }

calibrators/tag_based_pnp_calibrator/src/math.cpp

@@ -0,0 +1,61 @@
+// 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.
+
+#include <opencv2/core.hpp>
+#include <opencv2/opencv.hpp>
+#include <tag_based_pnp_calibrator/math.hpp>
+
+#include <vector>
+
+double getReprojectionError(
+  const std::vector<cv::Point2d> & points1, const std::vector<cv::Point2d> & points2)
+{
+  double error = 0.0;
+
+  for (std::size_t i = 0; i < points1.size(); i++) {
+    error += cv::norm(points1[i] - points2[i]);
+  }
+
+  return error / points1.size();
+}
+
+double getReprojectionError(
+  const std::vector<cv::Point3d> & object_points, const std::vector<cv::Point2d> & image_points,
+  const cv::Matx31d & translation_vector, const cv::Matx33d & rotation_matrix,
+  const cv::Matx33d & camera_matrix, const cv::Mat_<double> & distortion_coeffs)
+{
+  std::vector<cv::Point2d> projected_points;
+
+  cv::Matx31d rvec;
+  cv::Rodrigues(rotation_matrix, rvec);
+
+  cv::projectPoints(
+    object_points, rvec, translation_vector, camera_matrix, distortion_coeffs, projected_points);
+
+  return getReprojectionError(image_points, projected_points);
+}
+
+double getReprojectionError(
+  const std::vector<cv::Point3d> & object_points, const std::vector<cv::Point2d> & image_points,
+  const cv::Matx31d & translation_vector, const cv::Matx31d & rotation_vector,
+  const cv::Matx33d & camera_matrix, const cv::Mat_<double> & distortion_coeffs)
+{
+  std::vector<cv::Point2d> projected_points;
+
+  cv::projectPoints(
+    object_points, rotation_vector, translation_vector, camera_matrix, distortion_coeffs,
+    projected_points);
+
+  return getReprojectionError(image_points, projected_points);
+}