From 58f262cbb770d897d37343da58fd8c9ce1bce78b Mon Sep 17 00:00:00 2001 From: noacoohen Date: Mon, 3 Feb 2025 08:39:56 +0200 Subject: [PATCH 1/2] add rotation ppf for RGB --- src/proc/rotation-filter.cpp | 141 ++++++++++++++++++++++++++++------- src/proc/rotation-filter.h | 6 +- src/sensor.cpp | 9 ++- 3 files changed, 129 insertions(+), 27 deletions(-) diff --git a/src/proc/rotation-filter.cpp b/src/proc/rotation-filter.cpp index c3654280ca..36207e25f8 100644 --- a/src/proc/rotation-filter.cpp +++ b/src/proc/rotation-filter.cpp @@ -6,6 +6,7 @@ #include "stream.h" #include "core/video.h" #include "proc/rotation-filter.h" +#include namespace librealsense { @@ -52,32 +53,51 @@ namespace librealsense { register_option( RS2_OPTION_ROTATION, rotation_control ); } + rotation_filter::rotation_filter( std::vector< rs2_stream > streams_to_rotate ) + : stream_filter_processing_block( "Rotation Filter" ) + , _streams_to_rotate( streams_to_rotate ) + , _control_val( rotation_default_val ) + , _real_width( 0 ) + , _real_height( 0 ) + , _rotated_width( 0 ) + , _rotated_height( 0 ) + , _value( 0 ) + { + auto rotation_control = std::make_shared< ptr_option< int > >( rotation_min_val, + rotation_max_val, + rotation_step, + rotation_default_val, + &_control_val, + "Rotation angle" ); + + auto weak_rotation_control = std::weak_ptr< ptr_option< int > >( rotation_control ); + rotation_control->on_set( + [this, weak_rotation_control]( float val ) + { + auto strong_rotation_control = weak_rotation_control.lock(); + 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." ); + + _value = val; + } ); + + 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 ) 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 = f.get_profile().format(); _target_stream_profile = profile; if( _value == 90 || _value == -90 ) @@ -92,6 +112,12 @@ 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)) { @@ -106,10 +132,42 @@ namespace librealsense { } 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 ) + { + if( _value == 90 || _value == -90 ) + { + LOG_ERROR( "Rotating YUYV format is disabled for 90 or -90 degrees" ); + return f; + } + 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() ); + } + else + { + rotate_frame< 2 >( 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; + } + case 3: { + rotate_frame< 3 >( static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ), + static_cast< const uint8_t * >( src.get_data() ), src.get_width(), - src.get_height()); + src.get_height() ); + break; + } + case 4: { + rotate_frame< 4 >( 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; } @@ -190,10 +248,11 @@ namespace librealsense { // Define output dimensions int width_out = ( _value == 90 || _value == -90 ) ? height : width; // rotate by 180 will keep the values as is - int height_out = ( _value == 90 || _value == -90 ) ? width : height; // rotate by 180 will keep the values as is + 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 ) { @@ -220,6 +279,38 @@ namespace librealsense { } } } + + + 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(); + /// change extrinsics + for( auto & stream_type : _streams_to_rotate ){ + if( stream_type == type ) + { + return true; + } + } + return false; + } } diff --git a/src/proc/rotation-filter.h b/src/proc/rotation-filter.h index f5a542e21d..4579204f2a 100644 --- a/src/proc/rotation-filter.h +++ b/src/proc/rotation-filter.h @@ -15,18 +15,22 @@ 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_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; diff --git a/src/sensor.cpp b/src/sensor.cpp index 686a4dead6..18006ba6a7 100644 --- a/src/sensor.cpp +++ b/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; } From 5ee7e4d7203a489fa0f8b7ca89c7fd76c514f26d Mon Sep 17 00:00:00 2001 From: noacoohen Date: Thu, 6 Feb 2025 16:30:02 +0200 Subject: [PATCH 2/2] fix PR comments --- include/librealsense2/h/rs_processing.h | 2 +- include/librealsense2/h/rs_types.h | 1 + include/librealsense2/hpp/rs_processing.hpp | 19 ++- include/librealsense2/hpp/rs_types.hpp | 5 + src/proc/rotation-filter.cpp | 123 ++++-------------- src/proc/rotation-filter.h | 5 +- src/rs.cpp | 4 +- .../post-processing/test-rotation-filter.py | 2 +- wrappers/python/pyrs_processing.cpp | 3 +- 9 files changed, 54 insertions(+), 110 deletions(-) diff --git a/include/librealsense2/h/rs_processing.h b/include/librealsense2/h/rs_processing.h index e13e1484a8..c3a4fd3bef 100644 --- a/include/librealsense2/h/rs_processing.h +++ b/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 diff --git a/include/librealsense2/h/rs_types.h b/include/librealsense2/h/rs_types.h index 5543fa00fe..e380002c13 100644 --- a/include/librealsense2/h/rs_types.h +++ b/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; diff --git a/include/librealsense2/hpp/rs_processing.hpp b/include/librealsense2/hpp/rs_processing.hpp index 3344a54860..daa0a2cf77 100644 --- a/include/librealsense2/hpp/rs_processing.hpp +++ b/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; diff --git a/include/librealsense2/hpp/rs_types.hpp b/include/librealsense2/hpp/rs_types.hpp index 26390ae07c..3f842926e5 100644 --- a/include/librealsense2/hpp/rs_types.hpp +++ b/include/librealsense2/hpp/rs_types.hpp @@ -31,6 +31,11 @@ struct rs2_frame_callback }; typedef std::shared_ptr 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; diff --git a/src/proc/rotation-filter.cpp b/src/proc/rotation-filter.cpp index 36207e25f8..b17ecaaf24 100644 --- a/src/proc/rotation-filter.cpp +++ b/src/proc/rotation-filter.cpp @@ -17,40 +17,8 @@ namespace librealsense { rotation_filter::rotation_filter() - : stream_filter_processing_block( "Rotation Filter" ) - , _streams_to_rotate() - , _control_val( rotation_default_val ) - , _real_width( 0 ) - , _real_height( 0 ) - , _rotated_width( 0 ) - , _rotated_height( 0 ) - , _value( 0 ) + : rotation_filter( std::vector< rs2_stream >{ RS2_STREAM_DEPTH } ) { - auto rotation_control = std::make_shared< ptr_option< int > >( rotation_min_val, - rotation_max_val, - rotation_step, - rotation_default_val, - &_control_val, - "Rotation angle" ); - - auto weak_rotation_control = std::weak_ptr< ptr_option< int > >( rotation_control ); - rotation_control->on_set( - [this, weak_rotation_control]( float val ) - { - auto strong_rotation_control = weak_rotation_control.lock(); - 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." ); - - _value = val; - } ); - - register_option( RS2_OPTION_ROTATION, rotation_control ); } rotation_filter::rotation_filter( std::vector< rs2_stream > streams_to_rotate ) @@ -78,12 +46,11 @@ 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; } ); @@ -92,12 +59,12 @@ namespace librealsense { 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; auto src = f.as< rs2::video_frame >(); rs2::stream_profile profile = f.get_profile(); - auto format = f.get_profile().format(); + auto format = profile.format(); _target_stream_profile = profile; if( _value == 90 || _value == -90 ) @@ -121,59 +88,26 @@ namespace librealsense { 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: { - if( format == RS2_FORMAT_YUYV ) - { - if( _value == 90 || _value == -90 ) - { - LOG_ERROR( "Rotating YUYV format is disabled for 90 or -90 degrees" ); - return f; - } - 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() ); - } - else - { - rotate_frame< 2 >( 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; - } - case 3: { - rotate_frame< 3 >( static_cast< uint8_t * >( const_cast< void * >( tgt.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; - } - case 4: { - rotate_frame< 4 >( 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; } - - 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; } @@ -238,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 ) { @@ -248,8 +181,7 @@ namespace librealsense { // Define output dimensions int width_out = ( _value == 90 || _value == -90 ) ? height : width; // rotate by 180 will keep the values as is - int height_out - = ( _value == 90 || _value == -90 ) ? width : height; // rotate by 180 will keep the values as is + 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 ) @@ -257,31 +189,32 @@ namespace librealsense { 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) { + 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 ) { @@ -302,12 +235,10 @@ namespace librealsense { if( ! frame || frame.is< rs2::frameset >() ) return false; auto type = frame.get_profile().stream_type(); - /// change extrinsics + for( auto & stream_type : _streams_to_rotate ){ if( stream_type == type ) - { return true; - } } return false; } diff --git a/src/proc/rotation-filter.h b/src/proc/rotation-filter.h index 4579204f2a..e8d3a52250 100644 --- a/src/proc/rotation-filter.h +++ b/src/proc/rotation-filter.h @@ -14,14 +14,11 @@ 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; diff --git a/src/rs.cpp b/src/rs.cpp index d274bdcd7e..ae3f83d425 100644 --- a/src/rs.cpp +++ b/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 }; } diff --git a/unit-tests/post-processing/test-rotation-filter.py b/unit-tests/post-processing/test-rotation-filter.py index 8b515c7f73..74011274bd 100644 --- a/unit-tests/post-processing/test-rotation-filter.py +++ b/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 diff --git a/wrappers/python/pyrs_processing.cpp b/wrappers/python/pyrs_processing.cpp index 5dd80f2f0a..f1bd3c2942 100644 --- a/wrappers/python/pyrs_processing.cpp +++ b/wrappers/python/pyrs_processing.cpp @@ -3,6 +3,7 @@ Copyright(c) 2017 Intel Corporation. All Rights Reserved. */ #include "pyrealsense2.h" #include +#include void init_processing(py::module &m) { /** rs_processing.hpp **/ @@ -159,7 +160,7 @@ void init_processing(py::module &m) { .def(py::init(), "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_ 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"