From 5f7dcfe775c362131d153388ff00e3862adc02dd Mon Sep 17 00:00:00 2001 From: Jim Radford Date: Fri, 3 May 2019 21:48:19 -0700 Subject: [PATCH 1/6] Fix D435i IMU extrinsics Previously (the rotational part of) _depth_to_imu was applied to the IMU to get depth coordinates when we should have applied the inverse. In addition the translational part was negated. The aligning of the IMU with the depth camera is separated from the exported-to-user/eeprom-stored extrinsics such that the stored and default extrinsics now are with respect to the depth aligned IMU. --- src/ds5/ds5-motion.cpp | 42 +++++------------------------------------- src/ds5/ds5-motion.h | 19 ++++++++----------- 2 files changed, 13 insertions(+), 48 deletions(-) diff --git a/src/ds5/ds5-motion.cpp b/src/ds5/ds5-motion.cpp index 7b190cd712..ec33437dd5 100644 --- a/src/ds5/ds5-motion.cpp +++ b/src/ds5/ds5-motion.cpp @@ -258,41 +258,11 @@ namespace librealsense // D435i to use predefined values extrinsics _depth_to_imu = std::make_shared>([this]() { - try - { - pose ex{}; - auto extr = _mm_calib->get_extrinsic(RS2_STREAM_ACCEL); - ex = { { extr.rotation[0], extr.rotation[1], extr.rotation[2], - extr.rotation[3], extr.rotation[4], extr.rotation[5], - extr.rotation[6], extr.rotation[7], extr.rotation[8]}, - { extr.translation[0], extr.translation[1], extr.translation[2]} }; - return from_pose(ex); - } - catch (const std::exception &exc) - { - LOG_INFO("IMU EEPROM extrinsic is not available" << exc.what()); - throw; - } - }); - - _depth_to_imu_aligned = std::make_shared>([this]() - { - try - { - rs2_extrinsics extr = **_depth_to_imu; - float rot[9] = { 1.f, 0.f, 0.f, 0.f, 1.f, 0.f, 0.f, 0.f, 1.f }; - librealsense::copy(&extr.rotation, &rot, arr_size_bytes(rot)); - return extr; - } - catch (const std::exception &exc) - { - LOG_INFO("IMU EEPROM Aligned extrinsic is not available" << exc.what()); - throw; - } + return _mm_calib->get_extrinsic(RS2_STREAM_ACCEL); }); // Make sure all MM streams are positioned with the same extrinsics - environment::get_instance().get_extrinsics_graph().register_extrinsics(*_depth_stream, *_accel_stream, _depth_to_imu_aligned); + environment::get_instance().get_extrinsics_graph().register_extrinsics(*_depth_stream, *_accel_stream, _depth_to_imu); environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*_accel_stream, *_gyro_stream); register_stream_to_extrinsic_group(*_gyro_stream, 0); register_stream_to_extrinsic_group(*_accel_stream, 0); @@ -306,19 +276,17 @@ namespace librealsense std::function align_imu_axes = nullptr; // Perform basic IMU transformation to align orientation with Depth sensor CS. - float3x3 rotation{ {1,0,0}, {0,1,0}, {0,0,1} }; try { - librealsense::copy(&rotation, &(*_depth_to_imu)->rotation, sizeof(float3x3)); - align_imu_axes = [rotation](rs2_stream stream, frame_interface* fr, callback_invocation_holder callback) + float3x3 imu_to_depth = _mm_calib->imu_to_depth_alignment(); + align_imu_axes = [imu_to_depth](rs2_stream stream, frame_interface* fr, callback_invocation_holder callback) { if (fr->get_stream()->get_format() == RS2_FORMAT_MOTION_XYZ32F) { auto xyz = (float3*)(fr->get_frame_data()); // The IMU sensor orientation shall be aligned with depth sensor's coordinate system - //Reference spec : Bosch BMI055 - *xyz = rotation * (*xyz); + *xyz = imu_to_depth * (*xyz); } }; } diff --git a/src/ds5/ds5-motion.h b/src/ds5/ds5-motion.h index 7a93a0e1a5..1e8eb5df18 100644 --- a/src/ds5/ds5-motion.h +++ b/src/ds5/ds5-motion.h @@ -12,6 +12,7 @@ namespace librealsense public: virtual rs2_extrinsics get_extrinsic_to(rs2_stream) = 0; // Extrinsics are referenced to the Depth stream, except for TM1 virtual ds::imu_intrinsic get_intrinsic(rs2_stream) = 0; // With extrinsic from FE<->IMU only + virtual float3x3 imu_to_depth_alignment() = 0; }; class tm1_imu_calib_parser : public mm_calib_parser @@ -24,6 +25,8 @@ namespace librealsense tm1_imu_calib_parser(const tm1_imu_calib_parser&); ~tm1_imu_calib_parser(){}; + float3x3 imu_to_depth_alignment() { return {{1,0,0},{0,1,0},{0,0,1}}; } + rs2_extrinsics get_extrinsic_to(rs2_stream stream) { if (!(RS2_STREAM_ACCEL == stream) && !(RS2_STREAM_GYRO == stream) && !(RS2_STREAM_FISHEYE == stream)) @@ -82,6 +85,8 @@ namespace librealsense dm_v2_imu_calib_parser(const dm_v2_imu_calib_parser&); ~dm_v2_imu_calib_parser() {}; + float3x3 imu_to_depth_alignment() { return {{-1,0,0},{0,1,0},{0,0,-1}}; } //Reference spec : Bosch BMI055 + rs2_extrinsics get_extrinsic_to(rs2_stream stream) { if (!(RS2_STREAM_ACCEL == stream) && !(RS2_STREAM_GYRO == stream)) @@ -95,17 +100,9 @@ namespace librealsense } else { - LOG_INFO("IMU Extrinsic table error, switch to default calibration"); + LOG_INFO("IMU extrinsic table not found; using CAD values"); // D435i specific - BMI055 assembly transformation based on mechanical drawing (mm) - // ([[ -1. , 0. , 0. , 5.52], - // [ 0. , 1. , 0. , 5.1 ], - // [ 0. , 0. , -1. , -11.74], - // [ 0. , 0. , 0. , 1. ]]) - // The orientation matrix will be integrated into the IMU stream data - extr = { {-1.f, 0.f, 0.f, - 0.f, 1.f, 0.f, - 0.f, 0.f, -1.f}, - { 0.00552f, -0.0051f, -0.01174f} }; + extr = { { 1, 0, 0, 0, 1, 0, 0, 0, 1 }, { -0.00552f, 0.0051f, 0.01174f} }; } return extr; }; @@ -146,6 +143,7 @@ namespace librealsense ds::imu_intrinsic get_intrinsic(rs2_stream); rs2_extrinsics get_extrinsic(rs2_stream); // The extrinsic defined as Depth->Stream rigid-body transfom. const std::vector get_fisheye_calib_raw(); + float3x3 imu_to_depth_alignment() { return (*_calib_parser)->imu_to_depth_alignment(); } private: std::shared_ptr _hw_monitor; @@ -185,7 +183,6 @@ namespace librealsense lazy _gyro_intrinsic; lazy> _fisheye_calibration_table_raw; std::shared_ptr> _depth_to_imu; // Mechanical installation pose - std::shared_ptr> _depth_to_imu_aligned; // Translation component #ifdef _WIN32 // Bandwidth parameters from BOSCH BMI 055 spec' From 9b8a301557de75170260f9d982dc262dd1620cb2 Mon Sep 17 00:00:00 2001 From: Jim Radford Date: Fri, 3 May 2019 22:36:16 -0700 Subject: [PATCH 2/6] Remove non-standard fourcc syntax --- src/sensor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensor.cpp b/src/sensor.cpp index d155225131..a0b9b9ac89 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -1140,7 +1140,7 @@ namespace librealsense std::lock_guard lock(_mtx); if (nullptr == mode.pf) return 0; // Windows support is limited int index = 0; - if (mode.pf->fourcc == 'GYRO') + if (mode.pf->fourcc == rs_fourcc('G','Y','R','O')) index = 1; return ++counter[index]; From 2f55421d988fb8899e848ad265c48d01a0d7e9ce Mon Sep 17 00:00:00 2001 From: Jim Radford Date: Fri, 3 May 2019 22:36:59 -0700 Subject: [PATCH 3/6] Fix missing/inconsistent override --- src/sensor.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensor.h b/src/sensor.h index c983df5f21..959842c5c8 100644 --- a/src/sensor.h +++ b/src/sensor.h @@ -141,7 +141,7 @@ namespace librealsense unsigned long long get_frame_counter(const request_mapping & mode, const platform::frame_object& fo) const override; - rs2_timestamp_domain get_frame_timestamp_domain(const request_mapping & mode, const platform::frame_object& fo) const; + rs2_timestamp_domain get_frame_timestamp_domain(const request_mapping & mode, const platform::frame_object& fo) const override; }; class hid_sensor : public sensor_base From 1f9e4137e441fe04a884ee810fb28c3a893cd886 Mon Sep 17 00:00:00 2001 From: Jim Radford Date: Mon, 6 May 2019 18:45:07 -0700 Subject: [PATCH 4/6] Add pedantic parens to suppress warning --- common/model-views.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/common/model-views.cpp b/common/model-views.cpp index 137255152d..0791477b8b 100644 --- a/common/model-views.cpp +++ b/common/model-views.cpp @@ -4154,8 +4154,8 @@ namespace rs2 auto texture = upload_frame(std::move(frame)); - if ((selected_tex_source_uid == -1 && frame.get_profile().format() == RS2_FORMAT_Z16) || - frame.get_profile().format() != RS2_FORMAT_ANY && is_3d_texture_source(frame)) + if ((selected_tex_source_uid == -1 && frame.get_profile().format() == RS2_FORMAT_Z16) || + (frame.get_profile().format() != RS2_FORMAT_ANY && is_3d_texture_source(frame))) { texture_frame = texture; } From 5678e587e8f4a3dec9982731f51ef992e3f6ca56 Mon Sep 17 00:00:00 2001 From: Jim Radford Date: Wed, 8 May 2019 10:37:16 -0700 Subject: [PATCH 5/6] Fix compilation with TRACE_API error: return type 'nullptr_t' must match previous return type 'rs2_raw_data_buffer *' when lambda expression has unspecified explicit return type --- src/rs.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/rs.cpp b/src/rs.cpp index 9ae5efa88f..53256ca8e0 100644 --- a/src/rs.cpp +++ b/src/rs.cpp @@ -2247,7 +2247,7 @@ const rs2_raw_data_buffer* rs2_export_localization_map(const rs2_sensor* sensor, std::vector recv_buffer; if (pose_snr->export_relocalization_map(recv_buffer)) return new rs2_raw_data_buffer{ recv_buffer }; - return nullptr; + return (rs2_raw_data_buffer*)nullptr; } HANDLE_EXCEPTIONS_AND_RETURN(nullptr, sensor) From c31e55c1fa814ef744c62578d97b5d40e6c546da Mon Sep 17 00:00:00 2001 From: Jim Radford Date: Thu, 9 May 2019 10:05:16 -0700 Subject: [PATCH 6/6] Export rs2_create_error --- include/librealsense2/h/rs_types.h | 1 + src/rs.cpp | 3 ++- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/include/librealsense2/h/rs_types.h b/include/librealsense2/h/rs_types.h index 6acdefcbcb..3fe1896e57 100644 --- a/include/librealsense2/h/rs_types.h +++ b/include/librealsense2/h/rs_types.h @@ -237,6 +237,7 @@ typedef void (*rs2_frame_processor_callback_ptr)(rs2_frame*, rs2_source*, void*) typedef double rs2_time_t; /**< Timestamp format. units are milliseconds */ typedef long long rs2_metadata_type; /**< Metadata attribute type is defined as 64 bit signed integer*/ +rs2_error * rs2_create_error(const char* what, const char* name, const char* args, rs2_exception_type type); rs2_exception_type rs2_get_librealsense_exception_type(const rs2_error* error); const char* rs2_get_failed_function (const rs2_error* error); const char* rs2_get_failed_args (const rs2_error* error); diff --git a/src/rs.cpp b/src/rs.cpp index 53256ca8e0..8473e83111 100644 --- a/src/rs.cpp +++ b/src/rs.cpp @@ -120,10 +120,11 @@ struct rs2_error rs2_exception_type exception_type; }; -rs2_error * rs2_create_error(const char* what, const char* name, const char* args, rs2_exception_type type) +rs2_error *rs2_create_error(const char* what, const char* name, const char* args, rs2_exception_type type) BEGIN_API_CALL { return new rs2_error{ what, name, args, type }; } +NOEXCEPT_RETURN(nullptr, what, name, args, type) void notifications_processor::raise_notification(const notification n) {