Skip to content

Commit

Permalink
PR IntelRealSense#13168 from SamerKhshiboun: change get/set calibrati…
Browse files Browse the repository at this point in the history 
…on config API to work with JSONs directly
  • Loading branch information
@SamerKhshiboun
SamerKhshiboun authored Jul 21, 2024
2 parents 1f70737 + 3f7938b commit 42e7adf
Show file tree
Hide file tree
Showing 16 changed files with 736 additions and 467 deletions.
35 changes: 8 additions & 27 deletions include/librealsense2/h/rs_device.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
Expand Down
79 changes: 19 additions & 60 deletions include/librealsense2/hpp/rs_device.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<uint8_t> result;

rs2_error* e = nullptr;
auto buffer = rs2_calibration_config_to_json_string(_dev.get(), &calib_config, &e);

std::shared_ptr<const rs2_raw_data_buffer> 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.
Expand Down Expand Up @@ -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<uint8_t> 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<const rs2_raw_data_buffer> 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);
}
};
Expand Down
6 changes: 2 additions & 4 deletions src/auto-calibrated-device.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
14 changes: 2 additions & 12 deletions src/ds/d400/d400-auto-calibration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2407,22 +2407,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");
}
Expand Down
7 changes: 2 additions & 5 deletions src/ds/d400/d400-auto-calibration.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<hw_monitor> hwm);

private:
Expand Down
138 changes: 31 additions & 107 deletions src/ds/d500/d500-auto-calibration.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@

#include <rsutils/string/from.h>
#include <rsutils/json.h>

#include "d500-types/calibration-config.h"

namespace librealsense
{
Expand Down Expand Up @@ -401,141 +401,65 @@ 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
std::string d500_auto_calibrated::get_calibration_config() const
{
rs2_calibration_config_with_header* calib_config_with_header;
calibration_config_with_header* result;

// 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))
command cmd(ds::GET_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.require_response = true;

// send command to device and get response (calibration 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<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)
// check CRC before returning result
auto computed_crc32 = rsutils::number::calc_crc32(response.data() + sizeof(table_header),
sizeof(calibration_config));
result = reinterpret_cast<calibration_config_with_header*>(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<const uint8_t*>(&calib_config),
sizeof(rs2_calibration_config));
uint32_t computed_crc32 = rsutils::number::calc_crc32(reinterpret_cast<const uint8_t*>(&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<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;
table_header header(version, static_cast<uint16_t>(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<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.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<uint8_t> 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<uint8_t> crypto_signature_vector = json_data["calibration_config"]["crypto_signature"].get<std::vector<uint8_t>>();
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<hw_monitor> hwm)
{
_hw_monitor = hwm;
Expand Down
Loading

0 comments on commit 42e7adf

Please sign in to comment.