From ae84f6d2c565f8aa32310712c2496cdf35d73794 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Wed, 31 Dec 2025 12:28:51 -0500 Subject: [PATCH] [wpical] Refactor to use WPILib libraries and modern C++ conventions and improve UX (#7796) wpical was unable to use wpimath and its dependent libraries because Ceres was compiled with a different version of Eigen. Now that the Ceres build has been redone and shipped in #8151, we can now use wpimath and our C++ apriltag wrapper in wpical, allowing for major refactors. This includes: * Using `to_json` and `from_json` specializations to concisely serialize and deserialize all JSON files instead of manually handling JSON. * Removal of the `Fieldmap` and `Pose` classes, which were duplicates of the `AprilTagFieldLayout` and `Pose3d` classes respectively. * Using `AprilTagDetector` instead of the raw libapriltag library. * Using `Pose3d` instead of raw Eigen matrices. In addition, several other refactors were made to make the code more readable and to fix several UX issues and crashes. This includes: * Eagerly parsing every JSON file when selected by the user. This means JSON files are only parsed once on selection, instead of every time a downstream function wants to use the data. This also means invalid JSON can be detected upfront and a specific error shown immediately instead of a catch all error when trying to calibrate. * Using `std::optional` to indicate a calibration failed instead of status codes. * Processing videos on separate threads to not block the UI thread and take advantage of parallelization for camera calibration. (2x speedup on my laptop) * Removing the OpenCV calibration option, since mrcal should be better in every scenario. * Showing a progress bar for camera calibration. * Breaking up the massive `DisplayGui` function into separate functions which contain code for different popups. This also allowed for better organization and scoping of static variables. * Renaming variables to make their purpose more clear. * Displaying the tags present in a field layout when trying to combine multiple field layouts. Fixes #7722. --- tools/wpical/BUILD.bazel | 2 + tools/wpical/CMakeLists.txt | 6 +- tools/wpical/build.gradle | 6 +- tools/wpical/src/main/native/cpp/WPIcal.cpp | 1083 +++++++++-------- .../src/main/native/cpp/cameracalibration.cpp | 664 +++++----- .../src/main/native/cpp/fieldcalibration.cpp | 572 +++------ tools/wpical/src/main/native/cpp/fmap.cpp | 67 +- tools/wpical/src/main/native/cpp/tagpose.cpp | 46 - .../main/native/include/cameracalibration.hpp | 116 +- .../main/native/include/fieldcalibration.hpp | 41 +- .../src/main/native/include/fieldmap.hpp | 82 -- tools/wpical/src/main/native/include/fmap.hpp | 36 +- .../src/main/native/include/tagpose.hpp | 25 - .../src/test/native/cpp/test_calibrate.cpp | 147 +-- 14 files changed, 1334 insertions(+), 1559 deletions(-) delete mode 100644 tools/wpical/src/main/native/cpp/tagpose.cpp delete mode 100644 tools/wpical/src/main/native/include/fieldmap.hpp delete mode 100644 tools/wpical/src/main/native/include/tagpose.hpp diff --git a/tools/wpical/BUILD.bazel b/tools/wpical/BUILD.bazel index 9d3e6a30f0..ade7ea66c7 100644 --- a/tools/wpical/BUILD.bazel +++ b/tools/wpical/BUILD.bazel @@ -177,6 +177,7 @@ wpilib_cc_library( exclude = ["src/main/native/cpp/WPIcal.cpp"], ) + [ ":generate-resources", + ":generate-version", ], copts = copts, cxxopts = cxxopts, @@ -212,6 +213,7 @@ cc_binary( }), deps = [ ":wpical_lib", + "//glass", "//thirdparty/imgui_suite", ], ) diff --git a/tools/wpical/CMakeLists.txt b/tools/wpical/CMakeLists.txt index 185bcf036c..39d5abf7c5 100644 --- a/tools/wpical/CMakeLists.txt +++ b/tools/wpical/CMakeLists.txt @@ -79,7 +79,7 @@ else() -Wno-missing-field-initializers ) endif() -target_compile_options(wpical PRIVATE ${compile_flags}) +set_target_properties(${wpical_thirdparty_src} PROPERTIES COMPILE_FLAGS ${compile_flags}) find_package(OpenCV REQUIRED) find_package(Ceres CONFIG REQUIRED) @@ -89,7 +89,9 @@ find_package(SuiteSparse_config CONFIG REQUIRED) target_link_libraries( wpical apriltag + wpimath ${OpenCV_LIBS} + libglass wpigui wpiutil SuiteSparse::CHOLMOD_static @@ -126,7 +128,9 @@ if(WITH_TESTS) wpical_test googletest apriltag + wpimath ${OpenCV_LIBS} + libglass wpigui wpiutil SuiteSparse::CHOLMOD_static diff --git a/tools/wpical/build.gradle b/tools/wpical/build.gradle index 0255649e20..f76bc04a50 100644 --- a/tools/wpical/build.gradle +++ b/tools/wpical/build.gradle @@ -157,10 +157,11 @@ model { return } lib project: ':apriltag', library: 'apriltag', linkage: 'static' + lib project: ':glass', library: 'glass', linkage: 'static' lib project: ':wpimath', library: 'wpimath', linkage: 'static' - lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' lib project: ':wpigui', library: 'wpigui', linkage: 'static' lib project: ':thirdparty:imgui_suite', library: 'imguiSuite', linkage: 'static' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' nativeUtils.useRequiredLibrary(it, 'ceres') nativeUtils.useRequiredLibrary(it, 'opencv_static') it.cppCompiler.define 'OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT' @@ -210,10 +211,11 @@ model { return } lib project: ':apriltag', library: 'apriltag', linkage: 'static' + lib project: ':glass', library: 'glass', linkage: 'static' lib project: ':wpimath', library: 'wpimath', linkage: 'static' - lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' lib project: ':wpigui', library: 'wpigui', linkage: 'static' lib project: ':thirdparty:imgui_suite', library: 'imguiSuite', linkage: 'static' + lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' nativeUtils.useRequiredLibrary(it, 'ceres') nativeUtils.useRequiredLibrary(it, 'opencv_static') it.cppCompiler.define 'OPENCV_DISABLE_EIGEN_TENSOR_SUPPORT' diff --git a/tools/wpical/src/main/native/cpp/WPIcal.cpp b/tools/wpical/src/main/native/cpp/WPIcal.cpp index 76763d4613..a05ecca25b 100644 --- a/tools/wpical/src/main/native/cpp/WPIcal.cpp +++ b/tools/wpical/src/main/native/cpp/WPIcal.cpp @@ -2,25 +2,31 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include #include -#include #include #include -#include +#include #include #include #include #include +#include #include #include "cameracalibration.hpp" #include "fieldcalibration.hpp" -#include "fieldmap.hpp" #include "fmap.hpp" -#include "tagpose.hpp" +#include "wpi/apriltag/AprilTag.hpp" +#include "wpi/apriltag/AprilTagFieldLayout.hpp" +#include "wpi/glass/Context.hpp" +#include "wpi/glass/MainMenuBar.hpp" +#include "wpi/glass/Storage.hpp" #include "wpi/gui/portable-file-dialogs.h" #include "wpi/gui/wpigui.hpp" +#include "wpi/gui/wpigui_openurl.hpp" +#include "wpi/math/util/MathUtil.hpp" #include "wpi/util/json.hpp" namespace gui = wpi::gui; @@ -28,9 +34,9 @@ namespace gui = wpi::gui; const char* GetWPILibVersion(); #ifdef __linux__ -const bool showDebug = false; +constexpr bool showDebug = false; #else -const bool showDebug = true; +constexpr bool showDebug = true; #endif namespace wpical { @@ -42,10 +48,14 @@ std::string_view GetResource_wpical_128_png(); std::string_view GetResource_wpical_256_png(); std::string_view GetResource_wpical_512_png(); } // namespace wpical +static wpi::apriltag::AprilTagFieldLayout gIdealFieldLayout; +static std::string gInvalidLayoutPath; +static wpi::glass::MainMenuBar gMainMenu; +static wpical::CameraModel gCameraModel; -void drawCheck() { +void DrawCheck() { ImGui::SameLine(); - ImDrawList* draw_list = ImGui::GetWindowDrawList(); + ImDrawList* drawList = ImGui::GetWindowDrawList(); ImVec2 pos = ImGui::GetCursorScreenPos(); float size = ImGui::GetFontSize(); @@ -55,114 +65,507 @@ void drawCheck() { ImVec2 p3 = ImVec2(pos.x + size * 0.75f, pos.y + size * 0.3f); float thickness = 3.0f; - draw_list->AddLine(p1, p2, IM_COL32(0, 255, 0, 255), thickness); - draw_list->AddLine(p2, p3, IM_COL32(0, 255, 0, 255), thickness); + drawList->AddLine(p1, p2, IM_COL32(0, 255, 0, 255), thickness); + drawList->AddLine(p2, p3, IM_COL32(0, 255, 0, 255), thickness); ImGui::NewLine(); } -void processFileSelector(std::unique_ptr& selector, - std::string& selected_file) { +/** + * Processes a folder picker. + * @param selector The folder picker. + * @param selectedDirectoryPath The path to the selected folder. + */ +void ProcessDirectorySelector(std::unique_ptr& selector, + std::string& selectedDirectoryPath) { if (selector && selector->ready(0)) { auto selectedFiles = selector->result(); if (!selectedFiles.empty()) { - selected_file = selectedFiles[0]; + selectedDirectoryPath = selectedFiles; } selector.reset(); } } -void processFilesSelector(std::unique_ptr& selector, - std::vector& selected_files) { - if (selector && selector->ready(0)) { - auto selectedFiles = selector->result(); - if (!selectedFiles.empty()) { - selected_files = selectedFiles; - } - selector.reset(); - } -} - -void processDirectorySelector(std::unique_ptr& 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& selector, - const std::string& file_type, - const std::string& file_extensions) { +/** + * Adds a button that opens a file picker for picking one file. + * @param text The button label. + * @param selectedFilePath The path to the selected file. + * @param selector The file selector. + * @param fileType The human friendly name for the file filter. + * @param fileExtensions The list of file extensions patterns that the selected + * file must match. The list of patterns must be space-separated. + */ +void SelectFileButton(const char* text, std::string& selectedFilePath, + std::unique_ptr& selector, + const std::string& fileType, + const std::string& fileExtensions) { if (ImGui::Button(text)) { selector = std::make_unique( - "Select File", "", std::vector{file_type, file_extensions}, + "Select File", "", std::vector{fileType, fileExtensions}, pfd::opt::none); } -} - -void openFilesButton(const char* text, std::vector& selected_files, - std::unique_ptr& selector, - const std::string& file_type, - const std::string& file_extensions) { - if (ImGui::Button(text)) { - selector = std::make_unique( - "Select File", "", std::vector{file_type, file_extensions}, - pfd::opt::multiselect); + if (selector && selector->ready(0)) { + auto selectedFiles = selector->result(); + if (!selectedFiles.empty()) { + selectedFilePath = selectedFiles[0]; + } + selector.reset(); } } -void openDirectoryButton(const char* text, - std::unique_ptr& selector, - std::string& selected_directory) { +/** + * Adds a button to open a folder picker to select a folder. + * @param text The button label. + * @param selector The folder picker. + * @param selectedDirectory The selected directory. + */ +void SelectDirectoryButton(const char* text, + std::unique_ptr& selector, + std::string& selectedDirectory) { if (ImGui::Button(text)) { selector = std::make_unique("Select Directory", ""); } + ProcessDirectorySelector(selector, selectedDirectory); } -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: ", tag_id); - } - bool rv = false; - if (ImGui::BeginDragDropTarget()) { - if (const ImGuiPayload* payload = - ImGui::AcceptDragDropPayload("FieldCalibration")) { - file = *static_cast(payload->Data); - rv = true; +/** + * Sets up an error modal for the field layout being unable to be loaded. + */ +void FieldLoadingError() { + if (ImGui::BeginPopupModal("AprilTag Field Layout Loading Error", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::Text("Failed to load AprilTag field layout located at"); + ImGui::TextWrapped("%s", gInvalidLayoutPath.c_str()); + ImGui::TextWrapped("Please make sure field layout is valid."); + ImGui::Separator(); + if (ImGui::Button("OK", ImVec2(120, 0))) { + ImGui::CloseCurrentPopup(); } - ImGui::EndDragDropTarget(); + ImGui::EndPopup(); } - return rv; } -void saveCalibration(wpi::util::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(); +/** + * Sets up an error modal for a tag ID not being in the reference field layout + * when combining calibrations. + */ +void MissingTagInField() { + if (ImGui::BeginPopupModal("Tag ID Not In Field", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::Text("This tag is not available in the field."); + ImGui::TextWrapped( + "Drag another field to this tag or select another tag to drag this " + "field to."); + ImGui::Separator(); + if (ImGui::Button("OK", ImVec2(120, 0))) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } +} - std::ofstream fmap(output_directory + "/" + output_name + ".fmap"); - fmap << fmap::convertfmap(field).dump(4); - fmap.close(); +void CameraCalibrationSelectorButton(const char* text, + wpical::CameraModel& cameraModel) { + static std::unique_ptr selector; + if (ImGui::Button(text)) { + selector = std::make_unique( + "Select File", "", std::vector{"JSON Files", "*.json"}, + pfd::opt::none); + } + if (selector && selector->ready(0)) { + auto selectedFiles = selector->result(); + if (!selectedFiles.empty()) { + try { + cameraModel = wpi::util::json::parse(std::ifstream(selectedFiles[0])) + .get(); + } catch (...) { + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("Camera Calibration Loading Error"); + } + } + selector.reset(); + } +} - field.clear(); - output_directory.clear(); - isCalibrating = false; +void FieldSelectorButton(const char* text, + std::unique_ptr& selector, + wpi::apriltag::AprilTagFieldLayout& layout) { + if (ImGui::Button(text)) { + selector = std::make_unique( + "Select File", "", std::vector{"JSON", "*.json"}, + pfd::opt::none); + } + if (selector && selector->ready(0)) { + auto selectedFiles = selector->result(); + if (!selectedFiles.empty()) { + std::string idealLayoutPath = selectedFiles[0]; + try { + layout = wpi::util::json::parse(std::ifstream(idealLayoutPath)) + .get(); + } catch (...) { + gInvalidLayoutPath = idealLayoutPath; + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("AprilTag Field Layout Loading Error"); + } + } + selector.reset(); + } + if (layout.GetTags().size() > 0) { + DrawCheck(); + } +} + +void IdealFieldSelectorButton(const char* text) { + static std::unique_ptr idealFieldLayoutSelector; + FieldSelectorButton(text, idealFieldLayoutSelector, gIdealFieldLayout); +} + +void SaveCalibratedField(const wpi::apriltag::AprilTagFieldLayout& field, + std::string outputName, + std::unique_ptr& saveDirSelector) { + static std::string saveDir; + ProcessDirectorySelector(saveDirSelector, saveDir); + if (!saveDir.empty()) { + std::ofstream out(saveDir + "/" + outputName + ".json"); + out << wpi::util::json{field}.dump(4); + + std::ofstream fmap(saveDir + "/" + outputName + ".fmap"); + fmap << wpi::util::json{fmap::Fieldmap(field)}.dump(4); + + saveDir.clear(); + } +} + +void CalibrateCamera() { + static std::unique_ptr cameraVideoSelector; + static std::string cameraVideoPath; + static std::unique_ptr videoProcessor; + static bool calibrating = false; + + static double squareWidth = 0.709; + static double markerWidth = 0.551; + static int boardWidth = 12; + static int boardHeight = 8; + static int numWorkers = 8; + + if (ImGui::BeginPopupModal("Camera Calibration", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + // Camera Calibration Error calibration popup window + if (ImGui::BeginPopupModal("Camera Calibration Error", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::TextWrapped( + "Camera calibration failed. Please make sure you have uploaded the " + "correct video file"); + ImGui::Separator(); + if (ImGui::Button("OK", ImVec2(300, 0))) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + SelectFileButton("Select Camera Calibration Video", cameraVideoPath, + cameraVideoSelector, "Video Files", + "*.mp4 *.mov *.m4v *.mkv *.avi"); + ImGui::Text("%s", cameraVideoPath.c_str()); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Square Width (in)", &squareWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Marker Width (in)", &markerWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Board Width (squares)", &boardWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Board Height (squares)", &boardHeight); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Number of Workers", &numWorkers); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + + ImGui::Separator(); + if (calibrating) { + if (videoProcessor->IsFinished() && videoProcessor->GetCameraModel()) { + gCameraModel = *videoProcessor->GetCameraModel(); + std::filesystem::path myPath(cameraVideoPath); + auto outputPath = myPath.parent_path() / "cameracalibration.json"; + + std::ofstream output_file(outputPath); + output_file << wpi::util::json(gCameraModel).dump(4) << std::endl; + ImGui::CloseCurrentPopup(); + calibrating = false; + } else if (!videoProcessor->IsFinished()) { + double processed = videoProcessor->TotalFramesProcessed(); + double total = videoProcessor->TotalFrames(); + if (processed == total) { + // Done processing frames, show that calibration is happening + ImGui::Text("Calibrating, this may take a few minutes %c", + "|/-\\"[static_cast(ImGui::GetTime() / 0.05f) & 3]); + } else { + ImGui::ProgressBar( + processed / total, ImVec2(0.0f, 0.0f), + fmt::format("{}/{} frames", processed, total).c_str()); + } + } else { + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("Camera Calibration Error"); + calibrating = false; + } + } else if (ImGui::Button("Calibrate") && !cameraVideoPath.empty()) { + videoProcessor = std::make_unique( + numWorkers, squareWidth, markerWidth, boardWidth, boardHeight, + cameraVideoPath); + calibrating = true; + } + ImGui::SameLine(); + if (ImGui::Button("Close")) { + if (videoProcessor) { + videoProcessor->Stop(); + } + calibrating = false; + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } +} + +void CombineCalibrations() { + static std::unique_ptr calibratedFieldLayoutMultiselector; + static std::map + calibratedFieldLayouts; + static std::unique_ptr saveDirSelector; + static std::vector tags; + // Maps tag IDs to paths to JSON files containing field layouts + static std::map combinerMap; + static int currentCombinerTagId = 0; + + if (ImGui::BeginPopupModal("Combine Calibrations", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + IdealFieldSelectorButton("Select Ideal Map"); + + if (ImGui::Button("Select Field Calibrations")) { + calibratedFieldLayoutMultiselector = std::make_unique( + "Select File", "", std::vector{"JSON", "*.json"}, + pfd::opt::multiselect); + } + if (calibratedFieldLayoutMultiselector && + calibratedFieldLayoutMultiselector->ready(0)) { + auto selectedFiles = calibratedFieldLayoutMultiselector->result(); + if (!selectedFiles.empty()) { + std::map fieldLayouts; + for (auto& path : selectedFiles) { + try { + fieldLayouts.emplace( + path, wpi::util::json::parse(std::ifstream(path)) + .get()); + } catch (...) { + gInvalidLayoutPath = path; + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("AprilTag Field Layout Loading Error"); + } + } + calibratedFieldLayouts = fieldLayouts; + } + calibratedFieldLayoutMultiselector.reset(); + } + + if (gIdealFieldLayout.GetTags().size() > 0 && + !calibratedFieldLayouts.empty()) { + for (auto& [file, layout] : calibratedFieldLayouts) { + std::vector tagIds; + for (auto& tags : layout.GetTags()) { + tagIds.push_back(tags.ID); + } + auto text = fmt::format("{} tags: {}", + std::filesystem::path(file).filename().string(), + fmt::join(tagIds, ", ")); + ImGui::Selectable(text.c_str(), false, + ImGuiSelectableFlags_DontClosePopups); + if (ImGui::BeginDragDropSource()) { + ImGui::SetDragDropPayload("FieldCalibration", file.c_str(), + file.size()); + ImGui::TextUnformatted(file.c_str()); + ImGui::EndDragDropSource(); + } + } + + // If a JSON field layout gets dragged over to a tag, that tag's data will + // be pulled from the dragged JSON field layout + for (auto& [tagId, filePath] : combinerMap) { + if (!filePath.empty()) { + auto text = fmt::format("Tag ID {}: {}", tagId, filePath); + ImGui::TextUnformatted(text.c_str()); + } else { + ImGui::Text("Tag ID %i: ", + tagId); + } + if (ImGui::BeginDragDropTarget()) { + if (const ImGuiPayload* payload = + ImGui::AcceptDragDropPayload("FieldCalibration")) { + std::string path(static_cast(payload->Data), + payload->DataSize); + if (calibratedFieldLayouts.contains(path) && + calibratedFieldLayouts[path].GetTagPose(tagId)) { + filePath = path; + } else { + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("Tag ID Not In Field"); + } + } + ImGui::EndDragDropTarget(); + } + } + MissingTagInField(); + + // Allow users to add tags to the combined field layout + ImGui::InputInt("Tag ID", ¤tCombinerTagId); + ImGui::SameLine(); + // Only allow tag IDs that are present in the ideal field layout + if (ImGui::Button("Add") && + gIdealFieldLayout.GetTagPose(currentCombinerTagId)) { + // Empty string, indicates no selected layout to get tag data from + combinerMap.emplace(currentCombinerTagId, ""); + } + ImGui::SameLine(); + if (ImGui::Button("Remove")) { + combinerMap.erase(currentCombinerTagId); + } + } + ImGui::Separator(); + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + } + ImGui::SameLine(); + if (ImGui::Button("Download")) { + tags.clear(); + for (auto& [tagId, layoutPath] : combinerMap) { + if (layoutPath != "") { + auto tagPose = calibratedFieldLayouts[layoutPath].GetTagPose(tagId); + if (tagPose) { + // TODO: remove variable when clang 16 is available on Mac + wpi::apriltag::AprilTag tag{tagId, tagPose.value()}; + tags.emplace_back(tag); + } + } else { + wpi::apriltag::AprilTag tag{ + tagId, gIdealFieldLayout.GetTagPose(tagId).value()}; + tags.emplace_back(tag); + } + } + saveDirSelector = + std::make_unique("Select Download Directory", ""); + } + SaveCalibratedField({tags, gIdealFieldLayout.GetFieldLength(), + gIdealFieldLayout.GetFieldWidth()}, + "combined_calibration", saveDirSelector); + ImGui::EndPopup(); + } +} + +void VisualizeCalibration() { + static int focusedTag = 1; + static int referenceTag = 1; + static std::unique_ptr calibratedFieldLayoutSelector; + static wpi::apriltag::AprilTagFieldLayout currentCalibrationLayout; + if (ImGui::BeginPopupModal("Visualize Calibration", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + FieldSelectorButton("Select Calibrated Field Layout", + calibratedFieldLayoutSelector, + currentCalibrationLayout); + IdealFieldSelectorButton("Select Ideal Field Layout"); + + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Focused Tag", &focusedTag); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Reference Tag", &referenceTag); + + if (currentCalibrationLayout.GetTags().size() > 0 && + gIdealFieldLayout.GetTags().size() > 0) { + if (currentCalibrationLayout.GetTags().size() != + gIdealFieldLayout.GetTags().size()) { + ImGui::TextWrapped( + "The number of tags in the calibration output and the ideal field " + "layout do not match. Please ensure that the calibration output " + "and ideal field layout have the same number of tags."); + } else if (gIdealFieldLayout.GetTagPose(focusedTag) && + gIdealFieldLayout.GetTagPose(referenceTag)) { + auto referenceFocusedTagPose = + gIdealFieldLayout.GetTagPose(focusedTag).value(); + auto calibrationFocusedTagPose = + currentCalibrationLayout.GetTagPose(focusedTag).value(); + auto calibrationReferenceTagPose = + currentCalibrationLayout.GetTagPose(referenceTag).value(); + + auto diff = referenceFocusedTagPose - calibrationFocusedTagPose; + + auto ref = calibrationReferenceTagPose.Translation() - + calibrationFocusedTagPose.Translation(); + + ImGui::TextWrapped("X Difference: %f (m)", diff.X().value()); + ImGui::TextWrapped("Y Difference: %f (m)", diff.Y().value()); + ImGui::TextWrapped("Z Difference: %f (m)", diff.Z().value()); + + ImGui::TextWrapped("Yaw Difference %f°", diff.Rotation().Z().value()); + ImGui::TextWrapped("Pitch Difference %f°", diff.Rotation().Y().value()); + ImGui::TextWrapped("Roll Difference %f°", diff.Rotation().X().value()); + + ImGui::NewLine(); + + ImGui::TextWrapped("X Reference: %f (m)", ref.X().value()); + ImGui::TextWrapped("Y Reference: %f (m)", ref.Y().value()); + ImGui::TextWrapped("Z Reference: %f (m)", ref.Z().value()); + } else { + ImGui::TextWrapped( + "Please select tags that are in the ideal and calibrated field " + "layout"); + } + } + + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } +} + +static void DisplayMainMenu() { + ImGui::BeginMenuBar(); + + gMainMenu.WorkspaceMenu(); + wpi::gui::EmitViewMenu(); + + bool about = false; + if (ImGui::BeginMenu("Info")) { + if (ImGui::MenuItem("About")) { + about = true; + } + ImGui::EndMenu(); + } + + if (ImGui::BeginMenu("Docs")) { + if (ImGui::MenuItem("Online documentation")) { + wpi::gui::OpenURL( + "https://docs.wpilib.org/en/stable/docs/software/wpilib-tools/wpical/" + "index.html"); + } + ImGui::EndMenu(); + } + + ImGui::EndMenuBar(); + + if (about) { + ImGui::OpenPopup("About"); + about = false; + } + if (ImGui::BeginPopupModal("About", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::Text("WPIcal"); + ImGui::Separator(); + ImGui::Text("v%s", GetWPILibVersion()); + ImGui::Separator(); + ImGui::Text("Save location: %s", wpi::glass::GetStorageDir().c_str()); + ImGui::Text("%.3f ms/frame (%.1f FPS)", 1000.0f / ImGui::GetIO().Framerate, + ImGui::GetIO().Framerate); + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); } } @@ -181,66 +584,20 @@ static void DisplayGui() { ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoCollapse); - // main menu - ImGui::BeginMenuBar(); - gui::EmitViewMenu(); - if (ImGui::BeginMenu("View")) { - ImGui::EndMenu(); - } - ImGui::EndMenuBar(); - - static std::unique_ptr camera_intrinsics_selector; - static std::unique_ptr field_map_selector; - static std::unique_ptr output_calibration_json_selector; - static std::unique_ptr combination_calibrations_selector; - - static std::unique_ptr - field_calibration_directory_selector; - static std::unique_ptr download_directory_selector; - - static wpi::util::json field_calibration_json; - static wpi::util::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 selected_combination_calibrations; - - static std::map combiner_map; - static int current_combiner_tag_id = 0; - - static bool isCalibrating = false; - - cameracalibration::CameraModel cameraModel = { - .intrinsic_matrix = Eigen::Matrix::Identity(), - .distortion_coefficients = Eigen::Matrix::Zero(), - .avg_reprojection_error = 0.0}; - static bool mrcal = true; - - static double squareWidth = 0.709; - static double markerWidth = 0.551; - static int boardWidth = 12; - static int boardHeight = 8; - static double imagerWidth = 1920; - static double imagerHeight = 1080; + DisplayMainMenu(); + static wpi::apriltag::AprilTagFieldLayout calibratedFieldLayout; + static std::unique_ptr saveDirSelector; + static std::unique_ptr fieldVideoDirSelector; + static std::string fieldVideoDir; static int pinnedTag = 1; - - static int focusedTag = 1; - static int referenceTag = 1; - static int maxFRCTag = 22; - - static Fieldmap currentCalibrationMap; - static Fieldmap currentReferenceMap; - static Fieldmap currentCombinerMap; + static std::unique_ptr fieldCalibrator; + static bool calibrateButtonPressed = false; // camera matrix selector button - openFileButton("Select Camera Intrinsics JSON", selected_camera_intrinsics, - camera_intrinsics_selector, "JSON Files", "*.json"); - processFileSelector(camera_intrinsics_selector, selected_camera_intrinsics); + CameraCalibrationSelectorButton("Select Camera Intrinsics JSON", + gCameraModel); ImGui::SameLine(); ImGui::Text("Or"); @@ -248,61 +605,51 @@ static void DisplayGui() { // camera calibration button if (ImGui::Button("Calibrate Camera")) { - selected_camera_intrinsics.clear(); ImGui::OpenPopup("Camera Calibration"); } - if (!selected_camera_intrinsics.empty()) { - drawCheck(); + if (gCameraModel.avgReprojectionError != -1) { + DrawCheck(); } - // field json selector button - 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(); - } + IdealFieldSelectorButton("Select Field Layout JSON"); // field calibration directory selector button - openDirectoryButton("Select Field Calibration Directory", - field_calibration_directory_selector, - selected_field_calibration_directory); - processDirectorySelector(field_calibration_directory_selector, - selected_field_calibration_directory); + SelectDirectoryButton("Select Field Calibration Directory", + fieldVideoDirSelector, fieldVideoDir); - if (!selected_field_calibration_directory.empty()) { - drawCheck(); + if (!fieldVideoDir.empty()) { + DrawCheck(); } + auto isMissingInputs = fieldVideoDir.empty() || + gCameraModel.avgReprojectionError == -1 || + gIdealFieldLayout.GetTags().size() == 0; // pinned tag text field ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); ImGui::InputInt("Pinned Tag", &pinnedTag); - // calibrate button if (ImGui::Button("Calibrate!!!")) { - 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("Select Download Folder", ""); + if (!isMissingInputs) { + fieldCalibrator = std::make_unique(); + fieldCalibrator->Calibrate(fieldVideoDir, gCameraModel, gIdealFieldLayout, + pinnedTag, showDebug); + calibrateButtonPressed = true; } } - - processDirectorySelector(download_directory_selector, - selected_download_directory); - saveCalibration(field_calibration_json, selected_download_directory, - "field_calibration", isCalibrating); - + if (calibrateButtonPressed && fieldCalibrator->IsFinished()) { + if (auto layout = fieldCalibrator->GetAprilTagFieldLayout()) { + calibratedFieldLayout = *layout; + saveDirSelector = + std::make_unique("Select Download Directory", ""); + } else { + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("Field Calibration Error"); + } + calibrateButtonPressed = false; + } + SaveCalibratedField(calibratedFieldLayout, "field_calibration", + saveDirSelector); if (ImGui::Button("Visualize")) { ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); ImGui::OpenPopup("Visualize Calibration"); @@ -311,11 +658,10 @@ static void DisplayGui() { 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()) { + if (isMissingInputs) { ImGui::TextWrapped( - "Some inputs are empty! please enter your camera calibration video, " - "field map, and field calibration directory"); + "Some inputs are empty! Please enter your camera calibration video, " + "field layout, and field calibration directory"); } else if (!(pinnedTag > 0 && pinnedTag <= maxFRCTag)) { ImGui::TextWrapped( "The pinned tag is not within the normal range for FRC fields (1-22), " @@ -327,12 +673,12 @@ static void DisplayGui() { // error popup window if (ImGui::BeginPopupModal("Field Calibration Error", NULL, ImGuiWindowFlags_AlwaysAutoResize)) { - ImGui::TextWrapped( - "Field Calibration Failed - please try again, ensuring that:"); + ImGui::Text("Field Calibration Failed - please try again, ensuring that:"); ImGui::TextWrapped( "- You have selected the correct camera intrinsics JSON or camera " "calibration video"); - ImGui::TextWrapped("- You have selected the correct ideal field map JSON"); + ImGui::TextWrapped( + "- You have selected the correct ideal field layout JSON"); ImGui::TextWrapped( "- You have selected the correct field calibration video directory"); ImGui::TextWrapped( @@ -346,24 +692,13 @@ static void DisplayGui() { ImGui::EndPopup(); } - if (ImGui::BeginPopupModal("Fmap Conversion Error", NULL, + if (ImGui::BeginPopupModal("Camera Calibration Loading Error", NULL, ImGuiWindowFlags_AlwaysAutoResize)) { - ImGui::TextWrapped( - "Fmap conversion failed - you can still use the calibration output on " - "Limelight platforms by converting the .json output to .fmap using the " - "Limelight map builder tool"); - ImGui::TextWrapped("https://tools.limelightvision.io/map-builder"); - ImGui::Separator(); - if (ImGui::Button("OK", ImVec2(120, 0))) { - ImGui::CloseCurrentPopup(); - } - ImGui::EndPopup(); - } - - // successful field calibration popup window - if (ImGui::BeginPopupModal("Success", NULL, - ImGuiWindowFlags_AlwaysAutoResize)) { - ImGui::TextWrapped("Success, output JSON generated in selected directory"); + ImGui::Text("Could not load camera calibration JSON. Make sure that:"); + ImGui::TextWrapped("- Your camera calibration is valid JSON"); + ImGui::TextWrapped("- The camera matrix is either 3x3 or a 9 item array"); + ImGui::TextWrapped("- There's an array for distortion"); + ImGui::TextWrapped("- There's a reprojection error"); ImGui::Separator(); if (ImGui::Button("OK", ImVec2(120, 0))) { ImGui::CloseCurrentPopup(); @@ -372,314 +707,12 @@ static void DisplayGui() { } // camera calibration popup window - if (ImGui::BeginPopupModal("Camera Calibration", NULL, - ImGuiWindowFlags_AlwaysAutoResize)) { - // Camera Calibration Error calibration popup window - if (ImGui::BeginPopupModal("Camera Calibration Error", NULL, - ImGuiWindowFlags_AlwaysAutoResize)) { - ImGui::TextWrapped( - "Camera calibration failed. Please make sure you have uploaded the " - "correct video file"); - ImGui::Separator(); - if (ImGui::Button("OK", ImVec2(120, 0))) { - ImGui::CloseCurrentPopup(); - } - ImGui::EndPopup(); - } + CalibrateCamera(); - if (ImGui::Button("MRcal")) { - mrcal = true; - } - - ImGui::SameLine(); - ImGui::Text("or"); - ImGui::SameLine(); - - if (ImGui::Button("OpenCV")) { - mrcal = false; - } - - if (mrcal) { - 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); - ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); - ImGui::InputDouble("Marker Width (in)", &markerWidth); - ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); - ImGui::InputInt("Board Width (squares)", &boardWidth); - ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); - ImGui::InputInt("Board Height (squares)", &boardHeight); - ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); - ImGui::InputDouble("Image Width (pixels)", &imagerWidth); - ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); - ImGui::InputDouble("Image Height (pixels)", &imagerHeight); - - ImGui::Separator(); - if (ImGui::Button("Calibrate") && !selected_camera_intrinsics.empty()) { - std::cout << "calibration button pressed" << std::endl; - int ret = cameracalibration::calibrate( - selected_camera_intrinsics.c_str(), cameraModel, markerWidth, - boardWidth, boardHeight, imagerWidth, imagerHeight, showDebug); - if (ret == 0) { - size_t lastSeparatorPos = - selected_camera_intrinsics.find_last_of("/\\"); - std::string output_file_path; - - if (lastSeparatorPos != std::string::npos) { - output_file_path = - selected_camera_intrinsics.substr(0, lastSeparatorPos) - .append("/cameracalibration.json"); - } - - selected_camera_intrinsics = output_file_path; - - cameracalibration::dumpJson(cameraModel, output_file_path); - ImGui::CloseCurrentPopup(); - } else if (ret == 1) { - std::cout << "calibration failed and popup ready" << std::endl; - ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); - ImGui::OpenPopup("Camera Calibration Error"); - } else { - ImGui::CloseCurrentPopup(); - } - } - } else { - 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); - ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); - ImGui::InputDouble("Marker Width (in)", &markerWidth); - ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); - ImGui::InputInt("Board Width (squares)", &boardWidth); - ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); - ImGui::InputInt("Board Height (squares)", &boardHeight); - - ImGui::Separator(); - if (ImGui::Button("Calibrate") && !selected_camera_intrinsics.empty()) { - std::cout << "calibration button pressed" << std::endl; - int ret = cameracalibration::calibrate( - selected_camera_intrinsics.c_str(), cameraModel, squareWidth, - markerWidth, boardWidth, boardHeight, showDebug); - if (ret == 0) { - size_t lastSeparatorPos = - selected_camera_intrinsics.find_last_of("/\\"); - std::string output_file_path; - - if (lastSeparatorPos != std::string::npos) { - output_file_path = - selected_camera_intrinsics.substr(0, lastSeparatorPos) - .append("/cameracalibration.json"); - } - - selected_camera_intrinsics = output_file_path; - - cameracalibration::dumpJson(cameraModel, output_file_path); - ImGui::CloseCurrentPopup(); - } else if (ret == 1) { - std::cout << "calibration failed and popup ready" << std::endl; - ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); - ImGui::OpenPopup("Camera Calibration Error"); - } else { - ImGui::CloseCurrentPopup(); - } - } - } - - ImGui::SameLine(); - if (ImGui::Button("Close")) { - ImGui::CloseCurrentPopup(); - } - - ImGui::EndPopup(); - } - - // visualize calibration popup - if (ImGui::BeginPopupModal("Visualize Calibration", NULL, - ImGuiWindowFlags_AlwaysAutoResize)) { - openFileButton("Select Calibration JSON", output_calibration_json_path, - output_calibration_json_selector, "JSON", "*.json"); - processFileSelector(output_calibration_json_selector, - output_calibration_json_path); - - if (!output_calibration_json_path.empty()) { - ImGui::SameLine(); - drawCheck(); - } - - 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(); - drawCheck(); - } - - ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); - ImGui::InputInt("Focused Tag", &focusedTag); - ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); - ImGui::InputInt("Reference Tag", &referenceTag); - - 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::util::json::parse(calJson)); - currentReferenceMap = Fieldmap(wpi::util::json::parse(refJson)); - - 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; - - 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::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()); - } else { - ImGui::TextWrapped( - "Please select tags that are in the ideal field map and " - "calibration map"); - } - } - - if (ImGui::Button("Close")) { - ImGui::CloseCurrentPopup(); - } - 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::util::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", ¤t_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::util::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("Select Download Folder", ""); - } - - processDirectorySelector(download_directory_selector, - selected_download_directory); - saveCalibration(field_combination_json, selected_download_directory, - "combined_calibration", isCalibrating); - - ImGui::EndPopup(); - } + VisualizeCalibration(); + CombineCalibrations(); + FieldLoadingError(); ImGui::End(); } @@ -697,22 +730,28 @@ int main(int argc, char** argv) { saveDir = argv[1]; } - gui::CreateContext(); + wpi::gui::CreateContext(); + wpi::glass::CreateContext(); - gui::AddIcon(wpical::GetResource_wpical_16_png()); - gui::AddIcon(wpical::GetResource_wpical_32_png()); - gui::AddIcon(wpical::GetResource_wpical_48_png()); - gui::AddIcon(wpical::GetResource_wpical_64_png()); - gui::AddIcon(wpical::GetResource_wpical_128_png()); - gui::AddIcon(wpical::GetResource_wpical_256_png()); - gui::AddIcon(wpical::GetResource_wpical_512_png()); + wpi::gui::AddIcon(wpical::GetResource_wpical_16_png()); + wpi::gui::AddIcon(wpical::GetResource_wpical_32_png()); + wpi::gui::AddIcon(wpical::GetResource_wpical_48_png()); + wpi::gui::AddIcon(wpical::GetResource_wpical_64_png()); + wpi::gui::AddIcon(wpical::GetResource_wpical_128_png()); + wpi::gui::AddIcon(wpical::GetResource_wpical_256_png()); + wpi::gui::AddIcon(wpical::GetResource_wpical_512_png()); - gui::AddLateExecute(DisplayGui); + wpi::glass::SetStorageName("wpical"); + wpi::glass::SetStorageDir(saveDir.empty() ? wpi::gui::GetPlatformSaveFileDir() + : saveDir); - gui::Initialize("wpical", 600, 400); - gui::Main(); + wpi::gui::AddLateExecute(DisplayGui); - gui::DestroyContext(); + wpi::gui::Initialize("WPIcal", 900, 600); + wpi::gui::Main(); + + wpi::glass::DestroyContext(); + wpi::gui::DestroyContext(); return 0; } diff --git a/tools/wpical/src/main/native/cpp/cameracalibration.cpp b/tools/wpical/src/main/native/cpp/cameracalibration.cpp index 9df25cd911..f753e1d6b2 100644 --- a/tools/wpical/src/main/native/cpp/cameracalibration.cpp +++ b/tools/wpical/src/main/native/cpp/cameracalibration.cpp @@ -4,38 +4,38 @@ #include "cameracalibration.hpp" -#include -#include #include +#include #include +#include #include +#include #include +#include #include #include #include -static bool filter(std::vector charuco_corners, - std::vector charuco_ids, - std::vector> marker_corners, - std::vector marker_ids, int board_width, - int board_height) { - if (charuco_ids.empty() || charuco_corners.empty()) { +static bool filter(std::vector charucoCorners, + std::vector charucoIds, std::vector markerIds, + int boardWidth, int boardHeight) { + if (charucoIds.empty() || charucoCorners.empty()) { return false; } - if (charuco_corners.size() < 10 || charuco_ids.size() < 10) { + if (charucoCorners.size() < 10 || charucoIds.size() < 10) { return false; } - for (int i = 0; i < charuco_ids.size(); i++) { - if (charuco_ids.at(i) > (board_width - 1) * (board_height - 1) - 1) { + for (int i = 0; i < charucoIds.size(); i++) { + if (charucoIds.at(i) > (boardWidth - 1) * (boardHeight - 1) - 1) { return false; } } - for (int i = 0; i < marker_ids.size(); i++) { - if (marker_ids.at(i) > ((board_width * board_height) / 2) - 1) { + for (int i = 0; i < markerIds.size(); i++) { + if (markerIds.at(i) > ((boardWidth * boardHeight) / 2) - 1) { return false; } } @@ -43,362 +43,310 @@ static bool filter(std::vector charuco_corners, return true; } -int cameracalibration::calibrate(const std::string& input_video, - CameraModel& camera_model, float square_width, - float marker_width, int board_width, - int board_height, bool show_debug_window) { - // ChArUco Board - cv::aruco::Dictionary aruco_dict = - cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000); - cv::Ptr charuco_board = new cv::aruco::CharucoBoard( - cv::Size(board_width, board_height), square_width * 0.0254, - marker_width * 0.0254, aruco_dict); - cv::aruco::CharucoDetector charuco_detector(*charuco_board); - - // Video capture - cv::VideoCapture video_capture(input_video); - cv::Size frame_shape; - - std::vector> all_charuco_corners; - std::vector> all_charuco_ids; - - std::vector> all_obj_points; - std::vector> all_img_points; - - while (video_capture.grab()) { - cv::Mat frame; - video_capture.retrieve(frame); - - cv::Mat debug_image = frame; - - if (frame.empty()) { - break; - } - - // Detect - cv::Mat frame_gray; - cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); - - frame_shape = frame_gray.size(); - - std::vector charuco_corners; - std::vector charuco_ids; - std::vector> marker_corners; - std::vector marker_ids; - - std::vector obj_points; - std::vector img_points; - - charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids, - marker_corners, marker_ids); - - if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids, - board_width, board_height)) { - continue; - } - - charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points, - img_points); - - all_charuco_corners.push_back(charuco_corners); - all_charuco_ids.push_back(charuco_ids); - - all_obj_points.push_back(obj_points); - all_img_points.push_back(img_points); - - if (show_debug_window) { - cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids); - cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners, - charuco_ids); - cv::imshow("Frame", debug_image); - if (cv::waitKey(1) == 'q') { - break; - } - } - } - - video_capture.release(); - if (show_debug_window) { - cv::destroyAllWindows(); - } - - // Calibrate - cv::Mat camera_matrix, dist_coeffs; - std::vector r_vecs, t_vecs; - std::vector std_dev_intrinsics, std_dev_extrinsics, per_view_errors; - double repError; - - try { - // see https://stackoverflow.com/a/75865177 - int flags = cv::CALIB_RATIONAL_MODEL | cv::CALIB_USE_LU; - repError = cv::calibrateCamera( - all_obj_points, all_img_points, frame_shape, camera_matrix, dist_coeffs, - r_vecs, t_vecs, cv::noArray(), cv::noArray(), cv::noArray(), flags); - } catch (...) { - std::cout << "calibration failed" << std::endl; - return 1; - } - - std::vector matrix = {camera_matrix.begin(), - camera_matrix.end()}; - - std::vector distortion = {dist_coeffs.begin(), - dist_coeffs.end()}; - - camera_model.intrinsic_matrix = Eigen::Matrix(matrix.data()); - camera_model.distortion_coefficients = - Eigen::Matrix(distortion.data()); - camera_model.avg_reprojection_error = repError; - return 0; -} - -int cameracalibration::calibrate(const std::string& input_video, - CameraModel& camera_model, float square_width, - float marker_width, int board_width, - int board_height, double imagerWidthPixels, - double imagerHeightPixels, - bool show_debug_window) { - // ChArUco Board - cv::aruco::Dictionary aruco_dict = - cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000); - cv::Ptr charuco_board = new cv::aruco::CharucoBoard( - cv::Size(board_width, board_height), square_width * 0.0254, - marker_width * 0.0254, aruco_dict); - cv::aruco::CharucoDetector charuco_detector(*charuco_board); - - // Video capture - cv::VideoCapture video_capture(input_video); - cv::Size frame_shape; - - // Detection output - std::vector observation_boards; - std::vector frames_rt_toref; - - cv::Size boardSize(board_width - 1, board_height - 1); - cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels); - - while (video_capture.grab()) { - cv::Mat frame; - video_capture.retrieve(frame); - - cv::Mat debug_image = frame; - - if (frame.empty()) { - break; - } - - // Detect - cv::Mat frame_gray; - cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); - - frame_shape = frame_gray.size(); - - std::vector charuco_corners; - std::vector charuco_ids; - std::vector> marker_corners; - std::vector marker_ids; - - std::vector obj_points; - std::vector img_points; - - std::vector points((board_width - 1) * (board_height - 1)); - - charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids, - marker_corners, marker_ids); - - if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids, - board_width, board_height)) { - continue; - } - - charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points, - img_points); - - for (int i = 0; i < charuco_ids.size(); i++) { - int id = charuco_ids.at(i); - points[id].x = img_points.at(i).x; - points[id].y = img_points.at(i).y; - points[id].z = 1.0f; - } - - for (int i = 0; i < points.size(); i++) { - if (points[i].z != 1.0f) { - points[i].x = -1.0f; - points[i].y = -1.0f; - points[i].z = -1.0f; - } - } - - frames_rt_toref.push_back( - getSeedPose(points.data(), boardSize, imagerSize, square_width, 1000)); - observation_boards.insert(observation_boards.end(), points.begin(), - points.end()); - - if (show_debug_window) { - cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids); - cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners, - charuco_ids); - cv::imshow("Frame", debug_image); - if (cv::waitKey(1) == 'q') { - break; - } - } - } - - video_capture.release(); - if (show_debug_window) { - cv::destroyAllWindows(); - } - - if (observation_boards.empty()) { - std::cout << "calibration failed" << std::endl; - return 1; - } else { - std::cout << "points detected" << std::endl; - } - - std::unique_ptr cal_result = - mrcal_main(observation_boards, frames_rt_toref, boardSize, - square_width * 0.0254, imagerSize, 1000); - - auto& stats = *cal_result; - +namespace wpical { +CameraModel MrcalResultToCameraModel(mrcal_result& stats) { // Camera matrix and distortion coefficients - std::vector camera_matrix = { - // fx 0 cx - stats.intrinsics[0], 0, stats.intrinsics[2], - // 0 fy cy - 0, stats.intrinsics[1], stats.intrinsics[3], - // 0 0 1 - 0, 0, 1}; - - std::vector distortion_coefficients = {stats.intrinsics[4], - stats.intrinsics[5], - stats.intrinsics[6], - stats.intrinsics[7], - stats.intrinsics[8], - stats.intrinsics[9], - stats.intrinsics[10], - stats.intrinsics[11], - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0}; - - camera_model.intrinsic_matrix = - Eigen::Matrix(camera_matrix.data()); - camera_model.distortion_coefficients = - Eigen::Matrix(distortion_coefficients.data()); - camera_model.avg_reprojection_error = stats.rms_error; - - return 0; + return {Eigen::Matrix{ + // fx 0 cx + {stats.intrinsics[0], 0, stats.intrinsics[2]}, + // 0 fy cy + {0, stats.intrinsics[1], stats.intrinsics[3]}, + // 0 0 1 + {0, 0, 1}}, + Eigen::Matrix{ + stats.intrinsics[4], stats.intrinsics[5], stats.intrinsics[6], + stats.intrinsics[7], stats.intrinsics[8], stats.intrinsics[9], + stats.intrinsics[10], stats.intrinsics[11]}, + stats.rms_error}; } -int cameracalibration::calibrate(const std::string& input_video, - CameraModel& camera_model, float square_width, - int board_width, int board_height, - double imagerWidthPixels, - double imagerHeightPixels, - bool show_debug_window) { - // Video capture - cv::VideoCapture video_capture(input_video); +/** + * Container for data that's shared between threads. + */ +class Data { + public: + /** + * Adds a frame to the queue for workers to grab from. + * @param mat The mat to queue up. + */ + void AddFrame(cv::Mat&& mat); - // Detection output - std::vector observation_boards; - std::vector frames_rt_toref; + /** + * Returns a frame from the queue. + * @return A frame, or std::nullopt if the queue is empty. + */ + std::optional GetFrame(); - cv::Size boardSize(board_width - 1, board_height - 1); - cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels); + std::optional cameraModel; + std::queue queue; + wpi::util::mutex mutex; +}; - while (video_capture.grab()) { - cv::Mat frame; - video_capture.retrieve(frame); +void Data::AddFrame(cv::Mat&& mat) { + std::scoped_lock lock(mutex); + queue.push(std::move(mat)); +} - if (frame.empty()) { - break; - } - - // Detect - cv::Mat frame_gray; - cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); - - std::vector corners; - - bool found = cv::findChessboardCorners( - frame_gray, boardSize, corners, - cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE); - - if (found) { - std::vector current_points; - for (int i = 0; i < corners.size(); i++) { - current_points.push_back( - mrcal_point3_t{{corners.at(i).x, corners.at(i).y, 1.0f}}); - } - frames_rt_toref.push_back(getSeedPose(current_points.data(), boardSize, - imagerSize, square_width * 0.0254, - 1000)); - observation_boards.insert(observation_boards.end(), - current_points.begin(), current_points.end()); - } - if (show_debug_window) { - cv::drawChessboardCorners(frame, boardSize, corners, found); - cv::imshow("Checkerboard Detection", frame); - if (cv::waitKey(30) == 'q') { - break; - } - } - } - - video_capture.release(); - if (show_debug_window) { - cv::destroyAllWindows(); - } - - if (observation_boards.empty()) { - std::cout << "calibration failed" << std::endl; - return 1; +std::optional Data::GetFrame() { + std::scoped_lock lock(mutex); + if (queue.empty()) { + return std::nullopt; } else { - std::cout << "points detected" << std::endl; + cv::Mat mat = queue.front(); + queue.pop(); + return mat; + } +} + +class Worker { + public: + /** + * @param data A pointer to the queue of Mats. + * @param squareWidth The width of the full square in meters + * @param markerWidth The width of the marker in meters + * @param boardWidth How many markers wide the board is + * @param boardHeight How many markers tall the board is + */ + explicit Worker(std::shared_ptr data, float squareWidth, + float markerWidth, int boardWidth, int boardHeight); + + ~Worker(); + + void ProcessNextImage(cv::Mat image); + + auto GetData() { + return std::make_pair(m_observationBoards, m_framesRtToref); } - std::unique_ptr cal_result = - mrcal_main(observation_boards, frames_rt_toref, boardSize, - square_width * 0.0254, imagerSize, 1000); + int GetProcessedFrameCount(); - auto& stats = *cal_result; + void Stop(); - // Camera matrix and distortion coefficients - std::vector camera_matrix = { - // fx 0 cx - stats.intrinsics[0], 0, stats.intrinsics[2], - // 0 fy cy - 0, stats.intrinsics[1], stats.intrinsics[3], - // 0 0 1 - 0, 0, 1}; + private: + std::atomic_bool m_isDone{false}; + std::atomic_int m_framesProcessed{0}; + std::function()> m_getMat; + int m_boardWidth; + int m_boardHeight; + double m_squareWidth; + std::vector m_observationBoards; + std::vector m_framesRtToref; + // ChArUco Board + cv::aruco::Dictionary m_arucoDict = + cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000); + cv::aruco::CharucoBoard m_charucoBoard; + cv::aruco::CharucoDetector m_charucoDetector; +}; - std::vector distortion_coefficients = {stats.intrinsics[4], - stats.intrinsics[5], - stats.intrinsics[6], - stats.intrinsics[7], - stats.intrinsics[8], - stats.intrinsics[9], - stats.intrinsics[10], - stats.intrinsics[11], - 0.0, - 0.0, - 0.0, - 0.0, - 0.0, - 0.0}; - - // Save calibration output - camera_model.intrinsic_matrix = - Eigen::Matrix(camera_matrix.data()); - camera_model.distortion_coefficients = - Eigen::Matrix(distortion_coefficients.data()); - camera_model.avg_reprojection_error = stats.rms_error; - - return 0; +Worker::Worker(std::shared_ptr data, float squareWidth, float markerWidth, + int boardWidth, int boardHeight) + : m_boardWidth(boardWidth), + m_boardHeight(boardHeight), + m_squareWidth(squareWidth * 0.0254), + m_charucoBoard(cv::Size(boardWidth, boardHeight), squareWidth * 0.0254, + markerWidth * 0.0254, m_arucoDict), + m_charucoDetector(m_charucoBoard) { + std::thread([this, data] { + while (!m_isDone) { + std::optional mat = data->GetFrame(); + if (mat) { + ProcessNextImage(*mat); + ++m_framesProcessed; + } else { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } + }).detach(); } + +Worker::~Worker() { + Stop(); +} + +void Worker::ProcessNextImage(cv::Mat image) { + cv::Size imageSize = image.size(); + + std::vector charucoCorners; + std::vector charucoIds; + std::vector> markerCorners; + std::vector markerIds; + + std::vector objPoints; + std::vector imgPoints; + + std::vector points((m_boardWidth - 1) * (m_boardHeight - 1)); + + m_charucoDetector.detectBoard(image, charucoCorners, charucoIds, + markerCorners, markerIds); + + if (!filter(charucoCorners, charucoIds, markerIds, m_boardWidth, + m_boardHeight)) { + return; + } + + m_charucoBoard.matchImagePoints(charucoCorners, charucoIds, objPoints, + imgPoints); + + for (int i = 0; i < charucoIds.size(); i++) { + int id = charucoIds.at(i); + points[id].x = imgPoints.at(i).x; + points[id].y = imgPoints.at(i).y; + points[id].z = 1.0f; + } + + for (int i = 0; i < points.size(); i++) { + if (points[i].z != 1.0f) { + points[i].x = -1.0f; + points[i].y = -1.0f; + points[i].z = -1.0f; + } + } + + cv::Size boardSize(m_boardWidth - 1, m_boardHeight - 1); + m_framesRtToref.push_back( + getSeedPose(points.data(), boardSize, imageSize, m_squareWidth, 1000)); + m_observationBoards.insert(m_observationBoards.end(), points.begin(), + points.end()); +} + +int Worker::GetProcessedFrameCount() { + return m_framesProcessed; +} + +void Worker::Stop() { + m_isDone = true; +} + +CameraCalibrator::CameraCalibrator(size_t numWorkers, double squareWidth, + double markerWidth, int boardWidth, + int boardHeight, std::string& videoPath) + : m_state(std::make_shared()) { + for (size_t i = 0; i < numWorkers; i++) { + m_workers.emplace_back(std::make_unique( + m_state, squareWidth, markerWidth, boardWidth, boardHeight)); + } + cv::VideoCapture cap{videoPath}; + m_totalFrames = cap.get(cv::CAP_PROP_FRAME_COUNT); + std::thread([this, boardHeight, boardWidth, squareWidth, state = m_state, + capture = std::move(cap)]() mutable { + cv::Size frameShape{ + static_cast(capture.get(cv::CAP_PROP_FRAME_WIDTH)), + static_cast(capture.get(cv::CAP_PROP_FRAME_HEIGHT))}; + while (capture.grab() && !m_isFinished) { + cv::Mat frame; + capture.retrieve(frame); + cv::Mat frameGray; + cv::cvtColor(frame, frameGray, cv::COLOR_BGR2GRAY); + state->AddFrame(std::move(frameGray)); + } + while (TotalFramesProcessed() < TotalFrames() && !m_isFinished) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + if (m_isFinished) { + state->cameraModel = std::nullopt; + return; + } + std::vector allObservationBoards; + std::vector allFramesRtToref; + for (auto& worker : m_workers) { + auto data = worker->GetData(); + allObservationBoards.insert(allObservationBoards.end(), + data.first.begin(), data.first.end()); + allFramesRtToref.insert(allFramesRtToref.end(), data.second.begin(), + data.second.end()); + } + if (allObservationBoards.empty()) { + m_isFinished = true; + return; + } + auto result = mrcal_main(allObservationBoards, allFramesRtToref, + cv::Size(boardWidth - 1, boardHeight - 1), + squareWidth * 0.0254, frameShape, 1000); + state->cameraModel = MrcalResultToCameraModel(*result); + m_isFinished = true; + }).detach(); +} + +CameraCalibrator::~CameraCalibrator() { + Stop(); +} + +bool CameraCalibrator::IsFinished() { + return m_isFinished; +} + +std::optional CameraCalibrator::GetCameraModel() { + return m_state->cameraModel; +} + +int CameraCalibrator::TotalFramesProcessed() { + int totalProcessedFrames = 0; + for (auto& workers : m_workers) { + totalProcessedFrames += workers->GetProcessedFrameCount(); + } + return totalProcessedFrames; +} + +int CameraCalibrator::TotalFrames() { + return m_totalFrames; +} + +void CameraCalibrator::Stop() { + m_isFinished = true; + for (auto& workers : m_workers) { + workers->Stop(); + } +} + +void to_json(wpi::util::json& json, const CameraModel& cameraModel) { + std::vector cameraMatrix( + cameraModel.intrinsicMatrix.data(), + cameraModel.intrinsicMatrix.data() + cameraModel.intrinsicMatrix.size()); + std::vector distortionCoefficients( + cameraModel.distortionCoefficients.data(), + cameraModel.distortionCoefficients.data() + + cameraModel.distortionCoefficients.size()); + json = {{"camera_matrix", cameraMatrix}, + {"distortion_coefficients", distortionCoefficients}, + {"avg_reprojection_error", cameraModel.avgReprojectionError}}; +} + +void from_json(const wpi::util::json& json, CameraModel& cameraModel) { + bool isCalibdb = json.contains("camera"); + Eigen::Matrix3d cameraMatrix; + std::vector distortionCoeffs; + auto mat = json.at("camera_matrix"); + auto distortionCoefficients = json.at("distortion_coefficients"); + if (isCalibdb) { + // OpenCV format has data key + if (mat.contains("data")) { + auto data = mat.at("data").get>(); + cameraMatrix = Eigen::Matrix{data.data()}; + } else { + for (int i = 0; i < cameraMatrix.rows(); i++) { + for (int j = 0; j < cameraMatrix.cols(); j++) { + cameraMatrix(i, j) = mat[i][j]; + } + } + } + + // OpenCV format has data key + if (distortionCoefficients.contains("data")) { + distortionCoeffs = + distortionCoefficients.at("data").get>(); + } else { + distortionCoeffs = distortionCoefficients.get>(); + } + } else { + cameraMatrix = Eigen::Matrix{ + mat.get>().data()}; + distortionCoeffs = distortionCoefficients.get>(); + } + // CalibDB generates JSONs with 5 values. Just zero out the remaining 3 to get + // it to 8 + distortionCoeffs.resize(8, 0); + cameraModel = {cameraMatrix, + Eigen::Matrix{distortionCoeffs.data()}, + json.at("avg_reprojection_error")}; +} +} // namespace wpical diff --git a/tools/wpical/src/main/native/cpp/fieldcalibration.cpp b/tools/wpical/src/main/native/cpp/fieldcalibration.cpp index e11405b426..582de441b9 100644 --- a/tools/wpical/src/main/native/cpp/fieldcalibration.cpp +++ b/tools/wpical/src/main/native/cpp/fieldcalibration.cpp @@ -4,11 +4,8 @@ #include "fieldcalibration.hpp" -#include #include -#include #include -#include #include #include #include @@ -19,13 +16,19 @@ #include #include #include +#include #include #include #include #include -#include "apriltag.h" -#include "tag36h11.h" +#include "cameracalibration.hpp" +#include "wpi/apriltag/AprilTag.hpp" +#include "wpi/apriltag/AprilTagDetector.hpp" +#include "wpi/apriltag/AprilTagDetector_cv.hpp" +#include "wpi/apriltag/AprilTagFieldLayout.hpp" +#include "wpi/math/geometry/Rotation3d.hpp" +#include "wpi/math/geometry/Translation3d.hpp" struct Pose { Eigen::Vector3d p; @@ -34,9 +37,9 @@ struct Pose { }; struct Constraint { - int id_begin; - int id_end; - Pose t_begin_end; + int idBegin; + int idEnd; + Pose tBeginEnd; EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; @@ -90,401 +93,243 @@ class PoseGraphError { const double tagSizeMeters = 0.1651; -inline cameracalibration::CameraModel load_camera_model(std::string path) { - Eigen::Matrix camera_matrix; - Eigen::Matrix camera_distortion; +inline Eigen::Matrix4d EstimateTagPose(std::span tagCorners, + const wpical::CameraModel& cameraModel, + double tagSize) { + cv::Mat cameraMatrix; + cv::Mat cameraDistortion; - std::ifstream file(path); + cv::eigen2cv(cameraModel.intrinsicMatrix, cameraMatrix); + cv::eigen2cv(cameraModel.distortionCoefficients, cameraDistortion); - wpi::util::json json_data; - - try { - json_data = wpi::util::json::parse(file); - } catch (const wpi::util::json::parse_error& e) { - std::cout << e.what() << std::endl; + std::array corners; + for (int i = 0; i < 4; i++) { + corners[i] = {tagCorners[i * 2], tagCorners[i * 2 + 1]}; } - bool isCalibdb = json_data.contains("camera"); + std::vector points3dBoxBase = { + cv::Point3f(-tagSize / 2.0, tagSize / 2.0, 0.0), + cv::Point3f(tagSize / 2.0, tagSize / 2.0, 0.0), + cv::Point3f(tagSize / 2.0, -tagSize / 2.0, 0.0), + cv::Point3f(-tagSize / 2.0, -tagSize / 2.0, 0.0)}; - if (!isCalibdb) { - for (int i = 0; i < camera_matrix.rows(); i++) { - for (int j = 0; j < camera_matrix.cols(); j++) { - camera_matrix(i, j) = - json_data["camera_matrix"][(i * camera_matrix.cols()) + j]; - } - } + std::vector rVec; + std::vector tVec; - for (int i = 0; i < camera_distortion.rows(); i++) { - for (int j = 0; j < camera_distortion.cols(); j++) { - camera_distortion(i, j) = json_data["distortion_coefficients"] - [(i * camera_distortion.cols()) + j]; - } - } - } else { - for (int i = 0; i < camera_matrix.rows(); i++) { - for (int j = 0; j < camera_matrix.cols(); j++) { - try { - camera_matrix(i, j) = json_data["camera_matrix"][i][j]; - } catch (...) { - camera_matrix(i, j) = json_data["camera_matrix"]["data"] - [(i * camera_matrix.cols()) + j]; - } - } - } + cv::solvePnP(points3dBoxBase, corners, cameraMatrix, cameraDistortion, rVec, + tVec, false, cv::SOLVEPNP_SQPNP); - for (int i = 0; i < camera_distortion.rows(); i++) { - for (int j = 0; j < camera_distortion.cols(); j++) { - try { - camera_distortion(i, j) = - json_data["distortion_coefficients"] - [(i * camera_distortion.cols()) + j]; - } catch (...) { - camera_distortion(i, j) = 0.0; - } - } - } - } + cv::Mat rMat; + cv::Rodrigues(rVec, rMat); - cameracalibration::CameraModel camera_model{ - camera_matrix, camera_distortion, json_data["avg_reprojection_error"]}; - return camera_model; -} - -inline cameracalibration::CameraModel load_camera_model( - wpi::util::json json_data) { - // Camera matrix - Eigen::Matrix camera_matrix; - - for (int i = 0; i < camera_matrix.rows(); i++) { - for (int j = 0; j < camera_matrix.cols(); j++) { - camera_matrix(i, j) = - json_data["camera_matrix"][(i * camera_matrix.cols()) + j]; - } - } - - // Distortion coefficients - Eigen::Matrix camera_distortion; - - for (int i = 0; i < camera_distortion.rows(); i++) { - for (int j = 0; j < camera_distortion.cols(); j++) { - camera_distortion(i, j) = json_data["distortion_coefficients"] - [(i * camera_distortion.cols()) + j]; - } - } - - cameracalibration::CameraModel camera_model{ - camera_matrix, camera_distortion, json_data["avg_reprojection_error"]}; - return camera_model; -} - -inline std::map load_ideal_map(std::string path) { - std::ifstream file(path); - wpi::util::json json_data = wpi::util::json::parse(file); - std::map ideal_map; - - for (const auto& element : json_data["tags"]) { - ideal_map[element["ID"]] = element; - } - - return ideal_map; -} - -Eigen::Matrix get_tag_transform( - std::map& ideal_map, int tag_id) { - Eigen::Matrix transform = - Eigen::Matrix::Identity(); - - transform.block<3, 3>(0, 0) = - Eigen::Quaternion( - ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["W"], - ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["X"], - ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["Y"], - ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["Z"]) - .toRotationMatrix(); - - transform(0, 3) = ideal_map[tag_id]["pose"]["translation"]["x"]; - transform(1, 3) = ideal_map[tag_id]["pose"]["translation"]["y"]; - transform(2, 3) = ideal_map[tag_id]["pose"]["translation"]["z"]; - - return transform; -} - -inline Eigen::Matrix estimate_tag_pose( - apriltag_detection_t* tag_detection, - const Eigen::Matrix& camera_matrix, - const Eigen::Matrix& camera_distortion, double tag_size) { - cv::Mat camera_matrix_cv; - cv::Mat camera_distortion_cv; - - cv::eigen2cv(camera_matrix, camera_matrix_cv); - cv::eigen2cv(camera_distortion, camera_distortion_cv); - - std::vector points_2d = { - cv::Point2f(tag_detection->p[0][0], tag_detection->p[0][1]), - cv::Point2f(tag_detection->p[1][0], tag_detection->p[1][1]), - cv::Point2f(tag_detection->p[2][0], tag_detection->p[2][1]), - cv::Point2f(tag_detection->p[3][0], tag_detection->p[3][1])}; - - std::vector points_3d_box_base = { - cv::Point3f(-tag_size / 2.0, tag_size / 2.0, 0.0), - cv::Point3f(tag_size / 2.0, tag_size / 2.0, 0.0), - cv::Point3f(tag_size / 2.0, -tag_size / 2.0, 0.0), - cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, 0.0)}; - - std::vector r_vec; - std::vector t_vec; - - cv::solvePnP(points_3d_box_base, points_2d, camera_matrix_cv, - camera_distortion_cv, r_vec, t_vec, false, cv::SOLVEPNP_SQPNP); - - cv::Mat r_mat; - cv::Rodrigues(r_vec, r_mat); - - Eigen::Matrix camera_to_tag{ - {r_mat.at(0, 0), r_mat.at(0, 1), r_mat.at(0, 2), - t_vec.at(0)}, - {r_mat.at(1, 0), r_mat.at(1, 1), r_mat.at(1, 2), - t_vec.at(1)}, - {r_mat.at(2, 0), r_mat.at(2, 1), r_mat.at(2, 2), - t_vec.at(2)}, + Eigen::Matrix cameraToTag{ + {rMat.at(0, 0), rMat.at(0, 1), rMat.at(0, 2), + tVec.at(0)}, + {rMat.at(1, 0), rMat.at(1, 1), rMat.at(1, 2), + tVec.at(1)}, + {rMat.at(2, 0), rMat.at(2, 1), rMat.at(2, 2), + tVec.at(2)}, {0.0, 0.0, 0.0, 1.0}}; - return camera_to_tag; + return cameraToTag; } -inline void draw_tag_cube(cv::Mat& frame, - Eigen::Matrix camera_to_tag, - const Eigen::Matrix& camera_matrix, - const Eigen::Matrix& camera_distortion, - double tag_size) { - cv::Mat camera_matrix_cv; - cv::Mat camera_distortion_cv; +inline void DrawTagCube(cv::Mat& frame, Eigen::Matrix4d cameraToTag, + const wpical::CameraModel& cameraModel, + double tagSize) { + cv::Mat cameraMatrix; + cv::Mat cameraDistortion; - cv::eigen2cv(camera_matrix, camera_matrix_cv); - cv::eigen2cv(camera_distortion, camera_distortion_cv); + cv::eigen2cv(cameraModel.intrinsicMatrix, cameraMatrix); + cv::eigen2cv(cameraModel.distortionCoefficients, cameraDistortion); - std::vector points_3d_box_base = { - cv::Point3f(-tag_size / 2.0, tag_size / 2.0, 0.0), - cv::Point3f(tag_size / 2.0, tag_size / 2.0, 0.0), - cv::Point3f(tag_size / 2.0, -tag_size / 2.0, 0.0), - cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, 0.0)}; + std::vector points3dBoxBase = { + cv::Point3f(-tagSize / 2.0, tagSize / 2.0, 0.0), + cv::Point3f(tagSize / 2.0, tagSize / 2.0, 0.0), + cv::Point3f(tagSize / 2.0, -tagSize / 2.0, 0.0), + cv::Point3f(-tagSize / 2.0, -tagSize / 2.0, 0.0)}; - std::vector points_3d_box_top = { - cv::Point3f(-tag_size / 2.0, tag_size / 2.0, -tag_size), - cv::Point3f(tag_size / 2.0, tag_size / 2.0, -tag_size), - cv::Point3f(tag_size / 2.0, -tag_size / 2.0, -tag_size), - cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, -tag_size)}; + std::vector points3dBoxTop = { + cv::Point3f(-tagSize / 2.0, tagSize / 2.0, -tagSize), + cv::Point3f(tagSize / 2.0, tagSize / 2.0, -tagSize), + cv::Point3f(tagSize / 2.0, -tagSize / 2.0, -tagSize), + cv::Point3f(-tagSize / 2.0, -tagSize / 2.0, -tagSize)}; - std::vector points_2d_box_base = { + std::vector points2dBoxBase = { cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0)}; - std::vector points_2d_box_top = { + std::vector points2dBoxTop = { cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0)}; - Eigen::Matrix r_vec = camera_to_tag.block<3, 3>(0, 0); - Eigen::Matrix t_vec = camera_to_tag.block<3, 1>(0, 3); + Eigen::Matrix rVec = cameraToTag.block<3, 3>(0, 0); + Eigen::Matrix tVec = cameraToTag.block<3, 1>(0, 3); - cv::Mat r_vec_cv; - cv::Mat t_vec_cv; + cv::Mat rVecCv; + cv::Mat tVecCv; - cv::eigen2cv(r_vec, r_vec_cv); - cv::eigen2cv(t_vec, t_vec_cv); + cv::eigen2cv(rVec, rVecCv); + cv::eigen2cv(tVec, tVecCv); - cv::projectPoints(points_3d_box_base, r_vec_cv, t_vec_cv, camera_matrix_cv, - camera_distortion_cv, points_2d_box_base); - cv::projectPoints(points_3d_box_top, r_vec_cv, t_vec_cv, camera_matrix_cv, - camera_distortion_cv, points_2d_box_top); + cv::projectPoints(points3dBoxBase, rVecCv, tVecCv, cameraMatrix, + cameraDistortion, points2dBoxBase); + cv::projectPoints(points3dBoxTop, rVecCv, tVecCv, cameraMatrix, + cameraDistortion, points2dBoxTop); for (int i = 0; i < 4; i++) { - cv::Point2f& point_base = points_2d_box_base.at(i); - cv::Point2f& point_top = points_2d_box_top.at(i); + cv::Point2f& pointBase = points2dBoxBase.at(i); + cv::Point2f& pointTop = points2dBoxTop.at(i); - cv::line(frame, point_base, point_top, cv::Scalar(0, 255, 255), 5); + cv::line(frame, pointBase, pointTop, cv::Scalar(0, 255, 255), 5); - int i_next = (i + 1) % 4; - cv::Point2f& point_base_next = points_2d_box_base.at(i_next); - cv::Point2f& point_top_next = points_2d_box_top.at(i_next); + int next = (i + 1) % 4; + cv::Point2f& pointBaseNext = points2dBoxBase.at(next); + cv::Point2f& pointTopNext = points2dBoxTop.at(next); - cv::line(frame, point_base, point_base_next, cv::Scalar(0, 255, 255), 5); - cv::line(frame, point_top, point_top_next, cv::Scalar(0, 255, 255), 5); + cv::line(frame, pointBase, pointBaseNext, cv::Scalar(0, 255, 255), 5); + cv::line(frame, pointTop, pointTopNext, cv::Scalar(0, 255, 255), 5); } } -inline bool process_video_file( - apriltag_detector_t* tag_detector, - const Eigen::Matrix& camera_matrix, - const Eigen::Matrix& camera_distortion, double tag_size, +inline bool ProcessVideoFile( + wpi::apriltag::AprilTagDetector& detector, + const wpical::CameraModel& cameraModel, double tagSize, const std::string& path, std::map, Eigen::aligned_allocator>>& poses, std::vector>& constraints, - bool show_debug_window) { - if (show_debug_window) { + bool showDebugWindow) { + if (showDebugWindow) { cv::namedWindow("Processing Frame", cv::WINDOW_NORMAL); } - cv::VideoCapture video_input(path); + cv::VideoCapture videoInput(path); - if (!video_input.isOpened()) { - std::cout << "Unable to open video " << path << std::endl; + if (!videoInput.isOpened()) { return false; } cv::Mat frame; - cv::Mat frame_gray; - cv::Mat frame_debug; - - int frame_num = 0; - - while (video_input.read(frame)) { - std::cout << "Processing " << path << " - Frame " << frame_num++ - << std::endl; + cv::Mat frameGray; + cv::Mat frameDebug; + while (videoInput.read(frame)) { // Convert color frame to grayscale frame - cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); + cv::cvtColor(frame, frameGray, cv::COLOR_BGR2GRAY); // Clone color frame for debugging - frame_debug = frame.clone(); + frameDebug = frame.clone(); - // Detect tags - image_u8_t tag_image = {frame_gray.cols, frame_gray.rows, frame_gray.cols, - frame_gray.data}; - zarray_t* tag_detections = - apriltag_detector_detect(tag_detector, &tag_image); + auto results = wpi::apriltag::AprilTagDetect(detector, frameGray); // Skip this loop if there are no tags detected - if (zarray_size(tag_detections) == 0) { - std::cout << "No tags detected" << std::endl; + if (results.empty()) { continue; } // Find detection with the smallest tag ID - apriltag_detection_t* tag_detection_min = nullptr; - zarray_get(tag_detections, 0, &tag_detection_min); - - for (int i = 0; i < zarray_size(tag_detections); i++) { - apriltag_detection_t* tag_detection_i; - zarray_get(tag_detections, i, &tag_detection_i); - - if (tag_detection_i->id < tag_detection_min->id) { - tag_detection_min = tag_detection_i; + auto tagDetectionMin = results.front(); + for (auto detection : results) { + if (detection->GetId() < tagDetectionMin->GetId()) { + tagDetectionMin = detection; } } - - Eigen::Matrix camera_to_tag_min = estimate_tag_pose( - tag_detection_min, camera_matrix, camera_distortion, tag_size); + std::array cornerBuf; + Eigen::Matrix4d cameraToTagMin; + try { + cameraToTagMin = EstimateTagPose(tagDetectionMin->GetCorners(cornerBuf), + cameraModel, tagSize); + } catch (...) { + // SQPNP failed, probably because the camera model is bad + return false; + } // Find transformation from smallest tag ID - for (int i = 0; i < zarray_size(tag_detections); i++) { - apriltag_detection_t* tag_detection_i; - zarray_get(tag_detections, i, &tag_detection_i); - + for (auto detection : results) { // Add tag ID to poses - if (poses.find(tag_detection_i->id) == poses.end()) { - poses[tag_detection_i->id] = {Eigen::Vector3d(0.0, 0.0, 0.0), - Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)}; + if (poses.find(detection->GetId()) == poses.end()) { + poses[detection->GetId()] = {Eigen::Vector3d(0.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)}; } - + std::array corners; // Estimate camera to tag pose - Eigen::Matrix caamera_to_tag = estimate_tag_pose( - tag_detection_i, camera_matrix, camera_distortion, tag_size); - + Eigen::Matrix4d cameraToTag; + try { + cameraToTag = EstimateTagPose(detection->GetCorners(corners), + cameraModel, tagSize); + } catch (...) { + // SQPNP failed, probably because the camera model is bad + return false; + } // Draw debug cube - if (show_debug_window) { - draw_tag_cube(frame_debug, caamera_to_tag, camera_matrix, - camera_distortion, tag_size); + if (showDebugWindow) { + DrawTagCube(frameDebug, cameraToTag, cameraModel, tagSize); } // Skip finding transformation from smallest tag ID to itself - if (tag_detection_i->id == tag_detection_min->id) { + if (detection->GetId() == tagDetectionMin->GetId()) { continue; } - Eigen::Matrix tag_min_to_tag = - camera_to_tag_min.inverse() * caamera_to_tag; + Eigen::Matrix4d tagMinToTag = cameraToTagMin.inverse() * cameraToTag; // Constraint Constraint constraint; - constraint.id_begin = tag_detection_min->id; - constraint.id_end = tag_detection_i->id; - constraint.t_begin_end.p = tag_min_to_tag.block<3, 1>(0, 3); - constraint.t_begin_end.q = - Eigen::Quaterniond(tag_min_to_tag.block<3, 3>(0, 0)); + constraint.idBegin = tagDetectionMin->GetId(); + constraint.idEnd = detection->GetId(); + constraint.tBeginEnd.p = tagMinToTag.block<3, 1>(0, 3); + constraint.tBeginEnd.q = + Eigen::Quaterniond(tagMinToTag.block<3, 3>(0, 0)); constraints.push_back(constraint); } - apriltag_detections_destroy(tag_detections); - // Show debug - if (show_debug_window) { - cv::imshow("Processing Frame", frame_debug); + if (showDebugWindow) { + cv::imshow("Processing Frame", frameDebug); cv::waitKey(1); } } - video_input.release(); - if (show_debug_window) { + videoInput.release(); + if (showDebugWindow) { cv::destroyAllWindows(); } return true; } -int fieldcalibration::calibrate(std::string input_dir_path, - wpi::util::json& output_json, - std::string camera_model_path, - std::string ideal_map_path, int pinned_tag_id, - bool show_debug_window) { +wpical::FieldCalibrator::~FieldCalibrator() { + if (m_processingThread.joinable()) { + m_processingThread.detach(); + } +} + +std::optional wpical::calibrate( + std::string inputDirPath, wpical::CameraModel& cameraModel, + const wpi::apriltag::AprilTagFieldLayout& idealLayout, int pinnedTagId, + bool showDebugWindow) { // Silence OpenCV logging cv::utils::logging::setLogLevel( cv::utils::logging::LogLevel::LOG_LEVEL_SILENT); - // Load camera model - Eigen::Matrix3d camera_matrix; - Eigen::Matrix camera_distortion; - - try { - auto camera_model = load_camera_model(camera_model_path); - camera_matrix = camera_model.intrinsic_matrix; - camera_distortion = camera_model.distortion_coefficients; - } catch (...) { - return 1; - } - - wpi::util::json json = wpi::util::json::parse(std::ifstream(ideal_map_path)); - if (!json.contains("tags")) { - return 1; - } - - // Load ideal field map - std::map ideal_map; - try { - ideal_map = load_ideal_map(ideal_map_path); - } catch (...) { - 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; + bool pinnedTagFound = false; + // Check if pinned tag is in ideal layout + for (const auto& tag : idealLayout.GetTags()) { + if (tag.ID == pinnedTagId) { + pinnedTagFound = true; break; } } - if (!pinned_tag_found) { - return 1; + if (!pinnedTagFound) { + return std::nullopt; } // Apriltag detector - apriltag_detector_t* tag_detector = apriltag_detector_create(); - tag_detector->nthreads = 8; - - apriltag_family_t* tag_family = tag36h11_create(); - apriltag_detector_add_family(tag_detector, tag_family); + wpi::apriltag::AprilTagDetector detector; + detector.SetConfig({.numThreads = 8}); + detector.AddFamily("tag36h11"); // Find tag poses std::map, @@ -492,53 +337,44 @@ int fieldcalibration::calibrate(std::string input_dir_path, poses; std::vector> constraints; - for (const auto& entry : - std::filesystem::directory_iterator(input_dir_path)) { + for (const auto& entry : std::filesystem::directory_iterator(inputDirPath)) { if (entry.path().filename().string()[0] == '.') { continue; } - - const std::string path = entry.path().string(); - - bool success = process_video_file(tag_detector, camera_matrix, - camera_distortion, tagSizeMeters, path, - poses, constraints, show_debug_window); - - if (!success) { - std::cout << "Unable to process video " << path << std::endl; - return 1; + if (!ProcessVideoFile(detector, cameraModel, tagSizeMeters, + entry.path().string(), poses, constraints, + showDebugWindow)) { + return std::nullopt; } } // Build optimization problem ceres::Problem problem; - ceres::Manifold* quaternion_manifold = new ceres::EigenQuaternionManifold; + ceres::Manifold* quaternionManifold = new ceres::EigenQuaternionManifold; for (const auto& constraint : constraints) { - auto pose_begin_iter = poses.find(constraint.id_begin); - auto pose_end_iter = poses.find(constraint.id_end); + auto poseBeginIter = poses.find(constraint.idBegin); + auto poseEndIter = poses.find(constraint.idEnd); - ceres::CostFunction* cost_function = - PoseGraphError::Create(constraint.t_begin_end); + ceres::CostFunction* costFunction = + PoseGraphError::Create(constraint.tBeginEnd); - problem.AddResidualBlock(cost_function, nullptr, - pose_begin_iter->second.p.data(), - pose_begin_iter->second.q.coeffs().data(), - pose_end_iter->second.p.data(), - pose_end_iter->second.q.coeffs().data()); + problem.AddResidualBlock( + costFunction, nullptr, poseBeginIter->second.p.data(), + poseBeginIter->second.q.coeffs().data(), poseEndIter->second.p.data(), + poseEndIter->second.q.coeffs().data()); - problem.SetManifold(pose_begin_iter->second.q.coeffs().data(), - quaternion_manifold); - problem.SetManifold(pose_end_iter->second.q.coeffs().data(), - quaternion_manifold); + problem.SetManifold(poseBeginIter->second.q.coeffs().data(), + quaternionManifold); + problem.SetManifold(poseEndIter->second.q.coeffs().data(), + quaternionManifold); } // Pin tag - auto pinned_tag_iter = poses.find(pinned_tag_id); - if (pinned_tag_iter != poses.end()) { - problem.SetParameterBlockConstant(pinned_tag_iter->second.p.data()); - problem.SetParameterBlockConstant( - pinned_tag_iter->second.q.coeffs().data()); + auto pinnedTagIter = poses.find(pinnedTagId); + if (pinnedTagIter != poses.end()) { + problem.SetParameterBlockConstant(pinnedTagIter->second.p.data()); + problem.SetParameterBlockConstant(pinnedTagIter->second.q.coeffs().data()); } // Solve @@ -550,62 +386,30 @@ int fieldcalibration::calibrate(std::string input_dir_path, ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); - std::cout << summary.BriefReport() << std::endl; + Eigen::Matrix4d correctionA; + correctionA << 0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1; - // Output - std::map observed_map = ideal_map; + Eigen::Matrix4d correctionB; + correctionB << 0, 1, 0, 0, 0, 0, -1, 0, -1, 0, 0, 0, 0, 0, 0, 1; - Eigen::Matrix correction_a; - correction_a << 0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1; + Eigen::Matrix4d pinnedTagTransform = + idealLayout.GetTagPose(pinnedTagId)->ToMatrix(); - Eigen::Matrix correction_b; - correction_b << 0, 1, 0, 0, 0, 0, -1, 0, -1, 0, 0, 0, 0, 0, 0, 1; - - Eigen::Matrix pinned_tag_transform = - get_tag_transform(ideal_map, pinned_tag_id); - - for (const auto& [tag_id, pose] : poses) { + std::vector tags; + for (const auto& [tagId, pose] : poses) { // Transformation from pinned tag - Eigen::Matrix transform = - Eigen::Matrix::Identity(); + Eigen::Matrix4d transform = Eigen::Matrix4d::Identity(); transform.block<3, 3>(0, 0) = pose.q.toRotationMatrix(); transform.block<3, 1>(0, 3) = pose.p; // Transformation from world - Eigen::Matrix corrected_transform = - pinned_tag_transform * correction_a * transform * correction_b; - Eigen::Quaternion corrected_transform_q( - corrected_transform.block<3, 3>(0, 0)); - - observed_map[tag_id]["pose"]["translation"]["x"] = - corrected_transform(0, 3); - observed_map[tag_id]["pose"]["translation"]["y"] = - corrected_transform(1, 3); - observed_map[tag_id]["pose"]["translation"]["z"] = - corrected_transform(2, 3); - - observed_map[tag_id]["pose"]["rotation"]["quaternion"]["X"] = - corrected_transform_q.x(); - observed_map[tag_id]["pose"]["rotation"]["quaternion"]["Y"] = - corrected_transform_q.y(); - observed_map[tag_id]["pose"]["rotation"]["quaternion"]["Z"] = - corrected_transform_q.z(); - observed_map[tag_id]["pose"]["rotation"]["quaternion"]["W"] = - corrected_transform_q.w(); + Eigen::Matrix4d correctedTransform = + pinnedTagTransform * correctionA * transform * correctionB; + // TODO: remove variable when clang 16 is available on Mac + wpi::apriltag::AprilTag tag{tagId, wpi::math::Pose3d{correctedTransform}}; + tags.emplace_back(tag); } - - wpi::util::json observed_map_json; - - for (const auto& [tag_id, tag_json] : observed_map) { - observed_map_json["tags"].push_back(tag_json); - } - - observed_map_json["field"] = { - {"length", static_cast(json.at("field").at("length"))}, - {"width", static_cast(json.at("field").at("width"))}}; - - output_json = observed_map_json; - - return 0; + return wpi::apriltag::AprilTagFieldLayout{tags, idealLayout.GetFieldLength(), + idealLayout.GetFieldWidth()}; } diff --git a/tools/wpical/src/main/native/cpp/fmap.cpp b/tools/wpical/src/main/native/cpp/fmap.cpp index 6ed9944afa..c584eb2cd2 100644 --- a/tools/wpical/src/main/native/cpp/fmap.cpp +++ b/tools/wpical/src/main/native/cpp/fmap.cpp @@ -5,36 +5,49 @@ #include "fmap.hpp" #include +#include #include -wpi::util::json fmap::singleTag(int tag, const tag::Pose& tagpose) { - std::vector transform = {}; - for (int i = 0; i < 4; i++) { - for (int j = 0; j < 4; j++) { - transform.push_back(tagpose.transformMatrixFmap(i, j)); - } - } +#include - return {{"family", "apriltag3_36h11_classic"}, - {"id", tag}, - {"size", 165.1}, - {"transform", transform}, - {"unique", true}}; +#include "wpi/apriltag/AprilTagFieldLayout.hpp" + +using namespace fmap; + +Fieldmap::Fieldmap(std::string type, std::vector fiducials) + : type(std::move(type)), fiducials(std::move(fiducials)) {} + +Fieldmap::Fieldmap(const wpi::apriltag::AprilTagFieldLayout& layout) + : type("frc") { + // https://docs.limelightvision.io/docs/docs-limelight/pipeline-apriltag/apriltag-map-specification + for (auto tag : layout.GetTags()) { + // TODO: remove variable when clang 16 is available on Mac + Fiducial fiducial{"apriltag3_36h11_classic", tag.ID, 165.1, + tag.pose.ToMatrix(), 1}; + fiducials.emplace_back(fiducial); + } } -wpi::util::json fmap::convertfmap(const wpi::util::json& json) { - std::string fmapstart = "{\"fiducials\":["; - - std::string fmapend = "],\"type\":\"frc\"}"; - - Fieldmap fieldmap(json); - - for (int i = 0; i < fieldmap.getNumTags(); i++) { - fmapstart += singleTag(i + 1, fieldmap.getTag(i + 1)).dump(); - if (i != fieldmap.getNumTags() - 1) { - fmapstart += ","; - } - } - - return wpi::util::json::parse(fmapstart.append(fmapend)); +void fmap::to_json(wpi::util::json& json, const Fiducial& layout) { + json = {{"family", layout.family}, + {"id", layout.id}, + {"size", layout.size}, + {"transform", std::vector{layout.transform.data(), + layout.transform.data() + + layout.transform.size()}}, + {"unique", layout.unique}}; +} + +void fmap::from_json(const wpi::util::json& json, Fiducial& layout) { + auto vec = json.at("transform").get>(); + layout = {json.at("family"), json.at("id"), json.at("size"), + Eigen::Matrix4d{vec.data()}, json.at("unique")}; +} + +void fmap::to_json(wpi::util::json& json, const Fieldmap& layout) { + json = {{"type", "frc"}, {"fiducials", layout.fiducials}}; +} + +void fmap::from_json(const wpi::util::json& json, Fieldmap& layout) { + layout = {json.at("type"), json.at("fiducials")}; } diff --git a/tools/wpical/src/main/native/cpp/tagpose.cpp b/tools/wpical/src/main/native/cpp/tagpose.cpp deleted file mode 100644 index 8137679996..0000000000 --- a/tools/wpical/src/main/native/cpp/tagpose.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#include "tagpose.hpp" - -#include "wpi/util/deprecated.hpp" - -WPI_IGNORE_DEPRECATED -namespace tag { -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; - quaternion = Eigen::Quaterniond(w, x, y, z); - rotationMatrix = quaternion.toRotationMatrix(); - transformMatrixFmap.setZero(); - transformMatrixFmap.block<3, 3>(0, 0) = rotationMatrix; - transformMatrixFmap(0, 3) = xpos - (field_length_meters / 2.0); - transformMatrixFmap(1, 3) = ypos - (field_width_meters / 2.0); - transformMatrixFmap(2, 3) = zpos; - transformMatrixFmap(3, 3) = 1; - transformMatrixFmap(3, 0) = 0; - transformMatrixFmap(3, 1) = 0; - transformMatrixFmap(3, 2) = 0; - Eigen::Vector3d eulerAngles = rotationMatrix.eulerAngles(0, 1, 2); - rollRot = eulerAngles[0]; - pitchRot = eulerAngles[1]; - yawRot = eulerAngles[2]; -} - -wpi::util::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 diff --git a/tools/wpical/src/main/native/include/cameracalibration.hpp b/tools/wpical/src/main/native/include/cameracalibration.hpp index c1210270b4..11046cd68c 100644 --- a/tools/wpical/src/main/native/include/cameracalibration.hpp +++ b/tools/wpical/src/main/native/include/cameracalibration.hpp @@ -4,49 +4,97 @@ #pragma once -#include +#include +#include #include +#include #include #include +#include +#include +#include +#include +#include +#include #include "wpi/util/json.hpp" +#include "wpi/util/mutex.hpp" -namespace cameracalibration { +namespace wpical { struct CameraModel { - Eigen::Matrix intrinsic_matrix; - Eigen::Matrix distortion_coefficients; - double avg_reprojection_error; + Eigen::Matrix intrinsicMatrix; + Eigen::Matrix distortionCoefficients; + double avgReprojectionError = -1; }; -int calibrate(const std::string& input_video, CameraModel& camera_model, - float square_width, float marker_width, int board_width, - int board_height, bool show_debug_window); -int calibrate(const std::string& input_video, CameraModel& camera_model, - float square_width, float marker_width, int board_width, - int board_height, double imagerWidthPixels, - double imagerHeightPixels, bool show_debug_window); -int calibrate(const std::string& input_video, CameraModel& camera_model, - float square_width, int board_width, int board_height, - double imagerWidthPixels, double imagerHeightPixels, - bool show_debug_window); -static void dumpJson(CameraModel& camera_model, - const std::string& output_file_path) { - std::vector camera_matrix(camera_model.intrinsic_matrix.data(), - camera_model.intrinsic_matrix.data() + - camera_model.intrinsic_matrix.size()); - std::vector distortion_coefficients( - camera_model.distortion_coefficients.data(), - camera_model.distortion_coefficients.data() + - camera_model.distortion_coefficients.size()); +CameraModel MrcalResultToCameraModel(mrcal_result& stats); - wpi::util::json result = { - {"camera_matrix", camera_matrix}, - {"distortion_coefficients", distortion_coefficients}, - {"avg_reprojection_error", camera_model.avg_reprojection_error}}; +class Data; +class Worker; - std::ofstream output_file(output_file_path); - output_file << result.dump(4) << std::endl; - output_file.close(); -} -} // namespace cameracalibration +class CameraCalibrator { + public: + /** + * Opens the video at the path, and performs camera calibration using the + * frames in the video. + * + * @param numWorkers The number of threads to spawn. + * @param squareWidth The width of the full square in meters. + * @param markerWidth The width of the marker in meters. + * @param boardWidth How many markers wide the board is. + * @param boardHeight How many markers tall the board is. + * @param videoPath The absolute path to the video. + */ + CameraCalibrator(size_t numWorkers, double squareWidth, double markerWidth, + int boardWidth, int boardHeight, std::string& videoPath); + + ~CameraCalibrator(); + + /** + * Returns whether or not all frames have been processed or whether or not the + * camera calibrator has been stopped. + * @return true if all frames have been processed or if the camera calibrator + * has been stopped, false otherwise. + */ + bool IsFinished(); + + /** + * Gets the camera model. + * @return std::nullopt if the camera calibrator is not finished processing or + * if calibration failed. Returns CameraModel if it's finished and calibration + * was successful. If calibration is successful, there will always be a valid + * CameraModel before IsFinished returns true. + */ + std::optional GetCameraModel(); + + /** + * Returns the total number of frames that have been processed. + * @return The total number of processed frames. + */ + int TotalFramesProcessed(); + + /** + * Returns the total number of frames in the video. + * @return The total number of frames. + */ + int TotalFrames(); + + /** + * Stops all worker threads. + */ + void Stop(); + + private: + // Ensures that shared state lives until everything else is destroyed + std::shared_ptr m_state; + + std::atomic_bool m_isFinished{false}; + std::atomic_int m_totalFrames; + std::vector> m_workers; +}; + +void to_json(wpi::util::json& json, const CameraModel& cameraModel); + +void from_json(const wpi::util::json& json, CameraModel& cameraModel); +} // namespace wpical diff --git a/tools/wpical/src/main/native/include/fieldcalibration.hpp b/tools/wpical/src/main/native/include/fieldcalibration.hpp index 582e19cb8c..fc7d699ac3 100644 --- a/tools/wpical/src/main/native/include/fieldcalibration.hpp +++ b/tools/wpical/src/main/native/include/fieldcalibration.hpp @@ -4,13 +4,42 @@ #pragma once +#include #include +#include #include "cameracalibration.hpp" -#include "wpi/util/json.hpp" +#include "wpi/apriltag/AprilTagFieldLayout.hpp" -namespace fieldcalibration { -int calibrate(std::string input_dir_path, wpi::util::json& output_json, - std::string camera_model_path, std::string ideal_map_path, - int pinned_tag_id, bool show_debug_window); -} // namespace fieldcalibration +namespace wpical { +std::optional calibrate( + std::string inputDirPath, wpical::CameraModel& cameraModel, + const wpi::apriltag::AprilTagFieldLayout& idealLayout, int pinnedTagId, + bool showDebugWindow); + +class FieldCalibrator { + public: + ~FieldCalibrator(); + + bool IsFinished() { return m_isFinished; } + + std::optional GetAprilTagFieldLayout() { + return m_fieldLayout; + } + + void Calibrate(std::string inputDirPath, wpical::CameraModel& cameraModel, + const wpi::apriltag::AprilTagFieldLayout& idealLayout, + int pinnedTagId, bool showDebugWindow) { + m_processingThread = std::thread([=, this]() mutable { + this->m_fieldLayout = calibrate(inputDirPath, cameraModel, idealLayout, + pinnedTagId, showDebugWindow); + this->m_isFinished = true; + }); + } + + private: + std::atomic_bool m_isFinished{false}; + std::thread m_processingThread; + std::optional m_fieldLayout; +}; +} // namespace wpical diff --git a/tools/wpical/src/main/native/include/fieldmap.hpp b/tools/wpical/src/main/native/include/fieldmap.hpp deleted file mode 100644 index 352d4ab5d2..0000000000 --- a/tools/wpical/src/main/native/include/fieldmap.hpp +++ /dev/null @@ -1,82 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#pragma once - -#include -#include - -#include "tagpose.hpp" -#include "wpi/util/json.hpp" - -class Fieldmap { - public: - Fieldmap() = default; - explicit Fieldmap(const wpi::util::json& json) { - double field_length_meters = - static_cast(json.at("field").at("length")); - double field_width_meters = - static_cast(json.at("field").at("width")); - for (const auto& tag : json.at("tags").items()) { - double tag_id = static_cast(tag.value().at("ID")); - double tagXPos = - static_cast(tag.value().at("pose").at("translation").at("x")); - double tagYPos = - static_cast(tag.value().at("pose").at("translation").at("y")); - double tagZPos = - static_cast(tag.value().at("pose").at("translation").at("z")); - double tagWQuat = static_cast( - tag.value().at("pose").at("rotation").at("quaternion").at("W")); - double tagXQuat = static_cast( - tag.value().at("pose").at("rotation").at("quaternion").at("X")); - double tagYQuat = static_cast( - tag.value().at("pose").at("rotation").at("quaternion").at("Y")); - double tagZQuat = static_cast( - tag.value().at("pose").at("rotation").at("quaternion").at("Z")); - - 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 tagMap.at(tag); } - - int getNumTags() const { return tagMap.size(); } - - bool hasTag(int tag) { return tagMap.find(tag) != tagMap.end(); } - - wpi::util::json toJson() { - wpi::util::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); - if (angle > 180) { - return angle - 360; - } else if (angle < -180) { - return angle + 360; - } - return angle; - } - - void replaceTag(int tag_id, tag::Pose pose) { - tagMap.erase(tag_id); - tagMap.emplace(tag_id, pose); - } - - private: - double fieldLength; - double fieldWidth; - std::map tagMap; -}; diff --git a/tools/wpical/src/main/native/include/fmap.hpp b/tools/wpical/src/main/native/include/fmap.hpp index d63b3190d2..af1429a594 100644 --- a/tools/wpical/src/main/native/include/fmap.hpp +++ b/tools/wpical/src/main/native/include/fmap.hpp @@ -4,11 +4,39 @@ #pragma once -#include "fieldmap.hpp" -#include "tagpose.hpp" +#include +#include + +#include + +#include "wpi/apriltag/AprilTagFieldLayout.hpp" #include "wpi/util/json.hpp" namespace fmap { -wpi::util::json singleTag(int tag, const tag::Pose& tagpose); -wpi::util::json convertfmap(const wpi::util::json& json); +struct Fiducial { + std::string family; + int id; + double size; + Eigen::Matrix4d transform; + int unique = 1; +}; + +class Fieldmap { + public: + Fieldmap() = default; + Fieldmap(std::string type, std::vector fiducials); + + explicit Fieldmap(const wpi::apriltag::AprilTagFieldLayout& layout); + + std::string type; + std::vector fiducials; +}; + +void to_json(wpi::util::json& json, const Fiducial& layout); + +void from_json(const wpi::util::json& json, Fiducial& layout); + +void to_json(wpi::util::json& json, const Fieldmap& layout); + +void from_json(const wpi::util::json& json, Fieldmap& layout); } // namespace fmap diff --git a/tools/wpical/src/main/native/include/tagpose.hpp b/tools/wpical/src/main/native/include/tagpose.hpp deleted file mode 100644 index 596ed3c5c6..0000000000 --- a/tools/wpical/src/main/native/include/tagpose.hpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#pragma once - -#include -#include - -#include "wpi/util/json.hpp" - -namespace tag { -class Pose { - public: - 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::util::json toJson(); -}; -} // namespace tag diff --git a/tools/wpical/src/test/native/cpp/test_calibrate.cpp b/tools/wpical/src/test/native/cpp/test_calibrate.cpp index 3979d2a835..a5b582034c 100644 --- a/tools/wpical/src/test/native/cpp/test_calibrate.cpp +++ b/tools/wpical/src/test/native/cpp/test_calibrate.cpp @@ -2,6 +2,8 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. +#include +#include #include #include @@ -9,6 +11,8 @@ #include "cameracalibration.hpp" #include "fieldcalibration.hpp" #include "path_lookup.hpp" +#include "wpi/apriltag/AprilTagFieldLayout.hpp" +#include "wpi/apriltag/AprilTagFields.hpp" #include "wpi/util/json.hpp" const std::string projectRootPath = PROJECT_ROOT_PATH; @@ -21,12 +25,6 @@ const std::string calSavePath = 0, projectRootPath.find("/src/main/native/assets")) + "/build") : std::string(tmpdir_c_str); -cameracalibration::CameraModel cameraModel = { - .intrinsic_matrix = Eigen::Matrix::Identity(), - .distortion_coefficients = Eigen::Matrix::Zero(), - .avg_reprojection_error = 0.0}; - -wpi::util::json output_json; #ifdef __linux__ const std::string fileSuffix = ".avi"; @@ -36,80 +34,93 @@ const std::string fileSuffix = ".mp4"; const std::string videoLocation = "/fieldvideo"; #endif -TEST(Camera_CalibrationTest, OpenCV_Typical) { - int ret = cameracalibration::calibrate( - LookupPath(projectRootPath + "/testcalibration" + fileSuffix), - cameraModel, 0.709f, 0.551f, 12, 8, false); - cameracalibration::dumpJson(cameraModel, - calSavePath + "/cameracalibration.json"); - EXPECT_EQ(ret, 0); +TEST(CameraCalibrationTest, Typical) { + auto path = LookupPath(projectRootPath + "/testcalibration" + fileSuffix); + auto calibrator = wpical::CameraCalibrator(4, 0.709, 0.551, 12, 8, path); + while (!calibrator.IsFinished()) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + auto ret = calibrator.GetCameraModel(); + EXPECT_NE(ret, std::nullopt); + std::ofstream output_file(calSavePath + "/cameracalibration.json"); + output_file << wpi::util::json(ret.value()).dump(4) << std::endl; } -TEST(Camera_CalibrationTest, OpenCV_Atypical) { - int ret = cameracalibration::calibrate( - LookupPath(projectRootPath + videoLocation + "/long" + fileSuffix), - cameraModel, 0.709f, 0.551f, 12, 8, false); - EXPECT_EQ(ret, 1); +TEST(CameraCalibrationTest, Atypical) { + auto path = + LookupPath(projectRootPath + videoLocation + "/short" + fileSuffix); + auto calibrator = wpical::CameraCalibrator(4, 0.709, 0.551, 12, 8, path); + while (!calibrator.IsFinished()) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + auto ret = calibrator.GetCameraModel(); + EXPECT_EQ(ret, std::nullopt); } -TEST(Camera_CalibrationTest, MRcal_Typical) { - int ret = cameracalibration::calibrate( - LookupPath(projectRootPath + "/testcalibration" + fileSuffix), - cameraModel, .709f, 12, 8, 1080, 1920, false); - EXPECT_EQ(ret, 0); +TEST(FieldCalibrationTest, Typical) { + auto model = wpi::util::json::parse( + std::ifstream(calSavePath + "/cameracalibration.json")) + .get(); + auto ret = + wpical::calibrate(LookupPath(projectRootPath + videoLocation), model, + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::k2024Crescendo), + 3, false); + EXPECT_NE(ret, std::nullopt); } -TEST(Camera_CalibrationTest, MRcal_Atypical) { - int ret = cameracalibration::calibrate( - LookupPath(projectRootPath + videoLocation + "/short" + fileSuffix), - cameraModel, 0.709f, 12, 8, 1080, 1920, false); - EXPECT_EQ(ret, 1); +TEST(FieldCalibrationTest, Atypical_Bad_Camera_Model) { + wpical::CameraModel model{}; + auto ret = + wpical::calibrate(LookupPath(projectRootPath + videoLocation), model, + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::k2024Crescendo), + 3, false); + EXPECT_EQ(ret, std::nullopt); } -TEST(Field_CalibrationTest, Typical) { - int ret = fieldcalibration::calibrate( - LookupPath(projectRootPath + videoLocation), output_json, - calSavePath + "/cameracalibration.json", - LookupPath(projectRootPath + "/2024-crescendo.json"), 3, false); - EXPECT_EQ(ret, 0); +TEST(FieldCalibrationTest, Atypical_Bad_Field_Layout) { + auto model = wpi::util::json::parse( + std::ifstream(calSavePath + "/cameracalibration.json")) + .get(); + auto ret = + wpical::calibrate(LookupPath(projectRootPath + videoLocation), model, + wpi::apriltag::AprilTagFieldLayout{}, 3, false); + EXPECT_EQ(ret, std::nullopt); } -TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) { - int ret = fieldcalibration::calibrate( - LookupPath(projectRootPath + videoLocation), output_json, - LookupPath(projectRootPath + videoLocation + "/long" + fileSuffix), - LookupPath(projectRootPath + "/2024-crescendo.json"), 3, false); - EXPECT_EQ(ret, 1); +TEST(FieldCalibrationTest, Atypical_Bad_Input_Directory) { + auto model = wpi::util::json::parse( + std::ifstream(calSavePath + "/cameracalibration.json")) + .get(); + auto ret = + wpical::calibrate(LookupPath(projectRootPath), model, + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::k2024Crescendo), + 3, false); + EXPECT_EQ(ret, std::nullopt); } -TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) { - int ret = fieldcalibration::calibrate( - LookupPath(projectRootPath + videoLocation), output_json, - calSavePath + "/cameracalibration.json", - calSavePath + "/cameracalibration.json", 3, false); - EXPECT_EQ(ret, 1); +TEST(FieldCalibrationTest, Atypical_Bad_Pinned_Tag) { + auto model = wpi::util::json::parse( + std::ifstream(calSavePath + "/cameracalibration.json")) + .get(); + auto ret = + wpical::calibrate(LookupPath(projectRootPath + videoLocation), model, + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::k2024Crescendo), + 42, false); + EXPECT_EQ(ret, std::nullopt); } -TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) { - int ret = fieldcalibration::calibrate( - LookupPath(projectRootPath + ""), output_json, - calSavePath + "/cameracalibration.json", - LookupPath(projectRootPath + "/2024-crescendo.json"), 3, false); - EXPECT_EQ(ret, 1); -} - -TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) { - int ret = fieldcalibration::calibrate( - LookupPath(projectRootPath + videoLocation), output_json, - calSavePath + "/cameracalibration.json", - LookupPath(projectRootPath + "/2024-crescendo.json"), 42, false); - EXPECT_EQ(ret, 1); -} - -TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag_Negative) { - int ret = fieldcalibration::calibrate( - LookupPath(projectRootPath + videoLocation), output_json, - calSavePath + "/cameracalibration.json", - LookupPath(projectRootPath + "/2024-crescendo.json"), -1, false); - EXPECT_EQ(ret, 1); +TEST(FieldCalibrationTest, Atypical_Bad_Pinned_Tag_Negative) { + auto model = wpi::util::json::parse( + std::ifstream(calSavePath + "/cameracalibration.json")) + .get(); + auto ret = + wpical::calibrate(LookupPath(projectRootPath + videoLocation), model, + wpi::apriltag::AprilTagFieldLayout::LoadField( + wpi::apriltag::AprilTagField::k2024Crescendo), + -1, false); + EXPECT_EQ(ret, std::nullopt); }