PR #14060 from OhadMeir: Cherry Pick PR #14015 from OhadMeir: Add UYVY to YUYV conversion for D457

Author: Nir-Az

src/cuda/cuda-conversion.cu

@@ -223,6 +223,34 @@ __global__ void kernel_unpack_yuy2_bgra8_cuda(const uint8_t * src, uint8_t *dst,
     }
 }
 
+__global__ void kernel_uyvy_to_yuyv_cuda(uint16_t* src, uint16_t* dst, int count)
+{
+    int i = blockDim.x * blockIdx.x + threadIdx.x;
+
+    if (i >= count)
+        return;
+
+    dst[i] = ( ( src[i] >> 8 ) & 0x00FF ) | ( ( src[i] << 8 ) & 0xFF00 );
+}
+
+void rscuda::uyvy_to_yuyv_cuda_helper(const uint16_t* src, uint16_t* dst, int count)
+{
+    int numBlocks = count / RS2_CUDA_THREADS_PER_BLOCK;
+    auto d_src = alloc_dev<uint16_t>(count);
+    auto d_dst = alloc_dev<uint16_t>(count);
+
+    auto result = cudaMemcpy(d_src.get(), src, count * sizeof(uint16_t), cudaMemcpyHostToDevice);
+    assert(result == cudaSuccess);
+
+    kernel_uyvy_to_yuyv_cuda <<<numBlocks, RS2_CUDA_THREADS_PER_BLOCK >>>(d_src.get(), d_dst.get(), count);
+    cudaStreamSynchronize(0);
+
+    result = cudaGetLastError();
+    assert(result == cudaSuccess);
+
+    result = cudaMemcpy(dst, d_dst.get(), count * sizeof(uint16_t), cudaMemcpyDeviceToHost);
+    assert(result == cudaSuccess);
+}
 
 void rscuda::unpack_yuy2_cuda_helper(const uint8_t* h_src, uint8_t* h_dst, int n, rs2_format format)
 {

src/cuda/cuda-conversion.cuh

@@ -33,6 +33,7 @@ namespace rscuda
     template<class SOURCE>
     void y16_y16_from_y12i_10_cuda_helper(uint8_t* const dest[], int count, const SOURCE * source);
     void unpack_yuy2_cuda_helper(const uint8_t* src, uint8_t* dst, int n, rs2_format format);
+    void uyvy_to_yuyv_cuda_helper(const uint16_t* src, uint16_t* dst, int n);
 
     template<rs2_format FORMAT> void unpack_yuy2_cuda(uint8_t * const d[], const uint8_t * s, int n)
     {

src/device.cpp

@@ -194,23 +194,25 @@ void device::register_stream_to_extrinsic_group(const stream_interface& stream,
     }
 }
 
-std::vector<rs2_format> device::map_supported_color_formats(rs2_format source_format)
+std::vector< rs2_format > device::map_supported_color_formats( rs2_format source_format, bool should_map_source_format )
 {
     // Mapping from source color format to all of the compatible target color formats.
 
     std::vector<rs2_format> target_formats = { RS2_FORMAT_RGB8, RS2_FORMAT_RGBA8, RS2_FORMAT_BGR8, RS2_FORMAT_BGRA8 };
     switch (source_format)
     {
     case RS2_FORMAT_YUYV:
-        target_formats.push_back(RS2_FORMAT_YUYV);
         target_formats.push_back(RS2_FORMAT_Y8);
         break;
     case RS2_FORMAT_UYVY:
-        target_formats.push_back(RS2_FORMAT_UYVY);
         break;
     default:
         LOG_ERROR("Format is not supported for mapping");
     }
+
+    if( should_map_source_format )
+        target_formats.push_back( source_format );
+
     return target_formats;
 }
 

src/device.h

@@ -94,7 +94,7 @@ class device
     int add_sensor(const std::shared_ptr<sensor_interface>& sensor_base);
     int assign_sensor(const std::shared_ptr<sensor_interface>& sensor_base, uint8_t idx);
     void register_stream_to_extrinsic_group(const stream_interface& stream, uint32_t groupd_index);
-    std::vector<rs2_format> map_supported_color_formats(rs2_format source_format);
+    std::vector<rs2_format> map_supported_color_formats(rs2_format source_format, bool should_map_source_format = true);
 
     explicit device( std::shared_ptr< const device_info > const &, bool device_changed_notifications = true );
 

src/ds/d400/d400-color.cpp

@@ -255,8 +255,14 @@ namespace librealsense
             auto uvc_dev = raw_color_ep->get_uvc_device();
             if (uvc_dev->is_platform_jetson())
             {
-                // Work-around for discrepancy between the RGB YUYV descriptor and the parser . Use UYUV parser instead
-                color_ep.register_processing_block(processing_block_factory::create_pbf_vector<uyvy_converter>(RS2_FORMAT_YUYV, map_supported_color_formats(RS2_FORMAT_YUYV), RS2_STREAM_COLOR));
+                // Work-around for discrepancy between the RGB YUYV descriptor and the parser. Use UYUV parser instead.
+                // Bytes are reveiced swapped, so YUYV format is received as UYVY.
+                color_ep.register_processing_block( processing_block_factory::create_pbf_vector< uyvy_converter >( RS2_FORMAT_YUYV,
+                                                                                                                   map_supported_color_formats( RS2_FORMAT_YUYV, false ),
+                                                                                                                   RS2_STREAM_COLOR ) );
+                color_ep.register_processing_block( { { RS2_FORMAT_YUYV } },
+                                                    { { RS2_FORMAT_YUYV, RS2_STREAM_COLOR } },
+                                                    []() { return std::make_shared< uyvy_to_yuyv >(); } );
             }
             else
             {

src/proc/color-formats-converter.cpp

@@ -1140,4 +1140,19 @@ namespace librealsense
     {
         unpack_m420(_target_format, _target_stream, dest, source, width, height, actual_size);
     }
+
+    void uyvy_to_yuyv::process_function( uint8_t * const dest[], const uint8_t * source, int width, int height, int actual_size, int input_size)
+    {
+        auto in = reinterpret_cast< const uint16_t * >( source );
+        auto out = reinterpret_cast< uint16_t * >( dest[0] );
+
+        // Time measurments on Jetson yielded better results for the non-CUDA version
+    //#ifdef RS2_USE_CUDA
+        //if( rsutils::rs2_is_gpu_available() )
+        //    rscuda::uyvy_to_yuyv_cuda_helper( in, out, width * height );
+    //#else
+        for( size_t i = 0; i < width * height; ++i )
+            out[i] = ( ( in[i] >> 8 ) & 0x00FF ) | ( ( in[i] << 8 ) & 0xFF00 );
+    //#endif
+    }
 }

src/proc/color-formats-converter.h

@@ -73,4 +73,17 @@ namespace librealsense
             color_converter(name, target_format) {};
         void process_function( uint8_t * const dest[], const uint8_t * source, int width, int height, int actual_size, int input_size) override;
     };
-}
+
+    class LRS_EXTENSION_API uyvy_to_yuyv : public color_converter
+    {
+    public:
+        uyvy_to_yuyv() : uyvy_to_yuyv( "UYVY to YUYV Converter" ) {};
+
+    protected:
+        uyvy_to_yuyv( const char * name ) : color_converter( name, RS2_FORMAT_YUYV, RS2_STREAM_COLOR )
+        {
+        };
+
+        void process_function( uint8_t * const dest[], const uint8_t * source, int width, int height, int actual_size, int input_size ) override;
+    };
+    }