Skip to content

Commit

Permalink
Merge pull request #4160 from ev-mp/d435i_rgb_table
Browse files Browse the repository at this point in the history 
Updated RGB Extrinsic recovery procedure
  • Loading branch information
@ev-mp
ev-mp authored Jun 6, 2019
2 parents 4e3c214 + cf3a775 commit 3d451f0
Show file tree
Hide file tree
Showing 13 changed files with 158 additions and 106 deletions.
6 changes: 3 additions & 3 deletions src/core/advanced_mode.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,6 +152,9 @@ namespace librealsense
std::vector<uint8_t> serialize_json() const override;
void load_json(const std::string& json_content) override;

static const uint16_t HW_MONITOR_COMMAND_SIZE = 1000;
static const uint16_t HW_MONITOR_BUFFER_SIZE = 1024;

private:
void set_exposure(uvc_sensor& sensor, const exposure_control& val);
void set_auto_exposure(uvc_sensor& sensor, const auto_exposure_control& val);
Expand Down Expand Up @@ -208,9 +211,6 @@ namespace librealsense
bool _rgb_exposure_gain_bind;
bool _amplitude_factor_support;

static const uint16_t HW_MONITOR_COMMAND_SIZE = 1000;
static const uint16_t HW_MONITOR_BUFFER_SIZE = 1024;

preset get_all() const;
void set_all(const preset& p);

Expand Down
179 changes: 109 additions & 70 deletions src/ds5/ds5-factory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -508,7 +508,7 @@ namespace librealsense
ds5_motion(ctx, group),
ds5_advanced_mode_base(ds5_device::_hw_monitor, get_depth_sensor())
{
validate_and_restore_color_stream_extrinsic();
check_and_restore_rgb_stream_extrinsic();
}

std::shared_ptr<matcher> create_matcher(const frame_holder& frame) const override;
Expand All @@ -534,9 +534,9 @@ namespace librealsense
bool compress_while_record() const override { return false; }

private:
void validate_and_restore_color_stream_extrinsic()
void check_and_restore_rgb_stream_extrinsic()
{
try
for(auto iter = 0, rec =0; iter < 2; iter++, rec++)
{
std::vector<byte> cal;
try
Expand All @@ -548,119 +548,158 @@ namespace librealsense
LOG_WARNING("Cannot read RGB calibration table");
}

if (!validate_color_stream_extrinsic(cal))
restore_color_stream_extrinsic();
}
catch (...)
{
LOG_WARNING("Validation color stream extrinsic failed");
}
if (!is_rgb_extrinsic_valid(cal) && !rec)
{
restore_rgb_extrinsic();
}
else
break;
};
}

bool validate_color_stream_extrinsic(const std::vector<uint8_t>& raw_data)
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::rgb_calibration_table>(raw_data);
float3 trans_vector = table->translation_rect;
float3x3 rect_rot_mat = table->rotation_matrix_rect;

// check that data rotation and translation are not zero
for (auto i = 0; i < 3; i++)
if ( (table->header.version != 0 && table->header.version != 0xffff) && (table->header.table_size >= sizeof(ds::rgb_calibration_table) - sizeof(ds::table_header)))
{
if (std::fabs(trans_vector[i]) > std::numeric_limits<float>::epsilon())
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; 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())
return true;
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;
}
return false;
}
catch (...)
{
return false;
}
}

void restore_color_stream_extrinsic(const std::vector<byte>& calib)
void assign_rgb_stream_extrinsic(const std::vector<byte>& calib)
{
//write the calibration to its correct address
//write calibration to preset
command cmd(ds::fw_cmd::SETINTCALNEW, 0x20, 0x2);
cmd.data = calib;
ds5_device::_hw_monitor->send(cmd);

_color_calib_table_raw = [this]() { return get_raw_calibration_table(ds::rgb_calibration_id); };
_color_extrinsic = std::make_shared<lazy<rs2_extrinsics>>([this]() { return from_pose(ds::get_color_stream_extrinsic(*_color_calib_table_raw)); });
environment::get_instance().get_extrinsics_graph().register_extrinsics(*_color_stream, *_depth_stream, _color_extrinsic);
}

bool restore_from_address(const uint32_t address)
std::vector<byte> read_sector(const uint32_t address, const uint16_t size) const
{
const uint32_t bytes_to_read = 0x100;

//read the calibration from the address
command cmd(ds::fw_cmd::FRB, address, bytes_to_read);
auto calib = ds5_device::_hw_monitor->send(cmd);
if (validate_color_stream_extrinsic(calib))
{
restore_color_stream_extrinsic(calib);
return true;
}
LOG_WARNING("Restoring RGB Extrinsic from address" << address << " failed");
return false;
if (size > ds5_advanced_mode_base::HW_MONITOR_COMMAND_SIZE)
throw std::runtime_error(to_string() << "Device memory read failed. max size: "
<< int(ds5_advanced_mode_base::HW_MONITOR_COMMAND_SIZE)
<< ", requested: " << int(size));
command cmd(ds::fw_cmd::FRB, address, size);
return ds5_device::_hw_monitor->send(cmd);
}

bool restore_color_stream_extrinsic_from_gold_sector()
std::vector<byte> read_rgb_gold() const
{
command cmd(ds::fw_cmd::LOADINTCAL, 0x20, 0x1);
auto calib = ds5_device::_hw_monitor->send(cmd);
if (validate_color_stream_extrinsic(calib))
{
restore_color_stream_extrinsic(calib);
return true;
}
LOG_WARNING("Restore from gold_sector failed");
return false;
return ds5_device::_hw_monitor->send(cmd);
}

bool restore_color_stream_extrinsic()
std::vector<byte> restore_calib_factory_settings() const
{
command cmd(ds::fw_cmd::CAL_RESTORE_DFLT);
return ds5_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
{
LOG_WARNING("invalid RGB extrinsic was identified, recovery routine was invoked");

const uint32_t gold_address = 0x17c49c;
const uint32_t dynamic_address = 0x17b49c;

if (!restore_color_stream_extrinsic_from_gold_sector())
if (res = is_rgb_extrinsic_valid(read_rgb_gold()))
{
LOG_WARNING("RGB extrinsic recovery - Gold calibration is invalid, restore from an alternative sector");

if (!restore_from_address(gold_address))
restore_calib_factory_settings();
}
else
{
if (_fw_version == firmware_version("5.11.6.200"))
{
LOG_WARNING("RGB extrinsic recovery - Gold calibration from address " << gold_address << " is invalid, restore from an alternative sector");

if (!restore_from_address(dynamic_address))
{
LOG_WARNING("RGB extrinsic recovery - Dynamic calibration from address " << dynamic_address << " is invalid, recovery routine failed");

_color_extrinsic.reset();
return false;
}
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;
}

LOG_DEBUG("Suceeded to restore color stream extrinsic");
return true;
// 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_WARNING("RGB Extrinsic recovery routine failed");
return false;
LOG_ERROR("RGB Extrinsic recovery routine failed");
}
}
};
Expand Down
4 changes: 2 additions & 2 deletions src/ds5/ds5-motion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -296,7 +296,7 @@ namespace librealsense
};
}
catch (const std::exception& ex){
LOG_INFO("Motion Module extrinsic calibration is not available, report: " << ex.what());
LOG_INFO("Motion Module - no extrinsic calibration, " << ex.what());
}

try
Expand All @@ -311,7 +311,7 @@ namespace librealsense
}
catch (const std::exception& ex)
{
LOG_INFO("Motion Module intrinsic calibration is not available, report: " << ex.what());
LOG_INFO("Motion Module - no intrinsic calibration, " << ex.what());

// transform IMU axes if supported
hid_ep->register_on_before_frame_callback(align_imu_axes);
Expand Down
3 changes: 2 additions & 1 deletion src/ds5/ds5-private.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -148,6 +148,7 @@ namespace librealsense
GETAEROI = 0x45, // get auto-exposure region of interest
MMER = 0x4F, // MM EEPROM read ( from DS5 cache )
GET_EXTRINSICS = 0x53, // get extrinsics
CAL_RESTORE_DFLT= 0x61, // Reset Depth/RGB calibration to factory settings
SETINTCALNEW = 0x62, // Set Internal sub calibration table
SET_CAM_SYNC = 0x69, // set Inter-cam HW sync mode [0-default, 1-master, 2-slave]
GET_CAM_SYNC = 0x6A, // fet Inter-cam HW sync mode
Expand Down Expand Up @@ -516,7 +517,7 @@ namespace librealsense
float3x3 intrinsic_matrix_rect; // RGB intrinsic matrix after rectification
float3x3 rotation_matrix_rect; // Rotation matrix for rectification of RGB
float3 translation_rect; // Translation vector for rectification
float reserved[24];
uint8_t reserved[24];
};


Expand Down
Loading

0 comments on commit 3d451f0

Please sign in to comment.