Merge pull request IntelRealSense#2131 from aangerma/development

Adding intrinsics API
ForS1F · Jul 24, 2018 · b490105 · b490105
2 parents c4d84ae + f774518
commit b490105
Show file tree

Hide file tree

Showing 6 changed files with 38 additions and 7 deletions.
diff --git a/src/l500/l500-private.h b/src/l500/l500-private.h
@@ -25,7 +25,8 @@ namespace librealsense
         {
             HWReset = 0x20,
             GVD = 0x10,
-            GLD = 0x0f
+            GLD = 0x0f,
+            DPT_INTRINSICS_GET = 0x5A
         };
 
         enum gvd_fields

diff --git a/src/l500/l500.cpp b/src/l500/l500.cpp
@@ -57,6 +57,12 @@ namespace librealsense
         return results;
     }
 
+    std::vector<uint8_t> l500_device::get_raw_calibration_table() const
+    {
+        command cmd(ivcam2::fw_cmd::DPT_INTRINSICS_GET);
+        return _hw_monitor->send(cmd);
+    }
+
     l500_device::l500_device(std::shared_ptr<context> ctx,
                              const platform::backend_device_group& group,
                              bool register_device_notifications)
@@ -66,6 +72,8 @@ namespace librealsense
         _ir_stream(new stream(RS2_STREAM_INFRARED)),
         _confidence_stream(new stream(RS2_STREAM_CONFIDENCE))
     {
+        _calib_table_raw = [this]() { return get_raw_calibration_table(); };
+
         static const auto device_name = "Intel RealSense L500";
 
         using namespace ivcam2;
@@ -82,7 +90,7 @@ namespace librealsense
                     std::make_shared<locked_transfer>(backend.create_usb_device(group.usb_devices.front()),
                                                                                 get_depth_sensor()));
 //#endif
-
+        *_calib_table_raw;  //work around to bug on fw
         auto fw_version = _hw_monitor->get_firmware_version_string(GVD, fw_version_offset);
         auto serial = _hw_monitor->get_module_serial_string(GVD, module_serial_offset);
 

diff --git a/src/l500/l500.h b/src/l500/l500.h
@@ -165,8 +165,20 @@ namespace librealsense
 
             rs2_intrinsics get_intrinsics(const stream_profile& profile) const override
             {
-                // TODO
-                return rs2_intrinsics{};
+                auto res = *_owner->_calib_table_raw;
+                auto intr = (float*)res.data();
+
+                if (res.size() < sizeof(float) * 4)
+                    throw invalid_value_exception("size of calibration invalid");
+
+                rs2_intrinsics intrinsics;
+                intrinsics.width = profile.width;
+                intrinsics.height = profile.height;
+                intrinsics.fx = intr[0];
+                intrinsics.fy = intr[1];
+                intrinsics.ppx = intr[2];
+                intrinsics.ppy = intr[3];
+                return intrinsics;
             }
 
             stream_profiles init_stream_profiles() override
@@ -210,7 +222,7 @@ namespace librealsense
                 return results;
             }
 
-            float get_depth_scale() const override { return 0.001f; } // TODO
+            float get_depth_scale() const override {  return get_option(RS2_OPTION_DEPTH_UNITS).query(); }
 
             void create_snapshot(std::shared_ptr<depth_sensor>& snapshot) const  override
             {
@@ -224,6 +236,7 @@ namespace librealsense
             }
         private:
             const l500_device* _owner;
+            float _depth_units;
         };
 
         std::shared_ptr<uvc_sensor> create_depth_device(std::shared_ptr<context> ctx,
@@ -251,6 +264,9 @@ namespace librealsense
                     ivcam2::depth_xu,
                     ivcam2::IVCAM2_DEPTH_LASER_POWER, "Power of the l500 projector, with 0 meaning projector off"));
 
+            depth_ep->register_option(RS2_OPTION_DEPTH_UNITS, std::make_shared<const_value_option>("Number of meters represented by a single depth unit",
+                lazy<float>([]() { 
+                return 0.000125f; })));
             return depth_ep;
         }
 
@@ -266,6 +282,7 @@ namespace librealsense
 
         uvc_sensor& get_depth_sensor() { return dynamic_cast<uvc_sensor&>(get_sensor(_depth_device_idx)); }
 
+        std::vector<uint8_t> get_raw_calibration_table() const;
 
         l500_device(std::shared_ptr<context> ctx,
                     const platform::backend_device_group& group,
@@ -283,6 +300,8 @@ namespace librealsense
         std::shared_ptr<stream_interface> _ir_stream;
         std::shared_ptr<stream_interface> _confidence_stream;
 
+        lazy<std::vector<uint8_t>> _calib_table_raw;
+
         void force_hardware_reset() const;
     };
 }
diff --git a/tools/depth-quality/depth-quality-model.cpp b/tools/depth-quality/depth-quality-model.cpp
@@ -63,7 +63,7 @@ namespace rs2
             {
                 rs2::config cfg_alt;
                 cfg_alt.enable_stream(RS2_STREAM_DEPTH, 0, 0, 0, RS2_FORMAT_Z16, requested_fps);
-                cfg_alt.enable_stream(RS2_STREAM_INFRARED, 1, 0, 0, RS2_FORMAT_Y8, requested_fps);
+                cfg_alt.enable_stream(RS2_STREAM_INFRARED, 0, 0, 0, RS2_FORMAT_Y8, requested_fps);
                 cfgs.emplace_back(cfg_alt);
             }
 
@@ -78,6 +78,7 @@ namespace rs2
                     }
                     catch (...)
                     {
+                        _pipe.stop();
                         valid_config = false;
                         if (!_device_in_use)
                         {
@@ -764,6 +765,7 @@ namespace rs2
             _device_model->show_stream_selection = false;
             _depth_sensor_model = std::shared_ptr<rs2::subdevice_model>(
                 new subdevice_model(dev, dpt_sensor, _error_message));
+
             _depth_sensor_model->draw_streams_selector = false;
             _depth_sensor_model->draw_fps_selector = true;
 

diff --git a/tools/depth-quality/depth-quality-model.h b/tools/depth-quality/depth-quality-model.h
@@ -363,6 +363,7 @@ namespace rs2
 
             void on_frame(callback_type callback) { _metrics_model.callback = callback; }
 
+            float get_depth_scale() const { return _metrics_model._depth_scale_units; }
             rs2::device get_active_device(void) const;
 
         private:

diff --git a/tools/depth-quality/rs-depth-quality.cpp b/tools/depth-quality/rs-depth-quality.cpp
@@ -78,7 +78,7 @@ int main(int argc, const char * argv[]) try
         bool record,
         std::vector<single_metric_data>& samples)
     {
-        static const float TO_METERS = 0.001f;
+        float TO_METERS = model.get_depth_scale();
         static const float TO_MM = 1000.f;
         static const float TO_PERCENT = 100.f;