From 37ce53bbbcb801e6589244bddd0f0d9f91ef48be Mon Sep 17 00:00:00 2001 From: lbgos <285507185+lbgos@users.noreply.github.com> Date: Tue, 13 Oct 2026 04:45:39 +0300 Subject: [PATCH] Reconfirm saved bounds after tracking loss and verify drift at one hertz --- src/xrt/drivers/rift_s/rift_s_slam_guard.c | 24 ++++++++ src/xrt/drivers/rift_s/rift_s_slam_guard.h | 6 +- src/xrt/drivers/rift_s/rift_s_tracker.c | 68 +++++++++++++++------- tests/tests_rift_s_slam_guard.cpp | 47 +++++++++++++++ tests/tests_rift_s_world_anchor.cpp | 5 +- 5 files changed, 125 insertions(+), 25 deletions(-) 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 02f20897c..fb794055f 100644 --- a/src/xrt/drivers/rift_s/rift_s_slam_guard.c +++ b/src/xrt/drivers/rift_s/rift_s_slam_guard.c @@ -213,6 +213,10 @@ enter_diverged(struct rift_s_slam_guard *g, { g->state = RIFT_S_SLAM_GUARD_DIVERGED; g->stable_samples = 0; + // A held pose is still useful, but saved bounds need fresh visual confirmation + // after tracking loss. Do not publish a stale binding during recovery. + g->boundary_confirmed = false; + g->world_transition = false; // Continue the 3DoF orientation in the heading SLAM last showed. struct xrt_quat inv_fusion; @@ -234,11 +238,31 @@ enter_diverged(struct rift_s_slam_guard *g, void rift_s_slam_guard_suspend(struct rift_s_slam_guard *g, const struct xrt_quat *fusion_orientation) { + g->boundary_confirmed = false; + g->world_transition = false; if (g->state == RIFT_S_SLAM_GUARD_TRACKING) { enter_diverged(g, 0, fusion_orientation, false, 0.0f, 0.0f); } } +bool +rift_s_slam_guard_verify_world(struct rift_s_slam_guard *g, const struct xrt_pose *world_from_slam) +{ + float dot = fabsf(g->align.orientation.x * world_from_slam->orientation.x + + g->align.orientation.y * world_from_slam->orientation.y + + g->align.orientation.z * world_from_slam->orientation.z + + g->align.orientation.w * world_from_slam->orientation.w); + float angle = 2 * acosf(fminf(1, dot)); + bool agrees = pose_is_finite(world_from_slam) && + m_vec3_len(m_vec3_sub(g->align.position, world_from_slam->position)) < 0.04f && + angle < DEG_TO_RAD(3); + if (!agrees) { + g->boundary_confirmed = false; + g->world_transition = false; + } + return agrees; +} + //! Feeds a raw sample into the stability run used while acquiring or diverged. static enum rift_s_slam_guard_reason settle(struct rift_s_slam_guard *g, bool slam_valid, const struct xrt_pose *raw, int64_t ts_ns) 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 a303c54d7..55afdf291 100644 --- a/src/xrt/drivers/rift_s/rift_s_slam_guard.h +++ b/src/xrt/drivers/rift_s/rift_s_slam_guard.h @@ -52,7 +52,7 @@ struct rift_s_slam_guard { //! Saved world alignment is usable only until the backend changes its map. bool world_valid; - //! Saved-boundary confirmation is independent of temporary SLAM tracking. + //! Saved bounds require fresh visual confirmation after tracking is lost or suspended. bool boundary_confirmed; bool world_transition; struct xrt_pose transition_from, transition_to; @@ -166,6 +166,10 @@ rift_s_slam_guard_fallback_pose(const struct rift_s_slam_guard *g, void rift_s_slam_guard_suspend(struct rift_s_slam_guard *g, const struct xrt_quat *fusion_orientation); +//! Compare a fresh visual match with the current alignment; revoke saved bounds on disagreement. +bool +rift_s_slam_guard_verify_world(struct rift_s_slam_guard *g, const struct xrt_pose *world_from_slam); + //! Start the replacement backend's recovery interval without changing the held world pose. void rift_s_slam_guard_backend_started(struct rift_s_slam_guard *g, int64_t ts_ns); diff --git a/src/xrt/drivers/rift_s/rift_s_tracker.c b/src/xrt/drivers/rift_s/rift_s_tracker.c index 73fe4f6ad..fc034f421 100644 --- a/src/xrt/drivers/rift_s/rift_s_tracker.c +++ b/src/xrt/drivers/rift_s/rift_s_tracker.c @@ -664,6 +664,7 @@ rift_s_tracker_device_lost(struct rift_s_tracker *t) t->world_confirmations = 0; t->world_candidate_generation = t->slam_generation; t->world_candidate_ns = 0; + t->world_last_verify_ns = 0; os_mutex_unlock(&t->mutex); // Release backend-owned camera buffers immediately even while off-head. The // next fresh frame uses the existing restart path if recreation is unavailable. @@ -783,6 +784,8 @@ pause_slam_locked(struct rift_s_tracker *t) t->slam_restart_pending = t->tracking.slam_enabled && t->last_camera_arrival_ns != 0; // SLAM is realigned onto the held pose once it is consistent again. rift_s_slam_guard_suspend(&t->slam_guard, &t->pose.orientation); + t->world_confirmations = 0; + t->world_last_verify_ns = 0; RIFT_S_INFO("Head tracking paused (%s); holding last pose", t->standby ? "standby" : "camera timeout"); } @@ -801,6 +804,8 @@ rift_s_tracker_set_standby(struct rift_s_tracker *t, bool standby) os_mutex_lock(&t->mutex); if (standby != t->standby) { t->standby = standby; + t->world_last_verify_ns = 0; + t->world_confirmations = 0; if (standby) { pause_slam_locked(t); } else { @@ -943,6 +948,8 @@ rift_s_tracker_push_slam_frames(struct rift_s_tracker *t, bool restart = submit && t->tracking.slam_enabled && t->slam_restart_pending; if (restart) { t->slam_generation++; + t->world_confirmations = 0; + t->world_last_verify_ns = 0; t->slam_min_sample_ns = frame_mono_ns; t->slam_guard.world_valid = false; t->slam_guard.boundary_confirmed = false; @@ -1394,6 +1401,7 @@ world_read_command(struct rift_s_tracker *t) t->slam_guard.boundary_confirmed = false; t->slam_guard.require_relocalization = false; t->world_confirmations = 0; + t->world_last_verify_ns = 0; os_mutex_unlock(&t->mutex); RIFT_S_INFO("New room setup may establish a world; saved anchors retained until Accept"); } else if (strcmp(command, "save") == 0) { @@ -1416,6 +1424,8 @@ world_read_command(struct rift_s_tracker *t) if (!saved || !t->world_map_available) t->slam_guard.boundary_confirmed = false; t->slam_guard.world_transition = false; + t->world_confirmations = 0; + t->world_last_verify_ns = 0; os_mutex_unlock(&t->mutex); RIFT_S_INFO("Boundary world anchors saved=%d map_available=%d", saved, t->world_map_available); } else if (strcmp(command, "cancel") == 0) { @@ -1433,6 +1443,7 @@ world_read_command(struct rift_s_tracker *t) t->world_map_available = rift_s_world_anchor_has_map(t->world_anchor); t->slam_guard.require_relocalization = requires_relocalization; t->world_confirmations = 0; + t->world_last_verify_ns = 0; os_mutex_unlock(&t->mutex); } } @@ -1461,7 +1472,23 @@ rift_s_world_thread(void *ptr) timepoint_ns source_ns = 0; os_mutex_lock(&t->mutex); uint64_t generation = t->slam_generation; + bool skip = t->standby || + (!t->world_setup && t->slam_guard.boundary_confirmed && + t->slam_guard.state == RIFT_S_SLAM_GUARD_TRACKING && + !t->slam_guard.world_transition && !t->slam_restart_pending && + !t->feature_gate.lost && + capture_ns - t->world_last_verify_ns < U_TIME_1S_IN_NS); + if (!skip) + t->world_last_verify_ns = capture_ns; os_mutex_unlock(&t->mutex); + if (skip) { + for (int eye = 0; eye < RIFT_S_CAMERA_COUNT; eye++) + xrt_frame_reference(&frames[eye], NULL); + world_write_status(t); + os_nanosleep(20 * U_TIME_1MS_IN_NS); + os_thread_helper_lock(&t->world_thread); + continue; + } bool sample_ok = false; #ifdef XRT_FEATURE_SLAM // Wait for this exposure's SLAM sample, rather than triangulate against a prediction. The @@ -1485,7 +1512,8 @@ rift_s_world_thread(void *ptr) fusion = fusion_capture.pose.orientation; // A worn room is recognized from rare usable samples. Count the first failing gate so the // next session shows which capture/sample condition dominates instead of only usable=0. - bool settled = !t->standby && !t->slam_paused && !t->slam_restart_pending; + bool settled = !t->standby && !t->slam_paused && !t->slam_restart_pending && + !t->feature_gate.lost; bool gap_ok = source_ns >= capture_ns && source_ns - capture_ns <= 250 * U_TIME_1MS_IN_NS; bool flags_ok = (capture.relation_flags & XRT_SPACE_RELATION_POSITION_VALID_BIT) != 0; bool session_ok = capture_ns >= t->slam_min_sample_ns && generation == t->slam_generation; @@ -1510,22 +1538,13 @@ rift_s_world_thread(void *ptr) struct xrt_vec3 no_pose_position = rift_s_slam_guard_no_pose_position( t->base.tracking_origin ? t->base.tracking_origin->initial_offset.position.y : 0.0f); // Recording must pass the same displacement/gravity checks as a displayed pose. - bool record = valid && (t->world_setup || t->slam_guard.boundary_confirmed) && + // Saved landmarks must remain independent of VIO drift. Only explicit room setup + // changes the map; normal operation verifies against those saved landmarks. + bool record = valid && t->world_setup && !t->slam_guard.world_transition && rift_s_slam_guard_query(&t->slam_guard, &capture.pose, capture_ns, &fusion, &no_pose_position, &world_capture); - // Off-head the SLAM samples recognition needs are invalid by construction, so skip the - // matcher instead of burning the room-matching thread. Once localized, verify at a low - // rate; while searching, match every queued pair. - bool match_now = true; - if (t->standby) { - match_now = false; - } else if (!record && t->slam_guard.boundary_confirmed && !t->slam_guard.world_transition && - !t->world_setup) { - match_now = capture_ns - t->world_last_verify_ns >= 5LL * U_TIME_1S_IN_NS; - if (match_now) - t->world_last_verify_ns = capture_ns; - } + bool match_now = !t->standby; os_mutex_unlock(&t->mutex); if (!match_now) { // Frozen attempts/diagnostics show the skip; the shared loop tail still releases the @@ -1596,12 +1615,19 @@ rift_s_world_thread(void *ptr) struct xrt_vec3 up = {0, 1, 0}, world_up; math_quat_rotate_vec3(&candidate.orientation, &up, &world_up); os_mutex_lock(&t->mutex); - if (generation == t->slam_generation && !t->standby && !t->slam_restart_pending && + if (generation == t->slam_generation && !t->standby && !t->slam_paused && + !t->slam_restart_pending && !t->feature_gate.lost && world_up.y > 0.9961947f) { rift_s_slam_guard_yaw_of(&candidate.orientation, &candidate.orientation); struct xrt_vec3 rotated; math_quat_rotate_vec3(&candidate.orientation, &capture.pose.position, &rotated); candidate.position = m_vec3_sub(result.world_from_imu.position, rotated); + if (t->slam_guard.boundary_confirmed && + !rift_s_slam_guard_verify_world(&t->slam_guard, &candidate)) { + t->world_confirmations = 0; + t->world_last_verify_ns = 0; + RIFT_S_WARN("Saved room alignment changed; boundary unavailable until reconfirmed"); + } bool consistent = t->world_confirmations > 0 && t->world_candidate_generation == generation && capture_ns > t->world_candidate_ns && @@ -1622,16 +1648,16 @@ rift_s_world_thread(void *ptr) result.inliers[0], result.inliers[1], result.max_reprojection_px, candidate.position.x, candidate.position.y, candidate.position.z); } + } else { + t->world_confirmations = 0; } os_mutex_unlock(&t->mutex); } else if (!record) { - // Hold partial confirmations across invalid samples: usable SLAM samples are rare - // while searching, and one missed sample must not discard agreeing votes. The - // candidate still drops when stale or from an older SLAM session, so a transient - // wrong match cannot accumulate votes across minutes. Vote agreement (4 cm, 3 - // degrees, chained 3 s) and the 3-confirmation bar are unchanged. + // Missing samples can retain agreeing votes for 3 seconds. A usable sample + // rejecting the room, a bad pose or a paused/replaced session breaks the run. os_mutex_lock(&t->mutex); - if (t->world_candidate_generation != t->slam_generation || + if (valid || !settled || !session_ok || (sample_ok && flags_ok && !guard_ok) || + t->world_candidate_generation != t->slam_generation || capture_ns - t->world_candidate_ns > 3LL * U_TIME_1S_IN_NS) t->world_confirmations = 0; os_mutex_unlock(&t->mutex); diff --git a/tests/tests_rift_s_slam_guard.cpp b/tests/tests_rift_s_slam_guard.cpp index b255686b2..79b8ad908 100644 --- a/tests/tests_rift_s_slam_guard.cpp +++ b/tests/tests_rift_s_slam_guard.cpp @@ -505,3 +505,50 @@ TEST_CASE("A fresh backend gets a full recovery interval without moving the worl CHECK(s.resets == resets + 1); CHECK(dist(out.position, held.position) < 1e-6f); } + +TEST_CASE("Saved bounds are revoked on suspend or source tracking loss") +{ + Sim s; + xrt_pose raw = {yaw_quat(.2f), {.1f, .05f, -.2f}}, out; + REQUIRE(s.settle(raw) > 0); + xrt_pose saved = s.g.align; + rift_s_slam_guard_set_world(&s.g, &saved); + REQUIRE(s.g.boundary_confirmed); + const xrt_pose held = s.g.good; + SECTION("Short off-head interval") { rift_s_slam_guard_suspend(&s.g, &s.fusion); } + SECTION("Missing source pose") { CHECK_FALSE(s.step(false, raw, &out)); } + CHECK_FALSE(s.g.boundary_confirmed); + CHECK_FALSE(s.g.world_transition); + CHECK(dist(s.g.good.position, held.position) < 1e-6f); + REQUIRE(s.settle(raw) > 0); + CHECK_FALSE(s.g.boundary_confirmed); + rift_s_slam_guard_set_world(&s.g, &saved); + CHECK(s.g.boundary_confirmed); +} + +TEST_CASE("A fresh visual match detects saved boundary translation and yaw drift") +{ + Sim s; + xrt_pose raw = {yaw_quat(.2f), {.1f, .05f, -.2f}}; + REQUIRE(s.settle(raw) > 0); + const xrt_pose saved = s.g.align; + rift_s_slam_guard_set_world(&s.g, &saved); + REQUIRE(s.g.boundary_confirmed); + xrt_pose candidate = saved; + SECTION("Near alignment remains confirmed") { + candidate.position.x += .03f; + CHECK(rift_s_slam_guard_verify_world(&s.g, &candidate)); + CHECK(s.g.boundary_confirmed); + } + SECTION("Translation drift hides bounds without moving the held pose") { + candidate.position.x += .05f; + CHECK_FALSE(rift_s_slam_guard_verify_world(&s.g, &candidate)); + CHECK_FALSE(s.g.boundary_confirmed); + } + SECTION("Yaw drift hides bounds") { + candidate.orientation = yaw_quat(yaw_of(saved.orientation) + .06f); + CHECK_FALSE(rift_s_slam_guard_verify_world(&s.g, &candidate)); + CHECK_FALSE(s.g.boundary_confirmed); + } + CHECK(dist(s.g.align.position, saved.position) < 1e-6f); +} diff --git a/tests/tests_rift_s_world_anchor.cpp b/tests/tests_rift_s_world_anchor.cpp index fdb2bcaae..5ca49cb6b 100644 --- a/tests/tests_rift_s_world_anchor.cpp +++ b/tests/tests_rift_s_world_anchor.cpp @@ -585,9 +585,8 @@ TEST_CASE("Cross-session camera map replay", "[.camera-replay]") result.supporting_cameras, result.max_reprojection_px, result.relocalized, result.rejection ? result.rejection : "none"); if (!processed || !result.relocalized || !valid) { - // Mirror of the driver's hold: invalid samples keep agreeing votes; only a stale - // candidate drops them, so a transient wrong match cannot accumulate votes. - if (ts - previous_ns > 3000000000LL) + // Missing samples retain recent votes; a usable rejected match breaks the run. + if (valid || ts - previous_ns > 3000000000LL) confirmations = 0; continue; }