Enable rotate frames for RGB camera

include/librealsense2/h/rs_processing.h

@@ -228,7 +228,7 @@ rs2_processing_block* rs2_create_decimation_filter_block(rs2_error** error);
  * Creates post-processing filter block. This block accepts frames and applies rotation filter
  * \param[out] error  if non-null, receives any error that occurs during this call, otherwise, errors are ignored
  */
-rs2_processing_block * rs2_create_rotation_filter_block( rs2_error ** error );
+rs2_processing_block * rs2_create_rotation_filter_block( rs2_streams_list streams_to_rotate, rs2_error ** error );
 
 /**
 * Creates Depth post-processing filter block. This block accepts depth frames, applies temporal filter

include/librealsense2/h/rs_types.h

@@ -254,6 +254,7 @@ typedef struct rs2_sensor_list rs2_sensor_list;
 typedef struct rs2_sensor rs2_sensor;
 typedef struct rs2_options rs2_options;
 typedef struct rs2_options_list rs2_options_list;
+typedef struct rs2_streams_list rs2_streams_list;
 typedef struct rs2_options_changed_callback rs2_options_changed_callback;
 typedef struct rs2_devices_changed_callback rs2_devices_changed_callback;
 typedef struct rs2_notification rs2_notification;

include/librealsense2/hpp/rs_processing.hpp

@@ -859,12 +859,17 @@ namespace rs2
          * Rotation filter performs rotation of the frames
          */
         rotation_filter()
-            : filter( init(), 1 )
+            : filter( init( std::vector< rs2_stream >{ RS2_STREAM_DEPTH } ), 1 )
         {
         }
 
-        rotation_filter( float value )
-            : filter( init(), 1 )
+        rotation_filter( std::vector< rs2_stream > streams_to_rotate )
+            : filter( init( streams_to_rotate ), 1 )
+        {
+        }
+
+        rotation_filter( std::vector< rs2_stream > streams_to_rotate, float value )
+            : filter( init( streams_to_rotate ), 1 )
         {
             set_option( RS2_OPTION_ROTATION, value );
         }
@@ -883,10 +888,14 @@ namespace rs2
     private:
         friend class context;
 
-        std::shared_ptr< rs2_processing_block > init()
+        std::shared_ptr< rs2_processing_block > init( std::vector< rs2_stream > streams_to_rotate )
         {
             rs2_error * e = nullptr;
-            auto block = std::shared_ptr< rs2_processing_block >( rs2_create_rotation_filter_block( &e ),
+
+            rs2_streams_list streams_list;
+            streams_list.list = std::move( streams_to_rotate ); 
+
+            auto block = std::shared_ptr< rs2_processing_block >( rs2_create_rotation_filter_block( streams_list, &e ),
                                                                   rs2_delete_processing_block );
             error::handle( e );
             return block;

include/librealsense2/hpp/rs_types.hpp

@@ -31,6 +31,11 @@ struct rs2_frame_callback
 };
 typedef std::shared_ptr<rs2_frame_callback> rs2_frame_callback_sptr;
 
+struct rs2_streams_list 
+{
+    std::vector< rs2_stream > list;
+};
+
 struct rs2_frame_processor_callback
 {
     virtual void                            on_frame(rs2_frame * f, rs2_source * source) = 0;

src/proc/rotation-filter.cpp

@@ -6,6 +6,7 @@
 #include "stream.h"
 #include "core/video.h"
 #include "proc/rotation-filter.h"
+#include <rsutils/easylogging/easyloggingpp.h>
 
 namespace librealsense {
 
@@ -16,8 +17,13 @@ namespace librealsense {
 
 
     rotation_filter::rotation_filter()
+        : rotation_filter( std::vector< rs2_stream >{ RS2_STREAM_DEPTH } )
+    {
+    }
+
+    rotation_filter::rotation_filter( std::vector< rs2_stream > streams_to_rotate )
         : stream_filter_processing_block( "Rotation Filter" )
-        , _streams_to_rotate()
+        , _streams_to_rotate( streams_to_rotate )
         , _control_val( rotation_default_val )
         , _real_width( 0 )
         , _real_height( 0 )
@@ -40,44 +46,25 @@ namespace librealsense {
                 if( ! strong_rotation_control )
                     return;
 
-                std::lock_guard< std::mutex > lock( _mutex );
-
                 if( ! strong_rotation_control->is_valid( val ) )
                     throw invalid_value_exception( rsutils::string::from()
                                                    << "Unsupported rotation scale " << val << " is out of range." );
 
+                std::lock_guard< std::mutex > lock( _mutex );
                 _value = val;
             } );
 
-        register_option( RS2_OPTION_ROTATION, rotation_control ); 
+        register_option( RS2_OPTION_ROTATION, rotation_control );
     }
 
     rs2::frame rotation_filter::process_frame(const rs2::frame_source& source, const rs2::frame& f)
     {
-        if( _value == rotation_default_val )
+        if( _value == rotation_default_val || _streams_to_rotate.empty() )
             return f;
 
-        rs2::stream_profile profile = f.get_profile();
-        rs2_stream type = profile.stream_type();
-        rs2_extension tgt_type;
-        if( type == RS2_STREAM_INFRARED )
-        {
-            tgt_type = RS2_EXTENSION_VIDEO_FRAME;
-        }
-        else if( f.is< rs2::disparity_frame >() )
-        {
-            tgt_type = RS2_EXTENSION_DISPARITY_FRAME;
-        }
-        else if( f.is< rs2::depth_frame >() )
-        {
-            tgt_type = RS2_EXTENSION_DEPTH_FRAME;
-        }
-        else
-        {
-            return f;
-        }
-
         auto src = f.as< rs2::video_frame >();
+        rs2::stream_profile profile = f.get_profile();
+        auto format = profile.format();
         _target_stream_profile = profile;
 
         if( _value == 90 || _value == -90 )
@@ -92,30 +79,35 @@ namespace librealsense {
         }
         auto bpp = src.get_bytes_per_pixel();
         update_output_profile( f );
+        rs2_stream type = profile.stream_type();
+        rs2_extension tgt_type;
+        if( type == RS2_STREAM_COLOR || type == RS2_STREAM_INFRARED )
+            tgt_type = RS2_EXTENSION_VIDEO_FRAME;
+        else
+            tgt_type = f.is< rs2::disparity_frame >() ? RS2_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME;
 
         if (auto tgt = prepare_target_frame(f, source, tgt_type))
         {
-            switch( bpp )
+            if( format == RS2_FORMAT_YUYV && ( _value == 90 || _value == -90 ) )
             {
-            case 1: {
-                rotate_frame< 1 >( static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ),
-                                   static_cast< const uint8_t * >( src.get_data() ),
-                                   src.get_width(),
-                                   src.get_height() );
-                break;
+                LOG_ERROR( "Rotating YUYV format is disabled for 90 or -90 degrees" );
+                return f;
             }
 
-            case 2: {
-                rotate_frame< 2 >( static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ),
-                                   static_cast< const uint8_t * >( src.get_data() ) ,
+            if( format == RS2_FORMAT_YUYV )
+            {
+                rotate_YUYV_frame( static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ),
+                                   static_cast< const uint8_t * >( src.get_data() ),
                                    src.get_width(),
-                                   src.get_height());
-                break;
+                                   src.get_height() );
             }
-
-            default:
-                LOG_ERROR( "Rotation transform does not support format: "
-                           + std::string( rs2_format_to_string( tgt.get_profile().format() ) ) );
+            else
+            {
+                rotate_frame( static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ),
+                              static_cast< const uint8_t * >( src.get_data() ),
+                              src.get_width(),
+                              src.get_height(),
+                              bpp );
             }
             return tgt;
         }
@@ -180,8 +172,7 @@ namespace librealsense {
         return ret;
     }
 
-    template< size_t SIZE >
-    void rotation_filter::rotate_frame( uint8_t * const out, const uint8_t * source, int width, int height )
+    void rotation_filter::rotate_frame( uint8_t * const out, const uint8_t * source, int width, int height, int bpp )
     {
         if( _value != 90 && _value != -90 && _value != 180 )
         {
@@ -193,33 +184,64 @@ namespace librealsense {
         int height_out = ( _value == 90 || _value == -90 ) ? width : height;  // rotate by 180 will keep the values as is
 
         // Perform rotation
-        for( int i = 0; i < height; ++i ) 
+        for( int i = 0; i < height; ++i )
         {
             for( int j = 0; j < width; ++j )
             {
                 // Calculate source index
-                size_t src_index = ( i * width + j ) * SIZE;
+                size_t src_index = ( i * width + j ) * bpp;
 
                 // Determine output index based on rotation angle
                 size_t out_index;
                 if( _value == 90 )
                 {
-                    out_index = ( j * height + ( height - i - 1 ) ) * SIZE;
+                    out_index = ( j * height + ( height - i - 1 ) ) * bpp;
                 }
                 else if( _value == -90 )
                 {
-                    out_index = ( ( width - j - 1 ) * height + i ) * SIZE;
+                    out_index = ( ( width - j - 1 ) * height + i ) * bpp;
                 }
                 else  // 180 degrees
                 {
-                    out_index = ( ( height - i - 1 ) * width + ( width - j - 1 ) ) * SIZE;
+                    out_index = ( ( height - i - 1 ) * width + ( width - j - 1 ) ) * bpp;
                 }
 
                 // Copy pixel data from source to destination
-                std::memcpy( &out[out_index], &source[src_index], SIZE );
+                std::memcpy( &out[out_index], &source[src_index], bpp );
             }
         }
     }
+
+
+    void rotation_filter::rotate_YUYV_frame( uint8_t * const out, const uint8_t * source, int width, int height ) 
+    {
+        int frame_size = width * height * 2;  // YUYV has 2 bytes per pixel
+        for( int i = 0; i < frame_size; i += 4 )
+        {
+            // Find the corresponding flipped position in the output
+            int flipped_index = frame_size - 4 - i;
+
+            // Copy YUYV pixels in reverse order to rotate 180 degrees
+            out[flipped_index] = source[i];          // Y1
+            out[flipped_index + 1] = source[i + 1];  // U
+            out[flipped_index + 2] = source[i + 2];  // Y2
+            out[flipped_index + 3] = source[i + 3];  // V
+        }
+    }
+
+
+    bool rotation_filter::should_process( const rs2::frame & frame )
+    {
+        if( ! frame || frame.is< rs2::frameset >() )
+            return false;
+        auto type = frame.get_profile().stream_type();
+
+        for( auto & stream_type : _streams_to_rotate ){
+            if( stream_type == type )
+                return true;
+        }
+        return false;
+    }
 
 }
 

src/proc/rotation-filter.h

@@ -14,19 +14,20 @@ namespace librealsense
     {
     public:
         rotation_filter();
+        rotation_filter( std::vector< rs2_stream > streams_to_rotate );
 
     protected:
         rs2::frame prepare_target_frame(const rs2::frame& f, const rs2::frame_source& source, rs2_extension tgt_type);
-
-        template< size_t SIZE >
-        void rotate_frame( uint8_t * const out, const uint8_t * source, int width, int height );
+        void rotate_frame( uint8_t * const out, const uint8_t * source, int width, int height, int bpp );
+        void rotate_YUYV_frame( uint8_t * const out, const uint8_t * source, int width, int height );
 
         rs2::frame process_frame(const rs2::frame_source& source, const rs2::frame& f) override;
+        bool should_process( const rs2::frame & frame ) override;
 
     private:
         void    update_output_profile(const rs2::frame& f);
 
-        std::vector< stream_filter > _streams_to_rotate;
+        std::vector< rs2_stream > _streams_to_rotate;
         int                       _control_val;
         rs2::stream_profile       _source_stream_profile;
         rs2::stream_profile       _target_stream_profile;

src/rs.cpp

@@ -2784,9 +2784,9 @@ rs2_processing_block* rs2_create_decimation_filter_block(rs2_error** error) BEGI
 }
 NOARGS_HANDLE_EXCEPTIONS_AND_RETURN(nullptr)
 
-rs2_processing_block * rs2_create_rotation_filter_block( rs2_error ** error ) BEGIN_API_CALL
+rs2_processing_block* rs2_create_rotation_filter_block( rs2_streams_list streams_to_rotate, rs2_error ** error ) BEGIN_API_CALL
 {
-    auto block = std::make_shared< librealsense::rotation_filter >();
+    auto block = std::make_shared< librealsense::rotation_filter >( streams_to_rotate.list );
 
     return new rs2_processing_block{ block };
 }

src/sensor.cpp

@@ -249,6 +249,9 @@ void log_callback_end( uint32_t fps,
         dec->get_option(RS2_OPTION_STREAM_FILTER).set(RS2_STREAM_COLOR);
         dec->get_option(RS2_OPTION_STREAM_FORMAT_FILTER).set(RS2_FORMAT_ANY);
         res.push_back(dec);
+        std::vector< rs2_stream > streams_to_rotate;
+        streams_to_rotate.push_back( RS2_STREAM_COLOR );
+        res.push_back( std::make_shared< rotation_filter >( streams_to_rotate ) );
         return res;
     }
 
@@ -263,7 +266,11 @@ void log_callback_end( uint32_t fps,
             dec->get_option(RS2_OPTION_STREAM_FORMAT_FILTER).set(RS2_FORMAT_Z16);
             res.push_back(dec);
         }
-        res.push_back( std::make_shared< rotation_filter >( ));
+
+        std::vector< rs2_stream > streams_to_rotate;
+        streams_to_rotate.push_back( RS2_STREAM_DEPTH);
+        streams_to_rotate.push_back( RS2_STREAM_INFRARED );
+        res.push_back( std::make_shared< rotation_filter >( streams_to_rotate ) );
         return res;
     }
 

unit-tests/post-processing/test-rotation-filter.py

@@ -104,7 +104,7 @@ def validate_rotation_results(filtered_frame, angle):
     # Define rotation angles to test
     rotation_angles = [90, 180, -90]
     for angle in rotation_angles:
-        rotation_filter = rs.rotation_filter()
+        rotation_filter = rs.rotation_filter([rs.stream.depth])
         rotation_filter.set_option(rs.option.rotation, angle)
 
         # Start depth sensor

wrappers/python/pyrs_processing.cpp

@@ -3,6 +3,7 @@ Copyright(c) 2017 Intel Corporation. All Rights Reserved. */
 
 #include "pyrealsense2.h"
 #include <librealsense2/hpp/rs_processing.hpp>
+#include <memory>
 
 void init_processing(py::module &m) {
     /** rs_processing.hpp **/
@@ -159,7 +160,7 @@ void init_processing(py::module &m) {
         .def(py::init<float>(), "magnitude"_a);
 
     py::class_< rs2::rotation_filter, rs2::filter > rotation_filter(m, "rotation_filter","Performs rotation of frames." );
-    rotation_filter.def( py::init<>() ).def( py::init< float >(), "value"_a );
+    rotation_filter.def( py::init<>() ).def( py::init< std::vector< rs2_stream > >(), "value"_a );
 
     py::class_<rs2::temporal_filter, rs2::filter> temporal_filter(m, "temporal_filter", "Temporal filter smooths the image by calculating multiple frames "
                                                                   "with alpha and delta settings. Alpha defines the weight of current frame, and delta defines the"