Skip to content

Commit

Permalink
Merge pull request #5534 from ev-mp/motion_correction_fix
Browse files Browse the repository at this point in the history 
Fix motion and motion correction routines
  • Loading branch information
@dorodnic
dorodnic authored Jan 5, 2020
2 parents 5c6cff0 + 1373bec commit c331be9
Show file tree
Hide file tree
Showing 7 changed files with 98 additions and 87 deletions.
49 changes: 25 additions & 24 deletions src/ds5/ds5-motion.cpp
View file
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 @@ -164,10 +164,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 @@ -215,22 +215,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 @@ -312,34 +322,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
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
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
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
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
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
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;
};
}
}

0 comments on commit c331be9

Please sign in to comment.