From 964fd5aa87032da8e144ff38923f60530352d5dd Mon Sep 17 00:00:00 2001 From: noacoohen Date: Thu, 7 Nov 2024 17:23:58 +0200 Subject: [PATCH 1/8] rotate frames --- common/subdevice-model.cpp | 3 + include/librealsense2/h/rs_option.h | 1 + include/librealsense2/h/rs_processing.h | 6 + include/librealsense2/h/rs_types.h | 1 + include/librealsense2/hpp/rs_processing.hpp | 36 ++++ src/proc/CMakeLists.txt | 2 + src/proc/rotation-filter.cpp | 214 ++++++++++++++++++++ src/proc/rotation-filter.h | 41 ++++ src/realsense.def | 1 + src/rs.cpp | 10 + src/sensor.cpp | 2 + 11 files changed, 317 insertions(+) create mode 100644 src/proc/rotation-filter.cpp create mode 100644 src/proc/rotation-filter.h diff --git a/common/subdevice-model.cpp b/common/subdevice-model.cpp index 3a7914f6cb3..734a1d65b99 100644 --- a/common/subdevice-model.cpp +++ b/common/subdevice-model.cpp @@ -215,6 +215,9 @@ namespace rs2 model->enable(false); } + if( shared_filter->is< rotation_filter >() ) + model->enable( false ); + if (shared_filter->is()) { if (s->supports(RS2_CAMERA_INFO_PRODUCT_ID)) diff --git a/include/librealsense2/h/rs_option.h b/include/librealsense2/h/rs_option.h index 10d03fe2e8a..80c30bc8539 100644 --- a/include/librealsense2/h/rs_option.h +++ b/include/librealsense2/h/rs_option.h @@ -125,6 +125,7 @@ extern "C" { RS2_OPTION_SOC_PVT_TEMPERATURE, /**< Temperature of PVT SOC */ RS2_OPTION_GYRO_SENSITIVITY,/**< Control of the gyro sensitivity level, see rs2_gyro_sensitivity for values */ RS2_OPTION_REGION_OF_INTEREST,/**< The rectangular area used from the streaming profile */ + RS2_OPTION_ROTATION,/**Rotates frames*/ RS2_OPTION_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */ } rs2_option; diff --git a/include/librealsense2/h/rs_processing.h b/include/librealsense2/h/rs_processing.h index 745b72c54db..e13e1484a82 100644 --- a/include/librealsense2/h/rs_processing.h +++ b/include/librealsense2/h/rs_processing.h @@ -224,6 +224,12 @@ rs2_processing_block* rs2_create_align(rs2_stream align_to, rs2_error** error); */ 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 ); + /** * Creates Depth post-processing filter block. This block accepts depth frames, applies temporal filter * \param[out] error if non-null, receives any error that occurs during this call, otherwise, errors are ignored diff --git a/include/librealsense2/h/rs_types.h b/include/librealsense2/h/rs_types.h index 775fb5dd836..5543fa00fe8 100644 --- a/include/librealsense2/h/rs_types.h +++ b/include/librealsense2/h/rs_types.h @@ -161,6 +161,7 @@ typedef enum rs2_extension RS2_EXTENSION_SOFTWARE_DEVICE, RS2_EXTENSION_SOFTWARE_SENSOR, RS2_EXTENSION_DECIMATION_FILTER, + RS2_EXTENSION_ROTATION_FILTER, RS2_EXTENSION_THRESHOLD_FILTER, RS2_EXTENSION_DISPARITY_FILTER, RS2_EXTENSION_SPATIAL_FILTER, diff --git a/include/librealsense2/hpp/rs_processing.hpp b/include/librealsense2/hpp/rs_processing.hpp index 30a2181c50f..be2cc812c1c 100644 --- a/include/librealsense2/hpp/rs_processing.hpp +++ b/include/librealsense2/hpp/rs_processing.hpp @@ -851,6 +851,42 @@ namespace rs2 } }; + class rotation_filter : public filter + { + public: + /** + * Create rotation filter + * Rotation filter performs rotation of the frames + */ + rotation_filter() + : filter( init(), 1 ) + { + } + + rotation_filter( filter f ) + : filter( f ) + { + rs2_error * e = nullptr; + if( ! rs2_is_processing_block_extendable_to( f.get(), RS2_EXTENSION_ROTATION_FILTER, &e ) && ! e ) + { + _block.reset(); + } + error::handle( e ); + } + + private: + friend class context; + + std::shared_ptr< rs2_processing_block > init() + { + rs2_error * e = nullptr; + auto block = std::shared_ptr< rs2_processing_block >( rs2_create_rotation_filter_block( &e ), + rs2_delete_processing_block ); + error::handle( e ); + return block; + } + }; + class temporal_filter : public filter { public: diff --git a/src/proc/CMakeLists.txt b/src/proc/CMakeLists.txt index 1d37afe0af1..35784d69911 100644 --- a/src/proc/CMakeLists.txt +++ b/src/proc/CMakeLists.txt @@ -20,6 +20,7 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/synthetic-stream.cpp" "${CMAKE_CURRENT_LIST_DIR}/syncer-processing-block.cpp" "${CMAKE_CURRENT_LIST_DIR}/decimation-filter.cpp" + "${CMAKE_CURRENT_LIST_DIR}/rotation-filter.cpp" "${CMAKE_CURRENT_LIST_DIR}/spatial-filter.cpp" "${CMAKE_CURRENT_LIST_DIR}/temporal-filter.cpp" "${CMAKE_CURRENT_LIST_DIR}/hdr-merge.cpp" @@ -49,6 +50,7 @@ target_sources(${LRS_TARGET} "${CMAKE_CURRENT_LIST_DIR}/occlusion-filter.h" "${CMAKE_CURRENT_LIST_DIR}/synthetic-stream.h" "${CMAKE_CURRENT_LIST_DIR}/decimation-filter.h" + "${CMAKE_CURRENT_LIST_DIR}/rotation-filter.h" "${CMAKE_CURRENT_LIST_DIR}/spatial-filter.h" "${CMAKE_CURRENT_LIST_DIR}/temporal-filter.h" "${CMAKE_CURRENT_LIST_DIR}/hdr-merge.h" diff --git a/src/proc/rotation-filter.cpp b/src/proc/rotation-filter.cpp new file mode 100644 index 00000000000..35a1dbb92ca --- /dev/null +++ b/src/proc/rotation-filter.cpp @@ -0,0 +1,214 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#include +#include "option.h" +#include "stream.h" +#include "core/video.h" +#include "proc/rotation-filter.h" + +namespace librealsense { + + const int rotation_min_val = -90; + const int rotation_max_val = 180; + const int rotation_default_val = 0; + const int rotation_step = 90; + + rotation_filter::rotation_filter() : + stream_filter_processing_block("Rotation Filter"), + _control_val(rotation_default_val), + _real_width(0), + _real_height(0), + _rotated_width(0), + _rotated_height(0) + { + auto rotation_control = std::make_shared< ptr_option< int > >( + rotation_min_val, + rotation_max_val, + rotation_step, + rotation_default_val, + &_control_val, "Rotation scale"); + + 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 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; + + auto src = f.as(); + rs2::stream_profile profile = f.get_profile(); + _target_stream_profile = profile; + + if( _value == 90 || _value == -90 ) + { + _rotated_width = src.get_height(); + _rotated_height = src.get_width(); + } + else if( _value == 180 ) + { + _rotated_width = src.get_width(); + _rotated_height = src.get_height(); + } + 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_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME; + + + if (auto tgt = prepare_target_frame(f, source, tgt_type)) + { + int rotated_width = ( _value == 90 || _value == -90 ) ? src.get_height() : src.get_width(); + int rotated_height = ( _value == 90 || _value == -90 ) ? src.get_width() : src.get_height(); + + switch( bpp ) + { + case 1: { + rotate_depth< 1 >( static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ), + static_cast< const uint8_t * >( src.get_data() ), + rotated_height, + rotated_width ); + auto temp = static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ); + break; + } + + case 2: { + rotate_depth< 2 >( static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ), + static_cast< const uint8_t * >( src.get_data() ), + rotated_height, + rotated_width ); + auto temp = static_cast< uint16_t * >( const_cast< void * >( tgt.get_data() ) ); + break; + } + + default: + LOG_ERROR( "Rotation transform does not support format: " + + std::string( rs2_format_to_string( tgt.get_profile().format() ) ) ); + } + return tgt; + } + return f; + } + + void rotation_filter::update_output_profile(const rs2::frame& f) + { + _source_stream_profile = f.get_profile(); + + _target_stream_profile = _source_stream_profile.clone( _source_stream_profile.stream_type(), + _source_stream_profile.stream_index(), + _source_stream_profile.format() ); + + + auto src_vspi = dynamic_cast< video_stream_profile_interface * >( _source_stream_profile.get()->profile ); + if( ! src_vspi ) + throw std::runtime_error( "Stream profile interface is not video stream profile interface" ); + + auto tgt_vspi = dynamic_cast< video_stream_profile_interface * >( _target_stream_profile.get()->profile ); + if( ! tgt_vspi ) + throw std::runtime_error( "Profile is not video stream profile" ); + + rs2_intrinsics src_intrin = src_vspi->get_intrinsics(); + rs2_intrinsics tgt_intrin = tgt_vspi->get_intrinsics(); + + // Adjust width and height based on the rotation angle + if( _value == 90 || _value == -90 ) // 90 or -90 degrees rotation + { + _rotated_width = src_intrin.height; + _rotated_height = src_intrin.width; + tgt_intrin.fx = src_intrin.fy; + tgt_intrin.fy = src_intrin.fx; + tgt_intrin.ppx = src_intrin.ppy; + tgt_intrin.ppy = src_intrin.ppx; + } + else if( _value == 180 ) // 180 degrees rotation + { + _rotated_width = src_intrin.width; + _rotated_height = src_intrin.height; + tgt_intrin = src_intrin; + } + else { throw std::invalid_argument( "Unsupported rotation angle" ); } + + tgt_intrin.width = _rotated_width; + tgt_intrin.height = _rotated_height; + + tgt_vspi->set_intrinsics( [tgt_intrin]() { return tgt_intrin; } ); + tgt_vspi->set_dims( _rotated_width, _rotated_height ); + } + + rs2::frame rotation_filter::prepare_target_frame(const rs2::frame& f, const rs2::frame_source& source, rs2_extension tgt_type) + { + auto vf = f.as(); + auto ret = source.allocate_video_frame(_target_stream_profile, f, + vf.get_bytes_per_pixel(), + _rotated_width, + _rotated_height, + _rotated_width * vf.get_bytes_per_pixel(), + tgt_type); + + return ret; + } + + template< size_t SIZE > + void rotation_filter::rotate_depth( uint8_t * const out, const uint8_t * source, + int width, int height) + { + auto width_out = ( _value == 90 || _value == -90 ) ? height : width; + auto height_out = ( _value == 90 || _value == -90 ) ? width : height; + + for( int i = 0; i < height; ++i ) + { + for( int j = 0; j < width; ++j ) + { + auto src_index = ( i * width + j ) * SIZE; + size_t out_index; + + if( _value == 90 ) + { + // 90-degree rotation + out_index = ( j * width_out + ( width_out - i - 1 ) ) * SIZE; + } + else if( _value == -90 ) + { + // -90-degree rotation + out_index = ( ( height_out - j - 1 ) * width_out + i ) * SIZE; + } + else if( _value == 180 ) + { + // 180-degree rotation + out_index = ( ( height_out - i - 1 ) * width_out + ( width_out - j - 1 ) ) * SIZE; + } + else + { + throw std::invalid_argument( "Unsupported rotation angle" ); + } + + std::memcpy( &out[out_index], &source[src_index], SIZE ); + } + } + } + +} + diff --git a/src/proc/rotation-filter.h b/src/proc/rotation-filter.h new file mode 100644 index 00000000000..4d96fed9b2b --- /dev/null +++ b/src/proc/rotation-filter.h @@ -0,0 +1,41 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#pragma once + +#include "../include/librealsense2/hpp/rs_frame.hpp" +#include "../include/librealsense2/hpp/rs_processing.hpp" +#include "proc/synthetic-stream.h" + +namespace librealsense +{ + + class rotation_filter : public stream_filter_processing_block + { + public: + rotation_filter(); + + 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_depth( 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; + + private: + void update_output_profile(const rs2::frame& f); + + + int _control_val; + rs2::stream_profile _source_stream_profile; + rs2::stream_profile _target_stream_profile; + uint16_t _real_width; + uint16_t _real_height; + uint16_t _rotated_width; + uint16_t _rotated_height; + + int _value; + }; + MAP_EXTENSION( RS2_EXTENSION_ROTATION_FILTER, librealsense::rotation_filter ); + } diff --git a/src/realsense.def b/src/realsense.def index 06f4507611c..ab3acbf73c1 100644 --- a/src/realsense.def +++ b/src/realsense.def @@ -211,6 +211,7 @@ EXPORTS rs2_create_threshold rs2_create_units_transform rs2_create_decimation_filter_block + rs2_create_rotation_filter_block rs2_create_temporal_filter_block rs2_create_spatial_filter_block rs2_create_hole_filling_filter_block diff --git a/src/rs.cpp b/src/rs.cpp index ba1dd7649ca..3ebfe36ce1c 100644 --- a/src/rs.cpp +++ b/src/rs.cpp @@ -30,6 +30,7 @@ #include "proc/disparity-transform.h" #include "proc/syncer-processing-block.h" #include "proc/decimation-filter.h" +#include "proc/rotation-filter.h" #include "proc/spatial-filter.h" #include "proc/hole-filling-filter.h" #include "proc/color-formats-converter.h" @@ -1944,6 +1945,7 @@ int rs2_is_processing_block_extendable_to(const rs2_processing_block* f, rs2_ext switch (extension_type) { case RS2_EXTENSION_DECIMATION_FILTER: return VALIDATE_INTERFACE_NO_THROW((processing_block_interface*)(f->block.get()), librealsense::decimation_filter) != nullptr; + case RS2_EXTENSION_ROTATION_FILTER: return VALIDATE_INTERFACE_NO_THROW((processing_block_interface*)(f->block.get()), librealsense::rotation_filter) != nullptr; case RS2_EXTENSION_THRESHOLD_FILTER: return VALIDATE_INTERFACE_NO_THROW((processing_block_interface*)(f->block.get()), librealsense::threshold) != nullptr; case RS2_EXTENSION_DISPARITY_FILTER: return VALIDATE_INTERFACE_NO_THROW((processing_block_interface*)(f->block.get()), librealsense::disparity_transform) != nullptr; case RS2_EXTENSION_SPATIAL_FILTER: return VALIDATE_INTERFACE_NO_THROW((processing_block_interface*)(f->block.get()), librealsense::spatial_filter) != nullptr; @@ -2779,6 +2781,14 @@ 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 +{ + auto block = std::make_shared< librealsense::rotation_filter >(); + + return new rs2_processing_block{ block }; +} +NOARGS_HANDLE_EXCEPTIONS_AND_RETURN( nullptr ) + rs2_processing_block* rs2_create_temporal_filter_block(rs2_error** error) BEGIN_API_CALL { auto block = std::make_shared(); diff --git a/src/sensor.cpp b/src/sensor.cpp index f051cb2af41..2b18cd0985e 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -10,6 +10,7 @@ #include "image.h" #include "proc/synthetic-stream.h" #include "proc/decimation-filter.h" +#include "proc/rotation-filter.h" #include "global_timestamp_reader.h" #include "device-calibration.h" #include "core/notification.h" @@ -262,6 +263,7 @@ 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 >() ); return res; } From e04eb64b5b8b76afd067f9a611e58006a82edb98 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Thu, 7 Nov 2024 17:55:20 +0200 Subject: [PATCH 2/8] convert _value to float --- src/proc/rotation-filter.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/proc/rotation-filter.h b/src/proc/rotation-filter.h index 4d96fed9b2b..7b4083191a8 100644 --- a/src/proc/rotation-filter.h +++ b/src/proc/rotation-filter.h @@ -35,7 +35,7 @@ namespace librealsense uint16_t _rotated_width; uint16_t _rotated_height; - int _value; + float _value; }; MAP_EXTENSION( RS2_EXTENSION_ROTATION_FILTER, librealsense::rotation_filter ); } From 3ff4289e08c4260f77869111106c6debd2695385 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Thu, 7 Nov 2024 18:42:22 +0200 Subject: [PATCH 3/8] add case rotation_filter --- src/to-string.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/to-string.cpp b/src/to-string.cpp index 16f1dad3da0..bd814bb91b5 100644 --- a/src/to-string.cpp +++ b/src/to-string.cpp @@ -313,6 +313,7 @@ const char * get_string( rs2_extension value ) CASE( MAX_USABLE_RANGE_SENSOR ) CASE( DEBUG_STREAM_SENSOR ) CASE( CALIBRATION_CHANGE_DEVICE ) + CASE( ROTATION_FILTER ) default: assert( ! is_valid( value ) ); return UNKNOWN_VALUE; From 45103a44e59f6230bff3ddc593d6487f4f9a59a7 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Sun, 10 Nov 2024 14:57:19 +0200 Subject: [PATCH 4/8] fix PR review comments --- src/proc/rotation-filter.cpp | 2 -- src/proc/rotation-filter.h | 8 +++----- 2 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/proc/rotation-filter.cpp b/src/proc/rotation-filter.cpp index 35a1dbb92ca..33eb8e26432 100644 --- a/src/proc/rotation-filter.cpp +++ b/src/proc/rotation-filter.cpp @@ -91,7 +91,6 @@ namespace librealsense { static_cast< const uint8_t * >( src.get_data() ), rotated_height, rotated_width ); - auto temp = static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ); break; } @@ -100,7 +99,6 @@ namespace librealsense { static_cast< const uint8_t * >( src.get_data() ), rotated_height, rotated_width ); - auto temp = static_cast< uint16_t * >( const_cast< void * >( tgt.get_data() ) ); break; } diff --git a/src/proc/rotation-filter.h b/src/proc/rotation-filter.h index 7b4083191a8..2433bc463ed 100644 --- a/src/proc/rotation-filter.h +++ b/src/proc/rotation-filter.h @@ -3,8 +3,8 @@ #pragma once -#include "../include/librealsense2/hpp/rs_frame.hpp" -#include "../include/librealsense2/hpp/rs_processing.hpp" +#include +#include #include "proc/synthetic-stream.h" namespace librealsense @@ -26,15 +26,13 @@ namespace librealsense private: void update_output_profile(const rs2::frame& f); - - int _control_val; + int _control_val; rs2::stream_profile _source_stream_profile; rs2::stream_profile _target_stream_profile; uint16_t _real_width; uint16_t _real_height; uint16_t _rotated_width; uint16_t _rotated_height; - float _value; }; MAP_EXTENSION( RS2_EXTENSION_ROTATION_FILTER, librealsense::rotation_filter ); From 9a68ff98ab754d8265eee953b26d2be37f2efb8c Mon Sep 17 00:00:00 2001 From: noacoohen Date: Mon, 11 Nov 2024 11:41:00 +0200 Subject: [PATCH 5/8] fix PR comments --- src/proc/rotation-filter.cpp | 48 +++++++++++++++++------------------- 1 file changed, 23 insertions(+), 25 deletions(-) diff --git a/src/proc/rotation-filter.cpp b/src/proc/rotation-filter.cpp index 33eb8e26432..5849f572c78 100644 --- a/src/proc/rotation-filter.cpp +++ b/src/proc/rotation-filter.cpp @@ -173,36 +173,34 @@ namespace librealsense { void rotation_filter::rotate_depth( uint8_t * const out, const uint8_t * source, int width, int height) { - auto width_out = ( _value == 90 || _value == -90 ) ? height : width; - auto height_out = ( _value == 90 || _value == -90 ) ? width : height; + if( _value != 90 && _value != -90 && _value != 180 ) + { + throw std::invalid_argument( "Invalid rotation angle. Only 90, -90, and 180 degrees are supported." ); + } + + int width_out = ( _value == 90 || _value == -90 ) ? height : width; + int height_out = ( _value == 90 || _value == -90 ) ? width : height; + + auto compute_out_index = [&]( int i, int j ) + { + switch( static_cast< int >( _value ) ) + { + case 90: + return ( j * width_out + ( width_out - i - 1 ) ) * SIZE; + case -90: + return ( ( height_out - j - 1 ) * width_out + i ) * SIZE; + case 180: + return ( ( height_out - i - 1 ) * width_out + ( width_out - j - 1 ) ) * SIZE; + } + return size_t( 0 ); // Will not be reached due to angle validation + }; for( int i = 0; i < height; ++i ) { for( int j = 0; j < width; ++j ) { - auto src_index = ( i * width + j ) * SIZE; - size_t out_index; - - if( _value == 90 ) - { - // 90-degree rotation - out_index = ( j * width_out + ( width_out - i - 1 ) ) * SIZE; - } - else if( _value == -90 ) - { - // -90-degree rotation - out_index = ( ( height_out - j - 1 ) * width_out + i ) * SIZE; - } - else if( _value == 180 ) - { - // 180-degree rotation - out_index = ( ( height_out - i - 1 ) * width_out + ( width_out - j - 1 ) ) * SIZE; - } - else - { - throw std::invalid_argument( "Unsupported rotation angle" ); - } - + size_t src_index = ( i * width + j ) * SIZE; + size_t out_index = compute_out_index( i, j ); std::memcpy( &out[out_index], &source[src_index], SIZE ); } } From aaf602ccbbbca3c5b1edd8b982070f75582cf121 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Mon, 11 Nov 2024 15:51:28 +0200 Subject: [PATCH 6/8] fix rotation only for depth frames --- src/proc/rotation-filter.cpp | 71 +++++++++++++++++++++--------------- src/to-string.cpp | 1 + 2 files changed, 42 insertions(+), 30 deletions(-) diff --git a/src/proc/rotation-filter.cpp b/src/proc/rotation-filter.cpp index 5849f572c78..ba4f69120ff 100644 --- a/src/proc/rotation-filter.cpp +++ b/src/proc/rotation-filter.cpp @@ -54,8 +54,27 @@ namespace librealsense { if( _value == rotation_default_val ) return f; - auto src = f.as(); 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 >(); _target_stream_profile = profile; if( _value == 90 || _value == -90 ) @@ -70,20 +89,11 @@ 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_EXTENSION_DISPARITY_FRAME : RS2_EXTENSION_DEPTH_FRAME; - - if (auto tgt = prepare_target_frame(f, source, tgt_type)) { int rotated_width = ( _value == 90 || _value == -90 ) ? src.get_height() : src.get_width(); int rotated_height = ( _value == 90 || _value == -90 ) ? src.get_width() : src.get_height(); - + switch( bpp ) { case 1: { @@ -93,7 +103,7 @@ namespace librealsense { rotated_width ); break; } - + case 2: { rotate_depth< 2 >( static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ), static_cast< const uint8_t * >( src.get_data() ), @@ -101,7 +111,7 @@ namespace librealsense { rotated_width ); break; } - + default: LOG_ERROR( "Rotation transform does not support format: " + std::string( rs2_format_to_string( tgt.get_profile().format() ) ) ); @@ -170,8 +180,7 @@ namespace librealsense { } template< size_t SIZE > - void rotation_filter::rotate_depth( uint8_t * const out, const uint8_t * source, - int width, int height) + void rotation_filter::rotate_depth( uint8_t * const out, const uint8_t * source, int width, int height ) { if( _value != 90 && _value != -90 && _value != 180 ) { @@ -181,30 +190,32 @@ namespace librealsense { int width_out = ( _value == 90 || _value == -90 ) ? height : width; int height_out = ( _value == 90 || _value == -90 ) ? width : height; - auto compute_out_index = [&]( int i, int j ) - { - switch( static_cast< int >( _value ) ) - { - case 90: - return ( j * width_out + ( width_out - i - 1 ) ) * SIZE; - case -90: - return ( ( height_out - j - 1 ) * width_out + i ) * SIZE; - case 180: - return ( ( height_out - i - 1 ) * width_out + ( width_out - j - 1 ) ) * SIZE; - } - return size_t( 0 ); // Will not be reached due to angle validation - }; - for( int i = 0; i < height; ++i ) { for( int j = 0; j < width; ++j ) { size_t src_index = ( i * width + j ) * SIZE; - size_t out_index = compute_out_index( i, j ); + size_t out_index; + + if( _value == 90 ) + { + out_index = ( j * width_out + ( width_out - i - 1 ) ) * SIZE; + } + else if( _value == -90 ) + { + out_index = ( ( height_out - j - 1 ) * width_out + i ) * SIZE; + } + else + { // 180 degrees + out_index = ( ( height_out - i - 1 ) * width_out + ( width_out - j - 1 ) ) * SIZE; + } + std::memcpy( &out[out_index], &source[src_index], SIZE ); } } + } + } diff --git a/src/to-string.cpp b/src/to-string.cpp index bd814bb91b5..7bc1d95bf4e 100644 --- a/src/to-string.cpp +++ b/src/to-string.cpp @@ -460,6 +460,7 @@ std::string const & get_string_( rs2_option value ) CASE( OHM_TEMPERATURE ) CASE( SOC_PVT_TEMPERATURE ) CASE( GYRO_SENSITIVITY ) + CASE( ROTATION ) arr[RS2_OPTION_REGION_OF_INTEREST] = "Region of Interest"; #undef CASE return arr; From e09a01530553207632e121a019d0ba46007e4847 Mon Sep 17 00:00:00 2001 From: noacoohen Date: Wed, 13 Nov 2024 10:53:23 +0200 Subject: [PATCH 7/8] override should process to filter frames for rotate filter --- common/subdevice-model.cpp | 2 - include/librealsense2/hpp/rs_processing.hpp | 6 ++ src/proc/rotation-filter.cpp | 86 +++++++++++++++------ src/proc/rotation-filter.h | 19 +++-- src/sensor.cpp | 7 +- 5 files changed, 87 insertions(+), 33 deletions(-) diff --git a/common/subdevice-model.cpp b/common/subdevice-model.cpp index 734a1d65b99..b4dbf10ce58 100644 --- a/common/subdevice-model.cpp +++ b/common/subdevice-model.cpp @@ -194,8 +194,6 @@ namespace rs2 } catch (...) {} - auto filters = s->get_recommended_filters(); - for (auto&& f : s->get_recommended_filters()) { auto shared_filter = std::make_shared(f); diff --git a/include/librealsense2/hpp/rs_processing.hpp b/include/librealsense2/hpp/rs_processing.hpp index be2cc812c1c..3344a54860c 100644 --- a/include/librealsense2/hpp/rs_processing.hpp +++ b/include/librealsense2/hpp/rs_processing.hpp @@ -863,6 +863,12 @@ namespace rs2 { } + rotation_filter( float value ) + : filter( init(), 1 ) + { + set_option( RS2_OPTION_ROTATION, value ); + } + rotation_filter( filter f ) : filter( f ) { diff --git a/src/proc/rotation-filter.cpp b/src/proc/rotation-filter.cpp index ba4f69120ff..d8fbaf0b553 100644 --- a/src/proc/rotation-filter.cpp +++ b/src/proc/rotation-filter.cpp @@ -14,8 +14,46 @@ namespace librealsense { const int rotation_default_val = 0; const int rotation_step = 90; - rotation_filter::rotation_filter() : - stream_filter_processing_block("Rotation Filter"), + + 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 ) + { + 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< stream_filter > 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), @@ -27,7 +65,7 @@ namespace librealsense { rotation_max_val, rotation_step, rotation_default_val, - &_control_val, "Rotation scale"); + &_control_val, "Rotation angle"); auto weak_rotation_control = std::weak_ptr< ptr_option< int > >( rotation_control ); rotation_control->on_set( @@ -54,27 +92,8 @@ namespace librealsense { 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(); _target_stream_profile = profile; if( _value == 90 || _value == -90 ) @@ -89,6 +108,14 @@ 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)) { int rotated_width = ( _value == 90 || _value == -90 ) ? src.get_height() : src.get_width(); @@ -215,6 +242,19 @@ namespace librealsense { } } + + bool rotation_filter::should_process( const rs2::frame & frame ) + { + if( ! frame || frame.is< rs2::frameset >() ) + return false; + auto profile = frame.get_profile(); + for( auto stream : _streams_to_rotate ) + { + if( stream.match( frame ) ) + return true; + } + return false; + } } diff --git a/src/proc/rotation-filter.h b/src/proc/rotation-filter.h index 2433bc463ed..4e326920b9a 100644 --- a/src/proc/rotation-filter.h +++ b/src/proc/rotation-filter.h @@ -15,6 +15,8 @@ namespace librealsense public: rotation_filter(); + rotation_filter( std::vector< stream_filter > streams_to_rotate ); + protected: rs2::frame prepare_target_frame(const rs2::frame& f, const rs2::frame_source& source, rs2_extension tgt_type); @@ -23,16 +25,19 @@ namespace librealsense 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); - int _control_val; - rs2::stream_profile _source_stream_profile; - rs2::stream_profile _target_stream_profile; - uint16_t _real_width; - uint16_t _real_height; - uint16_t _rotated_width; - uint16_t _rotated_height; + std::vector< stream_filter > _streams_to_rotate; + int _control_val; + rs2::stream_profile _source_stream_profile; + rs2::stream_profile _target_stream_profile; + uint16_t _real_width; + uint16_t _real_height; + uint16_t _rotated_width; + uint16_t _rotated_height; float _value; }; MAP_EXTENSION( RS2_EXTENSION_ROTATION_FILTER, librealsense::rotation_filter ); diff --git a/src/sensor.cpp b/src/sensor.cpp index 2b18cd0985e..ef97f8d7dae 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -263,7 +263,12 @@ 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 streams_to_rotate; + stream_filter depth_filter( RS2_STREAM_DEPTH, RS2_FORMAT_Z16, -1); + streams_to_rotate.push_back( depth_filter ); + stream_filter ir_filter( RS2_STREAM_INFRARED, RS2_FORMAT_Y8, -1 ); + streams_to_rotate.push_back( ir_filter ); + res.push_back( std::make_shared< rotation_filter >( streams_to_rotate ) ); return res; } From 224ba485e79632775318f7ce6c426bccf8810d2c Mon Sep 17 00:00:00 2001 From: noacoohen Date: Wed, 13 Nov 2024 16:08:37 +0200 Subject: [PATCH 8/8] add rotation filter to rs-post-processing example and remove override of should process --- .../post-processing/rs-post-processing.cpp | 33 ++++++-- src/proc/rotation-filter.cpp | 81 +++++-------------- src/proc/rotation-filter.h | 4 - src/sensor.cpp | 7 +- 4 files changed, 50 insertions(+), 75 deletions(-) diff --git a/examples/post-processing/rs-post-processing.cpp b/examples/post-processing/rs-post-processing.cpp index 96355e7ef1c..da787dfa492 100644 --- a/examples/post-processing/rs-post-processing.cpp +++ b/examples/post-processing/rs-post-processing.cpp @@ -71,6 +71,7 @@ int main(int argc, char * argv[]) try // Declare filters rs2::decimation_filter dec_filter; // Decimation - reduces depth frame density + rs2::rotation_filter rot_filter; // Rotation - rotates frames rs2::threshold_filter thr_filter; // Threshold - removes values outside recommended range rs2::spatial_filter spat_filter; // Spatial - edge-preserving spatial smoothing rs2::temporal_filter temp_filter; // Temporal - reduces temporal noise @@ -85,6 +86,7 @@ int main(int argc, char * argv[]) try // The following order of emplacement will dictate the orders in which filters are applied filters.emplace_back("Decimate", dec_filter); + filters.emplace_back( "Rotate", rot_filter ); filters.emplace_back("Threshold", thr_filter); filters.emplace_back(disparity_filter_name, depth_to_disparity); filters.emplace_back("Spatial", spat_filter); @@ -115,11 +117,12 @@ int main(int argc, char * argv[]) try /* Apply filters. The implemented flow of the filters pipeline is in the following order: 1. apply decimation filter - 2. apply threshold filter - 3. transform the scene into disparity domain - 4. apply spatial filter - 5. apply temporal filter - 6. revert the results back (if step Disparity filter was applied + 2. apply rotation filter + 3. apply threshold filter + 4. transform the scene into disparity domain + 5. apply spatial filter + 6. apply temporal filter + 7. revert the results back (if step Disparity filter was applied to depth domain (each post processing block is optional and can be applied independantly). */ bool revert_disparity = false; @@ -269,11 +272,29 @@ void render_ui(float w, float h, std::vector& filters) ImGui::Checkbox(filter.filter_name.c_str(), &tmp_value); filter.is_enabled = tmp_value; ImGui::PopStyleColor(); + + + if( filter.filter_name == "Rotate" ) // Combo box specifically for the rotation filter + { + offset_y += elements_margin; + ImGui::PushItemWidth( w / 4 ); + ImGui::SetCursorPos( { offset_x, offset_y } ); + static const char * rotation_modes[] = { "0", "90", "180", "270" }; + static int current_rotation_mode = 0; + if( ImGui::Combo( "Rotation Angle", ¤t_rotation_mode, rotation_modes, 4 ) ) + { + float rotation_value = std::stof( rotation_modes[current_rotation_mode] ); + filter.supported_options[RS2_OPTION_ROTATION].value = rotation_value; - if (filter.supported_options.size() == 0) + // Set the filter's option using the new value + filter.filter.set_option( RS2_OPTION_ROTATION, rotation_value ); + } + } + if( filter.supported_options.size() == 0 ) { offset_y += elements_margin; } + // Draw a slider for each of the filter's options for (auto& option_slider_pair : filter.supported_options) { diff --git a/src/proc/rotation-filter.cpp b/src/proc/rotation-filter.cpp index d8fbaf0b553..6fe28f6a975 100644 --- a/src/proc/rotation-filter.cpp +++ b/src/proc/rotation-filter.cpp @@ -23,6 +23,7 @@ namespace librealsense { , _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, @@ -51,49 +52,32 @@ namespace librealsense { register_option( RS2_OPTION_ROTATION, rotation_control ); } - rotation_filter::rotation_filter( std::vector< stream_filter > 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) - { - 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 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; - auto src = f.as< rs2::video_frame >(); 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 >(); _target_stream_profile = profile; if( _value == 90 || _value == -90 ) @@ -109,13 +93,6 @@ 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)) { int rotated_width = ( _value == 90 || _value == -90 ) ? src.get_height() : src.get_width(); @@ -242,20 +219,6 @@ namespace librealsense { } } - - bool rotation_filter::should_process( const rs2::frame & frame ) - { - if( ! frame || frame.is< rs2::frameset >() ) - return false; - auto profile = frame.get_profile(); - for( auto stream : _streams_to_rotate ) - { - if( stream.match( frame ) ) - return true; - } - return false; - } - - + } diff --git a/src/proc/rotation-filter.h b/src/proc/rotation-filter.h index 4e326920b9a..fa841b2d582 100644 --- a/src/proc/rotation-filter.h +++ b/src/proc/rotation-filter.h @@ -15,8 +15,6 @@ namespace librealsense public: rotation_filter(); - rotation_filter( std::vector< stream_filter > streams_to_rotate ); - protected: rs2::frame prepare_target_frame(const rs2::frame& f, const rs2::frame_source& source, rs2_extension tgt_type); @@ -25,8 +23,6 @@ namespace librealsense 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); diff --git a/src/sensor.cpp b/src/sensor.cpp index ef97f8d7dae..686a4dead6c 100644 --- a/src/sensor.cpp +++ b/src/sensor.cpp @@ -263,12 +263,7 @@ void log_callback_end( uint32_t fps, dec->get_option(RS2_OPTION_STREAM_FORMAT_FILTER).set(RS2_FORMAT_Z16); res.push_back(dec); } - std::vector streams_to_rotate; - stream_filter depth_filter( RS2_STREAM_DEPTH, RS2_FORMAT_Z16, -1); - streams_to_rotate.push_back( depth_filter ); - stream_filter ir_filter( RS2_STREAM_INFRARED, RS2_FORMAT_Y8, -1 ); - streams_to_rotate.push_back( ir_filter ); - res.push_back( std::make_shared< rotation_filter >( streams_to_rotate ) ); + res.push_back( std::make_shared< rotation_filter >( )); return res; }