From b4efc368294271a61e5a2e4d9bdd9a7a0cfee5ca Mon Sep 17 00:00:00 2001 From: noacoohen Date: Thu, 7 Nov 2024 18:12:17 +0200 Subject: [PATCH] fix errors --- common/subdevice-model.cpp | 3 + src/proc/rotation-filter.cpp | 584 ++++++----------------------------- src/proc/rotation-filter.h | 22 +- 3 files changed, 109 insertions(+), 500 deletions(-) diff --git a/common/subdevice-model.cpp b/common/subdevice-model.cpp index 3a7914f6cb..734a1d65b9 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/src/proc/rotation-filter.cpp b/src/proc/rotation-filter.cpp index 548471e2da..788e2c74d4 100644 --- a/src/proc/rotation-filter.cpp +++ b/src/proc/rotation-filter.cpp @@ -1,20 +1,11 @@ // License: Apache 2.0. See LICENSE file in root directory. -// Copyright(c) 2017 Intel Corporation. All Rights Reserved. +// Copyright(c) 2024 Intel Corporation. All Rights Reserved. #include -#include - -#include -#include -#include "environment.h" #include "option.h" #include "stream.h" #include "core/video.h" -#include "proc/synthetic-stream.h" #include "proc/rotation-filter.h" -#include "proc/rotation-transform.h" - -#include namespace librealsense { @@ -25,20 +16,13 @@ namespace librealsense { rotation_filter::rotation_filter() : stream_filter_processing_block("Rotation Filter"), - _decimation_factor(rotation_default_val), _control_val(rotation_default_val), - _patch_size(rotation_default_val), - _kernel_size(_patch_size*_patch_size), - _real_width(), + _patch_size(2), + _real_width(0), _real_height(0), - _padded_width(0), - _padded_height(0), - _recalc_profile(false), - _options_changed(false) + _rotated_width(0), + _rotated_height(0) { - _stream_filter.stream = RS2_STREAM_DEPTH; - _stream_filter.format = RS2_FORMAT_Z16; - auto rotation_control = std::make_shared< ptr_option< int > >( rotation_min_val, rotation_max_val, @@ -59,7 +43,7 @@ namespace librealsense { throw invalid_value_exception( rsutils::string::from() << "Unsupported rotation scale " << val << " is out of range." ); - _value = _control_val; + _value = val; }); @@ -68,138 +52,110 @@ namespace librealsense { 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; - rs2_format format = profile.format(); - _padded_width = src.get_height(); - _padded_height = src.get_width(); - rs2_stream type = profile.stream_type(); + + 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 = src.get_width(); - int rotated_height = src.get_height(); + 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< const uint8_t * >( src.get_data() ), - static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ), - - rotated_width, - rotated_height); + 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< const uint8_t * >( src.get_data() ), - static_cast< uint8_t * >( const_cast< void * >( tgt.get_data() ) ), - rotated_width, - rotated_height); + } + + 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() ) ) ); } - /*if (format == RS2_FORMAT_Z16) - { - rotate_depth(static_cast(src.get_data()), - static_cast(const_cast(tgt.get_data())), - src.get_width(), src.get_height(), this->_patch_size); - } - else - { - rotate_others(format, src.get_data(), - const_cast(tgt.get_data()), - src.get_width(), src.get_height(), this->_patch_size); - } - return tgt;*/ + return tgt; } return f; } void rotation_filter::update_output_profile(const rs2::frame& f) { - if (_options_changed || f.get_profile().get() != _source_stream_profile.get()) - { - _options_changed = false; - _source_stream_profile = f.get_profile(); - const auto pf = _registered_profiles.find(std::make_tuple(_source_stream_profile.get(), _decimation_factor)); - if (_registered_profiles.end() != pf) - { - _target_stream_profile = pf->second; - auto tgt_vspi = dynamic_cast(_target_stream_profile.get()->profile); - if (!tgt_vspi) - throw std::runtime_error("Stream profile interface is not video stream profile interface"); - - auto f_pf = dynamic_cast(_source_stream_profile.get()->profile); - if (!f_pf) - throw std::runtime_error("Stream profile interface is not video stream profile interface"); - - rs2_intrinsics tgt_intrin = tgt_vspi->get_intrinsics(); - - // Update real/padded output frame size based on retrieved input properties - _real_width = f_pf->get_width() / _patch_size; - _real_height = f_pf->get_height() / _patch_size; - _padded_width = tgt_intrin.width; - _padded_height = tgt_intrin.height; - } - else - { - _recalc_profile = true; - } - } - - // Buld a new target profile for every system/filter change - if (_recalc_profile) - { - auto vp = _source_stream_profile.as(); - - auto tmp_profile = _source_stream_profile.clone(_source_stream_profile.stream_type(), _source_stream_profile.stream_index(), _source_stream_profile.format()); - auto src_vspi = dynamic_cast(_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(tmp_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(); + _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() ); - // recalculate real/padded output frame size based on new input porperties - _real_width = src_vspi->get_width() / _patch_size; - _real_height = src_vspi->get_height() / _patch_size; - // The resulted frame dimension will be dividible by 4; - _padded_width = _real_width + 3; - _padded_width /= 4; - _padded_width *= 4; + 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" ); - _padded_height = _real_height + 3; - _padded_height /= 4; - _padded_height *= 4; + 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" ); - tgt_intrin.width = _padded_width; - tgt_intrin.height = _padded_height; - tgt_intrin.fx = src_intrin.fx / _patch_size; - tgt_intrin.fy = src_intrin.fy / _patch_size; - tgt_intrin.ppx = src_intrin.ppx / _patch_size; - tgt_intrin.ppy = src_intrin.ppy / _patch_size; + rs2_intrinsics src_intrin = src_vspi->get_intrinsics(); + rs2_intrinsics tgt_intrin = tgt_vspi->get_intrinsics(); - tgt_vspi->set_intrinsics([tgt_intrin]() { return tgt_intrin; }); - tgt_vspi->set_dims(tgt_intrin.width, tgt_intrin.height); + // 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; + } + else if( _value == 180 ) // 180 degrees rotation + { + _rotated_width = src_intrin.width; + _rotated_height = src_intrin.height; + } + else { throw std::invalid_argument( "Unsupported rotation angle" ); } - _registered_profiles[std::make_tuple(_source_stream_profile.get(), _decimation_factor)] = _target_stream_profile = tmp_profile; + tgt_intrin.width = _rotated_width; + tgt_intrin.height = _rotated_height; + tgt_intrin.fx = src_intrin.fx / _patch_size; + tgt_intrin.fy = src_intrin.fy / _patch_size; + tgt_intrin.ppx = src_intrin.ppx / _patch_size; + tgt_intrin.ppy = src_intrin.ppy / _patch_size; - _recalc_profile = false; - } + 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) @@ -207,394 +163,52 @@ namespace librealsense { auto vf = f.as(); auto ret = source.allocate_video_frame(_target_stream_profile, f, vf.get_bytes_per_pixel(), - _padded_height, - _padded_width, - _padded_height * 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( const uint8_t * frame_data_in, uint8_t * const frame_data_out, - size_t width, size_t height) + void rotation_filter::rotate_depth( uint8_t * const out, const uint8_t * source, + int width, int height) { - auto width_out = height; - auto height_out = width; - - uint8_t buffer[8][8 * SIZE]; // = { 0 }; - for( int i = 0; i <= height - 8; i = i + 8 ) - { - for( int j = 0; j <= width - 8; j = j + 8 ) - { - for( int ii = 0; ii < 8; ++ii ) - { - for( int jj = 0; jj < 8; ++jj ) - { - auto source_index = ( ( j + jj ) + ( width * ( i + ii ) ) ) * SIZE; - memcpy( (void *)( &buffer[7 - jj][( 7 - ii ) * SIZE] ), &frame_data_in[source_index], SIZE ); - } - } - - for( int ii = 0; ii < 8; ++ii ) - { - auto out_index = ( ( ( height_out - 8 - j + 1 ) * width_out ) - i - 8 + (ii)*width_out ); - memcpy( &frame_data_out[(out_index)*SIZE], &( buffer[ii] ), 8 * SIZE ); - } - } - } - - } - - void rotation_filter::rotate_others(rs2_format format, const void * frame_data_in, void * frame_data_out, - size_t width_in, size_t height_in, size_t scale) - { - int sum = 0; - auto patch_size = scale * scale; - - switch (format) - { - case RS2_FORMAT_YUYV: - { - uint8_t* from = (uint8_t*)frame_data_in; - uint8_t* p = nullptr; - uint8_t* q = (uint8_t*)frame_data_out; - - auto w_2 = width_in >> 1; - auto rw_2 = _real_width >> 1; - auto pw_2 = _padded_width >> 1; - auto s2 = scale >> 1; - bool odd = (scale & 1); - for (int j = 0; j < _real_height; ++j) - { - for (int i = 0; i < rw_2; ++i) - { - p = from + scale * (j * w_2 + i) * 4; - sum = 0; - for (size_t n = 0; n < scale; ++n) - { - for (size_t m = 0; m < scale; ++m) - sum += p[m * 2]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - - p = from + scale * (j * w_2 + i) * 4 + 1; - sum = 0; - for (size_t n = 0; n < scale; ++n) - { - for (size_t m = 0; m < s2; ++m) - sum += 2 * p[m * 4]; - - if (odd) - sum += p[s2 * 4]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - - p = from + scale * (j * w_2 + i) * 4 + s2 * 4 + (odd ? 2 : 0); - sum = 0; - for (size_t n = 0; n < scale; ++n) - { - for (size_t m = 0; m < scale; ++m) - sum += p[m * 2]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - - p = from + scale * (j * w_2 + i) * 4 + 3; - sum = 0; - for (size_t n = 0; n < scale; ++n) - { - for (size_t m = 0; m < s2; ++m) - sum += 2 * p[m * 4]; - - if (odd) - sum += p[s2 * 4]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - } - - for (int i = rw_2; i < pw_2; ++i) - { - *q++ = 0; - *q++ = 0; - *q++ = 0; - *q++ = 0; - } - } - - for (int j = _real_height; j < _padded_height; ++j) - { - for (int i = 0; i < _padded_width; ++i) - { - *q++ = 0; - *q++ = 0; - } - } - } - break; + auto width_out = ( _value == 90 || _value == -90 ) ? height : width; + auto height_out = ( _value == 90 || _value == -90 ) ? width : height; - case RS2_FORMAT_UYVY: + for( int i = 0; i < height; ++i ) { - uint8_t* from = (uint8_t*)frame_data_in; - uint8_t* p = nullptr; - uint8_t* q = (uint8_t*)frame_data_out; - - auto w_2 = width_in >> 1; - auto rw_2 = _real_width >> 1; - auto pw_2 = _padded_width >> 1; - auto s2 = scale >> 1; - bool odd = (scale & 1); - for (int j = 0; j < _real_height; ++j) + for( int j = 0; j < width; ++j ) { - for (int i = 0; i < rw_2; ++i) - { - p = from + scale * (j * w_2 + i) * 4; - sum = 0; - for (size_t n = 0; n < scale; ++n) - { - for (size_t m = 0; m < s2; ++m) - sum += 2 * p[m * 4]; - - if (odd) - sum += p[s2 * 4]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - - p = from + scale * (j * w_2 + i) * 4 + 1; - sum = 0; - for (size_t n = 0; n < scale; ++n) - { - for (size_t m = 0; m < scale; ++m) - sum += p[m * 2]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - - p = from + scale * (j * w_2 + i) * 4 + 2; - sum = 0; - for (size_t n = 0; n < scale; ++n) - { - for (size_t m = 0; m < s2; ++m) - sum += 2 * p[m * 4]; - - if (odd) - sum += p[s2 * 4]; - - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - - p = from + scale * (j * w_2 + i) * 4 + s2 * 4 + (odd ? 3 : 1); - sum = 0; - for (size_t n = 0; n < scale; ++n) - { - for (size_t m = 0; m < scale; ++m) - sum += p[m * 2]; + auto src_index = ( i * width + j ) * SIZE; + size_t out_index; - p += w_2 * 4; - } - *q++ = (uint8_t)(sum / patch_size); - } - - for (int i = rw_2; i < pw_2; ++i) + if( _value == 90 ) { - *q++ = 0; - *q++ = 0; - *q++ = 0; - *q++ = 0; + // 90-degree rotation + out_index = ( j * width_out + ( width_out - i - 1 ) ) * SIZE; } - } - - for (int j = _real_height; j < _padded_height; ++j) - { - for (int i = 0; i < _padded_width; ++i) + else if( _value == -90 ) { - *q++ = 0; - *q++ = 0; + // -90-degree rotation + out_index = ( ( height_out - j - 1 ) * width_out + i ) * SIZE; } - } - } - break; - - case RS2_FORMAT_RGB8: - case RS2_FORMAT_BGR8: - { - uint8_t* from = (uint8_t*)frame_data_in; - uint8_t* p = nullptr; - uint8_t* q = (uint8_t*)frame_data_out;; - - for (int j = 0; j < _real_height; ++j) - { - for (int i = 0; i < _real_width; ++i) + else if( _value == 180 ) { - for (int k = 0; k < 3; ++k) - { - p = from + scale * (j * width_in + i) * 3 + k; - sum = 0; - for (size_t n = 0; n < scale; ++n) - { - for (size_t m = 0; m < scale; ++m) - sum += p[m * 3]; - - p += width_in * 3; - } - - *q++ = (uint8_t)(sum / patch_size); - } + // 180-degree rotation + out_index = ( ( height_out - i - 1 ) * width_out + ( width_out - j - 1 ) ) * SIZE; } - - for (int i = _real_width; i < _padded_width; ++i) + else { - *q++ = 0; - *q++ = 0; - *q++ = 0; + throw std::invalid_argument( "Unsupported rotation angle" ); } - } - for (int j = _real_height; j < _padded_height; ++j) - { - for (int i = 0; i < _padded_width; ++i) - { - *q++ = 0; - *q++ = 0; - *q++ = 0; - } + std::memcpy( &out[out_index], &source[src_index], SIZE ); } } - break; - - case RS2_FORMAT_RGBA8: - case RS2_FORMAT_BGRA8: - { - uint8_t* from = (uint8_t*)frame_data_in; - uint8_t* p = nullptr; - uint8_t* q = (uint8_t*)frame_data_out; - - for (int j = 0; j < _real_height; ++j) - { - for (int i = 0; i < _real_width; ++i) - { - for (int k = 0; k < 4; ++k) - { - p = from + scale * (j * width_in + i) * 4 + k; - sum = 0; - for (size_t n = 0; n < scale; ++n) - { - for (size_t m = 0; m < scale; ++m) - sum += p[m * 4]; - - p += width_in * 4; - } - - *q++ = (uint8_t)(sum / patch_size); - } - } - - for (int i = _real_width; i < _padded_width; ++i) - { - *q++ = 0; - *q++ = 0; - *q++ = 0; - *q++ = 0; - } - } - - for (int j = _real_height; j < _padded_height; ++j) - { - for (int i = 0; i < _padded_width; ++i) - { - *q++ = 0; - *q++ = 0; - *q++ = 0; - *q++ = 0; - } - } - } - break; - - case RS2_FORMAT_Y8: - { - uint8_t* from = (uint8_t*)frame_data_in; - uint8_t* p = nullptr; - uint8_t* q = (uint8_t*)frame_data_out; - - for (int j = 0; j < _real_height; ++j) - { - for (int i = 0; i < _real_width; ++i) - { - p = from + scale * (j * width_in + i); - sum = 0; - for (size_t n = 0; n < scale; ++n) - { - for (size_t m = 0; m < scale; ++m) - sum += p[m]; - - p += width_in; - } - - *q++ = (uint8_t)(sum / patch_size); - } - - for (int i = _real_width; i < _padded_width; ++i) - *q++ = 0; - } - - for (int j = _real_height; j < _padded_height; ++j) - { - for (int i = 0; i < _padded_width; ++i) - *q++ = 0; - } - } - break; - - case RS2_FORMAT_Y16: - { - uint16_t* from = (uint16_t*)frame_data_in; - uint16_t* p = nullptr; - uint16_t* q = (uint16_t*)frame_data_out; - - for (int j = 0; j < _real_height; ++j) - { - for (int i = 0; i < _real_width; ++i) - { - p = from + scale * (j * width_in + i); - sum = 0; - for (size_t n = 0; n < scale; ++n) - { - for (size_t m = 0; m < scale; ++m) - sum += p[m]; - - p += width_in; - } - - *q++ = (uint16_t)(sum / patch_size); - } - - for (int i = _real_width; i < _padded_width; ++i) - *q++ = 0; - } - - for (int j = _real_height; j < _padded_height; ++j) - { - for (int i = 0; i < _padded_width; ++i) - *q++ = 0; - } - } - break; - - default: - break; - } } + } diff --git a/src/proc/rotation-filter.h b/src/proc/rotation-filter.h index 4006edd9af..395d31aa7c 100644 --- a/src/proc/rotation-filter.h +++ b/src/proc/rotation-filter.h @@ -19,31 +19,23 @@ namespace librealsense 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( const uint8_t * frame_data_in, uint8_t * const frame_data_out, - size_t width_in, size_t height_in); + void rotate_depth( uint8_t * const out, const uint8_t * source, int width, int height ); - void rotate_others(rs2_format format, const void * frame_data_in, void * frame_data_out, - size_t width_in, size_t height_in, size_t scale); rs2::frame process_frame(const rs2::frame_source& source, const rs2::frame& f) override; private: void update_output_profile(const rs2::frame& f); - int _decimation_factor; int _control_val; - int _patch_size; - int _kernel_size; + int _patch_size; rs2::stream_profile _source_stream_profile; rs2::stream_profile _target_stream_profile; - std::map, rs2::stream_profile> _registered_profiles; - uint16_t _real_width; // Number of rows/columns with real datain the decimated image - uint16_t _real_height; // Correspond to w,h in the reference code - uint16_t _padded_width; // Corresponds to w4/h4 in the reference code - uint16_t _padded_height; - bool _recalc_profile; - bool _options_changed; // Tracking changes imposed by user + 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 );