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

Merge branch 'main' into 2027

Browse Source
This commit is contained in:
Peter Johnson
2025-01-16 23:17:59 -08:00
parent 1cad4f64a4 a9f3fc6b2c
commit d9f8fded09
92 changed files with 2748 additions and 534 deletions
Download Patch File Download Diff File Expand all files Collapse all files

500
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(),
@@ -114,15 +231,16 @@ static void DisplayGui() {
static int focusedTag = 1;
static int referenceTag = 1;
static int maxFRCTag = 22;
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");
@@ -134,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();
@@ -187,62 +280,46 @@ static void DisplayGui() {
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputInt("Pinned Tag", &pinnedTag);
if (pinnedTag < 1) {
pinnedTag = 1;
} else if (pinnedTag > 16) {
pinnedTag = 16;
}
// calibrate button
if (ImGui::Button("Calibrate!!!")) {
if (!selected_field_calibration_directory.empty() &&
!selected_camera_intrinsics.empty() && !selected_field_map.empty() &&
pinnedTag > 0 && pinnedTag <= 16) {
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("Fmap Conversion failed");
} 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("Field Calibration 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(
"Some inputs are empty! please enter your camera calibration video, "
"field map, and field calibration directory");
} else if (!(pinnedTag > 0 && pinnedTag <= 16)) {
ImGui::TextWrapped("Make sure the pinned tag is a valid april tag (1-16)");
} else if (!(pinnedTag > 0 && pinnedTag <= maxFRCTag)) {
ImGui::TextWrapped(
"The pinned tag is not within the normal range for FRC fields (1-22), "
"If you proceed, You may experience a bad calibration.");
} else {
ImGui::TextWrapped("Calibration Ready");
}
@@ -269,7 +346,7 @@ static void DisplayGui() {
ImGui::EndPopup();
}
if (ImGui::BeginPopupModal("Fmap Conversion failed", NULL,
if (ImGui::BeginPopupModal("Fmap Conversion Error", NULL,
ImGuiWindowFlags_AlwaysAutoResize)) {
ImGui::TextWrapped(
"Fmap conversion failed - you can still use the calibration output on "
@@ -323,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);
@@ -382,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);
@@ -449,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();
@@ -480,70 +530,76 @@ static void DisplayGui() {
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputInt("Reference Tag", &referenceTag);
if (focusedTag < 1) {
focusedTag = 1;
} else if (focusedTag > 16) {
focusedTag = 16;
}
if (referenceTag < 1) {
referenceTag = 1;
} else if (referenceTag > 16) {
referenceTag = 16;
}
if (!calibration_json_path.empty() && !selected_field_map.empty()) {
std::ifstream calJson(calibration_json_path);
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")) {
@@ -552,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();
}

19
wpical/src/main/native/cpp/fieldcalibration.cpp
View File

@@ -23,7 +23,6 @@
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include <wpi/json.h>
#include "apriltag.h"
#include "tag36h11.h"
@@ -433,7 +432,7 @@ inline bool process_video_file(
}
int fieldcalibration::calibrate(std::string input_dir_path,
std::string output_file_path,
wpi::json& output_json,
std::string camera_model_path,
std::string ideal_map_path, int pinned_tag_id,
bool show_debug_window) {
@@ -466,6 +465,19 @@ int fieldcalibration::calibrate(std::string input_dir_path,
return 1;
}
bool pinned_tag_found = false;
// Check if pinned tag is in ideal map
for (const auto& [tag_id, tag_json] : ideal_map) {
if (tag_id == pinned_tag_id) {
pinned_tag_found = true;
break;
}
}
if (!pinned_tag_found) {
return 1;
}
// Apriltag detector
apriltag_detector_t* tag_detector = apriltag_detector_create();
tag_detector->nthreads = 8;
@@ -592,8 +604,7 @@ int fieldcalibration::calibrate(std::string input_dir_path,
{"length", static_cast<double>(json.at("field").at("length"))},
{"width", static_cast<double>(json.at("field").at("width"))}};
std::ofstream output_file(output_file_path);
output_file << observed_map_json.dump(4) << std::endl;
output_json = observed_map_json;
return 0;
}

18
wpical/src/main/native/cpp/tagpose.cpp
View File

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

4
wpical/src/main/native/include/fieldcalibration.h
View File

@@ -6,10 +6,12 @@
#include <string>
#include <wpi/json.h>
#include "cameracalibration.h"
namespace fieldcalibration {
int calibrate(std::string input_dir_path, std::string output_file_path,
int calibrate(std::string input_dir_path, wpi::json& output_json,
std::string camera_model_path, std::string ideal_map_path,
int pinned_tag_id, bool show_debug_window);
} // namespace fieldcalibration

37
wpical/src/main/native/include/fieldmap.h
View File

@@ -5,7 +5,7 @@
#pragma once
#include <cmath>
#include <vector>
#include <map>
#include <tagpose.h>
#include <wpi/json.h>
@@ -19,6 +19,7 @@ class Fieldmap {
double field_width_meters =
static_cast<double>(json.at("field").at("width"));
for (const auto& tag : json.at("tags").items()) {
double tag_id = static_cast<int>(tag.value().at("ID"));
double tagXPos =
static_cast<double>(tag.value().at("pose").at("translation").at("x"));
double tagYPos =
@@ -34,15 +35,30 @@ class Fieldmap {
double tagZQuat = static_cast<double>(
tag.value().at("pose").at("rotation").at("quaternion").at("Z"));
tagVec.emplace_back(tagXPos, tagYPos, tagZPos, tagWQuat, tagXQuat,
tagYQuat, tagZQuat, field_length_meters,
field_width_meters);
tagMap.emplace(
tag_id, tag::Pose(tag_id, tagXPos, tagYPos, tagZPos, tagWQuat,
tagXQuat, tagYQuat, tagZQuat, field_length_meters,
field_width_meters));
}
fieldLength = field_length_meters;
fieldWidth = field_width_meters;
}
const tag::Pose& getTag(size_t tag) const { return tagVec[tag - 1]; }
const tag::Pose& getTag(size_t tag) const { return tagMap.at(tag); }
int getNumTags() const { return tagVec.size(); }
int getNumTags() const { return tagMap.size(); }
bool hasTag(int tag) { return tagMap.find(tag) != tagMap.end(); }
wpi::json toJson() {
wpi::json json;
for (auto& [key, val] : tagMap) {
json["tags"].push_back(val.toJson());
}
json["field"]["length"] = fieldLength;
json["field"]["width"] = fieldWidth;
return json;
}
static double minimizeAngle(double angle) {
angle = std::fmod(angle, 360);
@@ -54,6 +70,13 @@ class Fieldmap {
return angle;
}
void replaceTag(int tag_id, tag::Pose pose) {
tagMap.erase(tag_id);
tagMap.emplace(tag_id, pose);
}
private:
std::vector<tag::Pose> tagVec;
double fieldLength;
double fieldWidth;
std::map<int, tag::Pose> tagMap;
};

8
wpical/src/main/native/include/tagpose.h
View File

@@ -6,15 +6,19 @@
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <wpi/json.h>
namespace tag {
class Pose {
public:
Pose(double xpos, double ypos, double zpos, double w, double x, double y,
double z, double field_length_meters, double field_width_meters);
Pose(int tag_id, double xpos, double ypos, double zpos, double w, double x,
double y, double z, double field_length_meters,
double field_width_meters);
int tagId;
double xPos, yPos, zPos, yawRot, rollRot, pitchRot;
Eigen::Quaterniond quaternion;
Eigen::Matrix3d rotationMatrix;
Eigen::Matrix4d transformMatrixFmap;
wpi::json toJson();
};
} // namespace tag

27
wpical/src/test/native/cpp/test_calibrate.cpp
View File

@@ -19,6 +19,9 @@ cameracalibration::CameraModel cameraModel = {
.intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
.distortion_coefficients = Eigen::Matrix<double, 8, 1>::Zero(),
.avg_reprojection_error = 0.0};
wpi::json output_json;
#ifdef __linux__
const std::string fileSuffix = ".avi";
const std::string videoLocation = "/altfieldvideo";
@@ -58,7 +61,7 @@ TEST(Camera_CalibrationTest, MRcal_Atypical) {
TEST(Field_CalibrationTest, Typical) {
int ret = fieldcalibration::calibrate(
projectRootPath + videoLocation, calSavePath,
projectRootPath + videoLocation, output_json,
calSavePath + "/cameracalibration.json",
projectRootPath + "/2024-crescendo.json", 3, false);
EXPECT_EQ(ret, 0);
@@ -66,7 +69,7 @@ TEST(Field_CalibrationTest, Typical) {
TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) {
int ret = fieldcalibration::calibrate(
projectRootPath + videoLocation, calSavePath,
projectRootPath + videoLocation, output_json,
projectRootPath + videoLocation + "/long" + fileSuffix,
projectRootPath + "/2024-crescendo.json", 3, false);
EXPECT_EQ(ret, 1);
@@ -74,7 +77,7 @@ TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) {
TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) {
int ret = fieldcalibration::calibrate(
projectRootPath + videoLocation, calSavePath,
projectRootPath + videoLocation, output_json,
calSavePath + "/cameracalibration.json",
calSavePath + "/cameracalibration.json", 3, false);
EXPECT_EQ(ret, 1);
@@ -82,8 +85,24 @@ 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);
}
TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag) {
int ret = fieldcalibration::calibrate(
projectRootPath + videoLocation, output_json,
calSavePath + "/cameracalibration.json",
projectRootPath + "/2024-crescendo.json", 42, false);
EXPECT_EQ(ret, 1);
}
TEST(Field_CalibrationTest, Atypical_Bad_Pinned_Tag_Negative) {
int ret = fieldcalibration::calibrate(
projectRootPath + videoLocation, output_json,
calSavePath + "/cameracalibration.json",
projectRootPath + "/2024-crescendo.json", -1, false);
EXPECT_EQ(ret, 1);
}