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

Fix motion and motion correction routines #5534

Merged
merged 2 commits into from
Jan 5, 2020
Merged

Fix motion and motion correction routines #5534

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4c76dfe
Fix reported HID frame size required for unpacker
ev-mp Dec 24, 2019
1373bec
Fix behaviour of motion correction control for D400:
ev-mp Dec 24, 2019
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
49 changes: 25 additions & 24 deletions src/ds5/ds5-motion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,11 @@

namespace librealsense
{
std::map<uint32_t, rs2_format> fisheye_fourcc_to_rs2_format = {
const std::map<uint32_t, rs2_format> fisheye_fourcc_to_rs2_format = {
{rs_fourcc('R','A','W','8'), RS2_FORMAT_RAW8},
{rs_fourcc('G','R','E','Y'), RS2_FORMAT_RAW8},
};
std::map<uint32_t, rs2_stream> fisheye_fourcc_to_rs2_stream = {
const std::map<uint32_t, rs2_stream> fisheye_fourcc_to_rs2_stream = {
{rs_fourcc('R','A','W','8'), RS2_STREAM_FISHEYE},
{rs_fourcc('G','R','E','Y'), RS2_STREAM_FISHEYE},
};
Expand Down Expand Up @@ -163,10 +163,10 @@ namespace librealsense
rs2_motion_device_intrinsic ds5_motion::get_motion_intrinsics(rs2_stream stream) const
{
if (stream == RS2_STREAM_ACCEL)
return create_motion_intrinsics(*_accel_intrinsic);
return create_motion_intrinsics(**_accel_intrinsic);

if (stream == RS2_STREAM_GYRO)
return create_motion_intrinsics(*_gyro_intrinsic);
return create_motion_intrinsics(**_gyro_intrinsic);

throw std::runtime_error(to_string() << "Motion Intrinsics unknown for stream " << rs2_stream_to_string(stream) << "!");
}
Expand Down Expand Up @@ -214,22 +214,32 @@ namespace librealsense
hid_ep->register_option(RS2_OPTION_GLOBAL_TIME_ENABLED, enable_global_time_option);

// register pre-processing
bool enable_motion_correction = false;
if (hid_ep->supports_option(RS2_OPTION_ENABLE_MOTION_CORRECTION))
bool enable_imu_correction = false;
std::shared_ptr<enable_motion_correction> mm_correct_opt = nullptr;

// Motion intrinsic calibration presents is a prerequisite for motion correction.
try
{
auto&& motion_correction_opt = hid_ep->get_option(RS2_OPTION_ENABLE_MOTION_CORRECTION);
enable_motion_correction = motion_correction_opt.is_enabled();
// Writing to log to dereference underlying structure
LOG_INFO("Accel Sensitivity:" << (**_accel_intrinsic).sensitivity);
LOG_INFO("Gyro Sensitivity:" << (**_gyro_intrinsic).sensitivity);

mm_correct_opt = std::make_shared<enable_motion_correction>(hid_ep.get(),
option_range{ 0, 1, 1, 1 });
hid_ep->register_option(RS2_OPTION_ENABLE_MOTION_CORRECTION, mm_correct_opt);
}
catch (...) {}

hid_ep->register_processing_block(
{ {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL} },
{ {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL} },
[&, enable_motion_correction]() { return std::make_shared<acceleration_transform>(_mm_calib, enable_motion_correction);
[&, mm_correct_opt]() { return std::make_shared<acceleration_transform>(_mm_calib, mm_correct_opt);
});

hid_ep->register_processing_block(
{ {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} },
{ {RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO} },
[&, enable_motion_correction]() { return std::make_shared<gyroscope_transform>(_mm_calib, enable_motion_correction);
[&, mm_correct_opt]() { return std::make_shared<gyroscope_transform>(_mm_calib, mm_correct_opt);
});

uint16_t pid = static_cast<uint16_t>(strtoul(all_hid_infos.front().pid.data(), nullptr, 16));
Expand Down Expand Up @@ -311,34 +321,25 @@ namespace librealsense

_mm_calib = std::make_shared<mm_calib_handler>(_hw_monitor,_device_capabilities);

_accel_intrinsic = [this]() { return _mm_calib->get_intrinsic(RS2_STREAM_ACCEL); };
_gyro_intrinsic = [this]() { return _mm_calib->get_intrinsic(RS2_STREAM_GYRO); };
_accel_intrinsic = std::make_shared<lazy<ds::imu_intrinsic>>([this]() { return _mm_calib->get_intrinsic(RS2_STREAM_ACCEL); });
_gyro_intrinsic = std::make_shared<lazy<ds::imu_intrinsic>>([this]() { return _mm_calib->get_intrinsic(RS2_STREAM_GYRO); });
// D435i to use predefined values extrinsics
_depth_to_imu = std::make_shared<lazy<rs2_extrinsics>>([this]() { return _mm_calib->get_extrinsic(RS2_STREAM_ACCEL); });

initialize_fisheye_sensor(ctx,group);

// D435i to use predefined values extrinsics
_depth_to_imu = std::make_shared<lazy<rs2_extrinsics>>([this]()
{
return _mm_calib->get_extrinsic(RS2_STREAM_ACCEL);
});

// Make sure all MM streams are positioned with the same extrinsics
environment::get_instance().get_extrinsics_graph().register_extrinsics(*_depth_stream, *_accel_stream, _depth_to_imu);
environment::get_instance().get_extrinsics_graph().register_same_extrinsics(*_accel_stream, *_gyro_stream);
register_stream_to_extrinsic_group(*_gyro_stream, 0);
register_stream_to_extrinsic_group(*_accel_stream, 0);

// Try to add hid endpoint
// Try to add HID endpoint
auto hid_ep = create_hid_device(ctx, group.hid_devices, _fw_version);
if (hid_ep)
{
_motion_module_device_idx = static_cast<uint8_t>(add_sensor(hid_ep));

hid_ep->register_option(RS2_OPTION_ENABLE_MOTION_CORRECTION,
std::make_shared<enable_motion_correction>(hid_ep.get(),
_depth_to_imu,
option_range{ 0, 1, 1, 1 }));

// HID metadata attributes
hid_ep->get_raw_sensor()->register_metadata(RS2_FRAME_METADATA_FRAME_TIMESTAMP, make_hid_header_parser(&platform::hid_header::timestamp));
}
Expand Down
4 changes: 2 additions & 2 deletions src/ds5/ds5-motion.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -239,8 +239,8 @@ namespace librealsense
optional_value<uint8_t> _motion_module_device_idx;

std::shared_ptr<mm_calib_handler> _mm_calib;
lazy<ds::imu_intrinsic> _accel_intrinsic;
lazy<ds::imu_intrinsic> _gyro_intrinsic;
std::shared_ptr<lazy<ds::imu_intrinsic>> _accel_intrinsic;
std::shared_ptr<lazy<ds::imu_intrinsic>> _gyro_intrinsic;
lazy<std::vector<uint8_t>> _fisheye_calibration_table_raw;
std::shared_ptr<lazy<rs2_extrinsics>> _depth_to_imu; // Mechanical installation pose

Expand Down
9 changes: 4 additions & 5 deletions src/ds5/ds5-options.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -168,20 +168,19 @@ namespace librealsense
if (!is_valid(value))
throw invalid_value_exception(to_string() << "set(enable_motion_correction) failed! Given value " << value << " is out of range.");

_is_enabled = value > _opt_range.min;
_is_active = value > _opt_range.min;
_recording_function(*this);
}

float enable_motion_correction::query() const
{
auto is_enabled = _is_enabled.load();
return is_enabled ? _opt_range.max : _opt_range.min;
auto is_active = _is_active.load();
return is_active ? _opt_range.max : _opt_range.min;
}

enable_motion_correction::enable_motion_correction(sensor_base* mm_ep,
std::shared_ptr<librealsense::lazy<rs2_extrinsics>> depth_to_imu,
const option_range& opt_range)
: option_base(opt_range), _is_enabled(true), _depth_to_imu(**depth_to_imu)
: option_base(opt_range), _is_active(true)
{}

void enable_auto_exposure_option::set(float value)
Expand Down
9 changes: 3 additions & 6 deletions src/ds5/ds5-options.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,20 +61,17 @@ namespace librealsense

float query() const override;

bool is_enabled() const override { return _is_enabled.load(); }
bool is_enabled() const override { return true; }

const char* get_description() const override
{
return "Enable/Disable Automatic Motion Data Correction";
}

enable_motion_correction(sensor_base* mm_ep,
std::shared_ptr<librealsense::lazy<rs2_extrinsics>> depth_to_imu,
const option_range& opt_range);
enable_motion_correction(sensor_base* mm_ep, const option_range& opt_range);

private:
std::atomic<bool> _is_enabled;
rs2_extrinsics _depth_to_imu;
std::atomic<bool> _is_active;
};

class enable_auto_exposure_option : public option_base
Expand Down
2 changes: 1 addition & 1 deletion src/linux/backend-hid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -560,7 +560,7 @@ namespace librealsense
sensor_data sens_data{};
sens_data.sensor = hid_sensor{get_sensor_name()};

auto hid_data_size = channel_size - HID_METADATA_SIZE;
auto hid_data_size = channel_size - (metadata ? HID_METADATA_SIZE : 0);
// Populate HID IMU data - Header
metadata_hid_raw meta_data{};
meta_data.header.report_type = md_hid_report_type::hid_report_imu;
Expand Down
78 changes: 42 additions & 36 deletions src/proc/motion-transform.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,15 +1,11 @@
// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2019 Intel Corporation. All Rights Reserved.

#include "ds5/ds5-options.h"
#include "ds5/ds5-motion.h"
#include "synthetic-stream.h"
#include "motion-transform.h"

#include "../include/librealsense2/hpp/rs_sensor.hpp"
#include "../include/librealsense2/hpp/rs_processing.hpp"

#include "context.h"
#include "environment.h"
#include "image.h"

namespace librealsense
{
template<rs2_format FORMAT> void copy_hid_axes(byte * const dest[], const byte * source, double factor)
Expand Down Expand Up @@ -41,15 +37,35 @@ namespace librealsense
copy_hid_axes<FORMAT>(dest, source, gyro_transform_factor);
}

motion_transform::motion_transform(rs2_format target_format, rs2_stream target_stream, std::shared_ptr<mm_calib_handler> mm_calib, bool is_motion_correction_enabled)
: motion_transform("Motion Transform", target_format, target_stream, mm_calib, is_motion_correction_enabled)
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, bool is_motion_correction_enabled)
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_calib(mm_calib),
_is_motion_correction_enabled(is_motion_correction_enabled)
{}
_mm_correct_opt(mm_correct_opt)
{
if (mm_calib)
{
_imu2depth_cs_alignment_matrix = (*mm_calib).imu_to_depth_alignment();
if (_mm_correct_opt)
{
auto accel_intr = (*mm_calib).get_intrinsic(RS2_STREAM_ACCEL);
auto gyro_intr = (*mm_calib).get_intrinsic(RS2_STREAM_GYRO);
_accel_sensitivity = accel_intr.sensitivity;
_accel_bias = accel_intr.bias;
_gyro_sensitivity = gyro_intr.sensitivity;
_gyro_bias = gyro_intr.bias;
}
}
else
{
// TODO Define L500 base transformation alignment
_imu2depth_cs_alignment_matrix = { {1,0,0},{0,1,0}, {0,0,1} };
}
}

rs2::frame motion_transform::process_frame(const rs2::frame_source& source, const rs2::frame& f)
{
Expand All @@ -61,54 +77,44 @@ namespace librealsense

void motion_transform::correct_motion(rs2::frame* f)
{
if (!_mm_calib)
return;

auto xyz = (float3*)(f->get_data());

try
if (_mm_correct_opt)
{
auto accel_intrinsic = _mm_calib->get_intrinsic(RS2_STREAM_ACCEL);
auto gyro_intrinsic = _mm_calib->get_intrinsic(RS2_STREAM_GYRO);

if (_is_motion_correction_enabled)
if ((_mm_correct_opt->query() > 0.f)) // TBD resolve duality of is_enabled/is_active
{
auto&& s = f->get_profile().stream_type();
if (s == RS2_STREAM_ACCEL)
*xyz = (accel_intrinsic.sensitivity * (*xyz)) - accel_intrinsic.bias;
*xyz = (_accel_sensitivity * (*xyz)) - _accel_bias;

if (s == RS2_STREAM_GYRO)
*xyz = gyro_intrinsic.sensitivity * (*xyz) - gyro_intrinsic.bias;
*xyz = _gyro_sensitivity * (*xyz) - _gyro_bias;
}
}
catch (const std::exception& ex)
{
LOG_INFO("Motion Module - no intrinsic calibration, " << ex.what());
}

// The IMU sensor orientation shall be aligned with depth sensor's coordinate system
*xyz = _mm_calib->imu_to_depth_alignment() * (*xyz);
*xyz = _imu2depth_cs_alignment_matrix * (*xyz);
}

acceleration_transform::acceleration_transform(std::shared_ptr<mm_calib_handler> mm_calib, bool is_motion_correction_enabled)
: acceleration_transform("Acceleration Transform", mm_calib, is_motion_correction_enabled)
acceleration_transform::acceleration_transform(std::shared_ptr<mm_calib_handler> mm_calib, std::shared_ptr<enable_motion_correction> mm_correct_opt)
: acceleration_transform("Acceleration Transform", mm_calib, mm_correct_opt)
{}

acceleration_transform::acceleration_transform(const char * name, std::shared_ptr<mm_calib_handler> mm_calib, bool is_motion_correction_enabled)
: motion_transform(name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL, mm_calib, is_motion_correction_enabled)
acceleration_transform::acceleration_transform(const char * name, std::shared_ptr<mm_calib_handler> mm_calib, std::shared_ptr<enable_motion_correction> mm_correct_opt)
: motion_transform(name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_ACCEL, mm_calib, mm_correct_opt)
{}

void acceleration_transform::process_function(byte * const dest[], const byte * source, int width, int height, int actual_size)
{
unpack_accel_axes<RS2_FORMAT_MOTION_XYZ32F>(dest, source, width, height, actual_size);
}

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

gyroscope_transform::gyroscope_transform(const char * name, std::shared_ptr<mm_calib_handler> mm_calib, bool is_motion_correction_enabled)
: motion_transform(name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO, mm_calib, is_motion_correction_enabled)
gyroscope_transform::gyroscope_transform(const char * name, std::shared_ptr<mm_calib_handler> mm_calib, std::shared_ptr<enable_motion_correction> mm_correct_opt)
: motion_transform(name, RS2_FORMAT_MOTION_XYZ32F, RS2_STREAM_GYRO, mm_calib, mm_correct_opt)
{}

void gyroscope_transform::process_function(byte * const dest[], const byte * source, int width, int height, int actual_size)
Expand Down
34 changes: 21 additions & 13 deletions src/proc/motion-transform.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,45 +3,53 @@

#pragma once

#include "synthetic-stream.h"

#include "ds5/ds5-motion.h"

namespace librealsense
{
class enable_motion_correction;
class mm_calib_handler;
class functional_processing_block;

class motion_transform : public functional_processing_block
{
public:
motion_transform(rs2_format target_format, rs2_stream target_stream, std::shared_ptr<mm_calib_handler> mm_calib = nullptr, bool is_motion_correction_enabled = false);
motion_transform(rs2_format target_format, rs2_stream target_stream,
std::shared_ptr<mm_calib_handler> mm_calib = nullptr,
std::shared_ptr<enable_motion_correction> mm_correct_opt = nullptr);

protected:
motion_transform(const char* name, rs2_format target_format, rs2_stream target_stream, std::shared_ptr<mm_calib_handler> mm_calib, bool is_motion_correction_enabled);
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);
rs2::frame process_frame(const rs2::frame_source& source, const rs2::frame& f) override;

private:
void correct_motion(rs2::frame* f);

std::shared_ptr<mm_calib_handler> _mm_calib = nullptr;
bool _is_motion_correction_enabled = false;
std::shared_ptr<enable_motion_correction> _mm_correct_opt = nullptr;
float3x3 _accel_sensitivity;
float3 _accel_bias;
float3x3 _gyro_sensitivity;
float3 _gyro_bias;
float3x3 _imu2depth_cs_alignment_matrix; // Transform and align raw IMU axis [x,y,z] to be consistent with the Depth frame CS
};

class acceleration_transform : public motion_transform
{
public:
acceleration_transform(std::shared_ptr<mm_calib_handler> mm_calib = nullptr, bool is_motion_correction_enabled = false);
acceleration_transform(std::shared_ptr<mm_calib_handler> mm_calib = nullptr, std::shared_ptr<enable_motion_correction> mm_correct_opt = nullptr);

protected:
acceleration_transform(const char* name, std::shared_ptr<mm_calib_handler> mm_calib, bool is_motion_correction_enabled);
acceleration_transform(const char* name, std::shared_ptr<mm_calib_handler> mm_calib, std::shared_ptr<enable_motion_correction> mm_correct_opt);
void process_function(byte * const dest[], const byte * source, int width, int height, int actual_size) override;
};

class gyroscope_transform : public motion_transform
{
public:
gyroscope_transform(std::shared_ptr<mm_calib_handler> mm_calib = nullptr, bool is_motion_correction_enabled = false);
gyroscope_transform(std::shared_ptr<mm_calib_handler> mm_calib = nullptr, std::shared_ptr<enable_motion_correction> mm_correct_opt = nullptr);

protected:
gyroscope_transform(const char* name, std::shared_ptr<mm_calib_handler> mm_calib, bool is_motion_correction_enabled);
gyroscope_transform(const char* name, std::shared_ptr<mm_calib_handler> mm_calib, std::shared_ptr<enable_motion_correction> mm_correct_opt);
void process_function(byte * const dest[], const byte * source, int width, int height, int actual_size) override;
};
}
}