matrixfurry.com
The open source OpenXR runtime
1// Copyright 2020-2021, Collabora, Ltd.
2// SPDX-License-Identifier: BSL-1.0
3/*!
4 * @file
5 * @brief EuRoC playback functionality
6 * @author Mateo de Mayo <mateo.demayo@collabora.com>
7 * @ingroup drv_euroc
8 */
9
10#include "xrt/xrt_frame.h"
11#include "xrt/xrt_tracking.h"
12#include "xrt/xrt_frameserver.h"
13#include "os/os_threading.h"
14#include "util/u_debug.h"
15#include "util/u_misc.h"
16#include "util/u_time.h"
17#include "util/u_var.h"
18#include "util/u_sink.h"
19#include "tracking/t_frame_cv_mat_wrapper.hpp"
20#include "math/m_api.h"
21#include "math/m_filter_fifo.h"
22
23#include "euroc_driver.h"
24#include "euroc_interface.h"
25
26#include <algorithm>
27#include <chrono>
28#include <cstring>
29#include <stdint.h>
30#include <stdio.h>
31#include <fstream>
32#include <future>
33#include <thread>
34
35//! @see euroc_player_playback_config
36DEBUG_GET_ONCE_LOG_OPTION(euroc_log, "EUROC_LOG", U_LOGGING_WARN)
37DEBUG_GET_ONCE_OPTION(gt_device_name, "EUROC_GT_DEVICE_NAME", nullptr)
38DEBUG_GET_ONCE_OPTION(cam_count, "EUROC_CAM_COUNT", nullptr)
39DEBUG_GET_ONCE_OPTION(color, "EUROC_COLOR", nullptr)
40DEBUG_GET_ONCE_OPTION(gt, "EUROC_GT", nullptr)
41DEBUG_GET_ONCE_OPTION(skip_first, "EUROC_SKIP_FIRST", "0%")
42DEBUG_GET_ONCE_FLOAT_OPTION(scale, "EUROC_SCALE", 1.0f)
43DEBUG_GET_ONCE_BOOL_OPTION(max_speed, "EUROC_MAX_SPEED", false)
44DEBUG_GET_ONCE_FLOAT_OPTION(speed, "EUROC_SPEED", 1.0f)
45DEBUG_GET_ONCE_BOOL_OPTION(paused, "EUROC_PAUSED", false)
46DEBUG_GET_ONCE_BOOL_OPTION(send_all_imus_first, "EUROC_SEND_ALL_IMUS_FIRST", false)
47DEBUG_GET_ONCE_BOOL_OPTION(use_source_ts, "EUROC_USE_SOURCE_TS", false)
48DEBUG_GET_ONCE_BOOL_OPTION(play_from_start, "EUROC_PLAY_FROM_START", false)
49DEBUG_GET_ONCE_BOOL_OPTION(print_progress, "EUROC_PRINT_PROGRESS", false)
50
51#define EUROC_PLAYER_STR "Euroc Player"
52
53//! Match max cameras to slam sinks max camera count
54#define EUROC_MAX_CAMS XRT_TRACKING_MAX_SLAM_CAMS
55
56using std::async;
57using std::find_if;
58using std::ifstream;
59using std::is_same_v;
60using std::launch;
61using std::max_element;
62using std::pair;
63using std::stof;
64using std::string;
65using std::to_string;
66using std::vector;
67
68using img_sample = pair<timepoint_ns, string>;
69
70using imu_samples = vector<xrt_imu_sample>;
71using img_samples = vector<img_sample>;
72using gt_trajectory = vector<xrt_pose_sample>;
73
74enum euroc_player_ui_state
75{
76 UNINITIALIZED = 0,
77 NOT_STREAMING,
78 STREAM_PLAYING,
79 STREAM_PAUSED,
80 STREAM_ENDED
81};
82
83/*!
84 * Euroc player is in charge of the playback of a particular dataset.
85 *
86 * @implements xrt_fs
87 * @implements xrt_frame_node
88 */
89struct euroc_player
90{
91 struct xrt_fs base;
92 struct xrt_frame_node node;
93
94 // Sinks
95 struct xrt_frame_sink cam_sinks[EUROC_MAX_CAMS]; //!< Intermediate sink for each camera frames
96 struct xrt_imu_sink imu_sink; //!< Intermediate sink for IMU samples
97 struct xrt_slam_sinks in_sinks; //!< Pointers to intermediate sinks
98 struct xrt_slam_sinks out_sinks; //!< Pointers to downstream sinks
99
100 enum u_logging_level log_level; //!< Log messages with this priority and onwards
101 struct euroc_player_dataset_info dataset; //!< Contains information about the source dataset
102 struct euroc_player_playback_config playback; //!< Playback information. Prefer to fill it before stream start
103 struct xrt_fs_mode mode; //!< The only fs mode the euroc dataset provides
104 bool is_running; //!< Set only at start, stop and end of frameserver stream
105 timepoint_ns last_pause_ts; //!< Last time the stream was paused
106 struct os_thread_helper play_thread;
107
108 //! Next frame number to use, index in `imgs[i]`.
109 //! Note that this expects that both cameras provide the same amount of frames.
110 //! Furthermore, it is also expected that their timestamps match.
111 uint64_t img_seq; //!< Next frame number to use, index in `imgs[i]`
112 uint64_t imu_seq; //!< Next imu sample number to use, index in `imus`
113 imu_samples *imus; //!< List of all IMU samples read from the dataset
114 vector<img_samples> *imgs; //!< List of all image names to read from the dataset per camera
115 gt_trajectory *gt; //!< List of all groundtruth poses read from the dataset
116
117 // Timestamp correction fields (can be disabled through `use_source_ts`)
118 timepoint_ns base_ts; //!< First sample timestamp, stream timestamps are relative to this
119 timepoint_ns start_ts; //!< When did the dataset started to be played
120 timepoint_ns offset_ts; //!< Amount of ns to offset start_ns (pauses, skips, etc)
121
122 // UI related fields
123 enum euroc_player_ui_state ui_state;
124 struct u_var_button start_btn;
125 struct u_var_button pause_btn;
126 char progress_text[128];
127 struct u_sink_debug ui_cam_sinks[EUROC_MAX_CAMS]; //!< Sinks to display cam frames in UI
128 struct m_ff_vec3_f32 *gyro_ff; //!< Used for displaying IMU data
129 struct m_ff_vec3_f32 *accel_ff; //!< Same as `gyro_ff`
130};
131
132static void
133euroc_player_start_btn_cb(void *ptr);
134static void
135euroc_player_set_ui_state(struct euroc_player *ep, euroc_player_ui_state state);
136
137
138// Euroc functionality
139
140//! Parse and load all IMU samples into `samples`, assumes data.csv is well formed
141//! If `read_n` > 0, read at most that amount of samples
142//! Returns whether the appropriate data.csv file could be opened
143static bool
144euroc_player_preload_imu_data(const string &dataset_path, imu_samples *samples, int64_t read_n = -1)
145{
146 string csv_filename = dataset_path + "/mav0/imu0/data.csv";
147 ifstream fin{csv_filename};
148 if (!fin.is_open()) {
149 return false;
150 }
151
152 constexpr int COLUMN_COUNT = 6; // EuRoC imu columns: ts wx wy wz ax ay az
153 string line;
154 getline(fin, line); // Skip header line
155
156 while (getline(fin, line) && read_n-- != 0) {
157 timepoint_ns timestamp;
158 double v[COLUMN_COUNT];
159 size_t i = 0;
160 size_t j = line.find(',');
161 timestamp = stoll(line.substr(i, j));
162 for (size_t k = 0; k < COLUMN_COUNT; k++) {
163 i = j;
164 j = line.find(',', i + 1);
165 v[k] = stod(line.substr(i + 1, j));
166 }
167
168 xrt_imu_sample sample{timestamp, {v[3], v[4], v[5]}, {v[0], v[1], v[2]}};
169 samples->push_back(sample);
170 }
171 return true;
172}
173
174/*!
175 * Parse and load ground truth device name and trajectory into `gtdev` and
176 * `trajectory` respectively
177 *
178 * @param[in] dataset_path
179 * @param[in, out] gtdev The name of the groundtruth device found in the dataset if any or `nullptr`.
180 * Groundtruth data can come from different devices, so we use the first of:
181 * 1. The value prespecified in `gtdev`
182 * 2. vicon0: found in euroc "vicon room" datasets
183 * 3. mocap0: found in TUM-VI datasets with euroc format
184 * 4. state_groundtruth_estimate0: found in euroc as a postprocessed ground truth (we only use first 7 columns)
185 * 5. leica0: found in euroc "machine hall" datasets, only positional ground truth
186 * @param[out] trajectory The read trajectory
187 * @param[in] read_n If > 0, read at most that amount of gt poses
188 *
189 * @returns Whether the appropriate data.csv file could be opened
190 */
191static bool
192euroc_player_preload_gt_data(const string &dataset_path,
193 const char **gtdev,
194 gt_trajectory *trajectory,
195 int64_t read_n = -1)
196{
197 vector<const char *> gt_devices = {"vicon0", "mocap0", "state_groundtruth_estimate0", "leica0"};
198 if (*gtdev != nullptr && !string(*gtdev).empty()) {
199 gt_devices.insert(gt_devices.begin(), *gtdev);
200 }
201
202 ifstream fin;
203 for (const char *device : gt_devices) {
204 string csv_filename = dataset_path + "/mav0/" + device + "/data.csv";
205 fin = ifstream{csv_filename};
206 if (fin.is_open()) {
207 *gtdev = device;
208 break;
209 }
210 }
211
212 if (!fin.is_open()) {
213 return false;
214 }
215
216 constexpr int COLUMN_COUNT = 7; // EuRoC groundtruth columns: ts px py pz qw qx qy qz
217 string line;
218 getline(fin, line); // Skip header line
219
220 while (getline(fin, line) && read_n-- != 0) {
221 timepoint_ns timestamp;
222 float v[COLUMN_COUNT] = {0, 0, 0, 1, 0, 0, 0}; // Set identity orientation for leica0
223 size_t i = 0;
224 size_t j = line.find(',');
225 timestamp = stoll(line.substr(i, j));
226 for (size_t k = 0; k < COLUMN_COUNT && j != string::npos; k++) {
227 i = j;
228 j = line.find(',', i + 1);
229 v[k] = stof(line.substr(i + 1, j));
230 }
231
232 xrt_pose pose = {{v[4], v[5], v[6], v[3]}, {v[0], v[1], v[2]}};
233 trajectory->push_back({timestamp, pose});
234 }
235 return true;
236}
237
238//! Parse and load image names and timestamps into `samples`
239//! If read_n > 0, read at most that amount of samples
240//! Returns whether the appropriate data.csv file could be opened
241static bool
242euroc_player_preload_img_data(const string &dataset_path, img_samples &samples, size_t cam_id, int64_t read_n = -1)
243{
244 // Parse image data, assumes data.csv is well formed
245 string cam_name = "cam" + to_string(cam_id);
246 string imgs_path = dataset_path + "/mav0/" + cam_name + "/data";
247 string csv_filename = dataset_path + "/mav0/" + cam_name + "/data.csv";
248 ifstream fin{csv_filename};
249 if (!fin.is_open()) {
250 return false;
251 }
252
253 string line;
254 getline(fin, line); // Skip header line
255 while (getline(fin, line) && read_n-- != 0) {
256 size_t i = line.find(',');
257 timepoint_ns timestamp = stoll(line.substr(0, i));
258 string img_name_tail = line.substr(i + 1);
259
260 // Standard euroc datasets use CRLF line endings, so let's remove the extra '\r'
261 if (img_name_tail.back() == '\r') {
262 img_name_tail.pop_back();
263 }
264
265 string img_name = imgs_path + "/" + img_name_tail;
266 img_sample sample{timestamp, img_name};
267 samples.push_back(sample);
268 }
269 return true;
270}
271
272//! Trims cameras sequences so that they all start and end at the same sample
273//! Note that this function does not guarantee that the dataset is free of framedrops
274//! and it assumes it is properly formatted with monotonically increasing timestamps.
275static void
276euroc_player_match_cams_seqs(struct euroc_player *ep)
277{
278 // Find newest first timestamp and oldest last timestamp
279 timepoint_ns first_ts = INT64_MIN;
280 timepoint_ns last_ts = INT64_MAX;
281 for (const img_samples &imgs : *ep->imgs) {
282 EUROC_ASSERT(!imgs.empty(), "Camera with no samples");
283
284 timepoint_ns cam_first_ts = imgs.front().first;
285 if (cam_first_ts > first_ts) {
286 first_ts = cam_first_ts;
287 }
288
289 timepoint_ns cam_last_ts = imgs.back().first;
290 if (cam_last_ts < last_ts) {
291 last_ts = cam_last_ts;
292 }
293 }
294
295 auto is_first = [first_ts](const img_sample &s) { return s.first == first_ts; };
296 auto is_last = [last_ts](const img_sample &s) { return s.first == last_ts; };
297
298 for (img_samples &imgs : *ep->imgs) {
299 img_samples::iterator new_first = find_if(imgs.begin(), imgs.end(), is_first);
300 img_samples::iterator new_last = find_if(imgs.begin(), imgs.end(), is_last);
301 EUROC_ASSERT_(new_first != imgs.end() && new_last != imgs.end());
302 imgs.assign(new_first, new_last + 1);
303 }
304}
305
306static void
307euroc_player_preload(struct euroc_player *ep)
308{
309 ep->imus->clear();
310 euroc_player_preload_imu_data(ep->dataset.path, ep->imus);
311
312 for (size_t i = 0; i < ep->imgs->size(); i++) {
313 ep->imgs->at(i).clear();
314 euroc_player_preload_img_data(ep->dataset.path, ep->imgs->at(i), i);
315 }
316
317 euroc_player_match_cams_seqs(ep);
318
319 if (ep->dataset.has_gt) {
320 ep->gt->clear();
321 euroc_player_preload_gt_data(ep->dataset.path, &ep->dataset.gt_device_name, ep->gt);
322 }
323}
324
325//! Skips the first seconds of the dataset as specified by the user
326static void
327euroc_player_user_skip(struct euroc_player *ep)
328{
329
330 float skip_first_s = 0;
331 if (ep->playback.skip_perc) {
332 float skip_percentage = ep->playback.skip_first;
333 timepoint_ns last_ts = MAX(ep->imgs->at(0).back().first, ep->imus->back().timestamp_ns);
334 double dataset_length_s = (last_ts - ep->base_ts) / U_TIME_1S_IN_NS;
335 skip_first_s = dataset_length_s * skip_percentage / 100.0f;
336 } else {
337 skip_first_s = ep->playback.skip_first;
338 }
339
340 time_duration_ns skip_first_ns = skip_first_s * U_TIME_1S_IN_NS;
341 timepoint_ns skipped_ts = ep->base_ts + skip_first_ns;
342
343 while (ep->imu_seq < ep->imus->size() && ep->imus->at(ep->imu_seq).timestamp_ns < skipped_ts) {
344 ep->imu_seq++;
345 }
346
347 while (ep->img_seq < ep->imgs->at(0).size() && ep->imgs->at(0).at(ep->img_seq).first < skipped_ts) {
348 ep->img_seq++;
349 }
350
351 ep->offset_ts -= skip_first_ns / ep->playback.speed;
352}
353
354//! Determine and fill attributes of the dataset pointed by `path`
355//! Assertion fails if `path` does not point to an euroc dataset
356static void
357euroc_player_fill_dataset_info(const char *path, euroc_player_dataset_info *dataset)
358{
359 (void)snprintf(dataset->path, sizeof(dataset->path), "%s", path);
360 img_samples samples;
361 imu_samples _1;
362 gt_trajectory _2;
363
364 size_t i = 0;
365 bool has_camera = euroc_player_preload_img_data(dataset->path, samples, i, 1);
366 while ((has_camera = euroc_player_preload_img_data(dataset->path, samples, ++i, 0))) {
367 }
368 size_t cam_count = i;
369 EUROC_ASSERT(cam_count <= EUROC_MAX_CAMS, "Increase EUROC_MAX_CAMS (dataset with %zu cams)", cam_count);
370
371 bool has_imu = euroc_player_preload_imu_data(dataset->path, &_1, 0);
372 bool has_gt = euroc_player_preload_gt_data(dataset->path, &dataset->gt_device_name, &_2, 0);
373 bool is_valid_dataset = cam_count > 0 && has_imu;
374 EUROC_ASSERT(is_valid_dataset, "Invalid dataset %s", path);
375
376 cv::Mat first_cam0_img = cv::imread(samples[0].second, cv::IMREAD_ANYCOLOR);
377 dataset->cam_count = (int)cam_count;
378 dataset->is_colored = first_cam0_img.channels() == 3;
379 dataset->has_gt = has_gt;
380 dataset->width = first_cam0_img.cols;
381 dataset->height = first_cam0_img.rows;
382}
383
384
385// Playback functionality
386
387static struct euroc_player *
388euroc_player(struct xrt_fs *xfs)
389{
390 return (struct euroc_player *)xfs;
391}
392
393//! Wrapper around os_monotonic_get_ns to convert to int64_t and check ranges
394static timepoint_ns
395os_monotonic_get_ts()
396{
397 uint64_t uts = os_monotonic_get_ns();
398 EUROC_ASSERT(uts < INT64_MAX, "Timestamp=%" PRId64 " was greater than INT64_MAX=%ld", uts, INT64_MAX);
399 int64_t its = uts;
400 return its;
401}
402
403//! @returns maps a dataset timestamp to current time
404static timepoint_ns
405euroc_player_mapped_ts(struct euroc_player *ep, timepoint_ns ts)
406{
407 timepoint_ns relative_ts = ts - ep->base_ts; // Relative to imu0 first ts
408 ep->playback.speed = MAX(ep->playback.speed, 1.0 / 256);
409 double speed = ep->playback.speed;
410 timepoint_ns mapped_ts = ep->start_ts + ep->offset_ts + relative_ts / speed;
411 return mapped_ts;
412}
413
414//! Same as @ref euroc_player_mapped_ts but only if playback options allow it.
415static timepoint_ns
416euroc_player_mapped_playback_ts(struct euroc_player *ep, timepoint_ns ts)
417{
418 if (ep->playback.use_source_ts) {
419 return ts;
420 }
421 return euroc_player_mapped_ts(ep, ts);
422}
423
424static void
425euroc_player_load_next_frame(struct euroc_player *ep, int cam_index, struct xrt_frame *&xf)
426{
427 using xrt::auxiliary::tracking::FrameMat;
428 img_sample sample = ep->imgs->at(cam_index).at(ep->img_seq);
429 ep->playback.scale = CLAMP(ep->playback.scale, 1.0 / 16, 4);
430
431 // Load will be influenced by these playback options
432 bool allow_color = ep->playback.color;
433 float scale = ep->playback.scale;
434
435 // Load image from disk
436 timepoint_ns timestamp = euroc_player_mapped_playback_ts(ep, sample.first);
437 string img_name = sample.second;
438 EUROC_TRACE(ep, "cam%d img t = %ld filename = %s", cam_index, timestamp, img_name.c_str());
439 cv::ImreadModes read_mode = allow_color ? cv::IMREAD_ANYCOLOR : cv::IMREAD_GRAYSCALE;
440 cv::Mat img = cv::imread(img_name, read_mode); // If colored, reads in BGR order
441
442 if (scale != 1.0) {
443 cv::Mat tmp;
444 cv::resize(img, tmp, cv::Size(), scale, scale);
445 img = tmp;
446 }
447
448 // Create xrt_frame, it will be freed by FrameMat destructor
449 EUROC_ASSERT(xf == NULL || xf->reference.count > 0, "Must be given a valid or NULL frame ptr");
450 EUROC_ASSERT(timestamp >= 0, "Unexpected negative timestamp");
451 //! @todo Not using xrt_stereo_format because we use two sinks. It would
452 //! probably be better to refactor everything to use stereo frames instead.
453 FrameMat::Params params{XRT_STEREO_FORMAT_NONE, static_cast<uint64_t>(timestamp)};
454 auto wrap = img.channels() == 3 ? FrameMat::wrapR8G8B8 : FrameMat::wrapL8;
455 wrap(img, &xf, params);
456
457 // Fields that aren't set by FrameMat
458 xf->owner = ep;
459 xf->source_timestamp = sample.first;
460 xf->source_sequence = ep->img_seq;
461 xf->source_id = ep->base.source_id;
462}
463
464static void
465euroc_player_push_next_frame(struct euroc_player *ep)
466{
467 int cam_count = ep->playback.cam_count;
468
469 vector<xrt_frame *> xfs(cam_count, nullptr);
470 for (int i = 0; i < cam_count; i++) {
471 euroc_player_load_next_frame(ep, i, xfs[i]);
472 }
473
474 // TODO: Some SLAM systems expect synced frames, but that's not an
475 // EuRoC requirement. Adapt to work with unsynced datasets too.
476 for (int i = 1; i < cam_count; i++) {
477 EUROC_ASSERT(xfs[i - 1]->timestamp == xfs[i]->timestamp, "Unsynced frames");
478 }
479
480 ep->img_seq++;
481
482 for (int i = 0; i < cam_count; i++) {
483 xrt_sink_push_frame(ep->in_sinks.cams[i], xfs[i]);
484 }
485
486 for (int i = 0; i < cam_count; i++) {
487 xrt_frame_reference(&xfs[i], NULL);
488 }
489
490 size_t fcount = ep->imgs->at(0).size();
491 (void)snprintf(ep->progress_text, sizeof(ep->progress_text),
492 "Playback %.2f%% - Frame %" PRId64 "/%" PRId64 " - IMU %" PRId64 "/%" PRId64,
493 float(ep->img_seq) / float(fcount) * 100, ep->img_seq, fcount, ep->imu_seq, ep->imus->size());
494
495 if (ep->playback.print_progress) {
496 printf("%s\r", ep->progress_text);
497 (void)fflush(stdout);
498 }
499}
500
501static void
502euroc_player_push_next_imu(struct euroc_player *ep)
503{
504 xrt_imu_sample sample = ep->imus->at(ep->imu_seq++);
505 sample.timestamp_ns = euroc_player_mapped_playback_ts(ep, sample.timestamp_ns);
506 xrt_sink_push_imu(ep->in_sinks.imu, &sample);
507}
508
509static void
510euroc_player_push_all_gt(struct euroc_player *ep)
511{
512 if (!ep->out_sinks.gt) {
513 return;
514 }
515
516 for (xrt_pose_sample &sample : *ep->gt) {
517 sample.timestamp_ns = euroc_player_mapped_playback_ts(ep, sample.timestamp_ns);
518 xrt_sink_push_pose(ep->out_sinks.gt, &sample);
519 }
520}
521
522template <typename SamplesType>
523timepoint_ns
524euroc_player_get_next_euroc_ts(struct euroc_player *ep)
525{
526 if constexpr (is_same_v<SamplesType, imu_samples>) {
527 return ep->imus->at(ep->imu_seq).timestamp_ns;
528 } else {
529 return ep->imgs->at(0).at(ep->img_seq).first;
530 }
531}
532
533template <typename SamplesType>
534void
535euroc_player_sleep_until_next_sample(struct euroc_player *ep)
536{
537 using std::chrono::nanoseconds;
538 using timepoint_ch = std::chrono::time_point<std::chrono::steady_clock>;
539 using std::this_thread::sleep_until;
540
541 timepoint_ns next_sample_euroc_ts = euroc_player_get_next_euroc_ts<SamplesType>(ep);
542 timepoint_ns next_sample_mono_ts = euroc_player_mapped_ts(ep, next_sample_euroc_ts);
543 timepoint_ch next_sample_chrono_tp{nanoseconds{next_sample_mono_ts}};
544 sleep_until(next_sample_chrono_tp);
545
546#ifndef NDEBUG
547 // Complain when we are >1ms late. It can happen due to a busy scheduler.
548 double oversleep_ms = (os_monotonic_get_ts() - next_sample_mono_ts) / (double)U_TIME_1MS_IN_NS;
549 if (abs(oversleep_ms) > 1) {
550 string sample_type_name = "frame";
551 if constexpr (is_same_v<SamplesType, imu_samples>) {
552 sample_type_name = "imu";
553 }
554 EUROC_DEBUG(ep, "(%s) Woke up %.1fms late", sample_type_name.c_str(), oversleep_ms);
555 }
556#endif
557}
558
559//! Based on the SamplesType to stream, return a set of corresponding entities:
560//! the samples vector, sequence number, push and sleep functions.
561template <typename SamplesType>
562auto
563euroc_player_get_stream_set(struct euroc_player *ep)
564{
565 constexpr bool is_imu = is_same_v<SamplesType, imu_samples>;
566 const SamplesType *samples;
567 if constexpr (is_imu) {
568 samples = ep->imus;
569 } else {
570 samples = &ep->imgs->at(0);
571 }
572 uint64_t *sample_seq = is_imu ? &ep->imu_seq : &ep->img_seq;
573 auto push_next_sample = is_imu ? euroc_player_push_next_imu : euroc_player_push_next_frame;
574 auto sleep_until_next_sample = euroc_player_sleep_until_next_sample<SamplesType>;
575 return make_tuple(samples, sample_seq, push_next_sample, sleep_until_next_sample);
576}
577
578template <typename SamplesType>
579static void
580euroc_player_stream_samples(struct euroc_player *ep)
581{
582 // These fields correspond to IMU or frame streams depending on SamplesType
583 const auto [samples, sample_seq, push_next_sample, sleep_until_next_sample] =
584 euroc_player_get_stream_set<SamplesType>(ep);
585
586 while (*sample_seq < samples->size() && ep->is_running) {
587 while (ep->playback.paused) {
588 constexpr int64_t PAUSE_POLL_INTERVAL_NS = 15L * U_TIME_1MS_IN_NS;
589 os_nanosleep(PAUSE_POLL_INTERVAL_NS);
590 }
591
592 if (!ep->playback.max_speed) {
593 sleep_until_next_sample(ep);
594 }
595
596 push_next_sample(ep);
597 }
598}
599
600static void *
601euroc_player_stream(void *ptr)
602{
603 struct xrt_fs *xfs = (struct xrt_fs *)ptr;
604 struct euroc_player *ep = euroc_player(xfs);
605 EUROC_INFO(ep, "Starting euroc playback");
606
607 euroc_player_preload(ep);
608 ep->base_ts = MIN(ep->imgs->at(0).at(0).first, ep->imus->at(0).timestamp_ns);
609 ep->start_ts = os_monotonic_get_ts();
610 euroc_player_user_skip(ep);
611
612 // Push all IMU samples now if requested
613 if (ep->playback.send_all_imus_first) {
614 while (ep->imu_seq < ep->imus->size()) {
615 euroc_player_push_next_imu(ep);
616 }
617 }
618
619 // Push ground truth trajectory now if available (and not disabled)
620 if (ep->playback.gt) {
621 euroc_player_push_all_gt(ep);
622 }
623
624 // Launch image and IMU producers
625 auto serve_imus = async(launch::async, [ep] { euroc_player_stream_samples<imu_samples>(ep); });
626 auto serve_imgs = async(launch::async, [ep] { euroc_player_stream_samples<img_samples>(ep); });
627 // Note that the only fields of `ep` being modified in the threads are: img_seq, imu_seq and
628 // progress_text in single locations, thus no race conditions should occur.
629
630 // Wait for the end of both streams
631 serve_imgs.get();
632 serve_imus.get();
633
634 ep->is_running = false;
635
636 EUROC_INFO(ep, "Euroc dataset playback finished");
637 euroc_player_set_ui_state(ep, STREAM_ENDED);
638
639 return NULL;
640}
641
642
643// Frame server functionality
644
645static bool
646euroc_player_enumerate_modes(struct xrt_fs *xfs, struct xrt_fs_mode **out_modes, uint32_t *out_count)
647{
648 struct euroc_player *ep = euroc_player(xfs);
649
650 // Should be freed by caller
651 struct xrt_fs_mode *modes = U_TYPED_ARRAY_CALLOC(struct xrt_fs_mode, 1);
652 EUROC_ASSERT(modes != NULL, "Unable to calloc euroc playback modes");
653
654 // At first, it would sound like a good idea to list all possible playback
655 // modes here, however it gets more troublesome than it is worth, and there
656 // doesn't seem to be a good reason to use this feature here. Having said
657 // that, a basic fs mode will be provided, which consists of only the original
658 // properties of the dataset, and ignores the other playback options that can
659 // be tweaked in the UI.
660 modes[0] = ep->mode;
661
662 *out_modes = modes;
663 *out_count = 1;
664
665 return true;
666}
667
668static bool
669euroc_player_configure_capture(struct xrt_fs *xfs, struct xrt_fs_capture_parameters *cp)
670{
671 // struct euroc_player *ep = euroc_player(xfs);
672 EUROC_ASSERT(false, "Not implemented");
673 return false;
674}
675
676#define DEFINE_RECEIVE_CAM(cam_id) \
677 static void receive_cam##cam_id(struct xrt_frame_sink *sink, struct xrt_frame *xf) \
678 { \
679 struct euroc_player *ep = container_of(sink, struct euroc_player, cam_sinks[cam_id]); \
680 EUROC_TRACE(ep, "cam%d img t=%ld source_t=%ld", cam_id, xf->timestamp, xf->source_timestamp); \
681 u_sink_debug_push_frame(&ep->ui_cam_sinks[cam_id], xf); \
682 if (ep->out_sinks.cams[cam_id]) { \
683 xrt_sink_push_frame(ep->out_sinks.cams[cam_id], xf); \
684 } \
685 }
686
687
688DEFINE_RECEIVE_CAM(0)
689DEFINE_RECEIVE_CAM(1)
690DEFINE_RECEIVE_CAM(2)
691DEFINE_RECEIVE_CAM(3)
692DEFINE_RECEIVE_CAM(4)
693
694//! Be sure to define the same number of definition as EUROC_MAX_CAMS and to add them to `receive_cam`.
695static void (*receive_cam[EUROC_MAX_CAMS])(struct xrt_frame_sink *, struct xrt_frame *) = {
696 receive_cam0, //
697 receive_cam1, //
698 receive_cam2, //
699 receive_cam3, //
700 receive_cam4, //
701};
702
703static void
704receive_imu_sample(struct xrt_imu_sink *sink, struct xrt_imu_sample *s)
705{
706 struct euroc_player *ep = container_of(sink, struct euroc_player, imu_sink);
707
708 timepoint_ns ts = s->timestamp_ns;
709 xrt_vec3_f64 a = s->accel_m_s2;
710 xrt_vec3_f64 w = s->gyro_rad_secs;
711
712 // UI log
713 const xrt_vec3 gyro{(float)w.x, (float)w.y, (float)w.z};
714 const xrt_vec3 accel{(float)a.x, (float)a.y, (float)a.z};
715 m_ff_vec3_f32_push(ep->gyro_ff, &gyro, ts);
716 m_ff_vec3_f32_push(ep->accel_ff, &accel, ts);
717
718 // Trace log
719 EUROC_TRACE(ep, "imu t=%ld ax=%f ay=%f az=%f wx=%f wy=%f wz=%f", ts, a.x, a.y, a.z, w.x, w.y, w.z);
720 if (ep->out_sinks.imu) {
721 xrt_sink_push_imu(ep->out_sinks.imu, s);
722 }
723}
724
725//! This is the @ref xrt_fs stream start method, however as the euroc playback
726//! is heavily customizable, it will be managed through the UI. So, unless
727//! EUROC_PLAY_FROM_START is set, this will not start outputting frames until
728//! the user clicks the start button.
729static bool
730euroc_player_stream_start(struct xrt_fs *xfs,
731 struct xrt_frame_sink *xs,
732 enum xrt_fs_capture_type capture_type,
733 uint32_t descriptor_index)
734{
735 struct euroc_player *ep = euroc_player(xfs);
736
737 if (xs == NULL && capture_type == XRT_FS_CAPTURE_TYPE_TRACKING) {
738 EUROC_INFO(ep, "Starting Euroc Player in tracking mode");
739 if (ep->out_sinks.cams[0] == NULL) {
740 EUROC_WARN(ep, "No cam0 sink provided, will keep running but tracking is unlikely to work");
741 }
742 if (ep->playback.play_from_start) {
743 euroc_player_start_btn_cb(ep);
744 }
745 } else if (xs != NULL && capture_type == XRT_FS_CAPTURE_TYPE_CALIBRATION) {
746 EUROC_INFO(ep, "Starting Euroc Player in calibration mode, will stream only cam0 frames right away");
747 ep->out_sinks.cams[0] = xs;
748 euroc_player_start_btn_cb(ep);
749 } else {
750 EUROC_ASSERT(false, "Unsupported stream configuration xs=%p capture_type=%d", (void *)xs, capture_type);
751 return false;
752 }
753
754 ep->is_running = true;
755 return ep->is_running;
756}
757
758static bool
759euroc_player_slam_stream_start(struct xrt_fs *xfs, struct xrt_slam_sinks *sinks)
760{
761 struct euroc_player *ep = euroc_player(xfs);
762 ep->out_sinks = *sinks;
763 return euroc_player_stream_start(xfs, NULL, XRT_FS_CAPTURE_TYPE_TRACKING, 0);
764}
765
766static bool
767euroc_player_stream_stop(struct xrt_fs *xfs)
768{
769 struct euroc_player *ep = euroc_player(xfs);
770 ep->is_running = false;
771
772 // Destroy also stops the thread.
773 os_thread_helper_destroy(&ep->play_thread);
774
775 return true;
776}
777
778static bool
779euroc_player_is_running(struct xrt_fs *xfs)
780{
781 struct euroc_player *ep = euroc_player(xfs);
782 return ep->is_running;
783}
784
785
786// Frame node functionality
787
788static void
789euroc_player_break_apart(struct xrt_frame_node *node)
790{
791 struct euroc_player *ep = container_of(node, struct euroc_player, node);
792 euroc_player_stream_stop(&ep->base);
793}
794
795static void
796euroc_player_destroy(struct xrt_frame_node *node)
797{
798 struct euroc_player *ep = container_of(node, struct euroc_player, node);
799
800 delete ep->gt;
801 delete ep->imus;
802 delete ep->imgs;
803
804 u_var_remove_root(ep);
805 for (int i = 0; i < ep->dataset.cam_count; i++) {
806 u_sink_debug_destroy(&ep->ui_cam_sinks[i]);
807 }
808 m_ff_vec3_f32_free(&ep->gyro_ff);
809 m_ff_vec3_f32_free(&ep->accel_ff);
810
811 free(ep);
812}
813
814
815// UI functionality
816
817static void
818euroc_player_set_ui_state(struct euroc_player *ep, euroc_player_ui_state state)
819{
820 // -> UNINITIALIZED -> NOT_STREAMING -> STREAM_PLAYING <-> STREAM_PAUSED
821 // └> STREAM_ENDED <┘
822 euroc_player_ui_state prev_state = ep->ui_state;
823 if (state == NOT_STREAMING) {
824 EUROC_ASSERT_(prev_state == UNINITIALIZED);
825 ep->pause_btn.disabled = true;
826 snprintf(ep->progress_text, sizeof(ep->progress_text), "Stream has not started");
827 } else if (state == STREAM_PLAYING) {
828 EUROC_ASSERT_(prev_state == NOT_STREAMING || prev_state == STREAM_PAUSED);
829 ep->start_btn.disabled = true;
830 ep->pause_btn.disabled = false;
831 snprintf(ep->pause_btn.label, sizeof(ep->pause_btn.label), "Pause");
832 } else if (state == STREAM_PAUSED) {
833 EUROC_ASSERT_(prev_state == STREAM_PLAYING);
834 snprintf(ep->pause_btn.label, sizeof(ep->pause_btn.label), "Resume");
835 } else if (state == STREAM_ENDED) {
836 EUROC_ASSERT_(prev_state == STREAM_PLAYING || prev_state == STREAM_PAUSED);
837 ep->pause_btn.disabled = true;
838 } else {
839 EUROC_ASSERT(false, "Unexpected UI state transition from %d to %d", prev_state, state);
840 }
841 ep->ui_state = state;
842}
843
844static void
845euroc_player_start_btn_cb(void *ptr)
846{
847 struct euroc_player *ep = (struct euroc_player *)ptr;
848
849 int ret = 0;
850 ret |= os_thread_helper_init(&ep->play_thread);
851 ret |= os_thread_helper_start(&ep->play_thread, euroc_player_stream, ep);
852 EUROC_ASSERT(ret == 0, "Thread launch failure");
853
854 euroc_player_set_ui_state(ep, STREAM_PLAYING);
855}
856
857static void
858euroc_player_pause_btn_cb(void *ptr)
859{
860 //! @note: if you have groundtruth, pausing will unsync it from the tracker.
861
862 struct euroc_player *ep = (struct euroc_player *)ptr;
863 ep->playback.paused = !ep->playback.paused;
864
865 if (ep->playback.paused) {
866 ep->last_pause_ts = os_monotonic_get_ts();
867 } else {
868 time_duration_ns pause_length = os_monotonic_get_ts() - ep->last_pause_ts;
869 ep->offset_ts += pause_length;
870 }
871
872 euroc_player_set_ui_state(ep, ep->playback.paused ? STREAM_PAUSED : STREAM_PLAYING);
873}
874
875static void
876euroc_player_setup_gui(struct euroc_player *ep)
877{
878 // Set sinks to display in UI
879 for (int i = 0; i < ep->dataset.cam_count; i++) {
880 u_sink_debug_init(&ep->ui_cam_sinks[i]);
881 }
882 m_ff_vec3_f32_alloc(&ep->gyro_ff, 1000);
883 m_ff_vec3_f32_alloc(&ep->accel_ff, 1000);
884
885 // Set button callbacks
886 ep->start_btn.cb = euroc_player_start_btn_cb;
887 ep->start_btn.ptr = ep;
888 ep->pause_btn.cb = euroc_player_pause_btn_cb;
889 ep->pause_btn.ptr = ep;
890 euroc_player_set_ui_state(ep, NOT_STREAMING);
891
892 // Add UI wigets
893 u_var_add_root(ep, "Euroc Player", false);
894 u_var_add_ro_text(ep, ep->dataset.path, "Dataset");
895 u_var_add_ro_text(ep, ep->progress_text, "Progress");
896 u_var_add_button(ep, &ep->start_btn, "Start");
897 u_var_add_button(ep, &ep->pause_btn, "Pause");
898 u_var_add_log_level(ep, &ep->log_level, "Log level");
899
900 u_var_add_gui_header(ep, NULL, "Playback Options");
901 u_var_add_ro_text(ep, "Set these before starting the stream", "Note");
902 u_var_add_i32(ep, &ep->playback.cam_count, "Use N cams (if available)");
903 u_var_add_bool(ep, &ep->playback.color, "Color (if available)");
904 u_var_add_bool(ep, &ep->playback.gt, "Groundtruth (if available)");
905 u_var_add_bool(ep, &ep->playback.skip_perc, "Skip percentage, otherwise skips seconds");
906 u_var_add_f32(ep, &ep->playback.skip_first, "How much to skip");
907 u_var_add_f32(ep, &ep->playback.scale, "Scale");
908 u_var_add_bool(ep, &ep->playback.max_speed, "Max speed");
909 u_var_add_f64(ep, &ep->playback.speed, "Speed");
910 u_var_add_bool(ep, &ep->playback.send_all_imus_first, "Send all IMU samples first");
911 u_var_add_bool(ep, &ep->playback.use_source_ts, "Use original timestamps");
912
913 u_var_add_gui_header(ep, NULL, "Streams");
914 u_var_add_ro_ff_vec3_f32(ep, ep->gyro_ff, "Gyroscope");
915 u_var_add_ro_ff_vec3_f32(ep, ep->accel_ff, "Accelerometer");
916 for (int i = 0; i < ep->dataset.cam_count; i++) {
917 char label[] = "Camera NNNNNNNNNN";
918 (void)snprintf(label, sizeof(label), "Camera %d", i);
919 u_var_add_sink_debug(ep, &ep->ui_cam_sinks[i], label);
920 }
921}
922
923extern "C" void
924euroc_player_fill_default_config_for(struct euroc_player_config *config, const char *dataset_path)
925{
926 struct euroc_player_dataset_info dataset = {};
927 dataset.gt_device_name = debug_get_option_gt_device_name();
928 euroc_player_fill_dataset_info(dataset_path, &dataset);
929
930 struct euroc_player_playback_config playback = {};
931 const char *cam_count = debug_get_option_cam_count();
932 const char *color = debug_get_option_color();
933 const char *gt = debug_get_option_gt();
934 const char *skip_option = debug_get_option_skip_first();
935 playback.cam_count = (int)debug_string_to_num(cam_count, dataset.cam_count);
936 playback.color = color == nullptr ? dataset.is_colored : debug_string_to_bool(color);
937 playback.gt = gt == nullptr ? dataset.has_gt : debug_string_to_bool(gt);
938 playback.skip_perc = string(skip_option).back() == '%';
939 playback.skip_first = stof(skip_option);
940 playback.scale = debug_get_float_option_scale();
941 playback.max_speed = debug_get_bool_option_max_speed();
942 playback.speed = debug_get_float_option_speed();
943 playback.paused = debug_get_bool_option_paused();
944 playback.send_all_imus_first = debug_get_bool_option_send_all_imus_first();
945 playback.use_source_ts = debug_get_bool_option_use_source_ts();
946 playback.play_from_start = debug_get_bool_option_play_from_start();
947 playback.print_progress = debug_get_bool_option_print_progress();
948
949 config->log_level = debug_get_log_option_euroc_log();
950 config->dataset = dataset;
951 config->playback = playback;
952}
953
954// Euroc driver creation
955
956extern "C" struct xrt_fs *
957euroc_player_create(struct xrt_frame_context *xfctx, const char *path, struct euroc_player_config *config)
958{
959 struct euroc_player *ep = U_TYPED_CALLOC(struct euroc_player);
960
961 struct euroc_player_config *default_config = nullptr;
962 if (config == nullptr) {
963 default_config = U_TYPED_CALLOC(struct euroc_player_config);
964 euroc_player_fill_default_config_for(default_config, path);
965 config = default_config;
966 }
967
968 ep->log_level = config->log_level;
969 ep->dataset = config->dataset;
970 ep->playback = config->playback;
971
972 if (default_config != nullptr) {
973 free(default_config);
974 }
975
976 ep->mode = xrt_fs_mode{
977 ep->dataset.width,
978 ep->dataset.height,
979 ep->dataset.is_colored ? XRT_FORMAT_R8G8B8 : XRT_FORMAT_R8,
980 // Stereo euroc *is* supported, but we don't expose that through the
981 // xrt_fs interface as it will be managed through two different sinks.
982 XRT_STEREO_FORMAT_NONE,
983 };
984 EUROC_INFO(ep, "dataset information\n\tpath: %s\n\tcam_count: %d, is_colored: %d, width: %d, height: %d",
985 ep->dataset.path, ep->dataset.cam_count, ep->dataset.is_colored, ep->dataset.width,
986 ep->dataset.height);
987
988 // Using pointers to not mix vector with a C-compatible struct
989 ep->gt = new gt_trajectory{};
990 ep->imus = new imu_samples{};
991 ep->imgs = new vector<img_samples>(ep->dataset.cam_count);
992
993 euroc_player_setup_gui(ep);
994
995 EUROC_ASSERT(receive_cam[ARRAY_SIZE(receive_cam) - 1] != nullptr, "See `receive_cam` docs");
996 ep->in_sinks.cam_count = ep->dataset.cam_count;
997 for (int i = 0; i < ep->dataset.cam_count; i++) {
998 ep->cam_sinks[i].push_frame = receive_cam[i];
999 ep->in_sinks.cams[i] = &ep->cam_sinks[i];
1000 }
1001 ep->imu_sink.push_imu = receive_imu_sample;
1002 ep->in_sinks.imu = &ep->imu_sink;
1003
1004 struct xrt_fs *xfs = &ep->base;
1005 xfs->enumerate_modes = euroc_player_enumerate_modes;
1006 xfs->configure_capture = euroc_player_configure_capture;
1007 xfs->stream_start = euroc_player_stream_start;
1008 xfs->slam_stream_start = euroc_player_slam_stream_start;
1009 xfs->stream_stop = euroc_player_stream_stop;
1010 xfs->is_running = euroc_player_is_running;
1011
1012 (void)snprintf(xfs->name, sizeof(xfs->name), EUROC_PLAYER_STR);
1013 (void)snprintf(xfs->product, sizeof(xfs->product), EUROC_PLAYER_STR " Product");
1014 (void)snprintf(xfs->manufacturer, sizeof(xfs->manufacturer), EUROC_PLAYER_STR " Manufacturer");
1015 (void)snprintf(xfs->serial, sizeof(xfs->serial), EUROC_PLAYER_STR " Serial");
1016 xfs->source_id = 0xECD0FEED;
1017
1018 struct xrt_frame_node *xfn = &ep->node;
1019 xfn->break_apart = euroc_player_break_apart;
1020 xfn->destroy = euroc_player_destroy;
1021
1022 xrt_frame_context_add(xfctx, &ep->node);
1023
1024 EUROC_DEBUG(ep, "Euroc player created");
1025
1026 return xfs;
1027}