[wpical] Add JSON Combiner (#7640)

This new feature allows users to combine multiple Apriltag layouts. This can be useful for fields where the apriltags are split into two or more sections: (red/blue side, grouped together by task, etc.)
2026-06-19 00:41:43 +00:00 · 2025-01-10 02:30:17 -05:00
parent e37c35746a
commit 6125227836
7 changed files with 373 additions and 186 deletions
--- a/wpical/src/main/native/cpp/WPIcal.cpp
+++ b/wpical/src/main/native/cpp/WPIcal.cpp
@@ -9,12 +9,14 @@

 #include <fstream>
 #include <iostream>
+#include <map>
 #include <memory>
 #include <numbers>
 #include <string>
 #include <vector>

 #include <GLFW/glfw3.h>
+#include <fmt/format.h>
 #include <imgui.h>
 #include <portable-file-dialogs.h>
 #include <tagpose.h>
@@ -58,6 +60,112 @@ void drawCheck() {
  ImGui::NewLine();
 }

+void processFileSelector(std::unique_ptr<pfd::open_file>& selector,
+                         std::string& selected_file) {
+  if (selector && selector->ready(0)) {
+    auto selectedFiles = selector->result();
+    if (!selectedFiles.empty()) {
+      selected_file = selectedFiles[0];
+    }
+    selector.reset();
+  }
+}
+
+void processFilesSelector(std::unique_ptr<pfd::open_file>& selector,
+                          std::vector<std::string>& selected_files) {
+  if (selector && selector->ready(0)) {
+    auto selectedFiles = selector->result();
+    if (!selectedFiles.empty()) {
+      selected_files = selectedFiles;
+    }
+    selector.reset();
+  }
+}
+
+void processDirectorySelector(std::unique_ptr<pfd::select_folder>& selector,
+                              std::string& selected_directory) {
+  if (selector && selector->ready(0)) {
+    auto selectedFiles = selector->result();
+    if (!selectedFiles.empty()) {
+      selected_directory = selectedFiles;
+    }
+    selector.reset();
+  }
+}
+
+void openFileButton(const char* text, std::string& selected_file,
+                    std::unique_ptr<pfd::open_file>& selector,
+                    const std::string& file_type,
+                    const std::string& file_extensions) {
+  if (ImGui::Button(text)) {
+    selector = std::make_unique<pfd::open_file>(
+        "Select File", "", std::vector<std::string>{file_type, file_extensions},
+        pfd::opt::none);
+  }
+}
+
+void openFilesButton(const char* text, std::vector<std::string>& selected_files,
+                     std::unique_ptr<pfd::open_file>& selector,
+                     const std::string& file_type,
+                     const std::string& file_extensions) {
+  if (ImGui::Button(text)) {
+    selector = std::make_unique<pfd::open_file>(
+        "Select File", "", std::vector<std::string>{file_type, file_extensions},
+        pfd::opt::multiselect);
+  }
+}
+
+void openDirectoryButton(const char* text,
+                         std::unique_ptr<pfd::select_folder>& selector,
+                         std::string& selected_directory) {
+  if (ImGui::Button(text)) {
+    selector = std::make_unique<pfd::select_folder>("Select Directory", "");
+  }
+}
+
+std::string getFileName(std::string path) {
+  size_t lastSlash = path.find_last_of("/\\");
+  size_t lastDot = path.find_last_of(".");
+  return path.substr(lastSlash + 1, lastDot - lastSlash - 1);
+}
+
+static bool EmitEntryTarget(int tag_id, std::string& file) {
+  if (!file.empty()) {
+    auto text = fmt::format("{}: {}", tag_id, file);
+    ImGui::TextUnformatted(text.c_str());
+  } else {
+    ImGui::Text("Tag ID %i: <none (DROP HERE)>", tag_id);
+  }
+  bool rv = false;
+  if (ImGui::BeginDragDropTarget()) {
+    if (const ImGuiPayload* payload =
+            ImGui::AcceptDragDropPayload("FieldCalibration")) {
+      file = *(std::string*)payload->Data;
+      rv = true;
+    }
+    ImGui::EndDragDropTarget();
+  }
+  return rv;
+}
+
+void saveCalibration(wpi::json& field, std::string& output_directory,
+                     std::string output_name, bool& isCalibrating) {
+  if (!field.empty() && !output_directory.empty()) {
+    std::cout << "Saving calibration to " << output_directory << std::endl;
+    std::ofstream out(output_directory + "/" + output_name + ".json");
+    out << field.dump(4);
+    out.close();
+
+    std::ofstream fmap(output_directory + "/" + output_name + ".fmap");
+    fmap << fmap::convertfmap(field).dump(4);
+    fmap.close();
+
+    field.clear();
+    output_directory.clear();
+    isCalibrating = false;
+  }
+}
+
 static void DisplayGui() {
  ImGui::GetStyle().WindowRounding = 0;

@@ -82,19 +190,28 @@ static void DisplayGui() {
  ImGui::EndMenuBar();

  static std::unique_ptr<pfd::open_file> camera_intrinsics_selector;
-  static std::string selected_camera_intrinsics;
-
  static std::unique_ptr<pfd::open_file> field_map_selector;
-  static std::string selected_field_map;
+  static std::unique_ptr<pfd::open_file> output_calibration_json_selector;
+  static std::unique_ptr<pfd::open_file> combination_calibrations_selector;

  static std::unique_ptr<pfd::select_folder>
      field_calibration_directory_selector;
-  static std::string selected_field_calibration_directory;
-
  static std::unique_ptr<pfd::select_folder> download_directory_selector;
-  static std::string selected_download_directory;

-  static std::string calibration_json_path;
+  static wpi::json field_calibration_json;
+  static wpi::json field_combination_json;
+
+  static std::string selected_camera_intrinsics;
+  static std::string selected_field_map;
+  static std::string selected_field_calibration_directory;
+  static std::string selected_download_directory;
+  static std::string output_calibration_json_path;
+  static std::vector<std::string> selected_combination_calibrations;
+
+  static std::map<int, std::string> combiner_map;
+  static int current_combiner_tag_id = 0;
+
+  static bool isCalibrating = false;

  cameracalibration::CameraModel cameraModel = {
      .intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
@@ -118,13 +235,12 @@ static void DisplayGui() {

  static Fieldmap currentCalibrationMap;
  static Fieldmap currentReferenceMap;
+  static Fieldmap currentCombinerMap;

  // camera matrix selector button
-  if (ImGui::Button("Upload Camera Intrinsics")) {
-    camera_intrinsics_selector = std::make_unique<pfd::open_file>(
-        "Select Camera Intrinsics JSON", "",
-        std::vector<std::string>{"JSON", "*.json"}, pfd::opt::none);
-  }
+  openFileButton("Select Camera Intrinsics JSON", selected_camera_intrinsics,
+                 camera_intrinsics_selector, "JSON Files", "*.json");
+  processFileSelector(camera_intrinsics_selector, selected_camera_intrinsics);

  ImGui::SameLine();
  ImGui::Text("Or");
@@ -136,50 +252,25 @@ static void DisplayGui() {
    ImGui::OpenPopup("Camera Calibration");
  }

-  if (camera_intrinsics_selector) {
-    auto selectedFiles = camera_intrinsics_selector->result();
-    if (!selectedFiles.empty()) {
-      selected_camera_intrinsics = selectedFiles[0];
-    }
-    camera_intrinsics_selector.reset();
-  }
-
  if (!selected_camera_intrinsics.empty()) {
    drawCheck();
  }

  // field json selector button
-  if (ImGui::Button("Select Field Map JSON")) {
-    field_map_selector = std::make_unique<pfd::open_file>(
-        "Select Json File", "",
-        std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none);
-  }
-
-  if (field_map_selector) {
-    auto selectedFiles = field_map_selector->result();
-    if (!selectedFiles.empty()) {
-      selected_field_map = selectedFiles[0];
-    }
-    field_map_selector.reset();
-  }
+  openFileButton("Select Field Map JSON", selected_field_map,
+                 field_map_selector, "JSON Files", "*.json");
+  processFileSelector(field_map_selector, selected_field_map);

  if (!selected_field_map.empty()) {
    drawCheck();
  }

  // field calibration directory selector button
-  if (ImGui::Button("Select Field Calibration Folder")) {
-    field_calibration_directory_selector = std::make_unique<pfd::select_folder>(
-        "Select Field Calibration Folder", "");
-  }
-
-  if (field_calibration_directory_selector) {
-    auto selectedFiles = field_calibration_directory_selector->result();
-    if (!selectedFiles.empty()) {
-      selected_field_calibration_directory = selectedFiles;
-    }
-    field_calibration_directory_selector.reset();
-  }
+  openDirectoryButton("Select Field Calibration Directory",
+                      field_calibration_directory_selector,
+                      selected_field_calibration_directory);
+  processDirectorySelector(field_calibration_directory_selector,
+                           selected_field_calibration_directory);

  if (!selected_field_calibration_directory.empty()) {
    drawCheck();
@@ -191,46 +282,35 @@ static void DisplayGui() {

  // calibrate button
  if (ImGui::Button("Calibrate!!!")) {
-    if (!selected_field_calibration_directory.empty() &&
-        !selected_camera_intrinsics.empty() && !selected_field_map.empty()) {
+    int calibrationOutput = fieldcalibration::calibrate(
+        selected_field_calibration_directory.c_str(), field_calibration_json,
+        selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag,
+        showDebug);
+
+    if (calibrationOutput == 1) {
+      ImGui::OpenPopup("Field Calibration Error");
+    }
+
+    if (selected_download_directory.empty() &&
+        !field_calibration_json.empty() && !download_directory_selector) {
      download_directory_selector =
          std::make_unique<pfd::select_folder>("Select Download Folder", "");
-      if (download_directory_selector) {
-        auto selectedFiles = download_directory_selector->result();
-        if (!selectedFiles.empty()) {
-          selected_download_directory = selectedFiles;
-        }
-        download_directory_selector.reset();
-      }
-
-      calibration_json_path = selected_download_directory + "/output.json";
-
-      int calibrationOutput = fieldcalibration::calibrate(
-          selected_field_calibration_directory.c_str(), calibration_json_path,
-          selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag,
-          showDebug);
-
-      if (calibrationOutput == 1) {
-        ImGui::OpenPopup("Field Calibration Error");
-      } else if (calibrationOutput == 0) {
-        std::ifstream caljsonpath(calibration_json_path);
-        try {
-          wpi::json fmap = fmap::convertfmap(wpi::json::parse(caljsonpath));
-          std::ofstream out(selected_download_directory + "/output.fmap");
-          out << fmap.dump(4);
-          out.close();
-          ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
-          ImGui::OpenPopup("Visualize Calibration");
-        } catch (...) {
-          ImGui::OpenPopup("Fmap Conversion Error");
-        }
-      }
    }
  }
+
+  processDirectorySelector(download_directory_selector,
+                           selected_download_directory);
+  saveCalibration(field_calibration_json, selected_download_directory,
+                  "field_calibration", isCalibrating);
+
  if (ImGui::Button("Visualize")) {
    ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
    ImGui::OpenPopup("Visualize Calibration");
  }
+  if (ImGui::Button("Combine Calibrations")) {
+    ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
+    ImGui::OpenPopup("Combine Calibrations");
+  }
  if (selected_field_calibration_directory.empty() ||
      selected_camera_intrinsics.empty() || selected_field_map.empty()) {
    ImGui::TextWrapped(
@@ -320,21 +400,11 @@ static void DisplayGui() {
    }

    if (mrcal) {
-      if (ImGui::Button("Select Camera Calibration Video")) {
-        camera_intrinsics_selector = std::make_unique<pfd::open_file>(
-            "Select Camera Calibration Video", "",
-            std::vector<std::string>{"Video Files",
-                                     "*.mp4 *.mov *.m4v *.mkv *.avi"},
-            pfd::opt::none);
-      }
-
-      if (camera_intrinsics_selector) {
-        auto selectedFiles = camera_intrinsics_selector->result();
-        if (!selectedFiles.empty()) {
-          selected_camera_intrinsics = selectedFiles[0];
-        }
-        camera_intrinsics_selector.reset();
-      }
+      openFileButton("Select Camera Calibration Video",
+                     selected_camera_intrinsics, camera_intrinsics_selector,
+                     "Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi");
+      processFileSelector(camera_intrinsics_selector,
+                          selected_camera_intrinsics);

      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
      ImGui::InputDouble("Square Width (in)", &squareWidth);
@@ -379,21 +449,11 @@ static void DisplayGui() {
        }
      }
    } else {
-      if (ImGui::Button("Select Camera Calibration Video")) {
-        camera_intrinsics_selector = std::make_unique<pfd::open_file>(
-            "Select Camera Calibration Video", "",
-            std::vector<std::string>{"Video Files",
-                                     "*.mp4 *.mov *.m4v *.mkv *.avi"},
-            pfd::opt::none);
-      }
-
-      if (camera_intrinsics_selector) {
-        auto selectedFiles = camera_intrinsics_selector->result();
-        if (!selectedFiles.empty()) {
-          selected_camera_intrinsics = selectedFiles[0];
-        }
-        camera_intrinsics_selector.reset();
-      }
+      openFileButton("Select Camera Calibration Video",
+                     selected_camera_intrinsics, camera_intrinsics_selector,
+                     "Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi");
+      processFileSelector(camera_intrinsics_selector,
+                          selected_camera_intrinsics);

      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
      ImGui::InputDouble("Square Width (in)", &squareWidth);
@@ -446,26 +506,19 @@ static void DisplayGui() {
  // visualize calibration popup
  if (ImGui::BeginPopupModal("Visualize Calibration", NULL,
                             ImGuiWindowFlags_AlwaysAutoResize)) {
-    if (ImGui::Button("Load Calibrated Field")) {
-      calibration_json_path =
-          std::make_unique<pfd::open_file>(
-              "Select Json File", "",
-              std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
-              ->result()[0];
-    }
+    openFileButton("Select Calibration JSON", output_calibration_json_path,
+                   output_calibration_json_selector, "JSON", "*.json");
+    processFileSelector(output_calibration_json_selector,
+                        output_calibration_json_path);

-    if (!calibration_json_path.empty()) {
+    if (!output_calibration_json_path.empty()) {
      ImGui::SameLine();
      drawCheck();
    }

-    if (ImGui::Button("Load Reference Field")) {
-      selected_field_map =
-          std::make_unique<pfd::open_file>(
-              "Select Json File", "",
-              std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
-              ->result()[0];
-    }
+    openFileButton("Select Ideal Field Map", selected_field_map,
+                   field_map_selector, "JSON", "*.json");
+    processFileSelector(field_map_selector, selected_field_map);

    if (!selected_field_map.empty()) {
      ImGui::SameLine();
@@ -477,58 +530,76 @@ static void DisplayGui() {
    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
    ImGui::InputInt("Reference Tag", &referenceTag);

-    if (!calibration_json_path.empty() && !selected_field_map.empty()) {
-      std::ifstream calJson(calibration_json_path);
+    if (!output_calibration_json_path.empty() && !selected_field_map.empty()) {
+      std::ifstream calJson(output_calibration_json_path);
      std::ifstream refJson(selected_field_map);

      currentCalibrationMap = Fieldmap(wpi::json::parse(calJson));
      currentReferenceMap = Fieldmap(wpi::json::parse(refJson));

-      double xDiff = currentReferenceMap.getTag(focusedTag).xPos -
-                     currentCalibrationMap.getTag(focusedTag).xPos;
-      double yDiff = currentReferenceMap.getTag(focusedTag).yPos -
-                     currentCalibrationMap.getTag(focusedTag).yPos;
-      double zDiff = currentReferenceMap.getTag(focusedTag).zPos -
-                     currentCalibrationMap.getTag(focusedTag).zPos;
-      double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot -
-                       currentCalibrationMap.getTag(focusedTag).yawRot;
-      double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot -
-                         currentCalibrationMap.getTag(focusedTag).pitchRot;
-      double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot -
-                        currentCalibrationMap.getTag(focusedTag).rollRot;
+      if (currentCalibrationMap.getNumTags() !=
+          currentReferenceMap.getNumTags()) {
+        ImGui::TextWrapped(
+            "The number of tags in the calibration output and the ideal field "
+            "map "
+            "do not match. Please ensure that the calibration output and ideal "
+            "field "
+            "map have the same number of tags.");
+      } else if (currentReferenceMap.hasTag(focusedTag) &&
+                 currentReferenceMap.hasTag(referenceTag)) {
+        double xDiff = currentReferenceMap.getTag(focusedTag).xPos -
+                       currentCalibrationMap.getTag(focusedTag).xPos;
+        double yDiff = currentReferenceMap.getTag(focusedTag).yPos -
+                       currentCalibrationMap.getTag(focusedTag).yPos;
+        double zDiff = currentReferenceMap.getTag(focusedTag).zPos -
+                       currentCalibrationMap.getTag(focusedTag).zPos;
+        double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot -
+                         currentCalibrationMap.getTag(focusedTag).yawRot;
+        double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot -
+                           currentCalibrationMap.getTag(focusedTag).pitchRot;
+        double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot -
+                          currentCalibrationMap.getTag(focusedTag).rollRot;

-      double xRef = currentCalibrationMap.getTag(referenceTag).xPos -
-                    currentCalibrationMap.getTag(focusedTag).xPos;
-      double yRef = currentCalibrationMap.getTag(referenceTag).yPos -
-                    currentCalibrationMap.getTag(focusedTag).yPos;
-      double zRef = currentCalibrationMap.getTag(referenceTag).zPos -
-                    currentCalibrationMap.getTag(focusedTag).zPos;
+        double xRef = currentCalibrationMap.getTag(referenceTag).xPos -
+                      currentCalibrationMap.getTag(focusedTag).xPos;
+        double yRef = currentCalibrationMap.getTag(referenceTag).yPos -
+                      currentCalibrationMap.getTag(focusedTag).yPos;
+        double zRef = currentCalibrationMap.getTag(referenceTag).zPos -
+                      currentCalibrationMap.getTag(focusedTag).zPos;

-      ImGui::TextWrapped("X Difference: %s (m)", std::to_string(xDiff).c_str());
-      ImGui::TextWrapped("Y Difference: %s (m)", std::to_string(yDiff).c_str());
-      ImGui::TextWrapped("Z Difference: %s (m)", std::to_string(zDiff).c_str());
+        ImGui::TextWrapped("X Difference: %s (m)",
+                           std::to_string(xDiff).c_str());
+        ImGui::TextWrapped("Y Difference: %s (m)",
+                           std::to_string(yDiff).c_str());
+        ImGui::TextWrapped("Z Difference: %s (m)",
+                           std::to_string(zDiff).c_str());

-      ImGui::TextWrapped(
-          "Yaw Difference %s°",
-          std::to_string(
-              Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi)))
-              .c_str());
-      ImGui::TextWrapped(
-          "Pitch Difference %s°",
-          std::to_string(
-              Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi)))
-              .c_str());
-      ImGui::TextWrapped(
-          "Roll Difference %s°",
-          std::to_string(
-              Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi)))
-              .c_str());
+        ImGui::TextWrapped(
+            "Yaw Difference %s°",
+            std::to_string(
+                Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi)))
+                .c_str());
+        ImGui::TextWrapped(
+            "Pitch Difference %s°",
+            std::to_string(
+                Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi)))
+                .c_str());
+        ImGui::TextWrapped(
+            "Roll Difference %s°",
+            std::to_string(
+                Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi)))
+                .c_str());

-      ImGui::NewLine();
+        ImGui::NewLine();

-      ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str());
-      ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str());
-      ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str());
+        ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str());
+        ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str());
+        ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str());
+      } else {
+        ImGui::TextWrapped(
+            "Please select tags that are in the ideal field map and "
+            "calibration map");
+      }
    }

    if (ImGui::Button("Close")) {
@@ -537,6 +608,78 @@ static void DisplayGui() {
    ImGui::EndPopup();
  }

+  if (ImGui::BeginPopupModal("Combine Calibrations", NULL,
+                             ImGuiWindowFlags_AlwaysAutoResize)) {
+    openFileButton("Select Ideal Map", selected_field_map, field_map_selector,
+                   "JSON", "*.json");
+    processFileSelector(field_map_selector, selected_field_map);
+    if (!selected_field_map.empty()) {
+      drawCheck();
+      std::ifstream json(selected_field_map);
+      currentReferenceMap = Fieldmap(wpi::json::parse(json));
+      currentCombinerMap = currentReferenceMap;
+    }
+    openFilesButton("Select Field Calibrations",
+                    selected_combination_calibrations,
+                    combination_calibrations_selector, "JSON", "*.json");
+    processFilesSelector(combination_calibrations_selector,
+                         selected_combination_calibrations);
+
+    if (!selected_field_map.empty() &&
+        !selected_combination_calibrations.empty()) {
+      for (std::string& file : selected_combination_calibrations) {
+        ImGui::Selectable(getFileName(file).c_str(), false,
+                          ImGuiSelectableFlags_DontClosePopups);
+        if (ImGui::BeginDragDropSource()) {
+          ImGui::SetDragDropPayload("FieldCalibration", &file, sizeof(file));
+          ImGui::TextUnformatted(file.c_str());
+          ImGui::EndDragDropSource();
+        }
+      }
+
+      for (auto& [key, val] : combiner_map) {
+        EmitEntryTarget(key, val);
+      }
+
+      ImGui::InputInt("Tag ID", &current_combiner_tag_id);
+      ImGui::SameLine();
+      if (ImGui::Button("Add", ImVec2(0, 0)) &&
+          currentCombinerMap.hasTag(current_combiner_tag_id)) {
+        combiner_map.emplace(current_combiner_tag_id, "");
+      }
+      ImGui::SameLine();
+      if (ImGui::Button("Remove", ImVec2(0, 0))) {
+        combiner_map.erase(current_combiner_tag_id);
+      }
+    }
+    ImGui::Separator();
+    if (ImGui::Button("Close", ImVec2(0, 0))) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::SameLine();
+    if (ImGui::Button("Download", ImVec2(0, 0))) {
+      for (auto& [key, val] : combiner_map) {
+        std::ifstream json(val);
+        Fieldmap map(wpi::json::parse(json));
+        currentCombinerMap.replaceTag(key, map.getTag(key));
+      }
+      field_combination_json = currentCombinerMap.toJson();
+    }
+
+    if (selected_download_directory.empty() &&
+        !field_combination_json.empty() && !download_directory_selector) {
+      download_directory_selector =
+          std::make_unique<pfd::select_folder>("Select Download Folder", "");
+    }
+
+    processDirectorySelector(download_directory_selector,
+                             selected_download_directory);
+    saveCalibration(field_combination_json, selected_download_directory,
+                    "combined_calibration", isCalibrating);
+
+    ImGui::EndPopup();
+  }
+
  ImGui::End();
 }

--- a/wpical/src/main/native/cpp/fieldcalibration.cpp
+++ b/wpical/src/main/native/cpp/fieldcalibration.cpp
@@ -23,7 +23,6 @@
 #include <opencv2/highgui.hpp>
 #include <opencv2/imgproc.hpp>
 #include <opencv2/videoio.hpp>
-#include <wpi/json.h>

 #include "apriltag.h"
 #include "tag36h11.h"
@@ -433,7 +432,7 @@ inline bool process_video_file(
 }

 int fieldcalibration::calibrate(std::string input_dir_path,
-                                std::string output_file_path,
+                                wpi::json& output_json,
                                std::string camera_model_path,
                                std::string ideal_map_path, int pinned_tag_id,
                                bool show_debug_window) {
@@ -605,8 +604,7 @@ int fieldcalibration::calibrate(std::string input_dir_path,
      {"length", static_cast<double>(json.at("field").at("length"))},
      {"width", static_cast<double>(json.at("field").at("width"))}};

-  std::ofstream output_file(output_file_path);
-  output_file << observed_map_json.dump(4) << std::endl;
+  output_json = observed_map_json;

  return 0;
 }
--- a/wpical/src/main/native/cpp/tagpose.cpp
+++ b/wpical/src/main/native/cpp/tagpose.cpp
@@ -5,8 +5,10 @@
 #include <tagpose.h>

 namespace tag {
-Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y,
-           double z, double field_length_meters, double field_width_meters) {
+Pose::Pose(int tag_id, double xpos, double ypos, double zpos, double w,
+           double x, double y, double z, double field_length_meters,
+           double field_width_meters) {
+  tagId = tag_id;
  xPos = xpos;
  yPos = ypos;
  zPos = zpos;
@@ -26,4 +28,16 @@ Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y,
  pitchRot = eulerAngles[1];
  yawRot = eulerAngles[2];
 }
+
+wpi::json Pose::toJson() {
+  return {{"ID", tagId},
+          {"pose",
+           {{"translation", {{"x", xPos}, {"y", yPos}, {"z", zPos}}},
+            {"rotation",
+             {{"quaternion",
+               {{"W", quaternion.w()},
+                {"X", quaternion.x()},
+                {"Y", quaternion.y()},
+                {"Z", quaternion.z()}}}}}}}};
+}
 }  // namespace tag
--- a/wpical/src/main/native/include/fieldcalibration.h
+++ b/wpical/src/main/native/include/fieldcalibration.h
@@ -6,10 +6,12 @@

 #include <string>

+#include <wpi/json.h>
+
 #include "cameracalibration.h"

 namespace fieldcalibration {
-int calibrate(std::string input_dir_path, std::string output_file_path,
+int calibrate(std::string input_dir_path, wpi::json& output_json,
              std::string camera_model_path, std::string ideal_map_path,
              int pinned_tag_id, bool show_debug_window);
 }  // namespace fieldcalibration
--- a/wpical/src/main/native/include/fieldmap.h
+++ b/wpical/src/main/native/include/fieldmap.h
@@ -5,7 +5,7 @@
 #pragma once

 #include <cmath>
-#include <vector>
+#include <map>

 #include <tagpose.h>
 #include <wpi/json.h>
@@ -19,6 +19,7 @@ class Fieldmap {
    double field_width_meters =
        static_cast<double>(json.at("field").at("width"));
    for (const auto& tag : json.at("tags").items()) {
+      double tag_id = static_cast<int>(tag.value().at("ID"));
      double tagXPos =
          static_cast<double>(tag.value().at("pose").at("translation").at("x"));
      double tagYPos =
@@ -34,15 +35,30 @@ class Fieldmap {
      double tagZQuat = static_cast<double>(
          tag.value().at("pose").at("rotation").at("quaternion").at("Z"));

-      tagVec.emplace_back(tagXPos, tagYPos, tagZPos, tagWQuat, tagXQuat,
-                          tagYQuat, tagZQuat, field_length_meters,
-                          field_width_meters);
+      tagMap.emplace(
+          tag_id, tag::Pose(tag_id, tagXPos, tagYPos, tagZPos, tagWQuat,
+                            tagXQuat, tagYQuat, tagZQuat, field_length_meters,
+                            field_width_meters));
    }
+    fieldLength = field_length_meters;
+    fieldWidth = field_width_meters;
  }

-  const tag::Pose& getTag(size_t tag) const { return tagVec[tag - 1]; }
+  const tag::Pose& getTag(size_t tag) const { return tagMap.at(tag); }

-  int getNumTags() const { return tagVec.size(); }
+  int getNumTags() const { return tagMap.size(); }
+
+  bool hasTag(int tag) { return tagMap.find(tag) != tagMap.end(); }
+
+  wpi::json toJson() {
+    wpi::json json;
+    for (auto& [key, val] : tagMap) {
+      json["tags"].push_back(val.toJson());
+    }
+    json["field"]["length"] = fieldLength;
+    json["field"]["width"] = fieldWidth;
+    return json;
+  }

  static double minimizeAngle(double angle) {
    angle = std::fmod(angle, 360);
@@ -54,6 +70,13 @@ class Fieldmap {
    return angle;
  }

+  void replaceTag(int tag_id, tag::Pose pose) {
+    tagMap.erase(tag_id);
+    tagMap.emplace(tag_id, pose);
+  }
+
 private:
-  std::vector<tag::Pose> tagVec;
+  double fieldLength;
+  double fieldWidth;
+  std::map<int, tag::Pose> tagMap;
 };
--- a/wpical/src/main/native/include/tagpose.h
+++ b/wpical/src/main/native/include/tagpose.h
@@ -6,15 +6,19 @@

 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include <wpi/json.h>

 namespace tag {
 class Pose {
 public:
-  Pose(double xpos, double ypos, double zpos, double w, double x, double y,
-       double z, double field_length_meters, double field_width_meters);
+  Pose(int tag_id, double xpos, double ypos, double zpos, double w, double x,
+       double y, double z, double field_length_meters,
+       double field_width_meters);
+  int tagId;
  double xPos, yPos, zPos, yawRot, rollRot, pitchRot;
  Eigen::Quaterniond quaternion;
  Eigen::Matrix3d rotationMatrix;
  Eigen::Matrix4d transformMatrixFmap;
+  wpi::json toJson();
 };
 }  // namespace tag
--- a/wpical/src/test/native/cpp/test_calibrate.cpp
+++ b/wpical/src/test/native/cpp/test_calibrate.cpp
@@ -19,6 +19,9 @@ cameracalibration::CameraModel cameraModel = {
    .intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
    .distortion_coefficients = Eigen::Matrix<double, 8, 1>::Zero(),
    .avg_reprojection_error = 0.0};
+
+wpi::json output_json;
+
 #ifdef __linux__
 const std::string fileSuffix = ".avi";
 const std::string videoLocation = "/altfieldvideo";
@@ -58,7 +61,7 @@ TEST(Camera_CalibrationTest, MRcal_Atypical) {

 TEST(Field_CalibrationTest, Typical) {
  int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
      calSavePath + "/cameracalibration.json",
      projectRootPath + "/2024-crescendo.json", 3, false);
  EXPECT_EQ(ret, 0);
@@ -66,7 +69,7 @@ TEST(Field_CalibrationTest, Typical) {

 TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) {
  int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
      projectRootPath + videoLocation + "/long" + fileSuffix,
      projectRootPath + "/2024-crescendo.json", 3, false);
  EXPECT_EQ(ret, 1);
@@ -74,7 +77,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) {

 TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) {
  int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
      calSavePath + "/cameracalibration.json",
      calSavePath + "/cameracalibration.json", 3, false);
  EXPECT_EQ(ret, 1);
@@ -82,7 +85,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) {

 TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) {
  int ret = fieldcalibration::calibrate(
-      projectRootPath + "", calSavePath,
+      projectRootPath + "", output_json,
      calSavePath + "/cameracalibration.json",
      projectRootPath + "/2024-crescendo.json", 3, false);
  EXPECT_EQ(ret, 1);
@@ -90,7 +93,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) {

 TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) {
  int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
      calSavePath + "/cameracalibration.json",
      projectRootPath + "/2024-crescendo.json", 42, false);
  EXPECT_EQ(ret, 1);
@@ -98,7 +101,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) {

 TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag_Negative) {
  int ret = fieldcalibration::calibrate(
-      projectRootPath + videoLocation, calSavePath,
+      projectRootPath + videoLocation, output_json,
      calSavePath + "/cameracalibration.json",
      projectRootPath + "/2024-crescendo.json", -1, false);
  EXPECT_EQ(ret, 1);