Skip to content

Commit

Permalink
cr and removing extrinsics restoring methods from 36i
Browse files Browse the repository at this point in the history
  • Loading branch information
@remibettan
remibettan committed Jan 2, 2025
1 parent 8700357 commit 2d561a0
Show file tree
Hide file tree
Showing 4 changed files with 4 additions and 178 deletions.
1 change: 0 additions & 1 deletion src/ds/d400/d400-color.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
176 changes: 1 addition & 175 deletions src/ds/d400/d400-factory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()));
}


Expand All @@ -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<uint8_t>& raw_data) const
{
try
{
// verify extrinsic calibration table structure
auto table = ds::check_calib<ds::d400_rgb_calibration_table>(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<float>::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<float>::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,
Expand Down
2 changes: 1 addition & 1 deletion src/ds/d400/d400-private.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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" },
Expand Down
3 changes: 2 additions & 1 deletion src/gl/camera-shader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
};

Expand Down

0 comments on commit 2d561a0

Please sign in to comment.