Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

change get/set calibration config API to work with JSONs directly #13168

Merged
merged 2 commits into from
Jul 21, 2024
Merged

change get/set calibration config API to work with JSONs directly #13168

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
ef06acb
change get/set calibration config API to work with JSONs directly
SamerKhshiboun Jul 18, 2024
3f7938b
fix typos and add comment to test
SamerKhshiboun Jul 21, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
35 changes: 8 additions & 27 deletions include/librealsense2/h/rs_device.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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");
}
Expand Down
7 changes: 2 additions & 5 deletions src/ds/d400/d400-auto-calibration.h
View file
Open in desktop
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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
#include "d500-private.h"
#include <rsutils/string/from.h>
#include <rsutils/json.h>

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

namespace librealsense
{
Expand Down Expand Up @@ -301,141 +301,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 (safety_interface_config entry + header)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

safety_interface_config?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fixed.

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
Loading