The open source OpenXR runtime
0
fork

Configure Feed

Select the types of activity you want to include in your feed.

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
··· 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;