From 2d561a04f4fc75f000a3d05cf98fdedf8b29d381 Mon Sep 17 00:00:00 2001 From: Remi Bettan Date: Thu, 2 Jan 2025 10:18:02 +0200 Subject: [PATCH] cr and removing extrinsics restoring methods from 36i --- src/ds/d400/d400-color.h | 1 - src/ds/d400/d400-factory.cpp | 176 +---------------------------------- src/ds/d400/d400-private.h | 2 +- src/gl/camera-shader.cpp | 3 +- 4 files changed, 4 insertions(+), 178 deletions(-) diff --git a/src/ds/d400/d400-color.h b/src/ds/d400/d400-color.h index 634de348fe..67ed3904a7 100644 --- a/src/ds/d400/d400-color.h +++ b/src/ds/d400/d400-color.h @@ -53,7 +53,6 @@ namespace librealsense friend class d400_color_sensor; friend class rs435i_device; - friend class rs436i_device; friend class ds_color_common; uint8_t _color_device_idx = -1; diff --git a/src/ds/d400/d400-factory.cpp b/src/ds/d400/d400-factory.cpp index 643d521bbf..832f861287 100644 --- a/src/ds/d400/d400-factory.cpp +++ b/src/ds/d400/d400-factory.cpp @@ -969,10 +969,7 @@ namespace librealsense , firmware_logger_device( dev_info, d400_device::_hw_monitor, get_firmware_logs_command(), get_flash_logs_command()) { - check_and_restore_rgb_stream_extrinsic(); - if (_fw_version >= firmware_version(5, 16, 0, 0)) - register_feature( - std::make_shared< gyro_sensitivity_feature >(get_raw_motion_sensor(), get_motion_sensor())); + register_feature(std::make_shared< gyro_sensitivity_feature >(get_raw_motion_sensor(), get_motion_sensor())); } @@ -999,177 +996,6 @@ namespace librealsense return tags; }; bool compress_while_record() const override { return false; } - - private: - void check_and_restore_rgb_stream_extrinsic() - { - for (auto iter = 0, rec = 0; iter < 2; iter++, rec++) - { - std::vector< uint8_t > cal; - try - { - cal = *_color_calib_table_raw; - } - catch (...) - { - LOG_WARNING("Cannot read RGB calibration table"); - } - - if (!is_rgb_extrinsic_valid(cal) && !rec) - { - restore_rgb_extrinsic(); - } - else - break; - }; - } - - bool is_rgb_extrinsic_valid(const std::vector& raw_data) const - { - try - { - // verify extrinsic calibration table structure - auto table = ds::check_calib(raw_data); - - if ((table->header.version != 0 && table->header.version != 0xffff) && (table->header.table_size >= sizeof(ds::d400_rgb_calibration_table) - sizeof(ds::table_header))) - { - float3 trans_vector = table->translation_rect; - // Translation Heuristic tests - auto found = false; - for (auto i = 0; i < 3; i++) - { - //Nan/Infinity are not allowed - if (!std::isfinite(trans_vector[i])) - { - LOG_WARNING("RGB extrinsic - translation is corrupted: " << trans_vector); - return false; - } - // Translation must be assigned for at least one axis - if (std::fabs(trans_vector[i]) > std::numeric_limits::epsilon()) - found = true; - } - - if (!found) - { - LOG_WARNING("RGB extrinsic - invalid (zero) translation: " << trans_vector); - return false; - } - - // Rotation Heuristic tests - auto num_found = 0; - float3x3 rect_rot_mat = table->rotation_matrix_rect; - for (auto i = 0; i < 3; i++) - { - for (auto j = 0; j < 3; j++) - { - //Nan/Infinity are not allowed - if (!std::isfinite(rect_rot_mat(i, j))) - { - LOG_DEBUG("RGB extrinsic - rotation matrix corrupted:\n" << rect_rot_mat); - return false; - } - - if (std::fabs(rect_rot_mat(i, j)) > std::numeric_limits::epsilon()) - num_found++; - } - } - - bool res = (num_found >= 3); // At least three matrix indexes must be non-zero - if (!res) // At least three matrix indexes must be non-zero - LOG_DEBUG("RGB extrinsic - rotation matrix invalid:\n" << rect_rot_mat); - - return res; - } - else - { - LOG_WARNING("RGB extrinsic - header corrupted: " - << "Version: " << std::setfill('0') << std::setw(4) << std::hex << table->header.version - << ", type " << std::dec << table->header.table_type << ", size " << table->header.table_size); - return false; - } - } - catch (...) - { - return false; - } - } - - void assign_rgb_stream_extrinsic(const std::vector< uint8_t >& calib) - { - //write calibration to preset - command cmd(ds::fw_cmd::SETINTCALNEW, 0x20, 0x2); - cmd.data = calib; - d400_device::_hw_monitor->send(cmd); - } - - std::vector< uint8_t > read_sector(const uint32_t address, const uint16_t size) const - { - if (size > ds_advanced_mode_base::HW_MONITOR_COMMAND_SIZE) - throw std::runtime_error(rsutils::string::from() - << "Device memory read failed. max size: " - << int(ds_advanced_mode_base::HW_MONITOR_COMMAND_SIZE) - << ", requested: " << int(size)); - command cmd(ds::fw_cmd::FRB, address, size); - return d400_device::_hw_monitor->send(cmd); - } - - std::vector< uint8_t > read_rgb_gold() const - { - command cmd(ds::fw_cmd::LOADINTCAL, 0x20, 0x1); - return d400_device::_hw_monitor->send(cmd); - } - - std::vector< uint8_t > restore_calib_factory_settings() const - { - command cmd(ds::fw_cmd::CAL_RESTORE_DFLT); - return d400_device::_hw_monitor->send(cmd); - } - - void restore_rgb_extrinsic(void) - { - bool res = false; - LOG_WARNING("invalid RGB extrinsic was identified, recovery routine was invoked"); - try - { - if ((res = is_rgb_extrinsic_valid(read_rgb_gold()))) - { - restore_calib_factory_settings(); - } - else - { - if (_fw_version == firmware_version("5.11.6.200")) - { - const uint32_t gold_address = 0x17c49c; - const uint16_t bytes_to_read = 0x100; - auto alt_calib = read_sector(gold_address, bytes_to_read); - if ((res = is_rgb_extrinsic_valid(alt_calib))) - assign_rgb_stream_extrinsic(alt_calib); - else - res = false; - } - else - res = false; - } - - // Update device's internal state - if (res) - { - LOG_WARNING("RGB stream extrinsic successfully recovered"); - _color_calib_table_raw.reset(); - _color_extrinsic.get()->reset(); - environment::get_instance().get_extrinsics_graph().register_extrinsics(*_color_stream, *_depth_stream, _color_extrinsic); - } - else - { - LOG_ERROR("RGB Extrinsic recovery routine failed"); - _color_extrinsic.get()->reset(); - } - } - catch (...) - { - LOG_ERROR("RGB Extrinsic recovery routine failed"); - } - } }; class rs400_imu_device : public d400_motion, diff --git a/src/ds/d400/d400-private.h b/src/ds/d400/d400-private.h index dddff6ca6f..4afea9034a 100644 --- a/src/ds/d400/d400-private.h +++ b/src/ds/d400/d400-private.h @@ -161,7 +161,7 @@ namespace librealsense {RS435_RGB_PID, "5.8.15.0" }, {RS405U_PID, "5.8.15.0" }, {RS435I_PID, "5.12.7.100" }, - {RS436I_PID, "5.16.3.100" }, + {RS436I_PID, "5.16.3.100" }, // TODO - update final required FW version {RS416_PID, "5.8.15.0" }, {RS430I_PID, "5.8.15.0" }, {RS416_RGB_PID, "5.8.15.0" }, diff --git a/src/gl/camera-shader.cpp b/src/gl/camera-shader.cpp index 8f8a072540..c32b67f29d 100644 --- a/src/gl/camera-shader.cpp +++ b/src/gl/camera-shader.cpp @@ -164,7 +164,8 @@ namespace librealsense { auto dev_name = dev.get_info(RS2_CAMERA_INFO_NAME); if (starts_with(dev_name, "Intel RealSense D415")) index = 0; - if (starts_with(dev_name, "Intel RealSense D435")) index = 1; + if (starts_with(dev_name, "Intel RealSense D435") || + starts_with(dev_name, "Intel RealSense D436")) index = 1; if (starts_with(dev_name, "Intel RealSense D45")) index = 2; };