From 215fb0377f753a1238fee76f863c1f87a6d962c6 Mon Sep 17 00:00:00 2001 From: ohadmeir Date: Tue, 27 Aug 2024 16:26:56 +0300 Subject: [PATCH 1/3] Support depth_stereo_sensor interface over DDS --- src/dds/rs-dds-depth-sensor-proxy.cpp | 34 +++++++++++++++++++++++++++ src/dds/rs-dds-depth-sensor-proxy.h | 5 ++-- 2 files changed, 37 insertions(+), 2 deletions(-) diff --git a/src/dds/rs-dds-depth-sensor-proxy.cpp b/src/dds/rs-dds-depth-sensor-proxy.cpp index 098e4d53e0..7d7913d6dd 100644 --- a/src/dds/rs-dds-depth-sensor-proxy.cpp +++ b/src/dds/rs-dds-depth-sensor-proxy.cpp @@ -6,6 +6,7 @@ #include +#include #include #include @@ -23,6 +24,39 @@ float dds_depth_sensor_proxy::get_depth_scale() const return 0.001f; } +float dds_depth_sensor_proxy::get_stereo_baseline_mm() const +{ + if( auto opt = get_option_handler( RS2_OPTION_STEREO_BASELINE ) ) + return opt->get_value(); + + // Baseline not registered, try getting it from extrinsics + std::shared_ptr< stream_profile_interface > ir1; + std::shared_ptr< stream_profile_interface > ir2; + for( auto & prof : get_raw_stream_profiles() ) + { + if( prof->get_stream_type() == RS2_STREAM_INFRARED ) + { + if( prof->get_stream_index() == 1 ) + ir1 = prof; + if( prof->get_stream_index() == 2 ) + ir2 = prof; + } + } + + if( ir1 && ir2 ) + { + rs2_extrinsics out; + if( environment::get_instance().get_extrinsics_graph().try_fetch_extrinsics( *ir1, *ir2, &out ) ) + { + float baseline = std::sqrt( out.translation[0] * out.translation[0] + + out.translation[1] * out.translation[1] + + out.translation[2] * out.translation[2] ); + return baseline; + } + } + + throw std::runtime_error( "Not a stereo depth sensor. Cannot get basline information." ); +} void dds_depth_sensor_proxy::add_no_metadata( frame * const f, streaming_impl & streaming ) { diff --git a/src/dds/rs-dds-depth-sensor-proxy.h b/src/dds/rs-dds-depth-sensor-proxy.h index a00822fc31..61806cfb96 100644 --- a/src/dds/rs-dds-depth-sensor-proxy.h +++ b/src/dds/rs-dds-depth-sensor-proxy.h @@ -8,10 +8,10 @@ namespace librealsense { -// For cases when checking if this is< depth_sensor > (like realsense-viewer::subdevice_model) +// For cases when checking if this is< depth_sensor > or is< depth_stereo_sensor > (like realsense-viewer::subdevice_model and on-chip-calib) class dds_depth_sensor_proxy : public dds_sensor_proxy - , public depth_sensor + , public depth_stereo_sensor { using super = dds_sensor_proxy; @@ -25,6 +25,7 @@ class dds_depth_sensor_proxy // Needed by abstract interfaces float get_depth_scale() const override; + float get_stereo_baseline_mm() const override; protected: void add_no_metadata( frame *, streaming_impl & ) override; From 7f85f3d422a6878c177444fdc1c1d62c228c5b3b Mon Sep 17 00:00:00 2001 From: ohadmeir Date: Wed, 28 Aug 2024 16:30:40 +0300 Subject: [PATCH 2/3] Avoid releasing same frame twice during FL calib --- src/auto-calibrated-proxy.cpp | 9 ++++++--- src/ds/d500/d500-auto-calibration.cpp | 7 +------ src/ds/ds-calib-common.cpp | 2 +- 3 files changed, 8 insertions(+), 10 deletions(-) diff --git a/src/auto-calibrated-proxy.cpp b/src/auto-calibrated-proxy.cpp index 3289198115..51713a51aa 100644 --- a/src/auto-calibrated-proxy.cpp +++ b/src/auto-calibrated-proxy.cpp @@ -53,14 +53,16 @@ void auto_calibrated_proxy::write_calibration() const { if (_auto_calib_capability) _auto_calib_capability->write_calibration(); - throw std::runtime_error("Auto Calibration capability has not been initiated"); + else + throw std::runtime_error("Auto Calibration capability has not been initiated"); } void auto_calibrated_proxy::set_calibration_table(const std::vector& calibration) { if (_auto_calib_capability) _auto_calib_capability->set_calibration_table(calibration); - throw std::runtime_error("Auto Calibration capability has not been initiated"); + else + throw std::runtime_error("Auto Calibration capability has not been initiated"); } void auto_calibrated_proxy::reset_to_factory_calibration() const @@ -105,7 +107,8 @@ void auto_calibrated_proxy::set_calibration_config(const std::string& calibratio { if (_auto_calib_capability) _auto_calib_capability->set_calibration_config(calibration_config_json_str); - throw std::runtime_error("Auto Calibration capability has not been initiated"); + else + throw std::runtime_error("Auto Calibration capability has not been initiated"); } } //namespace librealsense diff --git a/src/ds/d500/d500-auto-calibration.cpp b/src/ds/d500/d500-auto-calibration.cpp index e96388d2af..65e557aaed 100644 --- a/src/ds/d500/d500-auto-calibration.cpp +++ b/src/ds/d500/d500-auto-calibration.cpp @@ -259,12 +259,7 @@ namespace librealsense float fy[2] = { -1.0f, -1.0f }; float left_rect_sides[4] = { 0.f }; - ds_calib_common::get_target_rect_info( left, - left_rect_sides, - fx[0], - fy[0], - 50, - progress_callback ); // Report 50% progress + ds_calib_common::get_target_rect_info( left, left_rect_sides, fx[0], fy[0], 50, progress_callback ); // Report 50% progress float right_rect_sides[4] = { 0.f }; ds_calib_common::get_target_rect_info( right, right_rect_sides, fx[1], fy[1], 75, progress_callback ); diff --git a/src/ds/ds-calib-common.cpp b/src/ds/ds-calib-common.cpp index 9ea03a41ac..af963650f1 100644 --- a/src/ds/ds-calib-common.cpp +++ b/src/ds/ds-calib-common.cpp @@ -100,7 +100,7 @@ namespace librealsense rect_sides_arr.emplace_back( rec_sides_cur ); } - rs2_release_frame( f ); + ff = {}; // Release the frame, don't need it for progress callback if( progress_callback ) progress_callback->on_update_progress( static_cast< float >( ++progress ) ); From 34a1fda403ec17fcfe988db759f12a0ac45d1fbd Mon Sep 17 00:00:00 2001 From: ohadmeir Date: Mon, 2 Sep 2024 14:46:37 +0300 Subject: [PATCH 3/3] Handle PR#13302 comments --- src/dds/rs-dds-depth-sensor-proxy.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/dds/rs-dds-depth-sensor-proxy.cpp b/src/dds/rs-dds-depth-sensor-proxy.cpp index 7d7913d6dd..6684a648a2 100644 --- a/src/dds/rs-dds-depth-sensor-proxy.cpp +++ b/src/dds/rs-dds-depth-sensor-proxy.cpp @@ -38,7 +38,7 @@ float dds_depth_sensor_proxy::get_stereo_baseline_mm() const { if( prof->get_stream_index() == 1 ) ir1 = prof; - if( prof->get_stream_index() == 2 ) + else if( prof->get_stream_index() == 2 ) ir2 = prof; } } @@ -55,7 +55,7 @@ float dds_depth_sensor_proxy::get_stereo_baseline_mm() const } } - throw std::runtime_error( "Not a stereo depth sensor. Cannot get basline information." ); + throw not_implemented_exception( "Not a stereo depth sensor. Cannot get basline information." ); } void dds_depth_sensor_proxy::add_no_metadata( frame * const f, streaming_impl & streaming )