From 07bd61493015a6921c3091f72c0445f2420fe827 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Tue, 23 Nov 2021 17:19:53 -0300 Subject: [PATCH] t/file: Extend calibration save/load interfaces with new v2 format --- .../calibration_v2.example.json | 5 +- .../calibration_v2.schema.json | 13 +- src/xrt/auxiliary/tracking/t_file.cpp | 135 ++++++++++++++---- src/xrt/auxiliary/tracking/t_tracking.h | 38 ++++- .../state_trackers/gui/gui_scene_calibrate.c | 11 +- src/xrt/state_trackers/prober/p_tracking.c | 10 +- 6 files changed, 155 insertions(+), 57 deletions(-) diff --git a/doc/example_configs/calibration_v2.example.json b/doc/example_configs/calibration_v2.example.json index 9e55985bc..c56b76487 100644 --- a/doc/example_configs/calibration_v2.example.json +++ b/doc/example_configs/calibration_v2.example.json @@ -1,8 +1,7 @@ { "$schema": "https://monado.pages.freedesktop.org/monado/calibration_v2.schema.json", - "file": { - "version": 2, - "created_on": "2021-11-23T14:33:51Z" + "metadata": { + "version": 2 }, "cameras": [ { diff --git a/doc/example_configs/calibration_v2.schema.json b/doc/example_configs/calibration_v2.schema.json index 9d427f2a4..575691f93 100644 --- a/doc/example_configs/calibration_v2.schema.json +++ b/doc/example_configs/calibration_v2.schema.json @@ -3,7 +3,7 @@ "title": "Monado calibration file schema v2", "type": "object", "required": [ - "file", + "metadata", "cameras", "opencv_stereo_calibrate" ], @@ -13,7 +13,7 @@ "title": "JSON Schema directive", "default": "https://monado.pages.freedesktop.org/monado/calibration_v2.schema.json" }, - "file": { + "metadata": { "description": "Calibration file metadata", "type": "object", "required": [ @@ -22,9 +22,6 @@ "properties": { "version": { "const": 2 - }, - "created_on": { - "$ref": "#/$defs/datetime" } } }, @@ -211,12 +208,6 @@ "type": "number" } } - }, - "datetime": { - "$comment": "The format specification might not be asserted against, that's why we also provide a regex pattern", - "type": "string", - "format": "date-time", - "pattern": "^[0-9]{4}-[0-9]{2}-[0-9]{2}T[0-9]{2}:[0-9]{2}:[0-9]{2}Z$" } } } diff --git a/src/xrt/auxiliary/tracking/t_file.cpp b/src/xrt/auxiliary/tracking/t_file.cpp index 862c2f80d..74f4e27a1 100644 --- a/src/xrt/auxiliary/tracking/t_file.cpp +++ b/src/xrt/auxiliary/tracking/t_file.cpp @@ -16,7 +16,7 @@ #include "os/os_time.h" -DEBUG_GET_ONCE_LOG_OPTION(calib_log, "CALIB_LOG", U_LOGGING_WARN) +DEBUG_GET_ONCE_LOG_OPTION(calib_log, "CALIB_LOG", U_LOGGING_INFO) #define CALIB_TRACE(...) U_LOG_IFL_T(debug_get_log_option_calib_log(), __VA_ARGS__) #define CALIB_DEBUG(...) U_LOG_IFL_D(debug_get_log_option_calib_log(), __VA_ARGS__) @@ -212,7 +212,7 @@ using xrt::auxiliary::util::json::JSONNode; extern "C" bool t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_calibration **out_data) { - using xrt::auxiliary::tracking::StereoCameraCalibrationWrapper; + t_stereo_camera_calibration *data_ptr = NULL; t_stereo_camera_calibration_alloc(&data_ptr, 5); // Hardcoded to 5 distortion parameters. StereoCameraCalibrationWrapper wrapped(data_ptr); @@ -274,7 +274,6 @@ t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_cal wrapped.view[1].use_fisheye = wrapped.view[0].use_fisheye; // clang-format on - CALIB_ASSERT_(wrapped.isDataStorageValid()); t_stereo_camera_calibration_reference(out_data, data_ptr); @@ -283,6 +282,23 @@ t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_cal return true; } +static bool +t_stereo_camera_calibration_load_path_v1(const char *calib_path, struct t_stereo_camera_calibration **out_data) +{ + CALIB_WARN("Deprecated function %s", __func__); + + FILE *calib_file = fopen(calib_path, "rb"); + if (calib_file == nullptr) { + CALIB_ERROR("Unable to open calibration file: '%s'", calib_path); + return false; + } + + bool success = t_stereo_camera_calibration_load_v1(calib_file, out_data); + fclose(calib_file); + + return success; +} + #define PINHOLE_RADTAN5 "pinhole_radtan5" #define FISHEYE_EQUIDISTANT4 "fisheye_equidistant4" @@ -368,24 +384,19 @@ t_camera_calibration_load_v2(cJSON *cjson_cam, t_camera_calibration *cc) return true; } -static bool -t_stereo_camera_calibration_load_v2(const char *calib_path, struct t_stereo_camera_calibration **out_stereo) +extern "C" bool +t_stereo_camera_calibration_from_json_v2(cJSON *cjson, struct t_stereo_camera_calibration **out_stereo) { - JSONNode json = JSONNode::loadFromFile(calib_path); - if (json.isInvalid()) { - CALIB_ERROR("Unable to open calibration file: '%s'", calib_path); - return false; - } - + JSONNode json{cjson}; StereoCameraCalibrationWrapper stereo{5}; // Hardcoded to 5 distortion parameters. // Load file metadata const int supported_version = 2; - int version = json["file"]["version"].asInt(supported_version); - if (json["file"]["version"].isInvalid()) { - CALIB_WARN("'file.version' not found in %s, will assume v2", calib_path); + int version = json["metadata"]["version"].asInt(supported_version); + if (json["metadata"]["version"].isInvalid()) { + CALIB_WARN("'metadata.version' not found, will assume version=%d", supported_version); } - CALIB_ASSERTR(version == supported_version, "Calibration file version (%d) != %d", version, supported_version); + CALIB_ASSERTR(version == supported_version, "Calibration json version (%d) != %d", version, supported_version); // Load cameras vector cameras = json["cameras"].asArray(); @@ -412,6 +423,17 @@ t_stereo_camera_calibration_load_v2(const char *calib_path, struct t_stereo_came return true; } +static bool +t_stereo_camera_calibration_load_path_v2(const char *calib_path, struct t_stereo_camera_calibration **out_stereo) +{ + JSONNode json = JSONNode::loadFromFile(calib_path); + if (json.isInvalid()) { + CALIB_ERROR("Unable to open calibration file: '%s'", calib_path); + return false; + } + return t_stereo_camera_calibration_from_json_v2(json.getCJSON(), out_stereo); +} + /* * @@ -422,12 +444,12 @@ t_stereo_camera_calibration_load_v2(const char *calib_path, struct t_stereo_came extern "C" bool t_stereo_camera_calibration_save_v1(FILE *calib_file, struct t_stereo_camera_calibration *data) { - using xrt::auxiliary::tracking::StereoCameraCalibrationWrapper; + CALIB_WARN("Deprecated function: %s", __func__); + StereoCameraCalibrationWrapper wrapped(data); // Dummy matrix cv::Mat dummy; - write_cv_mat(calib_file, &wrapped.view[0].intrinsics_mat); write_cv_mat(calib_file, &wrapped.view[1].intrinsics_mat); write_cv_mat(calib_file, &wrapped.view[0].distortion_mat); @@ -464,6 +486,21 @@ t_stereo_camera_calibration_save_v1(FILE *calib_file, struct t_stereo_camera_cal return true; } +static bool +t_stereo_camera_calibration_save_path_v1(const char *calib_path, struct t_stereo_camera_calibration *data) +{ + FILE *calib_file = fopen(calib_path, "wb"); + if (calib_file == nullptr) { + CALIB_ERROR("Unable to open calibration file: '%s'", calib_path); + return false; + } + + bool success = t_stereo_camera_calibration_save_v1(calib_file, data); + fclose(calib_file); + + return success; +} + //! Writes @p mat data into a @p jb as a json array. static JSONBuilder & operator<<(JSONBuilder &jb, const cv::Mat_ &mat) @@ -476,8 +513,8 @@ operator<<(JSONBuilder &jb, const cv::Mat_ &mat) return jb; } -static bool -t_stereo_camera_calibration_save_v2(const char *calib_path, struct t_stereo_camera_calibration *data) +extern "C" bool +t_stereo_camera_calibration_to_json_v2(cJSON **out_cjson, struct t_stereo_camera_calibration *data) { StereoCameraCalibrationWrapper wrapped(data); JSONBuilder jb{}; @@ -485,12 +522,9 @@ t_stereo_camera_calibration_save_v2(const char *calib_path, struct t_stereo_came jb << "{"; jb << "$schema" << "https://monado.pages.freedesktop.org/monado/calibration_v2.schema.json"; - jb << "file"; + jb << "metadata"; jb << "{"; jb << "version" << 2; - char datetime[OS_ISO_STR_SIZE]; - to_iso_string(os_realtime_get_ns(), datetime); - jb << "created_on" << datetime; jb << "}"; jb << "cameras"; @@ -553,10 +587,26 @@ t_stereo_camera_calibration_save_v2(const char *calib_path, struct t_stereo_came jb << "}"; - CALIB_INFO("Saving calibration file: %s", jb.getBuiltNode()->toString().c_str()); - return jb.getBuiltNode()->saveToFile(calib_path); + cJSON *cjson = jb.getBuiltNode()->getCJSON(); + *out_cjson = cJSON_Duplicate(cjson, true); + return true; } +static bool +t_stereo_camera_calibration_save_path_v2(const char *calib_path, struct t_stereo_camera_calibration *data) +{ + cJSON *cjson = NULL; + bool success = t_stereo_camera_calibration_to_json_v2(&cjson, data); + if (!success) { + return false; + } + + JSONNode json{cjson, true, nullptr}; // is_owner=true so it will free cjson object when leaving scope + CALIB_INFO("Saving calibration file: %s", json.toString(false).c_str()); + return json.saveToFile(calib_path); +} + + /* * * Helpers @@ -630,3 +680,38 @@ read_cv_mat(FILE *f, cv::Mat *m, const char *name) CALIB_ERROR("Mat dimension unknown mismatch: '%s'", name); return false; } + +static bool +has_json_extension(const char *filename) +{ + const char extension[] = ".json"; + size_t name_len = strlen(filename); + size_t ext_len = strlen(extension); + + if (name_len > ext_len) { + return strcmp(&filename[name_len - ext_len], extension) == 0; + } + + return false; +} + + +/* + * + * Exported functions + * + */ + +extern "C" bool +t_stereo_camera_calibration_load(const char *calib_path, struct t_stereo_camera_calibration **out_data) +{ + return has_json_extension(calib_path) ? t_stereo_camera_calibration_load_path_v2(calib_path, out_data) + : t_stereo_camera_calibration_load_path_v1(calib_path, out_data); +} + +extern "C" bool +t_stereo_camera_calibration_save(const char *calib_path, struct t_stereo_camera_calibration *data) +{ + return has_json_extension(calib_path) ? t_stereo_camera_calibration_save_path_v2(calib_path, data) + : t_stereo_camera_calibration_save_path_v1(calib_path, data); +} diff --git a/src/xrt/auxiliary/tracking/t_tracking.h b/src/xrt/auxiliary/tracking/t_tracking.h index 84b0bc249..012ae8d2b 100644 --- a/src/xrt/auxiliary/tracking/t_tracking.h +++ b/src/xrt/auxiliary/tracking/t_tracking.h @@ -13,6 +13,7 @@ #include "xrt/xrt_frame.h" #include "util/u_misc.h" +#include "cjson/cJSON.h" #include @@ -165,7 +166,7 @@ void t_stereo_camera_calibration_dump(struct t_stereo_camera_calibration *c); /*! - * Load stereo calibration data from a given file. + * Load stereo calibration data from a given file in v1 format (binary). * * @relates t_stereo_camera_calibration */ @@ -173,13 +174,46 @@ bool t_stereo_camera_calibration_load_v1(FILE *calib_file, struct t_stereo_camera_calibration **out_data); /*! - * Save the given stereo calibration data to the given file. + * Save the given stereo calibration data to the given file in v1 format (binary). * * @relates t_stereo_camera_calibration */ bool t_stereo_camera_calibration_save_v1(FILE *calib_file, struct t_stereo_camera_calibration *data); +/*! + * Parse the json object in v2 format into stereo calibration data. + * + * @relates t_stereo_camera_calibration + */ +bool +t_stereo_camera_calibration_from_json_v2(cJSON *json, struct t_stereo_camera_calibration **out_data); + +/*! + * Convert the given stereo calibration data into a json object in v2 format. + * + * @relates t_stereo_camera_calibration + */ +bool +t_stereo_camera_calibration_to_json_v2(cJSON **out_json, struct t_stereo_camera_calibration *data); + + +/*! + * Load stereo calibration data from a given file path. + * + * @relates t_stereo_camera_calibration + */ +bool +t_stereo_camera_calibration_load(const char *calib_path, struct t_stereo_camera_calibration **out_data); + +/*! + * Save the given stereo calibration data to the given file path. + * + * @relates t_stereo_camera_calibration + */ +bool +t_stereo_camera_calibration_save(const char *calib_path, struct t_stereo_camera_calibration *data); + /* * diff --git a/src/xrt/state_trackers/gui/gui_scene_calibrate.c b/src/xrt/state_trackers/gui/gui_scene_calibrate.c index f0479ba00..4e95e3726 100644 --- a/src/xrt/state_trackers/gui/gui_scene_calibrate.c +++ b/src/xrt/state_trackers/gui/gui_scene_calibrate.c @@ -77,7 +77,7 @@ save_calibration(struct calibration_scene *cs) saved_header(cs); igSetNextItemWidth(115); - igInputText(".calibration", cs->filename, sizeof(cs->filename), 0, NULL, NULL); + igInputText(".calibration.json", cs->filename, sizeof(cs->filename), 0, NULL, NULL); igSameLine(0.0f, 4.0f); static ImVec2 button_dims = {0, 0}; @@ -93,8 +93,8 @@ save_calibration(struct calibration_scene *cs) * */ - char tmp[sizeof(cs->filename) + 16]; - snprintf(tmp, sizeof(tmp), "%s.calibration", cs->filename); + char tmp[sizeof(cs->filename) + 32]; + snprintf(tmp, sizeof(tmp), "%s.calibration.json", cs->filename); u_file_get_path_in_config_dir(tmp, cs->settings->calibration_path, sizeof(cs->settings->calibration_path)); @@ -114,10 +114,7 @@ save_calibration(struct calibration_scene *cs) * */ - FILE *calib_file = fopen(cs->settings->calibration_path, "wb"); - t_stereo_camera_calibration_save_v1(calib_file, cs->status.stereo_data); - fclose(calib_file); - calib_file = NULL; + t_stereo_camera_calibration_save(cs->settings->calibration_path, cs->status.stereo_data); cs->saved = true; } diff --git a/src/xrt/state_trackers/prober/p_tracking.c b/src/xrt/state_trackers/prober/p_tracking.c index 49175452f..4ff9bd8c2 100644 --- a/src/xrt/state_trackers/prober/p_tracking.c +++ b/src/xrt/state_trackers/prober/p_tracking.c @@ -163,18 +163,10 @@ p_factory_ensure_frameserver(struct p_factory *fact) return; } - // Open the calibration file. - FILE *file = fopen(fact->settings.calibration_path, "rb"); - if (file == NULL) { - return; - } - // Parse the calibration data from the file. - if (!t_stereo_camera_calibration_load_v1(file, &fact->data)) { - fclose(file); + if (!t_stereo_camera_calibration_load(fact->settings.calibration_path, &fact->data)) { return; } - fclose(file); struct xrt_frame_sink *xsink = NULL; struct xrt_frame_sink *xsinks[4] = {0};