diff --git a/src/dds/rs-dds-device-proxy.cpp b/src/dds/rs-dds-device-proxy.cpp index 4c71c81c4a4..609feb31f91 100644 --- a/src/dds/rs-dds-device-proxy.cpp +++ b/src/dds/rs-dds-device-proxy.cpp @@ -47,12 +47,41 @@ static rs2_stream to_rs2_stream_type( std::string const & type_string ) } +rs2_distortion to_rs2_distortion( realdds::distortion_model model ) +{ + switch( model ) + { + case realdds::distortion_model::none: return RS2_DISTORTION_NONE; + case realdds::distortion_model::brown: return RS2_DISTORTION_BROWN_CONRADY; + case realdds::distortion_model::inverse_brown: return RS2_DISTORTION_INVERSE_BROWN_CONRADY; + case realdds::distortion_model::modified_brown: return RS2_DISTORTION_MODIFIED_BROWN_CONRADY; + default: + throw invalid_value_exception( "unexpected realdds distortion model: " + std::to_string( (int)model ) ); + } +} + + +rs2_intrinsics to_rs2_intrinsics( const realdds::video_intrinsics & intrinsics ) +{ + rs2_intrinsics intr; + intr.width = intrinsics.width; + intr.height = intrinsics.height; + intr.ppx = intrinsics.principal_point.x; + intr.ppy = intrinsics.principal_point.y; + intr.fx = intrinsics.focal_length.x; + intr.fy = intrinsics.focal_length.y; + intr.model = to_rs2_distortion( intrinsics.distortion.model ); + memcpy( intr.coeffs, intrinsics.distortion.coeffs.data(), sizeof( intr.coeffs ) ); + return intr; +} + + static rs2_video_stream to_rs2_video_stream( rs2_stream const stream_type, sid_index const & sidx, std::shared_ptr< realdds::dds_video_stream_profile > const & profile, - const std::set< realdds::video_intrinsics > & intrinsics ) + const realdds::video_intrinsics & intrinsics ) { - rs2_video_stream prof = {}; + rs2_video_stream prof; prof.type = stream_type; prof.index = sidx.index; prof.uid = sidx.sid; @@ -60,24 +89,7 @@ static rs2_video_stream to_rs2_video_stream( rs2_stream const stream_type, prof.height = profile->height(); prof.fps = profile->frequency(); prof.fmt = static_cast< rs2_format >( profile->encoding().to_rs2() ); - - // Handle intrinsics - auto intr = std::find_if( intrinsics.begin(), - intrinsics.end(), - [profile]( const realdds::video_intrinsics & intr ) - { return profile->width() == intr.width && profile->height() == intr.height; } ); - if( intr != intrinsics.end() ) // Some profiles don't have intrinsics - { - prof.intrinsics.width = intr->width; - prof.intrinsics.height = intr->height; - prof.intrinsics.ppx = intr->principal_point_x; - prof.intrinsics.ppy = intr->principal_point_y; - prof.intrinsics.fx = intr->focal_lenght_x; - prof.intrinsics.fy = intr->focal_lenght_y; - prof.intrinsics.model = static_cast< rs2_distortion >( intr->distortion_model ); - memcpy( prof.intrinsics.coeffs, intr->distortion_coeffs.data(), sizeof( prof.intrinsics.coeffs ) ); - } - + prof.intrinsics = to_rs2_intrinsics( intrinsics.scaled_to( profile->width(), profile->height() ) ); return prof; } @@ -398,25 +410,13 @@ void dds_device_proxy::set_video_profile_intrinsics( std::shared_ptr< stream_pro std::shared_ptr< realdds::dds_video_stream > stream ) const { auto vsp = std::dynamic_pointer_cast< video_stream_profile >( profile ); - auto & stream_intrinsics = stream->get_intrinsics(); - auto it = std::find_if( stream_intrinsics.begin(), - stream_intrinsics.end(), - [vsp]( const realdds::video_intrinsics & intr ) - { return vsp->get_width() == intr.width && vsp->get_height() == intr.height; } ); - - if( it != stream_intrinsics.end() ) // Some profiles don't have intrinsics - { - rs2_intrinsics intr; - intr.width = it->width; - intr.height = it->height; - intr.ppx = it->principal_point_x; - intr.ppy = it->principal_point_y; - intr.fx = it->focal_lenght_x; - intr.fy = it->focal_lenght_y; - intr.model = static_cast< rs2_distortion >( it->distortion_model ); - memcpy( intr.coeffs, it->distortion_coeffs.data(), sizeof( intr.coeffs ) ); - vsp->set_intrinsics( [intr]() { return intr; } ); - } + int const w = vsp->get_width(); + int const h = vsp->get_height(); + vsp->set_intrinsics( + [stream_intrinsics = stream->get_intrinsics(), w, h]() + { + return to_rs2_intrinsics( stream_intrinsics.scaled_to( w, h ) ); + } ); } diff --git a/src/device.cpp b/src/device.cpp index a9dd9bb48a4..18de25beae7 100644 --- a/src/device.cpp +++ b/src/device.cpp @@ -37,6 +37,23 @@ device::device( std::shared_ptr< const device_info > const & dev_info, : _dev_info( dev_info ) , _is_alive( std::make_shared< std::atomic< bool > >( true ) ) , _profiles_tags( [this]() { return get_profiles_tags(); } ) + , _format_conversion( + [this] + { + auto context = get_context(); + if( ! context ) + return format_conversion::full; + std::string const format_conversion( "format-conversion", 17 ); + std::string const full( "full", 4 ); + auto const value = context->get_settings().nested( format_conversion ).default_value( full ); + if( value == full ) + return format_conversion::full; + if( value == "basic" ) + return format_conversion::basic; + if( value == "raw" ) + return format_conversion::raw; + throw invalid_value_exception( "invalid " + format_conversion + " value '" + value + "'" ); + } ) { if( device_changed_notifications ) { @@ -199,25 +216,14 @@ std::vector device::map_supported_color_formats(rs2_format source_fo format_conversion device::get_format_conversion() const { - auto context = get_context(); - if( ! context ) - return format_conversion::full; - std::string const format_conversion( "format-conversion", 17 ); - std::string const full( "full", 4 ); - auto const value = context->get_settings().nested( format_conversion ).default_value( full ); - if( value == full ) - return format_conversion::full; - if( value == "basic" ) - return format_conversion::basic; - if( value == "raw" ) - return format_conversion::raw; - throw invalid_value_exception( "invalid " + format_conversion + " value '" + value + "'" ); + return *_format_conversion; } void device::tag_profiles(stream_profiles profiles) const { for (auto profile : profiles) { + LOG_DEBUG( " profile: " << profile ); for (auto tag : *_profiles_tags) { if (auto vp = dynamic_cast(profile.get())) diff --git a/src/device.h b/src/device.h index bdbaeef3071..1c2241020bd 100644 --- a/src/device.h +++ b/src/device.h @@ -86,6 +86,7 @@ class device std::shared_ptr< std::atomic< bool > > _is_alive; rsutils::subscription _device_change_subscription; rsutils::lazy< std::vector< tagged_profile > > _profiles_tags; + rsutils::lazy< format_conversion > _format_conversion; }; diff --git a/src/ds/d400/d400-color.cpp b/src/ds/d400/d400-color.cpp index e04ec323bfd..1c4e88483ed 100644 --- a/src/ds/d400/d400-color.cpp +++ b/src/ds/d400/d400-color.cpp @@ -129,6 +129,14 @@ namespace librealsense register_feature( std::make_shared< auto_exposure_roi_feature >( get_color_sensor(), _hw_monitor, true ) ); } + rs2_format d400_color::get_color_format() const + { + auto const format_conversion = get_format_conversion(); + rs2_format const color_format + = (format_conversion == format_conversion::full) ? RS2_FORMAT_RGB8 : RS2_FORMAT_YUYV; + return color_format; + } + void d400_color::init() { auto& color_ep = get_color_sensor(); diff --git a/src/ds/d400/d400-color.h b/src/ds/d400/d400-color.h index c07c60398f2..67ed3904a7a 100644 --- a/src/ds/d400/d400-color.h +++ b/src/ds/d400/d400-color.h @@ -33,6 +33,8 @@ namespace librealsense protected: void register_color_features(); + rs2_format get_color_format() const; + std::shared_ptr _color_stream; std::shared_ptr _ds_color_common; diff --git a/src/ds/d400/d400-device.cpp b/src/ds/d400/d400-device.cpp index 65eec6e71fe..5a1fb757bee 100644 --- a/src/ds/d400/d400-device.cpp +++ b/src/ds/d400/d400-device.cpp @@ -1203,6 +1203,13 @@ namespace librealsense } + rs2_format d400_device::get_ir_format() const + { + auto const format_conversion = get_format_conversion(); + return ( format_conversion == format_conversion::raw ) ? RS2_FORMAT_Y8I : RS2_FORMAT_Y8; + } + + double d400_device::get_device_time_ms() { //// TODO: Refactor the following query with an extension. diff --git a/src/ds/d400/d400-device.h b/src/ds/d400/d400-device.h index e77140f319f..aea4d260071 100644 --- a/src/ds/d400/d400-device.h +++ b/src/ds/d400/d400-device.h @@ -79,6 +79,8 @@ namespace librealsense ds::ds_caps parse_device_capabilities( const std::vector &gvd_buf ) const; + rs2_format get_ir_format() const; + //TODO - add these to device class as pure virtual methods command get_firmware_logs_command() const; command get_flash_logs_command() const; diff --git a/src/ds/d400/d400-factory.cpp b/src/ds/d400/d400-factory.cpp index 3f1d8775105..016cc4d5299 100644 --- a/src/ds/d400/d400-factory.cpp +++ b/src/ds/d400/d400-factory.cpp @@ -193,13 +193,13 @@ namespace librealsense auto usb_spec = get_usb_spec(); if (usb_spec >= platform::usb3_type || usb_spec == platform::usb_undefined) { - tags.push_back({ RS2_STREAM_COLOR, -1, 1280, 720, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_COLOR, -1, 1280, 720, get_color_format(), 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_DEPTH, -1, 1280, 720, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, -1, 1280, 720, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET }); } else { - tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, -1, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET }); } @@ -289,13 +289,13 @@ namespace librealsense if (usb_spec >= platform::usb3_type || usb_spec == platform::usb_undefined) { tags.push_back({ RS2_STREAM_DEPTH, -1, 720, 720, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); - tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, 1, 720, 720, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET }); } else { tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); - tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, 1, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET }); } return tags; @@ -561,13 +561,13 @@ namespace librealsense auto usb_spec = get_usb_spec(); if (usb_spec >= platform::usb3_type || usb_spec == platform::usb_undefined) { - tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_DEPTH, -1, 848, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, -1, 848, 480, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET }); } else { - tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, -1, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET }); } @@ -602,7 +602,7 @@ namespace librealsense { std::vector tags; - tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); return tags; @@ -638,13 +638,13 @@ namespace librealsense auto usb_spec = get_usb_spec(); if (usb_spec >= platform::usb3_type || usb_spec == platform::usb_undefined) { - tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_DEPTH, -1, 848, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, -1, 848, 480, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET }); } else { - tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, -1, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET }); } @@ -692,7 +692,7 @@ namespace librealsense int color_height = usb3mode ? 720 : 480; int fps = usb3mode ? 30 : 15; - tags.push_back({ RS2_STREAM_COLOR, -1, color_width, color_height, RS2_FORMAT_RGB8, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_COLOR, -1, color_width, color_height, get_color_format(), fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_DEPTH, -1, depth_width, depth_height, RS2_FORMAT_Z16, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_INFRARED, -1, depth_width, depth_height, RS2_FORMAT_Y8, fps, profile_tag::PROFILE_TAG_SUPERSET }); tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); @@ -935,12 +935,10 @@ namespace librealsense int color_height = 480; int fps = usb3mode ? 30 : 10; - auto format_conversion = get_format_conversion(); - tags.push_back( { RS2_STREAM_COLOR, -1, // index color_width, color_height, - ( format_conversion == format_conversion::full ) ? RS2_FORMAT_RGB8 : RS2_FORMAT_YUYV, + get_color_format(), fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } ); tags.push_back( { RS2_STREAM_DEPTH, @@ -952,7 +950,7 @@ namespace librealsense tags.push_back( { RS2_STREAM_INFRARED, -1, // index depth_width, depth_height, - ( format_conversion == format_conversion::raw ) ? RS2_FORMAT_Y8I : RS2_FORMAT_Y8, + get_ir_format(), fps, profile_tag::PROFILE_TAG_SUPERSET } ); @@ -1028,9 +1026,9 @@ namespace librealsense int color_height = usb3mode ? 720 : 480; int fps = usb3mode ? 30 : 15; - tags.push_back({ RS2_STREAM_COLOR, -1, color_width, color_height, RS2_FORMAT_RGB8, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); + tags.push_back({ RS2_STREAM_COLOR, -1, color_width, color_height, get_color_format(), fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({ RS2_STREAM_DEPTH, -1, depth_width, depth_height, RS2_FORMAT_Z16, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); - tags.push_back({ RS2_STREAM_INFRARED, -1, depth_width, depth_height, RS2_FORMAT_Y8, fps, profile_tag::PROFILE_TAG_SUPERSET }); + tags.push_back({ RS2_STREAM_INFRARED, -1, depth_width, depth_height, get_ir_format(), fps, profile_tag::PROFILE_TAG_SUPERSET }); tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_100, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT }); diff --git a/third-party/realdds/include/realdds/dds-stream-server.h b/third-party/realdds/include/realdds/dds-stream-server.h index 564b4374ad5..1e16bcba0f0 100644 --- a/third-party/realdds/include/realdds/dds-stream-server.h +++ b/third-party/realdds/include/realdds/dds-stream-server.h @@ -85,8 +85,8 @@ class dds_video_stream_server : public dds_stream_server void open( std::string const & topic_name, std::shared_ptr< dds_publisher > const & ) override; - void set_intrinsics( const std::set< video_intrinsics > & intrinsics ) { _intrinsics = intrinsics; } - const std::set< video_intrinsics > & get_intrinsics() const { return _intrinsics; } + void set_intrinsics( const video_intrinsics & intrinsics ) { _intrinsics = intrinsics; } + video_intrinsics const & get_intrinsics() const { return _intrinsics; } void start_streaming( const image_header & ); void stop_streaming() override; @@ -97,7 +97,7 @@ class dds_video_stream_server : public dds_stream_server private: void check_profile( std::shared_ptr< dds_stream_profile > const & ) const override; - std::set< video_intrinsics > _intrinsics; + video_intrinsics _intrinsics; image_header _image_header; }; diff --git a/third-party/realdds/include/realdds/dds-stream.h b/third-party/realdds/include/realdds/dds-stream.h index cc4571297ba..faaac267c4a 100644 --- a/third-party/realdds/include/realdds/dds-stream.h +++ b/third-party/realdds/include/realdds/dds-stream.h @@ -68,14 +68,14 @@ class dds_video_stream : public dds_stream typedef std::function< void( topics::image_msg && f ) > on_data_available_callback; void on_data_available( on_data_available_callback cb ) { _on_data_available = cb; } - void set_intrinsics( const std::set< video_intrinsics > & intrinsics ) { _intrinsics = intrinsics; } - const std::set< video_intrinsics > & get_intrinsics() const { return _intrinsics; } + void set_intrinsics( const video_intrinsics & intrinsics ) { _intrinsics = intrinsics; } + const video_intrinsics & get_intrinsics() const { return _intrinsics; } protected: void handle_data() override; bool can_start_streaming() const override { return _on_data_available != nullptr; } - std::set< video_intrinsics > _intrinsics; + video_intrinsics _intrinsics; on_data_available_callback _on_data_available = nullptr; }; diff --git a/third-party/realdds/include/realdds/dds-trinsics.h b/third-party/realdds/include/realdds/dds-trinsics.h index 15a58b03f10..9115b857aa4 100644 --- a/third-party/realdds/include/realdds/dds-trinsics.h +++ b/third-party/realdds/include/realdds/dds-trinsics.h @@ -1,41 +1,72 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2023 Intel Corporation. All Rights Reserved. +// Copyright(c) 2023-4 Intel Corporation. All Rights Reserved. #pragma once #include +#include #include #include #include #include +#include namespace realdds { +// Like rs2_distortion +// +enum class distortion_model +{ + none = 0, // No un/distort + brown, + + // These are to support librealsense legacy devices (for use in the adapter) + modified_brown, + inverse_brown +}; + +std::ostream & operator<<( std::ostream &, distortion_model ); + + +struct distortion_parameters +{ + distortion_model model; + std::array< float, 5 > coeffs; +}; + +std::ostream & operator<<( std::ostream &, distortion_parameters const & ); + + + // Internal properties of a video stream. // The sensor has a lens and thus has properties such as a focal point, distortion, and principal point. struct video_intrinsics { + using float2 = rsutils::number::float2; + int width = 0; // Horizontal resolution, in pixels int height = 0; // Vertical resolution, in pixels - float principal_point_x = 0.0f; // Pixel offset from the left edge - float principal_point_y = 0.0f; // Pixel offset from the top edge - float focal_lenght_x = 0.0f; // As a multiple of pixel width - float focal_lenght_y = 0.0f; // As a multiple of pixel height - int distortion_model = 0; // Distortion model of the image - std::array< float, 5 > distortion_coeffs = { 0 }; // Distortion model coefficients - - bool is_valid() const { return focal_lenght_x > 0 && focal_lenght_y > 0; } + float2 principal_point = { 0, 0 }; // Pixel offset from top-left edge + float2 focal_length = { 0, 0 }; // As a multiple of pixel width and height + distortion_parameters distortion = { distortion_model::none, { 0 } }; + + bool is_valid() const { return focal_length.x > 0 && focal_length.y > 0; } bool operator<( const video_intrinsics & rhs ) const { return width < rhs.width || ( width == rhs.width && height < rhs.height ); } + video_intrinsics scaled_to( int width, int height ) const; + rsutils::json to_json() const; static video_intrinsics from_json( rsutils::json const & j ); }; +std::ostream & operator<<( std::ostream &, video_intrinsics const & ); + + // Internal properties of a motion stream. // Contain scale, bias and variances for each dimension. struct motion_intrinsics diff --git a/third-party/realdds/include/realdds/topics/dds-topic-names.h b/third-party/realdds/include/realdds/topics/dds-topic-names.h index 41a58d094bf..b83e430ff0b 100644 --- a/third-party/realdds/include/realdds/topics/dds-topic-names.h +++ b/third-party/realdds/include/realdds/topics/dds-topic-names.h @@ -66,6 +66,13 @@ namespace notification { namespace key { extern std::string const accel; extern std::string const gyro; + extern std::string const width; + extern std::string const height; + extern std::string const principal_point; + extern std::string const focal_length; + extern std::string const model; + extern std::string const coefficients; + extern std::string const force_symmetry; } } } diff --git a/third-party/realdds/py/pyrealdds.cpp b/third-party/realdds/py/pyrealdds.cpp index 0dcb74aadbf..379c3fe26db 100644 --- a/third-party/realdds/py/pyrealdds.cpp +++ b/third-party/realdds/py/pyrealdds.cpp @@ -256,17 +256,47 @@ PYBIND11_MODULE(NAME, m) { topics.attr( "device_metadata" ) = realdds::topics::METADATA_TOPIC_NAME; topics.attr( "device_dfu" ) = realdds::topics::DFU_TOPIC_NAME; + using realdds::distortion_model; + py::enum_< distortion_model >( m, "distortion_model" ) + .value( "none", distortion_model::none ) + .value( "brown", distortion_model::brown ) + .value( "inverse_brown", distortion_model::inverse_brown ) + .value( "modified_brown", distortion_model::modified_brown ); + + using realdds::distortion_parameters; + py::class_< distortion_parameters >( m, "distortion_parameters" ) + .def( py::init<>() ) + .def_readwrite( "model", &distortion_parameters::model ) + .def_readwrite( "coeffs", &distortion_parameters::coeffs ) + .def( "__repr__", + []( distortion_parameters const & self ) -> std::string + { return rsutils::string::from() << "<" SNAME ".distortion_parameters " << self << ">"; } ); + + py::class_< rsutils::number::float2 >( m, "float2" ) + .def( py::init<>() ) + .def( py::init< float, float >() ) + .def_readwrite( "x", &rsutils::number::float2::x ) + .def_readwrite( "y", &rsutils::number::float2::y ) + .def( "length", &rsutils::number::float2::length ) + .def( "normalized", &rsutils::number::float2::normalized ) + .def( "__repr__", + []( rsutils::number::float2 const & self ) -> std::string + { return rsutils::string::from() << self.x << ',' << self.y; } ); + using realdds::video_intrinsics; py::class_< video_intrinsics, std::shared_ptr< video_intrinsics > >( m, "video_intrinsics" ) .def( py::init<>() ) .def_readwrite( "width", &video_intrinsics::width ) .def_readwrite( "height", &video_intrinsics::height ) - .def_readwrite( "principal_point_x", &video_intrinsics::principal_point_x ) - .def_readwrite( "principal_point_y", &video_intrinsics::principal_point_y ) - .def_readwrite( "focal_lenght_x", &video_intrinsics::focal_lenght_x ) - .def_readwrite( "focal_lenght_y", &video_intrinsics::focal_lenght_y ) - .def_readwrite( "distortion_model", &video_intrinsics::distortion_model ) - .def_readwrite( "distortion_coeffs", &video_intrinsics::distortion_coeffs ); + .def_readwrite( "principal_point", &video_intrinsics::principal_point ) + .def_readwrite( "focal_length", &video_intrinsics::focal_length ) + .def_readwrite( "distortion", &video_intrinsics::distortion ) + .def( "__repr__", + []( video_intrinsics const & self ) -> std::string + { return rsutils::string::from() << "<" SNAME ".video_intrinsics " << self << ">"; } ) + .def( "to_json", &video_intrinsics::to_json ) + .def( "scaled_to", &video_intrinsics::scaled_to ) + .def( "is_valid", &video_intrinsics::is_valid ); using realdds::motion_intrinsics; py::class_< motion_intrinsics, std::shared_ptr< motion_intrinsics > >( m, "motion_intrinsics" ) diff --git a/third-party/realdds/src/dds-device-impl.cpp b/third-party/realdds/src/dds-device-impl.cpp index 58fb3504f5f..b6b03ef65e6 100644 --- a/third-party/realdds/src/dds-device-impl.cpp +++ b/third-party/realdds/src/dds-device-impl.cpp @@ -673,8 +673,8 @@ void dds_device::impl::on_stream_options( json const & j, eprosima::fastdds::dds auto & stream_name = j.at( topics::notification::stream_options::key::stream_name ).string_ref(); auto stream_it = _streams.find( stream_name ); if( stream_it == _streams.end() ) - DDS_THROW( runtime_error, - "Received stream options for stream '" << stream_name << "' whose header was not received yet" ); + DDS_THROW( runtime_error, "stream '" << stream_name << "' options received out of order" ); + auto stream = stream_it->second; if( auto options_j = j.nested( topics::notification::stream_options::key::options ) ) { @@ -686,19 +686,36 @@ void dds_device::impl::on_stream_options( json const & j, eprosima::fastdds::dds options.push_back( option ); } - stream_it->second->init_options( options ); + stream->init_options( options ); } if( auto j_int = j.nested( topics::notification::stream_options::key::intrinsics ) ) { - if( auto video_stream = std::dynamic_pointer_cast< dds_video_stream >( stream_it->second ) ) + if( auto video_stream = std::dynamic_pointer_cast< dds_video_stream >( stream ) ) { - std::set< video_intrinsics > intrinsics; - for( auto & intr : j_int ) - intrinsics.insert( video_intrinsics::from_json( intr ) ); - video_stream->set_intrinsics( std::move( intrinsics ) ); + video_intrinsics intrinsics; + if( j_int.is_array() ) + { + // Old-style intrinsics; multiple resolutions are provided, but we pick only one + // TODO remove + LOG_WARNING( "[" << debug_name() << "] stream '" << stream->name() << "' is using old-style intrinsics format" ); + auto default_profile = std::dynamic_pointer_cast< dds_video_stream_profile >( video_stream->default_profile() ); + if( ! default_profile ) + DDS_THROW( runtime_error, "no default profile available to choose proper intrinsics" ); + for( auto & intr : j_int ) + { + intrinsics = video_intrinsics::from_json( intr ); + if( intrinsics.width == default_profile->width() && intrinsics.height == default_profile->height() ) + break; + } + } + else + { + intrinsics = video_intrinsics::from_json( j_int ); + } + video_stream->set_intrinsics( intrinsics ); } - else if( auto motion_stream = std::dynamic_pointer_cast< dds_motion_stream >( stream_it->second ) ) + else if( auto motion_stream = std::dynamic_pointer_cast< dds_motion_stream >( stream ) ) { motion_stream->set_accel_intrinsics( motion_intrinsics::from_json( j_int.at( topics::notification::stream_options::intrinsics::key::accel ) ) ); @@ -715,7 +732,7 @@ void dds_device::impl::on_stream_options( json const & j, eprosima::fastdds::dds filter_names.push_back( filter ); } - stream_it->second->set_recommended_filters( std::move( filter_names ) ); + stream->set_recommended_filters( std::move( filter_names ) ); } if( _streams.size() >= _n_streams_expected ) diff --git a/third-party/realdds/src/dds-device-server.cpp b/third-party/realdds/src/dds-device-server.cpp index 6bd62c426ae..6f065422155 100644 --- a/third-party/realdds/src/dds-device-server.cpp +++ b/third-party/realdds/src/dds-device-server.cpp @@ -124,20 +124,23 @@ static void on_discovery_stream_header( std::shared_ptr< dds_stream_server > con //LOG_DEBUG( "-----> CBOR size = " << json::to_cbor( stream_header_message.json_data() ).size() ); notifications.add_discovery_notification( std::move( stream_header_message ) ); - auto stream_options = json::array(); + json j_stream_options = json::object( { + { topics::notification::key::id, topics::notification::stream_options::id }, + { topics::notification::stream_options::key::stream_name, stream->name() }, + } ); + + auto & j_options = j_stream_options[topics::notification::stream_options::key::options] = json::array(); for( auto & opt : stream->options() ) - stream_options.push_back( std::move( opt->to_json() ) ); + j_options.push_back( std::move( opt->to_json() ) ); - json intrinsics; + auto & j_intrinsics = j_stream_options[topics::notification::stream_options::key::intrinsics]; if( auto video_stream = std::dynamic_pointer_cast< dds_video_stream_server >( stream ) ) { - intrinsics = json::array(); - for( auto & intr : video_stream->get_intrinsics() ) - intrinsics.push_back( intr.to_json() ); + j_intrinsics = video_stream->get_intrinsics().to_json(); } else if( auto motion_stream = std::dynamic_pointer_cast< dds_motion_stream_server >( stream ) ) { - intrinsics = json::object( { + j_intrinsics = json::object( { { topics::notification::stream_options::intrinsics::key::accel, motion_stream->get_accel_intrinsics().to_json() }, { topics::notification::stream_options::intrinsics::key::gyro, @@ -145,16 +148,11 @@ static void on_discovery_stream_header( std::shared_ptr< dds_stream_server > con } ); } - auto stream_filters = json::array(); + auto & j_filters = j_stream_options[topics::notification::stream_options::key::recommended_filters] = json::array(); for( auto & filter : stream->recommended_filters() ) - stream_filters.push_back( filter ); - topics::flexible_msg stream_options_message( json::object( { - { topics::notification::key::id, topics::notification::stream_options::id }, - { topics::notification::stream_options::key::stream_name, stream->name() }, - { topics::notification::stream_options::key::options, std::move( stream_options ) }, - { topics::notification::stream_options::key::intrinsics, std::move( intrinsics ) }, - { topics::notification::stream_options::key::recommended_filters, std::move( stream_filters ) }, - } ) ); + j_filters.push_back( filter ); + + topics::flexible_msg stream_options_message( std::move( j_stream_options ) ); json_string = slice( stream_options_message.custom_data< char const >(), stream_options_message._data.size() ); LOG_DEBUG( "-----> JSON = " << shorten_json_string( json_string, 300 ) << " size " << json_string.length() ); //LOG_DEBUG( "-----> CBOR size = " << json::to_cbor( stream_options_message.json_data() ).size() ); diff --git a/third-party/realdds/src/dds-trinsics.cpp b/third-party/realdds/src/dds-trinsics.cpp index e9ff1d16528..03b202606d3 100644 --- a/third-party/realdds/src/dds-trinsics.cpp +++ b/third-party/realdds/src/dds-trinsics.cpp @@ -1,50 +1,194 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2023 Intel Corporation. All Rights Reserved. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. #include #include +#include #include +using rsutils::json; + +#include namespace realdds { -rsutils::json video_intrinsics::to_json() const +std::ostream & operator<<( std::ostream & os, distortion_model model ) +{ + switch( model ) + { + case distortion_model::none: + os << "none"; + break; + + case distortion_model::brown: + os << "brown"; + break; + + case distortion_model::inverse_brown: + os << "inverse-brown"; + break; + + case distortion_model::modified_brown: + os << "modified-brown"; + break; + + default: + DDS_THROW( runtime_error, "unknown distortion model: " << int( model ) ); + } + return os; +} + + +std::ostream & operator<<( std::ostream & os, distortion_parameters const & p ) +{ + os << p.model; + switch( p.model ) + { + case distortion_model::brown: + case distortion_model::inverse_brown: + case distortion_model::modified_brown: + os << ":" << p.coeffs[0] << ',' << p.coeffs[1] << ',' << p.coeffs[2] << ',' << p.coeffs[3] << ',' << p.coeffs[4]; + break; + } + return os; +} + + +std::ostream & operator<<( std::ostream & os, video_intrinsics const & self ) +{ + return os << self.width << 'x' << self.height << " pp[" << self.principal_point.x << ',' << self.principal_point.y + << "] fl[" << self.focal_length.x << ',' << self.focal_length.y << "] distortion[" << self.distortion << "]"; +} + + +json video_intrinsics::to_json() const { - return rsutils::json::array( { - width, height, principal_point_x, principal_point_y, focal_lenght_x, focal_lenght_y, distortion_model, - distortion_coeffs[0], distortion_coeffs[1], distortion_coeffs[2], distortion_coeffs[3], distortion_coeffs[4] + json j = json::object( { + { topics::notification::stream_options::intrinsics::key::width, width }, + { topics::notification::stream_options::intrinsics::key::height, height }, + { topics::notification::stream_options::intrinsics::key::principal_point, principal_point }, + { topics::notification::stream_options::intrinsics::key::focal_length, focal_length }, } ); + switch( distortion.model ) + { + case distortion_model::none: + break; + + case distortion_model::brown: + j[topics::notification::stream_options::intrinsics::key::coefficients] = distortion.coeffs; + break; + + case distortion_model::inverse_brown: + j[topics::notification::stream_options::intrinsics::key::model] = "inverse-brown"; + j[topics::notification::stream_options::intrinsics::key::coefficients] = distortion.coeffs; + break; + + case distortion_model::modified_brown: + j[topics::notification::stream_options::intrinsics::key::model] = "modified-brown"; + j[topics::notification::stream_options::intrinsics::key::coefficients] = distortion.coeffs; + break; + + default: + DDS_THROW( runtime_error, "unknown distortion model: " << int( distortion.model ) ); + } + return j; } -/* static */ video_intrinsics video_intrinsics::from_json( rsutils::json const & j ) +/* static */ video_intrinsics video_intrinsics::from_json( json const & j ) { video_intrinsics ret; - int index = 0; + if( j.is_array() ) + { + int index = 0; - ret.width = j[index++].get< int >(); - ret.height = j[index++].get< int >(); - ret.principal_point_x = j[index++].get< float >(); - ret.principal_point_y = j[index++].get< float >(); - ret.focal_lenght_x = j[index++].get< float >(); - ret.focal_lenght_y = j[index++].get< float >(); - ret.distortion_model = j[index++].get< int >(); - ret.distortion_coeffs[0] = j[index++].get< float >(); - ret.distortion_coeffs[1] = j[index++].get< float >(); - ret.distortion_coeffs[2] = j[index++].get< float >(); - ret.distortion_coeffs[3] = j[index++].get< float >(); - ret.distortion_coeffs[4] = j[index++].get< float >(); + ret.width = j[index++].get< int >(); + ret.height = j[index++].get< int >(); + ret.principal_point.x = j[index++].get< float >(); + ret.principal_point.y = j[index++].get< float >(); + ret.focal_length.x = j[index++].get< float >(); + ret.focal_length.y = j[index++].get< float >(); + ret.distortion.model = (distortion_model)j[index++].get< int >(); + ret.distortion.coeffs[0] = j[index++].get< float >(); + ret.distortion.coeffs[1] = j[index++].get< float >(); + ret.distortion.coeffs[2] = j[index++].get< float >(); + ret.distortion.coeffs[3] = j[index++].get< float >(); + ret.distortion.coeffs[4] = j[index++].get< float >(); - if( index != j.size() ) - DDS_THROW( runtime_error, "expected end of json at index " + std::to_string( index ) ); + if( index != j.size() ) + DDS_THROW( runtime_error, "expected end of json at index " << index ); + } + else if( j.is_object() ) + { + // The intrinsics are communicated in an object: + // - A `width` and `height` are for the native stream resolution, as 16 - bit integer values + // - A `principal-point` defined as `[, ]` floating point values + // - A `focal-length`, also as `[, ]` floats + j.nested( topics::notification::stream_options::intrinsics::key::width ).get_to( ret.width ); + j.nested( topics::notification::stream_options::intrinsics::key::height ).get_to( ret.height ); + j.nested( topics::notification::stream_options::intrinsics::key::principal_point ).get_to( ret.principal_point ); + j.nested( topics::notification::stream_options::intrinsics::key::focal_length ).get_to( ret.focal_length ); + // A distortion model may be applied: + // - The `model` would specify which model is to be used, with the default of `brown` + // - The `coefficients` is an array of floating point values, the number and meaning which depend on the `model` + // - For `brown`, 5 points [k1, k2, p1, p2, k3] are needed + auto coeffs_j = j.nested( topics::notification::stream_options::intrinsics::key::coefficients ); + auto model_j = j.nested( topics::notification::stream_options::intrinsics::key::model ); + switch( model_j.type() ) + { + case json::value_t::discarded: + if( coeffs_j.is_array() && coeffs_j.size() == 5 ) + ret.distortion.model = distortion_model::brown; + else + ret.distortion.model = distortion_model::none; + break; + + case json::value_t::string: + if( "brown" == model_j.string_ref() ) + ret.distortion.model = distortion_model::brown; + else if( "none" == model_j.string_ref() ) + ret.distortion.model = distortion_model::none; + else if( "inverse-brown" == model_j.string_ref() ) + ret.distortion.model = distortion_model::inverse_brown; + else if( "modified-brown" == model_j.string_ref() ) + ret.distortion.model = distortion_model::modified_brown; + else + DDS_THROW( runtime_error, "unknown distortion model: " << model_j ); + break; + + default: + DDS_THROW( runtime_error, "invalid distortion model: " << model_j ); + } + switch( ret.distortion.model ) + { + case distortion_model::none: + if( coeffs_j ) + DDS_THROW( runtime_error, "distortion coefficients without a model" ); + ret.distortion.coeffs = { 0 }; + break; + + case distortion_model::brown: + if( ! coeffs_j.is_array() ) + DDS_THROW( runtime_error, "invalid distortion coefficients: " << coeffs_j ); + if( coeffs_j.size() != 5 ) + DDS_THROW( runtime_error, "distortion coefficients expected [k1,k2,p1,p2,k3]: " << coeffs_j ); + coeffs_j.get_to( ret.distortion.coeffs ); + break; + + default: + DDS_THROW( runtime_error, "invalid distortion model" ); + } + } + else + DDS_THROW( runtime_error, "unspected intrinsics json: " << j ); return ret; } -rsutils::json motion_intrinsics::to_json() const +json motion_intrinsics::to_json() const { - return rsutils::json::array( { + return json::array( { data[0][0], data[0][1], data[0][2], data[0][3], data[1][0], data[1][1], data[1][2], data[1][3], data[2][0], data[2][1], data[2][2], data[2][3], @@ -53,7 +197,7 @@ rsutils::json motion_intrinsics::to_json() const } ); } -/* static */ motion_intrinsics motion_intrinsics::from_json( rsutils::json const & j ) +/* static */ motion_intrinsics motion_intrinsics::from_json( json const & j ) { motion_intrinsics ret; int index = 0; @@ -83,9 +227,9 @@ rsutils::json motion_intrinsics::to_json() const return ret; } -rsutils::json extrinsics::to_json() const +json extrinsics::to_json() const { - return rsutils::json::array( { + return json::array( { rotation[0], rotation[1], rotation[2], rotation[3], rotation[4], rotation[5], rotation[6], rotation[7], rotation[8], @@ -93,7 +237,7 @@ rsutils::json extrinsics::to_json() const } ); } -/* static */ extrinsics extrinsics::from_json( rsutils::json const & j ) +/* static */ extrinsics extrinsics::from_json( json const & j ) { extrinsics ret; int index = 0; @@ -118,4 +262,41 @@ rsutils::json extrinsics::to_json() const } +video_intrinsics video_intrinsics::scaled_to( int const new_width, int const new_height ) const +{ + if( ! width || ! height ) + DDS_THROW( runtime_error, "cannot scale with 0 width/height" ); + + video_intrinsics scaled( *this ); + + bool const force_symmetry = false; // TODO + if( force_symmetry ) + { + // Cropping the frame so that the principal point is at the middle + scaled.width = 1 + 2 * static_cast< int >( std::min( principal_point.x, width - 1 - principal_point.x ) ); + scaled.height = 1 + 2 * static_cast< int >( std::min( principal_point.y, height - 1 - principal_point.y ) ); + scaled.principal_point.x = ( scaled.width - 1 ) / 2.f; + scaled.principal_point.y = ( scaled.height - 1 ) / 2.f; + } + + auto const scale_ratio_x = static_cast< float >( new_width ) / scaled.width; + auto const scale_ratio_y = static_cast< float >( new_height ) / scaled.height; + auto const scale_ratio = std::max( scale_ratio_x, scale_ratio_y ); + + auto const crop_x = ( scaled.width * scale_ratio - new_width ) * 0.5f; + auto const crop_y = ( scaled.height * scale_ratio - new_height ) * 0.5f; + + scaled.width = new_width; + scaled.height = new_height; + + scaled.principal_point.x = ( scaled.principal_point.x + 0.5f ) * scale_ratio - crop_x - 0.5f; + scaled.principal_point.y = ( scaled.principal_point.y + 0.5f ) * scale_ratio - crop_y - 0.5f; + + scaled.focal_length.x *= scale_ratio; + scaled.focal_length.y *= scale_ratio; + + return scaled; +} + + } // namespace realdds diff --git a/third-party/realdds/src/topics/dds-topic-names.cpp b/third-party/realdds/src/topics/dds-topic-names.cpp index aeaf9161780..e12ab97476c 100644 --- a/third-party/realdds/src/topics/dds-topic-names.cpp +++ b/third-party/realdds/src/topics/dds-topic-names.cpp @@ -48,6 +48,13 @@ namespace notification { namespace key { std::string const accel( "accel", 5 ); std::string const gyro( "gyro", 4 ); + std::string const width( "width", 5 ); + std::string const height( "height", 6 ); + std::string const principal_point( "principal-point", 15 ); + std::string const focal_length( "focal-length", 12 ); + std::string const model( "model", 5 ); + std::string const coefficients( "coefficients", 12 ); + std::string const force_symmetry( "force-symmetry", 14 ); } } } diff --git a/third-party/rsutils/include/rsutils/number/float3.h b/third-party/rsutils/include/rsutils/number/float3.h index 4d341a43b76..6d73b88fb8b 100644 --- a/third-party/rsutils/include/rsutils/number/float3.h +++ b/third-party/rsutils/include/rsutils/number/float3.h @@ -2,6 +2,8 @@ // Copyright(c) 2024 Intel Corporation. All Rights Reserved. #pragma once +#include + #include #include @@ -142,5 +144,14 @@ std::ostream & operator<<( std::ostream &, const float4 & ); std::ostream & operator<<( std::ostream &, const float3x3 & ); +// Allow j["key"] = hexarray( bytes ); +void to_json( json &, float2 const & ); +void to_json( json &, float3 const & ); +// Allow j.get< hexarray >(); +void from_json( json const &, float2 & ); +void from_json( json const &, float3 & ); +// See https://github.com/nlohmann/json#arbitrary-types-conversions + + } // namespace number } // namespace rsutils \ No newline at end of file diff --git a/third-party/rsutils/src/float3.cpp b/third-party/rsutils/src/float3.cpp index 02f978676c3..4ee112bd5a8 100644 --- a/third-party/rsutils/src/float3.cpp +++ b/third-party/rsutils/src/float3.cpp @@ -2,10 +2,13 @@ // Copyright(c) 2024 Intel Corporation. All Rights Reserved. #include +#include #include // sqrt, sqrtf #include +using rsutils::json; + namespace rsutils { namespace number { @@ -59,5 +62,36 @@ float3 float3::normalized() const } +void to_json( json & j, float2 const & f2 ) +{ + j = json::array( { f2.x, f2.y } ); +} + + +void to_json( json & j, float3 const & f3 ) +{ + j = json::array( { f3.x, f3.y, f3.z } ); +} + + +void from_json( json const & j, float2 & f2 ) +{ + if( ! j.is_array() || 2 != j.size() ) + throw rsutils::json::type_error::create( 317, "expected float2 array [x,y]", &j ); + j[0].get_to( f2.x ); + j[1].get_to( f2.y ); +} + + +void from_json( json const & j, float3 & f3 ) +{ + if( ! j.is_array() || 3 != j.size() ) + throw rsutils::json::type_error::create( 317, "expected float3 array [x,y,z]", &j ); + j[0].get_to( f3.x ); + j[1].get_to( f3.y ); + j[2].get_to( f3.z ); +} + + } // namespace number } // namespace rsutils diff --git a/tools/dds/dds-adapter/lrs-device-controller.cpp b/tools/dds/dds-adapter/lrs-device-controller.cpp index 1dfe8d8fa5c..691e3c239e8 100644 --- a/tools/dds/dds-adapter/lrs-device-controller.cpp +++ b/tools/dds/dds-adapter/lrs-device-controller.cpp @@ -53,18 +53,40 @@ using tools::lrs_device_controller; break +realdds::distortion_model to_realdds( rs2_distortion model ) +{ + switch( model ) + { + case RS2_DISTORTION_BROWN_CONRADY: return realdds::distortion_model::brown; + case RS2_DISTORTION_NONE: return realdds::distortion_model::none; + case RS2_DISTORTION_INVERSE_BROWN_CONRADY: return realdds::distortion_model::inverse_brown; + case RS2_DISTORTION_MODIFIED_BROWN_CONRADY: return realdds::distortion_model::modified_brown; + + default: + throw std::runtime_error( "unexpected rs2 distortion model: " + std::string( rs2_distortion_to_string( model ) ) ); + } +} + realdds::video_intrinsics to_realdds( const rs2_intrinsics & intr ) { realdds::video_intrinsics ret; ret.width = intr.width; ret.height = intr.height; - ret.principal_point_x = intr.ppx; - ret.principal_point_y = intr.ppy; - ret.focal_lenght_x = intr.fx; - ret.focal_lenght_y = intr.fy; - ret.distortion_model = intr.model; - memcpy( ret.distortion_coeffs.data(), intr.coeffs, sizeof( ret.distortion_coeffs ) ); + ret.principal_point.x = intr.ppx; + ret.principal_point.y = intr.ppy; + ret.focal_length.x = intr.fx; + ret.focal_length.y = intr.fy; + ret.distortion.model = realdds::distortion_model::none; + for( auto coeff : intr.coeffs ) + { + if( coeff != 0.f ) + { + ret.distortion.model = to_realdds( intr.model ); + memcpy( ret.distortion.coeffs.data(), intr.coeffs, sizeof( ret.distortion.coeffs ) ); + break; + } + } return ret; } @@ -130,7 +152,7 @@ std::vector< std::shared_ptr< realdds::dds_stream_server > > lrs_device_controll { std::map< std::string, realdds::dds_stream_profiles > stream_name_to_profiles; std::map< std::string, size_t > stream_name_to_default_profile; - std::map< std::string, std::set< realdds::video_intrinsics > > stream_name_to_video_intrinsics; + std::map< std::string, realdds::video_intrinsics > stream_name_to_video_intrinsics; // Iterate over all profiles of all sensors and build appropriate dds_stream_servers for( auto & sensor : _rs_dev.query_sensors() ) @@ -172,12 +194,25 @@ std::vector< std::shared_ptr< realdds::dds_stream_server > > lrs_device_controll realdds::dds_video_encoding::from_rs2( vsp.format() ), static_cast< uint16_t >( vsp.width() ), static_cast< int16_t >( vsp.height() ) ); - try + if( vsp.is_default() ) + { + try + { + auto intr = to_realdds( vsp.get_intrinsics() ); + LOG_DEBUG( "=============> " << profiles.size() << " default " << intr.to_json().dump( 4 ) ); + stream_name_to_video_intrinsics[stream_name] = intr; + } + catch( ... ) {} //Some profiles don't have intrinsics + } + else { - auto intr = to_realdds( vsp.get_intrinsics() ); - stream_name_to_video_intrinsics[stream_name].insert( intr ); + try + { + auto intr = to_realdds( vsp.get_intrinsics() ); + LOG_DEBUG( "-------------> " << profiles.size() << " " << intr.to_json().dump( 4 ) ); + } + catch( ... ) {} //Some profiles don't have intrinsics } - catch( ... ) {} //Some profiles don't have intrinsics } else if( auto const msp = rs2::motion_stream_profile( sp ) ) { @@ -202,8 +237,9 @@ std::vector< std::shared_ptr< realdds::dds_stream_server > > lrs_device_controll { if( sp.is_default() ) stream_name_to_default_profile[stream_name] = profiles.size(); + LOG_DEBUG( stream_name << " " << profiles.size() << ": " << profile->to_string() + << ( sp.is_default() ? " default" : "" ) ); profiles.push_back( profile ); - LOG_DEBUG( stream_name << ": " << profile->to_string() ); } } ); } diff --git a/unit-tests/dds/d435i.py b/unit-tests/dds/d435i.py index 3823cbbd077..eed41846253 100644 --- a/unit-tests/dds/d435i.py +++ b/unit-tests/dds/d435i.py @@ -315,100 +315,100 @@ def color_stream_intrinsics(): intr = dds.video_intrinsics(); intr.width = 320 intr.height = 180 - intr.principal_point_x = 161.7417755126953 - intr.principal_point_y = 90.47455596923828 - intr.focal_lenght_x = 227.0221710205078 - intr.focal_lenght_y = 227.1049346923828 - intr.distortion_model = 2 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 161.7417755126953 + intr.principal_point.y = 90.47455596923828 + intr.focal_length.x = 227.0221710205078 + intr.focal_length.y = 227.1049346923828 + intr.distortion.model = dds.distortion_model.inverse_brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 320 intr.height = 240 - intr.principal_point_x = 162.32237243652344 - intr.principal_point_y = 120.63274383544922 - intr.focal_lenght_x = 302.69622802734375 - intr.focal_lenght_y = 302.80657958984375 - intr.distortion_model = 2 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 162.32237243652344 + intr.principal_point.y = 120.63274383544922 + intr.focal_length.x = 302.69622802734375 + intr.focal_length.y = 302.80657958984375 + intr.distortion.model = dds.distortion_model.inverse_brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 424 intr.height = 240 - intr.principal_point_x = 214.32235717773438 - intr.principal_point_y = 120.63274383544922 - intr.focal_lenght_x = 302.69622802734375 - intr.focal_lenght_y = 302.80657958984375 - intr.distortion_model = 2 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 214.32235717773438 + intr.principal_point.y = 120.63274383544922 + intr.focal_length.x = 302.69622802734375 + intr.focal_length.y = 302.80657958984375 + intr.distortion.model = dds.distortion_model.inverse_brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 640 intr.height = 360 - intr.principal_point_x = 323.4835510253906 - intr.principal_point_y = 180.94911193847656 - intr.focal_lenght_x = 454.0443420410156 - intr.focal_lenght_y = 454.2098693847656 - intr.distortion_model = 2 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 323.4835510253906 + intr.principal_point.y = 180.94911193847656 + intr.focal_length.x = 454.0443420410156 + intr.focal_length.y = 454.2098693847656 + intr.distortion.model = dds.distortion_model.inverse_brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 640 intr.height = 480 - intr.principal_point_x = 324.6447448730469 - intr.principal_point_y = 241.26548767089844 - intr.focal_lenght_x = 605.3924560546875 - intr.focal_lenght_y = 605.6131591796875 - intr.distortion_model = 2 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 324.6447448730469 + intr.principal_point.y = 241.26548767089844 + intr.focal_length.x = 605.3924560546875 + intr.focal_length.y = 605.6131591796875 + intr.distortion.model = dds.distortion_model.inverse_brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 848 intr.height = 480 - intr.principal_point_x = 428.64471435546875 - intr.principal_point_y = 241.26548767089844 - intr.focal_lenght_x = 605.3924560546875 - intr.focal_lenght_y = 605.6131591796875 - intr.distortion_model = 2 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 428.64471435546875 + intr.principal_point.y = 241.26548767089844 + intr.focal_length.x = 605.3924560546875 + intr.focal_length.y = 605.6131591796875 + intr.distortion.model = dds.distortion_model.inverse_brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 960 intr.height = 540 - intr.principal_point_x = 485.2253112792969 - intr.principal_point_y = 271.4236755371094 - intr.focal_lenght_x = 681.0665283203125 - intr.focal_lenght_y = 681.3148193359375 - intr.distortion_model = 2 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 485.2253112792969 + intr.principal_point.y = 271.4236755371094 + intr.focal_length.x = 681.0665283203125 + intr.focal_length.y = 681.3148193359375 + intr.distortion.model = dds.distortion_model.inverse_brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 1280 intr.height = 720 - intr.principal_point_x = 646.9671020507813 - intr.principal_point_y = 361.8982238769531 - intr.focal_lenght_x = 908.0886840820313 - intr.focal_lenght_y = 908.4197387695313 - intr.distortion_model = 2 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 646.9671020507813 + intr.principal_point.y = 361.8982238769531 + intr.focal_length.x = 908.0886840820313 + intr.focal_length.y = 908.4197387695313 + intr.distortion.model = dds.distortion_model.inverse_brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 1920 intr.height = 1080 - intr.principal_point_x = 970.4506225585938 - intr.principal_point_y = 542.8473510742188 - intr.focal_lenght_x = 1362.133056640625 - intr.focal_lenght_y = 1362.629638671875 - intr.distortion_model = 2 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 970.4506225585938 + intr.principal_point.y = 542.8473510742188 + intr.focal_length.x = 1362.133056640625 + intr.focal_length.y = 1362.629638671875 + intr.distortion.model = dds.distortion_model.inverse_brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) return set( intrinsics ) @@ -420,79 +420,76 @@ def depth_ir_common_intrinsics(): intr = dds.video_intrinsics(); intr.width = 424 intr.height = 240 - intr.principal_point_x = 212.0788116455078 - intr.principal_point_y = 119.07991790771484 - intr.focal_lenght_x = 209.13233947753906 - intr.focal_lenght_y = 209.13233947753906 - intr.distortion_model = 4 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 212.0788116455078 + intr.principal_point.y = 119.07991790771484 + intr.focal_length.x = 209.13233947753906 + intr.focal_length.y = 209.13233947753906 + intr.distortion.model = dds.distortion_model.brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 480 intr.height = 270 - intr.principal_point_x = 240.08921813964844 - intr.principal_point_y = 134.00367736816406 - intr.focal_lenght_x = 236.7535858154297 - intr.focal_lenght_y = 236.7535858154297 - intr.distortion_model = 4 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 240.08921813964844 + intr.principal_point.y = 134.00367736816406 + intr.focal_length.x = 236.7535858154297 + intr.focal_length.y = 236.7535858154297 + intr.distortion.model = dds.distortion_model.brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 640 intr.height = 360 - intr.principal_point_x = 320.11895751953125 - intr.principal_point_y = 178.67156982421875 - intr.focal_lenght_x = 315.67144775390625 - intr.focal_lenght_y = 315.67144775390625 - intr.distortion_model = 4 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 320.11895751953125 + intr.principal_point.y = 178.67156982421875 + intr.focal_length.x = 315.67144775390625 + intr.focal_length.y = 315.67144775390625 + intr.distortion.model = dds.distortion_model.brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 640 intr.height = 480 - intr.principal_point_x = 320.14276123046875 - intr.principal_point_y = 238.4058837890625 - intr.focal_lenght_x = 378.80572509765625 - intr.focal_lenght_y = 378.80572509765625 - intr.distortion_model = 4 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 320.14276123046875 + intr.principal_point.y = 238.4058837890625 + intr.focal_length.x = 378.80572509765625 + intr.focal_length.y = 378.80572509765625 + intr.distortion.model = dds.distortion_model.brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 848 intr.height = 100 - intr.principal_point_x = 424.1576232910156 - intr.principal_point_y = 48.239837646484375 - intr.focal_lenght_x = 418.2646789550781 - intr.focal_lenght_y = 418.2646789550781 - intr.distortion_model = 4 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 424.1576232910156 + intr.principal_point.y = 48.239837646484375 + intr.focal_length.x = 418.2646789550781 + intr.focal_length.y = 418.2646789550781 + intr.distortion.model = dds.distortion_model.brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 848 intr.height = 480 - intr.principal_point_x = 424.1576232910156 - intr.principal_point_y = 238.23983764648438 - intr.focal_lenght_x = 418.2646789550781 - intr.focal_lenght_y = 418.2646789550781 - intr.distortion_model = 4 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 424.1576232910156 + intr.principal_point.y = 238.23983764648438 + intr.focal_length.x = 418.2646789550781 + intr.focal_length.y = 418.2646789550781 + intr.distortion.model = dds.distortion_model.brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intr = dds.video_intrinsics(); intr.width = 1280 intr.height = 720 - intr.principal_point_x = 640.2379150390625 - intr.principal_point_y = 357.3431396484375 - intr.focal_lenght_x = 631.3428955078125 - intr.focal_lenght_y = 631.3428955078125 - intr.distortion_model = 4 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] - intrinsics.append( intr ) + intr.principal_point.x = 640.2379150390625 + intr.principal_point.y = 357.3431396484375 + intr.focal_length.x = 631.3428955078125 + intr.focal_length.y = 631.3428955078125 return intrinsics @@ -503,12 +500,12 @@ def depth_stream_intrinsics(): intr = dds.video_intrinsics(); intr.width = 256 intr.height = 144 - intr.principal_point_x = 128.2379150390625 - intr.principal_point_y = 69.3431396484375 - intr.focal_lenght_x = 631.3428955078125 - intr.focal_lenght_y = 631.3428955078125 - intr.distortion_model = 4 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 128.2379150390625 + intr.principal_point.y = 69.3431396484375 + intr.focal_length.x = 631.3428955078125 + intr.focal_length.y = 631.3428955078125 + intr.distortion.model = dds.distortion_model.brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) intrinsics.extend( depth_ir_common_intrinsics() ) @@ -522,12 +519,12 @@ def ir_stream_intrinsics(): intr = dds.video_intrinsics(); intr.width = 1280 intr.height = 800 - intr.principal_point_x = 640.2379150390625 - intr.principal_point_y = 397.3431396484375 - intr.focal_lenght_x = 631.3428955078125 - intr.focal_lenght_y = 631.3428955078125 - intr.distortion_model = 4 - intr.distortion_coeffs = [0.0,0.0,0.0,0.0,0.0] + intr.principal_point.x = 640.2379150390625 + intr.principal_point.y = 397.3431396484375 + intr.focal_length.x = 631.3428955078125 + intr.focal_length.y = 631.3428955078125 + intr.distortion.model = dds.distortion_model.brown + intr.distortion.coeffs = [0.0,0.0,0.0,0.0,0.0] intrinsics.append( intr ) return set( intrinsics )