aruco calibration is in beta

Author: vdovetzi

hand_eye_calibration/include/hand_eye_calibration/calibrate_helpers.hpp

@@ -495,91 +495,8 @@ inline void findTarget2Cam(CalibrationPattern &pattern, CalibrationData &data) {
   const rclcpp::Logger &logger = pattern.getLogger();
   switch (pattern.getPatternName()) {
   case PatternOption::ARUCO: {
-    std::vector<std::vector<cv::Point2f>> imagePoints;
-    std::vector<std::vector<cv::Point3f>> objectPoints;
-    cv::Mat image;
-    cv::Size imageSize;
-
-    std::unordered_set<size_t> &rejectedImages = data.rejectedImages;
-
-    const size_t imageNum = *countImages(pattern.getDatasetPath());
-    const fs::path &datasetPath = pattern.getDatasetPath();
-
-    imagePoints.reserve(imageNum);
-    objectPoints.reserve(imageNum);
-
-    RCLCPP_INFO(logger,
-                "Press any key to go to the next image. Press 'd' to reject "
-                "the image");
-
-    for (size_t i = 0; i < imageNum; ++i) {
-      const std::string filename = std::format("{}.png", i);
-#if CV_VERSION_MAJOR == 4 && CV_VERSION_MINOR <= 9
-      image = cv::imread(datasetPath / IMG_FOLDERNAME / filename);
-#else
-      cv::imread(datasetPath / IMG_FOLDERNAME / filename, image);
-#endif
-
-      if (imageSize.empty()) {
-        imageSize = image.size();
-      }
-
-      pattern.detectOn(image);
-      const std::vector<cv::Point2f> &corners = pattern.getImgPoints();
-
-      if (corners.empty()) {
-        RCLCPP_WARN(logger, "Aruco marker not found in %s", filename.c_str());
-        rejectedImages.insert(i);
-        continue;
-      }
-
-      cv::aruco::drawDetectedMarkers(
-          image, std::vector<std::vector<cv::Point2f>>{corners});
-      cv::imshow(filename, image);
-      int32_t key = cv::waitKey(0);
-      if (key == 'd') {
-        RCLCPP_WARN(logger, "Image %s is rejected", filename.c_str());
-        rejectedImages.insert(i);
-        cv::destroyWindow(filename);
-        continue;
-      }
-
-      cv::destroyWindow(filename);
-
-      imagePoints.emplace_back(corners);
-      objectPoints.emplace_back(pattern.getObjPoints());
-    }
-
-    cv::destroyAllWindows();
-
-    if (imagePoints.size() < 3) {
-      throw std::runtime_error("Error: need more images. Need at least 3");
-    }
-
-    std::vector<cv::Mat> rvecs;
-
-    std::cout << std::format(
-        "Calibrating camera with {} valid images. It may take some time...\n",
-        imagePoints.size());
-
-    const double rms = cv::calibrateCamera(
-        objectPoints, imagePoints, imageSize, data.K, data.D, rvecs,
-        data.t_target2cam, 0,
-        cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30,
-                         1e-6));
-
-    std::vector<cv::Mat> &Rs = data.R_target2cam;
-    Rs.reserve(rvecs.size());
-    for (const cv::Mat &rvec : rvecs) {
-      cv::Mat R;
-      cv::Rodrigues(rvec, R);
-      Rs.emplace_back(R);
-    }
-
-    RCLCPP_DEBUG(logger,
-                 "Camera calibration successfull! Reprojection error is %f",
-                 rms);
-    break;
+    // TODO: add aruco routine
+    throw std::runtime_error("Not implemented!");
   }
   case PatternOption::CHARUCO: {
     // TODO: add charuco calibration routine