From c05c5d5c2e54ecab1690115ee98768aba3373cec Mon Sep 17 00:00:00 2001 From: Moshi Turner Date: Tue, 11 Apr 2023 23:45:02 -0500 Subject: [PATCH] st/gui: Add EuRoC recorder for DepthAI cameras --- src/xrt/state_trackers/gui/CMakeLists.txt | 1 + src/xrt/state_trackers/gui/gui_common.h | 8 ++ .../state_trackers/gui/gui_scene_main_menu.c | 5 + .../gui/gui_scene_record_euroc.c | 105 ++++++++++++++++++ 4 files changed, 119 insertions(+) create mode 100644 src/xrt/state_trackers/gui/gui_scene_record_euroc.c diff --git a/src/xrt/state_trackers/gui/CMakeLists.txt b/src/xrt/state_trackers/gui/CMakeLists.txt index d3cd8e94e..a42048f50 100644 --- a/src/xrt/state_trackers/gui/CMakeLists.txt +++ b/src/xrt/state_trackers/gui/CMakeLists.txt @@ -14,6 +14,7 @@ add_library( gui_scene_calibrate.c gui_scene_debug.c gui_scene_hand_tracking_demo.c + gui_scene_record_euroc.c gui_scene_main_menu.c gui_scene_record.c gui_scene_remote.c diff --git a/src/xrt/state_trackers/gui/gui_common.h b/src/xrt/state_trackers/gui/gui_common.h index 3b62e036a..c87f11789 100644 --- a/src/xrt/state_trackers/gui/gui_common.h +++ b/src/xrt/state_trackers/gui/gui_common.h @@ -220,6 +220,14 @@ gui_scene_debug(struct gui_program *p); void gui_scene_hand_tracking_demo(struct gui_program *p); +/*! + * EuRoC recorder for DepthAI cameras + * + * @ingroup gui + */ +void +gui_scene_record_euroc(struct gui_program *p); + /*! * Create a recording view scene. * diff --git a/src/xrt/state_trackers/gui/gui_scene_main_menu.c b/src/xrt/state_trackers/gui/gui_scene_main_menu.c index f02da336d..10541be5e 100644 --- a/src/xrt/state_trackers/gui/gui_scene_main_menu.c +++ b/src/xrt/state_trackers/gui/gui_scene_main_menu.c @@ -82,6 +82,11 @@ scene_render(struct gui_scene *scene, struct gui_program *p) gui_scene_hand_tracking_demo(p); } + if (igButton("EuRoC recorder (DepthAI Stereo)", button_dims)) { + gui_scene_delete_me(p, scene); + gui_scene_record_euroc(p); + } + igSeparator(); if (igButton("Exit", button_dims)) { diff --git a/src/xrt/state_trackers/gui/gui_scene_record_euroc.c b/src/xrt/state_trackers/gui/gui_scene_record_euroc.c new file mode 100644 index 000000000..035ba528c --- /dev/null +++ b/src/xrt/state_trackers/gui/gui_scene_record_euroc.c @@ -0,0 +1,105 @@ +// Copyright 2019-2023, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief Small hand-tracking demo scene + * @author Jakob Bornecrantz + * @ingroup gui + */ + +#include "xrt/xrt_results.h" +#include "xrt/xrt_system.h" +#include "xrt/xrt_config_have.h" +#include "xrt/xrt_config_drivers.h" +#include "xrt/xrt_config_drivers.h" + + +#include "math/m_api.h" +#include "math/m_space.h" +#include "tracking/t_hand_tracking.h" +#include "tracking/t_tracking.h" +#include "tracking/t_euroc_recorder.h" + +#include "util/u_var.h" +#include "util/u_sink.h" +#include "util/u_debug.h" +#include "util/u_pretty_print.h" +#include "util/u_system_helpers.h" + +#include "gui_common.h" + + +#ifdef XRT_BUILD_DRIVER_DEPTHAI +#include "depthai/depthai_interface.h" +#endif + +#ifdef XRT_BUILD_DRIVER_HANDTRACKING +#include "ht/ht_interface.h" +#endif + + +#if defined(XRT_BUILD_DRIVER_DEPTHAI) && defined(XRT_BUILD_DRIVER_HANDTRACKING) + + +void +gui_scene_record_euroc(struct gui_program *p) +{ + // Dummy so that gui_scene_debug doesn't try to probe. + struct u_system_devices *usysd = u_system_devices_allocate(); + struct xrt_system_devices *xsysd = &usysd->base; + p->xsysd = xsysd; + + struct xrt_frame_context xfctx; + struct depthai_slam_startup_settings settings = {0}; + + settings.frames_per_second = 60; + // This is not what we use for HT/SLAM, but it's better here because it lets us see calibration patterns in more + // detail. + // For now, if you use this you will have to manually multiply all fx, fy, cx, cy by 0.5. No distortion + // values, just camera projection values. + settings.half_size_ov9282 = false; + settings.want_cameras = true; + settings.want_imu = false; + + struct xrt_fs *the_fs = depthai_fs_slam(&xfctx, &settings); + + if (the_fs == NULL) { + xrt_system_devices_destroy(&xsysd); + return; + } + + + struct xrt_slam_sinks *slam_sinks = NULL; + slam_sinks = euroc_recorder_create(&xfctx, NULL, 2, false); + + u_var_add_root(usysd, "DepthAI Euroc recorder", 0); + euroc_recorder_add_ui(slam_sinks, usysd, ""); + + + struct xrt_slam_sinks gen_lock = {0}; + u_sink_force_genlock_create( // + &xfctx, // + slam_sinks->cams[0], // + slam_sinks->cams[1], // + &gen_lock.cams[0], // + &gen_lock.cams[1]); // + + gen_lock.imu = slam_sinks->imu; + + xrt_fs_slam_stream_start(the_fs, &gen_lock); + + gui_scene_debug(p); +} + + +#else /* XRT_BUILD_DRIVER_DEPTHAI */ + + +void +gui_scene_record_euroc(struct gui_program *p) +{ + // No-op +} + + +#endif /* XRT_BUILD_DRIVER_DEPTHAI */