From 4cfe58a5e9e66c16dec426b65e518b0fd93b1bc4 Mon Sep 17 00:00:00 2001 From: Mateo de Mayo Date: Tue, 23 Jan 2024 13:21:49 -0300 Subject: [PATCH] t/slam: Add hand_masks sink --- src/xrt/auxiliary/tracking/t_tracker_slam.cpp | 39 +++++++++++++++++-- src/xrt/include/xrt/xrt_tracking.h | 35 +++++++++++++++++ 2 files changed, 71 insertions(+), 3 deletions(-) diff --git a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp index 362e35eac..ee1b6efcc 100644 --- a/src/xrt/auxiliary/tracking/t_tracker_slam.cpp +++ b/src/xrt/auxiliary/tracking/t_tracker_slam.cpp @@ -264,6 +264,8 @@ struct TrackerSlam struct xrt_frame_sink cam_sinks[XRT_TRACKING_MAX_SLAM_CAMS]; //!< Sends camera frames to the SLAM system struct xrt_imu_sink imu_sink = {}; //!< Sends imu samples to the SLAM system struct xrt_pose_sink gt_sink = {}; //!< Register groundtruth trajectory for stats + struct xrt_hand_masks_sink hand_masks_sink = {}; //!< Register latest masks to ignore + bool submit; //!< Whether to submit data pushed to sinks to the SLAM tracker uint32_t cam_count; //!< Number of cameras used for tracking @@ -275,8 +277,10 @@ struct TrackerSlam struct openvr_tracker *ovr_tracker; //!< OpenVR lighthouse tracker // Used mainly for checking that the timestamps come in order - timepoint_ns last_imu_ts; //!< Last received IMU sample timestamp - vector last_cam_ts; //!< Last received image timestamp per cam + timepoint_ns last_imu_ts; //!< Last received IMU sample timestamp + vector last_cam_ts; //!< Last received image timestamp per cam + struct xrt_hand_masks_sample last_hand_masks; //!< Last received hand masks info + Mutex last_hand_masks_mutex; //!< Mutex for @ref last_hand_masks // Prediction @@ -1273,6 +1277,17 @@ t_slam_gt_sink_push(struct xrt_pose_sink *sink, xrt_pose_sample *sample) xrt_sink_push_pose(t.euroc_recorder->gt, sample); } +//! Receive and register masks to use in the next image +extern "C" void +t_slam_hand_mask_sink_push(struct xrt_hand_masks_sink *sink, struct xrt_hand_masks_sample *hand_masks) +{ + XRT_TRACE_MARKER(); + + auto &t = *container_of(sink, TrackerSlam, hand_masks_sink); + unique_lock lock(t.last_hand_masks_mutex); + t.last_hand_masks = *hand_masks; +} + //! Receive and send IMU samples to the external SLAM system extern "C" void t_slam_receive_imu(struct xrt_imu_sink *sink, struct xrt_imu_sample *s) @@ -1365,7 +1380,21 @@ receive_frame(TrackerSlam &t, struct xrt_frame *frame, uint32_t cam_index) default: SLAM_ERROR("Unknown image format"); return; } - // TODO masks + { + unique_lock lock(t.last_hand_masks_mutex); + for (auto &view : t.last_hand_masks.views) { + if (!view.enabled) { + continue; + } + + for (auto &hand : view.hands) { + if (!hand.enabled) { + continue; + } + // TODO@mateosss: add_mask(hand.rect); + } + }; + } { XRT_TRACE_IDENT(slam_push); @@ -1551,6 +1580,9 @@ t_slam_create(struct xrt_frame_context *xfctx, t.gt_sink.push_pose = t_slam_gt_sink_push; t.sinks.gt = &t.gt_sink; + t.hand_masks_sink.push_hand_masks = t_slam_hand_mask_sink_push; + t.sinks.hand_masks = &t.hand_masks_sink; + t.submit = config->submit_from_start; t.cam_count = config->cam_count; @@ -1563,6 +1595,7 @@ t_slam_create(struct xrt_frame_context *xfctx, t.last_imu_ts = INT64_MIN; t.last_cam_ts = vector(t.cam_count, INT64_MIN); + t.last_hand_masks = xrt_hand_masks_sample{}; t.pred_type = config->prediction; diff --git a/src/xrt/include/xrt/xrt_tracking.h b/src/xrt/include/xrt/xrt_tracking.h index 5a1777b21..e7dc38162 100644 --- a/src/xrt/include/xrt/xrt_tracking.h +++ b/src/xrt/include/xrt/xrt_tracking.h @@ -139,6 +139,22 @@ struct xrt_pose_sample struct xrt_pose pose; }; +/*! + * Masks (bounding boxes) of different hands from current views + */ +struct xrt_hand_masks_sample +{ + struct xrt_hand_masks_sample_camera + { + bool enabled; //!< Whether any hand mask for this camera is being reported + struct xrt_hand_masks_sample_hand + { + bool enabled; //!< Whether a mask for this hand is being reported + struct xrt_rect rect; //!< The mask itself in pixel coordinates + } hands[2]; + } views[XRT_TRACKING_MAX_SLAM_CAMS]; +}; + /*! * @interface xrt_imu_sink * @@ -167,6 +183,17 @@ struct xrt_pose_sink void (*push_pose)(struct xrt_pose_sink *, struct xrt_pose_sample *sample); }; +/*! + * @interface xrt_hand_masks_sink + * + * An object to push @ref xrt_hand_masks_sample to. + */ +struct xrt_hand_masks_sink +{ + void (*push_hand_masks)(struct xrt_hand_masks_sink *, struct xrt_hand_masks_sample *hand_masks); +}; + + /*! * Container of pointers to sinks that could be used for a SLAM system. Sinks * are considered disabled if they are null. @@ -177,6 +204,7 @@ struct xrt_slam_sinks struct xrt_frame_sink *cams[XRT_TRACKING_MAX_SLAM_CAMS]; struct xrt_imu_sink *imu; struct xrt_pose_sink *gt; //!< Can receive ground truth poses if available + struct xrt_hand_masks_sink *hand_masks; }; /*! @@ -293,6 +321,13 @@ xrt_sink_push_pose(struct xrt_pose_sink *sink, struct xrt_pose_sample *sample) sink->push_pose(sink, sample); } +//! @public @memberof xrt_hand_masks_sink +static inline void +xrt_sink_push_hand_masks(struct xrt_hand_masks_sink *sink, struct xrt_hand_masks_sample *hand_masks) +{ + sink->push_hand_masks(sink, hand_masks); +} + //! @public @memberof xrt_tracked_psmv static inline void xrt_tracked_psmv_get_tracked_pose(struct xrt_tracked_psmv *psmv,