The open source OpenXR runtime
t/euroc: Implement trajectory recording
authored by
Mateo de Mayo
and committed by
Moses Turner
5ba9efd5
8b8358f0
+95
-19
+73
-7
src/xrt/auxiliary/tracking/t_euroc_recorder.cpp
+73
-7
src/xrt/auxiliary/tracking/t_euroc_recorder.cpp
···
14
14
#include "util/u_sink.h"
15
15
#include "util/u_var.h"
16
16
#include "util/u_debug.h"
17
+
#include "xrt/xrt_defines.h"
18
+
#include "xrt/xrt_tracking.h"
17
19
18
20
#include <cassert>
19
21
#include <ctime>
···
26
28
27
29
#include <opencv2/imgcodecs.hpp>
28
30
29
-
DEBUG_GET_ONCE_BOOL_OPTION(euroc_recorder_use_jpg, "EUROC_RECORDER_USE_JPG", false);
30
-
31
-
//! @todo: Now that IMU sinks support groundtruth, we could save it here as well.
31
+
DEBUG_GET_ONCE_BOOL_OPTION(euroc_recorder_use_jpg, "EUROC_RECORDER_USE_JPG", false)
32
32
33
33
using std::lock_guard;
34
34
using std::mutex;
···
52
52
// Cloner sinks: copy frame to heap for quick release of the original
53
53
struct xrt_slam_sinks cloner_queues; //!< Queue sinks that write into cloner sinks
54
54
struct xrt_imu_sink cloner_imu_sink;
55
+
struct xrt_pose_sink cloner_gt_sink;
55
56
struct xrt_frame_sink cloner_left_sink;
56
57
struct xrt_frame_sink cloner_right_sink;
57
58
58
59
// Writer sinks: write copied frame to disk
59
60
struct xrt_slam_sinks writer_queues; //!< Queue sinks that write into writer sinks
60
61
struct xrt_imu_sink writer_imu_sink;
62
+
struct xrt_pose_sink writer_gt_sink;
61
63
struct xrt_frame_sink writer_left_sink;
62
64
struct xrt_frame_sink writer_right_sink;
63
65
64
66
queue<xrt_imu_sample> imu_queue{}; //!< IMU pushes get saved here and are delayed until left_frame pushes
65
67
mutex imu_queue_lock{}; //!< Lock for imu_queue
66
68
69
+
queue<xrt_pose_sample> gt_queue{}; //!< GT pushes get saved here and are delayed until left_frame pushes
70
+
mutex gt_queue_lock{}; //!< Lock for gt_queue
71
+
67
72
// CSV file handles, ofstream implementation is already buffered.
68
73
// Using pointers because of `container_of`
69
74
ofstream *imu_csv;
75
+
ofstream *gt_csv;
70
76
ofstream *left_cam_csv;
71
77
ofstream *right_cam_csv;
72
78
};
···
95
101
*er->imu_csv << "#timestamp [ns],w_RS_S_x [rad s^-1],w_RS_S_y [rad s^-1],w_RS_S_z [rad s^-1],"
96
102
"a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]" CSV_EOL;
97
103
104
+
create_directories(path + "/mav0/gt0");
105
+
er->gt_csv = new ofstream{path + "/mav0/gt0/data.csv"};
106
+
*er->gt_csv << std::fixed << std::setprecision(CSV_PRECISION);
107
+
*er->gt_csv << "#timestamp [ns],p_RS_R_x [m],p_RS_R_y [m],p_RS_R_z [m],"
108
+
"q_RS_w [],q_RS_x [],q_RS_y [],q_RS_z []" CSV_EOL;
109
+
98
110
create_directories(path + "/mav0/cam0/data");
99
111
er->left_cam_csv = new ofstream{path + "/mav0/cam0/data.csv"};
100
112
*er->left_cam_csv << "#timestamp [ns],filename" CSV_EOL;
···
107
119
static void
108
120
euroc_recorder_flush(struct euroc_recorder *er)
109
121
{
110
-
vector<xrt_imu_sample> samples;
122
+
// Flush IMU samples
123
+
vector<xrt_imu_sample> imu_samples;
111
124
112
125
{ // Move samples out of imu_queue into vector to minimize mutex contention
113
126
lock_guard lock{er->imu_queue_lock};
114
-
samples.reserve(er->imu_queue.size());
127
+
imu_samples.reserve(er->imu_queue.size());
115
128
while (!er->imu_queue.empty()) {
116
-
samples.push_back(er->imu_queue.front());
129
+
imu_samples.push_back(er->imu_queue.front());
117
130
er->imu_queue.pop();
118
131
}
119
132
}
120
133
121
134
// Write queued IMU samples to csv stream.
122
-
for (xrt_imu_sample &sample : samples) {
135
+
for (xrt_imu_sample &sample : imu_samples) {
123
136
xrt_sink_push_imu(&er->writer_imu_sink, &sample);
124
137
}
125
138
139
+
// Flush groundtruth samples
140
+
vector<xrt_pose_sample> gt_samples;
141
+
142
+
{ // Move samples out of gt_queue into vector to minimize mutex contention
143
+
lock_guard lock{er->gt_queue_lock};
144
+
gt_samples.reserve(er->gt_queue.size());
145
+
while (!er->gt_queue.empty()) {
146
+
gt_samples.push_back(er->gt_queue.front());
147
+
er->gt_queue.pop();
148
+
}
149
+
}
150
+
151
+
// Write queued gt samples to csv stream.
152
+
for (xrt_pose_sample &sample : gt_samples) {
153
+
xrt_sink_push_pose(&er->writer_gt_sink, &sample);
154
+
}
155
+
126
156
// Flush csv streams. Not necessary, doing it only to increase flush frequency
127
157
er->imu_csv->flush();
158
+
er->gt_csv->flush();
128
159
er->left_cam_csv->flush();
129
160
er->right_cam_csv->flush();
130
161
}
···
141
172
*er->imu_csv << ts << ",";
142
173
*er->imu_csv << w.x << "," << w.y << "," << w.z << ",";
143
174
*er->imu_csv << a.x << "," << a.y << "," << a.z << CSV_EOL;
175
+
}
176
+
177
+
extern "C" void
178
+
euroc_recorder_save_gt(xrt_pose_sink *sink, struct xrt_pose_sample *sample)
179
+
{
180
+
euroc_recorder *er = container_of(sink, euroc_recorder, writer_gt_sink);
181
+
182
+
timepoint_ns ts = sample->timestamp_ns;
183
+
xrt_vec3 p = sample->pose.position;
184
+
xrt_quat o = sample->pose.orientation;
185
+
186
+
*er->gt_csv << ts << ",";
187
+
*er->gt_csv << p.x << "," << p.y << "," << p.z << ",";
188
+
*er->gt_csv << o.w << "," << o.x << "," << o.y << "," << o.z << CSV_EOL;
144
189
}
145
190
146
191
static void
···
201
246
}
202
247
}
203
248
249
+
extern "C" void
250
+
euroc_recorder_receive_gt(xrt_pose_sink *sink, struct xrt_pose_sample *sample)
251
+
{
252
+
// This works similarly to euroc_recorder_receive_imu, read its comments
253
+
euroc_recorder *er = container_of(sink, euroc_recorder, cloner_gt_sink);
254
+
255
+
if (!er->recording) {
256
+
return;
257
+
}
258
+
259
+
{
260
+
lock_guard lock{er->gt_queue_lock};
261
+
er->gt_queue.push(*sample);
262
+
}
263
+
}
264
+
204
265
205
266
static void
206
267
euroc_recorder_receive_frame(euroc_recorder *er, struct xrt_frame *src_frame, bool is_left)
···
248
309
{
249
310
struct euroc_recorder *er = container_of(node, struct euroc_recorder, node);
250
311
delete er->imu_csv;
312
+
delete er->gt_csv;
251
313
delete er->left_cam_csv;
252
314
delete er->right_cam_csv;
253
315
delete er;
···
298
360
u_sink_queue_create(xfctx, 0, &er->cloner_left_sink, &er->cloner_queues.left);
299
361
u_sink_queue_create(xfctx, 0, &er->cloner_right_sink, &er->cloner_queues.right);
300
362
er->cloner_queues.imu = &er->cloner_imu_sink;
363
+
er->cloner_queues.gt = &er->cloner_gt_sink;
301
364
302
365
// Clone samples into heap and release original samples right after
303
366
er->cloner_imu_sink.push_imu = euroc_recorder_receive_imu;
367
+
er->cloner_gt_sink.push_pose = euroc_recorder_receive_gt;
304
368
er->cloner_left_sink.push_frame = euroc_recorder_receive_left;
305
369
er->cloner_right_sink.push_frame = euroc_recorder_receive_right;
306
370
···
308
372
u_sink_queue_create(xfctx, 0, &er->writer_left_sink, &er->writer_queues.left);
309
373
u_sink_queue_create(xfctx, 0, &er->writer_right_sink, &er->writer_queues.right);
310
374
er->writer_queues.imu = nullptr;
375
+
er->writer_queues.gt = nullptr;
311
376
312
377
// Write cloned samples to disk with these
313
378
er->writer_imu_sink.push_imu = euroc_recorder_save_imu;
379
+
er->writer_gt_sink.push_pose = euroc_recorder_save_gt;
314
380
er->writer_left_sink.push_frame = euroc_recorder_save_left;
315
381
er->writer_right_sink.push_frame = euroc_recorder_save_right;
316
382
+5
-3
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
+5
-3
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
···
816
816
817
817
t.slam_rels.push(rel, nts);
818
818
819
+
xrt_pose_sample sample{nts, rel.pose};
820
+
xrt_sink_push_pose(t.euroc_recorder->gt, &sample);
819
821
gt_ui_push(t, nts, rel.pose);
820
822
t.slam_traj_writer->push(nts, rel.pose);
821
823
···
1156
1158
1157
1159
//! Receive and register ground truth to use for trajectory error metrics.
1158
1160
extern "C" void
1159
-
t_slam_gt_sink_push(struct xrt_pose_sink *sink, timepoint_ns ts, struct xrt_pose *pose)
1161
+
t_slam_gt_sink_push(struct xrt_pose_sink *sink, xrt_pose_sample *sample)
1160
1162
{
1161
1163
auto &t = *container_of(sink, TrackerSlam, gt_sink);
1162
1164
1163
1165
if (t.gt.trajectory->empty()) {
1164
-
t.gt.origin = *pose;
1166
+
t.gt.origin = sample->pose;
1165
1167
gt_ui_setup(t);
1166
1168
}
1167
1169
1168
-
t.gt.trajectory->insert_or_assign(ts, *pose);
1170
+
t.gt.trajectory->insert_or_assign(sample->timestamp_ns, sample->pose);
1169
1171
}
1170
1172
1171
1173
//! Receive and send IMU samples to the external SLAM system
+5
-6
src/xrt/drivers/euroc/euroc_player.cpp
+5
-6
src/xrt/drivers/euroc/euroc_player.cpp
···
59
59
using std::vector;
60
60
61
61
using img_sample = pair<timepoint_ns, string>;
62
-
using gt_entry = pair<timepoint_ns, xrt_pose>;
63
62
64
63
using imu_samples = vector<xrt_imu_sample>;
65
64
using img_samples = vector<img_sample>;
66
-
using gt_trajectory = vector<gt_entry>;
65
+
using gt_trajectory = vector<xrt_pose_sample>;
67
66
68
67
enum euroc_player_ui_state
69
68
{
···
227
226
}
228
227
229
228
xrt_pose pose = {{v[4], v[5], v[6], v[3]}, {v[0], v[1], v[2]}};
230
-
trajectory->emplace_back(timestamp, pose);
229
+
trajectory->push_back({timestamp, pose});
231
230
}
232
231
return true;
233
232
}
···
497
496
return;
498
497
}
499
498
500
-
for (auto [ts, pose] : *ep->gt) {
501
-
ts = euroc_player_mapped_playback_ts(ep, ts);
502
-
xrt_sink_push_pose(ep->out_sinks.gt, ts, &pose);
499
+
for (xrt_pose_sample &sample : *ep->gt) {
500
+
sample.timestamp_ns = euroc_player_mapped_playback_ts(ep, sample.timestamp_ns);
501
+
xrt_sink_push_pose(ep->out_sinks.gt, &sample);
503
502
}
504
503
}
505
504
+12
-3
src/xrt/include/xrt/xrt_tracking.h
+12
-3
src/xrt/include/xrt/xrt_tracking.h
···
131
131
};
132
132
133
133
/*!
134
+
* Pose sample.
135
+
*/
136
+
struct xrt_pose_sample
137
+
{
138
+
timepoint_ns timestamp_ns;
139
+
struct xrt_pose pose;
140
+
};
141
+
142
+
/*!
134
143
* @interface xrt_imu_sink
135
144
*
136
145
* An object to send IMU samples to.
···
155
164
*/
156
165
struct xrt_pose_sink
157
166
{
158
-
void (*push_pose)(struct xrt_pose_sink *, timepoint_ns ts, struct xrt_pose *pose);
167
+
void (*push_pose)(struct xrt_pose_sink *, struct xrt_pose_sample *sample);
159
168
};
160
169
161
170
/*!
···
279
288
280
289
//! @public @memberof xrt_pose_sink
281
290
static inline void
282
-
xrt_sink_push_pose(struct xrt_pose_sink *sink, timepoint_ns ts, struct xrt_pose *pose)
291
+
xrt_sink_push_pose(struct xrt_pose_sink *sink, struct xrt_pose_sample *sample)
283
292
{
284
-
sink->push_pose(sink, ts, pose);
293
+
sink->push_pose(sink, sample);
285
294
}
286
295
287
296
//! @public @memberof xrt_tracked_psmv