Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enable rotate frames for RGB camera #13733

Open
wants to merge 2 commits into
base: development
Choose a base branch
from
Open

Enable rotate frames for RGB camera #13733

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
58f262c
add rotation ppf for RGB
noacoohen Feb 3, 2025
5ee7e4d
fix PR comments
noacoohen Feb 6, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
141 changes: 116 additions & 25 deletions src/proc/rotation-filter.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@
#include "stream.h"
#include "core/video.h"
#include "proc/rotation-filter.h"
#include <rsutils/easylogging/easyloggingpp.h>

namespace librealsense {

Expand Down Expand Up @@ -52,32 +53,51 @@ namespace librealsense {
register_option( RS2_OPTION_ROTATION, rotation_control );
}

rotation_filter::rotation_filter( std::vector< rs2_stream > streams_to_rotate )
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Code duplication. Is default constructor needed? If so have it call the other constructor with an empty vector {}.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fixed

: 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 );
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Lock after validating if valid value.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done


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 )
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should lock _mutex or place _value in a local variable and use it all over this function.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Same for update_output_profile and rotate_frame.

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();
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We already have the profile from 1 line above, you can do profile.format() here right?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fixed

_target_stream_profile = profile;

if( _value == 90 || _value == -90 )
Expand All @@ -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))
{
Expand All @@ -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" );
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we intend to support this at a future PR?

Copy link
Contributor Author

@noacoohen noacoohen Feb 6, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No support for 90 and 270 degrees rotation for YUYV format

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() ) ),
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Seems that using bpp as a normal function parameter instead of template will reduce code duplication.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done

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;
}

Expand Down Expand Up @@ -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 )
{
Expand All @@ -220,6 +279,38 @@ namespace librealsense {
}
}
}


void rotation_filter::rotate_YUYV_frame(uint8_t* const out, const uint8_t* source, int width, int height) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please use formatter. Spaces, { at new line, etc...

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Done

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
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What is this comment for?

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Removed

for( auto & stream_type : _streams_to_rotate ){
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

No need for { } for one line if/for. Where you are using, { should be placed on a new line.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fixed

if( stream_type == type )
{
return true;
}
}
return false;
}

}

6 changes: 5 additions & 1 deletion src/proc/rotation-filter.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
9 changes: 8 additions & 1 deletion src/sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand All @@ -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;
}

Expand Down
Loading