Merge pull request #3902 from radfordi/d435i-imu-extrinsics

Fix D435i IMU Extrinsics

common/model-views.cpp

@@ -4121,8 +4121,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;
                         }

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);

src/ds5/ds5-motion.cpp

@@ -263,41 +263,11 @@ namespace librealsense
         // D435i to use predefined values extrinsics
         _depth_to_imu = std::make_shared<lazy<rs2_extrinsics>>([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<lazy<rs2_extrinsics>>([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);
@@ -311,19 +281,17 @@ namespace librealsense
             std::function<void(rs2_stream stream, frame_interface* fr, callback_invocation_holder callback)> 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);
                     }
                 };
             }

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<uint8_t> get_fisheye_calib_raw();
+        float3x3 imu_to_depth_alignment() { return (*_calib_parser)->imu_to_depth_alignment(); }
 
     private:
         std::shared_ptr<hw_monitor> _hw_monitor;
@@ -185,7 +183,6 @@ namespace librealsense
         lazy<ds::imu_intrinsic>                 _gyro_intrinsic;
         lazy<std::vector<uint8_t>>              _fisheye_calibration_table_raw;
         std::shared_ptr<lazy<rs2_extrinsics>>   _depth_to_imu;                  // Mechanical installation pose
-        std::shared_ptr<lazy<rs2_extrinsics>>   _depth_to_imu_aligned;          // Translation component
 
 #ifdef _WIN32
         // Bandwidth parameters from BOSCH BMI 055 spec'

src/rs.cpp

@@ -119,10 +119,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)
 {
@@ -2262,7 +2263,7 @@ const rs2_raw_data_buffer* rs2_export_localization_map(const rs2_sensor* sensor,
     std::vector<uint8_t> 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)
 

src/sensor.cpp

@@ -1140,7 +1140,7 @@ namespace librealsense
         std::lock_guard<std::recursive_mutex> 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];

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