L515 IMU Calibration and Motion Correction #6587
Conversation
…y design / use default intrinsic in case valid calibration data is not available
…r custom calibration
src/ds5/ds5-motion.cpp
Outdated
| @@ -320,7 +316,7 @@ namespace librealsense | |||
| { | |||
| using namespace ds; | |||
|
|
|||
| _mm_calib = std::make_shared<mm_calib_handler>(_hw_monitor,_device_capabilities); | |||
| _mm_calib = std::make_shared<mm_calib_handler>(_hw_monitor, false, _device_capabilities); | |||
There was a problem hiding this comment.
Please replace the boolean with PID type, either raw or enumerated. This is required to assign the proper extrinsic based on CAD
src/ds5/ds5-motion.cpp
Outdated
| 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, bool l515_imu_cal, ds::d400_caps dev_cap) : | ||
| _hw_monitor(hw_monitor), _l515_table_on_flash(l515_imu_cal), _dev_cap(dev_cap) |
There was a problem hiding this comment.
_l515_table_on_flash to be refactored into _sku/dev_pid type
src/ds5/ds5-motion.cpp
Outdated
| @@ -470,6 +474,8 @@ namespace librealsense | |||
| prs = std::make_shared<dm_v2_imu_calib_parser>(raw, _dev_cap, valid); break; | |||
There was a problem hiding this comment.
The PID will be then passed here
src/ds5/ds5-motion.h
Outdated
| // valid extrinsic, the extrinsic_valid field should be set in the table and detected here so the values from the table can be used. | ||
| if (valid) | ||
| { | ||
| imu_calib_table = *(ds::check_calib<ds::dm_v2_calibration_table>(raw_data)); |
There was a problem hiding this comment.
This line may throw - needs to be protected
src/ds5/ds5-motion.h
Outdated
| if (_valid_extrinsic) | ||
| { | ||
| // only in case valid extrinsic is available in calibration data by calibration script in future or user custom calibration | ||
| librealsense::copy(&extr, &imu_calib_table.depth_to_imu, sizeof(rs2_extrinsics)); |
There was a problem hiding this comment.
The parsed data should be handled with lazy<> so it will be copied once
src/ds5/ds5-motion.h
Outdated
| @@ -151,7 +163,7 @@ namespace librealsense | |||
| throw std::runtime_error(to_string() << "Depth Module V2 does not support extrinsic for : " << rs2_stream_to_string(stream) << " !"); | |||
|
|
|||
| rs2_extrinsics extr; | |||
| if (1 == _calib_table.module_info.dm_v2_calib_table.extrinsic_valid) | |||
| if (_valid_extrinsic) | |||
| { | |||
| // The extrinsic is stored as array of floats / little-endian | |||
| librealsense::copy(&extr, &_calib_table.module_info.dm_v2_calib_table.depth_to_imu, sizeof(rs2_extrinsics)); | |||
There was a problem hiding this comment.
Handle the extrinsic with lazy to avoid copying the data
src/ds5/ds5-private.h
Outdated
| @@ -190,6 +190,7 @@ namespace librealsense | |||
| EN_ADV = 0x2D, // enable advanced mode | |||
| UAMG = 0X30, // get advanced mode status | |||
| PFD = 0x3b, // Disable power features <Parameter1 Name="0 - Disable, 1 - Enable" /> | |||
| READ_TABLE = 0x43, // read table from flash, for example, read imu calibration table, read_table 0x243 0 | |||
There was a problem hiding this comment.
This code is not complaint with D400 HWMon 0x43
Please refactor it into a separate enum/header
|
@ev-mp thanks for the review. I submitted changes with pid and extrinsic handling to address the issues. also updated d455 extrinsic. |
| else // unmapped configurations | ||
| { | ||
| _def_extr = { { 1, 0, 0, 0, 1, 0, 0, 0, 1 },{ 0.f, 0.f, 0.f } }; | ||
| _imu_2_depth_rot = { { -1,0,0 },{ 0,1,0 },{ 0,0,-1 } }; |
There was a problem hiding this comment.
Use identity matrix for default + add the unresolved PID to log
There was a problem hiding this comment.
IMU on new devices is now intentionally oriented and FW flips output such that output is consistent with D435i, it's better to use this matrix so in those unsupported cases the librealsense output will be correctly aligned with depth coordinate system.
There was a problem hiding this comment.
added more comment and pid to log
| // predefined platform specific extrinsic, IMU assembly transformation based on mechanical drawing (meters) | ||
| rs2_extrinsics _def_extr; | ||
|
|
||
| if (_pid == ds::RS435I_PID) |
There was a problem hiding this comment.
Pls add a TODO make - this section should be later refactored into a map with calib. data
Tracked on: RS5-7834