diff --git a/wpical/src/main/native/cpp/WPIcal.cpp b/wpical/src/main/native/cpp/WPIcal.cpp index 6e03027889a..f73105a50da 100644 --- a/wpical/src/main/native/cpp/WPIcal.cpp +++ b/wpical/src/main/native/cpp/WPIcal.cpp @@ -9,12 +9,14 @@ #include #include +#include #include #include #include #include #include +#include #include #include #include @@ -58,6 +60,112 @@ void drawCheck() { ImGui::NewLine(); } +void processFileSelector(std::unique_ptr& selector, + std::string& selected_file) { + if (selector && selector->ready(0)) { + auto selectedFiles = selector->result(); + if (!selectedFiles.empty()) { + selected_file = selectedFiles[0]; + } + selector.reset(); + } +} + +void processFilesSelector(std::unique_ptr& 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) { + if (ImGui::Button(text)) { + selector = std::make_unique( + "Select File", "", std::vector{file_type, file_extensions}, + 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); + } +} + +void openDirectoryButton(const char* text, + std::unique_ptr& selector, + std::string& selected_directory) { + if (ImGui::Button(text)) { + selector = std::make_unique("Select Directory", ""); + } +} + +std::string getFileName(std::string path) { + size_t lastSlash = path.find_last_of("/\\"); + size_t lastDot = path.find_last_of("."); + return path.substr(lastSlash + 1, lastDot - lastSlash - 1); +} + +static bool EmitEntryTarget(int tag_id, std::string& file) { + if (!file.empty()) { + auto text = fmt::format("{}: {}", tag_id, file); + ImGui::TextUnformatted(text.c_str()); + } else { + ImGui::Text("Tag ID %i: ", tag_id); + } + bool rv = false; + if (ImGui::BeginDragDropTarget()) { + if (const ImGuiPayload* payload = + ImGui::AcceptDragDropPayload("FieldCalibration")) { + file = *(std::string*)payload->Data; + rv = true; + } + ImGui::EndDragDropTarget(); + } + return rv; +} + +void saveCalibration(wpi::json& field, std::string& output_directory, + std::string output_name, bool& isCalibrating) { + if (!field.empty() && !output_directory.empty()) { + std::cout << "Saving calibration to " << output_directory << std::endl; + std::ofstream out(output_directory + "/" + output_name + ".json"); + out << field.dump(4); + out.close(); + + std::ofstream fmap(output_directory + "/" + output_name + ".fmap"); + fmap << fmap::convertfmap(field).dump(4); + fmap.close(); + + field.clear(); + output_directory.clear(); + isCalibrating = false; + } +} + static void DisplayGui() { ImGui::GetStyle().WindowRounding = 0; @@ -82,19 +190,28 @@ static void DisplayGui() { ImGui::EndMenuBar(); static std::unique_ptr camera_intrinsics_selector; - static std::string selected_camera_intrinsics; - static std::unique_ptr field_map_selector; - static std::string selected_field_map; + 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::string selected_field_calibration_directory; - static std::unique_ptr download_directory_selector; + + static wpi::json field_calibration_json; + static wpi::json field_combination_json; + + static std::string selected_camera_intrinsics; + static std::string selected_field_map; + static std::string selected_field_calibration_directory; static std::string selected_download_directory; + static std::string output_calibration_json_path; + static std::vector selected_combination_calibrations; - static std::string calibration_json_path; + static std::map combiner_map; + static int current_combiner_tag_id = 0; + + static bool isCalibrating = false; cameracalibration::CameraModel cameraModel = { .intrinsic_matrix = Eigen::Matrix::Identity(), @@ -118,13 +235,12 @@ static void DisplayGui() { static Fieldmap currentCalibrationMap; static Fieldmap currentReferenceMap; + static Fieldmap currentCombinerMap; // camera matrix selector button - if (ImGui::Button("Upload Camera Intrinsics")) { - camera_intrinsics_selector = std::make_unique( - "Select Camera Intrinsics JSON", "", - std::vector{"JSON", "*.json"}, pfd::opt::none); - } + openFileButton("Select Camera Intrinsics JSON", selected_camera_intrinsics, + camera_intrinsics_selector, "JSON Files", "*.json"); + processFileSelector(camera_intrinsics_selector, selected_camera_intrinsics); ImGui::SameLine(); ImGui::Text("Or"); @@ -136,50 +252,25 @@ static void DisplayGui() { ImGui::OpenPopup("Camera Calibration"); } - if (camera_intrinsics_selector) { - auto selectedFiles = camera_intrinsics_selector->result(); - if (!selectedFiles.empty()) { - selected_camera_intrinsics = selectedFiles[0]; - } - camera_intrinsics_selector.reset(); - } - if (!selected_camera_intrinsics.empty()) { drawCheck(); } // field json selector button - if (ImGui::Button("Select Field Map JSON")) { - field_map_selector = std::make_unique( - "Select Json File", "", - std::vector{"JSON Files", "*.json"}, pfd::opt::none); - } - - if (field_map_selector) { - auto selectedFiles = field_map_selector->result(); - if (!selectedFiles.empty()) { - selected_field_map = selectedFiles[0]; - } - field_map_selector.reset(); - } + openFileButton("Select Field Map JSON", selected_field_map, + field_map_selector, "JSON Files", "*.json"); + processFileSelector(field_map_selector, selected_field_map); if (!selected_field_map.empty()) { drawCheck(); } // field calibration directory selector button - if (ImGui::Button("Select Field Calibration Folder")) { - field_calibration_directory_selector = std::make_unique( - "Select Field Calibration Folder", ""); - } - - if (field_calibration_directory_selector) { - auto selectedFiles = field_calibration_directory_selector->result(); - if (!selectedFiles.empty()) { - selected_field_calibration_directory = selectedFiles; - } - field_calibration_directory_selector.reset(); - } + openDirectoryButton("Select Field Calibration Directory", + field_calibration_directory_selector, + selected_field_calibration_directory); + processDirectorySelector(field_calibration_directory_selector, + selected_field_calibration_directory); if (!selected_field_calibration_directory.empty()) { drawCheck(); @@ -191,46 +282,35 @@ static void DisplayGui() { // calibrate button if (ImGui::Button("Calibrate!!!")) { - if (!selected_field_calibration_directory.empty() && - !selected_camera_intrinsics.empty() && !selected_field_map.empty()) { + int calibrationOutput = fieldcalibration::calibrate( + selected_field_calibration_directory.c_str(), field_calibration_json, + selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag, + showDebug); + + if (calibrationOutput == 1) { + ImGui::OpenPopup("Field Calibration Error"); + } + + if (selected_download_directory.empty() && + !field_calibration_json.empty() && !download_directory_selector) { download_directory_selector = std::make_unique("Select Download Folder", ""); - if (download_directory_selector) { - auto selectedFiles = download_directory_selector->result(); - if (!selectedFiles.empty()) { - selected_download_directory = selectedFiles; - } - download_directory_selector.reset(); - } - - calibration_json_path = selected_download_directory + "/output.json"; - - int calibrationOutput = fieldcalibration::calibrate( - selected_field_calibration_directory.c_str(), calibration_json_path, - selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag, - showDebug); - - if (calibrationOutput == 1) { - ImGui::OpenPopup("Field Calibration Error"); - } else if (calibrationOutput == 0) { - std::ifstream caljsonpath(calibration_json_path); - try { - wpi::json fmap = fmap::convertfmap(wpi::json::parse(caljsonpath)); - std::ofstream out(selected_download_directory + "/output.fmap"); - out << fmap.dump(4); - out.close(); - ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); - ImGui::OpenPopup("Visualize Calibration"); - } catch (...) { - ImGui::OpenPopup("Fmap Conversion Error"); - } - } } } + + processDirectorySelector(download_directory_selector, + selected_download_directory); + saveCalibration(field_calibration_json, selected_download_directory, + "field_calibration", isCalibrating); + if (ImGui::Button("Visualize")) { ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); ImGui::OpenPopup("Visualize Calibration"); } + if (ImGui::Button("Combine Calibrations")) { + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("Combine Calibrations"); + } if (selected_field_calibration_directory.empty() || selected_camera_intrinsics.empty() || selected_field_map.empty()) { ImGui::TextWrapped( @@ -320,21 +400,11 @@ static void DisplayGui() { } if (mrcal) { - if (ImGui::Button("Select Camera Calibration Video")) { - camera_intrinsics_selector = std::make_unique( - "Select Camera Calibration Video", "", - std::vector{"Video Files", - "*.mp4 *.mov *.m4v *.mkv *.avi"}, - pfd::opt::none); - } - - if (camera_intrinsics_selector) { - auto selectedFiles = camera_intrinsics_selector->result(); - if (!selectedFiles.empty()) { - selected_camera_intrinsics = selectedFiles[0]; - } - camera_intrinsics_selector.reset(); - } + openFileButton("Select Camera Calibration Video", + selected_camera_intrinsics, camera_intrinsics_selector, + "Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi"); + processFileSelector(camera_intrinsics_selector, + selected_camera_intrinsics); ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); ImGui::InputDouble("Square Width (in)", &squareWidth); @@ -379,21 +449,11 @@ static void DisplayGui() { } } } else { - if (ImGui::Button("Select Camera Calibration Video")) { - camera_intrinsics_selector = std::make_unique( - "Select Camera Calibration Video", "", - std::vector{"Video Files", - "*.mp4 *.mov *.m4v *.mkv *.avi"}, - pfd::opt::none); - } - - if (camera_intrinsics_selector) { - auto selectedFiles = camera_intrinsics_selector->result(); - if (!selectedFiles.empty()) { - selected_camera_intrinsics = selectedFiles[0]; - } - camera_intrinsics_selector.reset(); - } + openFileButton("Select Camera Calibration Video", + selected_camera_intrinsics, camera_intrinsics_selector, + "Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi"); + processFileSelector(camera_intrinsics_selector, + selected_camera_intrinsics); ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); ImGui::InputDouble("Square Width (in)", &squareWidth); @@ -446,26 +506,19 @@ static void DisplayGui() { // visualize calibration popup if (ImGui::BeginPopupModal("Visualize Calibration", NULL, ImGuiWindowFlags_AlwaysAutoResize)) { - if (ImGui::Button("Load Calibrated Field")) { - calibration_json_path = - std::make_unique( - "Select Json File", "", - std::vector{"JSON Files", "*.json"}, pfd::opt::none) - ->result()[0]; - } + openFileButton("Select Calibration JSON", output_calibration_json_path, + output_calibration_json_selector, "JSON", "*.json"); + processFileSelector(output_calibration_json_selector, + output_calibration_json_path); - if (!calibration_json_path.empty()) { + if (!output_calibration_json_path.empty()) { ImGui::SameLine(); drawCheck(); } - if (ImGui::Button("Load Reference Field")) { - selected_field_map = - std::make_unique( - "Select Json File", "", - std::vector{"JSON Files", "*.json"}, pfd::opt::none) - ->result()[0]; - } + openFileButton("Select Ideal Field Map", selected_field_map, + field_map_selector, "JSON", "*.json"); + processFileSelector(field_map_selector, selected_field_map); if (!selected_field_map.empty()) { ImGui::SameLine(); @@ -477,63 +530,153 @@ static void DisplayGui() { ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); ImGui::InputInt("Reference Tag", &referenceTag); - if (!calibration_json_path.empty() && !selected_field_map.empty()) { - std::ifstream calJson(calibration_json_path); + if (!output_calibration_json_path.empty() && !selected_field_map.empty()) { + std::ifstream calJson(output_calibration_json_path); std::ifstream refJson(selected_field_map); currentCalibrationMap = Fieldmap(wpi::json::parse(calJson)); currentReferenceMap = Fieldmap(wpi::json::parse(refJson)); - double xDiff = currentReferenceMap.getTag(focusedTag).xPos - - currentCalibrationMap.getTag(focusedTag).xPos; - double yDiff = currentReferenceMap.getTag(focusedTag).yPos - - currentCalibrationMap.getTag(focusedTag).yPos; - double zDiff = currentReferenceMap.getTag(focusedTag).zPos - - currentCalibrationMap.getTag(focusedTag).zPos; - double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot - - currentCalibrationMap.getTag(focusedTag).yawRot; - double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot - - currentCalibrationMap.getTag(focusedTag).pitchRot; - double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot - - currentCalibrationMap.getTag(focusedTag).rollRot; - - 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()); + 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"); + } + } - ImGui::NewLine(); + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } - 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()); + if (ImGui::BeginPopupModal("Combine Calibrations", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + openFileButton("Select Ideal Map", selected_field_map, field_map_selector, + "JSON", "*.json"); + processFileSelector(field_map_selector, selected_field_map); + if (!selected_field_map.empty()) { + drawCheck(); + std::ifstream json(selected_field_map); + currentReferenceMap = Fieldmap(wpi::json::parse(json)); + currentCombinerMap = currentReferenceMap; } + openFilesButton("Select Field Calibrations", + selected_combination_calibrations, + combination_calibrations_selector, "JSON", "*.json"); + processFilesSelector(combination_calibrations_selector, + selected_combination_calibrations); + + if (!selected_field_map.empty() && + !selected_combination_calibrations.empty()) { + for (std::string& file : selected_combination_calibrations) { + ImGui::Selectable(getFileName(file).c_str(), false, + ImGuiSelectableFlags_DontClosePopups); + if (ImGui::BeginDragDropSource()) { + ImGui::SetDragDropPayload("FieldCalibration", &file, sizeof(file)); + ImGui::TextUnformatted(file.c_str()); + ImGui::EndDragDropSource(); + } + } - if (ImGui::Button("Close")) { + 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::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(); } diff --git a/wpical/src/main/native/cpp/fieldcalibration.cpp b/wpical/src/main/native/cpp/fieldcalibration.cpp index eaafd3a62e6..dec885ecf32 100644 --- a/wpical/src/main/native/cpp/fieldcalibration.cpp +++ b/wpical/src/main/native/cpp/fieldcalibration.cpp @@ -23,7 +23,6 @@ #include #include #include -#include #include "apriltag.h" #include "tag36h11.h" @@ -433,7 +432,7 @@ inline bool process_video_file( } int fieldcalibration::calibrate(std::string input_dir_path, - std::string output_file_path, + wpi::json& output_json, std::string camera_model_path, std::string ideal_map_path, int pinned_tag_id, bool show_debug_window) { @@ -605,8 +604,7 @@ int fieldcalibration::calibrate(std::string input_dir_path, {"length", static_cast(json.at("field").at("length"))}, {"width", static_cast(json.at("field").at("width"))}}; - std::ofstream output_file(output_file_path); - output_file << observed_map_json.dump(4) << std::endl; + output_json = observed_map_json; return 0; } diff --git a/wpical/src/main/native/cpp/tagpose.cpp b/wpical/src/main/native/cpp/tagpose.cpp index 761bf89573e..38977f09613 100644 --- a/wpical/src/main/native/cpp/tagpose.cpp +++ b/wpical/src/main/native/cpp/tagpose.cpp @@ -5,8 +5,10 @@ #include namespace tag { -Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y, - double z, double field_length_meters, double field_width_meters) { +Pose::Pose(int tag_id, double xpos, double ypos, double zpos, double w, + double x, double y, double z, double field_length_meters, + double field_width_meters) { + tagId = tag_id; xPos = xpos; yPos = ypos; zPos = zpos; @@ -26,4 +28,16 @@ Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y, pitchRot = eulerAngles[1]; yawRot = eulerAngles[2]; } + +wpi::json Pose::toJson() { + return {{"ID", tagId}, + {"pose", + {{"translation", {{"x", xPos}, {"y", yPos}, {"z", zPos}}}, + {"rotation", + {{"quaternion", + {{"W", quaternion.w()}, + {"X", quaternion.x()}, + {"Y", quaternion.y()}, + {"Z", quaternion.z()}}}}}}}}; +} } // namespace tag diff --git a/wpical/src/main/native/include/fieldcalibration.h b/wpical/src/main/native/include/fieldcalibration.h index d9de45990ba..ac8475666dd 100644 --- a/wpical/src/main/native/include/fieldcalibration.h +++ b/wpical/src/main/native/include/fieldcalibration.h @@ -6,10 +6,12 @@ #include +#include + #include "cameracalibration.h" namespace fieldcalibration { -int calibrate(std::string input_dir_path, std::string output_file_path, +int calibrate(std::string input_dir_path, wpi::json& output_json, std::string camera_model_path, std::string ideal_map_path, int pinned_tag_id, bool show_debug_window); } // namespace fieldcalibration diff --git a/wpical/src/main/native/include/fieldmap.h b/wpical/src/main/native/include/fieldmap.h index 22fefc28a44..5e581c73227 100644 --- a/wpical/src/main/native/include/fieldmap.h +++ b/wpical/src/main/native/include/fieldmap.h @@ -5,7 +5,7 @@ #pragma once #include -#include +#include #include #include @@ -19,6 +19,7 @@ class Fieldmap { 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 = @@ -34,15 +35,30 @@ class Fieldmap { double tagZQuat = static_cast( tag.value().at("pose").at("rotation").at("quaternion").at("Z")); - tagVec.emplace_back(tagXPos, tagYPos, tagZPos, tagWQuat, tagXQuat, - tagYQuat, tagZQuat, field_length_meters, - field_width_meters); + tagMap.emplace( + tag_id, tag::Pose(tag_id, tagXPos, tagYPos, tagZPos, tagWQuat, + tagXQuat, tagYQuat, tagZQuat, field_length_meters, + field_width_meters)); } + fieldLength = field_length_meters; + fieldWidth = field_width_meters; } - const tag::Pose& getTag(size_t tag) const { return tagVec[tag - 1]; } + const tag::Pose& getTag(size_t tag) const { return tagMap.at(tag); } - int getNumTags() const { return tagVec.size(); } + int getNumTags() const { return tagMap.size(); } + + bool hasTag(int tag) { return tagMap.find(tag) != tagMap.end(); } + + wpi::json toJson() { + wpi::json json; + for (auto& [key, val] : tagMap) { + json["tags"].push_back(val.toJson()); + } + json["field"]["length"] = fieldLength; + json["field"]["width"] = fieldWidth; + return json; + } static double minimizeAngle(double angle) { angle = std::fmod(angle, 360); @@ -54,6 +70,13 @@ class Fieldmap { return angle; } + void replaceTag(int tag_id, tag::Pose pose) { + tagMap.erase(tag_id); + tagMap.emplace(tag_id, pose); + } + private: - std::vector tagVec; + double fieldLength; + double fieldWidth; + std::map tagMap; }; diff --git a/wpical/src/main/native/include/tagpose.h b/wpical/src/main/native/include/tagpose.h index 422fc76a933..aeb06a426ca 100644 --- a/wpical/src/main/native/include/tagpose.h +++ b/wpical/src/main/native/include/tagpose.h @@ -6,15 +6,19 @@ #include #include +#include namespace tag { class Pose { public: - Pose(double xpos, double ypos, double zpos, double w, double x, double y, - double z, double field_length_meters, double field_width_meters); + Pose(int tag_id, double xpos, double ypos, double zpos, double w, double x, + double y, double z, double field_length_meters, + double field_width_meters); + int tagId; double xPos, yPos, zPos, yawRot, rollRot, pitchRot; Eigen::Quaterniond quaternion; Eigen::Matrix3d rotationMatrix; Eigen::Matrix4d transformMatrixFmap; + wpi::json toJson(); }; } // namespace tag diff --git a/wpical/src/test/native/cpp/test_calibrate.cpp b/wpical/src/test/native/cpp/test_calibrate.cpp index 751e2d23692..0e48bcf018d 100644 --- a/wpical/src/test/native/cpp/test_calibrate.cpp +++ b/wpical/src/test/native/cpp/test_calibrate.cpp @@ -19,6 +19,9 @@ cameracalibration::CameraModel cameraModel = { .intrinsic_matrix = Eigen::Matrix::Identity(), .distortion_coefficients = Eigen::Matrix::Zero(), .avg_reprojection_error = 0.0}; + +wpi::json output_json; + #ifdef __linux__ const std::string fileSuffix = ".avi"; const std::string videoLocation = "/altfieldvideo"; @@ -58,7 +61,7 @@ TEST(Camera_CalibrationTest, MRcal_Atypical) { TEST(Field_CalibrationTest, Typical) { int ret = fieldcalibration::calibrate( - projectRootPath + videoLocation, calSavePath, + projectRootPath + videoLocation, output_json, calSavePath + "/cameracalibration.json", projectRootPath + "/2024-crescendo.json", 3, false); EXPECT_EQ(ret, 0); @@ -66,7 +69,7 @@ TEST(Field_CalibrationTest, Typical) { TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) { int ret = fieldcalibration::calibrate( - projectRootPath + videoLocation, calSavePath, + projectRootPath + videoLocation, output_json, projectRootPath + videoLocation + "/long" + fileSuffix, projectRootPath + "/2024-crescendo.json", 3, false); EXPECT_EQ(ret, 1); @@ -74,7 +77,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) { TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) { int ret = fieldcalibration::calibrate( - projectRootPath + videoLocation, calSavePath, + projectRootPath + videoLocation, output_json, calSavePath + "/cameracalibration.json", calSavePath + "/cameracalibration.json", 3, false); EXPECT_EQ(ret, 1); @@ -82,7 +85,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) { TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) { int ret = fieldcalibration::calibrate( - projectRootPath + "", calSavePath, + projectRootPath + "", output_json, calSavePath + "/cameracalibration.json", projectRootPath + "/2024-crescendo.json", 3, false); EXPECT_EQ(ret, 1); @@ -90,7 +93,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) { TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) { int ret = fieldcalibration::calibrate( - projectRootPath + videoLocation, calSavePath, + projectRootPath + videoLocation, output_json, calSavePath + "/cameracalibration.json", projectRootPath + "/2024-crescendo.json", 42, false); EXPECT_EQ(ret, 1); @@ -98,7 +101,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) { TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag_Negative) { int ret = fieldcalibration::calibrate( - projectRootPath + videoLocation, calSavePath, + projectRootPath + videoLocation, output_json, calSavePath + "/cameracalibration.json", projectRootPath + "/2024-crescendo.json", -1, false); EXPECT_EQ(ret, 1);