mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2025-02-05 13:28:16 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/euroc: Add euroc device

Browse source
This commit is contained in:
Mateo de Mayo 2021-09-08 23:43:44 -03:00 committed by Jakob Bornecrantz
parent 3c618ec20c
commit 6de1ab8f8e
7 changed files with 313 additions and 24 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
src/xrt/drivers/CMakeLists.txt
View file

@ -342,7 +342,9 @@ endif()
if(XRT_BUILD_DRIVER_EUROC)
set(EUROC_SOURCE_FILES
euroc/euroc_driver.cpp
euroc/euroc_player.cpp
euroc/euroc_driver.h
euroc/euroc_device.c
euroc/euroc_interface.h
)

241
src/xrt/drivers/euroc/euroc_device.c Normal file
View file

@ -0,0 +1,241 @@
// Copyright 2021, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Fake device tracked with EuRoC datasets and SLAM.
* @author Mateo de Mayo <mateo.demayo@collabora.com>
* @ingroup drv_euroc
*/
#include "util/u_misc.h"
#include "util/u_debug.h"
#include "util/u_device.h"
#include "util/u_distortion_mesh.h"
#include "util/u_var.h"
#include "math/m_space.h"
#include "math/m_mathinclude.h"
#include "xrt/xrt_prober.h"
#include "xrt/xrt_tracking.h"
#include "euroc_driver.h"
#include <stdio.h>
DEBUG_GET_ONCE_BOOL_OPTION(euroc_hmd, "EUROC_HMD", false)
struct xrt_device *
euroc_device_create(struct xrt_prober *xp);
// Euroc Device Prober
/*!
* @implements xrt_auto_prober
*/
struct euroc_prober
{
struct xrt_auto_prober base;
};
static inline struct euroc_prober *
euroc_prober(struct xrt_auto_prober *p)
{
return (struct euroc_prober *)p;
}
static void
euroc_prober_destroy(struct xrt_auto_prober *p)
{
struct euroc_prober *epr = euroc_prober(p);
free(epr);
}
static int
euroc_prober_autoprobe(struct xrt_auto_prober *xap,
cJSON *attached_data,
bool no_hmds,
struct xrt_prober *xp,
struct xrt_device **out_xdevs)
{
struct euroc_prober *epr = euroc_prober(xap);
(void)epr;
struct xrt_device *xd = euroc_device_create(xp);
if (xd == NULL) {
return 0;
}
out_xdevs[0] = xd;
return 1;
}
struct xrt_auto_prober *
euroc_create_auto_prober()
{
// `ep` var name used for euroc_player, let's use `epr` instead
struct euroc_prober *epr = U_TYPED_CALLOC(struct euroc_prober);
epr->base.name = "Euroc Device";
epr->base.destroy = euroc_prober_destroy;
epr->base.lelo_dallas_autoprobe = euroc_prober_autoprobe;
return &epr->base;
}
// Euroc Device
struct euroc_device
{
struct xrt_device base;
struct xrt_tracked_slam *slam;
struct xrt_pose offset;
struct xrt_pose pose;
struct xrt_tracking_origin tracking_origin;
enum u_logging_level ll;
};
static inline struct euroc_device *
euroc_device(struct xrt_device *xdev)
{
return (struct euroc_device *)xdev;
}
static void
euroc_device_update_inputs(struct xrt_device *xdev)
{
return;
}
static void
euroc_device_get_tracked_pose(struct xrt_device *xdev,
enum xrt_input_name name,
uint64_t at_timestamp_ns,
struct xrt_space_relation *out_relation)
{
struct euroc_device *ed = euroc_device(xdev);
if (ed->slam) {
EUROC_ASSERT_(at_timestamp_ns < INT64_MAX);
xrt_tracked_slam_get_tracked_pose(ed->slam, at_timestamp_ns, out_relation);
int pose_bits = XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT;
bool pose_tracked = out_relation->relation_flags & pose_bits;
if (pose_tracked) {
ed->pose = out_relation->pose;
}
}
struct xrt_space_graph space_graph = {0};
m_space_graph_add_pose(&space_graph, &ed->pose);
m_space_graph_add_pose(&space_graph, &ed->offset);
m_space_graph_resolve(&space_graph, out_relation);
out_relation->relation_flags = (enum xrt_space_relation_flags)(
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT |
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
}
static void
euroc_get_view_pose(struct xrt_device *xdev,
const struct xrt_vec3 *eye_relation,
uint32_t view_index,
struct xrt_pose *out_pose)
{
(void)xdev;
u_device_get_view_pose(eye_relation, view_index, out_pose);
}
static void
euroc_device_destroy(struct xrt_device *xdev)
{
struct euroc_device *ed = euroc_device(xdev);
u_var_remove_root(ed);
u_device_free(&ed->base);
}
struct xrt_device *
euroc_device_create(struct xrt_prober *xp)
{
const char *euroc_path = debug_get_option_euroc_path();
if (euroc_path == NULL) {
return NULL;
}
bool is_hmd = debug_get_bool_option_euroc_hmd();
enum u_device_alloc_flags flags = U_DEVICE_ALLOC_NO_FLAGS;
if (is_hmd) {
flags |= U_DEVICE_ALLOC_HMD | U_DEVICE_ALLOC_TRACKING_NONE;
}
struct euroc_device *ed = U_DEVICE_ALLOCATE(struct euroc_device, flags, 1, 0);
EUROC_ASSERT(ed != NULL, "Unable to allocate device");
ed->pose = (struct xrt_pose){{0, 0, 0, 1}, {0, 0, 0}};
ed->offset = (struct xrt_pose){{0, 0, 0, 1}, {0.2, 1.3, -0.5}};
ed->ll = debug_get_log_option_euroc_log();
struct xrt_device *xd = &ed->base;
const char *dev_name;
if (is_hmd) {
xd->name = XRT_DEVICE_GENERIC_HMD;
xd->device_type = XRT_DEVICE_TYPE_HMD;
dev_name = "Euroc HMD";
} else {
xd->name = XRT_DEVICE_SIMPLE_CONTROLLER;
xd->device_type = XRT_DEVICE_TYPE_ANY_HAND_CONTROLLER;
dev_name = "Euroc Controller";
}
snprintf(xd->str, XRT_DEVICE_NAME_LEN, "%s", dev_name);
snprintf(xd->serial, XRT_DEVICE_NAME_LEN, "%s", dev_name);
// Fill in xd->hmd
if (is_hmd) {
struct u_device_simple_info info;
info.display.w_pixels = 1280;
info.display.h_pixels = 720;
info.display.w_meters = 0.13f;
info.display.h_meters = 0.07f;
info.lens_horizontal_separation_meters = 0.13f / 2.0f;
info.lens_vertical_position_meters = 0.07f / 2.0f;
info.views[0].fov = 85.0f * (M_PI / 180.0f);
info.views[1].fov = 85.0f * (M_PI / 180.0f);
bool ret = u_device_setup_split_side_by_side(xd, &info);
EUROC_ASSERT(ret, "Failed to setup HMD properties");
u_distortion_mesh_set_none(xd);
}
xd->tracking_origin = &ed->tracking_origin;
xd->tracking_origin->type = XRT_TRACKING_TYPE_EXTERNAL_SLAM;
xd->tracking_origin->offset.orientation.w = 1.0f;
snprintf(xd->tracking_origin->name, XRT_TRACKING_NAME_LEN, "%s %s", dev_name, "SLAM Tracker");
if (is_hmd) {
xd->inputs[0].name = XRT_INPUT_GENERIC_HEAD_POSE;
} else {
xd->inputs[0].name = XRT_INPUT_SIMPLE_GRIP_POSE;
}
xd->update_inputs = euroc_device_update_inputs;
xd->get_tracked_pose = euroc_device_get_tracked_pose;
xd->destroy = euroc_device_destroy;
if (is_hmd) {
xd->get_view_pose = euroc_get_view_pose;
}
int ret = xp->tracking->create_tracked_slam(xp->tracking, xd, &ed->slam);
if (ret < 0) {
EUROC_WARN(ed,
"Unable to create the SLAM tracker so the Euroc device won't be tracked.\n\t"
"Did you provide the appropriate SLAM dependency when compiling?");
// However, we can continue, maybe the user just wants to play with the euroc utilities
}
u_var_add_root(ed, dev_name, false);
u_var_add_pose(ed, &ed->pose, "pose");
u_var_add_pose(ed, &ed->offset, "offset");
u_var_add_pose(ed, &ed->tracking_origin.offset, "tracking offset");
return xd;
}

49
src/xrt/drivers/euroc/euroc_driver.h Normal file
View file

@ -0,0 +1,49 @@
// Copyright 2021, Collabora, Ltd.
// SPDX-License-Identifier: BSL-1.0
/*!
* @file
* @brief Internal header for the euroc driver utilities.
* @author Mateo de Mayo <mateo.demayo@collabora.com>
* @ingroup drv_euroc
*/
#pragma once
#include "util/u_logging.h"
#include "euroc_interface.h"
#include <assert.h>
#define EUROC_TRACE(e, ...) U_LOG_IFL_T(e->ll, __VA_ARGS__)
#define EUROC_DEBUG(e, ...) U_LOG_IFL_D(e->ll, __VA_ARGS__)
#define EUROC_INFO(e, ...) U_LOG_IFL_I(e->ll, __VA_ARGS__)
#define EUROC_WARN(e, ...) U_LOG_IFL_W(e->ll, __VA_ARGS__)
#define EUROC_ERROR(e, ...) U_LOG_IFL_E(e->ll, __VA_ARGS__)
#define EUROC_ASSERT(predicate, ...) \
do { \
bool p = predicate; \
if (!p) { \
U_LOG(U_LOGGING_ERROR, __VA_ARGS__); \
assert(false && "EUROC_ASSERT failed: " #predicate); \
exit(EXIT_FAILURE); \
} \
} while (false);
#define EUROC_ASSERT_(predicate) EUROC_ASSERT(predicate, "Assertion failed " #predicate)
DEBUG_GET_ONCE_LOG_OPTION(euroc_log, "EUROC_LOG", U_LOGGING_WARN)
DEBUG_GET_ONCE_OPTION(euroc_path, "EUROC_PATH", NULL)
#ifdef __cplusplus
extern "C" {
#endif
/*!
* @addtogroup drv_euroc
* @{
*/
/*!
* @}
*/
#ifdef __cplusplus
}
#endif

8
src/xrt/drivers/euroc/euroc_interface.h
View file

@ -34,6 +34,14 @@ extern "C" {
struct xrt_fs *
euroc_player_create(struct xrt_frame_context *xfctx, const char *path);
/*!
* Create a auto prober for the fake euroc device.
*
* @ingroup drv_euroc
*/
struct xrt_auto_prober *
euroc_create_auto_prober(void);
/*!
* @dir drivers/euroc
*

23
src/xrt/drivers/euroc/euroc_driver.cpp → src/xrt/drivers/euroc/euroc_player.cpp
View file

@ -11,40 +11,19 @@
#include "xrt/xrt_frameserver.h"
#include "os/os_threading.h"
#include "util/u_debug.h"
#include "util/u_logging.h"
#include "util/u_misc.h"
#include "util/u_var.h"
#include "tracking/t_frame_cv_mat_wrapper.hpp"
#include "math/m_filter_fifo.h"
#include "euroc_interface.h"
#include "euroc_driver.h"
#include <assert.h>
#include <stdio.h>
#include <fstream>
#define EUROC_PLAYER_STR "Euroc Player"
#define CLAMP(X, A, B) (MIN(MAX((X), (A)), (B)))
#define EUROC_TRACE(ep, ...) U_LOG_IFL_T(ep->ll, __VA_ARGS__)
#define EUROC_DEBUG(ep, ...) U_LOG_IFL_D(ep->ll, __VA_ARGS__)
#define EUROC_INFO(ep, ...) U_LOG_IFL_I(ep->ll, __VA_ARGS__)
#define EUROC_WARN(ep, ...) U_LOG_IFL_W(ep->ll, __VA_ARGS__)
#define EUROC_ERROR(ep, ...) U_LOG_IFL_E(ep->ll, __VA_ARGS__)
#define EUROC_ASSERT(predicate, ...) \
do { \
bool p = predicate; \
if (!p) { \
U_LOG(U_LOGGING_ERROR, __VA_ARGS__); \
assert(false && "EUROC_ASSERT failed: " #predicate); \
exit(EXIT_FAILURE); \
} \
} while (false);
#define EUROC_ASSERT_(predicate) EUROC_ASSERT(predicate, "Assertion failed " #predicate)
DEBUG_GET_ONCE_LOG_OPTION(euroc_log, "EUROC_LOG", U_LOGGING_WARN)
DEBUG_GET_ONCE_OPTION(euroc_path, "EUROC_PATH", NULL)
typedef std::pair<timepoint_ns, std::string> img_sample;
typedef std::vector<xrt_imu_sample> imu_samples;

4
src/xrt/drivers/meson.build
View file

@ -311,7 +311,9 @@ lib_drv_wmr = static_library(
lib_drv_euroc = static_library(
'drv_euroc',
files(
'euroc/euroc_driver.cpp',
'euroc/euroc_player.cpp',
'euroc/euroc_driver.h',
'euroc/euroc_device.c',
'euroc/euroc_interface.h',
),
include_directories: [xrt_include],

8
src/xrt/targets/common/target_lists.c
View file

@ -79,6 +79,10 @@
#include "wmr/wmr_common.h"
#endif
#ifdef XRT_BUILD_DRIVER_EUROC
#include "euroc/euroc_interface.h"
#endif
/*!
* Each entry should be a vendor ID (VID), product ID (PID), a "found" function,
@ -176,6 +180,10 @@ xrt_auto_prober_creator target_auto_list[] = {
rs_create_auto_prober,
#endif
#ifdef XRT_BUILD_DRIVER_EUROC
euroc_create_auto_prober,
#endif
#ifdef XRT_BUILD_DRIVER_QWERTY
qwerty_create_auto_prober,
#endif