The open source OpenXR runtime
d/euroc: Refactor player to accept a config struct on creation
+189
-101
+10
-4
src/xrt/auxiliary/util/u_debug.c
+10
-4
src/xrt/auxiliary/util/u_debug.c
···
50
50
}
51
51
52
52
bool
53
-
debug_get_bool_option(const char *name, bool _default)
53
+
debug_string_to_bool(const char *raw)
54
54
{
55
-
const char *raw = os_getenv(name);
56
55
bool ret;
57
-
58
56
if (raw == NULL) {
59
-
ret = _default;
57
+
ret = false;
60
58
} else if (!strcmp(raw, "false")) {
61
59
ret = false;
62
60
} else if (!strcmp(raw, "FALSE")) {
···
82
80
} else {
83
81
ret = true;
84
82
}
83
+
return ret;
84
+
}
85
+
86
+
bool
87
+
debug_get_bool_option(const char *name, bool _default)
88
+
{
89
+
const char *raw = os_getenv(name);
90
+
bool ret = raw == NULL ? _default : debug_string_to_bool(raw);
85
91
86
92
if (debug_get_bool_option_print()) {
87
93
U_LOG_RAW("%s=%s (%s)", name, ret ? "TRUE" : "FALSE", raw == NULL ? "nil" : raw);
+3
src/xrt/auxiliary/util/u_debug.h
+3
src/xrt/auxiliary/util/u_debug.h
+1
src/xrt/drivers/euroc/euroc_device.c
+1
src/xrt/drivers/euroc/euroc_device.c
-1
src/xrt/drivers/euroc/euroc_driver.h
-1
src/xrt/drivers/euroc/euroc_driver.h
+60
-1
src/xrt/drivers/euroc/euroc_interface.h
+60
-1
src/xrt/drivers/euroc/euroc_interface.h
···
9
9
10
10
#pragma once
11
11
12
+
#include "util/u_logging.h"
12
13
#include "xrt/xrt_frameserver.h"
13
14
14
15
#ifdef __cplusplus
···
27
28
*/
28
29
29
30
/*!
31
+
* Playback configuration for the euroc player.
32
+
*
33
+
* @ingroup drv_euroc
34
+
*/
35
+
struct euroc_player_playback_config
36
+
{
37
+
bool stereo; //!< Whether to stream both left and right sinks or only left
38
+
bool color; //!< If RGB available but this is false, images will be loaded in grayscale
39
+
bool gt; //!< Whether to send groundtruth data (if available) to the SLAM tracker
40
+
bool skip_perc; //!< Whether @ref skip_first represents percentage or seconds
41
+
float skip_first; //!< How much of the first dataset samples to skip, @see skip_perc
42
+
float scale; //!< Scale of each frame; e.g., 0.5 (half), 1.0 (avoids resize)
43
+
bool max_speed; //!< If true, push samples as fast as possible, other wise @see speed
44
+
double speed; //!< Intended reproduction speed if @ref max_speed is false
45
+
bool send_all_imus_first; //!< If enabled all imu samples will be sent before img samples
46
+
bool paused; //!< Whether to pause the playback
47
+
bool use_source_ts; //!< If true, use the original timestamps from the dataset
48
+
bool play_from_start; //!< If set, the euroc player does not wait for user input to start
49
+
bool print_progress; //!< Whether to print progress to stdout (useful for CLI runs)
50
+
};
51
+
52
+
/*!
53
+
* Describes information about a particular EuRoC dataset residing in @ref path.
54
+
*
55
+
* @ingroup drv_euroc
56
+
*/
57
+
struct euroc_player_dataset_info
58
+
{
59
+
char path[256];
60
+
bool is_stereo;
61
+
bool is_colored;
62
+
bool has_gt; //!< Whether this dataset has groundtruth data available
63
+
const char *gt_device_name;
64
+
uint32_t width;
65
+
uint32_t height;
66
+
};
67
+
68
+
/*!
69
+
* Configuration for the euroc player.
70
+
*
71
+
* @ingroup drv_euroc
72
+
*/
73
+
struct euroc_player_config
74
+
{
75
+
enum u_logging_level log_level;
76
+
struct euroc_player_dataset_info dataset;
77
+
struct euroc_player_playback_config playback;
78
+
};
79
+
80
+
/*!
81
+
* Fills in an @ref euroc_player_config with defaults based on the provided dataset path.
82
+
*
83
+
* @ingroup drv_euroc
84
+
*/
85
+
void
86
+
euroc_player_fill_default_config_for(struct euroc_player_config *config, const char *path);
87
+
88
+
/*!
30
89
* Create an euroc player from a path to a dataset.
31
90
*
32
91
* @ingroup drv_euroc
33
92
*/
34
93
struct xrt_fs *
35
-
euroc_player_create(struct xrt_frame_context *xfctx, const char *path);
94
+
euroc_player_create(struct xrt_frame_context *xfctx, const char *path, struct euroc_player_config *config);
36
95
37
96
/*!
38
97
* Create a auto prober for the fake euroc device.
+114
-94
src/xrt/drivers/euroc/euroc_player.cpp
+114
-94
src/xrt/drivers/euroc/euroc_player.cpp
···
19
19
#include "math/m_filter_fifo.h"
20
20
21
21
#include "euroc_driver.h"
22
+
#include "euroc_interface.h"
22
23
23
24
#include <algorithm>
24
25
#include <chrono>
26
+
#include <cstring>
25
27
#include <stdio.h>
26
28
#include <fstream>
27
29
#include <future>
28
30
#include <thread>
29
31
30
-
DEBUG_GET_ONCE_OPTION(euroc_gt_device_name, "EUROC_GT_DEVICE_NAME", NULL)
31
-
DEBUG_GET_ONCE_OPTION(euroc_skip_first, "EUROC_SKIP_FIRST", "0%")
32
-
DEBUG_GET_ONCE_BOOL_OPTION(euroc_play_from_start, "EUROC_PLAY_FROM_START", false)
33
-
DEBUG_GET_ONCE_BOOL_OPTION(euroc_use_source_ts, "EUROC_USE_SOURCE_TS", false)
34
-
DEBUG_GET_ONCE_BOOL_OPTION(euroc_print_progress, "EUROC_PRINT_PROGRESS", false)
35
-
DEBUG_GET_ONCE_BOOL_OPTION(euroc_max_speed, "EUROC_MAX_SPEED", false)
36
-
DEBUG_GET_ONCE_FLOAT_OPTION(euroc_playback_speed, "EUROC_PLAYBACK_SPEED", 1.0)
32
+
//! @see euroc_player_playback_config
33
+
DEBUG_GET_ONCE_OPTION(gt_device_name, "EUROC_GT_DEVICE_NAME", nullptr)
34
+
DEBUG_GET_ONCE_OPTION(stereo, "EUROC_STEREO", nullptr)
35
+
DEBUG_GET_ONCE_OPTION(color, "EUROC_COLOR", nullptr)
36
+
DEBUG_GET_ONCE_OPTION(gt, "EUROC_GT", nullptr)
37
+
DEBUG_GET_ONCE_OPTION(skip_first, "EUROC_SKIP_FIRST", "0%")
38
+
DEBUG_GET_ONCE_FLOAT_OPTION(scale, "EUROC_SCALE", 1.0f)
39
+
DEBUG_GET_ONCE_BOOL_OPTION(max_speed, "EUROC_MAX_SPEED", false)
40
+
DEBUG_GET_ONCE_FLOAT_OPTION(speed, "EUROC_SPEED", 1.0f)
41
+
DEBUG_GET_ONCE_BOOL_OPTION(paused, "EUROC_PAUSED", false)
42
+
DEBUG_GET_ONCE_BOOL_OPTION(send_all_imus_first, "EUROC_SEND_ALL_IMUS_FIRST", false)
43
+
DEBUG_GET_ONCE_BOOL_OPTION(user_source_ts, "EUROC_USER_SOURCE_TS", false)
44
+
DEBUG_GET_ONCE_BOOL_OPTION(play_from_start, "EUROC_PLAY_FROM_START", false)
45
+
DEBUG_GET_ONCE_BOOL_OPTION(print_progress, "EUROC_PRINT_PROGRESS", false)
37
46
38
47
#define EUROC_PLAYER_STR "Euroc Player"
39
48
#define CLAMP(X, A, B) (MIN(MAX((X), (A)), (B)))
···
82
91
struct xrt_slam_sinks in_sinks; //!< Pointers to intermediate sinks
83
92
struct xrt_slam_sinks out_sinks; //!< Pointers to downstream sinks
84
93
94
+
enum u_logging_level log_level; //!< Log messages with this priority and onwards
95
+
struct euroc_player_dataset_info dataset; //!< Contains information about the source dataset
96
+
struct euroc_player_playback_config playback; //!< Playback information. Prefer to fill it before stream start
97
+
struct xrt_fs_mode mode; //!< The only fs mode the euroc dataset provides
98
+
bool is_running; //!< Set only at start, stop and end of frameserver stream
99
+
timepoint_ns last_pause_ts; //!< Last time the stream was paused
85
100
struct os_thread_helper play_thread;
86
-
enum u_logging_level log_level;
87
-
struct xrt_fs_mode mode; //!< The only fs mode the euroc dataset provides
88
-
bool is_running; //!< Set only at start, stop and end of frameserver stream
89
-
90
-
//! Contains information about the source dataset; set only at start
91
-
struct
92
-
{
93
-
char path[256];
94
-
bool is_stereo;
95
-
bool is_colored;
96
-
bool has_gt; //!< Whether this dataset has groundtruth data available
97
-
uint32_t width;
98
-
uint32_t height;
99
-
} dataset;
100
101
101
102
//! Next frame number to use, index in `left_imgs` and `right_imgs`.
102
103
//! Note that this expects that both cameras provide the same amount of frames.
···
113
114
timepoint_ns start_ts; //!< When did the dataset started to be played
114
115
timepoint_ns offset_ts; //!< Amount of ns to offset start_ns (pauses, skips, etc)
115
116
116
-
//! Playback information.
117
-
//! Prefer to fill it before starting to push frames. Modifying them on
118
-
//! runtime will work with the debug sinks but probably not elsewhere
119
-
struct
120
-
{
121
-
bool stereo; //!< Whether to stream both left and right sinks or only left
122
-
bool color; //!< If RGB available but this is false, images will be loaded in grayscale
123
-
bool gt; //!< Whether to send groundtruth data (if available) to the SLAM tracker
124
-
bool skip_perc; //!< Whether @ref skip_first represents percentage or seconds
125
-
float skip_first; //!< How much of the first dataset samples to skip, @see skip_perc
126
-
float scale; //!< Scale of each frame; e.g., 0.5 (half), 1.0 (avoids resize)
127
-
bool max_speed; //!< If true, push samples as fast as possible, other wise @see speed
128
-
double speed; //!< Intended reproduction speed if @ref max_speed is false
129
-
bool send_all_imus_first; //!< If enabled all imu samples will be sent before img samples
130
-
bool paused; //!< Whether to pause the playback
131
-
bool use_source_ts; //!< If true, use the original timestamps from the dataset
132
-
} playback;
133
-
timepoint_ns last_pause_ts; //!< Last time the stream was paused
134
-
bool play_from_start; //!< If set, the euroc player does not way for user input to start
135
-
bool print_progress; //!< Whether to print progress to stdout (useful for CLI runs)
136
-
137
117
// UI related fields
138
118
enum euroc_player_ui_state ui_state;
139
119
struct u_var_button start_btn;
···
187
167
return true;
188
168
}
189
169
190
-
//! Parse and load ground truth trajectory into `trajectory`.
191
-
//! If read_n > 0, read at most that amount of samples
192
-
//! Returns whether the appropriate data.csv file could be opened
193
-
//! @note Groundtruth data can come from different devices so we use the first of:
194
-
//! 1. vicon0: found in euroc "vicon room" datasets
195
-
//! 2. mocap0: found in TUM-VI datasets with euroc format
196
-
//! 3. state_groundtruth_estimate0: found in euroc as a postprocessed ground truth (we only use first 7 columns)
197
-
//! 4. leica0: found in euroc "machine hall" datasets, only positional ground truth
198
-
//! You can also add your own gt device name with the EUROC_GT_DEVICE_NAME environment variable.
170
+
/*!
171
+
* Parse and load ground truth device name and trajectory into `gtdev` and
172
+
* `trajectory` respectively
173
+
*
174
+
* @param[in] dataset_path
175
+
* @param[in, out] gtdev The name of the groundtruth device found in the dataset if any or `nullptr`.
176
+
* Groundtruth data can come from different devices, so we use the first of:
177
+
* 1. The value prespecified in `gtdev`
178
+
* 2. vicon0: found in euroc "vicon room" datasets
179
+
* 3. mocap0: found in TUM-VI datasets with euroc format
180
+
* 4. state_groundtruth_estimate0: found in euroc as a postprocessed ground truth (we only use first 7 columns)
181
+
* 5. leica0: found in euroc "machine hall" datasets, only positional ground truth
182
+
* @param[out] trajectory The read trajectory
183
+
* @param[in] read_n If > 0, read at most that amount of gt poses
184
+
*
185
+
* @returns Whether the appropriate data.csv file could be opened
186
+
*/
199
187
static bool
200
-
euroc_player_preload_gt_data(const string &dataset_path, gt_trajectory *trajectory, int64_t read_n = -1)
188
+
euroc_player_preload_gt_data(const string &dataset_path,
189
+
const char **gtdev,
190
+
gt_trajectory *trajectory,
191
+
int64_t read_n = -1)
201
192
{
202
-
vector<string> gt_devices = {"vicon0", "mocap0", "state_groundtruth_estimate0", "leica0"};
203
-
const char *user_gtdev = debug_get_option_euroc_gt_device_name();
204
-
if (user_gtdev) {
205
-
gt_devices.insert(gt_devices.begin(), user_gtdev);
193
+
vector<const char *> gt_devices = {"vicon0", "mocap0", "state_groundtruth_estimate0", "leica0"};
194
+
if (*gtdev != nullptr && !string(*gtdev).empty()) {
195
+
gt_devices.insert(gt_devices.begin(), *gtdev);
206
196
}
207
197
208
198
ifstream fin;
209
-
for (const string &device : gt_devices) {
199
+
for (const char *device : gt_devices) {
210
200
string csv_filename = dataset_path + "/mav0/" + device + "/data.csv";
211
201
fin = ifstream{csv_filename};
212
202
if (fin.is_open()) {
203
+
*gtdev = device;
213
204
break;
214
205
}
215
206
}
···
319
310
320
311
if (ep->dataset.has_gt) {
321
312
ep->gt->clear();
322
-
euroc_player_preload_gt_data(ep->dataset.path, ep->gt);
313
+
euroc_player_preload_gt_data(ep->dataset.path, &ep->dataset.gt_device_name, ep->gt);
323
314
}
324
315
}
325
316
···
355
346
//! Determine and fill attributes of the dataset pointed by `path`
356
347
//! Assertion fails if `path` does not point to an euroc dataset
357
348
static void
358
-
euroc_player_fill_dataset_info(struct euroc_player *ep, const char *path)
349
+
euroc_player_fill_dataset_info(const char *path, euroc_player_dataset_info *dataset)
359
350
{
360
-
const char *euroc_path = debug_get_option_euroc_path();
361
-
EUROC_ASSERT(strcmp(euroc_path, path) == 0, "Unexpected path=%s differs from EUROC_PATH=%s", path, euroc_path);
362
-
363
-
snprintf(ep->dataset.path, sizeof(ep->dataset.path), "%s", path);
351
+
snprintf(dataset->path, sizeof(dataset->path), "%s", path);
364
352
img_samples samples;
365
353
imu_samples _1;
366
354
gt_trajectory _2;
367
-
bool has_right_camera = euroc_player_preload_img_data(ep->dataset.path, &samples, false, 0);
368
-
bool has_left_camera = euroc_player_preload_img_data(ep->dataset.path, &samples, true, 1);
369
-
bool has_imu = euroc_player_preload_imu_data(ep->dataset.path, &_1, 0);
370
-
bool has_gt = euroc_player_preload_gt_data(ep->dataset.path, &_2, 0);
355
+
bool has_right_camera = euroc_player_preload_img_data(dataset->path, &samples, false, 0);
356
+
bool has_left_camera = euroc_player_preload_img_data(dataset->path, &samples, true, 1);
357
+
bool has_imu = euroc_player_preload_imu_data(dataset->path, &_1, 0);
358
+
bool has_gt = euroc_player_preload_gt_data(dataset->path, &dataset->gt_device_name, &_2, 0);
371
359
bool is_valid_dataset = has_left_camera && has_imu;
372
360
EUROC_ASSERT(is_valid_dataset, "Invalid dataset %s", path);
373
361
374
362
cv::Mat first_left_img = cv::imread(samples[0].second, cv::IMREAD_ANYCOLOR);
375
-
ep->dataset.is_stereo = has_right_camera;
376
-
ep->dataset.is_colored = first_left_img.channels() == 3;
377
-
ep->dataset.has_gt = has_gt;
378
-
ep->dataset.width = first_left_img.cols;
379
-
ep->dataset.height = first_left_img.rows;
380
-
EUROC_INFO(ep, "dataset information\n\tpath: %s\n\tis_stereo: %d, is_colored: %d, width: %d, height: %d",
381
-
ep->dataset.path, ep->dataset.is_stereo, ep->dataset.is_colored, ep->dataset.width,
382
-
ep->dataset.height);
363
+
dataset->is_stereo = has_right_camera;
364
+
dataset->is_colored = first_left_img.channels() == 3;
365
+
dataset->has_gt = has_gt;
366
+
dataset->width = first_left_img.cols;
367
+
dataset->height = first_left_img.rows;
383
368
}
384
369
385
370
···
488
473
snprintf(ep->progress_text, sizeof(ep->progress_text), "Frames %lu/%lu - IMUs %lu/%lu", ep->img_seq,
489
474
ep->left_imgs->size(), ep->imu_seq, ep->imus->size());
490
475
491
-
if (ep->print_progress) {
476
+
if (ep->playback.print_progress) {
492
477
printf("Playback %.2f%% - Frame %lu/%lu\r", float(ep->img_seq) / float(ep->left_imgs->size()) * 100,
493
478
ep->img_seq, ep->left_imgs->size());
494
479
fflush(stdout);
···
731
716
if (ep->out_sinks.left == NULL) {
732
717
EUROC_WARN(ep, "No left sink provided, will keep running but tracking is unlikely to work");
733
718
}
734
-
if (ep->play_from_start) {
719
+
if (ep->playback.play_from_start) {
735
720
euroc_player_start_btn_cb(ep);
736
721
}
737
722
} else if (xs != NULL && capture_type == XRT_FS_CAPTURE_TYPE_CALIBRATION) {
···
910
895
u_var_add_sink_debug(ep, &ep->ui_right_sink, "Right Camera");
911
896
}
912
897
898
+
extern "C" void
899
+
euroc_player_fill_default_config_for(struct euroc_player_config *config, const char *dataset_path)
900
+
{
901
+
struct euroc_player_dataset_info dataset = {};
902
+
dataset.gt_device_name = debug_get_option_gt_device_name();
903
+
euroc_player_fill_dataset_info(dataset_path, &dataset);
904
+
905
+
struct euroc_player_playback_config playback = {};
906
+
const char *stereo = debug_get_option_stereo();
907
+
const char *color = debug_get_option_color();
908
+
const char *gt = debug_get_option_gt();
909
+
const char *skip_option = debug_get_option_skip_first();
910
+
playback.stereo = stereo == nullptr ? dataset.is_stereo : debug_string_to_bool(stereo);
911
+
playback.color = color == nullptr ? dataset.is_colored : debug_string_to_bool(color);
912
+
playback.gt = gt == nullptr ? dataset.has_gt : debug_string_to_bool(gt);
913
+
playback.skip_perc = string(skip_option).back() == '%';
914
+
playback.skip_first = stof(skip_option);
915
+
playback.scale = debug_get_float_option_scale();
916
+
playback.max_speed = debug_get_bool_option_max_speed();
917
+
playback.speed = debug_get_float_option_speed();
918
+
playback.paused = debug_get_bool_option_paused();
919
+
playback.send_all_imus_first = debug_get_bool_option_send_all_imus_first();
920
+
playback.use_source_ts = debug_get_bool_option_user_source_ts();
921
+
playback.play_from_start = debug_get_bool_option_play_from_start();
922
+
playback.print_progress = debug_get_bool_option_print_progress();
923
+
924
+
config->log_level = debug_get_log_option_euroc_log();
925
+
config->dataset = dataset;
926
+
config->playback = playback;
927
+
}
928
+
913
929
// Euroc driver creation
914
930
915
-
struct xrt_fs *
916
-
euroc_player_create(struct xrt_frame_context *xfctx, const char *path)
931
+
extern "C" struct xrt_fs *
932
+
euroc_player_create(struct xrt_frame_context *xfctx, const char *path, struct euroc_player_config *config)
917
933
{
918
934
struct euroc_player *ep = U_TYPED_CALLOC(struct euroc_player);
919
-
euroc_player_fill_dataset_info(ep, path);
935
+
936
+
struct euroc_player_config *default_config = nullptr;
937
+
if (config == nullptr) {
938
+
default_config = U_TYPED_CALLOC(struct euroc_player_config);
939
+
euroc_player_fill_default_config_for(default_config, path);
940
+
config = default_config;
941
+
}
942
+
943
+
ep->log_level = config->log_level;
944
+
ep->dataset = config->dataset;
945
+
ep->playback = config->playback;
946
+
947
+
if (default_config != nullptr) {
948
+
free(default_config);
949
+
}
950
+
920
951
ep->mode = xrt_fs_mode{
921
952
ep->dataset.width,
922
953
ep->dataset.height,
···
925
956
// xrt_fs interface as it will be managed through two different sinks.
926
957
XRT_STEREO_FORMAT_NONE,
927
958
};
959
+
EUROC_INFO(ep, "dataset information\n\tpath: %s\n\tis_stereo: %d, is_colored: %d, width: %d, height: %d",
960
+
ep->dataset.path, ep->dataset.is_stereo, ep->dataset.is_colored, ep->dataset.width,
961
+
ep->dataset.height);
928
962
929
963
// Using pointers to not mix vector with a C-compatible struct
930
964
ep->gt = new gt_trajectory{};
···
932
966
ep->left_imgs = new img_samples{};
933
967
ep->right_imgs = new img_samples{};
934
968
935
-
ep->playback.stereo = ep->dataset.is_stereo;
936
-
ep->playback.color = ep->dataset.is_colored;
937
-
ep->playback.gt = ep->dataset.has_gt;
938
-
ep->playback.skip_perc = string(debug_get_option_euroc_skip_first()).back() == '%';
939
-
ep->playback.skip_first = stof(debug_get_option_euroc_skip_first());
940
-
ep->playback.scale = 1.0;
941
-
ep->playback.max_speed = debug_get_bool_option_euroc_max_speed();
942
-
ep->playback.speed = debug_get_float_option_euroc_playback_speed();
943
-
ep->playback.send_all_imus_first = false;
944
-
ep->playback.use_source_ts = debug_get_bool_option_euroc_use_source_ts();
945
-
ep->play_from_start = debug_get_bool_option_euroc_play_from_start();
946
-
947
-
ep->log_level = debug_get_log_option_euroc_log();
948
-
ep->print_progress = debug_get_bool_option_euroc_print_progress();
949
969
euroc_player_setup_gui(ep);
950
970
951
971
ep->left_sink.push_frame = receive_left_frame;
+1
-1
src/xrt/state_trackers/prober/p_prober.c
+1
-1
src/xrt/state_trackers/prober/p_prober.c
···
1001
1001
#if defined(XRT_BUILD_DRIVER_EUROC)
1002
1002
const char *euroc_path = debug_get_option_euroc_path();
1003
1003
if (euroc_path != NULL) {
1004
-
*out_xfs = euroc_player_create(xfctx, euroc_path); // Euroc will exit if it can't be created
1004
+
*out_xfs = euroc_player_create(xfctx, euroc_path, NULL); // Euroc will exit if it can't be created
1005
1005
return 0;
1006
1006
}
1007
1007
#endif