D500 calib config to dev

include/librealsense2/h/rs_device.h

@@ -598,6 +598,23 @@ 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);
+
 #ifdef __cplusplus
 }
 #endif

include/librealsense2/h/rs_types.h

@@ -13,6 +13,8 @@
 extern "C" {
 #endif
 
+#include <stdint.h>
+
 /** \brief Category of the librealsense notification. */
 typedef enum rs2_notification_category{
     RS2_NOTIFICATION_CATEGORY_FRAMES_TIMEOUT,               /**< Frames didn't arrived within 5 seconds */
@@ -278,6 +280,54 @@ const char* rs2_get_failed_args                (const rs2_error* error);
 const char* rs2_get_error_message              (const rs2_error* error);
 void        rs2_free_error                     (rs2_error* error);
 
+#pragma pack(push, 1)
+/* rs2_calibration_roi - Array of four corners in Deph Frame Coordinate system that define a closed simple quadrangle (non-intersecting)*/
+typedef struct rs2_calibration_roi
+{
+    uint16_t mask_pixel[4][2];
+}rs2_calibration_roi;
+
+typedef struct float3_row_major { float x, y, z; } float3_row_major;
+typedef struct float3x3_row_major { float3_row_major x, y, z; } float3x3_row_major;
+
+typedef struct rs2_extrinsics_row_major
+{
+    float3x3_row_major rotation; // Rotation matrix
+    float3_row_major translation; // Metric units
+} rs2_extrinsics_row_major;
+
+typedef struct rs2_calibration_config
+{
+    uint8_t 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
+    rs2_calibration_roi roi[4];        // 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
+                                       // Segments 1-3 - structured identical to segment_#0 (reserved)
+                                       // The ROI segments can intersect, but each must be convex(angles <= 180 degrees).
+    uint8_t reserved1[12];
+    rs2_extrinsics_row_major camera_position;
+    uint8_t reserved2[300];
+    uint8_t crypto_signature[32];
+    uint8_t reserved3[39];
+} rs2_calibration_config;
+
+typedef struct rs2_calibration_config_header
+{
+    uint16_t version;       // major.minor. Big-endian
+    uint16_t table_type;    // type
+    uint32_t table_size;    // full size including: header footer
+    uint32_t calib_version; // major.minor.index
+    uint32_t crc32;         // crc of all the data in table excluding this header/CRC
+} rs2_calibration_config_header;
+
+typedef struct rs2_calibration_config_with_header
+{
+    rs2_calibration_config_header header;
+    rs2_calibration_config payload;
+} rs2_calibration_config_with_header;
+#pragma pack(pop)
+
 #ifdef __cplusplus
 }
 #endif

include/librealsense2/hpp/rs_device.hpp

@@ -867,6 +867,33 @@ 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
+        {
+            rs2_error* e = nullptr;
+            rs2_calibration_config calib_config;
+            rs2_get_calibration_config(_dev.get(), &calib_config, &e);
+            error::handle(e);
+            return calib_config;
+        }
+
+        /**
+        * 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)
+        {
+            rs2_error* e = nullptr;
+            rs2_set_calibration_config(_dev.get(), &calib_config, &e);
+            error::handle(e);
+        }
     };
 
     /*

src/auto-calibrated-device.h

@@ -25,6 +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;
     };
     MAP_EXTENSION(RS2_EXTENSION_AUTO_CALIBRATED_DEVICE, auto_calibrated_interface);
 }

src/ds/d400/d400-auto-calibration.cpp

@@ -2567,6 +2567,16 @@ namespace librealsense
             throw std::runtime_error("Failed to extract target dimension info!");
     }
 
+    rs2_calibration_config 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");
+    }
+
     void auto_calibrated::set_hw_monitor_for_auto_calib(std::shared_ptr<hw_monitor> hwm)
     {
         _hw_monitor = hwm;

src/ds/d400/d400-auto-calibration.h

@@ -60,6 +60,9 @@ 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;
+
         void set_hw_monitor_for_auto_calib(std::shared_ptr<hw_monitor> hwm);
 
     private:

src/ds/d500/d500-auto-calibration.cpp

@@ -300,6 +300,61 @@ 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;
+
+        // prepare command
+        using namespace ds;
+        command cmd(GET_HKR_CONFIG_TABLE,
+            static_cast<int>(d500_calib_location::d500_calib_flash_memory),
+            static_cast<int>(d500_calibration_table_id::calib_cfg_id),
+            static_cast<int>(d500_calib_type::d500_calib_dynamic));
+        auto res = _hw_monitor->send(cmd);
+
+        if (res.size() < sizeof(rs2_calibration_config_with_header))
+        {
+            throw io_exception(rsutils::string::from() << "Calibration config reading failed");
+        }
+        calib_config_with_header = reinterpret_cast<rs2_calibration_config_with_header*>(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)
+        {
+            throw invalid_value_exception(rsutils::string::from() << "Invalid CRC value for calibration config table");
+        }
+
+        return calib_config_with_header->payload;
+    }
+
+    void d500_auto_calibrated::set_calibration_config(const rs2_calibration_config& calib_config)
+    {
+        // calculate CRC
+        uint32_t computed_crc32 = rsutils::number::calc_crc32(reinterpret_cast<const uint8_t*>(&calib_config), 
+            sizeof(rs2_calibration_config));
+
+        // prepare vector of data to be sent (header + sp)
+        rs2_calibration_config_with_header calib_config_with_header;
+        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<uint16_t>(ds::d500_calibration_table_id::calib_cfg_id),
+            sizeof(rs2_calibration_config), calib_version, computed_crc32 };
+        calib_config_with_header.payload = calib_config;
+        auto data_as_ptr = reinterpret_cast<const uint8_t*>(&calib_config_with_header);
+
+        // prepare command
+        command cmd(ds::SET_HKR_CONFIG_TABLE,
+            static_cast<int>(ds::d500_calib_location::d500_calib_flash_memory),
+            static_cast<int>(ds::d500_calibration_table_id::calib_cfg_id),
+            static_cast<int>(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.require_response = false;
+
+        // send command 
+        _hw_monitor->send(cmd);
+    }
+
     void d500_auto_calibrated::set_hw_monitor_for_auto_calib(std::shared_ptr<hw_monitor> hwm)
     {
         _hw_monitor = hwm;

src/ds/d500/d500-auto-calibration.h

@@ -26,6 +26,10 @@ 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;
+
+
         void set_hw_monitor_for_auto_calib(std::shared_ptr<hw_monitor> hwm);
 
         enum class d500_calibration_state

src/ds/d500/d500-private.h

@@ -82,6 +82,7 @@ namespace librealsense
             rgb_calibration_id = 0xb9,
             rgb_lut_id = 0xba,
             imu_calibration_id = 0xbb,
+            calib_cfg_id = 0xc0dd,
             max_id = -1
         };
 

src/realsense.def

@@ -435,3 +435,5 @@ EXPORTS
     rs2_transform_point_to_point
     rs2_fov
     rs2_project_color_pixel_to_depth_pixel
+    rs2_get_calibration_config
+    rs2_set_calibration_config

src/rs.cpp

@@ -4380,3 +4380,23 @@ 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)
+

unit-tests/live/d500/test-get-set-calib-config-table.py

@@ -0,0 +1,97 @@
+# 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()

wrappers/python/pyrs_device.cpp

@@ -190,7 +190,9 @@ void init_device(py::module &m) {
             "queue1"_a, "queue2"_a, "queue3"_a, "target_width_mm"_a, "target_height_mm"_a, "callback"_a, py::call_guard<py::gil_scoped_release>())
         .def("get_calibration_table", &rs2::auto_calibrated_device::get_calibration_table, "Read current calibration table from flash.")
         .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("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<py::gil_scoped_release>())
+        .def("set_calibration_config", &rs2::auto_calibrated_device::set_calibration_config, "Set Calibration Config Table", "calibration_config"_a, py::call_guard<py::gil_scoped_release>());
 
     py::class_<rs2::device_calibration, rs2::device> device_calibration( m, "device_calibration" );
     device_calibration.def( py::init<rs2::device>(), "device"_a )