mirrors/monado
1
0
Fork 0
mirror of https://gitlab.freedesktop.org/monado/monado.git synced 2024-12-29 11:06:18 +00:00
Code Issues Actions Packages Projects Releases Wiki Activity

d/vive: Prepare data streaming and SLAM tracker

Browse source
This commit is contained in:
Mateo de Mayo 2022-07-25 12:25:47 -03:00 committed by Moses Turner
parent 3cbb7f6b9f
commit f0ceb40f6a
3 changed files with 135 additions and 11 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

67
src/xrt/drivers/vive/vive_device.c
View file

@ -96,10 +96,10 @@ vive_device_update_inputs(struct xrt_device *xdev)
}
static void
vive_device_get_tracked_pose(struct xrt_device *xdev,
enum xrt_input_name name,
uint64_t at_timestamp_ns,
struct xrt_space_relation *out_relation)
vive_device_get_3dof_tracked_pose(struct xrt_device *xdev,
enum xrt_input_name name,
uint64_t at_timestamp_ns,
struct xrt_space_relation *out_relation)
{
XRT_TRACE_MARKER();
@ -120,6 +120,62 @@ vive_device_get_tracked_pose(struct xrt_device *xdev,
*out_relation = relation;
}
//! Specific pose corrections for Basalt and a Valve Index headset
//! @todo Test and fix for other headsets (vive/vivepro)
XRT_MAYBE_UNUSED static inline struct xrt_pose
vive_device_correct_pose_from_basalt(struct xrt_pose pose)
{
struct xrt_quat q = {-0.70710678, 0, 0, 0.70710678};
math_quat_rotate(&q, &pose.orientation, &pose.orientation);
math_quat_rotate_vec3(&q, &pose.position, &pose.position);
return pose;
}
static void
vive_device_get_slam_tracked_pose(struct xrt_device *xdev,
enum xrt_input_name name,
uint64_t at_timestamp_ns,
struct xrt_space_relation *out_relation)
{
XRT_TRACE_MARKER();
struct vive_device *d = vive_device(xdev);
xrt_tracked_slam_get_tracked_pose(d->tracking.slam, at_timestamp_ns, out_relation);
int pose_bits = XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT;
bool pose_tracked = out_relation->relation_flags & pose_bits;
if (pose_tracked) {
#if defined(XRT_HAVE_BASALT_SLAM)
d->pose = vive_device_correct_pose_from_basalt(out_relation->pose);
#else
d->pose = out_relation->pose;
#endif
}
out_relation->pose = d->pose;
out_relation->relation_flags = (enum xrt_space_relation_flags)(
XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT |
XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT);
}
static void
vive_device_get_tracked_pose(struct xrt_device *xdev,
enum xrt_input_name name,
uint64_t at_timestamp_ns,
struct xrt_space_relation *out_relation)
{
XRT_TRACE_MARKER();
struct vive_device *d = vive_device(xdev);
if (d->tracking.slam_enabled && d->slam_over_3dof) {
vive_device_get_slam_tracked_pose(xdev, name, at_timestamp_ns, out_relation);
} else {
vive_device_get_3dof_tracked_pose(xdev, name, at_timestamp_ns, out_relation);
}
}
static void
vive_device_get_view_poses(struct xrt_device *xdev,
const struct xrt_vec3 *default_eye_relation,
@ -776,6 +832,9 @@ vive_setup_trackers(struct vive_device *d, struct vive_tracking_status status)
return false;
}
// SLAM tracker and hand tracker, if enabled, should've been initialized in
// the lighthouse builder.
return true;
}

6
src/xrt/drivers/vive/vive_device.h
View file

@ -19,6 +19,7 @@
#include "vive/vive_config.h"
#include "vive_lighthouse.h"
#include "xrt/xrt_tracking.h"
#ifdef __cplusplus
extern "C" {
@ -83,6 +84,9 @@ struct vive_device
//! Fields related to camera-based tracking (SLAM and hand tracking)
struct
{
//! SLAM tracker.
struct xrt_tracked_slam *slam;
//! Set at start. Whether the SLAM tracker was initialized.
bool slam_enabled;
@ -93,6 +97,8 @@ struct vive_device
//! Whether to track the HMD with 6dof SLAM or fallback to the 3dof tracker
bool slam_over_3dof;
struct xrt_pose pose;
};

73
src/xrt/targets/common/target_builder_lighthouse.c
View file

@ -9,9 +9,8 @@
*/
#include "tracking/t_hand_tracking.h"
// #include "tracking/t_tracking.h"
// #include "util/u_debug.h"
// #include "v4l2/v4l2_interface.h"
#include "tracking/t_tracking.h"
#include "xrt/xrt_config_drivers.h"
#include "xrt/xrt_device.h"
#include "xrt/xrt_prober.h"
@ -26,6 +25,7 @@
#include "target_builder_interface.h"
#include "vive/vive_config.h"
#include "v4l2/v4l2_interface.h"
#include "xrt/xrt_frameserver.h"
#include "xrt/xrt_results.h"
@ -124,6 +124,19 @@ get_selected_mode(struct xrt_fs *xfs)
return selected_mode;
}
static bool
visual_inertial_stream_start(struct xrt_fs *xfs, struct xrt_slam_sinks *sinks)
{
// Stream frames
struct xrt_frame_sink *sbs_sink = sinks->left;
bool success = xrt_fs_stream_start(xfs, sbs_sink, XRT_FS_CAPTURE_TYPE_TRACKING, get_selected_mode(xfs));
if (!success) {
return false;
}
return true;
}
static void
on_video_device(struct xrt_prober *xp,
struct xrt_prober_device *pdev,
@ -143,6 +156,30 @@ on_video_device(struct xrt_prober *xp,
}
}
static struct xrt_slam_sinks *
lighthouse_slam_track(struct u_system_devices *usysd)
{
struct xrt_slam_sinks *sinks = NULL;
struct vive_device *d = (struct vive_device *)usysd->base.roles.head;
#ifdef XRT_FEATURE_SLAM
int create_status = t_slam_create(&usysd->xfctx, NULL, &d->tracking.slam, &sinks);
if (create_status != 0) {
return NULL;
}
int start_status = t_slam_start(d->tracking.slam);
if (start_status != 0) {
return NULL;
}
LH_INFO("Lighthouse HMD SLAM tracker successfully started");
#endif
return sinks;
}
static bool
lighthouse_hand_track(struct u_system_devices *usysd,
struct xrt_prober *xp,
@ -334,8 +371,13 @@ setup_visual_trackers(struct u_system_devices *usysd,
vive_get_stereo_camera_calibration(hmd_config, &stereo_calib, &head_in_left_cam);
// Initialize SLAM tracker
struct xrt_slam_sinks *slam_sinks = NULL;
if (slam_enabled) {
LH_ASSERT(false, "Not implemented");
slam_sinks = lighthouse_slam_track(usysd);
if (slam_sinks == NULL) {
LH_WARN("Unable to setup the SLAM tracker");
return false;
}
}
// Initialize hand tracker
@ -359,7 +401,24 @@ setup_visual_trackers(struct u_system_devices *usysd,
struct xrt_frame_sink *entry_sbs_sink = NULL;
bool old_rgb_ht = debug_get_bool_option_ht_use_old_rgb();
if (hand_enabled) {
if (slam_enabled && hand_enabled && !old_rgb_ht) {
u_sink_split_create(&usysd->xfctx, slam_sinks->left, hand_sinks->left, &entry_left_sink);
u_sink_split_create(&usysd->xfctx, slam_sinks->right, hand_sinks->right, &entry_right_sink);
u_sink_stereo_sbs_to_slam_sbs_create(&usysd->xfctx, entry_left_sink, entry_right_sink, &entry_sbs_sink);
u_sink_create_format_converter(&usysd->xfctx, XRT_FORMAT_L8, entry_sbs_sink, &entry_sbs_sink);
} else if (slam_enabled && hand_enabled && old_rgb_ht) {
struct xrt_frame_sink *hand_sbs = NULL;
struct xrt_frame_sink *slam_sbs = NULL;
u_sink_stereo_sbs_to_slam_sbs_create(&usysd->xfctx, hand_sinks->left, hand_sinks->right, &hand_sbs);
u_sink_stereo_sbs_to_slam_sbs_create(&usysd->xfctx, slam_sinks->left, slam_sinks->right, &slam_sbs);
u_sink_create_format_converter(&usysd->xfctx, XRT_FORMAT_L8, slam_sbs, &slam_sbs);
u_sink_split_create(&usysd->xfctx, slam_sbs, hand_sbs, &entry_sbs_sink);
} else if (slam_enabled) {
entry_left_sink = slam_sinks->left;
entry_right_sink = slam_sinks->right;
u_sink_stereo_sbs_to_slam_sbs_create(&usysd->xfctx, entry_left_sink, entry_right_sink, &entry_sbs_sink);
u_sink_create_format_converter(&usysd->xfctx, XRT_FORMAT_L8, entry_sbs_sink, &entry_sbs_sink);
} else if (hand_enabled) {
enum xrt_format fmt = old_rgb_ht ? XRT_FORMAT_R8G8B8 : XRT_FORMAT_L8;
entry_left_sink = hand_sinks->left;
entry_right_sink = hand_sinks->right;
@ -375,8 +434,8 @@ setup_visual_trackers(struct u_system_devices *usysd,
struct xrt_slam_sinks entry_sinks = {
.left = entry_sbs_sink,
.right = NULL, // v4l2 streams a single SBS frame so we ignore the right sink
.imu = NULL,
.gt = NULL,
.imu = slam_enabled ? slam_sinks->imu : NULL,
.gt = slam_enabled ? slam_sinks->gt : NULL,
};
*out_sinks = entry_sinks;