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

L515 IMU Calibration and Motion Correction #6587

Merged
merged 8 commits into from
Jun 17, 2020
Merged

L515 IMU Calibration and Motion Correction #6587

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
fdffc43
L515 motion correction support and general improvement in keeping laz…
gwen2018 Jun 14, 2020
8c3933f
add fw requirement to calibration script, extra extrinsic handling fo…
gwen2018 Jun 14, 2020
c606979
correct fw version requirement comment typo
gwen2018 Jun 14, 2020
d480e93
align calibration instruction for D400 and L515 devices
gwen2018 Jun 14, 2020
e5d4e16
improvement with pid platform identification, update D455 extrinsic, …
gwen2018 Jun 15, 2020
a403abc
additional review feedback improvement - extrinsic handling
gwen2018 Jun 16, 2020
c810f66
update extrinsic value direction according to the coordinate system
gwen2018 Jun 16, 2020
474f1f8
add more comments to unsupported devices
gwen2018 Jun 16, 2020
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
67 changes: 47 additions & 20 deletions src/ds5/ds5-motion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,8 @@
#include "proc/motion-transform.h"
#include "proc/auto-exposure-processor.h"

#include "../l500/l500-private.h"

namespace librealsense
{
const std::map<uint32_t, rs2_format> fisheye_fourcc_to_rs2_format = {
Expand Down Expand Up @@ -220,13 +222,12 @@ namespace librealsense
// Motion intrinsic calibration presents is a prerequisite for motion correction.
try
{
// Writing to log to dereference underlying structure
LOG_INFO("Accel Sensitivity:" << (**_accel_intrinsic).sensitivity);
LOG_INFO("Gyro Sensitivity:" << (**_gyro_intrinsic).sensitivity);

mm_correct_opt = std::make_shared<enable_motion_correction>(hid_ep.get(),
option_range{ 0, 1, 1, 1 });
hid_ep->register_option(RS2_OPTION_ENABLE_MOTION_CORRECTION, mm_correct_opt);
if (_mm_calib)
{
mm_correct_opt = std::make_shared<enable_motion_correction>(hid_ep.get(),
option_range{ 0, 1, 1, 1 });
hid_ep->register_option(RS2_OPTION_ENABLE_MOTION_CORRECTION, mm_correct_opt);
}
}
catch (...) {}

Expand All @@ -242,10 +243,8 @@ namespace librealsense
[&, mm_correct_opt]() { return std::make_shared<gyroscope_transform>(_mm_calib, mm_correct_opt);
});

uint16_t pid = static_cast<uint16_t>(strtoul(all_hid_infos.front().pid.data(), nullptr, 16));

if ((camera_fw_version >= firmware_version(custom_sensor_fw_ver)) &&
(!val_in_range(pid, { ds::RS400_IMU_PID, ds::RS435I_PID, ds::RS430I_PID, ds::RS465_PID, ds::RS405_PID, ds::RS455_PID })))
(!val_in_range(_pid, { ds::RS400_IMU_PID, ds::RS435I_PID, ds::RS430I_PID, ds::RS465_PID, ds::RS405_PID, ds::RS455_PID })))
{
hid_ep->register_option(RS2_OPTION_MOTION_MODULE_TEMPERATURE,
std::make_shared<motion_module_temperature_option>(*raw_hid_ep));
Expand Down Expand Up @@ -320,12 +319,22 @@ namespace librealsense
{
using namespace ds;

_mm_calib = std::make_shared<mm_calib_handler>(_hw_monitor,_device_capabilities);
std::vector<platform::hid_device_info> hid_infos = group.hid_devices;

if (!hid_infos.empty())
{
// product id
_pid = static_cast<uint16_t>(strtoul(hid_infos.front().pid.data(), nullptr, 16));

// motion correction
_mm_calib = std::make_shared<mm_calib_handler>(_hw_monitor, _pid);

_accel_intrinsic = std::make_shared<lazy<ds::imu_intrinsic>>([this]() { return _mm_calib->get_intrinsic(RS2_STREAM_ACCEL); });
_gyro_intrinsic = std::make_shared<lazy<ds::imu_intrinsic>>([this]() { return _mm_calib->get_intrinsic(RS2_STREAM_GYRO); });
// D435i to use predefined values extrinsics
_depth_to_imu = std::make_shared<lazy<rs2_extrinsics>>([this]() { return _mm_calib->get_extrinsic(RS2_STREAM_ACCEL); });
_accel_intrinsic = std::make_shared<lazy<ds::imu_intrinsic>>([this]() { return _mm_calib->get_intrinsic(RS2_STREAM_ACCEL); });
_gyro_intrinsic = std::make_shared<lazy<ds::imu_intrinsic>>([this]() { return _mm_calib->get_intrinsic(RS2_STREAM_GYRO); });

// use predefined extrinsics
_depth_to_imu = std::make_shared<lazy<rs2_extrinsics>>([this]() { return _mm_calib->get_extrinsic(RS2_STREAM_ACCEL); });
}

initialize_fisheye_sensor(ctx,group);

Expand Down Expand Up @@ -441,17 +450,24 @@ namespace librealsense
_fisheye_device_idx = add_sensor(fisheye_ep);
}

mm_calib_handler::mm_calib_handler(std::shared_ptr<hw_monitor> hw_monitor, ds::d400_caps dev_cap) :
_hw_monitor(hw_monitor), _dev_cap(dev_cap)
mm_calib_handler::mm_calib_handler(std::shared_ptr<hw_monitor> hw_monitor, uint16_t pid) :
_hw_monitor(hw_monitor), _pid(pid)
{
_imu_eeprom_raw = [this]() { return get_imu_eeprom_raw(); };
_imu_eeprom_raw = [this]() {
if (_pid == L515_PID)
return get_imu_eeprom_raw_l515();
else
return get_imu_eeprom_raw();
};

_calib_parser = [this]() {

std::vector<uint8_t> raw(ds::tm1_eeprom_size);
uint16_t calib_id = ds::dm_v2_eeprom_id; //assume DM V2 IMU as default platform
bool valid = false;

if (_pid == L515_PID) calib_id = ds::l500_eeprom_id;

try
{
raw = *_imu_eeprom_raw;
Expand All @@ -460,16 +476,19 @@ namespace librealsense
}
catch(const std::exception&)
{
LOG_WARNING("IMU Calibration is not available, see the previous message");
// in case calibration table errors (invalid table, empty table, or corrupted table), data is invalid and default intrinsic and extrinsic will be used
LOG_WARNING("IMU Calibration is not available, default intrinsic and extrinsic will be used.");
}

std::shared_ptr<mm_calib_parser> prs = nullptr;
switch (calib_id)
{
case ds::dm_v2_eeprom_id: // DM V2 id
prs = std::make_shared<dm_v2_imu_calib_parser>(raw, _dev_cap, valid); break;
prs = std::make_shared<dm_v2_imu_calib_parser>(raw, _pid, valid); break;
case ds::tm1_eeprom_id: // TM1 id
prs = std::make_shared<tm1_imu_calib_parser>(raw); break;
case ds::l500_eeprom_id: // L515
prs = std::make_shared<l500_imu_calib_parser>(raw, valid); break;
default:
throw recoverable_exception(to_string() << "Motion Intrinsics unresolved - "
<< ((valid)? "device is not calibrated" : "invalid calib type "),
Expand All @@ -487,6 +506,14 @@ namespace librealsense
return _hw_monitor->send(cmd);
}

std::vector<uint8_t> mm_calib_handler::get_imu_eeprom_raw_l515() const
{
// read imu calibration table on L515
// READ_TABLE 0x243 0
command cmd(ivcam2::READ_TABLE, ivcam2::L515_IMU_TABLE, 0);
return _hw_monitor->send(cmd);
}

ds::imu_intrinsic mm_calib_handler::get_intrinsic(rs2_stream stream)
{
return (*_calib_parser)->get_intrinsic(stream);
Expand Down
Loading