The open source OpenXR runtime
t/euroc: Add euroc dataset recorder
authored by
Mateo de Mayo
and committed by
Jakob Bornecrantz
91ecbdb7
e245f4e5
+294
+2
src/xrt/auxiliary/CMakeLists.txt
+2
src/xrt/auxiliary/CMakeLists.txt
···
118
118
tracking/t_debug_hsv_filter.cpp
119
119
tracking/t_debug_hsv_picker.cpp
120
120
tracking/t_debug_hsv_viewer.cpp
121
+
tracking/t_euroc_recorder.cpp
122
+
tracking/t_euroc_recorder.h
121
123
tracking/t_file.cpp
122
124
tracking/t_frame_cv_mat_wrapper.cpp
123
125
tracking/t_frame_cv_mat_wrapper.hpp
+2
src/xrt/auxiliary/meson.build
+2
src/xrt/auxiliary/meson.build
···
215
215
'tracking/t_debug_hsv_filter.cpp',
216
216
'tracking/t_debug_hsv_picker.cpp',
217
217
'tracking/t_debug_hsv_viewer.cpp',
218
+
'tracking/t_euroc_recorder.cpp',
219
+
'tracking/t_euroc_recorder.h',
218
220
'tracking/t_file.cpp',
219
221
'tracking/t_frame_cv_mat_wrapper.cpp',
220
222
'tracking/t_frame_cv_mat_wrapper.hpp',
+257
src/xrt/auxiliary/tracking/t_euroc_recorder.cpp
+257
src/xrt/auxiliary/tracking/t_euroc_recorder.cpp
···
1
+
// Copyright 2021, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief EuRoC dataset recorder utility.
6
+
* @author Mateo de Mayo <mateo.demayo@collabora.com>
7
+
* @ingroup aux_tracking
8
+
*/
9
+
10
+
#include "t_euroc_recorder.h"
11
+
12
+
#include "os/os_time.h"
13
+
#include "util/u_frame.h"
14
+
#include "util/u_sink.h"
15
+
16
+
#include <cassert>
17
+
#include <ctime>
18
+
#include <filesystem>
19
+
#include <fstream>
20
+
#include <string>
21
+
#include <queue>
22
+
#include <iomanip>
23
+
24
+
#include <opencv2/imgcodecs.hpp>
25
+
26
+
using std::ofstream;
27
+
using std::queue;
28
+
using std::string;
29
+
using std::filesystem::create_directories;
30
+
31
+
struct euroc_recorder
32
+
{
33
+
struct xrt_frame_node node;
34
+
string path; //!< Destination path for the dataset
35
+
bool first_received; //!< Whether we have received the first sample
36
+
37
+
// Cloner sinks: copy frame to heap for quick release of the original
38
+
struct xrt_slam_sinks cloner_queues; //!< Queue sinks that write into cloner sinks
39
+
struct xrt_imu_sink cloner_imu_sink;
40
+
struct xrt_frame_sink cloner_left_sink;
41
+
struct xrt_frame_sink cloner_right_sink;
42
+
43
+
// Writer sinks: write copied frame to disk
44
+
struct xrt_slam_sinks writer_queues; //!< Queue sinks that write into writer sinks
45
+
struct xrt_imu_sink writer_imu_sink;
46
+
struct xrt_frame_sink writer_left_sink;
47
+
struct xrt_frame_sink writer_right_sink;
48
+
49
+
queue<xrt_imu_sample> imu_queue{}; //!< IMU pushes get saved here and are delayed until left_frame pushes
50
+
};
51
+
52
+
53
+
/*
54
+
*
55
+
* Writer sinks functionality
56
+
*
57
+
*/
58
+
59
+
static void
60
+
euroc_recorder_try_mkfiles(struct euroc_recorder *er)
61
+
{
62
+
// Create directory structure and files only on first received frame
63
+
if (er->first_received) {
64
+
return;
65
+
}
66
+
er->first_received = true;
67
+
68
+
string path = er->path;
69
+
70
+
create_directories(path + "/mav0/imu0");
71
+
ofstream imu_csv{path + "/mav0/imu0/data.csv"};
72
+
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],"
73
+
"a_RS_S_x [m s^-2],a_RS_S_y [m s^-2],a_RS_S_z [m s^-2]\r\n";
74
+
75
+
create_directories(path + "/mav0/cam0/data");
76
+
ofstream left_cam_csv{path + "/mav0/cam0/data.csv"};
77
+
left_cam_csv << "#timestamp [ns],filename\r\n";
78
+
79
+
create_directories(path + "/mav0/cam1/data");
80
+
ofstream right_cam_csv{path + "/mav0/cam1/data.csv"};
81
+
right_cam_csv << "#timestamp [ns],filename\r\n";
82
+
}
83
+
84
+
extern "C" void
85
+
euroc_recorder_save_imu(xrt_imu_sink *sink, struct xrt_imu_sample *sample)
86
+
{
87
+
euroc_recorder *er = container_of(sink, euroc_recorder, writer_imu_sink);
88
+
euroc_recorder_try_mkfiles(er);
89
+
90
+
timepoint_ns ts = sample->timestamp_ns;
91
+
xrt_vec3_f64 a = sample->accel_m_s2;
92
+
xrt_vec3_f64 w = sample->gyro_rad_secs;
93
+
94
+
std::ofstream imu_csv{string(er->path) + "/mav0/imu0/data.csv", std::ios::app};
95
+
imu_csv.setf(std::ios::fixed);
96
+
imu_csv << std::setprecision(20);
97
+
imu_csv << ts << ",";
98
+
imu_csv << w.x << "," << w.y << "," << w.z << ",";
99
+
imu_csv << a.x << "," << a.y << "," << a.z << "\r\n";
100
+
}
101
+
102
+
static void
103
+
euroc_recorder_save_frame(euroc_recorder *er, struct xrt_frame *frame, bool is_left)
104
+
{
105
+
euroc_recorder_try_mkfiles(er);
106
+
107
+
string path = string(er->path);
108
+
string cam_name = is_left ? "cam0" : "cam1";
109
+
uint64_t ts = frame->timestamp;
110
+
111
+
ofstream cam_csv{path + "/mav0/" + cam_name + "/data.csv", std::ios::app};
112
+
cam_csv << ts << "," << ts << ".png\r\n";
113
+
114
+
assert(frame->format == XRT_FORMAT_L8 || frame->format == XRT_FORMAT_R8G8B8); // Only formats supported
115
+
auto img_type = frame->format == XRT_FORMAT_L8 ? CV_8UC1 : CV_8UC3;
116
+
string img_path = path + "/mav0/" + cam_name + "/data/" + std::to_string(ts) + ".png";
117
+
cv::Mat img{(int)frame->height, (int)frame->width, img_type, frame->data};
118
+
cv::imwrite(img_path, img);
119
+
}
120
+
121
+
extern "C" void
122
+
euroc_recorder_save_left(struct xrt_frame_sink *sink, struct xrt_frame *frame)
123
+
{
124
+
euroc_recorder *er = container_of(sink, euroc_recorder, writer_left_sink);
125
+
euroc_recorder_save_frame(er, frame, true);
126
+
127
+
// Also, write queued IMU samples to disk now.
128
+
while (!er->imu_queue.empty()) {
129
+
xrt_imu_sample imu = er->imu_queue.front();
130
+
xrt_sink_push_imu(&er->writer_imu_sink, &imu);
131
+
er->imu_queue.pop();
132
+
}
133
+
}
134
+
135
+
extern "C" void
136
+
euroc_recorder_save_right(struct xrt_frame_sink *sink, struct xrt_frame *frame)
137
+
{
138
+
euroc_recorder *er = container_of(sink, euroc_recorder, writer_right_sink);
139
+
euroc_recorder_save_frame(er, frame, false);
140
+
}
141
+
142
+
143
+
/*
144
+
*
145
+
* Cloner sinks functionality
146
+
*
147
+
*/
148
+
149
+
extern "C" void
150
+
euroc_recorder_receive_imu(xrt_imu_sink *sink, struct xrt_imu_sample *sample)
151
+
{
152
+
// Contrary to frame sinks, we don't have separately threaded queues for IMU
153
+
// sinks so we use an std::queue to temporarily store IMU samples, later we
154
+
// write them to disk when writing left frames.
155
+
euroc_recorder *er = container_of(sink, euroc_recorder, cloner_imu_sink);
156
+
er->imu_queue.push(*sample);
157
+
}
158
+
159
+
160
+
static void
161
+
euroc_recorder_receive_frame(euroc_recorder *er, struct xrt_frame *src_frame, bool is_left)
162
+
{
163
+
// Let's clone the frame so that we can release the src_frame quickly
164
+
xrt_frame *copy = nullptr;
165
+
u_frame_clone(src_frame, ©);
166
+
167
+
xrt_sink_push_frame(is_left ? er->writer_queues.left : er->writer_queues.right, copy);
168
+
}
169
+
170
+
extern "C" void
171
+
euroc_recorder_receive_left(struct xrt_frame_sink *sink, struct xrt_frame *frame)
172
+
{
173
+
euroc_recorder *er = container_of(sink, euroc_recorder, cloner_left_sink);
174
+
euroc_recorder_receive_frame(er, frame, true);
175
+
}
176
+
177
+
extern "C" void
178
+
euroc_recorder_receive_right(struct xrt_frame_sink *sink, struct xrt_frame *frame)
179
+
{
180
+
euroc_recorder *er = container_of(sink, euroc_recorder, cloner_right_sink);
181
+
euroc_recorder_receive_frame(er, frame, false);
182
+
}
183
+
184
+
185
+
/*
186
+
*
187
+
* Frame node functionality
188
+
*
189
+
*/
190
+
191
+
extern "C" void
192
+
euroc_recorder_node_break_apart(struct xrt_frame_node *node)
193
+
{}
194
+
195
+
extern "C" void
196
+
euroc_recorder_node_destroy(struct xrt_frame_node *node)
197
+
{
198
+
struct euroc_recorder *er = container_of(node, struct euroc_recorder, node);
199
+
delete er;
200
+
}
201
+
202
+
203
+
/*
204
+
*
205
+
* Exported functions
206
+
*
207
+
*/
208
+
209
+
extern "C" xrt_slam_sinks *
210
+
euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path)
211
+
{
212
+
struct euroc_recorder *er = new euroc_recorder{};
213
+
214
+
struct xrt_frame_node *xfn = &er->node;
215
+
xfn->break_apart = euroc_recorder_node_break_apart;
216
+
xfn->destroy = euroc_recorder_node_destroy;
217
+
xrt_frame_context_add(xfctx, xfn);
218
+
219
+
// Determine dataset path
220
+
if (record_path != nullptr) {
221
+
er->path = record_path;
222
+
} else {
223
+
time_t seconds = os_realtime_get_ns() / U_1_000_000_000;
224
+
constexpr size_t size = sizeof("YYYYMMDDHHmmss");
225
+
char datetime[size] = {0};
226
+
strftime(datetime, size, "%Y%m%d%H%M%S", localtime(&seconds));
227
+
string default_path = string{"euroc_recording_"} + datetime;
228
+
er->path = default_path;
229
+
}
230
+
231
+
// Setup sink pipeline
232
+
233
+
// First, make the public queues that will clone frames in memory so that
234
+
// original frames can be released as soon as possible. Not doing this could
235
+
// result in frame queues from the user being filled up.
236
+
u_sink_queue_create(xfctx, 0, &er->cloner_left_sink, &er->cloner_queues.left);
237
+
u_sink_queue_create(xfctx, 0, &er->cloner_right_sink, &er->cloner_queues.right);
238
+
er->cloner_queues.imu = &er->cloner_imu_sink;
239
+
240
+
// Clone samples into heap and release original samples right after
241
+
er->cloner_imu_sink.push_imu = euroc_recorder_receive_imu;
242
+
er->cloner_left_sink.push_frame = euroc_recorder_receive_left;
243
+
er->cloner_right_sink.push_frame = euroc_recorder_receive_right;
244
+
245
+
// Then, make a queue to save frame sinks to disk in a separate thread
246
+
u_sink_queue_create(xfctx, 0, &er->writer_left_sink, &er->writer_queues.left);
247
+
u_sink_queue_create(xfctx, 0, &er->writer_right_sink, &er->writer_queues.right);
248
+
er->writer_queues.imu = nullptr;
249
+
250
+
// Write cloned samples to disk with these
251
+
er->writer_imu_sink.push_imu = euroc_recorder_save_imu;
252
+
er->writer_left_sink.push_frame = euroc_recorder_save_left;
253
+
er->writer_right_sink.push_frame = euroc_recorder_save_right;
254
+
255
+
xrt_slam_sinks *public_sinks = &er->cloner_queues;
256
+
return public_sinks;
257
+
}
+33
src/xrt/auxiliary/tracking/t_euroc_recorder.h
+33
src/xrt/auxiliary/tracking/t_euroc_recorder.h
···
1
+
// Copyright 2021, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief EuRoC dataset recorder utility.
6
+
* @author Mateo de Mayo <mateo.demayo@collabora.com>
7
+
* @ingroup aux_tracking
8
+
*/
9
+
10
+
#pragma once
11
+
12
+
#include "xrt/xrt_tracking.h"
13
+
#include "xrt/xrt_frame.h"
14
+
15
+
#ifdef __cplusplus
16
+
extern "C" {
17
+
#endif
18
+
19
+
/*!
20
+
* @brief Create SLAM sinks to record samples in EuRoC format.
21
+
*
22
+
* @param xfctx Frame context for the sinks.
23
+
* @param record_path Directory name to save the dataset or NULL for a default based on the current datetime.
24
+
* @return struct xrt_slam_sinks* Sinks to push samples to for recording.
25
+
*
26
+
* @ingroup aux_tracking
27
+
*/
28
+
struct xrt_slam_sinks *
29
+
euroc_recorder_create(struct xrt_frame_context *xfctx, const char *record_path);
30
+
31
+
#ifdef __cplusplus
32
+
}
33
+
#endif