diff --git a/common/subdevice-model.cpp b/common/subdevice-model.cpp index 3a7914f6cb..b4dbf10ce5 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); @@ -215,6 +213,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/examples/post-processing/rs-post-processing.cpp b/examples/post-processing/rs-post-processing.cpp index 96355e7ef1..fac5ccb246 100644 --- a/examples/post-processing/rs-post-processing.cpp +++ b/examples/post-processing/rs-post-processing.cpp @@ -22,6 +22,7 @@ struct filter_slider_ui std::string description; bool is_int; float value; + float step; rs2::option_range range; bool render(const float3& location, bool enabled); @@ -71,6 +72,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 +87,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 +118,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 +273,12 @@ 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.supported_options.size() == 0) + + 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) { @@ -316,11 +321,21 @@ bool filter_slider_ui::render(const float3& location, bool enabled) { int value_as_int = static_cast(value); value_changed = ImGui::SliderInt(("##" + name).c_str(), &value_as_int, static_cast(range.min), static_cast(range.max), "%.0f"); + if( step > 1 ) + { + value_as_int = static_cast< int >( range.min ) + + ( ( value_as_int - static_cast< int >( range.min ) ) / static_cast< int >( step ) ) + * static_cast< int >( step ); + } value = static_cast(value_as_int); } else { value_changed = ImGui::SliderFloat(("##" + name).c_str(), &value, range.min, range.max, "%.3f", 1.0f); + if( step > 0.0f ) + { + value = range.min + round( ( value - range.min ) / step ) * step; + } } ImGui::PopItemWidth(); @@ -356,12 +371,13 @@ filter_options::filter_options(const std::string name, rs2::filter& flt) : filter(flt), is_enabled(true) { - const std::array possible_filter_options = { + const std::array possible_filter_options = { RS2_OPTION_FILTER_MAGNITUDE, RS2_OPTION_FILTER_SMOOTH_ALPHA, RS2_OPTION_MIN_DISTANCE, RS2_OPTION_MAX_DISTANCE, - RS2_OPTION_FILTER_SMOOTH_DELTA + RS2_OPTION_FILTER_SMOOTH_DELTA, + RS2_OPTION_ROTATION }; //Go over each filter option and create a slider for it @@ -378,6 +394,7 @@ filter_options::filter_options(const std::string name, rs2::filter& flt) : supported_options[opt].name = name + "_" + opt_name; std::string prefix = "Filter "; supported_options[opt].label = opt_name; + supported_options[opt].step = range.step; } } } diff --git a/include/librealsense2/h/rs_option.h b/include/librealsense2/h/rs_option.h index 10d03fe2e8..80c30bc853 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 745b72c54d..e13e1484a8 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 775fb5dd83..5543fa00fe 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 30a2181c50..3344a54860 100644 --- a/include/librealsense2/hpp/rs_processing.hpp +++ b/include/librealsense2/hpp/rs_processing.hpp @@ -851,6 +851,48 @@ 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( float value ) + : filter( init(), 1 ) + { + set_option( RS2_OPTION_ROTATION, value ); + } + + 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/media/ros/ros_writer.cpp b/src/media/ros/ros_writer.cpp index beb836715b..c55d38f7da 100644 --- a/src/media/ros/ros_writer.cpp +++ b/src/media/ros/ros_writer.cpp @@ -2,6 +2,7 @@ // Copyright(c) 2019 Intel Corporation. All Rights Reserved. #include "proc/decimation-filter.h" +#include "proc/rotation-filter.h" #include "proc/threshold.h" #include "proc/disparity-transform.h" #include "proc/spatial-filter.h" @@ -513,6 +514,7 @@ namespace librealsense RETURN_IF_EXTENSION(block, RS2_EXTENSION_HOLE_FILLING_FILTER); RETURN_IF_EXTENSION(block, RS2_EXTENSION_HDR_MERGE); RETURN_IF_EXTENSION(block, RS2_EXTENSION_SEQUENCE_ID_FILTER); + RETURN_IF_EXTENSION(block, RS2_EXTENSION_ROTATION_FILTER); #undef RETURN_IF_EXTENSION diff --git a/src/proc/CMakeLists.txt b/src/proc/CMakeLists.txt index 1d37afe0af..35784d6991 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 0000000000..6fe28f6a97 --- /dev/null +++ b/src/proc/rotation-filter.cpp @@ -0,0 +1,224 @@ +// 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" ) + , _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 >(); + _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 ); + + 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 ); + 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 ); + 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 ) + { + 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; + + 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; + + 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/proc/rotation-filter.h b/src/proc/rotation-filter.h new file mode 100644 index 0000000000..fa841b2d58 --- /dev/null +++ b/src/proc/rotation-filter.h @@ -0,0 +1,40 @@ +// License: Apache 2.0. See LICENSE file in root directory. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#pragma once + +#include +#include +#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); + + 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/realsense.def b/src/realsense.def index 607307aaf6..e1e7a9b79d 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 5078ece181..07e88abdb4 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/rscore-pp-block-factory.cpp b/src/rscore-pp-block-factory.cpp index 679bcf63e0..d09fd7978b 100644 --- a/src/rscore-pp-block-factory.cpp +++ b/src/rscore-pp-block-factory.cpp @@ -4,6 +4,7 @@ #include "rscore-pp-block-factory.h" #include "proc/decimation-filter.h" +#include "proc/rotation-filter.h" #include "proc/disparity-transform.h" #include "proc/hdr-merge.h" #include "proc/hole-filling-filter.h" @@ -27,6 +28,8 @@ rscore_pp_block_factory::create_pp_block( std::string const & name, rsutils::jso if( rsutils::string::nocase_equal( name, "Decimation Filter" ) ) return std::make_shared< decimation_filter >(); + if( rsutils::string::nocase_equal( name, "Rotation Filter" ) ) + return std::make_shared< rotation_filter >(); if( rsutils::string::nocase_equal( name, "HDR Merge" ) ) // and Hdr Merge return std::make_shared< hdr_merge >(); if( rsutils::string::nocase_equal( name, "Filter By Sequence id" ) // name diff --git a/src/sensor.cpp b/src/sensor.cpp index f051cb2af4..686a4dead6 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; } diff --git a/src/to-string.cpp b/src/to-string.cpp index 16f1dad3da..7bc1d95bf4 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; @@ -459,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; diff --git a/unit-tests/post-processing/test-rotation-filter.py b/unit-tests/post-processing/test-rotation-filter.py new file mode 100644 index 0000000000..8b515c7f73 --- /dev/null +++ b/unit-tests/post-processing/test-rotation-filter.py @@ -0,0 +1,131 @@ +# License: Apache 2.0. See LICENSE file in root directory. +# Copyright(c) 2024 Intel Corporation. All Rights Reserved. + +#temporary fix to prevent the test from running on Win_SH_Py_DDS_CI +#test:donotrun:dds + +from rspy import test, repo +import pyrealsense2 as rs +import numpy as np + +# Parameters for frame creation and depth intrinsics +input_res_x = 640 +input_res_y = 480 +focal_length = 600 +depth_units = 0.001 +stereo_baseline_mm = 50 +frames = 5 # Number of frames to process + + +# Function to create depth intrinsics directly +def create_depth_intrinsics(): + depth_intrinsics = rs.intrinsics() + depth_intrinsics.width = input_res_x + depth_intrinsics.height = input_res_y + depth_intrinsics.ppx = input_res_x / 2.0 + depth_intrinsics.ppy = input_res_y / 2.0 + depth_intrinsics.fx = focal_length + depth_intrinsics.fy = focal_length + depth_intrinsics.model = rs.distortion.brown_conrady + depth_intrinsics.coeffs = [0, 0, 0, 0, 0] + return depth_intrinsics + + +# Function to create a video stream with specified parameters +def create_video_stream(depth_intrinsics): + vs = rs.video_stream() + vs.type = rs.stream.depth + vs.index = 0 + vs.uid = 0 + vs.width = input_res_x + vs.height = input_res_y + vs.fps = 30 + vs.bpp = 2 + vs.fmt = rs.format.z16 + vs.intrinsics = depth_intrinsics + return vs + + +# Function to create a synthetic frame +def create_frame(depth_stream_profile, index): + frame = rs.software_video_frame() + data = np.arange(input_res_x * input_res_y, dtype=np.uint16).reshape((input_res_y, input_res_x)) + frame.pixels = data.tobytes() + frame.bpp = 2 + frame.stride = input_res_x * 2 + frame.timestamp = index * 33 + frame.domain = rs.timestamp_domain.system_time + frame.frame_number = index + frame.profile = depth_stream_profile.as_video_stream_profile() + frame.depth_units = depth_units + return frame + + +# Function to validate rotated results based on the angle +def validate_rotation_results(filtered_frame, angle): + rotated_height = filtered_frame.profile.as_video_stream_profile().height() + rotated_width = filtered_frame.profile.as_video_stream_profile().width() + + # Reshape the rotated data according to its actual dimensions + rotated_data = np.frombuffer(filtered_frame.get_data(), dtype=np.uint16).reshape((rotated_height, rotated_width)) + + # Original data for comparison + original_data = np.arange(input_res_x * input_res_y, dtype=np.uint16).reshape((input_res_y, input_res_x)) + + # Determine the expected rotated result based on the angle + if angle == 90: + expected_data = np.rot90(original_data, k=-1) # Clockwise + elif angle == 180: + expected_data = np.rot90(original_data, k=2) # 180 degrees + elif angle == -90: + expected_data = np.rot90(original_data, k=1) # Counterclockwise + + # Convert numpy arrays to lists before comparison + rotated_list = rotated_data.flatten().tolist() + expected_list = expected_data.flatten().tolist() + + # Compare the flattened lists + test.check_equal_lists(rotated_list, expected_list) + + +################################################################################################ +with test.closure("Test rotation filter"): + # Set up software device and depth sensor + sw_dev = rs.software_device() + depth_sensor = sw_dev.add_sensor("Depth") + + # Initialize intrinsics and video stream profile + depth_intrinsics = create_depth_intrinsics() + vs = create_video_stream(depth_intrinsics) + depth_stream_profile = depth_sensor.add_video_stream(vs) + + frame_queue = rs.frame_queue(15) + + # Define rotation angles to test + rotation_angles = [90, 180, -90] + for angle in rotation_angles: + rotation_filter = rs.rotation_filter() + rotation_filter.set_option(rs.option.rotation, angle) + + # Start depth sensor + depth_sensor.open(depth_stream_profile) + depth_sensor.start(frame_queue) + + for i in range(frames): + # Create and process each frame + frame = create_frame(depth_stream_profile, i) + depth_sensor.on_video_frame(frame) + + # Wait for frames and apply rotation filter + depth_frame = frame_queue.wait_for_frame() + filtered_depth = rotation_filter.process(depth_frame) + + # Validate rotated frame results + validate_rotation_results(filtered_depth, angle) + + # Stop and close the sensor after each angle test + depth_sensor.stop() + depth_sensor.close() + +################################################################################################ +test.print_results_and_exit() \ No newline at end of file diff --git a/wrappers/python/pyrs_processing.cpp b/wrappers/python/pyrs_processing.cpp index 29dd9db7b4..5dd80f2f0a 100644 --- a/wrappers/python/pyrs_processing.cpp +++ b/wrappers/python/pyrs_processing.cpp @@ -65,6 +65,7 @@ void init_processing(py::module &m) { return new rs2::filter(filter_function, queue_size); }), "filter_function"_a, "queue_size"_a = 1) .def(BIND_DOWNCAST(filter, decimation_filter)) + .def( BIND_DOWNCAST( filter, rotation_filter ) ) .def(BIND_DOWNCAST(filter, disparity_transform)) .def(BIND_DOWNCAST(filter, hole_filling_filter)) .def(BIND_DOWNCAST(filter, spatial_filter)) @@ -157,6 +158,9 @@ void init_processing(py::module &m) { decimation_filter.def(py::init<>()) .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 ); + 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" "threshold for edge classification and preserving.");