fix depth to color extrinsics

IntelRealSense · Mar 2, 2021 · 695fd28 · 695fd28
1 parent da5f798
commit 695fd28
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 5 deletions.
diff --git a/src/ds5/ds5-color.cpp b/src/ds5/ds5-color.cpp
@@ -54,11 +54,6 @@ namespace librealsense
         using namespace ds;
         auto&& backend = ctx->get_backend();
 
-        _color_calib_table_raw = [this]() { return get_raw_calibration_table(rgb_calibration_id); };
-        _color_extrinsic = std::make_shared<lazy<rs2_extrinsics>>([this]() { return from_pose(get_color_stream_extrinsic(*_color_calib_table_raw)); });
-        environment::get_instance().get_extrinsics_graph().register_extrinsics(*_color_stream, *_depth_stream, _color_extrinsic);
-        register_stream_to_extrinsic_group(*_color_stream, 0);
-
         std::vector<platform::uvc_device_info> color_devs_info;
         // end point 3 is used for color sensor
         // except for D405, in which the color is part of the depth unit
@@ -108,6 +103,8 @@ namespace librealsense
 
     void ds5_color::init()
     {
+        using namespace ds;
+
         auto& color_ep = get_color_sensor();
         auto& raw_color_ep = get_raw_color_sensor();
 
@@ -142,6 +139,13 @@ namespace librealsense
 
         if (_separate_color)
         {
+            _color_calib_table_raw = [this]() { return get_raw_calibration_table(rgb_calibration_id); };
+            _color_extrinsic = std::make_shared<lazy<rs2_extrinsics>>([this]() {
+                return from_pose(get_color_stream_extrinsic(*_color_calib_table_raw));
+                });
+            environment::get_instance().get_extrinsics_graph().register_extrinsics(*_color_stream, *_depth_stream, _color_extrinsic);
+            register_stream_to_extrinsic_group(*_color_stream, 0);
+
             // Currently disabled for certain sensors
             if (!val_in_range(_pid, { ds::RS465_PID }))
             {
@@ -177,6 +181,8 @@ namespace librealsense
         }
         else
         {
+            ds5_device::register_color_depth_extrinsics();
+
             // attributes of md_rgb_control
             auto md_prop_offset = offsetof(metadata_raw, mode) +
                 offsetof(md_rgb_mode, rgb_mode) +

diff --git a/src/ds5/ds5-device.cpp b/src/ds5/ds5-device.cpp
@@ -1076,6 +1076,11 @@ namespace librealsense
         //TODO: Implement
     }
 
+    void ds5_device::register_color_depth_extrinsics()
+    {
+        environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*_depth_stream, *_color_stream);
+    }
+
     platform::usb_spec ds5_device::get_usb_spec() const
     {
         if(!supports_info(RS2_CAMERA_INFO_USB_TYPE_DESCRIPTOR))

diff --git a/src/ds5/ds5-device.h b/src/ds5/ds5-device.h
@@ -64,6 +64,7 @@ namespace librealsense
         std::vector<uint8_t> backup_flash(update_progress_callback_ptr callback) override;
         void update_flash(const std::vector<uint8_t>& image, update_progress_callback_ptr callback, int update_mode) override;
     protected:
+        void register_color_depth_extrinsics();
 
         std::vector<uint8_t> get_raw_calibration_table(ds::calibration_table_id table_id) const;
         std::vector<uint8_t> get_new_calibration_table() const;