matrixfurry.com
The open source OpenXR runtime
d/euroc: Add euroc runner
+150
-3
+1
-1
src/xrt/drivers/CMakeLists.txt
+1
-1
src/xrt/drivers/CMakeLists.txt
···
348
348
if(XRT_BUILD_DRIVER_EUROC)
349
349
add_library(
350
350
drv_euroc STATIC euroc/euroc_player.cpp euroc/euroc_driver.h euroc/euroc_device.c
351
-
euroc/euroc_interface.h
351
+
euroc/euroc_interface.h euroc/euroc_runner.c
352
352
)
353
353
target_link_libraries(
354
354
drv_euroc PRIVATE xrt-interfaces aux_util aux_tracking ${OpenCV_LIBRARIES}
+1
src/xrt/drivers/euroc/euroc_device.c
+1
src/xrt/drivers/euroc/euroc_device.c
-2
src/xrt/drivers/euroc/euroc_driver.h
-2
src/xrt/drivers/euroc/euroc_driver.h
+16
src/xrt/drivers/euroc/euroc_interface.h
+16
src/xrt/drivers/euroc/euroc_interface.h
···
102
102
euroc_create_auto_prober(void);
103
103
104
104
/*!
105
+
* Tracks an euroc dataset with the SLAM tracker.
106
+
*
107
+
* @param should_exit External exit condition, the run will end if it becomes true
108
+
* @param euroc_path Dataset path
109
+
* @param slam_config Path to config file for the SLAM system
110
+
* @param output_path Path to write resulting tracking data to
111
+
*
112
+
* @ingroup drv_euroc
113
+
*/
114
+
void
115
+
euroc_run_dataset(const char *euroc_path,
116
+
const char *slam_config,
117
+
const char *output_path,
118
+
const volatile bool *should_exit);
119
+
120
+
/*!
105
121
* @dir drivers/euroc
106
122
*
107
123
* @brief @ref drv_euroc files.
+1
src/xrt/drivers/euroc/euroc_player.cpp
+1
src/xrt/drivers/euroc/euroc_player.cpp
···
30
30
#include <thread>
31
31
32
32
//! @see euroc_player_playback_config
33
+
DEBUG_GET_ONCE_LOG_OPTION(euroc_log, "EUROC_LOG", U_LOGGING_WARN)
33
34
DEBUG_GET_ONCE_OPTION(gt_device_name, "EUROC_GT_DEVICE_NAME", nullptr)
34
35
DEBUG_GET_ONCE_OPTION(stereo, "EUROC_STEREO", nullptr)
35
36
DEBUG_GET_ONCE_OPTION(color, "EUROC_COLOR", nullptr)
+130
src/xrt/drivers/euroc/euroc_runner.c
+130
src/xrt/drivers/euroc/euroc_runner.c
···
1
+
// Copyright 2022, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief Play EuRoC datasets and track them with the SLAM tracker.
6
+
* @author Mateo de Mayo <mateo.demayo@collabora.com>
7
+
* @ingroup drv_euroc
8
+
*/
9
+
10
+
#include "euroc_driver.h"
11
+
#include "os/os_threading.h"
12
+
#include "os/os_time.h"
13
+
#include "tracking/t_tracking.h"
14
+
#include "util/u_logging.h"
15
+
#include "util/u_misc.h"
16
+
#include "xrt/xrt_config_have.h"
17
+
#include "xrt/xrt_defines.h"
18
+
#include "xrt/xrt_frame.h"
19
+
#include "xrt/xrt_frameserver.h"
20
+
#include "xrt/xrt_tracking.h"
21
+
22
+
#if !defined(XRT_HAVE_SLAM)
23
+
24
+
void
25
+
euroc_run_dataset(const char *euroc_path,
26
+
const char *slam_config,
27
+
const char *output_path,
28
+
const volatile bool *should_exit)
29
+
{}
30
+
31
+
#else
32
+
33
+
static struct euroc_player_config *
34
+
make_euroc_player_config(const char *euroc_path)
35
+
{
36
+
struct euroc_player_config *ep_config = U_TYPED_CALLOC(struct euroc_player_config);
37
+
euroc_player_fill_default_config_for(ep_config, euroc_path);
38
+
39
+
// Override config to be friendlier for CLI runs unless they were explicitly provided
40
+
if (getenv("EUROC_LOG") == NULL) {
41
+
ep_config->log_level = U_LOGGING_INFO;
42
+
}
43
+
if (getenv("EUROC_PLAY_FROM_START") == NULL) {
44
+
ep_config->playback.play_from_start = true;
45
+
}
46
+
if (getenv("EUROC_PRINT_PROGRESS") == NULL) {
47
+
ep_config->playback.print_progress = true;
48
+
}
49
+
if (getenv("EUROC_USE_SOURCE_TS") == NULL) {
50
+
ep_config->playback.use_source_ts = true;
51
+
}
52
+
if (getenv("EUROC_MAX_SPEED") == NULL) {
53
+
ep_config->playback.max_speed = true;
54
+
}
55
+
56
+
return ep_config;
57
+
}
58
+
59
+
static struct t_slam_tracker_config *
60
+
make_slam_tracker_config(const char *slam_config, const char *output_path)
61
+
{
62
+
struct t_slam_tracker_config *st_config = U_TYPED_CALLOC(struct t_slam_tracker_config);
63
+
t_slam_fill_default_config(st_config);
64
+
65
+
// Override config to be friendlier for CLI runs unless they were explicitly provided
66
+
if (getenv("SLAM_LOG") == NULL) {
67
+
st_config->log_level = U_LOGGING_INFO;
68
+
}
69
+
if (getenv("SLAM_SUBMIT_FROM_START") == NULL) {
70
+
st_config->submit_from_start = true;
71
+
}
72
+
if (getenv("SLAM_PREDICTION_TYPE") == NULL) {
73
+
st_config->prediction = SLAM_PRED_NONE;
74
+
}
75
+
if (getenv("SLAM_WRITE_CSVS") == NULL) {
76
+
st_config->write_csvs = true;
77
+
}
78
+
79
+
st_config->slam_config = slam_config;
80
+
st_config->csv_path = output_path;
81
+
82
+
return st_config;
83
+
}
84
+
85
+
void
86
+
euroc_run_dataset(const char *euroc_path,
87
+
const char *slam_config,
88
+
const char *output_path,
89
+
const volatile bool *should_exit)
90
+
{
91
+
struct euroc_player_config *ep_config = make_euroc_player_config(euroc_path);
92
+
struct t_slam_tracker_config *st_config = make_slam_tracker_config(slam_config, output_path);
93
+
94
+
// Frame context that will manage SLAM tracker and euroc player lifetimes
95
+
struct xrt_frame_context xfctx = {0};
96
+
97
+
// Start SLAM tracker
98
+
struct xrt_tracked_slam *xts = NULL;
99
+
struct xrt_slam_sinks *sinks = NULL;
100
+
int ret = t_slam_create(&xfctx, st_config, &xts, &sinks);
101
+
EUROC_ASSERT(ret == 0, "Failed to create slam tracker");
102
+
t_slam_start(xts);
103
+
104
+
// Stream euroc player into the tracker
105
+
struct xrt_fs *xfs = euroc_player_create(&xfctx, euroc_path, ep_config);
106
+
xrt_fs_slam_stream_start(xfs, sinks);
107
+
108
+
// Let's loop until both the player and the tracker finish
109
+
110
+
// Last two tracked poses, if they are the same we assume tracking stopped
111
+
struct xrt_space_relation a = {0};
112
+
struct xrt_space_relation b = {0};
113
+
b.pose.orientation.w = 42; // Make b different from a
114
+
115
+
bool tracking = true;
116
+
bool streaming = xrt_fs_is_running(xfs);
117
+
while ((streaming || tracking) && !*should_exit) {
118
+
os_nanosleep(0.2 * U_TIME_1S_IN_NS);
119
+
a = b;
120
+
xrt_tracked_slam_get_tracked_pose(xts, os_monotonic_get_ns(), &b);
121
+
tracking = memcmp(&a, &b, sizeof(struct xrt_space_relation)) != 0;
122
+
streaming = xrt_fs_is_running(xfs);
123
+
}
124
+
125
+
xrt_frame_context_destroy_nodes(&xfctx);
126
+
free(st_config);
127
+
free(ep_config);
128
+
}
129
+
130
+
#endif