Bug #57 » 0002-fix-engine-recover-the-anchor-constellation-when-one.patch
| common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt | ||
|---|---|---|
|
pipelineTrace?.constellationBootstrapped(clock.now().microseconds, "edges<min", ids.size, edgeCount, 0, emptyList()); return 0
|
||
|
}
|
||
|
// Closed-form K4 cascade first. It needs EVERY inter-anchor edge (the 4th anchor is
|
||
|
// trilaterated against the other three), so one permanently-missing link — e.g. a TV
|
||
|
// between two anchors that blocks their UWB range (NLOS never ranges through it) — makes
|
||
|
// it return null. When that happens, don't strand the frame on the raw (garbage-z) AoA
|
||
|
// seed: fall back to refining that seed against the edges we DO have (see
|
||
|
// [refineConstellationFromPrior]) so the mesh still crosses quorum.
|
||
|
var viaFallback = false
|
||
|
val solved = AnchorMeshBootstrap.bootstrap(ids, interAnchorDistances, rootId)
|
||
|
?: refineConstellationFromPrior(ids, rootId)?.also { viaFallback = true }
|
||
|
?: run { pipelineTrace?.constellationBootstrapped(clock.now().microseconds, "solve-null", ids.size, edgeCount, 0, emptyList()); return 0 }
|
||
|
// ORIENT the distance-solved shape into the mesh +Z=up world frame — the one thing
|
||
| ... | ... | |
|
// Emit the ORIENTED positions so a capture verifies the levelling directly (raised anchor
|
||
|
// at +z, base at ~0). "placed" = levelled + headed; "placed-raw" = orientation not yet
|
||
|
// available (still the arbitrary gauge), which itself flags a missing-bearing condition.
|
||
|
val baseStatus = if (orientation != null) "placed" else "placed-raw"
|
||
|
pipelineTrace?.constellationBootstrapped(
|
||
|
clock.now().microseconds, if (orientation != null) "placed" else "placed-raw",
|
||
|
clock.now().microseconds, if (viaFallback) "$baseStatus-degen" else baseStatus,
|
||
|
ids.size, edgeCount, placed,
|
||
|
placedPositions.map { (id, p) -> MofePipelineTrace.RefPoint(id, p) },
|
||
|
)
|
||
|
return placed
|
||
|
}
|
||
|
/**
|
||
|
* Fallback for an INCOMPLETE inter-anchor distance graph. [AnchorMeshBootstrap]'s closed-form
|
||
|
* cascade needs the COMPLETE graph (every anchor pair ranged) to place four anchors, because
|
||
|
* the fourth is trilaterated against the other three; a permanently-missing link — e.g. a TV
|
||
|
* between two anchors, which NLOS UWB never ranges through — leaves it one edge short and it
|
||
|
* returns null. Rather than freeze the frame on the raw (garbage-z) AoA seed forever, refine
|
||
|
* that seed with [AnchorConstellationSolver]: it honours every AVAILABLE inter-anchor distance,
|
||
|
* escapes a coplanar AoA seed, and leans on the prior only for the one gauge/fold DOF the
|
||
|
* missing edge leaves free. Returns the refined positions, or null when there is no usable
|
||
|
* prior yet (too few anchors seeded) or the fit is too poor to trust.
|
||
|
*
|
||
|
* Caveat: the free DOF (the two anchors either side of the gap fold about the line through the
|
||
|
* anchors that DO range them both) is resolved only by the prior, so those two anchors' shared
|
||
|
* placement is no better than the AoA seed until the link is restored — but every
|
||
|
* well-constrained anchor is corrected and, crucially, the frame reaches quorum, which the
|
||
|
* missing edge otherwise blocks entirely.
|
||
|
*/
|
||
|
private fun refineConstellationFromPrior(ids: Set<DeviceId>, rootId: DeviceId): Map<DeviceId, Vector3D>? {
|
||
|
val known = frameManager.getAllReferencePoints().associate { it.id to it.position }
|
||
|
val prior = LinkedHashMap<DeviceId, Vector3D>()
|
||
|
for (id in ids) prior[id] = if (id == rootId) Vector3D.ZERO else (known[id] ?: return null)
|
||
|
if (prior.size < config.minAnchorsForFusion) return null
|
||
|
val refined = AnchorConstellationSolver.refine(prior, interAnchorDistances, rootId)
|
||
|
// Reject a fit that cannot satisfy the edges we have — inconsistent/garbage ranging.
|
||
|
val maxSeedResidualMeters = 0.5
|
||
|
return if (refined.converged && refined.rmsResidualMeters <= maxSeedResidualMeters) refined.positions else null
|
||
|
}
|
||
|
// ═════════════════════════════════════════════════════════════════════
|
||
|
// Predicate registration
|
||
|
// ═════════════════════════════════════════════════════════════════════
|
||
| common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorMeshBootstrapTest.kt | ||
|---|---|---|
|
}
|
||
|
}
|
||
|
@Test
|
||
|
fun recovers_from_aoa_prior_when_one_link_is_missing() {
|
||
|
// A TV between two anchors permanently blocks their mutual UWB range (A2<->A3 here — NLOS),
|
||
|
// so the inter-anchor graph is K4 minus one edge. The closed-form cascade cannot place the
|
||
|
// 4th anchor from that (it needs all three edges to the placed set), so the frame used to
|
||
|
// stay pinned to the raw (garbage-z) AoA seed forever (solve-null every tick). The bootstrap
|
||
|
// now falls back to refining that AoA prior against the 5 edges it DOES have — reaching
|
||
|
// quorum and correcting the well-constrained geometry. The unranged fold (A2/A3 relative)
|
||
|
// is left to the prior and deliberately NOT asserted here.
|
||
|
val h = MofeTestHarness(MofeConfig()).build()
|
||
|
h.engine.initializeAsRoot(a1)
|
||
|
// AoA prior: correct-ish xy, GARBAGE z (what phone AoA produces) — the seed the fallback refines.
|
||
|
h.engine.registerMeshNode(a2, Vector3D(0.0, 0.5, -1.4))
|
||
|
h.engine.registerMeshNode(a3, Vector3D(1.3, 0.0, -1.1))
|
||
|
h.engine.registerMeshNode(a4, Vector3D(1.0, 0.3, 0.0))
|
||
|
assertTrue(h.frameManager.canSolvePositions, "precondition: AoA path already reached quorum")
|
||
|
// Feed every inter-anchor distance EXCEPT the blocked pair a2<->a3.
|
||
|
val ids = listOf(a1, a2, a3, a4)
|
||
|
repeat(6) {
|
||
|
for (i in ids.indices) for (j in i + 1 until ids.size) {
|
||
|
if (ids[i] == a2 && ids[j] == a3) continue // TV between A2 and A3: never ranges
|
||
|
val d = truth.getValue(ids[i]).distanceTo(truth.getValue(ids[j]))
|
||
|
h.engine.processInterAnchorRanging(ids[i], ids[j], d)
|
||
|
}
|
||
|
}
|
||
|
val placed = h.engine.bootstrapReferenceConstellation()
|
||
|
assertTrue(placed > 0, "fallback must re-seat anchors from the prior + available edges (got $placed)")
|
||
|
val refs = h.frameManager.getAllReferencePoints().associate { it.id to it.position }
|
||
|
// The 5 MEASURED edges must be honoured (the unranged a2-a3 fold is free, so not asserted).
|
||
|
var maxErr = 0.0
|
||
|
for (i in ids.indices) for (j in i + 1 until ids.size) {
|
||
|
if (ids[i] == a2 && ids[j] == a3) continue
|
||
|
val got = refs.getValue(ids[i]).distanceTo(refs.getValue(ids[j]))
|
||
|
val want = truth.getValue(ids[i]).distanceTo(truth.getValue(ids[j]))
|
||
|
maxErr = maxOf(maxErr, abs(got - want))
|
||
|
}
|
||
|
assertTrue(maxErr < 0.05,
|
||
|
"the fallback must satisfy the 5 measured inter-anchor distances; maxErr=${maxErr}m")
|
||
|
}
|
||
|
@Test
|
||
|
fun overwrites_aoa_seeded_frame_from_distances() {
|
||
|
// The field failure in the flesh: the frame is FIRST seeded from raw AoA (garbage
|
||