Skip to content

Commit

Permalink
Cleaner code in motion-transform
Browse files Browse the repository at this point in the history
  • Loading branch information
@OhadMeir
OhadMeir committed May 29, 2024
1 parent e70cded commit 99e9065
Show file tree
Hide file tree
Showing 6 changed files with 142 additions and 105 deletions.
4 changes: 2 additions & 2 deletions src/backend-device.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,8 @@ class backend_device : public virtual device
uint16_t get_pid() const { return _pid; }
std::shared_ptr< platform::backend > get_backend();

// Some FW versions use 16 bit accel values and some use 32 bit for higher accuracy.
virtual bool is_accel_high_accuracy() const { return false; }
// Some FW versions use 16 bit imu register values and some use 32 bit for higher accuracy.
virtual bool is_imu_high_accuracy() const { return false; }
// Gyro scale factor (raw to physical) can be different between product lines and FW versions.
virtual double get_gyro_default_scale() const { return 0.1; }

Expand Down
9 changes: 4 additions & 5 deletions src/ds/d400/d400-motion.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,14 +80,13 @@ namespace librealsense
}
catch (...) {}

// For FW >=5.16 the scale factor changed to 0.0001 to support higher resolution (diff between two adjacent samples)
double gyro_scale_factor = _fw_version >= firmware_version( 5, 16, 0, 0 ) ? 0.0001 : 0.1 ;

double gyro_scale_factor = get_gyro_default_scale();
bool high_accuracy = is_imu_high_accuracy();
motion_ep->register_processing_block(
{ {RS2_FORMAT_MOTION_XYZ32F} },
{ {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL}, {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} },
[&, mm_correct_opt, gyro_scale_factor]()
{ return std::make_shared< motion_to_accel_gyro >( _mm_calib, mm_correct_opt, gyro_scale_factor );
{ return std::make_shared< motion_to_accel_gyro >( _mm_calib, mm_correct_opt, gyro_scale_factor, high_accuracy );
});

return motion_ep;
Expand Down Expand Up @@ -150,7 +149,7 @@ namespace librealsense
return std::dynamic_pointer_cast< hid_sensor >( raw_sensor );
}

bool d400_motion::is_accel_high_accuracy() const
bool d400_motion::is_imu_high_accuracy() const
{
// D400 FW 5.16 and above use 32 bits in the struct, instead of 16.
return _fw_version >= firmware_version( 5, 16, 0, 0 );
Expand Down
2 changes: 1 addition & 1 deletion src/ds/d400/d400-motion.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ namespace librealsense
ds_motion_sensor & get_motion_sensor();
std::shared_ptr<hid_sensor > get_raw_motion_sensor();

bool is_accel_high_accuracy() const override;
bool is_imu_high_accuracy() const override;
double get_gyro_default_scale() const override;

protected:
Expand Down
6 changes: 3 additions & 3 deletions src/ds/ds-motion-common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -504,7 +504,7 @@ namespace librealsense
}
catch (...) {}

bool high_accuracy = _owner->is_accel_high_accuracy();
bool high_accuracy = _owner->is_imu_high_accuracy();
hid_ep->register_processing_block(
{ {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL} },
{ {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL} },
Expand All @@ -516,8 +516,8 @@ namespace librealsense
hid_ep->register_processing_block(
{ {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} },
{ {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} },
[&, mm_correct_opt, gyro_scale_factor]() {
return std::make_shared< gyroscope_transform >( _mm_calib, mm_correct_opt, gyro_scale_factor );
[&, mm_correct_opt, gyro_scale_factor, high_accuracy]()
{ return std::make_shared< gyroscope_transform >( _mm_calib, mm_correct_opt, gyro_scale_factor, high_accuracy );
});

return hid_ep;
Expand Down
178 changes: 104 additions & 74 deletions src/proc/motion-transform.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,75 +12,87 @@

namespace librealsense
{
void copy_hid_axes( uint8_t * const dest[], const uint8_t * source, double factor, bool high_accuracy, bool is_mipi)
class converter_16_bit : public imu_to_librs_converter
{
using namespace librealsense;
public:
converter_16_bit( double scale_factor ) : imu_to_librs_converter( scale_factor )
{
}

auto res = float3();
void convert( uint8_t * const dest[], const uint8_t * source ) override
{
// The backend puts the data in a struct with 32 bit fields, data is valid at the lower 16 bits only.
// Converting to int16_t before casting to float avoids incorrect handling of negative values and overflows.
hid_data hid = *reinterpret_cast< const hid_data * >( source );
hid.x = static_cast< int16_t >( hid.x );
hid.y = static_cast< int16_t >( hid.y );
hid.z = static_cast< int16_t >( hid.z );

float3 res = float3{ float( hid.x ), float( hid.y ), float( hid.z ) } * float( _scale_factor );
std::memcpy( dest[0], &res, sizeof( float3 ) );
}
};

if (is_mipi)
class converter_32_bit : public imu_to_librs_converter
{
public:
converter_32_bit( double scale_factor ) : imu_to_librs_converter( scale_factor )
{
if( ! high_accuracy )
{
auto hid = (hid_mipi_data *)( source );
res = float3{ float( hid->x ), float( hid->y ), float( hid->z ) } * float( factor );
}
else
{
auto hid = (hid_mipi_data_32 *)( source );
res = float3{ float( hid->x ), float( hid->y ), float( hid->z ) } * float( factor );
}
}
else

void convert( uint8_t * const dest[], const uint8_t * source ) override
{
auto hid = (hid_data*)(source);
// The backend puts the data in a struct with 32 bit fields. When accuracy is not high data is valid at the
// lower 16 bits only. We convert to int16_t before casting to float to avoid incorrect handling of negative
// values and overflows.
if( ! high_accuracy )
{
hid->x = static_cast< int16_t >( hid->x );
hid->y = static_cast< int16_t >( hid->y );
hid->z = static_cast< int16_t >( hid->z );
}
const hid_data * hid = reinterpret_cast< const hid_data * >( source );
float3 res = float3{ float( hid->x ), float( hid->y ), float( hid->z ) } * float( _scale_factor );
std::memcpy( dest[0], &res, sizeof( float3 ) );
}
};

res = float3{ float( hid->x ), float( hid->y ), float( hid->z ) } * float( factor );
class converter_16_bit_mipi : public imu_to_librs_converter
{
public:
converter_16_bit_mipi( double scale_factor ) : imu_to_librs_converter( scale_factor )
{
}

std::memcpy( dest[0], &res, sizeof( float3 ) );
}
void convert( uint8_t * const dest[], const uint8_t * source ) override
{
const hid_mipi_data * hid = reinterpret_cast< const hid_mipi_data * >( source );
float3 res = float3{ float( hid->x ), float( hid->y ), float( hid->z ) } * float( _scale_factor );
std::memcpy( dest[0], &res, sizeof( float3 ) );
}
};

// The Accelerometer input format: signed int 16bit. data units 1LSB=0.001g;
// Librealsense output format: floating point 32bit. units m/s^2,
void unpack_accel_axes( uint8_t * const dest[], const uint8_t * source, int width, int height, int output_size, bool high_accuracy, bool is_mipi = false)
class converter_32_bit_mipi : public imu_to_librs_converter
{
static constexpr float gravity = 9.80665f; // Standard Gravitation Acceleration
static constexpr double accelerator_transform_factor = 0.001*gravity;
public:
converter_32_bit_mipi( double scale_factor ) : imu_to_librs_converter( scale_factor )
{
}

copy_hid_axes( dest, source, accelerator_transform_factor, high_accuracy, is_mipi );
}
void convert( uint8_t * const dest[], const uint8_t * source ) override
{
const hid_mipi_data_32 * hid = reinterpret_cast< const hid_mipi_data_32 * >( source );
float3 res = float3{ float( hid->x ), float( hid->y ), float( hid->z ) } * float( _scale_factor );
std::memcpy( dest[0], &res, sizeof( float3 ) );
}
};

// The Gyro input format: signed int 16bit. data units depends on set sensitivity;
// Librealsense output format: floating point 32bit. units rad/sec,
void unpack_gyro_axes( uint8_t * const dest[], const uint8_t * source, int width, int height, int output_size,
double gyro_scale_factor = 0.1, bool is_mipi = false )
motion_transform::motion_transform( rs2_format target_format,
rs2_stream target_stream,
std::shared_ptr< mm_calib_handler > mm_calib,
std::shared_ptr< enable_motion_correction > mm_correct_opt )
: motion_transform( "Motion Transform", target_format, target_stream, mm_calib, mm_correct_opt )
{
const double gyro_transform_factor = deg2rad( gyro_scale_factor );
//high_accuracy=true (32 bit fields) when gyro_scale_factor=0.0001 for D400 FW version >= 5.16
//high_accuracy=false (16 bit fields) when gyro_scale_factor=0.1 for D400 FW version < 5.16 or 0.003814697265625 for D500
bool high_accuracy = ( gyro_scale_factor == 0.0001 );
copy_hid_axes( dest, source, gyro_transform_factor, high_accuracy, is_mipi );
}

motion_transform::motion_transform(rs2_format target_format, rs2_stream target_stream,
std::shared_ptr<mm_calib_handler> mm_calib, std::shared_ptr<enable_motion_correction> mm_correct_opt)
: motion_transform("Motion Transform", target_format, target_stream, mm_calib, mm_correct_opt)
{}

motion_transform::motion_transform(const char* name, rs2_format target_format, rs2_stream target_stream,
std::shared_ptr<mm_calib_handler> mm_calib, std::shared_ptr<enable_motion_correction> mm_correct_opt)
: functional_processing_block(name, target_format, target_stream, RS2_EXTENSION_MOTION_FRAME),
_mm_correct_opt(mm_correct_opt)
motion_transform::motion_transform( const char * name,
rs2_format target_format,
rs2_stream target_stream,
std::shared_ptr< mm_calib_handler > mm_calib,
std::shared_ptr< enable_motion_correction > mm_correct_opt )
: functional_processing_block( name, target_format, target_stream, RS2_EXTENSION_MOTION_FRAME )
, _mm_correct_opt( mm_correct_opt )
{
if (mm_calib)
{
Expand Down Expand Up @@ -142,17 +154,22 @@ namespace librealsense

motion_to_accel_gyro::motion_to_accel_gyro( std::shared_ptr< mm_calib_handler > mm_calib,
std::shared_ptr< enable_motion_correction > mm_correct_opt,
double gyro_scale_factor )
: motion_to_accel_gyro( "Accel_Gyro Transform", mm_calib, mm_correct_opt, gyro_scale_factor )
double gyro_scale_factor,
bool high_accuracy )
: motion_to_accel_gyro( "Accel_Gyro Transform", mm_calib, mm_correct_opt, gyro_scale_factor, high_accuracy )
{}

motion_to_accel_gyro::motion_to_accel_gyro( const char * name,
std::shared_ptr< mm_calib_handler > mm_calib,
std::shared_ptr< enable_motion_correction > mm_correct_opt,
double gyro_scale_factor )
double gyro_scale_factor,
bool high_accuracy )
: motion_transform(name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ANY, mm_calib, mm_correct_opt)
, _gyro_scale_factor(gyro_scale_factor)
{
if( high_accuracy )
_converter = std::make_unique< converter_32_bit_mipi >( deg2rad( gyro_scale_factor ) );
else
_converter = std::make_unique< converter_16_bit_mipi >( deg2rad( gyro_scale_factor ) );
configure_processing_callback();
}

Expand Down Expand Up @@ -208,23 +225,22 @@ namespace librealsense
set_processing_callback( make_frame_processor_callback( std::move( process_callback ) ) );
}

void motion_to_accel_gyro::process_function( uint8_t * const dest[], const uint8_t * source, int width, int height, int output_size, int actual_size)
void motion_to_accel_gyro::process_function( uint8_t * const dest[], const uint8_t * source, int, int, int, int )
{
if (source[0] == 1)
{
_target_stream = RS2_STREAM_ACCEL;
bool high_accuracy = ( _gyro_scale_factor != 0.1 );
unpack_accel_axes( dest, source, width, height, actual_size, high_accuracy, true );
}
else if (source[0] == 2)
{
_target_stream = RS2_STREAM_GYRO;
unpack_gyro_axes( dest, source, width, height, actual_size, _gyro_scale_factor, true );
}
else
{
throw("motion_to_accel_gyro::process_function - stream type not discovered");
}

_converter->convert( dest, source );
}

acceleration_transform::acceleration_transform( std::shared_ptr< mm_calib_handler > mm_calib,
Expand All @@ -233,33 +249,47 @@ namespace librealsense
: acceleration_transform( "Acceleration Transform", mm_calib, mm_correct_opt, high_accuracy )
{}

acceleration_transform::acceleration_transform(const char * name, std::shared_ptr<mm_calib_handler> mm_calib, std::shared_ptr<enable_motion_correction> mm_correct_opt, bool high_accuracy)
: motion_transform( name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL, mm_calib, mm_correct_opt)
, _high_accuracy( high_accuracy )
{}
acceleration_transform::acceleration_transform( const char * name,
std::shared_ptr< mm_calib_handler > mm_calib,
std::shared_ptr< enable_motion_correction > mm_correct_opt,
bool high_accuracy )
: motion_transform( name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL, mm_calib, mm_correct_opt )
{
static constexpr float gravity = 9.80665f; // Standard Gravitation Acceleration
static constexpr double accelerator_scale_factor = 0.001 * gravity;

if( high_accuracy )
_converter = std::make_unique< converter_32_bit >( accelerator_scale_factor );
else
_converter = std::make_unique< converter_16_bit >( accelerator_scale_factor );
}

void acceleration_transform::process_function( uint8_t * const dest[], const uint8_t * source, int width, int height, int output_size, int actual_size )
void acceleration_transform::process_function( uint8_t * const dest[], const uint8_t * source, int, int, int, int )
{
unpack_accel_axes( dest, source, width, height, actual_size, _high_accuracy );
_converter->convert( dest, source );
}

gyroscope_transform::gyroscope_transform( std::shared_ptr< mm_calib_handler > mm_calib,
std::shared_ptr< enable_motion_correction > mm_correct_opt,
double gyro_scale_factor )
: gyroscope_transform( "Gyroscope Transform", mm_calib, mm_correct_opt, gyro_scale_factor )
double gyro_scale_factor, bool high_accuracy )
: gyroscope_transform( "Gyroscope Transform", mm_calib, mm_correct_opt, gyro_scale_factor, high_accuracy )
{}

gyroscope_transform::gyroscope_transform( const char * name,
std::shared_ptr< mm_calib_handler > mm_calib,
std::shared_ptr< enable_motion_correction > mm_correct_opt,
double gyro_scale_factor )
double gyro_scale_factor, bool high_accuracy )
: motion_transform(name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO, mm_calib, mm_correct_opt)
, _gyro_scale_factor( gyro_scale_factor )
{}
{
if( high_accuracy )
_converter = std::make_unique< converter_32_bit >( deg2rad( gyro_scale_factor ) );
else
_converter = std::make_unique< converter_16_bit >( deg2rad( gyro_scale_factor ) );
}

void gyroscope_transform::process_function( uint8_t * const dest[], const uint8_t * source, int width, int height, int output_size, int actual_size )
void gyroscope_transform::process_function( uint8_t * const dest[], const uint8_t * source, int, int, int, int )
{
unpack_gyro_axes( dest, source, width, height, actual_size, _gyro_scale_factor );
_converter->convert( dest, source );
}
}

Loading

0 comments on commit 99e9065

Please sign in to comment.