diff --git a/src/xrt/drivers/CMakeLists.txt b/src/xrt/drivers/CMakeLists.txt index f51e2809a..d19020fff 100644 --- a/src/xrt/drivers/CMakeLists.txt +++ b/src/xrt/drivers/CMakeLists.txt @@ -350,6 +350,8 @@ if(XRT_BUILD_DRIVER_RIFT_S) rift_s/rift_s_controller.h rift_s/rift_s_position_filter.c rift_s/rift_s_position_filter.h + rift_s/rift_s_motion.cpp + rift_s/rift_s_motion.h rift_s/rift_s_slam_guard.c rift_s/rift_s_slam_guard.h rift_s/rift_s_firmware.c diff --git a/src/xrt/drivers/rift_s/rift_s.c b/src/xrt/drivers/rift_s/rift_s.c index 0e3986fd8..2627391b3 100644 --- a/src/xrt/drivers/rift_s/rift_s.c +++ b/src/xrt/drivers/rift_s/rift_s.c @@ -197,6 +197,12 @@ rift_s_system_create(struct xrt_prober *xp, goto cleanup; } + sys->motion = rift_s_motion_create(); + if (sys->motion == NULL) { + RIFT_S_ERROR("Failed to allocate controller motion cache"); + goto cleanup; + } + if (read_hmd_config(hid_hmd, &sys->hmd_config) < 0) { RIFT_S_ERROR("Failed to read HMD configuration"); goto cleanup; @@ -282,6 +288,11 @@ rift_s_system_free(struct rift_s_system *sys) /* Stop the packet reading thread */ os_thread_helper_destroy(&sys->oth); + /* The world worker reads SLAM and camera buffers owned by the frame context. */ + if (sys->tracker != NULL) { + rift_s_tracker_stop(sys->tracker); + } + /* Stop all the frame processing (has to happen before the cameras * and tracker are destroyed */ xrt_frame_context_destroy_nodes(&sys->xfctx); @@ -310,6 +321,7 @@ rift_s_system_free(struct rift_s_system *sys) os_mutex_destroy(&sys->dev_mutex); + rift_s_motion_destroy(&sys->motion); free(sys); } diff --git a/src/xrt/drivers/rift_s/rift_s.h b/src/xrt/drivers/rift_s/rift_s.h index 2ec8529ab..904961143 100644 --- a/src/xrt/drivers/rift_s/rift_s.h +++ b/src/xrt/drivers/rift_s/rift_s.h @@ -29,6 +29,7 @@ #include "rift_s_protocol.h" #include "rift_s_radio.h" #include "rift_s_tracker.h" +#include "rift_s_motion.h" #ifndef RIFT_S_H #define RIFT_S_H @@ -111,6 +112,8 @@ struct rift_s_system /* Controller devices */ struct rift_s_controller *controllers[MAX_TRACKED_DEVICES]; + //! Shared controller motion cache, fed at IMU capture time. + struct rift_s_motion *motion; /* Video feed handling */ struct xrt_frame_context xfctx; diff --git a/src/xrt/drivers/rift_s/rift_s_controller.c b/src/xrt/drivers/rift_s/rift_s_controller.c index cfb2f885f..e8fc466b2 100644 --- a/src/xrt/drivers/rift_s/rift_s_controller.c +++ b/src/xrt/drivers/rift_s/rift_s_controller.c @@ -188,6 +188,16 @@ attitude_sample(struct rift_s_controller *ctrl, unsigned int index) return &ctrl->attitude_history[(ctrl->attitude_history_start + index) % RIFT_S_ATTITUDE_HISTORY_CAPACITY]; } +static void +suspend_position_tracking(struct rift_s_controller *ctrl) +{ + rift_s_position_filter_suspend(&ctrl->position_filter); + if (ctrl->sys != NULL) { + rift_s_motion_reset_hand(ctrl->sys->motion, + ctrl->base.device_type == XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER ? 1 : 0); + } +} + /* Forget the device-to-host clock mapping and everything captured against it. The next sample * starts a new mapping. Caller holds the mutex. */ static void @@ -202,7 +212,7 @@ reset_clock_mapping(struct rift_s_controller *ctrl) ctrl->clock_stable_samples = 0; ctrl->optical_heading_valid = false; // Retained IMU times no longer match the optical capture times: hold the position. - rift_s_position_filter_freeze(&ctrl->position_filter); + suspend_position_tracking(ctrl); } bool @@ -262,7 +272,7 @@ rift_s_controller_record_attitude(struct rift_s_controller *ctrl, ctrl->attitude_history_count = ctrl->clock_stable_samples = 0; ctrl->optical_heading_valid = false; // Retained IMU times no longer match the optical capture times: hold the position. - rift_s_position_filter_freeze(&ctrl->position_filter); + suspend_position_tracking(ctrl); ctrl->diagnostics.reject_clock++; ctrl->diagnostics.reject_clock_by[RIFT_S_CLOCK_DISCONTINUITY]++; } @@ -651,7 +661,7 @@ reset_fusion_interval(struct rift_s_controller *ctrl, const struct xrt_quat *ori ctrl->gravity_accel_sum = ctrl->gravity_gyro_sum = (struct xrt_vec3){0}; ctrl->gravity_moving = false; ctrl->rest = (struct rift_s_rest_stats){0}; - rift_s_position_filter_freeze(&ctrl->position_filter); + suspend_position_tracking(ctrl); } static bool @@ -813,8 +823,6 @@ static void reset_position_filter(struct rift_s_controller *ctrl) { rift_s_position_filter_reset(&ctrl->position_filter); - ctrl->reacquire_offset = (struct xrt_vec3){0}; - ctrl->reacquire_started_ns = 0; float accel_noise = debug_get_float_option_rift_s_position_accel_noise(); float bias_walk = debug_get_float_option_rift_s_position_bias_walk(); if (accel_noise > 0) { @@ -833,15 +841,13 @@ reset_position_filter(struct rift_s_controller *ctrl) #define RIFT_S_POSITION_RESET_DISTANCE_M 0.15f // Longest render-time extrapolation of aim/grip. #define RIFT_S_MAX_PREDICTION_NS (100 * U_TIME_1MS_IN_NS) -// Resume the optical filter immediately, but remove the held-pose display offset over 100 ms. -#define RIFT_S_REACQUIRE_BLEND_NS (100 * U_TIME_1MS_IN_NS) enum position_tracking { POSITION_NONE, // never fixed: no position estimate POSITION_TRACKED, // fresh optical fix POSITION_COASTING, // IMU-only, within the position duration of the loss ladder - POSITION_LOST, // held at the last estimate, waiting for an optical reset + POSITION_LOST, // no reliable position confidence }; static enum position_tracking @@ -851,17 +857,13 @@ position_tracking_at(const struct rift_s_controller *ctrl, timepoint_ns t, bool if (!ctrl->position_filter.initialized) { return POSITION_NONE; } - if (ctrl->position_filter.frozen) { + if (ctrl->position_filter.suspended) { return POSITION_LOST; } time_duration_ns position_duration, velocity_duration; rift_s_tracking_loss_durations(ctrl->track_vision_updates, ctrl->track_reliable_updates, &position_duration, &velocity_duration); time_duration_ns age = t > ctrl->last_fix_ns ? t - ctrl->last_fix_ns : 0; - if (position_duration > RIFT_S_POSITION_FILTER_BRIDGE_NS) - position_duration = RIFT_S_POSITION_FILTER_BRIDGE_NS; - if (velocity_duration > position_duration) - velocity_duration = position_duration; *velocity_tracked = age <= velocity_duration; if (age > position_duration) { return POSITION_LOST; @@ -869,23 +871,40 @@ position_tracking_at(const struct rift_s_controller *ctrl, timepoint_ns t, bool return age > RIFT_S_COAST_AFTER_NS ? POSITION_COASTING : POSITION_TRACKED; } -// Apply the loss ladder at IMU time: coast, then hold the position once it is lost. -static void -update_position_tracking(struct rift_s_controller *ctrl, timepoint_ns capture_ns) +// Adapt the Monado filter's current uncertainty to the model reliability gate. Normal +// vision delivery latency must not activate a model merely because capture time is old. +void +rift_s_controller_update_motion(struct rift_s_controller *ctrl, timepoint_ns capture_ns) { + if (ctrl->sys == NULL || ctrl->sys->motion == NULL || !ctrl->position_filter.initialized) { + return; + } + struct xrt_device *head = rift_s_system_get_hmd(ctrl->sys); + struct xrt_space_relation head_relation = XRT_SPACE_RELATION_ZERO; + if (head == NULL || + head->get_tracked_pose(head, XRT_INPUT_GENERIC_HEAD_POSE, capture_ns, &head_relation) != XRT_SUCCESS || + (head_relation.relation_flags & + (XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT)) != + (XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_VALID_BIT)) { + return; + } bool velocity_tracked; enum position_tracking state = position_tracking_at(ctrl, capture_ns, &velocity_tracked); - struct rift_s_position_filter *f = &ctrl->position_filter; - f->coasting = state == POSITION_COASTING; - if (state == POSITION_LOST && !f->frozen) { - rift_s_position_filter_freeze(f); - RIFT_S_DEBUG("Controller position hand=%s state=lost age_ms=%.1f vision_updates=%u reliable_updates=%u" - " held=%.3f,%.3f,%.3f", - hand_alias(ctrl), (double)(capture_ns - ctrl->last_fix_ns) / U_TIME_1MS_IN_NS, - ctrl->track_vision_updates, ctrl->track_reliable_updates, f->state.x[0][0], - f->state.x[1][0], f->state.x[2][0]); - ctrl->track_vision_updates = ctrl->track_reliable_updates = 0; + struct rift_s_motion_sample sample = {0}; + sample.timestamp_ns = capture_ns; + sample.pose.orientation = ctrl->pose.orientation; + rift_s_position_filter_predict(&ctrl->position_filter, capture_ns, 0, &sample.pose.position, + &sample.linear_velocity); + sample.linear_acceleration = ctrl->world_accel; + sample.angular_velocity = ctrl->fusion.last.gyro; + for (int axis = 0; axis < 3; ++axis) { + sample.position_variance[axis] = + state != POSITION_LOST && !ctrl->position_filter.suspended && ctrl->track_vision_updates >= 3 + ? ctrl->position_filter.state.P[axis][0][0] + : -1; } + unsigned int hand = ctrl->base.device_type == XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER ? 1 : 0; + rift_s_motion_update(ctrl->sys->motion, hand, &sample, &head_relation.pose); } bool @@ -1018,8 +1037,8 @@ handle_imu_update(struct rift_s_controller *ctrl, timepoint_ns capture_ns = attitude_sample(ctrl, ctrl->attitude_history_count - 1)->capture_ns; ctrl->last_imu_capture_ns = capture_ns; if (ctrl->world_accel_valid) { - update_position_tracking(ctrl, capture_ns); rift_s_position_filter_imu(&ctrl->position_filter, capture_ns, &ctrl->world_accel); + rift_s_controller_update_motion(ctrl, capture_ns); } } if (log_imu) { @@ -1629,22 +1648,8 @@ rift_s_controller_get_fusion_pose(struct rift_s_controller *ctrl, return true; } -// Translation smoothing is only for presentation, never the capture-time association prior. -static bool -apply_reacquire_offset(const struct rift_s_controller *ctrl, timepoint_ns t, struct xrt_vec3 *position) -{ - if (ctrl->reacquire_started_ns == 0 || t >= ctrl->reacquire_started_ns + RIFT_S_REACQUIRE_BLEND_NS) { - return false; - } - double u = - t > ctrl->reacquire_started_ns ? (double)(t - ctrl->reacquire_started_ns) / RIFT_S_REACQUIRE_BLEND_NS : 0; - float weight = (float)(1 - u * u * (3 - 2 * u)); - *position = m_vec3_add(*position, m_vec3_mul_scalar(ctrl->reacquire_offset, weight)); - return true; -} - -// Aim/grip: gyro orientation and the unchanged IMU/optical position filter. After the bounded -// bridge, hold a valid estimated position indefinitely with optical confidence cleared. +// Motion-model estimates are separate from the optical association prior and are applied +// at the IMU origin. The device/aim/grip offsets are applied afterwards. static bool predict_aim_grip(struct rift_s_controller *ctrl, int64_t at_timestamp_ns, struct xrt_space_relation *rel) { @@ -1665,20 +1670,27 @@ predict_aim_grip(struct rift_s_controller *ctrl, int64_t at_timestamp_ns, struct struct xrt_vec3 velocity; rift_s_position_filter_predict(&ctrl->position_filter, at_timestamp_ns, RIFT_S_MAX_PREDICTION_NS, &rel->pose.position, &velocity); - rel->relation_flags |= XRT_SPACE_RELATION_POSITION_VALID_BIT; if (state != POSITION_LOST) { - rel->relation_flags |= XRT_SPACE_RELATION_POSITION_TRACKED_BIT; + rel->relation_flags |= + XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT; } if (state != POSITION_LOST && velocity_tracked) { rel->linear_velocity = velocity; rel->relation_flags |= XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT; } - if (apply_reacquire_offset(ctrl, at_timestamp_ns, &rel->pose.position)) { - // SteamVR must not extrapolate the filter velocity across this presentation correction. - rel->linear_velocity = (struct xrt_vec3){0}; - rel->relation_flags &= ~XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT; + source = state == POSITION_TRACKED ? "optical" : state == POSITION_COASTING ? "inertial" : "unreliable"; + if (ctrl->sys != NULL && ctrl->sys->motion != NULL) { + struct xrt_space_relation estimate = XRT_SPACE_RELATION_ZERO; + unsigned int hand = ctrl->base.device_type == XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER ? 1 : 0; + enum rift_s_motion_model model = + rift_s_motion_get(ctrl->sys->motion, hand, at_timestamp_ns, &estimate); + if (model != RIFT_S_MOTION_NONE) { + *rel = estimate; + static const char *const names[] = {"none", "at_rest", "near_head", + "hand_over_hand", "behind_back", "out_of_fov"}; + source = names[model]; + } } - source = state == POSITION_TRACKED ? "optical" : state == POSITION_COASTING ? "coast" : "held"; } if (source != ctrl->position_source) { bool fallback = state == POSITION_NONE; @@ -1751,16 +1763,23 @@ rift_s_controller_get_tracked_pose(struct xrt_device *xdev, } else { have_position = predict_aim_grip(ctrl, at_timestamp_ns, rel); } - bool position_valid = name == XRT_INPUT_GENERIC_TRACKER_POSE || - (rel->relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0; + const enum xrt_space_relation_flags position_flags = + rel->relation_flags & (XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT); + // Transform estimated coordinates through the lever arms without promoting confidence. + if (name != XRT_INPUT_GENERIC_TRACKER_POSE && have_position) { + rel->relation_flags |= XRT_SPACE_RELATION_POSITION_VALID_BIT; + } os_mutex_unlock(&ctrl->mutex); m_relation_chain_resolve(&xrc, out_relation); - if (!have_position || !position_valid) { - out_relation->relation_flags &= - ~(XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT | - XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT); + if (name != XRT_INPUT_GENERIC_TRACKER_POSE) { + out_relation->relation_flags = + (out_relation->relation_flags & + ~(XRT_SPACE_RELATION_POSITION_VALID_BIT | XRT_SPACE_RELATION_POSITION_TRACKED_BIT)) | + position_flags; + if (!have_position) + out_relation->relation_flags &= ~XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT; } return XRT_SUCCESS; @@ -1934,17 +1953,14 @@ fuse_optical_position(struct rift_s_controller *ctrl, timepoint_ns processing_ns) { struct xrt_vec3 variance = optical_position_variance(obs->position_covariance); - bool was_held = ctrl->position_filter.initialized && ctrl->position_filter.frozen; - struct xrt_vec3 held_position = {0}, velocity; - if (was_held) { - rift_s_position_filter_predict(&ctrl->position_filter, processing_ns, RIFT_S_MAX_PREDICTION_NS, - &held_position, &velocity); - apply_reacquire_offset(ctrl, processing_ns, &held_position); - } - bool was_lost = !ctrl->position_filter.initialized || ctrl->position_filter.frozen; + bool velocity_tracked; + bool age_lost = ctrl->position_filter.initialized && !ctrl->position_filter.suspended && + position_tracking_at(ctrl, obs->capture_ns, &velocity_tracked) == POSITION_LOST; + bool was_lost = age_lost || !ctrl->position_filter.initialized || ctrl->position_filter.suspended; enum rift_s_position_fix_result result = - rift_s_position_filter_fix(&ctrl->position_filter, obs->capture_ns, &obs->pose.position, &variance, - RIFT_S_POSITION_RESET_DISTANCE_M); + (age_lost ? rift_s_position_filter_reacquire : rift_s_position_filter_fix)( + &ctrl->position_filter, obs->capture_ns, &obs->pose.position, &variance, + RIFT_S_POSITION_RESET_DISTANCE_M); if (result == RIFT_S_POSITION_FIX_STALE) { return result; } @@ -1963,13 +1979,6 @@ fuse_optical_position(struct rift_s_controller *ctrl, return result; } if (result == RIFT_S_POSITION_FIX_RESET) { - if (was_held) { - struct xrt_vec3 resumed_position; - rift_s_position_filter_predict(&ctrl->position_filter, processing_ns, RIFT_S_MAX_PREDICTION_NS, - &resumed_position, &velocity); - ctrl->reacquire_offset = m_vec3_sub(held_position, resumed_position); - ctrl->reacquire_started_ns = processing_ns; - } // A new track: the loss ladder counts from here. ctrl->track_vision_updates = ctrl->track_reliable_updates = 0; RIFT_S_DEBUG("Controller position hand=%s state=reset reason=%s capture_ns=%" PRIi64 @@ -2016,7 +2025,7 @@ apply_optical_tilt(struct rift_s_controller *ctrl, timepoint_ns capture_ns, cons ctrl->last_optical_tilt_ns = capture_ns; } -void +bool rift_s_controller_push_pose_observation(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs) { struct rift_s_controller *ctrl = (struct rift_s_controller *)(xdev); @@ -2039,7 +2048,7 @@ rift_s_controller_push_pose_observation(struct xrt_device *xdev, const struct t_ if (reject != NULL) { log_fusion(ctrl, frame_mono_ns, processing_ns, reject, obs); os_mutex_unlock(&ctrl->mutex); - return; + return false; } // Candidates must pass position confirmation before changing the IMU heading or // the association prior. Otherwise a rejected fit can steer the next search. @@ -2047,7 +2056,7 @@ rift_s_controller_push_pose_observation(struct xrt_device *xdev, const struct t_ if (position_result == RIFT_S_POSITION_FIX_PENDING || position_result == RIFT_S_POSITION_FIX_STALE) { log_fusion(ctrl, frame_mono_ns, processing_ns, "reject_position", obs); os_mutex_unlock(&ctrl->mutex); - return; + return false; } struct xrt_quat optical = pose->orientation; math_quat_normalize(&optical); @@ -2091,6 +2100,7 @@ rift_s_controller_push_pose_observation(struct xrt_device *xdev, const struct t_ ctrl->diagnostics.optical_accepted++; log_fusion(ctrl, frame_mono_ns, processing_ns, "accepted", obs); os_mutex_unlock(&ctrl->mutex); + return true; } void @@ -2303,6 +2313,8 @@ rift_s_controller_update_configuration(struct rift_s_controller *ctrl, uint64_t ctrl->irled_requested = false; reset_position_filter(ctrl); ctrl->track_vision_updates = ctrl->track_reliable_updates = 0; + rift_s_motion_reset_hand(ctrl->sys->motion, + ctrl->base.device_type == XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER ? 1 : 0); ctrl->last_fix_ns = 0; ctrl->imu_descriptor[0] = '\0'; ctrl->gravity_initialized = false; diff --git a/src/xrt/drivers/rift_s/rift_s_controller.h b/src/xrt/drivers/rift_s/rift_s_controller.h index 68874d47d..f1149bbec 100644 --- a/src/xrt/drivers/rift_s/rift_s_controller.h +++ b/src/xrt/drivers/rift_s/rift_s_controller.h @@ -197,9 +197,6 @@ struct rift_s_controller float last_position_covariance[9]; //! Aim/grip position: IMU acceleration integrated at IMU rate, corrected by optical fixes. struct rift_s_position_filter position_filter; - //! Presentation-only translation correction after held position reacquires optics. - struct xrt_vec3 reacquire_offset; - timepoint_ns reacquire_started_ns; //! Successful and reliable optical fixes in the current track (inputs of the loss ladder). uint32_t track_vision_updates, track_reliable_updates; //! Capture time of the newest optical fix given to position_filter. @@ -405,9 +402,14 @@ rift_s_controller_push_observed_pose(struct xrt_device *xdev, timepoint_ns frame bool rift_s_controller_get_led_model(struct xrt_device *xdev, struct t_constellation_led_model *led_model); // Accept an optical fit. Yaw is corrected only when the fit itself observed it. -void +bool rift_s_controller_push_pose_observation(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs); +// Feed motion models from the current capture-time filter state. Caller holds the controller mutex +// and keeps the system devices alive, as the radio dispatch does. +void +rift_s_controller_update_motion(struct rift_s_controller *ctrl, timepoint_ns capture_ns); + // Controller pose callback. Optical loss preserves IMU orientation and estimates hand position. xrt_result_t rift_s_controller_get_tracked_pose(struct xrt_device *xdev, diff --git a/src/xrt/drivers/rift_s/rift_s_motion.cpp b/src/xrt/drivers/rift_s/rift_s_motion.cpp new file mode 100644 index 000000000..7a47dfaa3 --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_motion.cpp @@ -0,0 +1,737 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "rift_s_motion.h" + +#include +#include +#include +#include +#include +#include + +namespace { +struct Vec +{ + double x = 0, y = 0, z = 0; + Vec + operator+(Vec b) const + { + return {x + b.x, y + b.y, z + b.z}; + } + Vec + operator-(Vec b) const + { + return {x - b.x, y - b.y, z - b.z}; + } + Vec + operator*(double s) const + { + return {x * s, y * s, z * s}; + } + double + length() const + { + return std::sqrt(x * x + y * y + z * z); + } + bool + inside(double t) const + { + return -t < x && x < t && -t < y && y < t && -t < z && z < t; + } +}; +static Vec +cross(Vec a, Vec b) +{ + return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x}; +} +static double +dot(Vec a, Vec b) +{ + return a.x * b.x + a.y * b.y + a.z * b.z; +} +static Vec +components(Vec a, Vec b) +{ + return {a.x * b.x, a.y * b.y, a.z * b.z}; +} + +struct Quat +{ + double x = 0, y = 0, z = 0, w = 1; + Quat + normalized() const + { + double n = std::sqrt(x * x + y * y + z * z + w * w); + return {x / n, y / n, z / n, w / n}; + } + Quat + inverse() const + { + return Quat{-x, -y, -z, w}.normalized(); + } + Quat + operator*(Quat b) const + { + return Quat{w * b.x + x * b.w + y * b.z - z * b.y, w * b.y - x * b.z + y * b.w + z * b.x, + w * b.z + x * b.y - y * b.x + z * b.w, w * b.w - x * b.x - y * b.y - z * b.z} + .normalized(); + } + Vec + rotate(Vec v) const + { + Vec q{x, y, z}, t = cross(q, v) * 2; + return v + t * w + cross(q, t); + } +}; +static Quat +exp_rotation(Vec v) +{ + double angle = v.length(); + double scale = angle > 1e-12 ? std::sin(angle / 2) / angle : 0.5; + return Quat{v.x * scale, v.y * scale, v.z * scale, std::cos(angle / 2)}.normalized(); +} +static Vec +log_rotation(Quat q) +{ + q = q.normalized(); + double n = Vec{q.x, q.y, q.z}.length(); + if (n * n < 1e-20) + return Vec{q.x, q.y, q.z} * (2 / q.w); + double angle = std::abs(q.w) < 1e-10 ? (q.w > 0 ? M_PI : -M_PI) : 2 * std::atan(n / q.w); + return Vec{q.x, q.y, q.z} * (angle / n); +} +struct Pose +{ + Quat q; + Vec p; + Vec + apply(Vec v) const + { + return p + q.rotate(v); + } + Pose + inverse() const + { + Quat qi = q.inverse(); + return {qi, qi.rotate(p * -1)}; + } + Pose + operator*(Pose b) const + { + return {q * b.q, apply(b.p)}; + } +}; +static Pose +pose_from(const xrt_pose &p) +{ + return {{p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w}, + {p.position.x, p.position.y, p.position.z}}; +} +static xrt_pose +pose_to(Pose p) +{ + return {{float(p.q.x), float(p.q.y), float(p.q.z), float(p.q.w)}, {float(p.p.x), float(p.p.y), float(p.p.z)}}; +} +static Vec +vec_from(xrt_vec3 v) +{ + return {v.x, v.y, v.z}; +} +static xrt_vec3 +vec_to(Vec v) +{ + return {float(v.x), float(v.y), float(v.z)}; +} + +struct Sample +{ + double t = -1; + Pose pose; + Vec v, a, w; + std::array variance = {-1, -1, -1}; +}; +static Sample +sample_from(const rift_s_motion_sample &s) +{ + return {double(s.timestamp_ns) / U_TIME_1S_IN_NS, + pose_from(s.pose), + vec_from(s.linear_velocity), + vec_from(s.linear_acceleration), + vec_from(s.angular_velocity), + {s.position_variance[0], s.position_variance[1], s.position_variance[2]}}; +} +static Pose +predict(const Sample &s, double t) +{ + // Model prediction uses constant rates; acceleration is not a quadratic position term. + double dt = t - s.t; + return {s.pose.q * exp_rotation(s.w * dt), s.pose.p + s.v * dt}; +} +struct Record +{ + Sample sample, behavior; + double reliable_t = -1; + Vec reliable_position, relative_position; + Quat current_orientation; + int model = 0; + void + store(Sample s, const Pose &head) + { + sample = s; + current_orientation = s.pose.q; + if (std::all_of(s.variance.begin(), s.variance.end(), [](double v) { return 0 < v && v < 0.001; })) { + reliable_t = s.t; + reliable_position = s.pose.p; + relative_position = head.inverse().apply(s.pose.p); + } + } + bool + fresh(double epsilon) const + { + return sample.t <= reliable_t + epsilon; + } + Pose + source() const + { + return behavior.t == -1 ? Pose{current_orientation, reliable_position} : predict(behavior, sample.t); + } +}; + +struct PairSample +{ + double t; + Vec a, b; +}; +struct PairWindow +{ + std::deque items; + bool ready = false; + size_t allocated = 0; + void + push(PairSample s, double seconds, size_t capacity) + { + if (allocated == 0) + allocated = capacity; + if (items.size() == allocated) { + if (items.front().t + seconds < items.back().t + 0.0005) { + items.pop_front(); + ready = ready && items.size() >= 2; + } else + allocated += capacity; + } + items.push_back(s); + while (!items.empty() && !(s.t < items.front().t + seconds + 0.0005)) { + items.pop_front(); + ready = items.size() >= 2; + } + } + std::array + stats() const + { + Vec sa, sb, saa, sbb, sab; + for (auto &s : items) { + sa = sa + s.a; + sb = sb + s.b; + saa = saa + components(s.a, s.a); + sbb = sbb + components(s.b, s.b); + sab = sab + components(s.a, s.b); + } + double n = double(items.size()); + if (n < 2) + return {}; + return {sa * (1 / n), sb * (1 / n), (saa - components(sa, sa) * (1 / n)) * (1 / (n - 1)), + (sbb - components(sb, sb) * (1 / n)) * (1 / (n - 1)), + (sab - components(sa, sb) * (1 / n)) * (1 / (n - 1))}; + } + void + reset() + { + items.clear(); + ready = false; + } +}; + +struct Rest +{ + int state = 0; + PairWindow history; + Vec mean_a, mean_b; + Pose latch; + bool + update(const Record &r, Sample &out) + { + history.push({r.sample.t, r.sample.w, r.sample.a}, 0.1, 300); + double factor = state == 3 ? 3 : 1; + auto s = history.stats(); + bool variance_ok = history.ready && s[2].inside(factor * 0.000076) && s[3].inside(factor * 0.005); + if (variance_ok) { + mean_a = s[0]; + mean_b = s[1]; + } + bool mean_ok = + (r.sample.w - mean_a).inside(factor * 0.005) && (r.sample.a - mean_b).inside(factor * 0.065); + bool fresh = r.fresh(0.01), far = r.relative_position.length() > 0.8; + int next = state; + switch (state) { + case 0: + if (variance_ok) + next = 1; + break; + case 1: + if (fresh && far) + next = 2; + break; + case 2: + if (mean_ok) + next = 3; + else if (fresh && !far) + next = 1; + break; + case 3: + if (!mean_ok) + next = 0; + break; + } + if (state != 3 && next == 3) + latch = r.sample.pose; + state = next; + if (state != 3) + return false; + out = {}; + out.t = r.sample.t; + out.pose = latch; + return true; + } + void + reset() + { + state = 0; + history.reset(); + mean_a = {}; + mean_b = {}; + } +}; + +struct RotationModel +{ + int state = 0; + Sample output; + Pose correction; + Vec anchor; + double cap = 0.25; + std::deque ratios; + bool + update(const Record &r, const Pose &head, const Pose &torso, bool torso_valid, bool near, Sample &out) + { + bool fresh = r.fresh(0.0003); + double radius = r.relative_position.length(), height = r.reliable_position.y - head.p.y; + bool in_range = near ? radius < 0.25 : 0.25 < radius && radius < 1.1 && height > -0.6; + Vec delta = r.reliable_position - output.pose.p; + double distance = delta.length(), previous_t = output.t; + // Compute the return rate from the previous cache before replacing that cache. + Vec return_rate = r.sample.v; + if (distance > 0) + return_rate = return_rate + delta * (std::min(std::max(distance * 50, 0.25), cap) / distance); + int next = state; + switch (state) { + case 0: + if (!torso_valid) + break; + next = 1; + [[fallthrough]]; + case 1: + if (in_range && (near || fresh)) + next = fresh ? 2 : 3; + break; + case 2: + if (!fresh) + next = 3; + else if (!in_range) + next = 0; + break; + case 3: + if (fresh) + next = 4; + break; + case 4: + if (distance <= 0.001) + next = 0; + break; + } + if (next == 3 && state != 3) { + double seed = 0.2; + if (!near) { + if (height < -0.2) + seed = 0.5; + else if (!ratios.empty()) { + seed = 0; + for (double v : ratios) + seed += v; + seed /= ratios.size(); + } + } + correction = {exp_rotation({-0.087, 0, 0}), {0, 0.025, near ? 0.2 : -seed}}; + Pose source = r.source(); + anchor = torso.inverse().apply(source.p + source.q.rotate(correction.inverse().p)); + cap = 0.25; + } + state = next; + if (!near && state == 2) { + double w = r.sample.w.length(); + double ratio = w > 0 ? std::clamp(r.sample.v.length() / w, 0.2, 0.4) : 0.2; + if (ratios.size() == 500) + ratios.pop_front(); + ratios.push_back(ratio); + } + if (state == 3) { + output = r.sample; + output.variance = {-1, -1, -1}; + output.pose.p = torso.p + (r.sample.pose.q * correction.inverse().q).rotate(correction.p) + + torso.q.rotate(anchor); + cap = std::max(cap, 0.2 * r.sample.a.length()); + } else if (state == 4) { + Vec previous = output.pose.p; + output = r.sample; + output.variance = {-1, -1, -1}; + output.v = return_rate; + output.pose.p = previous + return_rate * (r.sample.t - previous_t); + } + if (state != 3 && state != 4) + return false; + out = output; + return true; + } + void + reset() + { + state = 0; + } +}; + +struct Behind +{ + int state = 0; + bool + update(const Record &r, const Pose &head, Sample &out) + { + bool fresh = r.fresh(0.0003); + Vec look = head.q.rotate({0, 0, 1}), delta = r.sample.pose.p - head.p; + look.y = delta.y = 0; + if (state == 0 && !fresh) { + bool behind = dot(look, delta) > 0 && r.reliable_position.y - head.p.y < -0.3 && + (r.reliable_position - head.p).length() < 1.2; + state = behind ? 1 : 2; + } else if (state != 0 && fresh) + state = 0; + if (state != 1) + return false; + out = r.sample; + out.pose.p = head.apply(r.relative_position); + return true; + } + void + reset() + { + state = 0; + } +}; + +struct HandOverHand +{ + int state = 0; + bool latch = false; + PairWindow history; + Sample output; + Vec other_support, torso_support; + bool + update(const Record &p, const Record &q, const Pose &head, const Pose &torso, Sample &out) + { + history.push({p.sample.t, p.sample.a, q.sample.a}, 0.1, 100); + auto s = history.stats(); + bool all_above = false, all_below = false; + if (history.items.size() >= 2 && s[2].x > 0.004 && s[2].y > 0.004 && s[2].z > 0.004 && + s[3].x >= 0.004 && s[3].y >= 0.004 && s[3].z >= 0.004) { + Vec corr{s[4].x / std::sqrt(s[2].x * s[3].x), s[4].y / std::sqrt(s[2].y * s[3].y), + s[4].z / std::sqrt(s[2].z * s[3].z)}; + all_above = corr.x > 0.25 && corr.y > 0.25 && corr.z > 0.25; + all_below = corr.x < 0.25 && corr.y < 0.25 && corr.z < 0.25; + } + bool lost = !p.fresh(0.05), other_fresh = q.fresh(0.05); + bool close = (q.sample.pose.inverse() * p.sample.pose).p.length() < 0.2; + Vec look = head.q.rotate({0, 0, 1}); + Vec dp = p.sample.pose.p - head.p, dq = q.sample.pose.p - head.p; + dp.y = dq.y = 0; + bool both_gate = dot(look, dp) > 0 && dot(look, dq) > 0; + bool mismatch = p.model != 0 && p.model != 3, other_model = q.model != 0; + if (!other_fresh) + latch = false; + int next = state; + switch (state) { + case 0: + if (!other_fresh) + break; + next = 1; + [[fallthrough]]; + case 1: + latch = true; + if (!other_fresh) + next = 0; + else if (!lost && close && !both_gate) + next = 2; + break; + case 2: + if (!other_fresh) + next = 0; + else if (lost) + next = all_above ? 3 : 4; + else if (!close) + next = 1; + break; + case 3: + if (mismatch || other_model) + next = 0; + else if (!lost || (other_fresh && all_below)) + next = 4; + break; + case 4: + if (mismatch || other_model || !lost || !other_fresh) + next = 0; + else if (latch && all_above) + next = 3; + break; + } + if (state != next && (next == 3 || next == 4)) { + Pose basis = next == 3 ? q.sample.pose : torso; + Vec support = basis.inverse().apply(state == 3 || state == 4 ? output.pose.p : p.sample.pose.p); + if (next == 3) + other_support = support; + else + torso_support = support; + } + state = next; + if (state != 3 && state != 4) + return false; + output = p.sample; + output.pose.p = state == 3 ? q.sample.pose.apply(other_support) : torso.apply(torso_support); + out = output; + return true; + } + void + reset() + { + state = 0; + history.reset(); + } +}; + +static bool +project_yaw(Quat q, Quat &yaw) +{ + Vec x = q.rotate({1, 0, 0}); + x.y = 0; + if (x.length() < 0.1) + return false; + yaw = exp_rotation({0, std::atan2(-x.z, x.x), 0}); + return true; +} + +struct Torso +{ + bool valid = false; + Pose output; + Quat previous_controller, previous_head; + PairWindow history; + void + update(const Sample &s, const Pose &head) + { + Vec position = head.apply({0, -0.075, 0.0805}); + Quat controller_yaw, head_yaw; + if (!project_yaw(head.q, head_yaw) || !project_yaw(s.pose.q, controller_yaw)) + return; + history.push({s.t, + {log_rotation(controller_yaw).y, log_rotation(head_yaw).y, + log_rotation(head_yaw.inverse() * controller_yaw).y}, + {}}, + 0.1, 100); + if (!history.ready) { + if (!valid) { + output = {head_yaw, position}; + valid = true; + } + return; + } + Vec variance = history.stats()[2]; + Quat result = output.q; + if (variance.z >= 0.001) { + if (std::abs(log_rotation(head_yaw.inverse() * result).y) >= 0.523 && variance.x >= 0.0027) { + double weight = std::abs(variance.y) / (std::abs(variance.x) + std::abs(variance.y)); + Quat target = controller_yaw * + exp_rotation(log_rotation(controller_yaw.inverse() * head_yaw) * weight); + Vec residual = log_rotation(result.inverse() * target); + result = result * exp_rotation({0, (residual.y > 0 ? 1 : -1) * 0.018 * 0.523, 0}); + } + } else { + result = result * (std::abs(variance.x) <= std::abs(variance.y) + ? previous_controller.inverse() * controller_yaw + : previous_head.inverse() * head_yaw); + } + // The output is itself projected, just like the source yaw filters. + Quat projected; + if (project_yaw(result, projected)) { + output = {projected, position}; + valid = true; + } + previous_controller = controller_yaw; + previous_head = head_yaw; + } +}; + +struct Hand +{ + Record record; + Torso torso; + Rest rest; + RotationModel near, rotation; + HandOverHand together; + Behind behind; + void + update(const Record &other, const Pose &head, const Pose &body, bool body_valid) + { + if (record.reliable_t == -1) + return; + Sample estimate; + int model = 0; + if (rest.update(record, estimate)) + model = 1; + else if (near.update(record, head, body, body_valid, true, estimate)) + model = 2; + else if (other.reliable_t != -1 && together.update(record, other, head, body, estimate)) + model = 3; + else if (behind.update(record, head, estimate)) + model = 4; + else if (rotation.update(record, head, body, body_valid, false, estimate)) + model = 5; + // Lower priorities must acquire again after another model has owned the output. + if (model != 0) { + if (model < 2) + near.reset(); + if (model < 3) + together.reset(); + if (model < 4) + behind.reset(); + if (model < 5) + rotation.reset(); + } + record.model = model; + record.behavior = model ? estimate : Sample{}; + } +}; + +static bool +finite(Vec v) +{ + return std::isfinite(v.x) && std::isfinite(v.y) && std::isfinite(v.z); +} +static bool +finite(Pose p) +{ + double n = p.q.x * p.q.x + p.q.y * p.q.y + p.q.z * p.q.z + p.q.w * p.q.w; + return finite(p.p) && std::isfinite(n) && n > 1e-12; +} +} // namespace + +struct rift_s_motion +{ + std::mutex mutex; + std::array hands; + Pose body; + bool body_valid = false; +}; + +extern "C" struct rift_s_motion * +rift_s_motion_create(void) +{ + return new (std::nothrow) rift_s_motion; +} + +extern "C" void +rift_s_motion_destroy(struct rift_s_motion **motion) +{ + if (motion == nullptr) + return; + delete *motion; + *motion = nullptr; +} + +extern "C" void +rift_s_motion_reset_hand(struct rift_s_motion *motion, unsigned int hand) +{ + if (motion == nullptr || hand >= 2) + return; + std::lock_guard lock(motion->mutex); + motion->hands[hand] = Hand{}; + motion->body_valid = motion->hands[1 - hand].torso.valid; + if (motion->body_valid) + motion->body = motion->hands[1 - hand].torso.output; +} + +extern "C" void +rift_s_motion_update(struct rift_s_motion *motion, + unsigned int hand, + const struct rift_s_motion_sample *sample, + const struct xrt_pose *reference) +{ + if (motion == nullptr || hand >= 2 || sample == nullptr || reference == nullptr || sample->timestamp_ns <= 0) + return; + Sample s = sample_from(*sample); + Pose head = pose_from(*reference); + if (!finite(s.pose) || !finite(head) || !finite(s.v) || !finite(s.a) || !finite(s.w)) + return; + s.pose.q = s.pose.q.normalized(); + head.q = head.q.normalized(); + std::lock_guard lock(motion->mutex); + Hand &p = motion->hands[hand], &q = motion->hands[1 - hand]; + if (s.t <= p.record.sample.t) + return; + p.record.store(s, head); + p.torso.update(s, head); + if (p.torso.valid) { + motion->body = p.torso.output; + motion->body_valid = true; + if (q.torso.valid) { + motion->body.q = motion->body.q * + exp_rotation(log_rotation(motion->body.q.inverse() * q.torso.output.q) * 0.5); + } + } + p.update(q.record, head, motion->body, motion->body_valid); +} + +extern "C" enum rift_s_motion_model +rift_s_motion_get(struct rift_s_motion *motion, + unsigned int hand, + timepoint_ns timestamp_ns, + struct xrt_space_relation *out_relation) +{ + if (motion == nullptr || hand >= 2 || out_relation == nullptr) + return RIFT_S_MOTION_NONE; + std::lock_guard lock(motion->mutex); + const Record &r = motion->hands[hand].record; + if (r.model == 0) + return RIFT_S_MOTION_NONE; + const Sample &s = r.behavior; + double time = std::clamp(double(timestamp_ns) / U_TIME_1S_IN_NS, s.t, s.t + 1.0); + *out_relation = {}; + out_relation->pose = pose_to(predict(s, time)); + out_relation->linear_velocity = vec_to(s.v); + out_relation->angular_velocity = vec_to(s.pose.q.rotate(s.w)); + out_relation->relation_flags = static_cast( + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT | + XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT | XRT_SPACE_RELATION_ANGULAR_VELOCITY_VALID_BIT); + // Attachment models pass the input matrix through. Rest and rotation models emit -I. + if (std::all_of(s.variance.begin(), s.variance.end(), [](double v) { return v > 0; })) { + out_relation->relation_flags = static_cast( + out_relation->relation_flags | XRT_SPACE_RELATION_POSITION_VALID_BIT | + XRT_SPACE_RELATION_POSITION_TRACKED_BIT); + } + return static_cast(r.model); +} diff --git a/src/xrt/drivers/rift_s/rift_s_motion.h b/src/xrt/drivers/rift_s/rift_s_motion.h new file mode 100644 index 000000000..6ea8dc782 --- /dev/null +++ b/src/xrt/drivers/rift_s/rift_s_motion.h @@ -0,0 +1,66 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once + +#include "xrt/xrt_defines.h" +#include "util/u_time.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct rift_s_motion; + +// Samples are at the controller IMU origin, before the device/aim/grip lever arms. +struct rift_s_motion_sample +{ + timepoint_ns timestamp_ns; + struct xrt_pose pose; + struct xrt_vec3 linear_velocity; + struct xrt_vec3 linear_acceleration; + // Angular rate in the controller frame. Relations returned by get use tracking-frame rates. + struct xrt_vec3 angular_velocity; + // A negative diagonal marks a position estimate without a reliable filter position. + double position_variance[3]; +}; + +enum rift_s_motion_model +{ + RIFT_S_MOTION_NONE, + RIFT_S_MOTION_AT_REST, + RIFT_S_MOTION_NEAR_HEAD, + RIFT_S_MOTION_HAND_OVER_HAND, + RIFT_S_MOTION_BEHIND_BACK, + RIFT_S_MOTION_OUT_OF_FOV, +}; + +struct rift_s_motion * +rift_s_motion_create(void); + +void +rift_s_motion_destroy(struct rift_s_motion **motion); + +void +rift_s_motion_reset_hand(struct rift_s_motion *motion, unsigned int hand); + +// hand: 0 left, 1 right. The shared cache retains the other hand's most recent sample. +// The reference pose must share the controller's tracking frame. A missing reference leaves +// the previous cache intact; it cannot initialize a new hand model. +void +rift_s_motion_update(struct rift_s_motion *motion, + unsigned int hand, + const struct rift_s_motion_sample *sample, + const struct xrt_pose *reference); + +// Query is read-only: neither render prediction nor lever-arm transforms enter the model cache. +// Rest/rotation estimates have no position confidence; attachment models retain the input confidence. +// The caller keeps its optical result whenever no model is active. +enum rift_s_motion_model +rift_s_motion_get(struct rift_s_motion *motion, + unsigned int hand, + timepoint_ns timestamp_ns, + struct xrt_space_relation *out_relation); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/rift_s/rift_s_position_filter.c b/src/xrt/drivers/rift_s/rift_s_position_filter.c index 4cbce421f..c882cf796 100644 --- a/src/xrt/drivers/rift_s/rift_s_position_filter.c +++ b/src/xrt/drivers/rift_s/rift_s_position_filter.c @@ -15,8 +15,6 @@ #include #include -//! Velocity decay time constant while coasting without optical fixes. -#define COAST_VELOCITY_TAU_S 0.5 #define INITIAL_VELOCITY_VARIANCE (0.5 * 0.5) #define INITIAL_BIAS_VARIANCE (0.5 * 0.5) //! Longest single integration step; larger gaps are dropped radio packets. @@ -34,7 +32,6 @@ propagate(const struct rift_s_position_filter *f, const struct xrt_vec3 *accel, timepoint_ns t) { - const bool coasting = f->coasting; double dt = (double)(t - s->t) / U_TIME_1S_IN_NS; s->t = t; if (dt <= 0) { @@ -43,21 +40,13 @@ propagate(const struct rift_s_position_filter *f, if (dt > MAX_STEP_S) { dt = MAX_STEP_S; } - if (coasting && f->track_updates < RIFT_S_POSITION_FILTER_MATURE_UPDATES) { - // A young track's velocity comes from a handful of fixes, possibly a wrong one: hold. - for (int axis = 0; axis < 3; axis++) { - s->x[axis][1] = 0; - } - return; - } const double a[3] = {accel->x, accel->y, accel->z}; - const double d = coasting ? exp(-dt / COAST_VELOCITY_TAU_S) : 1.0; - const double F[3][3] = {{1, dt, -0.5 * dt * dt}, {0, d, -d * dt}, {0, 0, 1}}; + const double F[3][3] = {{1, dt, -0.5 * dt * dt}, {0, 1, -dt}, {0, 0, 1}}; for (int axis = 0; axis < 3; axis++) { double *x = s->x[axis]; double u = a[axis] - x[2]; x[0] += x[1] * dt + 0.5 * u * dt * dt; - x[1] = d * (x[1] + u * dt); + x[1] += u * dt; double FP[3][3] = {{0}}; double (*P)[3] = s->P[axis]; @@ -133,28 +122,12 @@ rift_s_position_filter_imu(struct rift_s_position_filter *f, timepoint_ns t, con f->last_accel = *accel_world; return; } - if (f->frozen) { + if (f->suspended) { f->state.t = t; } else { propagate(f, &f->state, accel_world, t); } f->last_accel = *accel_world; - if (f->initialized && t - f->last_fix_ns > 120 * U_TIME_1MS_IN_NS) { - // No measurement can correct acceleration/tilt error while occluded. Bound the - // displacement from the last optical fix, including before coasting was noticed. - double delta[3] = {f->state.x[0][0] - f->last_fix_position.x, f->state.x[1][0] - f->last_fix_position.y, - f->state.x[2][0] - f->last_fix_position.z}; - double distance = sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); - if (distance > 0.1) { - const double base[3] = {f->last_fix_position.x, f->last_fix_position.y, f->last_fix_position.z}; - for (int axis = 0; axis < 3; ++axis) { - f->state.x[axis][0] = base[axis] + delta[axis] * 0.1 / distance; - f->state.x[axis][1] = 0; - } - } - if (t - f->last_fix_ns > RIFT_S_POSITION_FILTER_BRIDGE_NS) - rift_s_position_filter_freeze(f); - } if (f->history_count == RIFT_S_POSITION_FILTER_HISTORY) { f->history_start = (f->history_start + 1) % RIFT_S_POSITION_FILTER_HISTORY; f->history_count--; @@ -197,13 +170,17 @@ confirm_candidate(struct rift_s_position_filter *f, return f->candidate_count >= RIFT_S_POSITION_FILTER_CONFIRM_FIXES; } -enum rift_s_position_fix_result -rift_s_position_filter_fix(struct rift_s_position_filter *f, +static enum rift_s_position_fix_result +position_filter_fix(struct rift_s_position_filter *f, timepoint_ns capture, const struct xrt_vec3 *position, const struct xrt_vec3 *variance, - float reset_distance_m) + float reset_distance_m, + bool confirmed_reacquire) { + // Optical age loss has already passed Constellation's track confirmation. + // A clock discontinuity still needs independent filter confirmation. + confirmed_reacquire = confirmed_reacquire && !f->suspended; // First retained input after the capture time. unsigned int next = 0; while (next < f->history_count && input_at(f, next)->t <= capture) { @@ -211,7 +188,7 @@ rift_s_position_filter_fix(struct rift_s_position_filter *f, } // A fix needs the state at or before its capture time; the oldest retained input only has // the state after it. - bool reset = !f->initialized || f->frozen; + bool reset = !f->initialized || f->suspended || confirmed_reacquire; bool stale = f->history_count != 0 ? next == 0 : capture < f->state.t; if (f->initialized && stale) { f->stale++; @@ -235,7 +212,8 @@ rift_s_position_filter_fix(struct rift_s_position_filter *f, f->innovation_count++; } } - if (reset && f->initialized && !confirm_candidate(f, capture, position, reset_distance_m)) { + if (reset && f->initialized && !confirmed_reacquire && + !confirm_candidate(f, capture, position, reset_distance_m)) { f->rejected++; return RIFT_S_POSITION_FIX_PENDING; } @@ -252,20 +230,39 @@ rift_s_position_filter_fix(struct rift_s_position_filter *f, f->initialized = true; f->last_fix_ns = capture; f->last_fix_position = *position; - f->frozen = false; - f->coasting = false; + f->suspended = false; // Inputs before the capture keep their states; later ones are replayed from the fused state. replay(f, s, next); return reset ? RIFT_S_POSITION_FIX_RESET : RIFT_S_POSITION_FIX_UPDATED; } -void -rift_s_position_filter_freeze(struct rift_s_position_filter *f) +enum rift_s_position_fix_result +rift_s_position_filter_fix(struct rift_s_position_filter *f, + timepoint_ns capture, + const struct xrt_vec3 *position, + const struct xrt_vec3 *variance, + float reset_distance_m) { - if (!f->initialized || f->frozen) { + return position_filter_fix(f, capture, position, variance, reset_distance_m, false); +} + +enum rift_s_position_fix_result +rift_s_position_filter_reacquire(struct rift_s_position_filter *f, + timepoint_ns capture, + const struct xrt_vec3 *position, + const struct xrt_vec3 *variance, + float reset_distance_m) +{ + return position_filter_fix(f, capture, position, variance, reset_distance_m, true); +} + +void +rift_s_position_filter_suspend(struct rift_s_position_filter *f) +{ + if (!f->initialized || f->suspended) { return; } - f->frozen = true; + f->suspended = true; for (int axis = 0; axis < 3; axis++) { f->state.x[axis][1] = 0; } @@ -279,7 +276,7 @@ rift_s_position_filter_predict(const struct rift_s_position_filter *f, struct xrt_vec3 *out_velocity) { time_duration_ns ahead = t - f->state.t; - if (ahead < 0 || f->frozen || (f->initialized && t - f->last_fix_ns > RIFT_S_POSITION_FILTER_BRIDGE_NS)) { + if (ahead < 0 || f->suspended) { ahead = 0; } else if (ahead > max_horizon_ns) { ahead = max_horizon_ns; @@ -292,21 +289,6 @@ rift_s_position_filter_predict(const struct rift_s_position_filter *f, out_position->x = (float)(s->x[0][0] + s->x[0][1] * dt); out_position->y = (float)(s->x[1][0] + s->x[1][1] * dt); out_position->z = (float)(s->x[2][0] + s->x[2][1] * dt); - if (f->initialized && t - f->last_fix_ns > 120 * U_TIME_1MS_IN_NS) { - // Queries can run beyond the last IMU sample, including during a radio gap. - double delta[3] = {out_position->x - f->last_fix_position.x, out_position->y - f->last_fix_position.y, - out_position->z - f->last_fix_position.z}; - double distance = sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); - if (distance > 0.1) { - out_position->x = (float)(f->last_fix_position.x + delta[0] * 0.1 / distance); - out_position->y = (float)(f->last_fix_position.y + delta[1] * 0.1 / distance); - out_position->z = (float)(f->last_fix_position.z + delta[2] * 0.1 / distance); - *out_velocity = (struct xrt_vec3){0}; - } - if (t - f->last_fix_ns > RIFT_S_POSITION_FILTER_BRIDGE_NS) { - *out_velocity = (struct xrt_vec3){0}; - } - } } void diff --git a/src/xrt/drivers/rift_s/rift_s_position_filter.h b/src/xrt/drivers/rift_s/rift_s_position_filter.h index eb956d641..36ab47f0a 100644 --- a/src/xrt/drivers/rift_s/rift_s_position_filter.h +++ b/src/xrt/drivers/rift_s/rift_s_position_filter.h @@ -9,6 +9,7 @@ #include "xrt/xrt_defines.h" #include "util/u_time.h" +#include "tracking/t_constellation_motion.h" #ifdef __cplusplus extern "C" { @@ -16,7 +17,7 @@ extern "C" { /*! * Defaults chosen on the synthetic cases in tests_rift_s_position_filter (noise at rest, delayed - * fixes during a hand swing, slowly varying acceleration error); no Meta values are used. + * fixes during a hand swing, slowly varying acceleration error); no Meta values were recovered. * Continuous white acceleration noise density, (m/s^2)^2 per Hz, and bias random walk, * (m/s^2)^2 per second. */ @@ -25,25 +26,12 @@ extern "C" { /*! * Mutually consistent fixes needed before a fix that disagrees with the estimate (a gated - * innovation, or any fix while the position is held after loss) replaces it. A single wrong + * innovation, or any fix after an IMU clock discontinuity) replaces it. A single wrong * constellation fit, e.g. a partial ring seen behind the back, must not move the hand. */ #define RIFT_S_POSITION_FILTER_CONFIRM_FIXES 3 //! Longest gap between consecutive candidate fixes of one confirmation. -#define RIFT_S_POSITION_FILTER_CONFIRM_GAP_NS (250 * U_TIME_1MS_IN_NS) -/*! - * Fixes before a track's velocity is trusted for coasting; younger tracks hold their position - * when fixes stop. Matches the "more than 10 updates" step of the loss ladder. - */ -#define RIFT_S_POSITION_FILTER_MATURE_UPDATES 11 - -/*! - * Longest optical gap the IMU bridges before the position is frozen and reported lost: the - * 0.5 s step of the loss ladder. Fixes arrive every 33 ms, so in view only real occlusion - * reaches it; displacement while bridging stays bounded to 10 cm from the last fix. - */ -#define RIFT_S_POSITION_FILTER_BRIDGE_NS (500 * U_TIME_1MS_IN_NS) - +#define RIFT_S_POSITION_FILTER_CONFIRM_GAP_NS T_CONSTELLATION_TRACK_MAX_GAP_NS //! IMU inputs retained for replaying after a delayed optical fix: 1 s at 500 Hz. #define RIFT_S_POSITION_FILTER_HISTORY 512 @@ -68,10 +56,8 @@ struct rift_s_position_filter_input struct rift_s_position_filter { bool initialized; - //! Velocity damping and no bias update: no optical fix for longer than the coast threshold. - bool coasting; - //! Position frozen and velocity zero: tracking was lost; waits for an optical reset. - bool frozen; + //! IMU clock or device discontinuity: stop propagation until a confirmed optical reset. + bool suspended; //! Process noise; set by reset(), may be overridden afterwards. double accel_noise_density, bias_random_walk; struct rift_s_position_filter_state state; @@ -114,7 +100,7 @@ rift_s_position_filter_imu(struct rift_s_position_filter *f, timepoint_ns t, con /*! * Fuse an optical position at its capture time. @p variance is the per-axis measurement variance * in m^2. The first fix initializes the filter. An innovation beyond @p reset_distance_m, or any fix - * while frozen, is a candidate: it re-initializes position and velocity (the Windows "6DoF reset") + * while suspended, is a candidate: it re-initializes position and velocity (the Windows "6DoF reset") * only once RIFT_S_POSITION_FILTER_CONFIRM_FIXES consecutive candidates agree within * @p reset_distance_m of each other; until then the estimate is kept and PENDING returned. */ @@ -125,9 +111,18 @@ rift_s_position_filter_fix(struct rift_s_position_filter *f, const struct xrt_vec3 *variance, float reset_distance_m); -//! Stop integrating: hold the current position with zero velocity until the next reset. +//! Seed a Constellation-confirmed track after optical age loss, retaining IMU history and noise. +//! Stale samples and clock/device discontinuities still use the normal rejection/confirmation gates. +enum rift_s_position_fix_result +rift_s_position_filter_reacquire(struct rift_s_position_filter *f, + timepoint_ns capture, + const struct xrt_vec3 *position, + const struct xrt_vec3 *variance, + float reset_distance_m); + +//! Suspend integration after an IMU clock/device discontinuity, until the next optical reset. void -rift_s_position_filter_freeze(struct rift_s_position_filter *f); +rift_s_position_filter_suspend(struct rift_s_position_filter *f); /*! * Position and velocity at @p t. Times after the last IMU sample extrapolate with constant @@ -141,7 +136,7 @@ rift_s_position_filter_predict(const struct rift_s_position_filter *f, struct xrt_vec3 *out_velocity); /*! - * Windows controller loss ladder (its default configuration): how long position and velocity + * Controller loss duration configuration: how long position and velocity * stay tracked after the last optical update, by the number of successful and reliable optical * updates in the current track. */ diff --git a/src/xrt/drivers/rift_s/rift_s_slam_guard.c b/src/xrt/drivers/rift_s/rift_s_slam_guard.c index 65009b9c7..02f20897c 100644 --- a/src/xrt/drivers/rift_s/rift_s_slam_guard.c +++ b/src/xrt/drivers/rift_s/rift_s_slam_guard.c @@ -6,7 +6,7 @@ * * Validate source samples, separately from historical and predicted queries. A lost pose holds * position and follows IMU attitude. Ordinary loss retains the map alignment; a backend reset - * invalidates it. A saved world requires visual relocalization, never a held-head realignment. + * invalidates it. Temporary tracking does not confirm a saved boundary's world frame. * @ingroup drv_rift_s */ #include "rift_s_slam_guard.h" @@ -47,13 +47,21 @@ rift_s_slam_guard_pose_usable(const struct xrt_pose *p, const struct xrt_quat *f void rift_s_slam_guard_set_world(struct rift_s_slam_guard *g, const struct xrt_pose *world_from_slam) { + bool confirmed = g->boundary_confirmed; g->boundary_confirmed = true; - g->world_transition = g->world_valid && g->have_good && g->state == RIFT_S_SLAM_GUARD_TRACKING; + g->world_transition = confirmed && g->world_valid && g->have_good && + g->state == RIFT_S_SLAM_GUARD_TRACKING; if (g->world_transition) { g->transition_from = g->align; g->transition_to = *world_from_slam; g->transition_start_ns = g->good_ns; } else { + if (g->world_valid && g->have_good) { + struct xrt_pose inverse, raw_good; + math_pose_invert(&g->align, &inverse); + math_pose_transform(&inverse, &g->good, &raw_good); + math_pose_transform(world_from_slam, &raw_good, &g->good); + } g->align = *world_from_slam; } g->world_valid = true; @@ -100,7 +108,6 @@ rift_s_slam_guard_query(const struct rift_s_slam_guard *g, const struct xrt_pose struct xrt_pose *out) { if (g->state != RIFT_S_SLAM_GUARD_TRACKING || !g->world_valid || - (g->require_relocalization && !g->boundary_confirmed) || !rift_s_slam_guard_pose_usable(raw, fusion)) return false; struct xrt_pose aligned; apply_align(&g->align, raw, &aligned); @@ -293,14 +300,6 @@ rift_s_slam_guard_update(struct rift_s_slam_guard *g, struct xrt_pose *out_pose, enum rift_s_slam_guard_reason *out_reason) { - if (g->require_relocalization && !g->boundary_confirmed && g->world_valid) { - // A failed/cancelled setup can restore saved bounds while the fresh frame is still - // active. Revoke that frame before either source or predicted poses are published. - g->world_valid = false; - g->world_transition = false; - if (g->state == RIFT_S_SLAM_GUARD_TRACKING) - enter_diverged(g, ts_ns, fusion_orientation, false, 0, 0); - } bool usable = slam_valid && rift_s_slam_guard_pose_usable(slam_pose, fusion_orientation); if (usable) { g->invalid_since_ns = 0; @@ -387,14 +386,8 @@ rift_s_slam_guard_update(struct rift_s_slam_guard *g, } if (!g->world_valid) { - if (g->require_relocalization && !g->boundary_confirmed) { - // SteamVR's saved standing origin belongs to the persisted map, not this VIO - // run's arbitrary yaw/translation. Only a visual match can join those frames. - *out_reason = RIFT_S_SLAM_GUARD_RELOCALIZING; - *out_pose = fallback; - return false; - } - // An explicitly new room can start a fresh frame before it has saved landmarks. + // Track in a session frame while matching the saved room. SteamVR must keep + // its saved bounds in a separate universe until boundary_confirmed is true. struct xrt_quat inv_raw, delta; math_quat_invert(&slam_pose->orientation, &inv_raw); math_quat_rotate(&fallback.orientation, &inv_raw, &delta); diff --git a/src/xrt/drivers/rift_s/rift_s_slam_guard.h b/src/xrt/drivers/rift_s/rift_s_slam_guard.h index c9c9a222f..a303c54d7 100644 --- a/src/xrt/drivers/rift_s/rift_s_slam_guard.h +++ b/src/xrt/drivers/rift_s/rift_s_slam_guard.h @@ -57,7 +57,7 @@ struct rift_s_slam_guard bool world_transition; struct xrt_pose transition_from, transition_to; int64_t transition_start_ns; - //! A saved standing frame requires visual confirmation before publishing position tracking. + //! Saved bounds require visual confirmation; session position tracking remains available. bool require_relocalization; enum rift_s_slam_guard_state state; diff --git a/src/xrt/drivers/rift_s/rift_s_tracker.c b/src/xrt/drivers/rift_s/rift_s_tracker.c index 194fabd40..672f73dc5 100644 --- a/src/xrt/drivers/rift_s/rift_s_tracker.c +++ b/src/xrt/drivers/rift_s/rift_s_tracker.c @@ -603,12 +603,27 @@ rift_s_tracker_create(struct xrt_tracking_origin *origin, } void -rift_s_tracker_destroy(struct rift_s_tracker *t) +rift_s_tracker_stop(struct rift_s_tracker *t) { + // Drain an in-flight camera submission and prevent subsequent callbacks from queuing work. + os_mutex_lock(&t->slam_mutex); + os_mutex_lock(&t->mutex); + t->ready_for_data = false; + os_mutex_unlock(&t->mutex); + os_mutex_unlock(&t->slam_mutex); + if (t->world_thread_initialized) - os_thread_helper_destroy(&t->world_thread); + os_thread_helper_stop_and_wait(&t->world_thread); for (int eye = 0; eye < RIFT_S_CAMERA_COUNT; eye++) xrt_frame_reference(&t->world_frames[eye], NULL); +} + +void +rift_s_tracker_destroy(struct rift_s_tracker *t) +{ + rift_s_tracker_stop(t); + if (t->world_thread_initialized) + os_thread_helper_destroy(&t->world_thread); rift_s_world_anchor_destroy(t->world_anchor); rift_s_passthrough_destroy(t->base.camera); t_stereo_camera_calibration_reference(&t->stereo_calib, NULL); @@ -1207,11 +1222,21 @@ world_write_status(struct rift_s_tracker *t) : (t->slam_guard.boundary_confirmed && !t->slam_guard.world_transition) ? "localized" : t->world_map_available ? "relocalizing" : "unanchored"; + struct rift_s_world_diagnostics diagnostics = t->world_diagnostics; + bool standby = t->standby, paused = t->slam_paused, restarting = t->slam_restart_pending; + uint64_t generation = t->slam_generation; + uint32_t confirmations = t->world_confirmations; os_mutex_unlock(&t->mutex); static timepoint_ns last_write_ns; static const char *last_state; timepoint_ns now_ns = os_monotonic_get_ns(); if (state == last_state && now_ns - last_write_ns < 500 * U_TIME_1MS_IN_NS) return; + if (state != last_state && strcmp(state, "relocalizing") == 0) { + RIFT_S_WARN("Guardian not found: saved room is not localized; boundary unavailable. " + "Uncover headset cameras and look around. If the room stays unavailable, run Room Setup."); + } else if (state != last_state && strcmp(state, "unanchored") == 0) { + RIFT_S_WARN("Guardian not found: no usable saved room map. Run Room Setup to anchor the boundary."); + } last_write_ns = now_ns; last_state = state; char path[1024], temp[1050]; @@ -1219,7 +1244,21 @@ world_write_status(struct rift_s_tracker *t) snprintf(temp, sizeof(temp), "%s.tmp", path); FILE *file = fopen(temp, "w"); if (!file) return; - fprintf(file, "%lld %s\n", (long long)os_monotonic_get_ns(), state); + // The first two fields remain the guard's status protocol. Diagnostics are + // available without a debug launch or stopping tracking with a debugger. + const struct rift_s_world_anchor_result *result = &diagnostics.result; + fprintf(file, "%lld %s attempts=%" PRIu64 " usable=%" PRIu64 + " capture_ns=%lld sample_ns=%lld pose_valid=%d flags=%u standby=%d paused=%d restart=%d" + " generation=%" PRIu64 " confirmations=%u keyframes=%u" + " features=%u/%u/%u/%u/%u matches=%u/%u/%u/%u/%u inliers=%u/%u/%u/%u/%u" + " matched=%d rejection=%s\n", + (long long)os_monotonic_get_ns(), state, diagnostics.attempts, diagnostics.usable, + (long long)diagnostics.capture_ns, (long long)diagnostics.sample_ns, diagnostics.pose_valid, + diagnostics.pose_flags, standby, paused, restarting, generation, confirmations, result->keyframes, + result->features[0], result->features[1], result->features[2], result->features[3], result->features[4], + result->matches[0], result->matches[1], result->matches[2], result->matches[3], result->matches[4], + result->inliers[0], result->inliers[1], result->inliers[2], result->inliers[3], result->inliers[4], + result->relocalized, result->rejection ? result->rejection : "none"); if (fclose(file) == 0) rename(temp, path); } @@ -1352,6 +1391,11 @@ rift_s_world_thread(void *ptr) valid ? &capture.pose.orientation : NULL, &result); attempts++; usable += valid; + os_mutex_lock(&t->mutex); + t->world_diagnostics = (struct rift_s_world_diagnostics){ + .attempts = attempts, .usable = usable, .capture_ns = capture_ns, .sample_ns = source_ns, + .pose_valid = valid, .pose_flags = capture.relation_flags, .result = result}; + os_mutex_unlock(&t->mutex); if (capture_ns - last_attempt_log_ns >= U_TIME_1S_IN_NS) { last_attempt_log_ns = capture_ns; RIFT_S_INFO("World attempt=%" PRIu64 " usable=%" PRIu64 diff --git a/src/xrt/drivers/rift_s/rift_s_tracker.h b/src/xrt/drivers/rift_s/rift_s_tracker.h index af52811a1..6f54ca28e 100644 --- a/src/xrt/drivers/rift_s/rift_s_tracker.h +++ b/src/xrt/drivers/rift_s/rift_s_tracker.h @@ -35,6 +35,15 @@ struct rift_s_hmd_config; +struct rift_s_world_diagnostics +{ + uint64_t attempts, usable; + timepoint_ns capture_ns, sample_ns; + bool pose_valid; + uint32_t pose_flags; + struct rift_s_world_anchor_result result; +}; + enum rift_s_tracker_pose { RIFT_S_TRACKER_POSE_IMU, @@ -65,6 +74,7 @@ struct rift_s_tracker struct xrt_pose world_candidate; uint64_t world_candidate_generation; timepoint_ns world_candidate_ns; + struct rift_s_world_diagnostics world_diagnostics; //! Camera arrival watchdog uses the host clock, not predicted pose time. timepoint_ns last_camera_arrival_ns; @@ -181,6 +191,9 @@ rift_s_tracker_create(struct xrt_tracking_origin *origin, struct rift_s_hmd_config *hmd_config); void rift_s_tracker_start(struct rift_s_tracker *t); +//! Join consumers and release queued camera frames before destroying the frame context. +void +rift_s_tracker_stop(struct rift_s_tracker *t); void rift_s_tracker_set_standby(struct rift_s_tracker *t, bool standby); void diff --git a/src/xrt/drivers/rift_s/rift_s_world_anchor.cpp b/src/xrt/drivers/rift_s/rift_s_world_anchor.cpp index a444c9abc..9b1b767f4 100644 --- a/src/xrt/drivers/rift_s/rift_s_world_anchor.cpp +++ b/src/xrt/drivers/rift_s/rift_s_world_anchor.cpp @@ -183,6 +183,29 @@ spread(const std::vector &pixels, const rift_s_camera_calibration & return occupied >= 4 && maxx - minx > 0.25 * c.roi.extent.w && maxy - miny > 0.2 * c.roi.extent.h; } +// OpenCV seeds PnP in the perspective plane. A native pixel has a different +// scale there at each fisheye bearing. Only the final native-pixel score decides support. +double +perspective_pixel_scale(const rift_s_camera_calibration &c, cv::Vec3d ray) +{ + double radius = std::hypot(ray[0], ray[1]), theta = std::atan2(radius, ray[2]); + cv::Vec2d angular = radius > 1e-12 ? cv::Vec2d{ray[0], ray[1]} * (theta / radius) + : cv::Vec2d{0, 0}; + constexpr double eps = 1e-6; + auto dx = (distort(c, angular + cv::Vec2d{eps, 0}) - distort(c, angular - cv::Vec2d{eps, 0})) / (2 * eps); + auto dy = (distort(c, angular + cv::Vec2d{0, eps}) - distort(c, angular - cv::Vec2d{0, eps})) / (2 * eps); + cv::Matx22d distortion{dx[0], dy[0], dx[1], dy[1]}; + double gain = theta > 1e-12 ? std::tan(theta) / theta : 1; + double radial = theta > 1e-8 ? (1 / (ray[2] * ray[2]) - gain) / (theta * theta) : 2.0 / 3; + cv::Matx22d perspective = gain * cv::Matx22d::eye() + radial * cv::Matx22d{ + angular[0] * angular[0], angular[0] * angular[1], angular[0] * angular[1], angular[1] * angular[1]}; + cv::Matx22d jacobian = perspective * distortion.inv() * cv::Matx22d{1 / c.projection.fx, 0, 0, 1 / c.projection.fy}; + auto squared = jacobian.t() * jacobian; + double eigenvalue = 0.5 * (squared(0, 0) + squared(1, 1) + + std::hypot(squared(0, 0) - squared(1, 1), 2 * squared(0, 1))); + return std::sqrt(eigenvalue); +} + // PnP supplies the robust seed. All subsequent fitting is on the four parameters // allowed by gravity, using the calibrated fisheye residual rather than perspective pixels. bool @@ -628,7 +651,7 @@ struct rift_s_world_anchor cv::Mat reference_images[RIFT_S_WORLD_MAX_CAMERAS]; Features reference_features[RIFT_S_WORLD_MAX_CAMERAS]; std::string record_dir; - unsigned recorded_pairs = 0; + unsigned recorded_pairs = 0, record_limit = 600; cv::Ptr orb = cv::ORB::create(1400, 1.2f, 8, 19, 0, 2, cv::ORB::HARRIS_SCORE, 31, 12); }; @@ -702,6 +725,7 @@ rift_s_world_anchor_solve_rig_gravity(const rift_s_world_anchor_config *config, const auto &c = config->camera[eye]; std::vector world; std::vector normalized, pixels; + double threshold = 0; for (uint32_t i = 0; i < counts[eye]; ++i) { const auto &o = obs[eye][i]; cv::Vec3d ray; @@ -712,10 +736,10 @@ rift_s_world_anchor_solve_rig_gravity(const rift_s_world_anchor_config *config, world.emplace_back(o.world_point.x, o.world_point.y, o.world_point.z); normalized.emplace_back(ray[0] / ray[2], ray[1] / ray[2]); pixels.emplace_back(o.pixel.x, o.pixel.y); + threshold = std::max(threshold, MAX_ERROR_PX * perspective_pixel_scale(c, ray)); } cv::Vec3d rv, tv; cv::Mat inliers; - double threshold = MAX_ERROR_PX / std::max(c.projection.fx, c.projection.fy); result->rejection = "pnp"; bool solved = cv::solvePnPRansac(world, normalized, cv::Matx33d::eye(), cv::noArray(), rv, tv, @@ -950,9 +974,15 @@ rift_s_world_anchor_create(const rift_s_world_anchor_config *config, const char a->config = *config; resolve_boundary_path(a->config); a->path = path ? path : ""; - const char *record = std::getenv("RIFT_S_WORLD_RECORD_DIR"), - *bench = std::getenv("MONADO_STEAMVR_BENCH"); - if (record && record[0] && bench && (std::strcmp(bench, "1") == 0 || std::strcmp(bench, "true") == 0)) { + const char *record = std::getenv("RIFT_S_WORLD_RECORD_DIR"); + if (record && record[0]) { + // Diagnostics must not enable synthetic poses or override headset proximity. + if (const char *limit = std::getenv("RIFT_S_WORLD_RECORD_LIMIT")) { + char *end = nullptr; + unsigned long value = std::strtoul(limit, &end, 10); + if (end != limit && *end == 0 && value > 0 && value <= 600) + a->record_limit = unsigned(value); + } if (mkdir(record, 0700) == 0 || errno == EEXIST) { a->record_dir = record; FILE *file = std::fopen((a->record_dir + "/calibration.bin").c_str(), "wb"); @@ -984,7 +1014,7 @@ rift_s_world_anchor_record_rig_frames(rift_s_world_anchor *a, unsigned count, const xrt_pose *raw) { - if (!a || a->record_dir.empty() || a->recorded_pairs >= 600 || !frames || count < 2 || + if (!a || a->record_dir.empty() || a->recorded_pairs >= a->record_limit || !frames || count < 2 || count > RIFT_S_WORLD_MAX_CAMERAS || !frames[0]) return; for (unsigned eye = 0; eye < count; ++eye) { diff --git a/src/xrt/drivers/rift_s/rift_s_world_anchor.h b/src/xrt/drivers/rift_s/rift_s_world_anchor.h index 41cd3e5c1..a05c5b40e 100644 --- a/src/xrt/drivers/rift_s/rift_s_world_anchor.h +++ b/src/xrt/drivers/rift_s/rift_s_world_anchor.h @@ -37,8 +37,8 @@ struct rift_s_world_anchor_result bool recorded, relocalized; }; -// Opt-in bench recorder. Writes at most 600 synchronized camera sets and capture-time SLAM poses. -// RIFT_S_WORLD_RECORD_DIR is honored only with MONADO_STEAMVR_BENCH enabled. +// Opt-in diagnostics recorder. Writes synchronized camera sets and capture-time SLAM poses. +// RIFT_S_WORLD_RECORD_DIR does not change tracking. RIFT_S_WORLD_RECORD_LIMIT caps sets at 1..600. void rift_s_world_anchor_record_frames(struct rift_s_world_anchor *anchor, const struct xrt_frame *left, diff --git a/src/xrt/include/tracking/t_constellation_motion.h b/src/xrt/include/tracking/t_constellation_motion.h new file mode 100644 index 000000000..296e97f0a --- /dev/null +++ b/src/xrt/include/tracking/t_constellation_motion.h @@ -0,0 +1,8 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once + +#include + +//! Maximum capture gap for consecutive optical tracking candidates. +#define T_CONSTELLATION_TRACK_MAX_GAP_NS INT64_C(300000000) diff --git a/src/xrt/include/tracking/t_constellation_tracking.h b/src/xrt/include/tracking/t_constellation_tracking.h index c6ec881db..70075d5d0 100644 --- a/src/xrt/include/tracking/t_constellation_tracking.h +++ b/src/xrt/include/tracking/t_constellation_tracking.h @@ -107,8 +107,8 @@ struct t_constellation_tracked_device_callbacks void (*push_brightness_update)(struct xrt_device *xdev, uint8_t average_brightness); //! True only when optical heading and capture-time attitude support a rotation prior. bool (*has_heading_prior)(struct xrt_device *xdev, timepoint_ns capture_ns); - //! Optional: preferred over push_observed_pose when set, carries fit quality and yaw observability. - void (*push_pose_observation)(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs); + //! Optional: preferred over push_observed_pose. False releases the hypothesis and its blobs. + bool (*push_pose_observation)(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs); }; struct t_constellation_tracked_device_connection * diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_driver.cpp b/src/xrt/state_trackers/steamvr_drv/ovrd_driver.cpp index fbaa2eef4..4f94eb311 100644 --- a/src/xrt/state_trackers/steamvr_drv/ovrd_driver.cpp +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_driver.cpp @@ -19,6 +19,7 @@ #include #include #include +#include #include "math/m_api.h" #include "ovrd_log.hpp" @@ -26,6 +27,8 @@ #include "ovrd_display.hpp" #include "ovrd_camera.hpp" #include "ovrd_roomsetup_guard_process.hpp" +#include "ovrd_world_universe.hpp" +#include "../../drivers/rift_s/rift_s_world_control.h" #include #include "openvr_driver.h" @@ -81,6 +84,28 @@ is_rift_s_touch(const struct xrt_device *xdev) return xdev->name == XRT_DEVICE_TOUCH_CONTROLLER && strncmp(xdev->str, prefix, sizeof(prefix) - 1) == 0; } +static void +update_rift_s_universe(vr::PropertyContainerHandle_t container, uint64_t &universe, int64_t &last_poll) +{ + int64_t now = os_monotonic_get_ns(); + if (now - last_poll < 500000000LL) return; + last_poll = now; + char path[1024]; + rift_s_world_control_path("status", path, sizeof(path)); + std::ifstream stream(path); + int64_t stamp = 0; + std::string status; + stream >> stamp >> status; + static const int64_t started = now; + uint64_t next = rift_s_world_universe(status, stamp, now, started); + if (next == universe) return; + auto error = vr::VRProperties()->SetUint64Property(container, vr::Prop_CurrentUniverseId_Uint64, next); + if (error == vr::TrackedProp_Success) { + universe = next; + ovrd_log("Rift S room universe=%llu status=%s\n", (unsigned long long)next, status.c_str()); + } +} + // DebugRequest bench commands expire so a stopped script cannot hold a trigger. // Commands: bench x y z pitch trigger; bench6 x y z pitch yaw roll trigger. // Angles are radians. "bench off" returns to tracking. @@ -896,6 +921,8 @@ public: std::this_thread::sleep_for(std::chrono::milliseconds(1)); if (m_unObjectId != vr::k_unTrackedDeviceIndexInvalid) { + if (is_rift_s_touch(m_xdev)) + update_rift_s_universe(m_ulPropertyContainer, m_worldUniverse, m_worldPoll); vr::VRServerDriverHost()->TrackedDevicePoseUpdated(m_unObjectId, GetPose(), sizeof(vr::DriverPose_t)); } @@ -928,7 +955,8 @@ public: m_ulPropertyContainer = vr::VRProperties()->TrackedDeviceToPropertyContainer(m_unObjectId); // return a constant that's not 0 (invalid) or 1 (reserved for Oculus) - vr::VRProperties()->SetUint64Property(m_ulPropertyContainer, vr::Prop_CurrentUniverseId_Uint64, 2); + m_worldUniverse = is_rift_s_touch(m_xdev) ? 3 : 2; + vr::VRProperties()->SetUint64Property(m_ulPropertyContainer, vr::Prop_CurrentUniverseId_Uint64, m_worldUniverse); vr::VRProperties()->SetInt32Property(m_ulPropertyContainer, vr::Prop_DeviceClass_Int32, vr::TrackedDeviceClass_Controller); if (m_hand == XRT_HAND_LEFT) { @@ -1260,6 +1288,8 @@ public: vr::DriverPose_t m_pose; vr::TrackedDeviceIndex_t m_unObjectId; vr::PropertyContainerHandle_t m_ulPropertyContainer; + uint64_t m_worldUniverse = 3; + int64_t m_worldPoll = 0; bool m_emulate_index_controller = false; @@ -1370,6 +1400,8 @@ private: vr::TrackedDeviceIndex_t m_trackedDeviceIndex = 0; vr::PropertyContainerHandle_t m_ulPropertyContainer = vr::k_ulInvalidPropertyContainer; + uint64_t m_worldUniverse = 3; + int64_t m_worldPoll = 0; float m_flSecondsFromVsyncToPhotons = -1; float m_flDisplayFrequency = -1; @@ -1448,6 +1480,8 @@ CDeviceDriver_Monado::PoseUpdateThreadFunction() std::this_thread::sleep_for(std::chrono::milliseconds(1)); if (m_benchProximity) vr::VRDriverInput()->UpdateBooleanComponent(m_benchProximity, true, 0); + if (strncmp(m_xdev->str, "Oculus Rift S", 13) == 0) + update_rift_s_universe(m_ulPropertyContainer, m_worldUniverse, m_worldPoll); vr::VRServerDriverHost()->TrackedDevicePoseUpdated(m_trackedDeviceIndex, GetPose(), sizeof(vr::DriverPose_t)); } @@ -1482,7 +1516,8 @@ CDeviceDriver_Monado::Activate(vr::TrackedDeviceIndex_t unObjectId) ovrd_log("MONADO_STEAMVR_BENCH enabled: synthetic fallback and awake HMD\n"); } // return a constant that's not 0 (invalid) or 1 (reserved for Oculus) - vr::VRProperties()->SetUint64Property( m_ulPropertyContainer, vr::Prop_CurrentUniverseId_Uint64, 2); + m_worldUniverse = strncmp(m_xdev->str, "Oculus Rift S", 13) == 0 ? 3 : 2; + vr::VRProperties()->SetUint64Property(m_ulPropertyContainer, vr::Prop_CurrentUniverseId_Uint64, m_worldUniverse); if (m_xdev->camera) { vr::EVRInitError queue_error = vr::VRInitError_None; @@ -1672,12 +1707,8 @@ CDeviceDriver_Monado::GetPose() .shouldApplyHeadModel = !m_xdev->supported.position_tracking, .deviceIsConnected = true, }; - // A loss of positional tracking still has a finite held position. Dropping it sent Y=0 to games. - apply_pose(&rel, &t, XRT_SPACE_RELATION_POSITION_VALID_BIT); - if (!(rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT)) { - t.result = vr::TrackingResult_Running_OutOfRange; - t.poseIsValid = false; - } + // Rift S keeps a finite fallback while VIO recovers or the saved room awaits visual confirmation. + apply_head_pose(&rel, &t, strcmp(m_xdev->str, "Oculus Rift S") == 0); #ifdef DUMP_POSE ovrd_log("get hmd pose %f %f %f %f, %f %f %f\n", t.qRotation.x, t.qRotation.y, t.qRotation.z, t.qRotation.w, diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_pose.hpp b/src/xrt/state_trackers/steamvr_drv/ovrd_pose.hpp index a7dcb095e..d6517d8b7 100644 --- a/src/xrt/state_trackers/steamvr_drv/ovrd_pose.hpp +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_pose.hpp @@ -96,6 +96,22 @@ apply_pose(struct xrt_space_relation *rel, copy_pose_velocities(rel, m_pose); } +inline void +apply_head_pose(struct xrt_space_relation *rel, vr::DriverPose_t *pose, bool allow_position_estimate) +{ + apply_pose(rel, pose, XRT_SPACE_RELATION_POSITION_VALID_BIT, true); + if (allow_position_estimate) { + const auto valid = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_POSITION_VALID_BIT; + pose->poseIsValid = finite_pose(rel->pose) && (rel->relation_flags & valid) == valid; + if (pose->poseIsValid) copy_quat(&rel->pose.orientation, &pose->qRotation); + } + if ((rel->relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) == 0 || + (rel->relation_flags & XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT) == 0) { + pose->result = vr::TrackingResult_Running_OutOfRange; + if (!allow_position_estimate) pose->poseIsValid = false; + } +} + inline void transform_rift_s_touch_relation(struct xrt_space_relation *rel, const struct xrt_pose *grip_to_model, diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_floor.hpp b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_floor.hpp index 90314e2be..9334f1c06 100644 --- a/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_floor.hpp +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_floor.hpp @@ -84,9 +84,9 @@ public: frame.write(active); } bool - publish(bool initialize_manual = false) + publish(bool initialize_manual = false, bool saved_plane_trusted = true) { - if (!handle || !paths) + if (!saved_plane_trusted || !handle || !paths) return false; vr::HmdMatrix34_t standing{}, setup{}; if (!transforms(standing, setup)) diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard.cpp b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard.cpp index 36638c426..a3db8ba35 100644 --- a/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard.cpp +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_roomsetup_guard.cpp @@ -105,6 +105,7 @@ static bool update_world_notification(vr::IVRNotifications ¬ifications, warning.c_str(), vr::EVRNotificationStyle_Application, nullptr, &created) != vr::VRNotificationError_OK) return false; id = created; + fprintf(stderr, "monado-roomsetup-guard: Guardian notification %u: %s\n", created, warning.c_str()); } delivered = warning; return true; @@ -261,6 +262,10 @@ int main(int argc, char **argv) fs::path path = calibration_path(log_path); OverflowLog diagnostics(log_path); if (!system || !setup || path.empty() || !snapshot(*setup, path)) { shutdown(); return 1; } + char model[128] = {}; + system->GetStringTrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd, vr::Prop_ModelNumber_String, + model, sizeof(model)); + const bool is_rift_s = std::strstr(model, "Oculus Rift S") != nullptr; vr::VROverlayHandle_t overlay = vr::k_ulOverlayHandleInvalid; if (overlays) overlays->CreateOverlay("monado.roomsetup.guard", "Monado playspace setup", &overlay); if (write(ready, "1", 1) != 1) { shutdown(); return 1; } @@ -301,6 +306,7 @@ int main(int argc, char **argv) auto floor_deadline = std::chrono::steady_clock::time_point{}; bool running = true; bool world_setup = false; + bool saved_plane_trusted = !is_rift_s; auto world_cancel_deadline = std::chrono::steady_clock::time_point{}; std::string world_warning; vr::VRNotificationId world_notification = 0; @@ -338,6 +344,10 @@ int main(int argc, char **argv) if (saved && compositor_failed()) failed = true; } if ((next == 2 || next == 3 || drawing(next)) && !world_setup && !failed) { + // Capture trust before begin switches the tracker into a new setup frame. + saved_plane_trusted = !is_rift_s || (world_status() == "localized" && + system->GetUint64TrackedDeviceProperty(vr::k_unTrackedDeviceIndex_Hmd, + vr::Prop_CurrentUniverseId_Uint64) == 2); world_setup = true; world_cancel_deadline = {}; world_command("begin"); @@ -383,9 +393,9 @@ int main(int argc, char **argv) if (status == "save_failed") warning = "Visual boundary map could not be saved. Check Monado config write access before restarting VR."; else if (status == "unanchored") - warning = "Boundary has no visual anchor. Complete room setup once to save its physical location."; + warning = "Guardian not found: no usable saved room map. Run Room Setup to anchor the boundary."; else if (status == "relocalizing") - warning = "Boundary not confirmed, look around. Temporary tracking is active."; + warning = "Guardian not found. Uncover headset cameras and look around. If the room stays unavailable, run Room Setup."; auto now = std::chrono::steady_clock::now(); if (warning != world_warning && notifications && overlays && now >= next_notification_attempt) { next_notification_attempt = now + std::chrono::seconds(1); @@ -395,7 +405,8 @@ int main(int argc, char **argv) update_world_notification(*notifications, overlay, warning, world_warning, world_notification); } if (floor_pending) { - if (step == 0 || floor->publish(step == 2) || std::chrono::steady_clock::now() > floor_deadline) + if (step == 0 || floor->publish(step == 2, saved_plane_trusted) || + std::chrono::steady_clock::now() > floor_deadline) floor_pending = false; } floor->update_frame(step == 2 || step == 3 || step == 4 || step == 5 || step == 7); diff --git a/src/xrt/state_trackers/steamvr_drv/ovrd_world_universe.hpp b/src/xrt/state_trackers/steamvr_drv/ovrd_world_universe.hpp new file mode 100644 index 000000000..6e1f6aa7a --- /dev/null +++ b/src/xrt/state_trackers/steamvr_drv/ovrd_world_universe.hpp @@ -0,0 +1,15 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#pragma once + +#include +#include + +// Universe 2 contains the saved room. Universe 3 has no saved room calibration. +// A session frame must never use universe 2's floor, heading or collision bounds. +static inline uint64_t +rift_s_world_universe(std::string_view status, int64_t stamp, int64_t now, int64_t started = 0) +{ + bool fresh = stamp > 0 && stamp >= started && stamp <= now && now - stamp <= 2000000000LL; + return fresh && (status == "localized" || status == "setup") ? 2 : 3; +} diff --git a/src/xrt/targets/common/target_builder_rift_s.c b/src/xrt/targets/common/target_builder_rift_s.c index 588cd6920..1b124ebf2 100644 --- a/src/xrt/targets/common/target_builder_rift_s.c +++ b/src/xrt/targets/common/target_builder_rift_s.c @@ -44,7 +44,7 @@ enum u_logging_level rift_s_log_level; * */ -DEBUG_GET_ONCE_LOG_OPTION(rift_s_log, "RIFT_S_LOG", U_LOGGING_INFO) +DEBUG_GET_ONCE_LOG_OPTION(rift_s_log, "RIFT_S_LOG", U_LOGGING_WARN) #ifdef XRT_BUILD_DRIVER_HANDTRACKING DEBUG_GET_ONCE_BOOL_OPTION(rift_s_hand_tracking_as_controller, "RIFT_S_HAND_TRACKING_AS_CONTROLLERS", false) diff --git a/src/xrt/targets/steamvr_drv/CMakeLists.txt b/src/xrt/targets/steamvr_drv/CMakeLists.txt index f8b23ee1f..363565f5a 100644 --- a/src/xrt/targets/steamvr_drv/CMakeLists.txt +++ b/src/xrt/targets/steamvr_drv/CMakeLists.txt @@ -86,7 +86,6 @@ install( if(TARGET monado-roomsetup-guard) install(TARGETS monado-roomsetup-guard RUNTIME DESTINATION ${CMAKE_INSTALL_DATADIR}/steamvr-monado/bin/linux64) endif() - if(CMAKE_SYSTEM_NAME STREQUAL "Linux") configure_file(${PROJECT_SOURCE_DIR}/scripts/rifts-setup.in ${PROJECT_BINARY_DIR}/rifts-setup @ONLY) configure_file(${PROJECT_SOURCE_DIR}/scripts/rifts-session-install.in ${PROJECT_BINARY_DIR}/rifts-session-install @ONLY) diff --git a/src/xrt/tracking/constellation/internal/constrained_pose.cpp b/src/xrt/tracking/constellation/internal/constrained_pose.cpp index 596d93923..502d84833 100644 --- a/src/xrt/tracking/constellation/internal/constrained_pose.cpp +++ b/src/xrt/tracking/constellation/internal/constrained_pose.cpp @@ -1,6 +1,7 @@ // Copyright 2026, lbgos // SPDX-License-Identifier: BSL-1.0 #include "constrained_pose.h" +#include "tracking/t_constellation_motion.h" #include #include @@ -918,8 +919,8 @@ prior_pairs(Context &ctx, const Pose &prior) void constellation_constrained_default_config(constellation_constrained_config *c) { - /* Score/association gates and the angular separation follow the stock Windows defaults. - * Everything else is Monado policy chosen with the synthetic + /* Score/association gates and the angular separation are compiled defaults named in the + * handoff specification. Everything else is Monado policy chosen with the synthetic * partial-ring tests in tests_constellation_constrained: object-space ray noise 1 mm, * position std 3 cm, yaw std 20 degrees, ambiguity gap chi-square 9, at least 4 observations * of 3 distinct LEDs (cold and tracking), at most one unmatched blob within 8 mm (twice the @@ -1362,7 +1363,7 @@ void constellation_constrained_default_motion_limits(constellation_motion_limits *limits) { // Proposed values: fast hand motion stays below about 3 m/s; 2 cm covers fit noise. - *limits = {3.0, 0.02, 10 * pi / 180, 300000000}; + *limits = {3.0, 0.02, 10 * pi / 180, T_CONSTELLATION_TRACK_MAX_GAP_NS}; } bool diff --git a/src/xrt/tracking/constellation/internal/constrained_pose.h b/src/xrt/tracking/constellation/internal/constrained_pose.h index d4555e3dc..01d1ef2e7 100644 --- a/src/xrt/tracking/constellation/internal/constrained_pose.h +++ b/src/xrt/tracking/constellation/internal/constrained_pose.h @@ -66,7 +66,7 @@ enum constellation_constrained_solver_status struct constellation_constrained_config { - //! Object-space score gate in metres (stock default, not an active setting). + //! Object-space score gate in metres (compiled default from the handoff spec, not an active setting). double score_gate_m; //! Broad object-space association gate in metres for candidate edges. double association_gate_m; @@ -88,18 +88,18 @@ struct constellation_constrained_config double minimum_bearing_separation_rad; double minimum_depth_m; double maximum_depth_m; - /* Proposed noise/uncertainty policy, not device calibration. */ + /* Proposed noise/uncertainty policy, not recovered device calibration. */ double ray_noise_std_m; double maximum_position_std_m; double maximum_yaw_std_rad; //! Minimum likelihood gap (chi-square units of ray_noise_std_m) to the best competing assignment. double ambiguity_chi_square; - /* Monado verification policy, beyond the stock scorer count (inliers > sample size): + /* Monado verification policy, beyond the recovered scorer count (inliers > sample size): * accepted observations required when acquiring without a trusted position prior, and the * distinct physical LEDs among them. Repeated views of one LED do not fix its identity. */ unsigned minimum_cold_inliers; unsigned minimum_cold_distinct_leds; - /* The same counts for searches around a trusted prior. The stock minimum (inliers > sample + /* The same counts for searches around a trusted prior. The recovered minimum (inliers > sample * size, so 2 for the retained prior) let one stale prior keep re-verifying itself on two blobs * in worn use; a published pose now needs this much evidence on every path. */ unsigned minimum_tracking_inliers; diff --git a/src/xrt/tracking/constellation/t_constellation_tracking.c b/src/xrt/tracking/constellation/t_constellation_tracking.c index af47f97b8..95c5acf7c 100644 --- a/src/xrt/tracking/constellation/t_constellation_tracking.c +++ b/src/xrt/tracking/constellation/t_constellation_tracking.c @@ -33,7 +33,7 @@ #include "internal/ransac_pnp.h" #include "internal/sample.h" -DEBUG_GET_ONCE_LOG_OPTION(ct_log, "CONSTELLATION_LOG", U_LOGGING_INFO) +DEBUG_GET_ONCE_LOG_OPTION(ct_log, "CONSTELLATION_LOG", U_LOGGING_WARN) DEBUG_GET_ONCE_FLOAT_OPTION(ct_score_gate, "CONSTELLATION_SCORE_GATE_M", 0.004) DEBUG_GET_ONCE_FLOAT_OPTION(ct_association_gate, "CONSTELLATION_ASSOCIATION_GATE_M", 0.15) DEBUG_GET_ONCE_FLOAT_OPTION(ct_ray_noise, "CONSTELLATION_RAY_NOISE_M", 0.001) @@ -371,19 +371,22 @@ constellation_tracked_device_connection_has_heading(struct t_constellation_track return ret; } -static void +static bool constellation_tracked_device_connection_notify_observation(struct t_constellation_tracked_device_connection *ctdc, const struct t_constellation_pose_observation *obs) { + bool accepted = false; os_mutex_lock(&ctdc->lock); if (!ctdc->disconnected) { if (ctdc->cb->push_pose_observation) { - ctdc->cb->push_pose_observation(ctdc->xdev, obs); + accepted = ctdc->cb->push_pose_observation(ctdc->xdev, obs); } else if (ctdc->cb->push_observed_pose) { ctdc->cb->push_observed_pose(ctdc->xdev, obs->capture_ns, &obs->pose); + accepted = true; } } os_mutex_unlock(&ctdc->lock); + return accepted; } static const char *ct_stage_names[CT_STAGE_COUNT] = {"fast", "constrained", "p3p"}; @@ -798,10 +801,10 @@ track_gate_locked(struct t_constellation_tracker *ct, d->last_failure_capture_ns = capture_ns; d->last_failure_processing_ns = os_monotonic_get_ns(); } - log_track_transition_locked(ct, device, before, capture_ns, - verdict == CONSTELLATION_TRACK_HOLD_RESTART ? "jumps" - : verdict == CONSTELLATION_TRACK_PUBLISH ? "confirmed" - : "candidate"); + if (verdict != CONSTELLATION_TRACK_PUBLISH) { + log_track_transition_locked(ct, device, before, capture_ns, + verdict == CONSTELLATION_TRACK_HOLD_RESTART ? "jumps" : "candidate"); + } return verdict == CONSTELLATION_TRACK_PUBLISH ? NULL : constellation_track_verdict_name(verdict); } @@ -833,7 +836,14 @@ observation_from_result(const struct constellation_constrained_result *r, /* Publish one rig pose per exposure. Labels are evidence from this accepted hypothesis, * never independent ownership carried from an earlier frame. */ -static bool +enum joint_pose_submission +{ + JOINT_POSE_HELD, + JOINT_POSE_FUSION_REJECTED, + JOINT_POSE_PUBLISHED, +}; + +static enum joint_pose_submission submit_joint_pose(struct t_constellation_tracker *ct, struct tracking_sample_device_state *state, struct constellation_tracking_sample *sample, @@ -842,6 +852,7 @@ submit_joint_pose(struct t_constellation_tracker *ct, struct constellation_tracker_device *device = &ct->devices[state->dev_index]; struct constellation_constrained_result *r = &state->joint_result; os_mutex_lock(&ct->tracked_device_lock); + const struct constellation_track previous_track = device->track; const char *held = NULL; if (device->have_last_seen_pose && sample->timestamp <= device->last_seen_pose_ts) { device->diagnostics.stale++; @@ -856,8 +867,25 @@ submit_joint_pose(struct t_constellation_tracker *ct, log_device_summary_locked(ct, device, false); os_mutex_unlock(&ct->tracked_device_lock); log_joint_result(ct, sample, state, stage, held); - return false; + return JOINT_POSE_HELD; } + struct xrt_pose P_xrworld_model, P_xrworld_device; + pose_flip_YZ(&r->P_world_model, &P_xrworld_model); + math_pose_transform(&P_xrworld_model, &device->led_model.P_model_device, &P_xrworld_device); + struct t_constellation_pose_observation obs; + observation_from_result(r, sample->timestamp, &P_xrworld_device, &obs); + if (!constellation_tracked_device_connection_notify_observation(device->connection, &obs)) { + // Fusion declined this pose. Keep its accepted prior and leave the blobs available. + device->track = previous_track; + device->diagnostics.last_failure = "fusion_rejected"; + device->diagnostics.last_failure_capture_ns = sample->timestamp; + device->diagnostics.last_failure_processing_ns = os_monotonic_get_ns(); + log_device_summary_locked(ct, device, false); + os_mutex_unlock(&ct->tracked_device_lock); + log_joint_result(ct, sample, state, stage, "fusion_rejected"); + return JOINT_POSE_FUSION_REJECTED; + } + log_track_transition_locked(ct, device, previous_track.state, sample->timestamp, "confirmed"); device->have_last_seen_pose = true; device->last_seen_pose_ts = sample->timestamp; device->last_seen_pose = r->P_world_model; @@ -914,11 +942,6 @@ submit_joint_pose(struct t_constellation_tracker *ct, os_mutex_unlock(&ct->cam[v].bw_lock); } - struct xrt_pose P_xrworld_model, P_xrworld_device; - pose_flip_YZ(&state->final_pose, &P_xrworld_model); - math_pose_transform(&P_xrworld_model, &device->led_model.P_model_device, &P_xrworld_device); - struct t_constellation_pose_observation obs; - observation_from_result(r, sample->timestamp, &P_xrworld_device, &obs); struct xrt_vec3 head = {NAN, NAN, NAN}; if (head_relative(sample, device, &r->P_world_model, &head)) { device->diagnostics.last_published_head = head; @@ -964,7 +987,6 @@ submit_joint_pose(struct t_constellation_tracker *ct, projected ? maximum_pixel_error : NAN, r->fit_chi_square, r->fit_chi_square_limit, r->cost_m); } constellation_tracked_device_connection_notify_brightness_update(device->connection, brightness / r->inliers); - constellation_tracked_device_connection_notify_observation(device->connection, &obs); if (ct->controller_masks_sink) { for (unsigned v = 0; v < sample->n_views; v++) { @@ -987,7 +1009,7 @@ submit_joint_pose(struct t_constellation_tracker *ct, log_device_summary_locked(ct, device, false); os_mutex_unlock(&ct->tracked_device_lock); log_joint_result(ct, sample, state, stage, "none"); - return true; + return JOINT_POSE_PUBLISHED; } /* A competing candidate keeps a shared blob only with this much more inlier support. */ @@ -1004,17 +1026,45 @@ arbitrate_and_submit(struct t_constellation_tracker *ct, { const struct constellation_constrained_result *claims[CONSTELLATION_MAX_DEVICES]; bool published[CONSTELLATION_MAX_DEVICES] = {0}; + bool viable[CONSTELLATION_MAX_DEVICES] = {0}; + bool attempted[CONSTELLATION_MAX_DEVICES] = {0}; enum constellation_constrained_claim blocked[CONSTELLATION_MAX_DEVICES]; unsigned blockers[CONSTELLATION_MAX_DEVICES]; for (unsigned d = 0; d < sample->n_devices; d++) { claims[d] = &sample->devices[d].joint_result; published[d] = sample->devices[d].found_device_pose; + viable[d] = candidates[d]; + } + for (unsigned round = 0; round < sample->n_devices; round++) { + // Held optical candidates remain competitors. Only fusion rejection releases a claim. + bool competitors[CONSTELLATION_MAX_DEVICES] = {0}; + for (unsigned d = 0; d < sample->n_devices; d++) { + competitors[d] = viable[d] && !published[d] && claims[d] != NULL; + } + constellation_constrained_arbitrate_with_owners(claims, published, competitors, blocked, blockers, + sample->n_devices, ARBITRATION_SUPPORT_MARGIN); + bool released = false; + for (unsigned d = 0; d < sample->n_devices; d++) { + candidates[d] = competitors[d]; + if (!competitors[d] || attempted[d]) { + continue; + } + attempted[d] = true; + enum joint_pose_submission result = submit_joint_pose(ct, &sample->devices[d], sample, stage); + if (result == JOINT_POSE_FUSION_REJECTED) { + claims[d] = NULL; + released = true; + } else if (result == JOINT_POSE_PUBLISHED) { + published[d] = true; + } + } + if (!released) { + break; + } } - constellation_constrained_arbitrate_with_owners(claims, published, candidates, blocked, blockers, - sample->n_devices, ARBITRATION_SUPPORT_MARGIN); for (unsigned d = 0; d < sample->n_devices; d++) { struct tracking_sample_device_state *state = &sample->devices[d]; - if (blocked[d] != CONSTELLATION_CLAIM_NONE) { + if (!attempted[d] && blocked[d] != CONSTELLATION_CLAIM_NONE) { os_mutex_lock(&ct->tracked_device_lock); ct->devices[state->dev_index].diagnostics.shared_blob++; ct->devices[state->dev_index].diagnostics.shared_by[blocked[d]]++; @@ -1061,9 +1111,6 @@ arbitrate_and_submit(struct t_constellation_tracker *ct, : blocked[d] == CONSTELLATION_CLAIM_CANDIDATE ? "shared_blob_candidate" : "shared_blob_rejected"); } - if (candidates[d]) { - submit_joint_pose(ct, state, sample, stage); - } } } @@ -1459,7 +1506,7 @@ constellation_tracker_process_frame_fast(struct xrt_frame_sink *sink, struct xrt os_mutex_unlock(&ct->tracked_device_lock); /* Fast path: a trusted position prior (recent optical fix) bounds association, so the - * stock low-count gates apply. Everything else goes to the long search. */ + * recovered low-count gates apply. Everything else goes to the long search. */ struct constellation_constrained_view views[CONSTELLATION_MAX_CAMERAS]; unsigned n_views = joint_views(ct, sample, views); unsigned blob_count = sample_blob_count(sample); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index e3c42de9f..a616052c1 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -142,9 +142,20 @@ if(XRT_MODULE_CONSTELLATION_TRACKING) target_include_directories(tests_constellation PRIVATE ../src/xrt/tracking/constellation) target_link_libraries(tests_constellation PRIVATE xrt-external-catch2 constellation_tracker aux_math) add_test(NAME tests_constellation COMMAND tests_constellation) + + add_executable(tests_constellation_publication tests_constellation_publication.c) + target_include_directories(tests_constellation_publication PRIVATE ../src/xrt/tracking/constellation ${OpenCV_INCLUDE_DIRS}) + target_link_libraries(tests_constellation_publication PRIVATE constellation_tracker aux_math aux_util_sink) + set_target_properties(tests_constellation_publication PROPERTIES LINKER_LANGUAGE CXX) + add_test(NAME tests_constellation_publication COMMAND tests_constellation_publication) endif() if(XRT_BUILD_DRIVER_RIFT_S) + add_executable(tests_rift_s_motion tests_rift_s_motion.cpp ../src/xrt/drivers/rift_s/rift_s_motion.cpp) + target_include_directories(tests_rift_s_motion PRIVATE ../src/xrt/drivers/rift_s) + target_link_libraries(tests_rift_s_motion PRIVATE xrt-external-catch2 aux_math aux_util) + add_test(NAME tests_rift_s_motion COMMAND tests_rift_s_motion) + add_executable(tests_rift_s_tracking tests_rift_s_tracking.cpp) target_include_directories(tests_rift_s_tracking PRIVATE ../src/xrt/drivers/rift_s) target_link_libraries(tests_rift_s_tracking PRIVATE xrt-external-catch2 drv_rift_s aux_math aux_tracking) diff --git a/tests/tests_constellation_constrained.cpp b/tests/tests_constellation_constrained.cpp index 498b9509b..7e84482ce 100644 --- a/tests/tests_constellation_constrained.cpp +++ b/tests/tests_constellation_constrained.cpp @@ -88,7 +88,7 @@ struct Scene math_pose_invert(&v.P_world_cam, &v.P_cam_world); constellation_constrained_default_config(&config); config.time_budget_us = 2000000; - // These fixtures exercise the stock scorer minimum (inliers > sample size) with three + // These fixtures exercise the recovered scorer minimum (inliers > sample size) with three // LEDs, so they relax Monado's stricter cold-acquisition and tracking policy. config.minimum_cold_inliers = 3; config.minimum_cold_distinct_leds = 0; @@ -877,7 +877,7 @@ TEST_CASE("Cold low-count acquisition never accepts a wrong pose in randomized p CHECK(arbitrated_same_tilt.wrong == 0); } -TEST_CASE("Stock minimum count alone admits wrong identities that the cold policy rejects") +TEST_CASE("Recovered minimum count alone admits wrong identities that the cold policy rejects") { // Documents why the cold policy exists: inliers > sample size is necessary, not sufficient. constellation_constrained_config minimum; diff --git a/tests/tests_constellation_publication.c b/tests/tests_constellation_publication.c new file mode 100644 index 000000000..c273954fa --- /dev/null +++ b/tests/tests_constellation_publication.c @@ -0,0 +1,213 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include +#include + +// Include the production publication path to check ownership before and after fusion acceptance. +#include "../src/xrt/tracking/constellation/t_constellation_tracking.c" + +#define CHECK(condition) \ + do { \ + if (!(condition)) { \ + fprintf(stderr, "CHECK failed at %s:%d: %s\n", __FILE__, __LINE__, #condition); \ + return 1; \ + } \ + } while (0) + +static bool accept_observation; +static unsigned observation_calls; + +static xrt_result_t +fake_get_tracked_pose(struct xrt_device *xdev, + enum xrt_input_name name, + int64_t at_timestamp_ns, + struct xrt_space_relation *out_relation) +{ + (void)xdev; + (void)name; + (void)at_timestamp_ns; + *out_relation = (struct xrt_space_relation)XRT_SPACE_RELATION_ZERO; + out_relation->pose.orientation.w = 1.0f; + out_relation->relation_flags = XRT_SPACE_RELATION_ORIENTATION_VALID_BIT; + return XRT_SUCCESS; +} + +static bool +fake_push_pose_observation(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs) +{ + (void)xdev; + if (obs->capture_ns != INT64_C(1100000000)) { + return false; + } + observation_calls++; + return accept_observation; +} + +static int +run_submit_acceptance_contract(enum constellation_track_state initial_state) +{ + struct t_constellation_tracker tracker = {0}; + struct t_constellation_tracked_device_connection connection = {0}; + struct t_constellation_tracked_device_callbacks callbacks = {0}; + struct xrt_device xdev = {0}; + struct constellation_tracking_sample sample = {0}; + struct constellation_tracker_device *device = &tracker.devices[0]; + struct tracking_sample_device_state *state = &sample.devices[0]; + + CHECK(os_mutex_init(&tracker.tracked_device_lock) == 0); + CHECK(os_mutex_init(&connection.lock) == 0); + tracker.log_level = U_LOGGING_ERROR; + connection.xdev = &xdev; + connection.cb = &callbacks; + connection.tracker = &tracker; + callbacks.push_pose_observation = fake_push_pose_observation; + xdev.get_tracked_pose = fake_get_tracked_pose; + device->connection = &connection; + device->alias = "fixture"; + device->last_matched_cam = -1; + device->led_model.P_device_model.orientation.w = 1.0f; + device->led_model.P_model_device.orientation.w = 1.0f; + + constellation_track_init(&device->track); + device->track.state = initial_state; + device->track.ref.P_world_model.orientation.w = 1.0f; + device->track.ref.imu_world.w = 1.0f; + device->track.ref.capture_ns = INT64_C(1000000000); + device->track.published = device->track.ref; + device->track.have_published = true; + device->track.ref_orientation_observed = false; + device->have_last_seen_pose = true; + device->last_seen_pose_ts = INT64_C(1000000000); + device->last_seen_pose.position.x = -7.0f; + device->last_matched_blobs = 17; + device->diagnostics.published = 23; + + sample.timestamp = INT64_C(1100000000); + sample.n_devices = 1; + sample.n_views = 0; + sample.views[0].P_cam_world.orientation.w = 1.0f; + state->dev_index = 0; + state->joint_result.P_world_model.orientation.w = 1.0f; + state->joint_result.inliers = 1; + state->final_pose.position.x = -9.0f; + state->found_device_pose = false; + + const struct constellation_track before_reject = device->track; + accept_observation = false; + observation_calls = 0; + CHECK(submit_joint_pose(&tracker, state, &sample, "fixture") == JOINT_POSE_FUSION_REJECTED); + CHECK(observation_calls == 1); + CHECK(device->track.state == before_reject.state); + CHECK(device->track.ref.capture_ns == before_reject.ref.capture_ns); + CHECK(device->track.ref.P_world_model.position.x == before_reject.ref.P_world_model.position.x); + CHECK(device->track.ref.imu_world.w == before_reject.ref.imu_world.w); + CHECK(device->track.published.capture_ns == before_reject.published.capture_ns); + CHECK(device->track.have_published == before_reject.have_published); + CHECK(device->track.ref_orientation_observed == before_reject.ref_orientation_observed); + CHECK(device->track.consecutive_jumps == before_reject.consecutive_jumps); + CHECK(device->have_last_seen_pose); + CHECK(device->last_seen_pose_ts == INT64_C(1000000000)); + CHECK(device->last_seen_pose.position.x == -7.0f); + CHECK(device->last_matched_blobs == 17); + CHECK(device->diagnostics.published == 23); + CHECK(device->diagnostics.confirmed == 0); + CHECK(!state->found_device_pose); + CHECK(state->final_pose.position.x == -9.0f); + + accept_observation = true; + CHECK(submit_joint_pose(&tracker, state, &sample, "fixture") == JOINT_POSE_PUBLISHED); + CHECK(observation_calls == 2); + CHECK(state->found_device_pose); + CHECK(device->last_seen_pose_ts == INT64_C(1100000000)); + CHECK(device->track.ref.capture_ns == INT64_C(1100000000)); + CHECK(device->diagnostics.published == 24); + CHECK(device->diagnostics.confirmed == (initial_state == CONSTELLATION_TRACK_CONFIRMING)); + + os_mutex_destroy(&connection.lock); + os_mutex_destroy(&tracker.tracked_device_lock); + return 0; +} + +struct arbitration_device +{ + struct xrt_device base; + bool accept; + unsigned calls; +}; + +static bool +arbitration_push_pose(struct xrt_device *xdev, const struct t_constellation_pose_observation *obs) +{ + struct arbitration_device *device = (struct arbitration_device *)xdev; + device->calls++; + return device->accept && obs->capture_ns == INT64_C(1100000000); +} + +static int +run_arbitration_contract(unsigned stronger_index, bool accept_stronger, bool hold_stronger) +{ + struct t_constellation_tracker tracker = {0}; + struct constellation_tracking_sample sample = {0}; + struct t_constellation_tracked_device_connection connections[2] = {0}; + struct t_constellation_tracked_device_callbacks callbacks = {.push_pose_observation = arbitration_push_pose}; + struct arbitration_device devices[2] = {0}; + bool candidates[CONSTELLATION_MAX_DEVICES] = {true, true}; + const unsigned weaker_index = 1 - stronger_index; + CHECK(os_mutex_init(&tracker.tracked_device_lock) == 0); + tracker.log_level = U_LOGGING_ERROR; + sample.n_devices = 2; + sample.timestamp = INT64_C(1100000000); + sample.views[0].P_cam_world.orientation.w = 1.0f; + for (unsigned d = 0; d < 2; d++) { + CHECK(os_mutex_init(&connections[d].lock) == 0); + connections[d].xdev = &devices[d].base; + connections[d].cb = &callbacks; + connections[d].tracker = &tracker; + devices[d].base.get_tracked_pose = fake_get_tracked_pose; + devices[d].accept = d == weaker_index || accept_stronger; + struct constellation_tracker_device *device = &tracker.devices[d]; + device->connection = &connections[d]; + device->alias = d == stronger_index ? "stronger" : "weaker"; + device->led_model.P_device_model.orientation.w = 1.0f; + device->led_model.P_model_device.orientation.w = 1.0f; + constellation_track_init(&device->track); + device->track.state = d == stronger_index && hold_stronger ? CONSTELLATION_TRACK_LOST + : CONSTELLATION_TRACK_TRACKING; + device->track.ref.capture_ns = INT64_C(1000000000); + device->track.ref.P_world_model.orientation.w = 1.0f; + device->track.ref.imu_world.w = 1.0f; + struct tracking_sample_device_state *state = &sample.devices[d]; + state->dev_index = d; + state->joint_result.P_world_model.orientation.w = 1.0f; + state->joint_result.inliers = d == stronger_index ? 8 : 4; + state->joint_result.distinct_leds = 4; + state->joint_result.num_assignments = 1; + state->joint_result.assignments[0].camera_index = 0; + state->joint_result.assignments[0].blob_index = 0; + } + arbitrate_and_submit(&tracker, &sample, candidates, "fixture"); + CHECK(devices[stronger_index].calls == (hold_stronger ? 0 : 1)); + CHECK(devices[weaker_index].calls == (!accept_stronger && !hold_stronger ? 1 : 0)); + CHECK(sample.devices[stronger_index].found_device_pose == (accept_stronger && !hold_stronger)); + CHECK(sample.devices[weaker_index].found_device_pose == (!accept_stronger && !hold_stronger)); + CHECK(tracker.devices[stronger_index].diagnostics.published == (accept_stronger && !hold_stronger)); + CHECK(tracker.devices[weaker_index].diagnostics.published == (!accept_stronger && !hold_stronger)); + if (hold_stronger) { + CHECK(tracker.devices[stronger_index].track.state == CONSTELLATION_TRACK_CONFIRMING); + } + for (unsigned d = 0; d < 2; d++) { + os_mutex_destroy(&connections[d].lock); + } + os_mutex_destroy(&tracker.tracked_device_lock); + return 0; +} + +int +main(void) +{ + return run_submit_acceptance_contract(CONSTELLATION_TRACK_TRACKING) || + run_submit_acceptance_contract(CONSTELLATION_TRACK_CONFIRMING) || + run_arbitration_contract(0, false, false) || run_arbitration_contract(1, false, false) || + run_arbitration_contract(0, true, false) || run_arbitration_contract(1, true, false) || + run_arbitration_contract(0, false, true) || run_arbitration_contract(1, false, true); +} diff --git a/tests/tests_rift_s_controller_geometry.cpp b/tests/tests_rift_s_controller_geometry.cpp index 57365742f..d545361fe 100644 --- a/tests/tests_rift_s_controller_geometry.cpp +++ b/tests/tests_rift_s_controller_geometry.cpp @@ -16,7 +16,7 @@ struct FactoryCamera std::array tangential; xrt_matrix_4x4 device_from_camera; }; -// Factory camera calibration read from a Rift S headset. +// VM124 factory camera calibration recorded in the real r14 session. const std::array factory = {{ {{190.499924f, 190.499924f, 312.359253f, 233.292801f}, {0.274657339f, 0.072651282f, -0.284994245f, 0.224762872f, -0.0842394754f, 0.0130622024f}, diff --git a/tests/tests_rift_s_metadata.cpp b/tests/tests_rift_s_metadata.cpp index a61333d3d..6a39906a0 100644 --- a/tests/tests_rift_s_metadata.cpp +++ b/tests/tests_rift_s_metadata.cpp @@ -237,7 +237,7 @@ TEST_CASE("Rift S controller IR LED register matches the Windows radio requests" REQUIRE(rift_s_encode_controller_irled_read(read.data()) == 4); CHECK(read == std::array{0x28, 0x00, 0xe8, 0x03}); - // The Windows runtime wrote 19/40000 to both controllers. + // The Windows session wrote 19/40000 to both controllers (OVRService input.cpp:2277). rift_s_controller_irled_config cfg{40000, 19}; std::array write{}; REQUIRE(rift_s_encode_controller_irled_write(&cfg, write.data()) == 12); diff --git a/tests/tests_rift_s_motion.cpp b/tests/tests_rift_s_motion.cpp new file mode 100644 index 000000000..82d0fe1c0 --- /dev/null +++ b/tests/tests_rift_s_motion.cpp @@ -0,0 +1,236 @@ +// Copyright 2026, lbgos +// SPDX-License-Identifier: BSL-1.0 +#include "catch_amalgamated.hpp" +#include "rift_s_motion.h" +#include "math/m_api.h" +#include "math/m_vec3.h" +#include + +namespace { +constexpr timepoint_ns MS = U_TIME_1MS_IN_NS; +struct Motion +{ + rift_s_motion *ctx = rift_s_motion_create(); + xrt_pose head = {XRT_QUAT_IDENTITY, {0, 1.6f, 0}}; + rift_s_motion_sample sample = {}; + Motion() + { + sample.pose.orientation = XRT_QUAT_IDENTITY; + sample.timestamp_ns = 1000 * MS; + for (double &v : sample.position_variance) + v = 1e-5; + } + ~Motion() + { + rift_s_motion_destroy(&ctx); + } + void + step(unsigned hand, xrt_vec3 p, bool reliable, time_duration_ns dt = 2 * MS) + { + sample.timestamp_ns += dt; + sample.pose.position = p; + for (double &v : sample.position_variance) + v = reliable ? 1e-5 : -1; + rift_s_motion_update(ctx, hand, &sample, &head); + } + rift_s_motion_model + get(unsigned hand, xrt_space_relation &rel, time_duration_ns ahead = 0) + { + return rift_s_motion_get(ctx, hand, sample.timestamp_ns + ahead, &rel); + } +}; +void +check_vec(xrt_vec3 a, xrt_vec3 b, double margin = 1e-6) +{ + CHECK(a.x == Catch::Approx(b.x).margin(margin)); + CHECK(a.y == Catch::Approx(b.y).margin(margin)); + CHECK(a.z == Catch::Approx(b.z).margin(margin)); +} +} // namespace + +TEST_CASE("Behind-back loss follows the saved head-relative position for either hand") +{ + for (unsigned hand : {0u, 1u}) { + Motion m; + xrt_space_relation rel{}; + const xrt_vec3 hand_position = {hand ? 0.3f : -0.3f, 1.1f, 0.5f}; + m.step(hand, hand_position, true); + CHECK(m.get(hand, rel) == RIFT_S_MOTION_NONE); + m.step(hand, hand_position, false); + REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_BEHIND_BACK); + check_vec(rel.pose.position, hand_position); + const xrt_vec3 up = {0, 1, 0}; + math_quat_from_angle_vector(1.57079632679f, &up, &m.head.orientation); + m.head.position = {1, 1.7f, 2}; + // The unobserved filter position can be nonsense. It must not replace the reliable support. + m.step(hand, {100, -100, 100}, false, 800 * MS); + REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_BEHIND_BACK); + const xrt_vec3 expected = {1.5f, 1.2f, 2 + (hand ? -0.3f : 0.3f)}; + check_vec(rel.pose.position, expected); + CHECK_FALSE(rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT); + CHECK(rel.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT); + m.step(hand, expected, true); + CHECK(m.get(hand, rel) != RIFT_S_MOTION_BEHIND_BACK); + } +} + +TEST_CASE("Out-of-view wrist rotation uses an arm pivot and returns through the correction-rate state") +{ + for (unsigned hand : {0u, 1u}) { + Motion m; + xrt_space_relation rel{}; + const xrt_vec3 start = {hand ? 0.3f : -0.3f, 1.45f, -0.5f}; + m.step(hand, start, true); + m.step(hand, start, true); + m.step(hand, start, false); + REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_OUT_OF_FOV); + check_vec(rel.pose.position, start); + const xrt_vec3 up = {0, 1, 0}; + math_quat_from_angle_vector(1.57079632679f, &up, &m.sample.pose.orientation); + m.step(hand, {5, -20, -8}, false); + REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_OUT_OF_FOV); + // Rotation of a 20 cm forearm pivot changes the IMU position; it is not a frozen translation. + CHECK(m_vec3_len(m_vec3_sub(rel.pose.position, start)) > 0.25f); + CHECK(rel.pose.position.y > 1.3f); + CHECK_FALSE(rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT); + xrt_vec3 estimate = rel.pose.position; + xrt_vec3 target = m_vec3_add(estimate, {0.03f, 0, 0}); + m.sample.linear_velocity = {0, 0, 0}; + m.step(hand, target, true); + REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_OUT_OF_FOV); + CHECK(rel.pose.position.x == Catch::Approx(estimate.x + 0.0005f).margin(2e-6)); + CHECK(rel.linear_velocity.x == Catch::Approx(0.25f).margin(1e-5)); + // The optical return drives the model with a bounded correction rate, with no fixed blend timer. + for (unsigned i = 0; i < 100 && m.get(hand, rel) == RIFT_S_MOTION_OUT_OF_FOV; ++i) + m.step(hand, target, true); + CHECK(m.get(hand, rel) == RIFT_S_MOTION_NONE); + } +} + +TEST_CASE("Motion freshness is a covariance-qualified filter sample, independent of vision cadence") +{ + Motion m; + xrt_space_relation rel{}; + const xrt_vec3 start = {0.3f, 1.45f, -0.5f}; + for (unsigned i = 0; i < 100; ++i) { + m.step(1, start, true); + CHECK(m.get(1, rel) == RIFT_S_MOTION_NONE); + } + // The gate is strict; equality with the reliability limit cannot refresh the support. + for (double &v : m.sample.position_variance) + v = 0.001; + m.sample.timestamp_ns += 2 * MS; + rift_s_motion_update(m.ctx, 1, &m.sample, &m.head); + CHECK(m.get(1, rel) == RIFT_S_MOTION_OUT_OF_FOV); + const auto old = rel; + // Queries do not change the cache or another hand's model. + for (int i = 0; i < 10; ++i) { + CHECK(m.get(1, rel, 10 * MS) == RIFT_S_MOTION_OUT_OF_FOV); + CHECK(m.get(0, rel) == RIFT_S_MOTION_NONE); + } + REQUIRE(m.get(1, rel) == RIFT_S_MOTION_OUT_OF_FOV); + check_vec(rel.pose.position, old.pose.position); +} + +TEST_CASE("Near-head loss follows wrist rotation and expires only after spatial convergence") +{ + Motion m; + xrt_space_relation rel{}; + const xrt_vec3 start = {0.1f, 1.6f, -0.05f}; + m.step(0, start, true); + m.step(0, start, false); + REQUIRE(m.get(0, rel) == RIFT_S_MOTION_NEAR_HEAD); + check_vec(rel.pose.position, start); + const xrt_vec3 up = {0, 1, 0}; + math_quat_from_angle_vector(0.5f, &up, &m.sample.pose.orientation); + m.step(0, {10, -30, 4}, false, 600 * MS); + REQUIRE(m.get(0, rel) == RIFT_S_MOTION_NEAR_HEAD); + CHECK(m_vec3_len(m_vec3_sub(rel.pose.position, start)) > 0.05f); + CHECK(rel.pose.position.y > 1.5f); +} + +TEST_CASE("At-rest wins priority until measured motion releases its latch") +{ + Motion m; + xrt_space_relation rel{}; + const xrt_vec3 start = {0.9f, 1.6f, -0.1f}; + for (int i = 0; i < 60; ++i) + m.step(0, start, true); + REQUIRE(m.get(0, rel) == RIFT_S_MOTION_AT_REST); + m.step(0, {40, -20, 30}, false, 700 * MS); + REQUIRE(m.get(0, rel) == RIFT_S_MOTION_AT_REST); + check_vec(rel.pose.position, start); + check_vec(rel.linear_velocity, {0, 0, 0}); + // Lower models were reset by the latch and cannot publish an old estimate on release. + m.sample.angular_velocity = {1, 0, 0}; + m.step(0, start, false); + CHECK(m.get(0, rel) == RIFT_S_MOTION_NONE); + m.step(0, start, true); + m.step(0, start, false); + CHECK(m.get(0, rel) == RIFT_S_MOTION_OUT_OF_FOV); +} + +TEST_CASE("Correlated close hands attach to the observed hand and preserve the attachment on handover") +{ + for (unsigned hand : {0u, 1u}) { + Motion m; + xrt_space_relation rel{}; + const xrt_vec3 p = {0.2f, 1.4f, -0.5f}, q = {0.3f, 1.4f, -0.5f}; + for (int i = 0; i < 60; ++i) { + const float a = i % 2 ? 0.3f : -0.3f; + m.sample.linear_acceleration = {a, 2 * a, 3 * a}; + m.step(1 - hand, q, true); + m.step(hand, p, true); + } + for (int i = 0; i < 16; ++i) { + const float a = i % 2 ? 0.3f : -0.3f; + m.sample.linear_acceleration = {a, 2 * a, 3 * a}; + m.step(1 - hand, q, true); + m.step(hand, p, false); + } + REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_HAND_OVER_HAND); + check_vec(rel.pose.position, p); + m.step(1 - hand, {q.x + 0.1f, q.y + 0.05f, q.z}, true); + m.step(hand, p, false); + REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_HAND_OVER_HAND); + check_vec(rel.pose.position, {p.x + 0.1f, p.y + 0.05f, p.z}); + const auto before = rel.pose.position; + // Opposite acceleration correlations switch the support, without resetting position. + for (int i = 0; i < 60; ++i) { + float a = i % 2 ? 0.3f : -0.3f; + m.sample.linear_acceleration = {-a, -2 * a, -3 * a}; + m.step(1 - hand, {q.x + 0.1f, q.y + 0.05f, q.z}, true); + m.sample.linear_acceleration = {a, 2 * a, 3 * a}; + m.step(hand, p, false); + } + REQUIRE(m.get(hand, rel) == RIFT_S_MOTION_HAND_OVER_HAND); + check_vec(rel.pose.position, before); + rift_s_motion_reset_hand(m.ctx, hand); + CHECK(m.get(hand, rel) == RIFT_S_MOTION_NONE); + } +} + +TEST_CASE("Model prediction uses body angular rate and queries cannot seed an unobserved hand") +{ + Motion m; + xrt_space_relation rel{}; + m.sample.pose.orientation = {0, 0, 0, 1}; + m.step(0, {0.1f, 1.6f, -0.1f}, false); + CHECK(m.get(0, rel) == RIFT_S_MOTION_NONE); + const xrt_vec3 axis = {1, 0, 0}; + math_quat_from_angle_vector(0.7f, &axis, &m.sample.pose.orientation); + m.sample.angular_velocity = {0, 1, 0}; + m.step(0, {0.1f, 1.6f, -0.1f}, true); + m.step(0, {0.1f, 1.6f, -0.1f}, false); + REQUIRE(m.get(0, rel, 10 * MS) == RIFT_S_MOTION_NEAR_HEAD); + xrt_quat expected; + math_quat_integrate_velocity(&m.sample.pose.orientation, &m.sample.angular_velocity, 0.01f, &expected); + CHECK(rel.pose.orientation.x == Catch::Approx(expected.x).margin(1e-6)); + CHECK(rel.pose.orientation.y == Catch::Approx(expected.y).margin(1e-6)); + CHECK(rel.pose.orientation.z == Catch::Approx(expected.z).margin(1e-6)); + CHECK(rel.pose.orientation.w == Catch::Approx(expected.w).margin(1e-6)); + xrt_vec3 world_rate; + math_quat_rotate_vec3(&m.sample.pose.orientation, &m.sample.angular_velocity, &world_rate); + check_vec(rel.angular_velocity, world_rate); + CHECK(m.get(1, rel) == RIFT_S_MOTION_NONE); +} diff --git a/tests/tests_rift_s_position_filter.cpp b/tests/tests_rift_s_position_filter.cpp index cf7206571..1b4362d4e 100644 --- a/tests/tests_rift_s_position_filter.cpp +++ b/tests/tests_rift_s_position_filter.cpp @@ -29,7 +29,7 @@ position_at(const rift_s_position_filter &f, timepoint_ns t) return p; } -TEST_CASE("Rift S loss ladder matches the Windows defaults") +TEST_CASE("Rift S configured loss durations retain independent position and velocity gates") { time_duration_ns p, v; rift_s_tracking_loss_durations(0, 0, &p, &v); @@ -114,50 +114,27 @@ TEST_CASE("Rift S position filter follows motion between delayed optical fixes") CHECK(f->gated == 0); } -TEST_CASE("Rift S position filter coasts with bounded drift, holds when frozen and resets on reacquire") +TEST_CASE("Rift S position propagation preserves measured motion outside the camera view") { auto f = make_filter(); - const xrt_vec3 zero = {0, 0, 0}; const xrt_vec3 here = {0.3f, 1.0f, -0.4f}; - timepoint_ns t = MS; - for (; t < 1000 * MS; t += IMU_STEP) { - rift_s_position_filter_imu(f.get(), t, &zero); - if ((t / IMU_STEP) % 16 == 0) { - rift_s_position_filter_fix(f.get(), t, &here, &OPTICAL_VARIANCE, 0.15f); - } - } - // Optical loss with a 0.5 m/s^2 uncorrected acceleration error: coast for the 0.5 s ladder step. - f->coasting = true; - const xrt_vec3 error = {0.5f, 0, 0}; - for (timepoint_ns end = t + 500 * MS; t < end; t += IMU_STEP) { - rift_s_position_filter_imu(f.get(), t, &error); - } - xrt_vec3 coasted = {(float)f->state.x[0][0], (float)f->state.x[1][0], (float)f->state.x[2][0]}; - // The 500 ms bridge with decaying velocity: a(tau t - tau^2 (1 - e^-t/tau)) = 4.6 cm. - CHECK(fabsf(coasted.x - here.x) < 0.05f); - CHECK(fabsf(coasted.x - here.x) > 0.0f); - - rift_s_position_filter_freeze(f.get()); - for (timepoint_ns end = t + 2000 * MS; t < end; t += IMU_STEP) { - rift_s_position_filter_imu(f.get(), t, &error); - } - xrt_vec3 held = position_at(*f, t + 50 * MS); - CHECK(held.x == Catch::Approx(coasted.x).margin(1e-6)); + REQUIRE(rift_s_position_filter_fix(f.get(), MS, &here, &OPTICAL_VARIANCE, 0.15f) == RIFT_S_POSITION_FIX_RESET); + // A hand accelerated at 1 m/s^2 for a second travels 0.5 m. Optical loss does not + // introduce an arbitrary displacement clamp, velocity damping, or reset maturity. + const xrt_vec3 accel = {1, 0, 0}; + for (timepoint_ns t = MS + IMU_STEP; t <= 1001 * MS; t += IMU_STEP) + rift_s_position_filter_imu(f.get(), t, &accel); xrt_vec3 p, v; - rift_s_position_filter_predict(f.get(), t, 100 * MS, &p, &v); - CHECK(v.x == 0.0f); - - // Reacquire somewhere else: three consistent fixes re-initialize. - const xrt_vec3 there = {0.6f, 1.1f, -0.2f}; - CHECK(rift_s_position_filter_fix(f.get(), t - 96 * MS, &there, &OPTICAL_VARIANCE, 0.15f) == - RIFT_S_POSITION_FIX_PENDING); - CHECK(rift_s_position_filter_fix(f.get(), t - 63 * MS, &there, &OPTICAL_VARIANCE, 0.15f) == - RIFT_S_POSITION_FIX_PENDING); - CHECK(f->frozen); - CHECK(rift_s_position_filter_fix(f.get(), t - 30 * MS, &there, &OPTICAL_VARIANCE, 0.15f) == - RIFT_S_POSITION_FIX_RESET); - CHECK_FALSE(f->frozen); - CHECK(position_at(*f, t).x == Catch::Approx(there.x).margin(0.01)); + rift_s_position_filter_predict(f.get(), 1001 * MS, 100 * MS, &p, &v); + CHECK(p.x == Catch::Approx(here.x + 0.5).margin(1e-6)); + CHECK(v.x == Catch::Approx(1).margin(1e-6)); + CHECK(f->track_updates == 1); + CHECK_FALSE(f->suspended); + // A clock discontinuity is separate: suspend propagation until a new accepted track. + rift_s_position_filter_suspend(f.get()); + rift_s_position_filter_imu(f.get(), 1003 * MS, &accel); + CHECK(position_at(*f, 1003 * MS).x == Catch::Approx(p.x).margin(1e-6)); + CHECK(f->state.x[0][1] == 0); } TEST_CASE("Rift S position filter resets on a confirmed large innovation and rejects fixes older than its history") @@ -227,7 +204,7 @@ TEST_CASE("Rift S position filter stays close to optical under a time-varying ac // r13 worn log, left hand behind the back: after a long track was held, single wrong fits ~1 m // away re-initialized the filter, and coasting on their velocity drifted it further. -TEST_CASE("Rift S held position ignores isolated wrong fits and needs a consistent reacquire") +TEST_CASE("Rift S suspended filter ignores isolated wrong fits and needs a consistent reacquire") { auto f = make_filter(); const xrt_vec3 zero = {0, 0, 0}; @@ -239,7 +216,7 @@ TEST_CASE("Rift S held position ignores isolated wrong fits and needs a consiste rift_s_position_filter_fix(f.get(), t - 20 * MS, &here, &OPTICAL_VARIANCE, 0.15f); } } - rift_s_position_filter_freeze(f.get()); + rift_s_position_filter_suspend(f.get()); // Scattered single fits, each one frame apart, never two in agreement. const xrt_vec3 wrong[] = {{-0.99f, 0.29f, 0.40f}, {-1.02f, -0.18f, 0.71f}, @@ -255,7 +232,7 @@ TEST_CASE("Rift S held position ignores isolated wrong fits and needs a consiste CHECK(p.x == Catch::Approx(here.x).margin(1e-3)); CHECK(p.z == Catch::Approx(here.z).margin(1e-3)); } - CHECK(f->frozen); + CHECK(f->suspended); // The hand comes back: three consistent fixes take over. const xrt_vec3 back = {0.25f, 0.05f, 0.1f}; enum rift_s_position_fix_result r = RIFT_S_POSITION_FIX_PENDING; @@ -269,60 +246,22 @@ TEST_CASE("Rift S held position ignores isolated wrong fits and needs a consiste CHECK(position_at(*f, t).x == Catch::Approx(back.x + 0.02f).margin(0.01)); } -TEST_CASE("Rift S young track holds instead of coasting on velocity from a few fixes") +TEST_CASE("Replacement fixes confirm throughout the Constellation capture cadence") { - auto f = make_filter(); - const xrt_vec3 zero = {0, 0, 0}; - timepoint_ns t = MS; - // Two fixes 33 ms apart and 3 cm apart (a wrong pair implies ~1 m/s), then nothing. - const xrt_vec3 a = {0.07f, 0.17f, 0.59f}, b = {0.10f, 0.17f, 0.59f}; - rift_s_position_filter_fix(f.get(), t, &a, &OPTICAL_VARIANCE, 0.15f); - for (timepoint_ns end = t + 33 * MS; t < end; t += IMU_STEP) - rift_s_position_filter_imu(f.get(), t, &zero); - rift_s_position_filter_fix(f.get(), t, &b, &OPTICAL_VARIANCE, 0.15f); - CHECK(f->track_updates == 2); - const double fused = f->state.x[0][0]; - f->coasting = true; - for (timepoint_ns end = t + 250 * MS; t < end; t += IMU_STEP) - rift_s_position_filter_imu(f.get(), t, &zero); - CHECK(f->state.x[0][0] == Catch::Approx(fused).margin(1e-6)); - CHECK(f->state.x[0][1] == 0.0); -} - -TEST_CASE("Optical loss bounds IMU displacement and freezes after the bridge") -{ - auto f = make_filter(); - xrt_vec3 here = {0.5f, 1.2f, -0.9f}, error = {-20, 0, 0}; - REQUIRE(rift_s_position_filter_fix(f.get(), MS, &here, &OPTICAL_VARIANCE, 0.15f) == RIFT_S_POSITION_FIX_RESET); - f->track_updates = 30; - for (timepoint_ns t = 3 * MS; t <= 1000 * MS; t += IMU_STEP) { - f->coasting = t > 121 * MS; - rift_s_position_filter_imu(f.get(), t, &error); - if (t > 121 * MS) - CHECK(fabs(position_at(*f, t).x - here.x) <= 0.101); + for (const time_duration_ns gap : {240 * MS, 280 * MS, 300 * MS, 301 * MS}) { + CAPTURE(gap); + auto f = make_filter(); + const xrt_vec3 here = {0, 1, -0.5f}, replacement = {0.5f, 1, -0.5f}, zero = {}; + timepoint_ns t = 1000 * MS; + REQUIRE(rift_s_position_filter_fix(f.get(), t, &here, &OPTICAL_VARIANCE, 0.15f) == + RIFT_S_POSITION_FIX_RESET); + for (unsigned i = 0; i < RIFT_S_POSITION_FILTER_CONFIRM_FIXES; ++i) { + t += gap; + rift_s_position_filter_imu(f.get(), t, &zero); + const auto result = rift_s_position_filter_fix(f.get(), t, &replacement, &OPTICAL_VARIANCE, 0.15f); + CHECK(result == (i + 1 == RIFT_S_POSITION_FILTER_CONFIRM_FIXES && + gap <= T_CONSTELLATION_TRACK_MAX_GAP_NS + ? RIFT_S_POSITION_FIX_RESET : RIFT_S_POSITION_FIX_PENDING)); + } } - CHECK(f->frozen); - xrt_vec3 held = position_at(*f, 1000 * MS); - for (timepoint_ns t = 1001 * MS; t < 2000 * MS; t += IMU_STEP) - rift_s_position_filter_imu(f.get(), t, &error); - CHECK(position_at(*f, 2000 * MS).x == held.x); -} - -TEST_CASE("Optical-loss queries bound prediction during a radio gap") -{ - auto f = make_filter(); - xrt_vec3 here = {0.5f, 1.2f, -0.9f}, zero = {0, 0, 0}; - REQUIRE(rift_s_position_filter_fix(f.get(), MS, &here, &OPTICAL_VARIANCE, 0.15f) == RIFT_S_POSITION_FIX_RESET); - f->track_updates = 30; - rift_s_position_filter_imu(f.get(), 100 * MS, &zero); - f->state.x[0][0] = here.x - 0.09; - f->state.x[0][1] = -4; - xrt_vec3 p, v; - rift_s_position_filter_predict(f.get(), 150 * MS, 50 * MS, &p, &v); - CHECK(p.x == Catch::Approx(here.x - 0.1).margin(1e-5)); - CHECK(v.x == 0); - // Past the bridge the held estimate is not extrapolated. - rift_s_position_filter_predict(f.get(), RIFT_S_POSITION_FILTER_BRIDGE_NS + 100 * MS, 50 * MS, &p, &v); - CHECK(p.x == Catch::Approx(here.x - 0.09).margin(1e-5)); - CHECK(v.x == 0); } diff --git a/tests/tests_rift_s_slam_guard.cpp b/tests/tests_rift_s_slam_guard.cpp index 8d66e0e04..b255686b2 100644 --- a/tests/tests_rift_s_slam_guard.cpp +++ b/tests/tests_rift_s_slam_guard.cpp @@ -1,6 +1,7 @@ // Copyright 2026, lbgos // SPDX-License-Identifier: BSL-1.0 #include "catch_amalgamated.hpp" +#include "../src/xrt/state_trackers/steamvr_drv/ovrd_world_universe.hpp" #include "rift_s/rift_s_slam_guard.h" #include "math/m_api.h" #include "math/m_vec3.h" @@ -364,20 +365,27 @@ TEST_CASE("SLAM cannot publish invalid quaternions or a gravity flip") CHECK(rift_s_slam_guard_pose_usable(&inverted, &inverted.orientation)); } -TEST_CASE("Saved boundary publishes only its recovered world frame after restart") +TEST_CASE("Failed room localization keeps HMD tracking in an uncalibrated universe") { Sim s; s.no_pose = {0, 1.6f, 0}; s.g.require_relocalization = true; xrt_pose raw = {yaw_quat(-0.8f), {0.4f, 0.2f, -0.3f}}, out; - rift_s_slam_guard_reason reason; - for (unsigned i = 0; i < 160; ++i) - CHECK_FALSE(s.step(true, raw, &out, &reason)); - CHECK(reason == RIFT_S_SLAM_GUARD_RELOCALIZING); - CHECK_FALSE(s.g.world_valid); + REQUIRE(s.settle(raw) > 0); + REQUIRE(s.g.world_valid); CHECK_FALSE(s.g.boundary_confirmed); + CHECK(rift_s_world_universe("relocalizing", s.ts, s.ts) == 3); CHECK(s.g.resets == 0); + xrt_vec3 first = s.g.good.position; + for (unsigned i = 0; i < 200; ++i) { + raw.position.x += 0.002f; + REQUIRE(s.step(true, raw, &out)); + } + CHECK(dist(first, out.position) == Catch::Approx(0.4f).margin(1e-5)); + CHECK_FALSE(s.g.boundary_confirmed); + CHECK(rift_s_slam_guard_query(&s.g, &raw, s.ts, &s.fusion, &s.no_pose, &out)); + // A later room match switches directly to the saved coordinates before enabling bounds. xrt_pose world = {yaw_quat(0.6f), {-0.7f, 1.3f, 0.3f}}; rift_s_slam_guard_set_world(&s.g, &world); REQUIRE(s.step(true, raw, &out)); @@ -387,42 +395,48 @@ TEST_CASE("Saved boundary publishes only its recovered world frame after restart CHECK(yaw_of(out.orientation) == Catch::Approx(-0.2f).margin(1e-6)); CHECK_FALSE(s.g.world_transition); CHECK(s.g.divergences == 0); + CHECK(rift_s_world_universe("localized", s.ts, s.ts) == 2); - // The replacement VIO gauge must be matched again, while the saved standing origin stays put. + // A backend reset loses the room binding, but cannot strand a stable replacement VIO. s.g.reset_pending = true; REQUIRE(rift_s_slam_guard_take_reset(&s.g, s.ts)); rift_s_slam_guard_suspend(&s.g, &s.fusion); raw = {yaw_quat(1.2f), {-0.3f, 0.1f, 0.2f}}; - for (unsigned i = 0; i < 160; ++i) - CHECK_FALSE(s.step(true, raw, &out, &reason)); - CHECK(reason == RIFT_S_SLAM_GUARD_RELOCALIZING); - CHECK_FALSE(s.g.world_valid); + REQUIRE(s.settle(raw) > 0); CHECK_FALSE(s.g.boundary_confirmed); + CHECK(rift_s_world_universe("relocalizing", s.ts, s.ts) == 3); xrt_pose inverse, recovered; math_pose_invert(&raw, &inverse); math_pose_transform(&expected, &inverse, &recovered); rift_s_slam_guard_set_world(&s.g, &recovered); REQUIRE(s.step(true, raw, &out)); CHECK(dist(out.position, expected.position) < 1e-6); - CHECK(yaw_of(out.orientation) == Catch::Approx(yaw_of(expected.orientation)).margin(1e-6)); CHECK_FALSE(s.g.world_transition); } -TEST_CASE("Restoring saved bounds revokes a room setup's unconfirmed frame") +TEST_CASE("Room calibration is enabled only by a fresh confirmed room or explicit setup") { + const int64_t now = 10000000000LL; + CHECK(rift_s_world_universe("setup", now, now) == 2); + CHECK(rift_s_world_universe("localized", now, now) == 2); + for (auto status : {"", "unanchored", "relocalizing", "save_failed"}) + CHECK(rift_s_world_universe(status, now, now) == 3); + CHECK(rift_s_world_universe("localized", 0, now) == 3); + CHECK(rift_s_world_universe("localized", now + 1, now) == 3); + CHECK(rift_s_world_universe("localized", now - 2000000001LL, now) == 3); + CHECK(rift_s_world_universe("localized", now - 1, now, now) == 3); + CHECK(rift_s_world_universe("localized", now, now + 1, now) == 2); + Sim s; s.no_pose = {0, 1.6f, 0}; xrt_pose raw = XRT_POSE_IDENTITY, out; REQUIRE(s.settle(raw) > 0); - REQUIRE(s.g.world_valid); s.g.require_relocalization = true; - CHECK_FALSE(rift_s_slam_guard_query(&s.g, &raw, s.ts, &s.fusion, &s.no_pose, &out)); - CHECK_FALSE(s.step(true, raw, &out)); - CHECK_FALSE(s.g.world_valid); - CHECK_FALSE(s.g.world_transition); + CHECK(rift_s_slam_guard_query(&s.g, &raw, s.ts, &s.fusion, &s.no_pose, &out)); + REQUIRE(s.step(true, raw, &out)); + CHECK_FALSE(s.g.boundary_confirmed); xrt_pose saved{yaw_quat(0.4f), {-0.2f, 1.6f, 0.3f}}; rift_s_slam_guard_set_world(&s.g, &saved); - REQUIRE(s.settle(raw) > 0); REQUIRE(s.step(true, raw, &out)); CHECK(dist(out.position, saved.position) < 1e-6); CHECK(yaw_of(out.orientation) == Catch::Approx(0.4f).margin(1e-6)); diff --git a/tests/tests_rift_s_tracking.cpp b/tests/tests_rift_s_tracking.cpp index ce75684ca..d7a88baed 100644 --- a/tests/tests_rift_s_tracking.cpp +++ b/tests/tests_rift_s_tracking.cpp @@ -16,6 +16,47 @@ #include #include #include +#include + +TEST_CASE("Rift S tracker stop joins the world consumer and drains borrowed camera frames") +{ + rift_s_tracker tracker{}; + os_mutex_init(&tracker.mutex); + os_mutex_init(&tracker.slam_mutex); + os_thread_helper_init(&tracker.world_thread); + tracker.world_thread_initialized = true; + tracker.ready_for_data = true; + struct Worker { + rift_s_tracker *tracker; + std::atomic exited{false}; + } worker{&tracker}; + auto consume = [](void *ptr) -> void * { + auto &worker = *static_cast(ptr); + auto &thread = worker.tracker->world_thread; + os_thread_helper_lock(&thread); + while (os_thread_helper_is_running_locked(&thread)) + os_thread_helper_wait_locked(&thread); + os_thread_helper_unlock(&thread); + worker.exited = true; + return nullptr; + }; + REQUIRE(os_thread_helper_start(&tracker.world_thread, consume, &worker) == 0); + xrt_frame frame{}; + frame.reference.count = 1; + frame.destroy = [](xrt_frame *) { FAIL("The camera owner must still own its buffer"); }; + xrt_frame_reference(&tracker.world_frames[0], &frame); + REQUIRE(frame.reference.count == 2); + rift_s_tracker_stop(&tracker); + CHECK(worker.exited); + CHECK_FALSE(tracker.ready_for_data); + CHECK(tracker.world_frames[0] == nullptr); + CHECK(frame.reference.count == 1); + // Teardown calls stop again after frame nodes have been destroyed. + rift_s_tracker_stop(&tracker); + os_thread_helper_destroy(&tracker.world_thread); + os_mutex_destroy(&tracker.slam_mutex); + os_mutex_destroy(&tracker.mutex); +} TEST_CASE("Rift S optical yaw takes ten percent of the shortest world yaw") { @@ -77,7 +118,7 @@ get_test_head_pose(xrt_device *, xrt_input_name, int64_t, xrt_space_relation *ou return XRT_SUCCESS; } -TEST_CASE("Rift S controller coordinates hold after loss and stay invalid before acquisition") +TEST_CASE("Rift S controller estimated coordinates survive loss of position confidence") { rift_s_hmd hmd = {}; hmd.base.get_tracked_pose = get_test_head_pose; @@ -114,8 +155,8 @@ TEST_CASE("Rift S controller coordinates hold after loss and stay invalid before CHECK((rel.relation_flags & XRT_SPACE_RELATION_ORIENTATION_VALID_BIT) != 0); CHECK((rel.relation_flags & XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT) != 0); CHECK(((rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0) == - (optical_ts != 0)); - // Optical confidence expires, but the held position remains usable. + (optical_ts == 1999000000)); + // Expired confidence does not discard the numeric estimate. const bool tracked = optical_ts == 1999000000; const bool held = optical_ts == 1000000000; CHECK(((rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) != 0) == tracked); @@ -125,12 +166,12 @@ TEST_CASE("Rift S controller coordinates hold after loss and stay invalid before vr::DriverPose_t pose = {}; pose.poseIsValid = true; pose.result = vr::TrackingResult_Running_OK; - apply_pose(&rel, &pose, XRT_SPACE_RELATION_POSITION_VALID_BIT, true); - CHECK(pose.poseIsValid == (tracked || held)); - CHECK(pose.result == (tracked || held ? vr::TrackingResult_Running_OK + apply_rift_s_touch_pose(&rel, &pose); + CHECK(pose.poseIsValid); + CHECK(pose.result == (tracked ? vr::TrackingResult_Running_OK : vr::TrackingResult_Running_OutOfRange)); - CHECK(pose.vecPosition[0] == Catch::Approx(tracked || held ? rel.pose.position.x : 0)); - CHECK(pose.vecPosition[2] == Catch::Approx(tracked || held ? rel.pose.position.z : 0)); + CHECK(pose.vecPosition[0] == Catch::Approx(rel.pose.position.x)); + CHECK(pose.vecPosition[2] == Catch::Approx(rel.pose.position.z)); CHECK(pose.qRotation.w == Catch::Approx(1)); // HMD call sites retain their original tracked-position requirement. vr::DriverPose_t head_pose = {}; @@ -176,7 +217,8 @@ TEST_CASE("Rift S mature track bridges a short optical gap and clears optical co }; CHECK(valid_after(30, 400 * U_TIME_1MS_IN_NS)); CHECK(valid_after(30, 490 * U_TIME_1MS_IN_NS)); - CHECK_FALSE(valid_after(30, 510 * U_TIME_1MS_IN_NS)); + CHECK(valid_after(30, 510 * U_TIME_1MS_IN_NS)); + CHECK_FALSE(valid_after(30, 3001LL * U_TIME_1MS_IN_NS)); CHECK_FALSE(valid_after(1, 300 * U_TIME_1MS_IN_NS)); os_mutex_destroy(&ctrl.mutex); os_mutex_destroy(&sys.dev_mutex); @@ -458,7 +500,7 @@ TEST_CASE("Rift S IMU clock re-anchors after a stale first timestamp instead of CHECK(ctrl.attitude_history_count == 0); CHECK(ctrl.clock_stable_samples == 0); CHECK_FALSE(ctrl.optical_heading_valid); - CHECK(ctrl.position_filter.frozen); + CHECK(ctrl.position_filter.suspended); // After the resync, ordinary late resends are still rejected without another resync. imu_report(base_us + 499 * 2000u, arrival + 1000000); CHECK(ctrl.diagnostics.clock_resyncs == 1); @@ -1166,95 +1208,6 @@ check_vec(xrt_vec3 actual, xrt_vec3 expected) } } // namespace -TEST_CASE("Rift S loss hold and reacquire keep the published hand valid and continuous") -{ - const timepoint_ns ms = U_TIME_1MS_IN_NS; - const xrt_vec3 here = {0.3f, 1.4f, -0.4f}, error = {4, -6, 2}; - for (bool right : {false, true}) { - PublishedPoseController fixture(right); - auto &ctrl = fixture.ctrl; - const timepoint_ns start = os_monotonic_get_ns() - 2000 * ms; - fixture.measured(XRT_QUAT_IDENTITY, here, start); - ctrl.track_vision_updates = ctrl.track_reliable_updates = ctrl.position_filter.track_updates = 30; - // Optical loss while the IMU keeps rotating the wrist. Its acceleration error must - // never move the IMU origin farther than the existing 10 cm coast bound. - for (timepoint_ns t = start + 2 * ms; t <= start + 2000 * ms; t += 2 * ms) { - const xrt_vec3 up = {0, 1, 0}; - math_quat_from_angle_vector(float(t - start) / U_TIME_1S_IN_NS, &up, &ctrl.pose.orientation); - ctrl.fusion.rot = ctrl.pose.orientation; - ctrl.last_imu_capture_ns = t; - REQUIRE(rift_s_controller_record_attitude(&ctrl, t, t)); - ctrl.position_filter.coasting = t - start > 120 * ms; - rift_s_position_filter_imu(&ctrl.position_filter, t, &error); - for (auto input : {XRT_INPUT_TOUCH_AIM_POSE, XRT_INPUT_TOUCH_GRIP_POSE}) { - auto rel = fixture.query(input, t); - REQUIRE((rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0); - if (t - start > 500 * ms) { - CHECK_FALSE((rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT) != - 0); - CHECK_FALSE( - (rel.relation_flags & XRT_SPACE_RELATION_LINEAR_VELOCITY_VALID_BIT) != 0); - } - const auto &offset = - input == XRT_INPUT_TOUCH_AIM_POSE ? ctrl.P_device_aim : ctrl.P_device_grip; - xrt_pose imu_device_offset, expected; - math_pose_transform(&ctrl.P_imu_device, &offset, &imu_device_offset); - const xrt_pose anchor = {ctrl.pose.orientation, here}; - math_pose_transform(&anchor, &imu_device_offset, &expected); - CHECK(m_vec3_len(m_vec3_sub(rel.pose.position, expected.position)) <= 0.10001f); - CHECK(rel.pose.orientation.y == Catch::Approx(expected.orientation.y).margin(2e-6)); -#ifdef TEST_RIFT_S_STEAMVR_POSE - const auto model_offset = - rift_s_touch_grip_to_model(right ? XRT_HAND_RIGHT : XRT_HAND_LEFT); - if (input == XRT_INPUT_TOUCH_GRIP_POSE) - math_pose_transform(&rel.pose, &model_offset, &rel.pose); - vr::DriverPose_t pose{}; - pose.poseIsValid = true; - pose.result = vr::TrackingResult_Running_OK; - apply_pose(&rel, &pose, XRT_SPACE_RELATION_POSITION_VALID_BIT, true); - REQUIRE(pose.poseIsValid); - CHECK(pose.result == vr::TrackingResult_Running_OK); - CHECK(pose.vecPosition[1] > 1.0); -#endif - } - } - REQUIRE(ctrl.position_filter.frozen); - // The first two fits remain candidates. The third resumes the unchanged optical - // filter, while only the displayed translation blends out over 100 ms. - const auto held = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, os_monotonic_get_ns()); - t_constellation_pose_observation obs{}; - obs.pose = {ctrl.pose.orientation, {-0.2f, 1.5f, -0.5f}}; - obs.position_covariance[0] = obs.position_covariance[4] = obs.position_covariance[8] = 1e-5f; - for (int i = 0; i < 3; ++i) { - obs.capture_ns = start + (1894 + 33 * i) * ms; - rift_s_controller_push_pose_observation(&ctrl.base, &obs); - CHECK(ctrl.position_filter.frozen == (i < 2)); - const auto rel = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, os_monotonic_get_ns()); - CHECK(m_vec3_len(m_vec3_sub(rel.pose.position, held.pose.position)) < 0.01f); - } - REQUIRE(ctrl.diagnostics.optical_accepted == 1); - const timepoint_ns recovered = os_monotonic_get_ns(); - auto previous = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, recovered); - for (int i = 1; i <= 12; ++i) { - auto rel = fixture.query(XRT_INPUT_TOUCH_GRIP_POSE, recovered + i * 10 * ms); - CHECK(m_vec3_len(m_vec3_sub(rel.pose.position, previous.pose.position)) < 0.09f); - REQUIRE((rel.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0); - previous = rel; - } - // At the end the presentation offset is exactly gone, including future queries. - xrt_vec3 p, v; - rift_s_position_filter_predict(&ctrl.position_filter, recovered + 120 * ms, 100 * ms, &p, &v); - xrt_pose imu_device_offset, expected; - math_pose_transform(&ctrl.P_imu_device, &ctrl.P_device_grip, &imu_device_offset); - const xrt_pose target = {ctrl.pose.orientation, p}; - math_pose_transform(&target, &imu_device_offset, &expected); - check_vec(previous.pose.position, expected.position); - // Presentation queries never replace the capture-time optical association prior. - const auto prior = fixture.query(XRT_INPUT_GENERIC_TRACKER_POSE, obs.capture_ns); - check_vec(prior.pose.position, obs.pose.position); - } -} - TEST_CASE("Rift S grip stays at its center during rotation for either hand") { const xrt_vec3 center = {0.3f, 1.2f, -0.5f}; @@ -1722,6 +1675,66 @@ TEST_CASE("HMD capture-time IMU attitude predicts with a world-space gyro") } #ifdef TEST_RIFT_S_STEAMVR_POSE +TEST_CASE("Rift S motion estimates pass through production grip/model offsets without changing optical association") +{ + for (bool left:{true,false}) { + rift_s_hmd hmd{}; + hmd.base.get_tracked_pose=get_test_head_pose; + rift_s_system sys{}; + sys.hmd=&hmd; + sys.motion=rift_s_motion_create(); + REQUIRE(sys.motion != nullptr); + rift_s_controller ctrl{}; + ctrl.sys=&sys; + ctrl.base.device_type=left ? XRT_DEVICE_TYPE_LEFT_HAND_CONTROLLER : XRT_DEVICE_TYPE_RIGHT_HAND_CONTROLLER; + ctrl.gravity_initialized=true; + ctrl.pose.orientation=XRT_QUAT_IDENTITY; + ctrl.P_imu_device={XRT_QUAT_IDENTITY,{0.015f,0.02f,-0.01f}}; + rift_s_controller_pose_frames(left,&ctrl.P_device_aim,&ctrl.P_device_grip); + os_mutex_init(&ctrl.mutex); + const timepoint_ns t=1000000000; + const xrt_vec3 position={left ? 0.7f : 1.3f,1.1f,2.5f}, variance={1e-5f,1e-5f,1e-5f}; + rift_s_position_filter_reset(&ctrl.position_filter); + rift_s_position_filter_fix(&ctrl.position_filter,t,&position,&variance,0.15f); + ctrl.last_fix_ns=ctrl.last_imu_capture_ns=t; + ctrl.track_vision_updates=3; + ctrl.last_tracked_pose={XRT_QUAT_IDENTITY,position}; + ctrl.last_tracked_pose_ts=t; + ctrl.attitude_history_count=2; + ctrl.attitude_history[0]={t-2000000,t-2000000,t-2000000,XRT_QUAT_IDENTITY,{0,0,0},true}; + ctrl.attitude_history[1]={t,t,t,XRT_QUAT_IDENTITY,{0,0,0},true}; + rift_s_controller_update_motion(&ctrl,t); + xrt_pose imu={XRT_QUAT_IDENTITY,position}, device, expected; + math_pose_transform(&imu,&ctrl.P_imu_device,&device); + math_pose_transform(&device,&ctrl.P_device_grip,&expected); + xrt_space_relation grip{}; + REQUIRE(rift_s_controller_get_tracked_pose(&ctrl.base,XRT_INPUT_TOUCH_GRIP_POSE,t,&grip)==XRT_SUCCESS); + check_vec(grip.pose.position,expected.position); + CHECK(grip.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT); + const xrt_vec3 zero={0,0,0}; + rift_s_position_filter_imu(&ctrl.position_filter,t+2000000,&zero); + ctrl.last_fix_ns=t-U_TIME_1S_IN_NS; + rift_s_controller_update_motion(&ctrl,t+2000000); + REQUIRE(rift_s_controller_get_tracked_pose(&ctrl.base,XRT_INPUT_TOUCH_GRIP_POSE,t+2000000,&grip)==XRT_SUCCESS); + check_vec(grip.pose.position,expected.position); + CHECK_FALSE(grip.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT); + xrt_space_relation prior{}; + REQUIRE(rift_s_controller_get_tracked_pose(&ctrl.base,XRT_INPUT_GENERIC_TRACKER_POSE,t+2000000,&prior)==XRT_SUCCESS); + check_vec(prior.pose.position,position); + CHECK(prior.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT); + const auto grip_model=rift_s_touch_grip_to_model(left ? XRT_HAND_LEFT : XRT_HAND_RIGHT); + const xrt_pose origin=XRT_POSE_IDENTITY; + transform_rift_s_touch_relation(&grip,&grip_model,&origin); + vr::DriverPose_t pose{}; + apply_rift_s_touch_pose(&grip,&pose); + CHECK(pose.poseIsValid); + CHECK(pose.result==vr::TrackingResult_Running_OutOfRange); + CHECK(pose.vecPosition[1]>1.0); + rift_s_motion_destroy(&sys.motion); + os_mutex_destroy(&ctrl.mutex); + } +} + TEST_CASE("Rift S Touch model and world transforms preserve estimated coordinates and confidence") { for (bool position_valid : {false, true}) { @@ -1807,6 +1820,36 @@ TEST_CASE("Rift S Touch OpenVR adapter rejects a nonfinite estimate") CHECK(pose.vecPosition[0] == 0); } +TEST_CASE("Rift S HMD fallback remains usable during relocalization without claiming tracked position") +{ + xrt_space_relation rel=XRT_SPACE_RELATION_ZERO; + rel.pose={XRT_QUAT_IDENTITY,{0.2f,1.6f,-0.1f}}; + rel.relation_flags=static_cast(XRT_SPACE_RELATION_POSITION_VALID_BIT | + XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); + vr::DriverPose_t pose{}; + pose.poseIsValid=true; + pose.result=vr::TrackingResult_Running_OK; + apply_head_pose(&rel,&pose,true); + CHECK(pose.poseIsValid); + CHECK(pose.result==vr::TrackingResult_Running_OutOfRange); + CHECK(pose.vecPosition[1]==Catch::Approx(1.6f)); + CHECK_FALSE(rel.relation_flags & XRT_SPACE_RELATION_POSITION_TRACKED_BIT); + apply_head_pose(&rel,&pose,false); + CHECK_FALSE(pose.poseIsValid); + // Standby keeps the finite held pose usable, while still reporting OutOfRange. + rel.relation_flags=static_cast(rel.relation_flags & ~XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); + pose.poseIsValid=true; + apply_head_pose(&rel,&pose,true); + CHECK(pose.poseIsValid); + CHECK(pose.vecPosition[1]==Catch::Approx(1.6f)); + CHECK(pose.result==vr::TrackingResult_Running_OutOfRange); + rel.relation_flags=static_cast(XRT_SPACE_RELATION_ORIENTATION_VALID_BIT | + XRT_SPACE_RELATION_ORIENTATION_TRACKED_BIT); + pose.poseIsValid=true; + apply_head_pose(&rel,&pose,true); + CHECK_FALSE(pose.poseIsValid); +} + TEST_CASE("SteamVR held pose clears velocity retained from the last tracked pose") { xrt_space_relation rel = XRT_SPACE_RELATION_ZERO; @@ -1903,7 +1946,7 @@ TEST_CASE("Rift S unconfirmed position candidates cannot rotate or replace the a obs.inliers = obs.distinct_leds = 6; obs.position_covariance[0] = obs.position_covariance[4] = obs.position_covariance[8] = 1e-6f; obs.hypothesis = "test"; - rift_s_controller_push_pose_observation(&ctrl.base, &obs); + REQUIRE(rift_s_controller_push_pose_observation(&ctrl.base, &obs)); const xrt_pose trusted = ctrl.last_tracked_pose; const timepoint_ns trusted_ns = ctrl.last_tracked_pose_ts; const auto tilt_updates = ctrl.optical_tilt_updates; @@ -1912,7 +1955,7 @@ TEST_CASE("Rift S unconfirmed position candidates cannot rotate or replace the a math_quat_from_angle_vector(DEG_TO_RAD(30.f), &yaw_axis, &obs.pose.orientation); for (int i = 1; i <= 2; ++i) { obs.capture_ns = trusted_ns + i * 10000000LL; - rift_s_controller_push_pose_observation(&ctrl.base, &obs); + CHECK_FALSE(rift_s_controller_push_pose_observation(&ctrl.base, &obs)); CHECK(ctrl.last_tracked_pose_ts == trusted_ns); CHECK(ctrl.last_tracked_pose.position.x == trusted.position.x); CHECK(ctrl.fusion.rot.y == Catch::Approx(0).margin(1e-6)); @@ -1922,7 +1965,7 @@ TEST_CASE("Rift S unconfirmed position candidates cannot rotate or replace the a // A continuous fit at the trusted position clears the rejected candidate. obs.capture_ns += 10000000LL; obs.pose = trusted; - rift_s_controller_push_pose_observation(&ctrl.base, &obs); + CHECK(rift_s_controller_push_pose_observation(&ctrl.base, &obs)); CHECK(ctrl.position_filter.candidate_count == 0); CHECK(ctrl.last_tracked_pose_ts == obs.capture_ns); CHECK(ctrl.diagnostics.optical_accepted == 2); @@ -1931,6 +1974,53 @@ TEST_CASE("Rift S unconfirmed position candidates cannot rotate or replace the a os_mutex_destroy(&ctrl.mutex); } +TEST_CASE("Rift S confirmed optical reacquisition seeds an age-lost controller without a second search") +{ + rift_s_controller ctrl{}; + ctrl.gravity_initialized = true; + ctrl.device_type = RIFT_S_DEVICE_RIGHT_CONTROLLER; + ctrl.fusion.rot = ctrl.pose.orientation = XRT_QUAT_IDENTITY; + os_mutex_init(&ctrl.mutex); + rift_s_position_filter_reset(&ctrl.position_filter); + ctrl.position_filter.accel_noise_density = 0.31; + ctrl.position_filter.bias_random_walk = 0.002; + for (int i = 0; i < 100; ++i) + REQUIRE(rift_s_controller_record_attitude(&ctrl, 1000000000LL + i * 2000000LL, + 1000000000LL + i * 2000000LL)); + t_constellation_pose_observation obs{}; + obs.capture_ns = 1100000000LL; + obs.pose = XRT_POSE_IDENTITY; + obs.pose.position = {0.1f, 1.1f, -0.4f}; + obs.position_covariance[0] = obs.position_covariance[4] = obs.position_covariance[8] = 1e-6f; + obs.hypothesis = "confirmed_constellation"; + REQUIRE(rift_s_controller_push_pose_observation(&ctrl.base, &obs)); + const xrt_vec3 zero{}; + for (int i = 100; i <= 420; ++i) { + const timepoint_ns ns = 1000000000LL + i * 2000000LL; + rift_s_position_filter_imu(&ctrl.position_filter, ns, &zero); + REQUIRE(rift_s_controller_record_attitude(&ctrl, ns, ns)); + } + obs.capture_ns = 1800000000LL; + obs.pose.position.x += 1; + SECTION("Age-based optical loss accepts the already confirmed new track") { + REQUIRE(rift_s_controller_push_pose_observation(&ctrl.base, &obs)); + CHECK(ctrl.position_filter.track_updates == 1); + CHECK(ctrl.position_filter.history_count > 0); + CHECK(ctrl.position_filter.state.t == 1840000000LL); + CHECK(ctrl.position_filter.state.x[0][0] == Catch::Approx(obs.pose.position.x)); + CHECK(ctrl.position_filter.accel_noise_density == 0.31); + CHECK(ctrl.position_filter.bias_random_walk == 0.002); + CHECK(ctrl.diagnostics.optical_accepted == 2); + } + SECTION("A clock discontinuity retains the filter's confirmation gate") { + rift_s_position_filter_suspend(&ctrl.position_filter); + CHECK_FALSE(rift_s_controller_push_pose_observation(&ctrl.base, &obs)); + CHECK(ctrl.position_filter.suspended); + CHECK(ctrl.diagnostics.optical_accepted == 1); + } + os_mutex_destroy(&ctrl.mutex); +} + #ifdef TEST_RIFT_S_STEAMVR_POSE TEST_CASE("Rift S complete grip to SteamVR model chain preserves metric position in a rotated world") { @@ -2001,7 +2091,7 @@ TEST_CASE("Rift S controller does not integrate missing radio motion") CHECK(ctrl.fusion.rot.w == Catch::Approx(before.w)); CHECK(ctrl.attitude_history_count == 0); CHECK_FALSE(ctrl.optical_heading_valid); - CHECK(ctrl.position_filter.frozen); + CHECK(ctrl.position_filter.suspended); REQUIRE(rift_s_controller_record_attitude(&ctrl, 3000000000LL, 3000000000LL)); rift_s_attitude_sample sample{}; CHECK_FALSE(rift_s_controller_get_attitude(&ctrl, 2000000000LL, &sample)); diff --git a/tests/tests_rift_s_world_anchor.cpp b/tests/tests_rift_s_world_anchor.cpp index 83d495157..e2b6099c2 100644 --- a/tests/tests_rift_s_world_anchor.cpp +++ b/tests/tests_rift_s_world_anchor.cpp @@ -3,6 +3,7 @@ #include "catch_amalgamated.hpp" #include "rift_s_world_anchor.h" #include "rift_s_slam_guard.h" +#include "../src/xrt/state_trackers/steamvr_drv/ovrd_world_universe.hpp" #include #include #include @@ -505,7 +506,8 @@ TEST_CASE("Cross-session camera map replay", "[.camera-replay]") }; unsigned keyframes = 0, matched = 0, attempted = 0, confirmations = 0; int64_t start_ns = 0, confirmed_ns = 0, previous_ns = 0; - xrt_pose previous = XRT_POSE_IDENTITY, confirmed = XRT_POSE_IDENTITY, confirmed_head = XRT_POSE_IDENTITY; + xrt_pose previous = XRT_POSE_IDENTITY, confirmed = XRT_POSE_IDENTITY, confirmed_head = XRT_POSE_IDENTITY, + confirmed_raw = XRT_POSE_IDENTITY; double max_error = 0, max_vote_translation = 0, max_vote_angle = 0; for (const auto &[dir, record] : {std::pair{std::string(session_a), true}, std::pair{std::string(session_b), false}}) { @@ -618,6 +620,7 @@ TEST_CASE("Cross-session camera map replay", "[.camera-replay]") confirmed_ns = ts; confirmed = candidate; confirmed_head = result.world_from_imu; + confirmed_raw = raw; } std::printf( "Replay time_s=%.3f inliers=%u/%u error_px=%.3f votes=%u transform=%.4f,%.4f,%.4f\n", @@ -637,6 +640,29 @@ TEST_CASE("Cross-session camera map replay", "[.camera-replay]") if (!load_only) REQUIRE(keyframes > 0); REQUIRE(confirmed_ns > 0); + if (!load_only && std::strcmp(session_a, session_b) == 0) { + // Replaying the same exposures must recover their original gauge, not only + // an internally consistent but displaced room pose. + CHECK(cv::norm(translation(confirmed)) < 0.025); + cv::Vec3d delta; + cv::Rodrigues(rotation(confirmed.orientation), delta); + CHECK(cv::norm(delta) < M_PI / 180); + } + rift_s_slam_guard guard{}; + rift_s_slam_guard_init(&guard); + guard.require_relocalization = c.require_boundary; + CHECK(rift_s_world_universe("relocalizing", confirmed_ns, confirmed_ns) == 3); + rift_s_slam_guard_set_world(&guard, &confirmed); + REQUIRE(guard.world_valid); + REQUIRE(guard.boundary_confirmed); + CHECK_FALSE(guard.world_transition); + CHECK(rift_s_world_universe("localized", confirmed_ns, confirmed_ns) == 2); + xrt_pose published; + math_pose_transform(&guard.align, &confirmed_raw, &published); + CHECK(cv::norm(translation(published) - translation(confirmed_head)) < 0.0001); + std::printf("Replay publication helper universe=2 saved_world_error_m=%.6f\n", + cv::norm(translation(published) - translation(confirmed_head))); + } TEST_CASE("Native fisheye recovery tolerates subpixel peripheral feature noise") @@ -659,6 +685,27 @@ TEST_CASE("Native fisheye recovery tolerates subpixel peripheral feature noise") CHECK(result.inliers[1] >= 60); } +TEST_CASE("Fisheye PnP seed retains native-pixel support at peripheral bearings") +{ + auto c = calibration(); + xrt_pose expected{{0, 0.24740396f, 0, 0.96891242f}, {0.2f, 0.1f, -0.45f}}; + auto peripheral = observations(c, expected); + for (auto &eye : peripheral) { + eye.erase(std::remove_if(eye.begin(), eye.end(), [](const auto &o) { + return o.pixel.x > 200 && o.pixel.x < 450; + }), eye.end()); + for (unsigned i = 0; i < eye.size(); ++i) { + eye[i].pixel.x += float(1.4 * std::sin(i * 1.7)); + eye[i].pixel.y += float(1.4 * std::cos(i * 2.3)); + } + } + rift_s_world_anchor_result result{}; + REQUIRE(solve(c, peripheral, result)); + CHECK(result.supporting_cameras == 2); + CHECK(result.max_reprojection_px <= 2.5f); + CHECK(cv::norm(translation(result.world_from_imu) - translation(expected)) < 0.025); +} + TEST_CASE("Native reprojection drops a few wrong correspondences without losing the room") { auto c = calibration(); diff --git a/tests/tests_steamvr_roomsetup_guard.cpp b/tests/tests_steamvr_roomsetup_guard.cpp index 81df9c401..ea6617e2f 100644 --- a/tests/tests_steamvr_roomsetup_guard.cpp +++ b/tests/tests_steamvr_roomsetup_guard.cpp @@ -152,6 +152,10 @@ TEST_CASE("Saved floor sends a numeric detection and only seeds manual entry onc } } paths; roomsetup::SavedFloor floor{&messages, &paths}; + // An unlocalized room must not inject either the old plane or a manual-floor reset. + CHECK_FALSE(floor.publish(true, false)); + CHECK_FALSE(floor.publish(false, false)); + CHECK(messages.sent.empty()); REQUIRE(floor.publish()); REQUIRE(messages.sent.size() == 1); auto *result = cJSON_Parse(messages.sent[0].c_str()); @@ -166,6 +170,8 @@ TEST_CASE("Saved floor sends a numeric detection and only seeds manual entry onc REQUIRE(reset); CHECK(cJSON_GetNumberValue(cJSON_GetObjectItem(reset, "value_source")) == 2); cJSON_Delete(reset); + CHECK_FALSE(floor.publish(true, false)); + CHECK(messages.sent.size() == 3); } TEST_CASE("Boundary warning retries failed delivery and removal")