The open source OpenXR runtime
t/slam: Use euroc recorder in slam tracker
authored by
Mateo de Mayo
and committed by
Jakob Bornecrantz
1cc80f54
91ecbdb7
+24
+24
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
+24
src/xrt/auxiliary/tracking/t_tracker_slam.cpp
···
11
11
#include "xrt/xrt_tracking.h"
12
12
#include "xrt/xrt_frameserver.h"
13
13
#include "util/u_debug.h"
14
+
#include "util/u_var.h"
14
15
#include "os/os_threading.h"
15
16
#include "math/m_space.h"
17
+
#include "tracking/t_euroc_recorder.h"
16
18
17
19
#include <slam_tracker.hpp>
18
20
#include <opencv2/core/mat.hpp>
···
172
174
struct os_thread_helper oth; //!< Thread where the external SLAM system runs
173
175
MatFrame *cv_wrapper; //!< Wraps a xrt_frame in a cv::Mat to send to the SLAM system
174
176
177
+
struct xrt_slam_sinks *euroc_recorder; //!< EuRoC dataset recording sinks
178
+
bool euroc_record; //!< When true, samples will be saved to disk in EuRoC format
179
+
175
180
// Used for checking that the timestamps come in order
176
181
mutable timepoint_ns last_imu_ts = INT64_MIN;
177
182
mutable timepoint_ns last_left_ts = INT64_MIN;
···
197
202
imu_sample sample{ts, a.x, a.y, a.z, w.x, w.y, w.z};
198
203
t.slam->push_imu_sample(sample);
199
204
SLAM_TRACE("imu t=%ld a=[%f,%f,%f] w=[%f,%f,%f]", ts, a.x, a.y, a.z, w.x, w.y, w.z);
205
+
206
+
if (t.euroc_record) {
207
+
xrt_sink_push_imu(t.euroc_recorder->imu, s);
208
+
}
200
209
201
210
// Check monotonically increasing timestamps
202
211
SLAM_DASSERT(ts > t.last_imu_ts, "Sample (%ld) is older than last (%ld)", ts, t.last_imu_ts)
···
252
261
{
253
262
auto &t = *container_of(sink, TrackerSlam, left_sink);
254
263
push_frame(t, frame, true);
264
+
265
+
if (t.euroc_record) {
266
+
xrt_sink_push_frame(t.euroc_recorder->left, frame);
267
+
}
255
268
}
256
269
257
270
extern "C" void
···
259
272
{
260
273
auto &t = *container_of(sink, TrackerSlam, right_sink);
261
274
push_frame(t, frame, false);
275
+
276
+
if (t.euroc_record) {
277
+
xrt_sink_push_frame(t.euroc_recorder->right, frame);
278
+
}
262
279
}
263
280
264
281
extern "C" void
···
278
295
auto &t = *t_ptr; // Needed by SLAM_DEBUG
279
296
SLAM_DEBUG("Destroying SLAM tracker");
280
297
os_thread_helper_destroy(&t_ptr->oth);
298
+
u_var_remove_root(t_ptr);
281
299
delete t_ptr->slam;
282
300
delete t_ptr->cv_wrapper;
283
301
delete t_ptr;
···
341
359
SLAM_ASSERT(ret == 0, "Unable to initialize thread");
342
360
343
361
xrt_frame_context_add(xfctx, &t.node);
362
+
363
+
t.euroc_recorder = euroc_recorder_create(xfctx, NULL);
364
+
365
+
// Setup UI
366
+
u_var_add_root(&t, "SLAM Tracker", true);
367
+
u_var_add_bool(&t, &t.euroc_record, "Record as EuRoC");
344
368
345
369
*out_xts = &t.base;
346
370
*out_sink = &t.sinks;