diff --git a/include/librealsense2/h/rs_device.h b/include/librealsense2/h/rs_device.h index 7fb559f6704..fc3ca8996e1 100644 --- a/include/librealsense2/h/rs_device.h +++ b/include/librealsense2/h/rs_device.h @@ -598,40 +598,21 @@ float rs2_calculate_target_z_cpp(rs2_device* device, rs2_frame_queue* queue1, rs float rs2_calculate_target_z(rs2_device* device, rs2_frame_queue* queue1, rs2_frame_queue* queue2, rs2_frame_queue* queue3, float target_width, float target_height, rs2_update_progress_callback_ptr progress_callback, void* client_data, rs2_error** error); - /** * rs2_get_calibration_config -* \param[in] device The device -* \param[out] calib_config Calibration Configuration struct to be filled -* \param[out] error If non-null, receives any error that occurs during this call, otherwise, errors are ignored -*/ -void rs2_get_calibration_config(rs2_device* device, rs2_calibration_config* calib_config, rs2_error** error); - -/** -* rs2_set_calibration_config -* \param[in] device The device -* \param[in] calib_config Calibration Configuration struct to set -* \param[out] error If non-null, receives any error that occurs during this call, otherwise, errors are ignored -*/ -void rs2_set_calibration_config(rs2_device* device, rs2_calibration_config const* calib_config, rs2_error** error); - -/** -* rs2_json_string_to_calibration_config -* \param[in] device The device -* \param[in] json_str JSON string to convert -* \param[out] calib_config Calibration config struct result +* \param[in] device Device * \param[out] error If non-null, receives any error that occurs during this call, otherwise, errors are ignored +* \return JSON string representing the calibration config as rs2_raw_data_buffer */ -void rs2_json_string_to_calibration_config(rs2_device* device, const char* json_str, rs2_calibration_config* calib_config, rs2_error** error); +const rs2_raw_data_buffer* rs2_get_calibration_config(rs2_device* device, rs2_error** error); /** -* rs2_calibration_config_to_json_string -* \param[in] device The device -* \param[in] calib_config Calibration config to convert -* \param[out] error If non-null, receives any error that occurs during this call, otherwise, errors are ignored -* \return JSON string representing the calibration config as rs2_raw_data_buffer +* rs2_set_calibration_config +* \param[in] sensor Safety sensor +* \param[in] calibration_config_json_str Calibration config as JSON string +* \param[out] error If non-null, receives any error that occurs during this call, otherwise, errors are ignored */ -const rs2_raw_data_buffer* rs2_calibration_config_to_json_string(rs2_device* device, rs2_calibration_config const* calib_config, rs2_error** error); +void rs2_set_calibration_config(rs2_device* device, const char* calibration_config_json_str, rs2_error** error); #ifdef __cplusplus } diff --git a/include/librealsense2/hpp/rs_device.hpp b/include/librealsense2/hpp/rs_device.hpp index b43218ce16d..4068dc5a532 100644 --- a/include/librealsense2/hpp/rs_device.hpp +++ b/include/librealsense2/hpp/rs_device.hpp @@ -695,48 +695,6 @@ namespace rs2 error::handle(e); } - /** - * json_string_to_calibration_config - * \param[in] json_str JSON string to convert to calibration config struct - * \param[out] error If non-null, receives any error that occurs during this call, otherwise, errors are ignored - * \return calib_config Calibration Configuration struct to be filled - */ - rs2_calibration_config json_string_to_calibration_config(const std::string& json_str) const - { - rs2_error* e = nullptr; - rs2_calibration_config calib_config; - rs2_json_string_to_calibration_config(_dev.get(), json_str.c_str(), &calib_config, &e); - error::handle(e); - return calib_config; - } - - /** - * calibration_config_to_json_string - * \param[in] calib_config Calibration config struct to convert to JSON string - * \param[out] error If non-null, receives any error that occurs during this call, otherwise, errors are ignored - * \return calib_config Calibration Configuration struct to be filled - */ - std::string calibration_config_to_json_string(const rs2_calibration_config& calib_config) const - { - std::vector result; - - rs2_error* e = nullptr; - auto buffer = rs2_calibration_config_to_json_string(_dev.get(), &calib_config, &e); - - std::shared_ptr list(buffer, rs2_delete_raw_data); - error::handle(e); - - auto size = rs2_get_raw_data_size(list.get(), &e); - error::handle(e); - - auto start = rs2_get_raw_data(list.get(), &e); - error::handle(e); - - result.insert(result.begin(), start, start + size); - - return std::string(result.begin(), result.end()); - } - /** * Run target-based focal length calibration for D400 Stereo Cameras * \param[in] left_queue: container for left IR frames with resoluton of 1280x720 and the target in the center of 320x240 pixels ROI. @@ -910,30 +868,31 @@ namespace rs2 return result; } - /** - * get_calibration_config - * \param[out] error If non-null, receives any error that occurs during this call, otherwise, errors are ignored - * \return calib_config Calibration Configuration struct to be filled - */ - rs2_calibration_config get_calibration_config() const + std::string get_calibration_config() const { + std::vector result; + rs2_error* e = nullptr; - rs2_calibration_config calib_config; - rs2_get_calibration_config(_dev.get(), &calib_config, &e); + auto buffer = rs2_get_calibration_config(_dev.get(), &e); + + std::shared_ptr list(buffer, rs2_delete_raw_data); error::handle(e); - return calib_config; + + auto size = rs2_get_raw_data_size(list.get(), &e); + error::handle(e); + + auto start = rs2_get_raw_data(list.get(), &e); + error::handle(e); + + result.insert(result.begin(), start, start + size); + + return std::string(result.begin(), result.end()); } - - /** - * set_calibration_config - * \param[out] error If non-null, receives any error that occurs during this call, otherwise, errors are ignored - * \param[out] error If non-null, receives any error that occurs during this call, otherwise, errors are ignored - * \return calib_config Calibration Configuration struct to be filled - */ - void set_calibration_config(const rs2_calibration_config& calib_config) + + void set_calibration_config(const std::string& calibration_config_json_str) const { rs2_error* e = nullptr; - rs2_set_calibration_config(_dev.get(), &calib_config, &e); + rs2_set_calibration_config(_dev.get(), calibration_config_json_str.c_str(), &e); error::handle(e); } }; diff --git a/src/auto-calibrated-device.h b/src/auto-calibrated-device.h index 21e24653447..50b2ff6dc61 100644 --- a/src/auto-calibrated-device.h +++ b/src/auto-calibrated-device.h @@ -25,10 +25,8 @@ namespace librealsense float* const health, int health_size, rs2_update_progress_callback_sptr progress_callback) = 0; virtual float calculate_target_z(rs2_frame_queue* queue1, rs2_frame_queue* queue2, rs2_frame_queue* queue3, float target_w, float target_h, rs2_update_progress_callback_sptr progress_callback) = 0; - virtual rs2_calibration_config get_calibration_config() const = 0; - virtual void set_calibration_config(const rs2_calibration_config& calib_config) = 0; - virtual std::string calibration_config_to_json_string(const rs2_calibration_config& calib_config) const = 0; - virtual rs2_calibration_config json_string_to_calibration_config(const std::string& json_str) const = 0; + virtual std::string get_calibration_config() const = 0; + virtual void set_calibration_config(const std::string& calibration_config_json_str) const = 0; }; MAP_EXTENSION(RS2_EXTENSION_AUTO_CALIBRATED_DEVICE, auto_calibrated_interface); } diff --git a/src/ds/d400/d400-auto-calibration.cpp b/src/ds/d400/d400-auto-calibration.cpp index 79a574b0d4e..780f26a5827 100644 --- a/src/ds/d400/d400-auto-calibration.cpp +++ b/src/ds/d400/d400-auto-calibration.cpp @@ -2567,22 +2567,12 @@ namespace librealsense throw std::runtime_error("Failed to extract target dimension info!"); } - rs2_calibration_config auto_calibrated::get_calibration_config() const + std::string auto_calibrated::get_calibration_config() const { throw std::runtime_error(rsutils::string::from() << "Calibration Config not applicable for this device"); } - void auto_calibrated::set_calibration_config(const rs2_calibration_config& calib_config) - { - throw std::runtime_error(rsutils::string::from() << "Calibration Config not applicable for this device"); - } - - std::string auto_calibrated::calibration_config_to_json_string(const rs2_calibration_config& calib_config) const - { - throw std::runtime_error(rsutils::string::from() << "Calibration Config not applicable for this device"); - } - - rs2_calibration_config auto_calibrated::json_string_to_calibration_config(const std::string& json_str) const + void auto_calibrated::set_calibration_config(const std::string& calibration_config_json_str) const { throw std::runtime_error(rsutils::string::from() << "Calibration Config not applicable for this device"); } diff --git a/src/ds/d400/d400-auto-calibration.h b/src/ds/d400/d400-auto-calibration.h index e7cb9c035b5..476b5163fde 100644 --- a/src/ds/d400/d400-auto-calibration.h +++ b/src/ds/d400/d400-auto-calibration.h @@ -60,11 +60,8 @@ namespace librealsense float* const health, int health_size, rs2_update_progress_callback_sptr progress_callback) override; float calculate_target_z(rs2_frame_queue* queue1, rs2_frame_queue* queue2, rs2_frame_queue* queue3, float target_width, float target_height, rs2_update_progress_callback_sptr progress_callback) override; - rs2_calibration_config get_calibration_config() const override; - void set_calibration_config(const rs2_calibration_config& calib_config) override; - std::string calibration_config_to_json_string(const rs2_calibration_config& calib_config) const override; - rs2_calibration_config json_string_to_calibration_config(const std::string& json_str) const override; - + std::string get_calibration_config() const override; + void set_calibration_config(const std::string& calibration_config_json_str) const override; void set_hw_monitor_for_auto_calib(std::shared_ptr hwm); private: diff --git a/src/ds/d500/d500-auto-calibration.cpp b/src/ds/d500/d500-auto-calibration.cpp index 97d7d5e80f0..9b67b231073 100644 --- a/src/ds/d500/d500-auto-calibration.cpp +++ b/src/ds/d500/d500-auto-calibration.cpp @@ -6,7 +6,7 @@ #include "d500-private.h" #include #include - +#include "d500-types/calibration-config.h" namespace librealsense { @@ -301,47 +301,51 @@ namespace librealsense throw std::runtime_error(rsutils::string::from() << "Calculate T not applicable for this device"); } - rs2_calibration_config d500_auto_calibrated::get_calibration_config() const - { - rs2_calibration_config_with_header* calib_config_with_header; + std::string d500_auto_calibrated::get_calibration_config() const + { + calibration_config_with_header* result; // prepare command - using namespace ds; - command cmd(GET_HKR_CONFIG_TABLE, - static_cast(d500_calib_location::d500_calib_flash_memory), - static_cast(d500_calibration_table_id::calib_cfg_id), - static_cast(d500_calib_type::d500_calib_dynamic)); - auto res = _hw_monitor->send(cmd); - - if (res.size() < sizeof(rs2_calibration_config_with_header)) + command cmd(ds::GET_HKR_CONFIG_TABLE, + static_cast(ds::d500_calib_location::d500_calib_flash_memory), + static_cast(ds::d500_calibration_table_id::calib_cfg_id), + static_cast(ds::d500_calib_type::d500_calib_dynamic)); + cmd.require_response = true; + + // send command to device and get response (safety_interface_config entry + header) + std::vector< uint8_t > response = _hw_monitor->send(cmd); + if (response.size() < sizeof(calibration_config_with_header)) { - throw io_exception(rsutils::string::from() << "Calibration config reading failed"); + throw io_exception(rsutils::string::from() << "Calibration Config Read Failed"); } - calib_config_with_header = reinterpret_cast(res.data()); - // check CRC before returning result - auto computed_crc32 = rsutils::number::calc_crc32(res.data() + sizeof(rs2_calibration_config_header), sizeof(rs2_calibration_config)); - if (computed_crc32 != calib_config_with_header->header.crc32) + // check CRC before returning result + auto computed_crc32 = rsutils::number::calc_crc32(response.data() + sizeof(table_header), + sizeof(calibration_config)); + result = reinterpret_cast(response.data()); + if (computed_crc32 != result->get_table_header().get_crc32()) { - throw invalid_value_exception(rsutils::string::from() << "Invalid CRC value for calibration config table"); + throw invalid_value_exception(rsutils::string::from() << "Calibration Config Invalid CRC Value"); } - return calib_config_with_header->payload; + rsutils::json j = result->get_calibration_config().to_json(); + return j.dump(); } - void d500_auto_calibrated::set_calibration_config(const rs2_calibration_config& calib_config) + void d500_auto_calibrated::set_calibration_config(const std::string& calibration_config_json_str) const { + rsutils::json json_data = rsutils::json::parse(calibration_config_json_str); + calibration_config calib_config(json_data["calibration_config"]); + // calculate CRC - uint32_t computed_crc32 = rsutils::number::calc_crc32(reinterpret_cast(&calib_config), - sizeof(rs2_calibration_config)); + uint32_t computed_crc32 = rsutils::number::calc_crc32(reinterpret_cast(&calib_config), sizeof(calibration_config)); - // prepare vector of data to be sent (header + sp) - rs2_calibration_config_with_header calib_config_with_header; + // prepare vector of data to be sent (header + calibration_config) uint16_t version = ((uint16_t)0x01 << 8) | 0x01; // major=0x01, minor=0x01 --> ver = major.minor uint32_t calib_version = 0; // ignoring this field, as requested by sw architect - calib_config_with_header.header = { version, static_cast(ds::d500_calibration_table_id::calib_cfg_id), - sizeof(rs2_calibration_config), calib_version, computed_crc32 }; - calib_config_with_header.payload = calib_config; + table_header header(version, static_cast(ds::d500_calibration_table_id::calib_cfg_id), sizeof(calibration_config), + calib_version, computed_crc32); + calibration_config_with_header calib_config_with_header(header, calib_config); auto data_as_ptr = reinterpret_cast(&calib_config_with_header); // prepare command @@ -349,93 +353,13 @@ namespace librealsense static_cast(ds::d500_calib_location::d500_calib_flash_memory), static_cast(ds::d500_calibration_table_id::calib_cfg_id), static_cast(ds::d500_calib_type::d500_calib_dynamic)); - cmd.data.insert(cmd.data.end(), data_as_ptr, data_as_ptr + sizeof(rs2_calibration_config_with_header)); + cmd.data.insert(cmd.data.end(), data_as_ptr, data_as_ptr + sizeof(calibration_config_with_header)); cmd.require_response = false; // send command _hw_monitor->send(cmd); } - std::string d500_auto_calibrated::calibration_config_to_json_string(const rs2_calibration_config& calib_config) const - { - - rsutils::json json_data; - json_data["calibration_config"]["calib_roi_num_of_segments"] = calib_config.calib_roi_num_of_segments; - - for (int roi_index = 0; roi_index < 4; roi_index++) - { - for (int mask_index = 0; mask_index < 4; mask_index++) - { - json_data["calibration_config"]["roi"][roi_index][mask_index][0] = calib_config.roi[roi_index].mask_pixel[mask_index][0]; - json_data["calibration_config"]["roi"][roi_index][mask_index][1] = calib_config.roi[roi_index].mask_pixel[mask_index][1]; - } - } - - auto& rotation = json_data["calibration_config"]["camera_position"]["rotation"]; - rotation[0][0] = calib_config.camera_position.rotation.x.x; - rotation[0][1] = calib_config.camera_position.rotation.x.y; - rotation[0][2] = calib_config.camera_position.rotation.x.z; - rotation[1][0] = calib_config.camera_position.rotation.y.x; - rotation[1][1] = calib_config.camera_position.rotation.y.y; - rotation[1][2] = calib_config.camera_position.rotation.y.z; - rotation[2][0] = calib_config.camera_position.rotation.z.x; - rotation[2][1] = calib_config.camera_position.rotation.z.y; - rotation[2][2] = calib_config.camera_position.rotation.z.z; - - auto& translation = json_data["calibration_config"]["camera_position"]["translation"]; - translation[0] = calib_config.camera_position.translation.x; - translation[1] = calib_config.camera_position.translation.y; - translation[2] = calib_config.camera_position.translation.z; - - // fill crypto signature array - size_t number_of_elements = sizeof(calib_config.crypto_signature) / sizeof(calib_config.crypto_signature[0]); - std::vector crypto_signature_byte_array(number_of_elements); - memcpy(crypto_signature_byte_array.data(), calib_config.crypto_signature, sizeof(calib_config.crypto_signature)); - json_data["calibration_config"]["crypto_signature"] = crypto_signature_byte_array; - - return json_data.dump(); - - } - - rs2_calibration_config d500_auto_calibrated::json_string_to_calibration_config(const std::string& json_str) const - { - rsutils::json json_data = rsutils::json::parse(json_str); - rs2_calibration_config calib_config; - - calib_config.calib_roi_num_of_segments = json_data["calibration_config"]["calib_roi_num_of_segments"]; - - for (int roi_index = 0; roi_index < 4; roi_index++) - { - for (int mask_index = 0; mask_index < 4; mask_index++) - { - calib_config.roi[roi_index].mask_pixel[mask_index][0] = json_data["calibration_config"]["roi"][roi_index][mask_index][0]; - calib_config.roi[roi_index].mask_pixel[mask_index][1] = json_data["calibration_config"]["roi"][roi_index][mask_index][1]; - } - } - - auto& rotation = json_data["calibration_config"]["camera_position"]["rotation"]; - calib_config.camera_position.rotation.x.x = rotation[0][0]; - calib_config.camera_position.rotation.x.y = rotation[0][1]; - calib_config.camera_position.rotation.x.z = rotation[0][2]; - calib_config.camera_position.rotation.y.x = rotation[1][0]; - calib_config.camera_position.rotation.y.y = rotation[1][1]; - calib_config.camera_position.rotation.y.z = rotation[1][2]; - calib_config.camera_position.rotation.z.x = rotation[2][0]; - calib_config.camera_position.rotation.z.y = rotation[2][1]; - calib_config.camera_position.rotation.z.z = rotation[2][2]; - - auto& translation = json_data["calibration_config"]["camera_position"]["translation"]; - calib_config.camera_position.translation.x = translation[0]; - calib_config.camera_position.translation.y = translation[1]; - calib_config.camera_position.translation.z = translation[2]; - - // fill crypto signature array - std::vector crypto_signature_vector = json_data["calibration_config"]["crypto_signature"].get>(); - std::memcpy(calib_config.crypto_signature, crypto_signature_vector.data(), crypto_signature_vector.size() * sizeof(uint8_t)); - - return calib_config; - } - void d500_auto_calibrated::set_hw_monitor_for_auto_calib(std::shared_ptr hwm) { _hw_monitor = hwm; diff --git a/src/ds/d500/d500-auto-calibration.h b/src/ds/d500/d500-auto-calibration.h index 2233068ad10..8039c7e0e0f 100644 --- a/src/ds/d500/d500-auto-calibration.h +++ b/src/ds/d500/d500-auto-calibration.h @@ -26,10 +26,8 @@ namespace librealsense float* const health, int health_size, rs2_update_progress_callback_sptr progress_callback) override; float calculate_target_z(rs2_frame_queue* queue1, rs2_frame_queue* queue2, rs2_frame_queue* queue3, float target_width, float target_height, rs2_update_progress_callback_sptr progress_callback) override; - rs2_calibration_config get_calibration_config() const override; - void set_calibration_config(const rs2_calibration_config& calib_config) override; - std::string calibration_config_to_json_string(const rs2_calibration_config& calib_config) const override; - rs2_calibration_config json_string_to_calibration_config(const std::string& json_str) const override; + std::string get_calibration_config() const override; + void set_calibration_config(const std::string& calibration_config_json_str) const override; void set_hw_monitor_for_auto_calib(std::shared_ptr hwm); diff --git a/src/ds/d500/d500-types/calibration-config.h b/src/ds/d500/d500-types/calibration-config.h new file mode 100644 index 00000000000..4ed4e680047 --- /dev/null +++ b/src/ds/d500/d500-types/calibration-config.h @@ -0,0 +1,298 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "common.h" + +namespace librealsense +{ + +#pragma pack(push, 1) + + /* + class calibration_roi + Handles the calibration region of interesets corners: + - Array of four corners in Deph Frame Coordinate system that define a closed simple quadrangle (non-intersecting) + - Segment 0 = convex tetragon + - The vertices of the tetragon are ordered clockwise + - Vertex = [x, y] = pixel coordinates in the reference depth map + - 0-based coordinates: [0, 0] = center of the top-left pixel + */ + class calibration_roi + { + public: + calibration_roi() + { + for (size_t i = 0; i < 4; ++i) + { + m_mask_pixel[i][0] = 0; + m_mask_pixel[i][1] = 0; + } + } + + explicit calibration_roi(const json &j, const std::string &field_name = "roi") + { + validate_json(j, field_name); + for (size_t i = 0; i < 4; ++i) + { + m_mask_pixel[i][0] = j[i][0]; + m_mask_pixel[i][1] = j[i][1]; + } + } + + json to_json() const + { + json j; + for (const auto &elem : m_mask_pixel) + { + j.push_back({elem[0], elem[1]}); + } + return j; + } + + private: + std::array, 4> m_mask_pixel; + + void validate_json(const json &j, const std::string &field_name) + { + if (!j.is_array() || j.size() != 4) + { + throw std::invalid_argument("Invalid JSON for " + field_name + ": must be an array of 4 elements."); + } + for (const auto &elem : j) + { + if (!elem.is_array() || elem.size() != 2) + { + throw std::invalid_argument("Invalid JSON for " + field_name + ": each element must be an array of 2 elements."); + } + if (!elem[0].is_number_unsigned() || !elem[1].is_number_unsigned()) + { + throw std::invalid_argument("Invalid JSON for " + field_name + ": each coordinate must be an unsigned integer."); + } + } + } + }; + + class float3_row_major + { + + public: + float3_row_major(float x = 0.f, float y = 0.f, float z = 0.f) : m_x(x), m_y(y), m_z(z) + { + } + + explicit float3_row_major(const json &j, const std::string &field_name = "float3_row_major") + { + validate_json(j, field_name); + m_x = j[0]; + m_y = j[1]; + m_z = j[2]; + } + + json to_json() const + { + return {m_x, m_y, m_z}; + } + + private: + float m_x, m_y, m_z; + + void validate_json(const json &j, const std::string &field_name) + { + if (!j.is_array() || j.size() != 3) + { + throw std::invalid_argument("Invalid JSON for " + field_name + ": must be an array of 3 elements."); + } + if (!j[0].is_number_float() || !j[1].is_number_float() || !j[2].is_number_float()) + { + throw std::invalid_argument("Invalid JSON for " + field_name + ": each element must be a float."); + } + } + }; + + class float3x3_row_major + { + + public: + float3x3_row_major() + { + } + + explicit float3x3_row_major(const json &j, const std::string &field_name = "float3x3_row_major") + { + validate_json(j, field_name); + for (size_t i = 0; i < 3; ++i) + { + m_matrix[i] = float3_row_major(j[i]); + } + } + + json to_json() const + { + json j; + for (const auto &elem : m_matrix) + { + j.push_back(elem.to_json()); + } + return j; + } + + private: + std::array m_matrix; + + void validate_json(const json &j, const std::string &field_name) + { + if (!j.is_array() || j.size() != 3) + { + throw std::invalid_argument("Invalid JSON for " + field_name + " : must be an array of 3 elements."); + } + for (const auto &elem : j) + { + if (!elem.is_array() || elem.size() != 3 || !elem[0].is_number_float() || !elem[1].is_number_float() || !elem[2].is_number_float()) + { + throw std::invalid_argument("Invalid JSON for " + field_name + " : must be a 3x3 array of floats."); + } + } + } + }; + + /* + class extrinsics_row_major + Handles rotation and translation of the camera extrinsics + */ + class extrinsics_row_major + { + public: + extrinsics_row_major() + { + } + + explicit extrinsics_row_major(const json &j, const std::string &field_name = "extrinsics_row_major") + { + validate_json(j, field_name); + m_rotation = float3x3_row_major(j.at("rotation"), "rotation"); + m_translation = float3_row_major(j.at("translation"), "translation"); + } + + json to_json() const + { + return {{"rotation", m_rotation.to_json()}, {"translation", m_translation.to_json()}}; + } + + private: + float3x3_row_major m_rotation; + float3_row_major m_translation; + + void validate_json(const json &j, const std::string &field_name) + { + if (!j.contains("rotation")) { + throw std::invalid_argument("Invalid JSON for " + field_name + ": must contain 'rotation' field."); + } + if (!j.contains("translation")) { + throw std::invalid_argument("Invalid JSON for " + field_name + ": must contain 'translation' field."); + } + } + }; + + + class calibration_config + { + public: + explicit calibration_config(const json &j) + { + validate_json(j); + m_calib_roi_num_of_segments = j.at("calib_roi_num_of_segments"); + m_camera_position = extrinsics_row_major(j.at("camera_position"), "camera_position"); + for (size_t i = 0; i < 4; ++i) + { + m_roi[i] = calibration_roi(j.at("roi")[i], "roi"); + } + for (size_t i = 0; i < 32; ++i) + { + m_crypto_signature[i] = j.at("crypto_signature")[i]; + } + } + + json to_json() const + { + json j; + j["calib_roi_num_of_segments"] = m_calib_roi_num_of_segments; + j["roi"] = json::array(); + for (const auto &elem : m_roi) + { + j["roi"].push_back(elem.to_json()); + } + j["camera_position"] = m_camera_position.to_json(); + j["crypto_signature"] = json::array(); + for (const auto &elem : m_crypto_signature) + { + j["crypto_signature"].push_back(elem); + } + return j; + } + + private: + + uint8_t m_calib_roi_num_of_segments // Within 0-4 range: 0 - Default. No limitations. Full FOV can be used in TC. + // 1-4: Segments defined.The segment must be sequential + std::array m_roi; // see calibration_roi class documentation + std::array m_reserved1 = {0}; + extrinsics_row_major m_camera_position; // used for verification. see extrinsics_row_major class documentation + std::array m_reserved2 = {0}; + std::array m_crypto_signature; // SHA2 or similar + std::array m_reserved3 = {0}; + + void validate_json(const json &j) + { + if (!j.contains("calib_roi_num_of_segments")) + { + throw std::invalid_argument("Invalid JSON for calibration_config: must contain 'calib_roi_num_of_segments' field."); + } + if (!j.contains("roi")) + { + throw std::invalid_argument("Invalid JSON for calibration_config: must contain 'roi' field."); + } + if (!j.contains("camera_position")) + { + throw std::invalid_argument("Invalid JSON for calibration_config: must contain 'camera_position' field."); + } + if (!j.contains("crypto_signature")) + { + throw std::invalid_argument("Invalid JSON for calibration_config: must contain 'crypto_signature' field."); + } + } + }; + + /*** + * calibration_config_with_header class + * Consists of calibration config table and a table header + * According to flash 0.92: + * Version major.minor: 0x01 0x01 + * table type ctCalibCFG(0xC0DD) + * table size 496 + */ + class calibration_config_with_header + { + public: + calibration_config_with_header(table_header header, const calibration_config &calib_config) : m_header(header), m_calib_config(calib_config) + { + } + + calibration_config get_calibration_config() const + { + return m_calib_config; + } + + table_header get_table_header() const + { + return m_header; + } + + private: + table_header m_header; + calibration_config m_calib_config; + }; + +#pragma pack(pop) +} diff --git a/src/ds/d500/d500-types/common.h b/src/ds/d500/d500-types/common.h new file mode 100644 index 00000000000..17cd088cb60 --- /dev/null +++ b/src/ds/d500/d500-types/common.h @@ -0,0 +1,47 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#pragma once + +#include +#include +#include +#include +#include + +namespace librealsense +{ + using rsutils::json; + using rsutils::json_validator::validate_json_field; + +#pragma pack(push, 1) + + class table_header + { + public: + table_header(uint16_t version, uint16_t table_type, uint32_t table_size, uint32_t calib_version, uint32_t crc32) : m_version(version), m_table_type(table_type), m_table_size(table_size), m_calib_version(calib_version), m_crc32(crc32) + { + } + + uint32_t get_crc32() const + { + return m_crc32; + } + + private: + uint16_t m_version; // major.minor. Big-endian + uint16_t m_table_type; // type + uint32_t m_table_size; // full size including: header footer + uint32_t m_calib_version; // major.minor.index + uint32_t m_crc32; // crc of all the data in table excluding this header/CRC + }; + +#pragma pack(pop) + + // helper function + template + std::vector native_arr_to_std_vector(const T (&arr)[N]) + { + return std::vector(std::begin(arr), std::end(arr)); + } +} \ No newline at end of file diff --git a/src/realsense.def b/src/realsense.def index 14de1fb8737..06f4507611c 100644 --- a/src/realsense.def +++ b/src/realsense.def @@ -438,5 +438,3 @@ EXPORTS rs2_project_color_pixel_to_depth_pixel rs2_get_calibration_config rs2_set_calibration_config - rs2_calibration_config_to_json_string - rs2_json_string_to_calibration_config diff --git a/src/rs.cpp b/src/rs.cpp index aa9e6e490ab..dfb43569adf 100644 --- a/src/rs.cpp +++ b/src/rs.cpp @@ -4389,50 +4389,27 @@ float rs2_calculate_target_z(rs2_device* device, rs2_frame_queue* queue1, rs2_fr } } HANDLE_EXCEPTIONS_AND_RETURN(-1.f, device, queue1, queue2, queue3, target_width, target_height) -void rs2_get_calibration_config(rs2_device* device, rs2_calibration_config* calib_config, rs2_error** error) BEGIN_API_CALL -{ - VALIDATE_NOT_NULL(device); - VALIDATE_NOT_NULL(calib_config); - - auto auto_calib = VALIDATE_INTERFACE(device->device, librealsense::auto_calibrated_interface); - *calib_config = auto_calib->get_calibration_config(); -} -HANDLE_EXCEPTIONS_AND_RETURN(, device, calib_config) -void rs2_set_calibration_config(rs2_device* device, rs2_calibration_config const* calib_config, rs2_error** error) BEGIN_API_CALL -{ - VALIDATE_NOT_NULL(device); - VALIDATE_NOT_NULL(calib_config); - - auto auto_calib = VALIDATE_INTERFACE(device->device, librealsense::auto_calibrated_interface); - auto_calib->set_calibration_config(*calib_config); -} -HANDLE_EXCEPTIONS_AND_RETURN(, device, calib_config) - -void rs2_json_string_to_calibration_config( +const rs2_raw_data_buffer* rs2_get_calibration_config( rs2_device* device, - const char* json_str, - rs2_calibration_config* calib_config, rs2_error** error) BEGIN_API_CALL { VALIDATE_NOT_NULL(device); - VALIDATE_NOT_NULL(calib_config); auto auto_calib = VALIDATE_INTERFACE(device->device, librealsense::auto_calibrated_interface); - *calib_config = auto_calib->json_string_to_calibration_config(json_str); + auto ret_str = auto_calib->get_calibration_config(); + std::vector vec(ret_str.begin(), ret_str.end()); + return new rs2_raw_data_buffer{ std::move(vec) }; } -HANDLE_EXCEPTIONS_AND_RETURN(, device, calib_config) +HANDLE_EXCEPTIONS_AND_RETURN(nullptr, device) -const rs2_raw_data_buffer* rs2_calibration_config_to_json_string( +void rs2_set_calibration_config( rs2_device* device, - rs2_calibration_config const* calib_config, + const char* calibration_config_json_str, rs2_error** error) BEGIN_API_CALL { VALIDATE_NOT_NULL(device); - VALIDATE_NOT_NULL(calib_config); + VALIDATE_NOT_NULL(calibration_config_json_str); auto auto_calib = VALIDATE_INTERFACE(device->device, librealsense::auto_calibrated_interface); - auto ret_str = auto_calib->calibration_config_to_json_string(*calib_config); - std::vector vec(ret_str.begin(), ret_str.end()); - return new rs2_raw_data_buffer{ std::move(vec) }; + auto_calib->set_calibration_config(calibration_config_json_str); } -HANDLE_EXCEPTIONS_AND_RETURN(nullptr, device, calib_config) - +HANDLE_EXCEPTIONS_AND_RETURN(, device, calibration_config_json_str) diff --git a/unit-tests/live/d500/test-get-set-calib-config-table-api.py b/unit-tests/live/d500/test-get-set-calib-config-table-api.py new file mode 100644 index 00000000000..c6c52612e66 --- /dev/null +++ b/unit-tests/live/d500/test-get-set-calib-config-table-api.py @@ -0,0 +1,168 @@ +# License: Apache 2.0. See LICENSE file in root directory. +# Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +# test:donotrun + +import pyrealsense2 as rs +from rspy import test, log +import json + +############################################################################################# +# Tests +############################################################################################# + +test.start("Verify auto calibrated device extension") +ctx = rs.context() +dev = ctx.query_devices()[0] +ac_dev = dev.as_auto_calibrated_device() +test.finish() + +############################################################################################# + +test.start("Writing calibration config, then reading and comparing calibration config JSONs") + +# save current calibration config json in order to restore it at the end +original_calib_config = ac_dev.get_calibration_config() + +# calibration config JSON String to be written +new_calib_config = ''' +{ + "calibration_config": + { + "calib_roi_num_of_segments": 0, + "roi": [ [[0,0], [0,0], [0,0], [0,0]], [[0,0], [0,0], [0,0], [0,0]], [[0,0], [0,0], [0,0], [0,0]], [[0,0], [0,0], [0,0], [0,0]] ], + "camera_position": + { + "rotation": + [ + [ 0.0, 0.0, 1.0], + [-1.0, 0.0, 0.0], + [ 0.0, -1.0, 0.0] + ], + "translation": [0.0, 0.0, 0.27] + }, + "crypto_signature": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + } +} +''' + +# write the above calib config table to the device +ac_dev.set_calibration_config(new_calib_config) + +# read the current calib config table from the device +read_result = ac_dev.get_calibration_config() + +# verify the JSON objects are equal (comparing JSON object because +# the JSON string can have different order of inner fields +test.check_equal(json.loads(read_result), json.loads(new_calib_config)) + +test.finish() + +############################################################################################# + +test.start("Trying to write bad calib config with a missing field") + +original_json = json.loads(new_calib_config) + +# List of keys in the top-level JSON object +keys = list(original_json.keys()) + +# Generate JSON dictionaries, each missing a different key +json_variants = [] +for key in keys: + variant = original_json.copy() + del variant[key] + json_variants.append(variant) + +for i, variant in enumerate(json_variants): + try: + log.d("Testing set calibration config with a missing field: ", keys[i]) + # write the above calib config table to the device + ac_dev.set_calibration_config(json.dumps(variant)) + test.fail("Exception was expected while setting calibration config with a missing field: " + keys[i]) + except Exception as e: + test.check_equal(e, "Invalid JSON for calibration_config: must contain '" + keys[i] + "' field.") + +test.finish() + +############################################################################################# + +test.start("Trying to write bad calib config with a missing field from camera position section") + +keys = list(original_json["camera_position"].keys()) + +# Generate JSON dictionaries, each missing a different key +json_variants = [] +for key in keys: + variant = original_json.copy() + del variant["camera_position"][key] + json_variants.append(variant) + +for i, variant in enumerate(json_variants): + try: + log.d("Testing set calibration config with a missing field: ", keys[i]) + # write the above calib config table to the device + ac_dev.set_calibration_config(json.dumps(variant)) + test.fail("Exception was expected while setting calibration config with a missing field: " + keys[i]) + except Exception as e: + test.check_equal(e, "Invalid JSON for camera_position: must contain '" + keys[i] + "' field.") + +test.finish() + +############################################################################################# + +test.start("Trying to write bad calib config with a missing roi values, or wrong roi types") + +# Generate JSON dictionaries, each missing a different key + +variant = original_json.copy() +del variant["roi"][0] +try: + log.d("Testing set calibration config with a missing element in roi") + # write the above calib config table to the device + ac_dev.set_calibration_config(json.dumps(variant)) + test.fail("Exception was expected while setting calibration config with a missing field in roi") +except Exception as e: + test.check_equal(e, "Invalid JSON for roi: must be an array of 4 elements.") + +variant = original_json.copy() +del variant["roi"][0][0] +try: + log.d("Testing set calibration config with a missing element in roi") + # write the above calib config table to the device + ac_dev.set_calibration_config(json.dumps(variant)) + test.fail("Exception was expected while setting calibration config with a missing field in roi") +except Exception as e: + test.check_equal(e, "Invalid JSON for roi: each element must be an array of 2 elements.") + +variant = original_json.copy() +variant["roi"][0][0] = 1.2 +try: + log.d("Testing set calibration config with an invalid type in roi") + # write the above calib config table to the device + ac_dev.set_calibration_config(json.dumps(variant)) + test.fail("Exception was expected while setting calibration config with an invalid type in roi") +except Exception as e: + test.check_equal(e, "Invalid JSON for roi: each coordinate must be an unsigned integer.") + +test.finish() + +############################################################################################# + +test.start("Restore original calibration config table") + +# restore original calibration config table +ac_dev.set_calibration_config(original_calib_config) + +# read the current calib config table from the device +read_result = ac_dev.get_calibration_config() + +# verify the JSON objects are equal (comparing JSON object because +# the JSON string can have different order of inner fields +test.check_equal(json.loads(read_result), json.loads(original_calib_config)) + +test.finish() + +############################################################################################# + +test.print_results_and_exit() diff --git a/unit-tests/live/d500/test-get-set-calib-config-table-json.py b/unit-tests/live/d500/test-get-set-calib-config-table-json.py deleted file mode 100644 index 2b0b308b34d..00000000000 --- a/unit-tests/live/d500/test-get-set-calib-config-table-json.py +++ /dev/null @@ -1,74 +0,0 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2024 Intel Corporation. All Rights Reserved. - -# test:donotrun - -import pyrealsense2 as rs -from rspy import test, log -import json - -############################################################################################# -# Tests -############################################################################################# - -test.start("Verify auto calibrated device extension") -ctx = rs.context() -dev = ctx.query_devices()[0] -ac_dev = dev.as_auto_calibrated_device() -test.finish() - -############################################################################################# - -test.start("Writing calibration config, then reading and comparing calibration config JSONs") - -# save current calibration config in order to restore it at the end -previous_result = ac_dev.get_calibration_config() - -# calibration config JSON String to be written -calib_config_json_str = ''' -{ - "calibration_config": - { - "calib_roi_num_of_segments": 0, - "roi": [ [[0,0], [0,0], [0,0], [0,0]], [[0,0], [0,0], [0,0], [0,0]], [[0,0], [0,0], [0,0], [0,0]], [[0,0], [0,0], [0,0], [0,0]] ], - "camera_position": - { - "rotation": - [ - [ 0.0, 0.0, 1.0], - [-1.0, 0.0, 0.0], - [ 0.0, -1.0, 0.0] - ], - "translation": [0.0, 0.0, 0.27] - }, - "crypto_signature": [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] - } -} -''' - -# generate calibration config struct from JSON string -new_calib_config = ac_dev.json_string_to_calibration_config(calib_config_json_str) - -# write the above new_calib_config table to the device -ac_dev.set_calibration_config(new_calib_config) - -# read the current calib config table from the device -read_result = ac_dev.get_calibration_config() - -# verify the tables are equal -test.check_equal(read_result, new_calib_config) - -# verify the JSON objects are equal (comparing JSON object because -# the JSON string can have different order of inner fields -read_result_json_str = ac_dev.calibration_config_to_json_string(read_result) -new_calib_config_json_str = ac_dev.calibration_config_to_json_string(new_calib_config) -test.check_equal(json.loads(new_calib_config_json_str), json.loads(read_result_json_str)) - -# restore original calibration config table -ac_dev.set_calibration_config(previous_result) - -test.finish() - -############################################################################################# - -test.print_results_and_exit() diff --git a/unit-tests/live/d500/test-get-set-calib-config-table.py b/unit-tests/live/d500/test-get-set-calib-config-table.py deleted file mode 100644 index 956232fd9fc..00000000000 --- a/unit-tests/live/d500/test-get-set-calib-config-table.py +++ /dev/null @@ -1,97 +0,0 @@ -# License: Apache 2.0. See LICENSE file in root directory. -# Copyright(c) 2024 Intel Corporation. All Rights Reserved. - -# test:donotrun - -import pyrealsense2 as rs -from rspy import test, log - - -def generate_camera_position(): - # rotation matrix - rx = rs.float3(0.0, 0.0, 1.0) - ry = rs.float3(-1.0, 0.0, 0.0) - rz = rs.float3(0.0, -1.0, 0.0) - rotation = rs.float3x3(rx, ry, rz) - # translation vector [m] - translation = rs.float3(0.0, 0.0, 0.27) - return rs.extrinsics_table(rotation, translation) - - -def generate_calib_config_table(): - calib_roi = rs.calibration_roi(0, 0, 0, 0, 0, 0, 0, 0) - calib_config = rs.calibration_config() - calib_config.calib_roi_num_of_segments = 0 - calib_config.roi = [calib_roi, calib_roi, calib_roi, calib_roi] - calib_config.reserved1 = [3] * 12 - calib_config.camera_position = generate_camera_position() - calib_config.reserved2 = [0] * 300 - calib_config.crypto_signature = [0] * 32 - calib_config.reserved3 = [0] * 39 - return calib_config - - -def is_equal_roi(first, second): - for i in range(0, 4): - for j in range(0, 4): - for k in range(0, 2): - if first[i].mask_pixel[j][k] != second[i].mask_pixel[j][k]: - return False - return True - - -def is_float3_equal(first, second): - return first.x == second.x and first.y == second.y and first.z == second.z - - -def is_float3x3_equal(first, second): - return (is_float3_equal(first.x, second.x) and is_float3_equal(first.y, second.y) - and is_float3_equal(first.z, second.z)) - - -def is_equal_extrinsics_table(first, second): - if not is_float3x3_equal(first.rotation, second.rotation): - return False - if not is_float3_equal(first.translation, second.translation): - return False - return True - - -def is_equal_calib_configs(first, second): - if first.calib_roi_num_of_segments != second.calib_roi_num_of_segments: - print("calib_roi_num_of_segments is different") - return False - if not is_equal_roi(first.roi, second.roi): - print("roi is different") - return False - if first.reserved1 != second.reserved1: - print("reserved1 is different") - return False - if not is_equal_extrinsics_table(first.camera_position, second.camera_position): - print("camera_position is different") - return False - if first.reserved2 != second.reserved2: - print("calib_roi_num_of_segments is different") - return False - if first.crypto_signature != second.crypto_signature: - print("crypto_signature is different") - return False - if first.reserved3 != second.reserved3: - print("reserved3 is different") - return False - return True - - -dev = test.find_first_device_or_exit() -ac_dev = dev.as_auto_calibrated_device() - -############################################################################################# -with test.closure("Set / Get calib config table"): - generated_calib_config = generate_calib_config_table() - ac_dev.set_calibration_config(generated_calib_config) - current_calib_config = ac_dev.get_calibration_config() - test.check(is_equal_calib_configs(generated_calib_config, current_calib_config)) - -############################################################################################# - -test.print_results_and_exit() diff --git a/wrappers/python/pyrs_device.cpp b/wrappers/python/pyrs_device.cpp index 3505b47f7b5..cb11c6e8b8e 100644 --- a/wrappers/python/pyrs_device.cpp +++ b/wrappers/python/pyrs_device.cpp @@ -192,9 +192,7 @@ void init_device(py::module &m) { .def("set_calibration_table", &rs2::auto_calibrated_device::set_calibration_table, "Set current table to dynamic area.") .def("reset_to_factory_calibration", &rs2::auto_calibrated_device::reset_to_factory_calibration, "Reset device to factory calibration.") .def("get_calibration_config", &rs2::auto_calibrated_device::get_calibration_config, "Get Calibration Config Table", py::call_guard()) - .def("set_calibration_config", &rs2::auto_calibrated_device::set_calibration_config, "Set Calibration Config Table", "calibration_config"_a, py::call_guard()) - .def("calibration_config_to_json_string", &rs2::auto_calibrated_device::calibration_config_to_json_string, "Convert calibration config to JSON string", "calibration_config"_a, py::call_guard()) - .def("json_string_to_calibration_config", &rs2::auto_calibrated_device::json_string_to_calibration_config, "Convert JSON string to calibration config", "json_str"_a, py::call_guard()); + .def("set_calibration_config", &rs2::auto_calibrated_device::set_calibration_config, "Set Calibration Config Table", "application_config_json_str"_a, py::call_guard()); py::class_ device_calibration( m, "device_calibration" ); device_calibration.def( py::init(), "device"_a ) diff --git a/wrappers/python/pyrs_types.cpp b/wrappers/python/pyrs_types.cpp index ddb9f2654ef..e053dae7f21 100644 --- a/wrappers/python/pyrs_types.cpp +++ b/wrappers/python/pyrs_types.cpp @@ -64,44 +64,5 @@ void init_types(py::module &m) { .def_readwrite("max_x", &rs2::region_of_interest::max_x) .def_readwrite("max_y", &rs2::region_of_interest::max_y); - py::class_ calibration_roi(m, "calibration_roi"); // No docstring in C++ - calibration_roi.def(py::init<>()) - .def(py::init()) - .def_property(BIND_RAW_2D_ARRAY_PROPERTY(rs2_calibration_roi, mask_pixel, uint16_t, 4, 2), - "Array of four corners in Deph Frame Coordinate system that define a closed simple quadrangle"); - - py::class_ float3(m, "float3"); // No docstring in C++ - float3.def(py::init<>()) - .def(py::init()) - .def_readwrite("x", &float3_row_major::x, "x") - .def_readwrite("y", &float3_row_major::y, "y") - .def_readwrite("z", &float3_row_major::z, "z"); - - py::class_ float3x3(m, "float3x3"); // No docstring in C++ - float3x3.def(py::init<>()) - .def(py::init()) - .def_readwrite("x", &float3x3_row_major::x, "x") - .def_readwrite("y", &float3x3_row_major::y, "y") - .def_readwrite("z", &float3x3_row_major::z, "z"); - - py::class_ extrinsics_table(m, "extrinsics_table"); // No docstring in C++ - extrinsics_table.def(py::init<>()) - .def(py::init()) - .def_readwrite("rotation", &rs2_extrinsics_row_major::rotation, "Rotation Value") - .def_readwrite("translation", &rs2_extrinsics_row_major::translation, "Translation Value"); - - py::class_ calibration_config(m, "calibration_config"); // No docstring in C++ - calibration_config.def(py::init<>()) - .def_readwrite("calib_roi_num_of_segments", &rs2_calibration_config::calib_roi_num_of_segments, "calib_roi_num_of_segments") - .def_property(BIND_RAW_ARRAY_PROPERTY(rs2_calibration_config, roi, rs2_calibration_roi, 4), "roi") - .def_property(BIND_RAW_ARRAY_PROPERTY(rs2_calibration_config, reserved1, uint8_t, sizeof(rs2_calibration_config::reserved1)), "reserved1") - .def_readwrite("camera_position", &rs2_calibration_config::camera_position, "camera_position") - .def_property(BIND_RAW_ARRAY_PROPERTY(rs2_calibration_config, reserved2, uint8_t, sizeof(rs2_calibration_config::reserved2)), "reserved2") - .def_property(BIND_RAW_ARRAY_PROPERTY(rs2_calibration_config, crypto_signature, uint8_t, sizeof(rs2_calibration_config::crypto_signature)), "crypto_signature") - .def_property(BIND_RAW_ARRAY_PROPERTY(rs2_calibration_config, reserved3, uint8_t, sizeof(rs2_calibration_config::reserved3)), "reserved3") - .def("__eq__", [](const rs2_calibration_config& self, const rs2_calibration_config& other) { - return self == other; - }); - /** end rs_types.hpp **/ }