[wpical] Add support for new Apriltags (ID 16-22) (wpilibsuite#7619)

calcmogul · Jan 1, 2025 · ce60bd5 · ce60bd5
1 parent 468a3c6
commit ce60bd5
Show file tree

Hide file tree

Showing 3 changed files with 39 additions and 25 deletions.
diff --git a/wpical/src/main/native/cpp/WPIcal.cpp b/wpical/src/main/native/cpp/WPIcal.cpp
@@ -114,6 +114,8 @@ static void DisplayGui() {
   static int focusedTag = 1;
   static int referenceTag = 1;
 
+  static int maxFRCTag = 22;
+
   static Fieldmap currentCalibrationMap;
   static Fieldmap currentReferenceMap;
 
@@ -187,17 +189,10 @@ static void DisplayGui() {
   ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
   ImGui::InputInt("Pinned Tag", &pinnedTag);
 
-  if (pinnedTag < 1) {
-    pinnedTag = 1;
-  } else if (pinnedTag > 16) {
-    pinnedTag = 16;
-  }
-
   // calibrate button
   if (ImGui::Button("Calibrate!!!")) {
     if (!selected_field_calibration_directory.empty() &&
-        !selected_camera_intrinsics.empty() && !selected_field_map.empty() &&
-        pinnedTag > 0 && pinnedTag <= 16) {
+        !selected_camera_intrinsics.empty() && !selected_field_map.empty()) {
       download_directory_selector =
           std::make_unique<pfd::select_folder>("Select Download Folder", "");
       if (download_directory_selector) {
@@ -216,7 +211,7 @@ static void DisplayGui() {
           showDebug);
 
       if (calibrationOutput == 1) {
-        ImGui::OpenPopup("Fmap Conversion failed");
+        ImGui::OpenPopup("Field Calibration Error");
       } else if (calibrationOutput == 0) {
         std::ifstream caljsonpath(calibration_json_path);
         try {
@@ -227,7 +222,7 @@ static void DisplayGui() {
           ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
           ImGui::OpenPopup("Visualize Calibration");
         } catch (...) {
-          ImGui::OpenPopup("Field Calibration Error");
+          ImGui::OpenPopup("Fmap Conversion Error");
         }
       }
     }
@@ -241,8 +236,10 @@ static void DisplayGui() {
     ImGui::TextWrapped(
         "Some inputs are empty! please enter your camera calibration video, "
         "field map, and field calibration directory");
-  } else if (!(pinnedTag > 0 && pinnedTag <= 16)) {
-    ImGui::TextWrapped("Make sure the pinned tag is a valid april tag (1-16)");
+  } else if (!(pinnedTag > 0 && pinnedTag <= maxFRCTag)) {
+    ImGui::TextWrapped(
+        "The pinned tag is not within the normal range for FRC fields (1-22), "
+        "If you proceed, You may experience a bad calibration.");
   } else {
     ImGui::TextWrapped("Calibration Ready");
   }
@@ -269,7 +266,7 @@ static void DisplayGui() {
     ImGui::EndPopup();
   }
 
-  if (ImGui::BeginPopupModal("Fmap Conversion failed", NULL,
+  if (ImGui::BeginPopupModal("Fmap Conversion Error", NULL,
                              ImGuiWindowFlags_AlwaysAutoResize)) {
     ImGui::TextWrapped(
         "Fmap conversion failed - you can still use the calibration output on "
@@ -480,18 +477,6 @@ static void DisplayGui() {
     ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
     ImGui::InputInt("Reference Tag", &referenceTag);
 
-    if (focusedTag < 1) {
-      focusedTag = 1;
-    } else if (focusedTag > 16) {
-      focusedTag = 16;
-    }
-
-    if (referenceTag < 1) {
-      referenceTag = 1;
-    } else if (referenceTag > 16) {
-      referenceTag = 16;
-    }
-
     if (!calibration_json_path.empty() && !selected_field_map.empty()) {
       std::ifstream calJson(calibration_json_path);
       std::ifstream refJson(selected_field_map);

diff --git a/wpical/src/main/native/cpp/fieldcalibration.cpp b/wpical/src/main/native/cpp/fieldcalibration.cpp
@@ -466,6 +466,19 @@ int fieldcalibration::calibrate(std::string input_dir_path,
     return 1;
   }
 
+  bool pinned_tag_found = false;
+  // Check if pinned tag is in ideal map
+  for (const auto& [tag_id, tag_json] : ideal_map) {
+    if (tag_id == pinned_tag_id) {
+      pinned_tag_found = true;
+      break;
+    }
+  }
+
+  if (!pinned_tag_found) {
+    return 1;
+  }
+
   // Apriltag detector
   apriltag_detector_t* tag_detector = apriltag_detector_create();
   tag_detector->nthreads = 8;

diff --git a/wpical/src/test/native/cpp/test_calibrate.cpp b/wpical/src/test/native/cpp/test_calibrate.cpp
@@ -87,3 +87,19 @@ TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) {
       projectRootPath + "/2024-crescendo.json", 3, false);
   EXPECT_EQ(ret, 1);
 }
+
+TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) {
+  int ret = fieldcalibration::calibrate(
+      projectRootPath + videoLocation, calSavePath,
+      calSavePath + "/cameracalibration.json",
+      projectRootPath + "/2024-crescendo.json", 42, false);
+  EXPECT_EQ(ret, 1);
+}
+
+TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag_Negative) {
+  int ret = fieldcalibration::calibrate(
+      projectRootPath + videoLocation, calSavePath,
+      calSavePath + "/cameracalibration.json",
+      projectRootPath + "/2024-crescendo.json", -1, false);
+  EXPECT_EQ(ret, 1);
+}