PR #11850 from Ohad: DDS formats conversion

src/dds/rs-dds-device-proxy.cpp

@@ -136,17 +136,14 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< context > ctx, std::shared_
         int sensor_index = 0;
     };
     std::map< std::string, sensor_info > sensor_name_to_info;
-    // We assign (sid,index) based on the stream type:
-    // LibRS assigns streams names based on the type followed by an index if it's not 0.
-    // I.e., sid is based on the type, and index is 0 unless there's more than 1 in which case it's 1-based.
-    int counts[RS2_STREAM_COUNT] = { 0 };
-    // Count how many streams per type
-    _dds_dev->foreach_stream( [&]( std::shared_ptr< realdds::dds_stream > const & stream )
-                              { ++counts[to_rs2_stream_type( stream->type_string() )]; } );
-    // Anything that's more than 1 stream starts the count at 1, otherwise 0
-    for( int & count : counts )
-        count = ( count > 1 ) ? 1 : 0;
-    // Now we can finally assign (sid,index):
+
+    // dds_streams bear stream type and index information, we add it to a dds_sensor_proxy mapped by a newly generated
+    // unique ID. After the sensor initialization we get all the "final" profiles from formats-converter with type and
+    // index but without IDs. We need to find the dds_stream that each profile was created from so we create a map from
+    // type and index to dds_stream ID and index, because the dds_sensor_proxy holds a map from sidx to dds_stream. We
+    // need both the ID from that map key and the stream itself (for intrinsics information)
+    std::map< sid_index, sid_index > type_and_index_to_dds_stream_sidx;
+
     _dds_dev->foreach_stream(
         [&]( std::shared_ptr< realdds::dds_stream > const & stream )
         {
@@ -160,11 +157,14 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< context > ctx, std::shared_
                 _software_sensors.push_back( sensor_info.proxy );
             }
             auto stream_type = to_rs2_stream_type( stream->type_string() );
-            sid_index sidx( stream_type, counts[stream_type]++ );
+            int index = get_index_from_stream_name( stream->name() );
+            sid_index sidx( environment::get_instance().generate_stream_id(), index);
+            sid_index type_and_index( stream_type, index );
             _stream_name_to_librs_stream[stream->name()]
                 = std::make_shared< librealsense::stream >( stream_type, sidx.index );
             sensor_info.proxy->add_dds_stream( sidx, stream );
             _stream_name_to_owning_sensor[stream->name()] = sensor_info.proxy;
+            type_and_index_to_dds_stream_sidx.insert( { type_and_index, sidx }  );
             LOG_DEBUG( sidx.to_string() << " " << stream->sensor_name() << " : " << stream->name() );
 
             software_sensor & sensor = get_software_sensor( sensor_info.sensor_index );
@@ -209,6 +209,32 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< context > ctx, std::shared_
             }
         } );  // End foreach_stream lambda
 
+    for( auto & sensor_info : sensor_name_to_info )
+    {
+        sensor_info.second.proxy->initialization_done( dev->device_info().product_id, dev->device_info().product_line );
+
+        // Set profile's ID based on the dds_stream's ID (index already set). Connect the profile to the extrinsics graph.
+        for( auto & profile : sensor_info.second.proxy->get_stream_profiles() )
+        {
+            sid_index type_and_index( profile->get_stream_type(), profile->get_stream_index());
+
+            auto & streams = sensor_info.second.proxy->streams();
+
+            sid_index sidx = type_and_index_to_dds_stream_sidx[type_and_index];
+            auto stream_iter = streams.find( sidx );
+            if( stream_iter == streams.end() )
+            {
+                LOG_DEBUG( "Did not find dds_stream of profile (" << profile->get_stream_type() << ", "
+                                                                  << profile->get_stream_index() << ")" );
+                continue;
+            }
+
+            profile->set_unique_id( stream_iter->first.sid );
+            set_profile_intrinsics( profile, stream_iter->second );
+
+            _stream_name_to_profiles[stream_iter->second->name()].push_back( profile );  // For extrinsics
+        }
+    }
 
     if( _dds_dev->supports_metadata() )
     {
@@ -261,6 +287,74 @@ dds_device_proxy::dds_device_proxy( std::shared_ptr< context > ctx, std::shared_
 }
 
 
+int dds_device_proxy::get_index_from_stream_name( const std::string & name ) const
+{
+    int index = 0;
+
+    auto delimiter_pos = name.find( '_' );
+    if( delimiter_pos != std::string::npos )
+    {
+        std::string index_as_string = name.substr( delimiter_pos + 1, name.size() );
+        index = std::stoi( index_as_string );
+    }
+
+    return index;
+}
+
+
+void dds_device_proxy::set_profile_intrinsics( std::shared_ptr< stream_profile_interface > & profile,
+                                               const std::shared_ptr< realdds::dds_stream > & stream ) const
+{
+    if( auto video_stream = std::dynamic_pointer_cast< realdds::dds_video_stream >( stream ) )
+    {
+        set_video_profile_intrinsics( profile, video_stream );
+    }
+    else if( auto motion_stream = std::dynamic_pointer_cast< realdds::dds_motion_stream >( stream ) )
+    {
+        set_motion_profile_intrinsics( profile, motion_stream );
+    }
+}
+
+
+void dds_device_proxy::set_video_profile_intrinsics( std::shared_ptr< stream_profile_interface > profile,
+                                                     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; } );
+    }
+}
+
+
+void dds_device_proxy::set_motion_profile_intrinsics( std::shared_ptr< stream_profile_interface > profile,
+                                                      std::shared_ptr< realdds::dds_motion_stream > stream ) const
+{
+    auto msp = std::dynamic_pointer_cast< motion_stream_profile >( profile );
+    auto & stream_intrinsics = stream->get_intrinsics();
+    rs2_motion_device_intrinsic intr;
+    memcpy( intr.data, stream_intrinsics.data.data(), sizeof( intr.data ) );
+    memcpy( intr.noise_variances, stream_intrinsics.noise_variances.data(), sizeof( intr.noise_variances ) );
+    memcpy( intr.bias_variances, stream_intrinsics.bias_variances.data(), sizeof( intr.bias_variances ) );
+    msp->set_intrinsics( [intr]() { return intr; } );
+}
+
+
 std::shared_ptr< dds_sensor_proxy > dds_device_proxy::create_sensor( const std::string & sensor_name )
 {
     if( sensor_name.compare( "RGB Camera" ) == 0 )

src/dds/rs-dds-device-proxy.h

@@ -12,6 +12,9 @@
 
 namespace realdds {
 class dds_device;
+class dds_stream;
+class dds_video_stream;
+class dds_motion_stream;
 }  // namespace realdds
 
 
@@ -37,6 +40,14 @@ class dds_device_proxy : public software_device
     std::map< std::string, std::shared_ptr< librealsense::stream > > _stream_name_to_librs_stream;
     std::map< std::string, std::shared_ptr< dds_sensor_proxy > > _stream_name_to_owning_sensor;
 
+    int get_index_from_stream_name( const std::string & name ) const;
+    void set_profile_intrinsics( std::shared_ptr< stream_profile_interface > & profile,
+                                 const std::shared_ptr< realdds::dds_stream > & stream ) const;
+    void set_video_profile_intrinsics( std::shared_ptr< stream_profile_interface > profile,
+                                       std::shared_ptr< realdds::dds_video_stream > stream ) const;
+    void set_motion_profile_intrinsics( std::shared_ptr< stream_profile_interface > profile,
+                                       std::shared_ptr< realdds::dds_motion_stream > stream ) const;
+
 public:
     dds_device_proxy( std::shared_ptr< context > ctx, std::shared_ptr< realdds::dds_device > const & dev );
 

src/dds/rs-dds-internal-data.cpp

@@ -0,0 +1,178 @@
+// License: Apache 2.0. See LICENSE file in root directory.
+// Copyright(c) 2023 Intel Corporation. All Rights Reserved.
+
+#include "rs-dds-internal-data.h"
+
+#include <librealsense2/h/rs_sensor.h>
+#include <proc/processing-blocks-factory.h>
+#include <proc/color-formats-converter.h>
+#include <proc/depth-formats-converter.h>
+#include <proc/y8i-to-y8y8.h>
+#include <proc/y12i-to-y16y16.h>
+#include <proc/y12i-to-y16y16-mipi.h>
+
+namespace librealsense {
+
+static const std::string RS405_PID = "0B5B";
+static const std::string RS415_PID = "0AD3";
+static const std::string RS435_RGB_PID = "0B07";
+static const std::string RS435I_PID = "0B3A";
+static const std::string RS455_PID = "0B5C";
+static const std::string RS457_PID = "ABCD";
+
+
+std::vector< rs2_format > target_formats( rs2_format source_format )
+{
+    // Mapping from source color format to all of the compatible target color formats.
+    std::vector< rs2_format > formats = { RS2_FORMAT_RGB8, RS2_FORMAT_RGBA8, RS2_FORMAT_BGR8, RS2_FORMAT_BGRA8 };
+
+    switch( source_format )
+    {
+    case RS2_FORMAT_YUYV:
+        formats.push_back( RS2_FORMAT_YUYV );
+        formats.push_back( RS2_FORMAT_Y16 );
+        formats.push_back( RS2_FORMAT_Y8 );
+        break;
+    case RS2_FORMAT_UYVY:
+        formats.push_back( RS2_FORMAT_UYVY );
+        break;
+    default:
+        throw std::runtime_error( "Format is not supported for mapping" );
+    }
+
+    return formats;
+}
+
+std::vector< tagged_profile > dds_rs_internal_data::get_profiles_tags( const std::string & product_id,
+                                                                       const std::string & product_line )
+{
+    std::vector< tagged_profile > tags;
+
+    if( product_id == RS405_PID )
+    {
+        tags.push_back( { RS2_STREAM_COLOR, -1, 848, 480, RS2_FORMAT_RGB8, 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 if( product_id == RS415_PID )
+    {
+        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_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 if( product_id == RS435_RGB_PID )
+    {
+        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_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 if( product_id == RS435I_PID )
+    {
+        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_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 } );
+        tags.push_back( { RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } );
+        tags.push_back( { RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } );
+        tags.push_back( { RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 100, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } );
+    }
+    else if( product_id == RS455_PID )
+    {
+        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_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 } );
+        tags.push_back( { RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } );
+        tags.push_back( { RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } );
+        tags.push_back( { RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, 100, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } );
+    }
+    else if( product_id == RS457_PID )
+    {
+        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_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } );
+    }
+    else if( product_line == "D400" )
+    {
+        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_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } );
+        tags.push_back( { RS2_STREAM_INFRARED, 2, 1280, 720, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET } );
+    }
+
+    // For other devices empty tags will be returned, no default profile defined.
+
+    return tags;
+}
+
+
+std::vector< processing_block_factory > dds_rs_internal_data::get_profile_converters( const std::string & product_id,
+                                                                                      const std::string & product_line )
+{
+    std::vector< processing_block_factory > factories;
+    std::vector< processing_block_factory > tmp;
+
+    if( product_line == "D400" )
+    {
+        if( product_id == RS457_PID )
+        {
+            tmp = processing_block_factory::create_pbf_vector< uyvy_converter >( RS2_FORMAT_UYVY,
+                                                                                 target_formats( RS2_FORMAT_UYVY ),
+                                                                                 RS2_STREAM_COLOR );
+            for( auto & it : tmp )
+                factories.push_back( std::move( it ) );
+            tmp = processing_block_factory::create_pbf_vector< uyvy_converter >( RS2_FORMAT_YUYV,
+                                                                                 target_formats( RS2_FORMAT_YUYV ),
+                                                                                 RS2_STREAM_COLOR );
+            for( auto & it : tmp )
+                factories.push_back( std::move( it ) );
+            factories.push_back( { { { RS2_FORMAT_Y12I } },
+                                   { { RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 1 },
+                                     { RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 2 } },
+                                   []() { return std::make_shared< y12i_to_y16y16_mipi >(); } } );
+        }
+        else
+        {
+            tmp = processing_block_factory::create_pbf_vector< yuy2_converter >( RS2_FORMAT_YUYV,
+                                                                                 target_formats( RS2_FORMAT_YUYV ),
+                                                                                 RS2_STREAM_COLOR );
+            for( auto & it : tmp )
+                factories.push_back( std::move( it ) );
+            factories.push_back( processing_block_factory::create_id_pbf( RS2_FORMAT_RAW16, RS2_STREAM_COLOR ) );
+            factories.push_back( { { { RS2_FORMAT_Y12I } },
+                                   { { RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 1 },
+                                     { RS2_FORMAT_Y16, RS2_STREAM_INFRARED, 2 } },
+                                   []() { return std::make_shared< y12i_to_y16y16 >(); } } );
+        }
+        tmp = processing_block_factory::create_pbf_vector< uyvy_converter >(
+                               RS2_FORMAT_UYVY,
+                               target_formats( RS2_FORMAT_UYVY ),
+                               RS2_STREAM_INFRARED );
+        for( auto & it : tmp )
+            factories.push_back( std::move( it ) );
+        factories.push_back( processing_block_factory::create_id_pbf( RS2_FORMAT_Y8, RS2_STREAM_INFRARED, 1 ) );
+        factories.push_back( processing_block_factory::create_id_pbf( RS2_FORMAT_Z16, RS2_STREAM_DEPTH ) );
+        factories.push_back( { { { RS2_FORMAT_W10 } },
+                               { { RS2_FORMAT_RAW10, RS2_STREAM_INFRARED, 1 } },
+                               []() { return std::make_shared< w10_converter >( RS2_FORMAT_RAW10 ); } } );
+        factories.push_back( { { { RS2_FORMAT_W10 } },
+                               { { RS2_FORMAT_Y10BPACK, RS2_STREAM_INFRARED, 1 } },
+                               []() { return std::make_shared< w10_converter >( RS2_FORMAT_Y10BPACK ); } } );
+        factories.push_back( { { { RS2_FORMAT_Y8I } },
+                               { { RS2_FORMAT_Y8, RS2_STREAM_INFRARED, 1 },
+                                 { RS2_FORMAT_Y8, RS2_STREAM_INFRARED, 2 } },
+                               []() { return std::make_shared< y8i_to_y8y8 >(); } } );
+
+        // Motion convertion is done on the camera side.
+        // Intrinsics table is being read from the camera flash and it is too much to set here, this file is ment as a
+        // temporary solution, the camera should send all this data in a designated topic.
+        factories.push_back( { { { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL } },
+                               { { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL } },
+                               []() { return std::make_shared< identity_processing_block >(); } } );
+        factories.push_back( { { { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO } },
+                               { { RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO } },
+                               []() { return std::make_shared< identity_processing_block >(); } } );
+    }
+
+    // For other devices empty factories will be returned, raw profiles will be used.
+
+    return factories;
+}
+
+}  // namespace librealsense