commit 2be78e3eae78616657c47e0e25a25d29463d2b05
author Jim Ostrowski <yimmy13@gmail.com> Sat Mar 25 11:57:54 2023 -0700
committer Jim Ostrowski <yimmy13@gmail.com> Mon Oct 23 00:07:02 2023 -0700
tree 34dbd5d2cc19c729b4a54615df781d852393d09d
parent 3fc17d5f0e443a5be20506d40d3e9131b5d48d62

Add AprilTag support to charuco_lib and improve multi-camera cal code

May remove AprilTag from charuco lib later, and use target_mapper flow
For now, this allows us to do multi-camera calibration using AprilTags
instead of Charuco Diamonds

Cleaned up calibrate_multi_cameras to make it flow a bit better

Change-Id: If6f0536f57f8ee28bdad96ce3527dba71cd9be65
Signed-off-by: Jim Ostrowski <yimmy13@gmail.com>

frc971/vision/charuco_lib.cc

diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index b8edcae..d460c2f 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -203,6 +203,11 @@
     marker_length_ = 0.15;
     square_length_ = 0.2;
     dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_250);
+  } else if (target_type_ == TargetType::kAprilTag) {
+    marker_length_ = 0.1016;
+    square_length_ = 0.1524;
+    dictionary_ =
+        cv::aruco::getPredefinedDictionary(cv::aruco::DICT_APRILTAG_16h5);
   } else {
     // Bail out if it's not a supported target
     LOG(FATAL) << "Target type undefined: "
@@ -241,14 +246,15 @@
       LOG(INFO) << "Skipping, due to z value too small: " << result.z();
     } else if (FLAGS_draw_axes == true) {
       result /= result.z();
-      if (target_type_ == TargetType::kCharuco) {
+      if (target_type_ == TargetType::kCharuco ||
+          target_type_ == TargetType::kAprilTag) {
         cv::aruco::drawAxis(rgb_image, calibration_.CameraIntrinsics(),
                             calibration_.CameraDistCoeffs(), rvecs[i], tvecs[i],
-                            0.1);
+                            square_length_);
       } else {
         cv::drawFrameAxes(rgb_image, calibration_.CameraIntrinsics(),
                           calibration_.CameraDistCoeffs(), rvecs[i], tvecs[i],
-                          0.1);
+                          square_length_);
       }
     }
     std::stringstream ss;
@@ -434,7 +440,8 @@
                 << ", not enough marker IDs for charuco board, got "
                 << marker_ids.size() << ", needed " << FLAGS_min_charucos;
       }
-    } else if (target_type_ == TargetType::kAruco) {
+    } else if (target_type_ == TargetType::kAruco ||
+               target_type_ == TargetType::kAprilTag) {
       // estimate pose for arucos doesn't return valid, so marking true
       valid = true;
       std::vector<cv::Vec3d> rvecs, tvecs;
@@ -567,9 +574,11 @@
     return TargetType::kCharuco;
   } else if (str == "charuco_diamond") {
     return TargetType::kCharucoDiamond;
+  } else if (str == "apriltag") {
+    return TargetType::kAprilTag;
   } else {
     LOG(FATAL) << "Unknown target type: " << str
-               << ", expected: aruco|charuco|charuco_diamond";
+               << ", expected: apriltag|aruco|charuco|charuco_diamond";
   }
 }
 
@@ -584,6 +593,9 @@
     case TargetType::kCharucoDiamond:
       os << "charuco_diamond";
       break;
+    case TargetType::kAprilTag:
+      os << "apriltag";
+      break;
   }
   return os;
 }

frc971/vision/charuco_lib.h

diff --git a/frc971/vision/charuco_lib.h b/frc971/vision/charuco_lib.h
index 2f274bb..62923db 100644
--- a/frc971/vision/charuco_lib.h
+++ b/frc971/vision/charuco_lib.h
@@ -83,7 +83,8 @@
 enum class TargetType : uint8_t {
   kAruco = 0,
   kCharuco = 1,
-  kCharucoDiamond = 2
+  kCharucoDiamond = 2,
+  kAprilTag = 3
 };
 
 TargetType TargetTypeFromString(std::string_view str);