Skip to content

Commit

Permalink
change get/set calibration config API to work with JSONs directly
Browse files Browse the repository at this point in the history
  • Loading branch information
SamerKhshiboun committed Jul 18, 2024
1 parent da82fd0 commit abf876a
Show file tree
Hide file tree
Showing 19 changed files with 789 additions and 473 deletions.
35 changes: 8 additions & 27 deletions include/librealsense2/h/rs_device.h
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
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
5 changes: 0 additions & 5 deletions include/librealsense2/hpp/rs_types.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -211,9 +211,4 @@ namespace rs2
inline std::ostream & operator << (std::ostream & o, rs2_vector v) { return o << v.x << ", " << v.y << ", " << v.z; }
inline std::ostream & operator << (std::ostream & o, rs2_quaternion q) { return o << q.x << ", " << q.y << ", " << q.z << ", " << q.w; }

inline bool operator==(rs2_calibration_config const& self, rs2_calibration_config const& other)
{
return !std::memcmp(&self, &other, sizeof(rs2_calibration_config));
}

#endif // LIBREALSENSE_RS2_TYPES_HPP
6 changes: 2 additions & 4 deletions src/auto-calibrated-device.h
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
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
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
Loading

0 comments on commit abf876a

Please sign in to comment.