Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-21 01:01:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpical] Add JSON Combiner (#7640)

Browse Source 
This new feature allows users to combine multiple Apriltag layouts. This can be useful for fields where the apriltags are split into two or more sections: (red/blue side, grouped together by task, etc.)
This commit is contained in:
Elliot Scher
2025-01-10 02:30:17 -05:00
committed by GitHub
parent e37c35746a
commit 6125227836
7 changed files with 373 additions and 186 deletions
Download Patch File Download Diff File Expand all files Collapse all files

471
wpical/src/main/native/cpp/WPIcal.cpp
View File

@@ -9,12 +9,14 @@
#include <fstream>
#include <iostream>
#include <map>
#include <memory>
#include <numbers>
#include <string>
#include <vector>
#include <GLFW/glfw3.h>
#include <fmt/format.h>
#include <imgui.h>
#include <portable-file-dialogs.h>
#include <tagpose.h>
@@ -58,6 +60,112 @@ void drawCheck() {
ImGui::NewLine();
}
void processFileSelector(std::unique_ptr<pfd::open_file>& selector,
std::string& selected_file) {
if (selector && selector->ready(0)) {
auto selectedFiles = selector->result();
if (!selectedFiles.empty()) {
selected_file = selectedFiles[0];
}
selector.reset();
}
}
void processFilesSelector(std::unique_ptr<pfd::open_file>& selector,
std::vector<std::string>& selected_files) {
if (selector && selector->ready(0)) {
auto selectedFiles = selector->result();
if (!selectedFiles.empty()) {
selected_files = selectedFiles;
}
selector.reset();
}
}
void processDirectorySelector(std::unique_ptr<pfd::select_folder>& selector,
std::string& selected_directory) {
if (selector && selector->ready(0)) {
auto selectedFiles = selector->result();
if (!selectedFiles.empty()) {
selected_directory = selectedFiles;
}
selector.reset();
}
}
void openFileButton(const char* text, std::string& selected_file,
std::unique_ptr<pfd::open_file>& selector,
const std::string& file_type,
const std::string& file_extensions) {
if (ImGui::Button(text)) {
selector = std::make_unique<pfd::open_file>(
"Select File", "", std::vector<std::string>{file_type, file_extensions},
pfd::opt::none);
}
}
void openFilesButton(const char* text, std::vector<std::string>& selected_files,
std::unique_ptr<pfd::open_file>& selector,
const std::string& file_type,
const std::string& file_extensions) {
if (ImGui::Button(text)) {
selector = std::make_unique<pfd::open_file>(
"Select File", "", std::vector<std::string>{file_type, file_extensions},
pfd::opt::multiselect);
}
}
void openDirectoryButton(const char* text,
std::unique_ptr<pfd::select_folder>& selector,
std::string& selected_directory) {
if (ImGui::Button(text)) {
selector = std::make_unique<pfd::select_folder>("Select Directory", "");
}
}
std::string getFileName(std::string path) {
size_t lastSlash = path.find_last_of("/\\");
size_t lastDot = path.find_last_of(".");
return path.substr(lastSlash + 1, lastDot - lastSlash - 1);
}
static bool EmitEntryTarget(int tag_id, std::string& file) {
if (!file.empty()) {
auto text = fmt::format("{}: {}", tag_id, file);
ImGui::TextUnformatted(text.c_str());
} else {
ImGui::Text("Tag ID %i: <none (DROP HERE)>", tag_id);
}
bool rv = false;
if (ImGui::BeginDragDropTarget()) {
if (const ImGuiPayload* payload =
ImGui::AcceptDragDropPayload("FieldCalibration")) {
file = *(std::string*)payload->Data;
rv = true;
}
ImGui::EndDragDropTarget();
}
return rv;
}
void saveCalibration(wpi::json& field, std::string& output_directory,
std::string output_name, bool& isCalibrating) {
if (!field.empty() && !output_directory.empty()) {
std::cout << "Saving calibration to " << output_directory << std::endl;
std::ofstream out(output_directory + "/" + output_name + ".json");
out << field.dump(4);
out.close();
std::ofstream fmap(output_directory + "/" + output_name + ".fmap");
fmap << fmap::convertfmap(field).dump(4);
fmap.close();
field.clear();
output_directory.clear();
isCalibrating = false;
}
}
static void DisplayGui() {
ImGui::GetStyle().WindowRounding = 0;
@@ -82,19 +190,28 @@ static void DisplayGui() {
ImGui::EndMenuBar();
static std::unique_ptr<pfd::open_file> camera_intrinsics_selector;
static std::string selected_camera_intrinsics;
static std::unique_ptr<pfd::open_file> field_map_selector;
static std::string selected_field_map;
static std::unique_ptr<pfd::open_file> output_calibration_json_selector;
static std::unique_ptr<pfd::open_file> combination_calibrations_selector;
static std::unique_ptr<pfd::select_folder>
field_calibration_directory_selector;
static std::string selected_field_calibration_directory;
static std::unique_ptr<pfd::select_folder> download_directory_selector;
static std::string selected_download_directory;
static std::string calibration_json_path;
static wpi::json field_calibration_json;
static wpi::json field_combination_json;
static std::string selected_camera_intrinsics;
static std::string selected_field_map;
static std::string selected_field_calibration_directory;
static std::string selected_download_directory;
static std::string output_calibration_json_path;
static std::vector<std::string> selected_combination_calibrations;
static std::map<int, std::string> combiner_map;
static int current_combiner_tag_id = 0;
static bool isCalibrating = false;
cameracalibration::CameraModel cameraModel = {
.intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
@@ -118,13 +235,12 @@ static void DisplayGui() {
static Fieldmap currentCalibrationMap;
static Fieldmap currentReferenceMap;
static Fieldmap currentCombinerMap;
// camera matrix selector button
if (ImGui::Button("Upload Camera Intrinsics")) {
camera_intrinsics_selector = std::make_unique<pfd::open_file>(
"Select Camera Intrinsics JSON", "",
std::vector<std::string>{"JSON", "*.json"}, pfd::opt::none);
}
openFileButton("Select Camera Intrinsics JSON", selected_camera_intrinsics,
camera_intrinsics_selector, "JSON Files", "*.json");
processFileSelector(camera_intrinsics_selector, selected_camera_intrinsics);
ImGui::SameLine();
ImGui::Text("Or");
@@ -136,50 +252,25 @@ static void DisplayGui() {
ImGui::OpenPopup("Camera Calibration");
}
if (camera_intrinsics_selector) {
auto selectedFiles = camera_intrinsics_selector->result();
if (!selectedFiles.empty()) {
selected_camera_intrinsics = selectedFiles[0];
}
camera_intrinsics_selector.reset();
}
if (!selected_camera_intrinsics.empty()) {
drawCheck();
}
// field json selector button
if (ImGui::Button("Select Field Map JSON")) {
field_map_selector = std::make_unique<pfd::open_file>(
"Select Json File", "",
std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none);
}
if (field_map_selector) {
auto selectedFiles = field_map_selector->result();
if (!selectedFiles.empty()) {
selected_field_map = selectedFiles[0];
}
field_map_selector.reset();
}
openFileButton("Select Field Map JSON", selected_field_map,
field_map_selector, "JSON Files", "*.json");
processFileSelector(field_map_selector, selected_field_map);
if (!selected_field_map.empty()) {
drawCheck();
}
// field calibration directory selector button
if (ImGui::Button("Select Field Calibration Folder")) {
field_calibration_directory_selector = std::make_unique<pfd::select_folder>(
"Select Field Calibration Folder", "");
}
if (field_calibration_directory_selector) {
auto selectedFiles = field_calibration_directory_selector->result();
if (!selectedFiles.empty()) {
selected_field_calibration_directory = selectedFiles;
}
field_calibration_directory_selector.reset();
}
openDirectoryButton("Select Field Calibration Directory",
field_calibration_directory_selector,
selected_field_calibration_directory);
processDirectorySelector(field_calibration_directory_selector,
selected_field_calibration_directory);
if (!selected_field_calibration_directory.empty()) {
drawCheck();
@@ -191,46 +282,35 @@ static void DisplayGui() {
// calibrate button
if (ImGui::Button("Calibrate!!!")) {
if (!selected_field_calibration_directory.empty() &&
!selected_camera_intrinsics.empty() && !selected_field_map.empty()) {
int calibrationOutput = fieldcalibration::calibrate(
selected_field_calibration_directory.c_str(), field_calibration_json,
selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag,
showDebug);
if (calibrationOutput == 1) {
ImGui::OpenPopup("Field Calibration Error");
}
if (selected_download_directory.empty() &&
!field_calibration_json.empty() && !download_directory_selector) {
download_directory_selector =
std::make_unique<pfd::select_folder>("Select Download Folder", "");
if (download_directory_selector) {
auto selectedFiles = download_directory_selector->result();
if (!selectedFiles.empty()) {
selected_download_directory = selectedFiles;
}
download_directory_selector.reset();
}
calibration_json_path = selected_download_directory + "/output.json";
int calibrationOutput = fieldcalibration::calibrate(
selected_field_calibration_directory.c_str(), calibration_json_path,
selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag,
showDebug);
if (calibrationOutput == 1) {
ImGui::OpenPopup("Field Calibration Error");
} else if (calibrationOutput == 0) {
std::ifstream caljsonpath(calibration_json_path);
try {
wpi::json fmap = fmap::convertfmap(wpi::json::parse(caljsonpath));
std::ofstream out(selected_download_directory + "/output.fmap");
out << fmap.dump(4);
out.close();
ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
ImGui::OpenPopup("Visualize Calibration");
} catch (...) {
ImGui::OpenPopup("Fmap Conversion Error");
}
}
}
}
processDirectorySelector(download_directory_selector,
selected_download_directory);
saveCalibration(field_calibration_json, selected_download_directory,
"field_calibration", isCalibrating);
if (ImGui::Button("Visualize")) {
ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
ImGui::OpenPopup("Visualize Calibration");
}
if (ImGui::Button("Combine Calibrations")) {
ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
ImGui::OpenPopup("Combine Calibrations");
}
if (selected_field_calibration_directory.empty() ||
selected_camera_intrinsics.empty() || selected_field_map.empty()) {
ImGui::TextWrapped(
@@ -320,21 +400,11 @@ static void DisplayGui() {
}
if (mrcal) {
if (ImGui::Button("Select Camera Calibration Video")) {
camera_intrinsics_selector = std::make_unique<pfd::open_file>(
"Select Camera Calibration Video", "",
std::vector<std::string>{"Video Files",
"*.mp4 *.mov *.m4v *.mkv *.avi"},
pfd::opt::none);
}
if (camera_intrinsics_selector) {
auto selectedFiles = camera_intrinsics_selector->result();
if (!selectedFiles.empty()) {
selected_camera_intrinsics = selectedFiles[0];
}
camera_intrinsics_selector.reset();
}
openFileButton("Select Camera Calibration Video",
selected_camera_intrinsics, camera_intrinsics_selector,
"Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi");
processFileSelector(camera_intrinsics_selector,
selected_camera_intrinsics);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputDouble("Square Width (in)", &squareWidth);
@@ -379,21 +449,11 @@ static void DisplayGui() {
}
}
} else {
if (ImGui::Button("Select Camera Calibration Video")) {
camera_intrinsics_selector = std::make_unique<pfd::open_file>(
"Select Camera Calibration Video", "",
std::vector<std::string>{"Video Files",
"*.mp4 *.mov *.m4v *.mkv *.avi"},
pfd::opt::none);
}
if (camera_intrinsics_selector) {
auto selectedFiles = camera_intrinsics_selector->result();
if (!selectedFiles.empty()) {
selected_camera_intrinsics = selectedFiles[0];
}
camera_intrinsics_selector.reset();
}
openFileButton("Select Camera Calibration Video",
selected_camera_intrinsics, camera_intrinsics_selector,
"Video Files", "*.mp4 *.mov *.m4v *.mkv *.avi");
processFileSelector(camera_intrinsics_selector,
selected_camera_intrinsics);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputDouble("Square Width (in)", &squareWidth);
@@ -446,26 +506,19 @@ static void DisplayGui() {
// visualize calibration popup
if (ImGui::BeginPopupModal("Visualize Calibration", NULL,
ImGuiWindowFlags_AlwaysAutoResize)) {
if (ImGui::Button("Load Calibrated Field")) {
calibration_json_path =
std::make_unique<pfd::open_file>(
"Select Json File", "",
std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
->result()[0];
}
openFileButton("Select Calibration JSON", output_calibration_json_path,
output_calibration_json_selector, "JSON", "*.json");
processFileSelector(output_calibration_json_selector,
output_calibration_json_path);
if (!calibration_json_path.empty()) {
if (!output_calibration_json_path.empty()) {
ImGui::SameLine();
drawCheck();
}
if (ImGui::Button("Load Reference Field")) {
selected_field_map =
std::make_unique<pfd::open_file>(
"Select Json File", "",
std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
->result()[0];
}
openFileButton("Select Ideal Field Map", selected_field_map,
field_map_selector, "JSON", "*.json");
processFileSelector(field_map_selector, selected_field_map);
if (!selected_field_map.empty()) {
ImGui::SameLine();
@@ -477,58 +530,76 @@ static void DisplayGui() {
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputInt("Reference Tag", &referenceTag);
if (!calibration_json_path.empty() && !selected_field_map.empty()) {
std::ifstream calJson(calibration_json_path);
if (!output_calibration_json_path.empty() && !selected_field_map.empty()) {
std::ifstream calJson(output_calibration_json_path);
std::ifstream refJson(selected_field_map);
currentCalibrationMap = Fieldmap(wpi::json::parse(calJson));
currentReferenceMap = Fieldmap(wpi::json::parse(refJson));
double xDiff = currentReferenceMap.getTag(focusedTag).xPos -
currentCalibrationMap.getTag(focusedTag).xPos;
double yDiff = currentReferenceMap.getTag(focusedTag).yPos -
currentCalibrationMap.getTag(focusedTag).yPos;
double zDiff = currentReferenceMap.getTag(focusedTag).zPos -
currentCalibrationMap.getTag(focusedTag).zPos;
double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot -
currentCalibrationMap.getTag(focusedTag).yawRot;
double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot -
currentCalibrationMap.getTag(focusedTag).pitchRot;
double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot -
currentCalibrationMap.getTag(focusedTag).rollRot;
if (currentCalibrationMap.getNumTags() !=
currentReferenceMap.getNumTags()) {
ImGui::TextWrapped(
"The number of tags in the calibration output and the ideal field "
"map "
"do not match. Please ensure that the calibration output and ideal "
"field "
"map have the same number of tags.");
} else if (currentReferenceMap.hasTag(focusedTag) &&
currentReferenceMap.hasTag(referenceTag)) {
double xDiff = currentReferenceMap.getTag(focusedTag).xPos -
currentCalibrationMap.getTag(focusedTag).xPos;
double yDiff = currentReferenceMap.getTag(focusedTag).yPos -
currentCalibrationMap.getTag(focusedTag).yPos;
double zDiff = currentReferenceMap.getTag(focusedTag).zPos -
currentCalibrationMap.getTag(focusedTag).zPos;
double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot -
currentCalibrationMap.getTag(focusedTag).yawRot;
double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot -
currentCalibrationMap.getTag(focusedTag).pitchRot;
double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot -
currentCalibrationMap.getTag(focusedTag).rollRot;
double xRef = currentCalibrationMap.getTag(referenceTag).xPos -
currentCalibrationMap.getTag(focusedTag).xPos;
double yRef = currentCalibrationMap.getTag(referenceTag).yPos -
currentCalibrationMap.getTag(focusedTag).yPos;
double zRef = currentCalibrationMap.getTag(referenceTag).zPos -
currentCalibrationMap.getTag(focusedTag).zPos;
double xRef = currentCalibrationMap.getTag(referenceTag).xPos -
currentCalibrationMap.getTag(focusedTag).xPos;
double yRef = currentCalibrationMap.getTag(referenceTag).yPos -
currentCalibrationMap.getTag(focusedTag).yPos;
double zRef = currentCalibrationMap.getTag(referenceTag).zPos -
currentCalibrationMap.getTag(focusedTag).zPos;
ImGui::TextWrapped("X Difference: %s (m)", std::to_string(xDiff).c_str());
ImGui::TextWrapped("Y Difference: %s (m)", std::to_string(yDiff).c_str());
ImGui::TextWrapped("Z Difference: %s (m)", std::to_string(zDiff).c_str());
ImGui::TextWrapped("X Difference: %s (m)",
std::to_string(xDiff).c_str());
ImGui::TextWrapped("Y Difference: %s (m)",
std::to_string(yDiff).c_str());
ImGui::TextWrapped("Z Difference: %s (m)",
std::to_string(zDiff).c_str());
ImGui::TextWrapped(
"Yaw Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::TextWrapped(
"Pitch Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::TextWrapped(
"Roll Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::TextWrapped(
"Yaw Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::TextWrapped(
"Pitch Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::TextWrapped(
"Roll Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::NewLine();
ImGui::NewLine();
ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str());
ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str());
ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str());
ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str());
ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str());
ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str());
} else {
ImGui::TextWrapped(
"Please select tags that are in the ideal field map and "
"calibration map");
}
}
if (ImGui::Button("Close")) {
@@ -537,6 +608,78 @@ static void DisplayGui() {
ImGui::EndPopup();
}
if (ImGui::BeginPopupModal("Combine Calibrations", NULL,
ImGuiWindowFlags_AlwaysAutoResize)) {
openFileButton("Select Ideal Map", selected_field_map, field_map_selector,
"JSON", "*.json");
processFileSelector(field_map_selector, selected_field_map);
if (!selected_field_map.empty()) {
drawCheck();
std::ifstream json(selected_field_map);
currentReferenceMap = Fieldmap(wpi::json::parse(json));
currentCombinerMap = currentReferenceMap;
}
openFilesButton("Select Field Calibrations",
selected_combination_calibrations,
combination_calibrations_selector, "JSON", "*.json");
processFilesSelector(combination_calibrations_selector,
selected_combination_calibrations);
if (!selected_field_map.empty() &&
!selected_combination_calibrations.empty()) {
for (std::string& file : selected_combination_calibrations) {
ImGui::Selectable(getFileName(file).c_str(), false,
ImGuiSelectableFlags_DontClosePopups);
if (ImGui::BeginDragDropSource()) {
ImGui::SetDragDropPayload("FieldCalibration", &file, sizeof(file));
ImGui::TextUnformatted(file.c_str());
ImGui::EndDragDropSource();
}
}
for (auto& [key, val] : combiner_map) {
EmitEntryTarget(key, val);
}
ImGui::InputInt("Tag ID", &current_combiner_tag_id);
ImGui::SameLine();
if (ImGui::Button("Add", ImVec2(0, 0)) &&
currentCombinerMap.hasTag(current_combiner_tag_id)) {
combiner_map.emplace(current_combiner_tag_id, "");
}
ImGui::SameLine();
if (ImGui::Button("Remove", ImVec2(0, 0))) {
combiner_map.erase(current_combiner_tag_id);
}
}
ImGui::Separator();
if (ImGui::Button("Close", ImVec2(0, 0))) {
ImGui::CloseCurrentPopup();
}
ImGui::SameLine();
if (ImGui::Button("Download", ImVec2(0, 0))) {
for (auto& [key, val] : combiner_map) {
std::ifstream json(val);
Fieldmap map(wpi::json::parse(json));
currentCombinerMap.replaceTag(key, map.getTag(key));
}
field_combination_json = currentCombinerMap.toJson();
}
if (selected_download_directory.empty() &&
!field_combination_json.empty() && !download_directory_selector) {
download_directory_selector =
std::make_unique<pfd::select_folder>("Select Download Folder", "");
}
processDirectorySelector(download_directory_selector,
selected_download_directory);
saveCalibration(field_combination_json, selected_download_directory,
"combined_calibration", isCalibrating);
ImGui::EndPopup();
}
ImGui::End();
}