From f77451867a289df08bd776d0b598f2978882a297 Mon Sep 17 00:00:00 2001 From: aangerma Date: Thu, 19 Jul 2018 19:49:18 +0300 Subject: [PATCH] Added intrinsics to l500 and adjust DQT to work with l500. --- src/l500/l500-private.h | 3 ++- src/l500/l500.cpp | 10 ++++++++- src/l500/l500.h | 25 ++++++++++++++++++--- tools/depth-quality/depth-quality-model.cpp | 4 +++- tools/depth-quality/depth-quality-model.h | 1 + tools/depth-quality/rs-depth-quality.cpp | 2 +- 6 files changed, 38 insertions(+), 7 deletions(-) diff --git a/src/l500/l500-private.h b/src/l500/l500-private.h index f3e1fb96c1..da9aeb8275 100644 --- 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 index 5229a369e8..0e59b0d6e8 100644 --- a/src/l500/l500.cpp +++ b/src/l500/l500.cpp @@ -57,6 +57,12 @@ namespace librealsense return results; } + std::vector 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 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(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 index d9bd487a24..842e14b07a 100644 --- 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 @@ -212,7 +224,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& snapshot) const override { @@ -226,6 +238,7 @@ namespace librealsense } private: const l500_device* _owner; + float _depth_units; }; std::shared_ptr create_depth_device(std::shared_ptr ctx, @@ -253,6 +266,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("Number of meters represented by a single depth unit", + lazy([]() { + return 0.000125f; }))); return depth_ep; } @@ -268,6 +284,7 @@ namespace librealsense uvc_sensor& get_depth_sensor() { return dynamic_cast(get_sensor(_depth_device_idx)); } + std::vector get_raw_calibration_table() const; l500_device(std::shared_ptr ctx, const platform::backend_device_group& group, @@ -285,6 +302,8 @@ namespace librealsense std::shared_ptr _ir_stream; std::shared_ptr _confidence_stream; + lazy> _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 index c96fdac4c6..cb3a24760d 100644 --- 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( 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 index 86a4cc44bd..57f733328c 100644 --- 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 index fed8da4528..bb31401751 100644 --- 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& 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;