From 0202cb922383e2ac1657a8a34dc6a31b4b807880 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Sat, 23 Oct 2021 22:52:31 -0300 Subject: [PATCH] st/prober: Improve usage of SLAM tracking frameservers Use a similar "hardcoded" idea as in p_factory_ensure_frameserver. This fix usage of SLAM sources in other contexts like calibration, at the cost of requiring a device to call create_tracked_slam at least once. (again, similar to how psmv/psvr/hand tracking work already) --- src/xrt/drivers/euroc/euroc_device.c | 15 +++-- src/xrt/state_trackers/prober/p_prober.c | 35 +++++------ src/xrt/state_trackers/prober/p_tracking.c | 73 +++++++++++++--------- 3 files changed, 67 insertions(+), 56 deletions(-) diff --git a/src/xrt/drivers/euroc/euroc_device.c b/src/xrt/drivers/euroc/euroc_device.c index fb7dc06ac..3a38e48f9 100644 --- a/src/xrt/drivers/euroc/euroc_device.c +++ b/src/xrt/drivers/euroc/euroc_device.c @@ -251,18 +251,17 @@ euroc_device_create(struct xrt_prober *xp) xd->get_view_pose = euroc_get_view_pose; } - int ret = xp->tracking->create_tracked_slam(xp->tracking, xd, &ed->slam); - if (ret < 0) { - EUROC_WARN(ed, - "Unable to create the SLAM tracker so the Euroc device won't be tracked.\n\t" - "Did you provide the appropriate SLAM dependency when compiling?"); - // However, we can continue, maybe the user just wants to play with the euroc utilities - } - u_var_add_root(ed, dev_name, false); u_var_add_pose(ed, &ed->pose, "pose"); u_var_add_pose(ed, &ed->offset, "offset"); u_var_add_pose(ed, &ed->tracking_origin.offset, "tracking offset"); + bool tracked = xp->tracking->create_tracked_slam(xp->tracking, xd, &ed->slam) >= 0; + if (!tracked) { + EUROC_WARN(ed, "Unable to setup the SLAM tracker"); + euroc_device_destroy(xd); + return NULL; + } + return xd; } diff --git a/src/xrt/state_trackers/prober/p_prober.c b/src/xrt/state_trackers/prober/p_prober.c index 1639df0c0..a56eefddf 100644 --- a/src/xrt/state_trackers/prober/p_prober.c +++ b/src/xrt/state_trackers/prober/p_prober.c @@ -977,15 +977,6 @@ p_open_video_device(struct xrt_prober *xp, XRT_MAYBE_UNUSED struct prober_device *pdev = (struct prober_device *)xpdev; -#if defined(XRT_BUILD_DRIVER_EUROC) - // TODO: If both VF_PATH and EUROC_PATH are set, VF will be ignored on calibration - const char *euroc_path = debug_get_option_euroc_path(); - if (euroc_path != NULL) { - *out_xfs = euroc_player_create(xfctx, euroc_path); // Euroc will exit if it can't be created - return 0; - } -#endif - #if defined(XRT_BUILD_DRIVER_VF) const char *path = debug_get_option_vf_path(); if (path != NULL) { @@ -997,6 +988,14 @@ p_open_video_device(struct xrt_prober *xp, } #endif +#if defined(XRT_BUILD_DRIVER_EUROC) + const char *euroc_path = debug_get_option_euroc_path(); + if (euroc_path != NULL) { + *out_xfs = euroc_player_create(xfctx, euroc_path); // Euroc will exit if it can't be created + return 0; + } +#endif + #if defined(XRT_HAVE_V4L2) if (pdev->num_v4ls == 0) { return -1; @@ -1020,17 +1019,17 @@ p_list_video_devices(struct xrt_prober *xp, xrt_prober_list_video_cb cb, void *p { struct prober *p = (struct prober *)xp; - const char *path = debug_get_option_vf_path(); - if (path != NULL) { - cb(xp, NULL, "Video File", "Collabora", path, ptr); + // Video sources from drivers (at most one will be listed) + const char *vf_path = debug_get_option_vf_path(); + const char *euroc_path = debug_get_option_euroc_path(); + + if (vf_path != NULL) { + cb(xp, NULL, "Video File", "Collabora", vf_path, ptr); + } else if (euroc_path != NULL) { + cb(xp, NULL, "Euroc Dataset", "Collabora", euroc_path, ptr); } - path = debug_get_option_euroc_path(); - if (path != NULL) { - cb(xp, NULL, "Euroc Dataset", "Collabora", path, ptr); - } - - // Loop over all devices and find video devices. + // Video sources from video devices for (size_t i = 0; i < p->num_devices; i++) { struct prober_device *pdev = &p->devices[i]; diff --git a/src/xrt/state_trackers/prober/p_tracking.c b/src/xrt/state_trackers/prober/p_tracking.c index 1e7b14873..91a0fb809 100644 --- a/src/xrt/state_trackers/prober/p_tracking.c +++ b/src/xrt/state_trackers/prober/p_tracking.c @@ -234,6 +234,47 @@ p_factory_ensure_frameserver(struct p_factory *fact) // Start the stream now. xrt_fs_stream_start(fact->xfs, xsink, XRT_FS_CAPTURE_TYPE_TRACKING, fact->settings.camera_mode); } + +//! @todo Similar to p_factory_ensure_frameserver but for SLAM sources. +//! Therefore we can only have one SLAM tracker at a time, with exactly one SLAM +//! tracked device. It would be good to solve these artificial restrictions. +static bool +p_factory_ensure_slam_frameserver(struct p_factory *fact) +{ + // Factory frameserver is already in use + if (fact->xfs != NULL) { + return false; + } + + // SLAM tracker with EuRoC frameserver + +#ifdef XRT_BUILD_DRIVER_EUROC + if (debug_get_option_euroc_path() != NULL) { + struct xrt_slam_sinks empty_sinks = {0}; + struct xrt_slam_sinks *sinks = &empty_sinks; + + xrt_prober_open_video_device(&fact->p->base, NULL, &fact->xfctx, &fact->xfs); + assert(fact->xfs->source_id == 0xECD0FEED && "xfs is not Euroc, unsynced open_video_device?"); + +#ifdef XRT_HAVE_SLAM + int ret = t_slam_create(&fact->xfctx, &fact->xts, &sinks); + if (ret != 0) { + U_LOG_W("Unable to initialize SLAM tracking, the Euroc driver will not be tracked"); + } +#else + U_LOG_W("SLAM tracking support is disabled, the Euroc driver will not be tracked"); +#endif + + xrt_fs_slam_stream_start(fact->xfs, sinks); + + return true; + } +#endif + + // No SLAM sources were started + return false; +} + #endif @@ -344,15 +385,7 @@ p_factory_create_tracked_slam(struct xrt_tracking_factory *xfact, struct xrt_tracked_slam *xts = NULL; -#ifdef XRT_BUILD_DRIVER_EUROC - if (debug_get_option_euroc_path() != NULL) { - // The euroc slam tracker was already created on p_tracking_init because the - // euroc player is not a device so it needs to be started from somewhere - goto end; - } -#endif - -end: + p_factory_ensure_slam_frameserver(fact); if (!fact->started_xts) { xts = fact->xts; @@ -372,6 +405,7 @@ end: #endif } + /* * * "Exported" prober functions. @@ -402,27 +436,6 @@ p_tracking_init(struct prober *p) // Finally set us as the tracking factory. p->base.tracking = &fact->base; -#ifdef XRT_BUILD_DRIVER_EUROC - if (debug_get_option_euroc_path() != NULL) { - struct xrt_slam_sinks empty_sinks = {0}; - struct xrt_slam_sinks *sinks = &empty_sinks; - - // fact->xfs *will* be an euroc frame server after open, because of prober open_video_device - xrt_prober_open_video_device(&fact->p->base, NULL, &fact->xfctx, &fact->xfs); - -#ifdef XRT_HAVE_SLAM - int ret = t_slam_create(&fact->xfctx, &fact->xts, &sinks); - if (ret != 0) { - U_LOG_W("Unable to initialize SLAM tracking, the Euroc driver will not be tracked"); - } -#else - U_LOG_W("SLAM tracking support is disabled, the Euroc driver will not be tracked"); -#endif - - xrt_fs_slam_stream_start(fact->xfs, sinks); - } -#endif - return 0; }