Skip to content

T265 Mapping/Relocalization/Jumping Options #4321

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

Merged
merged 12 commits into from
Jul 11, 2019
Merged

T265 Mapping/Relocalization/Jumping Options #4321

Show file tree
Hide file tree
Changes from 10 commits
Commits
Show all changes
12 commits
Select commit Hold shift + click to select a range
38ca168
Suppress warning about assigning in an if
radfordi Jun 28, 2019
143805a
libtm: SIXDOF_MODE is a bit fields so allow bit field operations
radfordi Jun 28, 2019
43f6f56
libtm: add support for enabling/disabling jumping
radfordi Jun 28, 2019
df925ab
tm-device: add options for mapping, relocalization and jumping
radfordi Jun 28, 2019
8a7b749
Give details when complaining about failures to inspect an option
radfordi Jun 28, 2019
05f7b0c
Use rs_fourcc to avoid non-standard wide character constants
radfordi Jul 8, 2019
2f05c0c
Pose jumping flag should be inverted for backwards compatability
radfordi Jul 8, 2019
f263620
Support disabling dynamic calibration
radfordi Jul 8, 2019
8510332
Remove explicit glfw droppings from realsense-gl (taken care of by th…
radfordi Jul 9, 2019
15c47fe
Upgrade TM2 firmware to 0.0.18.6100
radfordi Jul 9, 2019
f865e2f
Trivial cleanups of doc/t265.md
radfordi Jul 11, 2019
de76d3a
Documentation of the T365 specific options
radfordi Jul 11, 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
4 changes: 4 additions & 0 deletions include/librealsense2/h/rs_option.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,10 @@ extern "C" {
RS2_OPTION_HARDWARE_PRESET, /**< Hardware stream configuration */
RS2_OPTION_GLOBAL_TIME_ENABLED, /**< disable global time */
RS2_OPTION_APD_TEMPERATURE, /**< APD temperature*/
RS2_OPTION_ENABLE_MAPPING, /**< Enable an internal map */
Copy link
Collaborator

@ev-mp ev-mp Jul 10, 2019

Choose a reason for hiding this comment

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

The new features/usage also should be elaborated in Readme

RS2_OPTION_ENABLE_RELOCALIZATION, /**< Enable appearance based relocalization */
RS2_OPTION_ENABLE_POSE_JUMPING, /**< Enable position jumping */
RS2_OPTION_ENABLE_DYNAMIC_CALIBRATION, /**< Enable dynamic calibration */
RS2_OPTION_COUNT /**< Number of enumeration values. Not a valid input: intended to be used in for-loops. */
} rs2_option;

Expand Down
4 changes: 2 additions & 2 deletions src/ds5/ds5-factory.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -663,7 +663,7 @@ namespace librealsense
LOG_WARNING("invalid RGB extrinsic was identified, recovery routine was invoked");
try
{
if (res = is_rgb_extrinsic_valid(read_rgb_gold()))
if ((res = is_rgb_extrinsic_valid(read_rgb_gold())))
{
restore_calib_factory_settings();
}
Expand All @@ -674,7 +674,7 @@ namespace librealsense
const uint32_t gold_address = 0x17c49c;
const uint16_t bytes_to_read = 0x100;
auto alt_calib = read_sector(gold_address, bytes_to_read);
if (res = is_rgb_extrinsic_valid(alt_calib))
if ((res = is_rgb_extrinsic_valid(alt_calib)))
assign_rgb_stream_extrinsic(alt_calib);
else
res = false;
Expand Down
8 changes: 1 addition & 7 deletions src/gl/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,8 +33,6 @@ set(REALSENSE_GL_CPP
${LZ4_DIR}/lz4.c
)

#set(DEPENDENCIES realsense2 ${OPENGL_LIBRARIES} glfw)
#replaced with
include(${CMAKE_SOURCE_DIR}/CMake/opengl_config.cmake)

if (${BUILD_SHARED_LIBS} AND ${BUILD_EASYLOGGINGPP})
Expand All @@ -57,20 +55,16 @@ endif()

include_directories(${LZ4_DIR})

#TODO - replace glfw dependency with
#find_package(glfw REQUIRED)
target_include_directories(${PROJECT_NAME}
PRIVATE
${CMAKE_CURRENT_LIST_DIR}/..
${CMAKE_CURRENT_LIST_DIR}/../../third-party/glad
${CMAKE_CURRENT_LIST_DIR}/../../common
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../../include>
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/../../third-party/glfw/include>
$<INSTALL_INTERFACE:/../../include>
$<INSTALL_INTERFACE:/../../third-party/glfw/include>
)

set_target_properties(${PROJECT_NAME} PROPERTIES FOLDER Library)
set_target_properties(${PROJECT_NAME} PROPERTIES VERSION ${REALSENSE_VERSION_STRING} SOVERSION "${REALSENSE_VERSION_MAJOR}.${REALSENSE_VERSION_MINOR}")
write_basic_package_version_file("${CMAKE_CURRENT_BINARY_DIR}/realsense2-glConfigVersion.cmake"
Expand Down
132 changes: 66 additions & 66 deletions src/image.cpp
View file
Open in desktop

Large diffs are not rendered by default.

4 changes: 2 additions & 2 deletions src/sensor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -749,9 +749,9 @@ namespace librealsense

void uvc_sensor::try_register_pu(rs2_option id)
{
auto opt = std::make_shared<uvc_pu_option>(*this, id);
try
{
auto opt = std::make_shared<uvc_pu_option>(*this, id);
auto range = opt->get_range();
if (range.max <= range.min || range.step <= 0 || range.def < range.min || range.def > range.max) return;

Expand All @@ -763,7 +763,7 @@ namespace librealsense
}
catch (...)
{
LOG_WARNING("Exception was thrown when inspecting properties of a sensor");
LOG_WARNING("Exception was thrown when inspecting " << this->get_info(RS2_CAMERA_INFO_NAME) << " property " << opt->get_description());
}
}

Expand Down
10 changes: 0 additions & 10 deletions src/sensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,16 +20,6 @@
#include <functional>
#include <core/debug.h>

template<typename T>
uint32_t rs_fourcc(const T a, const T b, const T c, const T d)
{
static_assert((std::is_integral<T>::value), "rs_fourcc supports integral built-in types only");
return ((static_cast<uint32_t>(a) << 24) |
(static_cast<uint32_t>(b) << 16) |
(static_cast<uint32_t>(c) << 8) |
(static_cast<uint32_t>(d) << 0));
}

namespace librealsense
{
class device;
Expand Down
31 changes: 31 additions & 0 deletions src/tm2/tm-device.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -178,6 +178,32 @@ namespace librealsense
tm2_sensor& _ep;
};

template <perc::SIXDOF_MODE flag, perc::SIXDOF_MODE depends_on, bool invert = false>
class tracking_mode_option : public option_base
{
public:
float query() const override { return !!(s._tm_mode & flag) ^ invert ? 1 : 0; }

void set(float value) override {
if (s._is_streaming)
throw io_exception("Option is read-only while streaming");
s._tm_mode = (!!value ^ invert) ? (s._tm_mode | flag) : (s._tm_mode & ~flag);
}

const char* get_description() const override { return description; }

bool is_enabled() const override { return !depends_on || (s._tm_mode & depends_on) ? true : false; }

bool is_read_only() const override { return s._is_streaming; }

explicit tracking_mode_option(tm2_sensor& sensor, const char *description_) :
s(sensor), description(description_), option_base(option_range{ 0, 1, !!(sensor._tm_mode & flag) ^ invert ? 1.f : 0.f, 1 }) { }

private:
tm2_sensor &s;
const char *description;
};

class asic_temperature_option : public temperature_option
{
public:
Expand Down Expand Up @@ -563,6 +589,7 @@ namespace librealsense
case RS2_STREAM_POSE:
{
auto tm_profile = _tm_supported_profiles.sixDof[stream_index];
tm_profile.mode = _tm_mode;
Copy link
Collaborator

@ev-mp ev-mp Jul 2, 2019

Choose a reason for hiding this comment

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

These options are for Pose stream only. Is it possible to configure those as N/A if the user request raw IMU and/or FE streams without Pose ?

Copy link
Contributor Author

@radfordi radfordi Jul 2, 2019

Choose a reason for hiding this comment

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

The mode is already set to reasonable defaults, as part of the profiles, for all stream types (including pose). For pose I override it rather than create a permutation of all possible modes in the profiles and then have to select the profile that is asked for. mode doesn't quite fit the profile concept, but it's the only way we have to pass these parameters for now.

if (convertTm2InterruptRate(tm_profile.interruptRate) == sp.fps)
{
_tm_active_profiles.set(tm_profile, true);
Expand Down Expand Up @@ -1439,6 +1466,10 @@ namespace librealsense
_sensor->register_option(rs2_option::RS2_OPTION_GAIN, std::make_shared<gain_option>(*_sensor));
_sensor->register_option(rs2_option::RS2_OPTION_ENABLE_AUTO_EXPOSURE, std::make_shared<exposure_mode_option>(*_sensor));

_sensor->register_option(rs2_option::RS2_OPTION_ENABLE_MAPPING, std::make_shared<tracking_mode_option<perc::SIXDOF_MODE_ENABLE_MAPPING, perc::SIXDOF_MODE_NORMAL, false>>(*_sensor, "Use an on device map (recommended)"));
_sensor->register_option(rs2_option::RS2_OPTION_ENABLE_RELOCALIZATION, std::make_shared<tracking_mode_option<perc::SIXDOF_MODE_ENABLE_RELOCALIZATION, perc::SIXDOF_MODE_ENABLE_MAPPING, false>>(*_sensor, "Use appearance based relocalization (depends on mapping)"));
_sensor->register_option(rs2_option::RS2_OPTION_ENABLE_POSE_JUMPING, std::make_shared<tracking_mode_option<perc::SIXDOF_MODE_DISABLE_JUMPING, perc::SIXDOF_MODE_ENABLE_MAPPING, true>>(*_sensor, "Allow pose jumping (depends on mapping)"));
_sensor->register_option(rs2_option::RS2_OPTION_ENABLE_DYNAMIC_CALIBRATION, std::make_shared<tracking_mode_option<perc::SIXDOF_MODE_DISABLE_DYNAMIC_CALIBRATION, perc::SIXDOF_MODE_NORMAL, true>>(*_sensor, "Enable dynamic calibration (recommended)"));

// Assing the extrinsic nodes to the default group
auto tm2_profiles = _sensor->get_stream_profiles();
Expand Down
3 changes: 3 additions & 0 deletions src/tm2/tm-device.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -130,12 +130,15 @@ namespace librealsense
std::shared_ptr<playback_device>_loopback;
perc::TrackingData::Profile _tm_supported_profiles;
perc::TrackingData::Profile _tm_active_profiles;
perc::SIXDOF_MODE _tm_mode = perc::SIXDOF_MODE_ENABLE_MAPPING | perc::SIXDOF_MODE_ENABLE_RELOCALIZATION;
mutable std::condition_variable _async_op;
mutable async_op_state _async_op_status;
mutable std::vector<uint8_t> _async_op_res_buffer;

float last_exposure = 200.f;
float last_gain = 1.f;
bool manual_exposure = false;

template <perc::SIXDOF_MODE flag, perc::SIXDOF_MODE depends_on, bool invert> friend class tracking_mode_option;
};
}
4 changes: 4 additions & 0 deletions src/types.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -278,6 +278,10 @@ namespace librealsense
CASE(APD_TEMPERATURE)
CASE(HARDWARE_PRESET)
CASE(GLOBAL_TIME_ENABLED)
CASE(ENABLE_MAPPING)
CASE(ENABLE_RELOCALIZATION)
CASE(ENABLE_POSE_JUMPING)
CASE(ENABLE_DYNAMIC_CALIBRATION)
default: assert(!is_valid(value)); return UNKNOWN_VALUE;
}
#undef CASE
Expand Down
11 changes: 11 additions & 0 deletions src/types.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1710,6 +1710,17 @@ namespace librealsense
bool _valid;
T _value;
};

}

template<typename T>
uint32_t rs_fourcc(const T a, const T b, const T c, const T d)
{
static_assert((std::is_integral<T>::value), "rs_fourcc supports integral built-in types only");
return ((static_cast<uint32_t>(a) << 24) |
(static_cast<uint32_t>(b) << 16) |
(static_cast<uint32_t>(c) << 8) |
(static_cast<uint32_t>(d) << 0));
}

namespace std {
Expand Down
4 changes: 2 additions & 2 deletions third-party/libtm/fw/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,8 @@
# Copyright(c) 2019 Intel Corporation. All Rights Reserved.
cmake_minimum_required(VERSION 3.1.3)

set( FW_VERSION "0.0.18.5715")
set( FW_SHA1 cc12cf05f387d80f65e98c4d41fc883725124a08)
set( FW_VERSION "0.0.18.6100")
set( FW_SHA1 858b786215ff66cf8cf93fa85393ca6268dedee4)
Copy link
Collaborator

@ev-mp ev-mp Jul 10, 2019

Choose a reason for hiding this comment

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

@radfordi , the FW 6100 has already been uploaded. If this PR was pending the FW release - then pls check and confirm to merge.

Copy link
Contributor Author

@radfordi radfordi Jul 10, 2019

Choose a reason for hiding this comment

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

@ev-mp, I confirmed that the FW had been uploaded before pushing this particular commit.

set(APP_VERSION "2.0.19.271")
set(APP_SHA1 cab0011e9e18edc8bcca20afb2f944399ac8b81c)
set( BL_VERSION "1.0.1.112")
Expand Down
22 changes: 20 additions & 2 deletions third-party/libtm/libtm/include/TrackingCommon.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,8 @@
#define fopen_s(pFile,filename,mode) ((*(pFile))=fopen((filename), (mode)))==NULL
#endif

#include <type_traits>

namespace perc {

#ifdef _WIN32
Expand Down Expand Up @@ -138,8 +140,24 @@ namespace perc {
SIXDOF_MODE_NORMAL = 0X0000,
SIXDOF_MODE_FAST_PLAYBACK = 0x0001,
SIXDOF_MODE_ENABLE_MAPPING = 0x0002,
SIXDOF_MODE_ENABLE_RELOCALIZATION = 0x0004,
SIXDOF_MODE_MAX = ((SIXDOF_MODE_FAST_PLAYBACK | SIXDOF_MODE_ENABLE_MAPPING | SIXDOF_MODE_ENABLE_RELOCALIZATION) + 1)
SIXDOF_MODE_ENABLE_RELOCALIZATION = 0x0004,
SIXDOF_MODE_DISABLE_JUMPING = 0x0008,
SIXDOF_MODE_DISABLE_DYNAMIC_CALIBRATION = 0x0010,
SIXDOF_MODE_MAX = ((SIXDOF_MODE_FAST_PLAYBACK | SIXDOF_MODE_ENABLE_MAPPING | SIXDOF_MODE_ENABLE_RELOCALIZATION | SIXDOF_MODE_DISABLE_JUMPING | SIXDOF_MODE_DISABLE_DYNAMIC_CALIBRATION) + 1)
} SIXDOF_MODE;

inline SIXDOF_MODE &operator|=(SIXDOF_MODE &x, SIXDOF_MODE y) {
return x = static_cast<SIXDOF_MODE>(static_cast<typename std::underlying_type<SIXDOF_MODE>::type>(x) |
static_cast<typename std::underlying_type<SIXDOF_MODE>::type>(y));
}
inline SIXDOF_MODE &operator&=(SIXDOF_MODE &x, SIXDOF_MODE y) {
return x = static_cast<SIXDOF_MODE>(static_cast<typename std::underlying_type<SIXDOF_MODE>::type>(x) &
static_cast<typename std::underlying_type<SIXDOF_MODE>::type>(y));
}
inline SIXDOF_MODE operator~(SIXDOF_MODE x) {
return static_cast<SIXDOF_MODE>(~static_cast<typename std::underlying_type<SIXDOF_MODE>::type>(x));
}
inline SIXDOF_MODE operator|(SIXDOF_MODE x, SIXDOF_MODE y) { return x |= y; }
inline SIXDOF_MODE operator&(SIXDOF_MODE x, SIXDOF_MODE y) { return x &= y; }

}
2 changes: 1 addition & 1 deletion third-party/libtm/libtm/src/Device.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -729,7 +729,7 @@ namespace perc {

for (uint8_t i = 0; i < SixDofProfileMax; i++)
{
sixDofProfile.set(false, (SIXDOF_MODE_ENABLE_MAPPING | SIXDOF_MODE_ENABLE_RELOCALIZATION), SIXDOF_INTERRUPT_RATE::SIXDOF_INTERRUPT_RATE_IMU, (SixDofProfileType)i);
sixDofProfile.set(false, SIXDOF_MODE_ENABLE_MAPPING | SIXDOF_MODE_ENABLE_RELOCALIZATION, SIXDOF_INTERRUPT_RATE::SIXDOF_INTERRUPT_RATE_IMU, (SixDofProfileType)i);
profile.set(sixDofProfile, false);
}

Expand Down