The open source OpenXR runtime
d/euroc: Improve player tolerance to more datasets
Makes all usable euroc and tumvi datasets stream properly by fixing
some border cases that a couple of datasets had.
+43
-23
+43
-23
src/xrt/drivers/euroc/euroc_player.cpp
+43
-23
src/xrt/drivers/euroc/euroc_player.cpp
···
20
20
21
21
#include "euroc_driver.h"
22
22
23
+
#include <algorithm>
23
24
#include <chrono>
24
25
#include <stdio.h>
25
26
#include <fstream>
···
38
39
#define CLAMP(X, A, B) (MIN(MAX((X), (A)), (B)))
39
40
40
41
using std::async;
42
+
using std::find_if;
41
43
using std::ifstream;
42
44
using std::is_same_v;
43
45
using std::launch;
···
83
85
struct os_thread_helper play_thread;
84
86
enum u_logging_level log_level;
85
87
struct xrt_fs_mode mode; //!< The only fs mode the euroc dataset provides
86
-
bool is_running; //!< Set only at start and stop of frameserver stream
88
+
bool is_running; //!< Set only at start, stop and end of frameserver stream
87
89
88
90
//! Contains information about the source dataset; set only at start
89
91
struct
···
107
109
gt_trajectory *gt; //!< List of all groundtruth poses read from the dataset
108
110
109
111
// Timestamp correction fields (can be disabled through `use_source_ts`)
110
-
timepoint_ns base_ts; //!< First imu0 timestamp, samples timestamps are relative to this
112
+
timepoint_ns base_ts; //!< First sample timestamp, stream timestamps are relative to this
111
113
timepoint_ns start_ts; //!< When did the dataset started to be played
112
114
timepoint_ns offset_ts; //!< Amount of ns to offset start_ns (pauses, skips, etc)
113
115
···
152
154
// Euroc functionality
153
155
154
156
//! Parse and load all IMU samples into `samples`, assumes data.csv is well formed
155
-
//! If `ep` is not null, will set `ep->base_ts` with the first timestamp read
156
157
//! If `read_n` > 0, read at most that amount of samples
157
158
//! Returns whether the appropriate data.csv file could be opened
158
159
static bool
159
-
euroc_player_preload_imu_data(struct euroc_player *ep,
160
-
const string &dataset_path,
161
-
imu_samples *samples,
162
-
int64_t read_n = -1)
160
+
euroc_player_preload_imu_data(const string &dataset_path, imu_samples *samples, int64_t read_n = -1)
163
161
{
164
162
string csv_filename = dataset_path + "/mav0/imu0/data.csv";
165
163
ifstream fin{csv_filename};
···
170
168
constexpr int COLUMN_COUNT = 6; // EuRoC imu columns: ts wx wy wz ax ay az
171
169
string line;
172
170
getline(fin, line); // Skip header line
173
-
bool set_base_ts = ep != NULL;
174
171
175
172
while (getline(fin, line) && read_n-- != 0) {
176
173
timepoint_ns timestamp;
···
184
181
v[k] = stod(line.substr(i + 1, j));
185
182
}
186
183
187
-
// Save first IMU sample timestamp
188
-
if (set_base_ts) {
189
-
ep->base_ts = timestamp;
190
-
set_base_ts = false;
191
-
}
192
-
193
184
xrt_imu_sample sample{timestamp, {v[3], v[4], v[5]}, {v[0], v[1], v[2]}};
194
185
samples->push_back(sample);
195
186
}
···
283
274
return true;
284
275
}
285
276
277
+
//! Trims left and right sequences so that they start and end at the same sample
278
+
//! Note that this function does not guarantee that the dataset is free of framedrops.
279
+
static void
280
+
euroc_player_match_stereo_seqs(struct euroc_player *ep)
281
+
{
282
+
img_samples ls = *ep->left_imgs;
283
+
img_samples rs = *ep->right_imgs;
284
+
285
+
// Assumes dataset is properly formatted with monotonically increasing timestamps
286
+
timepoint_ns first_ts = MAX(ls.at(0).first, rs.at(0).first);
287
+
timepoint_ns last_ts = MIN(ls.back().first, rs.back().first);
288
+
289
+
auto is_first = [first_ts](const img_sample &s) { return s.first == first_ts; };
290
+
auto is_last = [last_ts](const img_sample &s) { return s.first == last_ts; };
291
+
292
+
img_samples::iterator lfirst = find_if(ls.begin(), ls.end(), is_first);
293
+
img_samples::iterator llast = find_if(ls.begin(), ls.end(), is_last);
294
+
EUROC_ASSERT_(lfirst != ls.end() && llast != ls.end());
295
+
296
+
img_samples::iterator rfirst = find_if(rs.begin(), rs.end(), is_first);
297
+
img_samples::iterator rlast = find_if(rs.begin(), rs.end(), is_last);
298
+
EUROC_ASSERT_(rfirst != rs.end() && rlast != rs.end());
299
+
300
+
ep->left_imgs->assign(lfirst, llast + 1);
301
+
ep->right_imgs->assign(rfirst, rlast + 1);
302
+
}
303
+
286
304
static void
287
305
euroc_player_preload(struct euroc_player *ep)
288
306
{
289
307
ep->imus->clear();
290
-
euroc_player_preload_imu_data(ep, ep->dataset.path, ep->imus);
308
+
euroc_player_preload_imu_data(ep->dataset.path, ep->imus);
291
309
292
310
ep->left_imgs->clear();
293
311
euroc_player_preload_img_data(ep->dataset.path, ep->left_imgs, true);
···
295
313
if (ep->dataset.is_stereo) {
296
314
ep->right_imgs->clear();
297
315
euroc_player_preload_img_data(ep->dataset.path, ep->right_imgs, false);
316
+
317
+
euroc_player_match_stereo_seqs(ep);
298
318
}
299
319
300
320
if (ep->dataset.has_gt) {
···
321
341
time_duration_ns skip_first_ns = skip_first_s * U_TIME_1S_IN_NS;
322
342
timepoint_ns skipped_ts = ep->base_ts + skip_first_ns;
323
343
324
-
while (ep->imus->at(ep->imu_seq).timestamp_ns < skipped_ts) {
344
+
while (ep->imu_seq < ep->imus->size() && ep->imus->at(ep->imu_seq).timestamp_ns < skipped_ts) {
325
345
ep->imu_seq++;
326
346
}
327
347
328
-
while (ep->left_imgs->at(ep->img_seq).first < skipped_ts) {
348
+
while (ep->img_seq < ep->left_imgs->size() && ep->left_imgs->at(ep->img_seq).first < skipped_ts) {
329
349
ep->img_seq++;
330
350
}
331
351
···
346
366
gt_trajectory _2;
347
367
bool has_right_camera = euroc_player_preload_img_data(ep->dataset.path, &samples, false, 0);
348
368
bool has_left_camera = euroc_player_preload_img_data(ep->dataset.path, &samples, true, 1);
349
-
bool has_imu = euroc_player_preload_imu_data(NULL, ep->dataset.path, &_1, 0);
369
+
bool has_imu = euroc_player_preload_imu_data(ep->dataset.path, &_1, 0);
350
370
bool has_gt = euroc_player_preload_gt_data(ep->dataset.path, &_2, 0);
351
371
bool is_valid_dataset = has_left_camera && has_imu;
352
372
EUROC_ASSERT(is_valid_dataset, "Invalid dataset %s", path);
···
381
401
return its;
382
402
}
383
403
384
-
//! @returns maps a timestamp to current time (wrt. ep->start_ts)
385
-
//! from the original euroc timestamp (uses first imu0 timestamp as base time)
404
+
//! @returns maps a dataset timestamp to current time
386
405
static timepoint_ns
387
406
euroc_player_mapped_ts(struct euroc_player *ep, timepoint_ns ts)
388
407
{
···
583
602
EUROC_INFO(ep, "Starting euroc playback");
584
603
585
604
euroc_player_preload(ep);
586
-
euroc_player_user_skip(ep);
587
-
605
+
ep->base_ts = MIN(ep->left_imgs->at(0).first, ep->imus->at(0).timestamp_ns);
588
606
ep->start_ts = os_monotonic_get_ts();
607
+
euroc_player_user_skip(ep);
589
608
590
609
// Push all IMU samples now if requested
591
610
if (ep->playback.send_all_imus_first) {
···
608
627
// Wait for the end of both streams
609
628
serve_imgs.get();
610
629
serve_imus.get();
630
+
631
+
ep->is_running = false;
611
632
612
633
EUROC_INFO(ep, "Euroc dataset playback finished");
613
634
euroc_player_set_ui_state(ep, STREAM_ENDED);
···
761
782
{
762
783
struct euroc_player *ep = container_of(node, struct euroc_player, node);
763
784
euroc_player_stream_stop(&ep->base);
764
-
return;
765
785
}
766
786
767
787
static void