Skip to content

Commit

Permalink
PR IntelRealSense#12788 from Eran: DDS scaled intrinsics
Browse files Browse the repository at this point in the history
  • Loading branch information
@maloel
maloel authored Apr 3, 2024
2 parents 29b61a6 + 0ae3078 commit 29ce06f
Show file tree
Hide file tree
Showing 34 changed files with 965 additions and 496 deletions.
76 changes: 51 additions & 25 deletions src/dds/rs-dds-device-proxy.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,12 +47,41 @@ static rs2_stream to_rs2_stream_type( std::string const & type_string )
}


rs2_distortion to_rs2_distortion( realdds::distortion_model model )
{
switch( model )
{
case realdds::distortion_model::none: return RS2_DISTORTION_NONE;
case realdds::distortion_model::brown: return RS2_DISTORTION_BROWN_CONRADY;
case realdds::distortion_model::inverse_brown: return RS2_DISTORTION_INVERSE_BROWN_CONRADY;
case realdds::distortion_model::modified_brown: return RS2_DISTORTION_MODIFIED_BROWN_CONRADY;
default:
throw invalid_value_exception( "unexpected realdds distortion model: " + std::to_string( (int)model ) );
}
}


rs2_intrinsics to_rs2_intrinsics( const realdds::video_intrinsics & intrinsics )
{
rs2_intrinsics intr;
intr.width = intrinsics.width;
intr.height = intrinsics.height;
intr.ppx = intrinsics.principal_point.x;
intr.ppy = intrinsics.principal_point.y;
intr.fx = intrinsics.focal_length.x;
intr.fy = intrinsics.focal_length.y;
intr.model = to_rs2_distortion( intrinsics.distortion.model );
memcpy( intr.coeffs, intrinsics.distortion.coeffs.data(), sizeof( intr.coeffs ) );
return intr;
}


static rs2_video_stream to_rs2_video_stream( rs2_stream const stream_type,
sid_index const & sidx,
std::shared_ptr< realdds::dds_video_stream_profile > const & profile,
const std::set< realdds::video_intrinsics > & intrinsics )
{
rs2_video_stream prof = {};
rs2_video_stream prof;
prof.type = stream_type;
prof.index = sidx.index;
prof.uid = sidx.sid;
Expand All @@ -68,14 +97,7 @@ static rs2_video_stream to_rs2_video_stream( rs2_stream const stream_type,
{ return profile->width() == intr.width && profile->height() == intr.height; } );
if( intr != intrinsics.end() ) // Some profiles don't have intrinsics
{
prof.intrinsics.width = intr->width;
prof.intrinsics.height = intr->height;
prof.intrinsics.ppx = intr->principal_point_x;
prof.intrinsics.ppy = intr->principal_point_y;
prof.intrinsics.fx = intr->focal_lenght_x;
prof.intrinsics.fy = intr->focal_lenght_y;
prof.intrinsics.model = static_cast< rs2_distortion >( intr->distortion_model );
memcpy( prof.intrinsics.coeffs, intr->distortion_coeffs.data(), sizeof( prof.intrinsics.coeffs ) );
prof.intrinsics = to_rs2_intrinsics( *intr );
}

return prof;
Expand Down Expand Up @@ -398,25 +420,29 @@ void dds_device_proxy::set_video_profile_intrinsics( std::shared_ptr< stream_pro
std::shared_ptr< realdds::dds_video_stream > stream ) const
{
auto vsp = std::dynamic_pointer_cast< video_stream_profile >( profile );
auto & stream_intrinsics = stream->get_intrinsics();
auto it = std::find_if( stream_intrinsics.begin(),
stream_intrinsics.end(),
[vsp]( const realdds::video_intrinsics & intr )
{ return vsp->get_width() == intr.width && vsp->get_height() == intr.height; } );
int const w = vsp->get_width();
int const h = vsp->get_height();

if( it != stream_intrinsics.end() ) // Some profiles don't have intrinsics
auto & stream_intrinsics = stream->get_intrinsics();
if( 1 == stream_intrinsics.size() )
{
// A single set of intrinsics will get scaled to any profile resolution
vsp->set_intrinsics( [intrinsics = *stream_intrinsics.begin(), w, h]()
{ return to_rs2_intrinsics( intrinsics.scaled_to( w, h ) ); } );
}
else
{
rs2_intrinsics intr;
intr.width = it->width;
intr.height = it->height;
intr.ppx = it->principal_point_x;
intr.ppy = it->principal_point_y;
intr.fx = it->focal_lenght_x;
intr.fy = it->focal_lenght_y;
intr.model = static_cast< rs2_distortion >( it->distortion_model );
memcpy( intr.coeffs, it->distortion_coeffs.data(), sizeof( intr.coeffs ) );
vsp->set_intrinsics( [intr]() { return intr; } );
// When we have multiple sets of intrinsics (one per resolution), we're limited to these
auto it = std::find_if( stream_intrinsics.begin(),
stream_intrinsics.end(),
[w, h]( const realdds::video_intrinsics & intr )
{ return intr.width == w && intr.height == h; } );
if( it != stream_intrinsics.end() ) // Some profiles don't have intrinsics
{
vsp->set_intrinsics( [intr = to_rs2_intrinsics( *it )]() { return intr; } );
}
}

}


Expand Down
31 changes: 18 additions & 13 deletions src/device.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,23 @@ device::device( std::shared_ptr< const device_info > const & dev_info,
: _dev_info( dev_info )
, _is_alive( std::make_shared< std::atomic< bool > >( true ) )
, _profiles_tags( [this]() { return get_profiles_tags(); } )
, _format_conversion(
[this]
{
auto context = get_context();
if( ! context )
return format_conversion::full;
std::string const format_conversion( "format-conversion", 17 );
std::string const full( "full", 4 );
auto const value = context->get_settings().nested( format_conversion ).default_value( full );
if( value == full )
return format_conversion::full;
if( value == "basic" )
return format_conversion::basic;
if( value == "raw" )
return format_conversion::raw;
throw invalid_value_exception( "invalid " + format_conversion + " value '" + value + "'" );
} )
{
if( device_changed_notifications )
{
Expand Down Expand Up @@ -199,19 +216,7 @@ std::vector<rs2_format> device::map_supported_color_formats(rs2_format source_fo

format_conversion device::get_format_conversion() const
{
auto context = get_context();
if( ! context )
return format_conversion::full;
std::string const format_conversion( "format-conversion", 17 );
std::string const full( "full", 4 );
auto const value = context->get_settings().nested( format_conversion ).default_value( full );
if( value == full )
return format_conversion::full;
if( value == "basic" )
return format_conversion::basic;
if( value == "raw" )
return format_conversion::raw;
throw invalid_value_exception( "invalid " + format_conversion + " value '" + value + "'" );
return *_format_conversion;
}

void device::tag_profiles(stream_profiles profiles) const
Expand Down
1 change: 1 addition & 0 deletions src/device.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,6 +86,7 @@ class device
std::shared_ptr< std::atomic< bool > > _is_alive;
rsutils::subscription _device_change_subscription;
rsutils::lazy< std::vector< tagged_profile > > _profiles_tags;
rsutils::lazy< format_conversion > _format_conversion;
};


Expand Down
8 changes: 8 additions & 0 deletions src/ds/d400/d400-color.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -129,6 +129,14 @@ namespace librealsense
register_feature( std::make_shared< auto_exposure_roi_feature >( get_color_sensor(), _hw_monitor, true ) );
}

rs2_format d400_color::get_color_format() const
{
auto const format_conversion = get_format_conversion();
rs2_format const color_format
= (format_conversion == format_conversion::full) ? RS2_FORMAT_RGB8 : RS2_FORMAT_YUYV;
return color_format;
}

void d400_color::init()
{
auto& color_ep = get_color_sensor();
Expand Down
2 changes: 2 additions & 0 deletions src/ds/d400/d400-color.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,8 @@ namespace librealsense
protected:
void register_color_features();

rs2_format get_color_format() const;

std::shared_ptr<stream_interface> _color_stream;
std::shared_ptr<ds_color_common> _ds_color_common;

Expand Down
7 changes: 7 additions & 0 deletions src/ds/d400/d400-device.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1203,6 +1203,13 @@ namespace librealsense
}


rs2_format d400_device::get_ir_format() const
{
auto const format_conversion = get_format_conversion();
return ( format_conversion == format_conversion::raw ) ? RS2_FORMAT_Y8I : RS2_FORMAT_Y8;
}


double d400_device::get_device_time_ms()
{
//// TODO: Refactor the following query with an extension.
Expand Down
2 changes: 2 additions & 0 deletions src/ds/d400/d400-device.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,8 @@ namespace librealsense

ds::ds_caps parse_device_capabilities( const std::vector<uint8_t> &gvd_buf ) const;

rs2_format get_ir_format() const;

//TODO - add these to device class as pure virtual methods
command get_firmware_logs_command() const;
command get_flash_logs_command() const;
Expand Down
30 changes: 14 additions & 16 deletions src/ds/d400/d400-factory.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -193,13 +193,13 @@ namespace librealsense
auto usb_spec = get_usb_spec();
if (usb_spec >= platform::usb3_type || usb_spec == platform::usb_undefined)
{
tags.push_back({ RS2_STREAM_COLOR, -1, 1280, 720, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_COLOR, -1, 1280, 720, get_color_format(), 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_DEPTH, -1, 1280, 720, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, -1, 1280, 720, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET });
}
else
{
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, -1, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET });
}
Expand Down Expand Up @@ -289,13 +289,13 @@ namespace librealsense
if (usb_spec >= platform::usb3_type || usb_spec == platform::usb_undefined)
{
tags.push_back({ RS2_STREAM_DEPTH, -1, 720, 720, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, 1, 720, 720, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET });
}
else
{
tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, 1, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET });
}
return tags;
Expand Down Expand Up @@ -561,13 +561,13 @@ namespace librealsense
auto usb_spec = get_usb_spec();
if (usb_spec >= platform::usb3_type || usb_spec == platform::usb_undefined)
{
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_DEPTH, -1, 848, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, -1, 848, 480, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET });
}
else
{
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, -1, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET });
}
Expand Down Expand Up @@ -602,7 +602,7 @@ namespace librealsense
{
std::vector<tagged_profile> tags;

tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });

return tags;
Expand Down Expand Up @@ -638,13 +638,13 @@ namespace librealsense
auto usb_spec = get_usb_spec();
if (usb_spec >= platform::usb3_type || usb_spec == platform::usb_undefined)
{
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_DEPTH, -1, 848, 480, RS2_FORMAT_Z16, 30, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, -1, 848, 480, RS2_FORMAT_Y8, 30, profile_tag::PROFILE_TAG_SUPERSET });
}
else
{
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, RS2_FORMAT_RGB8, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_COLOR, -1, 640, 480, get_color_format(), 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_DEPTH, -1, 640, 480, RS2_FORMAT_Z16, 15, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, -1, 640, 480, RS2_FORMAT_Y8, 15, profile_tag::PROFILE_TAG_SUPERSET });
}
Expand Down Expand Up @@ -692,7 +692,7 @@ namespace librealsense
int color_height = usb3mode ? 720 : 480;
int fps = usb3mode ? 30 : 15;

tags.push_back({ RS2_STREAM_COLOR, -1, color_width, color_height, RS2_FORMAT_RGB8, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_COLOR, -1, color_width, color_height, get_color_format(), fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_DEPTH, -1, depth_width, depth_height, RS2_FORMAT_Z16, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, -1, depth_width, depth_height, RS2_FORMAT_Y8, fps, profile_tag::PROFILE_TAG_SUPERSET });
tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
Expand Down Expand Up @@ -935,12 +935,10 @@ namespace librealsense
int color_height = 480;
int fps = usb3mode ? 30 : 10;

auto format_conversion = get_format_conversion();

tags.push_back( { RS2_STREAM_COLOR,
-1, // index
color_width, color_height,
( format_conversion == format_conversion::full ) ? RS2_FORMAT_RGB8 : RS2_FORMAT_YUYV,
get_color_format(),
fps,
profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT } );
tags.push_back( { RS2_STREAM_DEPTH,
Expand All @@ -952,7 +950,7 @@ namespace librealsense
tags.push_back( { RS2_STREAM_INFRARED,
-1, // index
depth_width, depth_height,
( format_conversion == format_conversion::raw ) ? RS2_FORMAT_Y8I : RS2_FORMAT_Y8,
get_ir_format(),
fps,
profile_tag::PROFILE_TAG_SUPERSET } );

Expand Down Expand Up @@ -1028,9 +1026,9 @@ namespace librealsense
int color_height = usb3mode ? 720 : 480;
int fps = usb3mode ? 30 : 15;

tags.push_back({ RS2_STREAM_COLOR, -1, color_width, color_height, RS2_FORMAT_RGB8, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_COLOR, -1, color_width, color_height, get_color_format(), fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_DEPTH, -1, depth_width, depth_height, RS2_FORMAT_Z16, fps, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({ RS2_STREAM_INFRARED, -1, depth_width, depth_height, RS2_FORMAT_Y8, fps, profile_tag::PROFILE_TAG_SUPERSET });
tags.push_back({ RS2_STREAM_INFRARED, -1, depth_width, depth_height, get_ir_format(), fps, profile_tag::PROFILE_TAG_SUPERSET });
tags.push_back({RS2_STREAM_GYRO, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_200, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_63, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
tags.push_back({RS2_STREAM_ACCEL, -1, 0, 0, RS2_FORMAT_MOTION_XYZ32F, (int)odr::IMU_FPS_100, profile_tag::PROFILE_TAG_SUPERSET | profile_tag::PROFILE_TAG_DEFAULT });
Expand Down
Loading

0 comments on commit 29ce06f

Please sign in to comment.