From 35dc34fe27197db9afc8eec2db7f112058cdc0ce Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 6 Aug 2026 17:56:48 +0000 Subject: [PATCH 4/5] feat(engine): detect a biased inter-anchor edge by graph residual and down-weight it MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Part A (#63) — detect. A tight, stable inter-anchor ranging bias (capture4's cross-room links) breaks neither the triangle-inequality guard nor per-link STRONG/WEAK health, yet warps the frame. Add InterAnchorLinkReport.biasedEdges: solve the constellation, then flag an edge whose post-solve graph residual |‖p_a-p_b‖ - d| is both absolutely large AND dominant over the median edge (lopsided). CRITICAL: on an exactly-rigid graph (n=4, 6 edges = 6 DOF) the solve absorbs the bias with ZERO residual, so detection is gated to REDUNDANT graphs only (n>=5, edges > 3n-6). Surface it as InterAnchorLink.biased / report.biased / hasBiasedLink, and wire interAnchorLinkReport() to populate it (stability-gated by sample count, so a transient never trips it). Part B (#69b-ii) — down-weight. At the leader's refineAnchorConstellation() solve site, build a per-edge weights map that down-weights the flagged edges and pass it to AnchorConstellationSolver.refine, so the good edges out-vote the bias instead of the frame warping. Guardrail: only the surplus over 3n-6 edges is down-weighted, so the graph never goes under-determined. No flags (or the default-safe flag off) => empty map => byte-identical solve. Tests: BiasedInterAnchorEdgeTest — rigid-graph-absorbs-bias (no flag), redundant graph flags exactly the biased edge, detect->down-weight recovers the anchor, the report flag wiring, and end-to-end through the engine. Full :common:jvmTest green (771 tests, 0 failures). Co-Authored-By: Claude Opus 4.8 Claude-Session: https://claude.ai/code/session_01WppuiKZt4CuQxX4N7k6SVR --- .../mofe/engine/MultiObserverFusionEngine.kt | 66 +++++- .../com/aether/mofe/model/ConfigTypes.kt | 10 + .../com/aether/mofe/model/InterAnchorLink.kt | 93 +++++++- .../mofe/engine/BiasedInterAnchorEdgeTest.kt | 201 ++++++++++++++++++ 4 files changed, 368 insertions(+), 2 deletions(-) create mode 100644 common/src/commonTest/kotlin/com/aether/mofe/engine/BiasedInterAnchorEdgeTest.kt diff --git a/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt b/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt index 4a84eb8..880839a 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/MultiObserverFusionEngine.kt @@ -577,6 +577,52 @@ class MultiObserverFusionEngine( return if (n == 0) Double.NaN else kotlin.math.sqrt(sumSq / n) } + /** + * #63 — inter-anchor edges flagged BIASED by GRAPH RESIDUAL on the given [positions] and measured + * [distances]: a stable, lopsided link the rigid solve cannot reconcile with the rest of the graph + * (the capture4 cross-room bias the triangle-inequality guard and per-link health both miss). + * Returns each biased pair (canonical order) → its residual, EMPTY unless the graph is REDUNDANT + * (≥5 anchors / >3n−6 edges) — on an exactly-rigid graph a single bias is absorbed with ZERO + * residual and is undetectable (see [InterAnchorLinkReport.biasedEdges]). Adds a STABILITY gate on + * top of the pure detector: a bias is trusted only from a WELL-SAMPLED link (≥ strongMinSamples), + * never a transient bad read — reusing the same per-link sample accounting the STRONG/WEAK health + * uses. Single-sources detection for both [interAnchorLinkReport]'s flag and the refine down-weight. + */ + private fun biasedInterAnchorEdges( + positions: Map, + distances: Map, Double>, + ): Map, Double> { + val candidates = InterAnchorLinkReport.biasedEdges(positions, distances, interAnchorLinkThresholds) + if (candidates.isEmpty()) return candidates + return candidates.filterKeys { pair -> + val st = interAnchorLinkStats[pair] ?: interAnchorLinkStats[pair.second to pair.first] + (st?.count ?: 0) >= interAnchorLinkThresholds.strongMinSamples + } + } + + /** + * #69b-ii — per-edge weights that down-weight the biased edges [biasedInterAnchorEdges] finds, for + * [AnchorConstellationSolver.refine]. GUARDRAIL: never push the solve under-determined. A rigid 3-D + * graph of n nodes needs ≥ 3n−6 full-weight edges; only the SURPLUS is down-weighted (most-biased + * first), so at least 3n−6 edges keep unit weight. Detection already requires a redundant graph + * (edges > 3n−6), so the budget is ≥ 1 for a lone biased edge. No flags (or the feature off) ⇒ + * EMPTY map ⇒ the refine is byte-identical to the un-weighted solve. + */ + private fun biasedEdgeWeights( + positions: Map, + distances: Map, Double>, + ): Map, Double> { + if (!config.anchorConstellationDownWeightBiasedEnabled) return emptyMap() + val biased = biasedInterAnchorEdges(positions, distances) + if (biased.isEmpty()) return emptyMap() + val usable = distances.keys.filter { it.first in positions && it.second in positions } + val nodes = usable.flatMapTo(HashSet()) { listOf(it.first, it.second) } + val budget = usable.size - maxOf(3 * nodes.size - 6, 0) // full-weight edges to preserve = 3n−6 + if (budget <= 0) return emptyMap() + val w = config.anchorConstellationBiasedEdgeWeight + return biased.entries.sortedByDescending { it.value }.take(budget).associate { it.key to w } + } + /** Reset the stability run so a fresh convergence must re-confirm before the frame can lock. */ private fun resetLockStability() { lastShapeSignature = null; stableSolveTicks = 0 } @@ -1309,11 +1355,18 @@ class MultiObserverFusionEngine( val edges = correctedInterAnchorDistances().filterKeys { it.first in anchors && it.second in anchors } if (edges.size < config.anchorConstellationMinEdges) return 0 + // #69b-ii: down-weight any inter-anchor edge flagged BIASED by graph residual on the CURRENT + // (already-placed) constellation, so the good edges out-vote the bias in this refine instead of + // the frame warping to absorb it. EMPTY on a rigid/clean graph (or the feature off) ⇒ the + // solve is byte-identical to before. The guardrail in [biasedEdgeWeights] keeps ≥ 3n−6 full- + // weight edges so the graph never goes under-determined. + val edgeWeights = biasedEdgeWeights(anchors, edges) val result = AnchorConstellationSolver.refine( initial = anchors, distances = edges, rootId = rootId, priorWeight = config.anchorConstellationPriorWeight, + weights = edgeWeights, ) if (result.maxCorrectionMeters < config.anchorConstellationMinCorrectionMeters) return 0 @@ -1763,7 +1816,18 @@ class MultiObserverFusionEngine( sampleCount = st.count, ) } - return InterAnchorLinkReport.build(ids, samples, nowMicros, interAnchorLinkThresholds) + // #63: mark stable, lopsided edges by graph residual on the current solved constellation. Root + // sits at the gauge origin; distances are rotation-invariant so the oriented frame positions + // serve directly. EMPTY on a rigid/clean graph ⇒ no biased flags ⇒ report is unchanged. + val rootId = frameManager.topology?.frame?.originDeviceId + val positions = buildMap { + if (rootId != null) put(rootId, Vector3D.ZERO) + frameManager.getAllReferencePoints().forEach { if (it.id != rootId) put(it.id, it.position) } + } + val corrected = correctedInterAnchorDistances() + .filterKeys { it.first in positions && it.second in positions } + val biasedPairs = biasedInterAnchorEdges(positions, corrected).keys + return InterAnchorLinkReport.build(ids, samples, nowMicros, interAnchorLinkThresholds, biasedPairs) } /** diff --git a/common/src/commonMain/kotlin/com/aether/mofe/model/ConfigTypes.kt b/common/src/commonMain/kotlin/com/aether/mofe/model/ConfigTypes.kt index 9d90dee..92641c4 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/model/ConfigTypes.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/model/ConfigTypes.kt @@ -410,6 +410,16 @@ data class MofeConfig( /** Prior pull toward the current (AoA) positions; fixes the rotational gauge * without resisting the distance-constrained shape. Small. */ val anchorConstellationPriorWeight: Double = 0.005, + /** #63/#69b-ii: down-weight an inter-anchor edge the graph-residual detector flags BIASED (a + * stable, lopsided link — see [InterAnchorLinkReport.biasedEdges]) in the leader's rigid-body + * refine, so the good edges out-vote the bias instead of the frame warping to absorb it. Only + * ever bites on a REDUNDANT graph (≥5 anchors / >3n−6 edges) with a dominant, well-sampled + * outlier, and never down-weights so many edges that the solve goes under-determined — so with + * no such edge it is a no-op (empty weight map ⇒ byte-identical solve). */ + val anchorConstellationDownWeightBiasedEnabled: Boolean = true, + /** Weight applied to a down-weighted biased inter-anchor edge. ≪1 so the good edges dominate the + * fit, but non-zero so the edge is only SOFTENED, not deleted — the distance graph stays connected. */ + val anchorConstellationBiasedEdgeWeight: Double = 0.01, // ── Frame-lock TRUST gate (accuracy > speed) ─────────────────────────────── // The frame LOCK (frameEstablished) freezes the constellation to stop jitter; a diff --git a/common/src/commonMain/kotlin/com/aether/mofe/model/InterAnchorLink.kt b/common/src/commonMain/kotlin/com/aether/mofe/model/InterAnchorLink.kt index 8557d0e..9f0df75 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/model/InterAnchorLink.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/model/InterAnchorLink.kt @@ -1,5 +1,7 @@ package com.aether.mofe.model +import kotlin.math.abs + /** * Health of one anchor↔anchor UWB link, for the calibration / mesh-health UI. * @@ -40,6 +42,15 @@ data class InterAnchorLinkThresholds( val strongMinSamples: Int = 3, /** |measured − solved| at/below this (m) is a STRONG link; above it is WEAK. */ val strongResidualMeters: Double = 0.20, + /** #63 graph-residual BIAS detection ([InterAnchorLinkReport.biasedEdges]). An edge is flagged + * biased when its post-solve graph residual |‖p_a−p_b‖ − d| clears BOTH this absolute floor (m) … */ + val biasResidualMeters: Double = 0.10, + /** … AND this multiple of the MEDIAN edge residual, so only a lopsided edge that stands out from + * the consensus is flagged — a uniform/global error (every edge equally off) flags nothing. On a + * minimally-redundant graph (5 anchors, one surplus edge) the solve spreads a lone bias the most, + * leaving the biased edge only ~2.4× the median, so the gate sits at 2× (the biased edge is still + * the sole one clearing both this AND the absolute floor). */ + val biasDominanceRatio: Double = 2.0, ) /** One classified anchor↔anchor link. [a] < [b] by device id, so a pair appears once. */ @@ -55,6 +66,12 @@ data class InterAnchorLink( val ageMicros: Long?, /** Measurements folded into this link. */ val sampleCount: Int, + /** #63: a stable, LOPSIDED link — it ranges fine (often [InterAnchorLinkStatus.STRONG]) yet carries + * a large, dominant post-solve GRAPH residual the rigid solve could not absorb, i.e. a directional + * ranging bias. Orthogonal to [status] (a biased edge is usually still "ranging well"). Only ever + * set on a REDUNDANT constellation (≥5 anchors / >3n−6 edges); on an exactly-rigid graph a bias is + * absorbed with zero residual and is undetectable this way. See [InterAnchorLinkReport.biasedEdges]. */ + val biased: Boolean = false, ) /** @@ -68,10 +85,17 @@ data class InterAnchorLinkReport( val strong: Int, val weak: Int, val missing: Int, + /** Links flagged [InterAnchorLink.biased] by graph residual (#63). 0 unless [build] was given the + * solved positions to detect against, and the constellation is redundant enough to detect at all. */ + val biased: Int = 0, ) { /** A distance graph with a MISSING expected link is not rigid — the solve is blocked/degraded. */ val hasMissingLink: Boolean get() = missing > 0 + /** At least one stable, lopsided inter-anchor link — a directional ranging bias that a redundant + * solve can down-weight (#69b-ii) but a rigid one silently absorbs into a warped frame. */ + val hasBiasedLink: Boolean get() = biased > 0 + companion object { val EMPTY = InterAnchorLinkReport(emptyList(), 0, 0, 0, 0, 0) @@ -79,12 +103,17 @@ data class InterAnchorLinkReport( * Pure classifier: for every unordered pair of [anchorIds], look up its accumulated * [samples] and classify STRONG / WEAK / MISSING at [nowMicros]. Order-independent * (pair key is normalized by device id). No engine or IO — unit-tested directly. + * + * [biasedPairs] (canonical order, from [biasedEdges]) marks the [InterAnchorLink.biased] flag + * on the matching links. Empty (the default) ⇒ every link biased=false and biased count 0 — + * byte-identical to the ranging-only classification. */ fun build( anchorIds: List, samples: Map, InterAnchorLinkSample>, nowMicros: Long, thresholds: InterAnchorLinkThresholds = InterAnchorLinkThresholds(), + biasedPairs: Set> = emptySet(), ): InterAnchorLinkReport { if (anchorIds.size < 2) return EMPTY val ids = anchorIds.distinct() @@ -92,6 +121,7 @@ data class InterAnchorLinkReport( var strong = 0 var weak = 0 var missing = 0 + var biased = 0 for (i in ids.indices) { for (j in i + 1 until ids.size) { val a = ids[i] @@ -112,6 +142,10 @@ data class InterAnchorLinkReport( InterAnchorLinkStatus.WEAK -> weak++ InterAnchorLinkStatus.MISSING -> missing++ } + // A MISSING link has no distance, so it can never be in biasedPairs; a biased flag + // therefore only ever lands on a link that is actually ranging. + val isBiased = key in biasedPairs + if (isBiased) biased++ links.add( InterAnchorLink( a = key.first, @@ -121,11 +155,68 @@ data class InterAnchorLinkReport( residualMeters = s?.residualMeters, ageMicros = age, sampleCount = s?.sampleCount ?: 0, + biased = isBiased, ), ) } } - return InterAnchorLinkReport(links, ids.size, ids.size * (ids.size - 1) / 2, strong, weak, missing) + return InterAnchorLinkReport( + links, ids.size, ids.size * (ids.size - 1) / 2, strong, weak, missing, biased, + ) + } + + /** + * #63 — flag stable, lopsided inter-anchor edges by GRAPH RESIDUAL. Given the solved anchor + * [positions] (any rigid orientation — inter-anchor distances are rotation/translation + * invariant) and the measured inter-anchor [distances], return each biased edge (canonical + * order) mapped to its post-solve residual |‖p_a−p_b‖ − d|. + * + * The gap this closes: the triangle-inequality guard ([CrossAnchorFuser.applyGeometric- + * ConsistencyCheck]) catches only distances that break geometry outright, and the per-link + * STRONG/WEAK health catches a large per-sample residual — but a TIGHT, STABLE bias (a fixed + * offset on one cross-room link, the capture4 field failure) breaks neither: it ranges + * consistently and can satisfy the triangle inequality. It only shows up once the WHOLE graph + * is solved together and one edge cannot be reconciled with the rest. + * + * ── CRITICAL CORRECTNESS INVARIANT (honor + do not "optimise" away) ─────────────────────── + * This works ONLY on a REDUNDANT (over-determined) graph. A rigid 3-D constellation of n + * nodes has 3n−6 internal DOF. With EXACTLY 3n−6 edges (n=4 ⇒ 6 edges = complete K4) the + * least-squares solve has just enough freedom to satisfy every edge, so a biased edge is + * absorbed by WARPING the frame and its post-solve residual is ZERO — invisible here. Only + * when edges > 3n−6 (n≥5, e.g. K5 = 10 edges > 9 DOF) does the bias have nowhere to hide and + * surface as a dominant residual. Below that this returns EMPTY, by design, not as a miss. + * + * A flagged edge must be both ABSOLUTELY large (≥ [InterAnchorLinkThresholds.biasResidualMeters]) + * and DOMINANT over the median edge residual (≥ ratio×median), so a single lopsided link is + * caught while a uniform/global error (which no single down-weight could fix) flags nothing. + */ + fun biasedEdges( + positions: Map, + distances: Map, Double>, + thresholds: InterAnchorLinkThresholds = InterAnchorLinkThresholds(), + ): Map, Double> { + // Canonicalize; keep only edges whose BOTH endpoints have a solved position + a valid range. + val edges = HashMap, Double>() + for ((pair, d) in distances) { + val (a, b) = pair + if (a == b || d <= 0.0 || !d.isFinite()) continue + if (a !in positions || b !in positions) continue + edges[if (a.value <= b.value) a to b else b to a] = d + } + // n = anchors actually in the ranging graph (not merely positioned) — that is what the + // 3n−6 rigidity DOF is measured against. + val nodes = edges.keys.flatMapTo(HashSet()) { listOf(it.first, it.second) } + val n = nodes.size + if (n < 5) return emptyMap() // < 5 ⇒ at best exactly-rigid ⇒ bias absorbed + val dof = 3 * n - 6 + if (edges.size <= dof) return emptyMap() // not redundant ⇒ residual is zero ⇒ undetectable + val resid = edges.mapValues { (pair, d) -> + abs(positions.getValue(pair.first).distanceTo(positions.getValue(pair.second)) - d) + } + val median = resid.values.sorted().let { it[it.size / 2] } + val floor = thresholds.biasResidualMeters + val ratio = thresholds.biasDominanceRatio + return resid.filterValues { r -> r >= floor && r >= ratio * median } } } } diff --git a/common/src/commonTest/kotlin/com/aether/mofe/engine/BiasedInterAnchorEdgeTest.kt b/common/src/commonTest/kotlin/com/aether/mofe/engine/BiasedInterAnchorEdgeTest.kt new file mode 100644 index 0000000..03a7222 --- /dev/null +++ b/common/src/commonTest/kotlin/com/aether/mofe/engine/BiasedInterAnchorEdgeTest.kt @@ -0,0 +1,201 @@ +package com.aether.mofe.engine + +import com.aether.mofe.integration.MofeTestHarness +import com.aether.mofe.model.DeviceId +import com.aether.mofe.model.InterAnchorLinkReport +import com.aether.mofe.model.InterAnchorLinkSample +import com.aether.mofe.model.MofeConfig +import com.aether.mofe.model.Vector3D +import kotlin.math.abs +import kotlin.test.Test +import kotlin.test.assertEquals +import kotlin.test.assertFalse +import kotlin.test.assertTrue + +/** + * #63 (detect) + #69b-ii (down-weight) — flag a STABLE, LOPSIDED inter-anchor edge by GRAPH RESIDUAL, + * then down-weight it in the rigid-body solve so the good edges out-vote the bias. + * + * The gap these close: [CrossAnchorFuser.applyGeometricConsistencyCheck] catches only triangle- + * inequality breaks, and per-link STRONG/WEAK health catches a large per-sample residual — but a + * tight, stable cross-room bias (capture4: +0.57/+0.75 m on the long links) breaks neither and warps + * the frame. It only surfaces once the WHOLE graph is solved and one edge cannot be reconciled. + * + * ── CRITICAL CORRECTNESS FACT (the whole reason for the redundancy gate) ───────────────────────── + * On an EXACTLY-RIGID 4-anchor graph (6 edges = 6 DOF) the least-squares solve has just enough + * freedom to satisfy every edge, so the biased edge's post-solve residual is ZERO — the solve absorbs + * the bias by WARPING the frame and detection is impossible. Graph-residual detection therefore works + * ONLY on a REDUNDANT graph (≥5 anchors / >3n−6 edges). Both facts are asserted below. + */ +class BiasedInterAnchorEdgeTest { + + private val a1 = DeviceId("A1"); private val a2 = DeviceId("A2") + private val a3 = DeviceId("A3"); private val a4 = DeviceId("A4"); private val a5 = DeviceId("A5") + + private fun dist(a: Vector3D, b: Vector3D) = a.distanceTo(b) + private fun canon(a: DeviceId, b: DeviceId) = if (a.value <= b.value) a to b else b to a + + /** Every unordered inter-anchor distance for [pts], with [biasPair] inflated by [bias] m. */ + private fun distances( + pts: Map, biasPair: Pair, bias: Double, + ): Map, Double> { + val ids = pts.keys.toList() + val out = HashMap, Double>() + for (i in ids.indices) for (j in i + 1 until ids.size) { + val (x, y) = ids[i] to ids[j] + val biased = (x to y) == biasPair || (y to x) == biasPair + out[x to y] = dist(pts.getValue(x), pts.getValue(y)) + if (biased) bias else 0.0 + } + return out + } + + private val truth5 = mapOf( + a1 to Vector3D(0.0, 0.0, 0.0), a2 to Vector3D(2.0, 0.0, 0.0), + a3 to Vector3D(0.0, 2.0, 0.0), a4 to Vector3D(1.0, 1.0, 0.6), a5 to Vector3D(2.0, 2.0, 0.3), + ) + + // ── Part A: DETECT ─────────────────────────────────────────────────────────────────────────── + + @Test + fun exactly_rigid_four_anchor_graph_absorbs_the_bias_so_nothing_is_flagged() { + // 4 anchors = 6 edges = 6 DOF: the solve satisfies every edge, so the +0.6 bias is absorbed + // into a warped frame with ZERO post-solve residual — undetectable by graph residual, BY DESIGN. + val truth4 = mapOf( + a1 to Vector3D(0.0, 0.0, 0.0), a2 to Vector3D(2.0, 0.0, 0.0), + a3 to Vector3D(0.0, 2.0, 0.0), a4 to Vector3D(1.0, 1.0, 0.6), + ) + val biased = distances(truth4, a2 to a3, 0.6) + val solved = AnchorConstellationSolver.refine(truth4, biased, rootId = a1) + + // The frame really did warp (A3 pushed off truth) — the bias went somewhere … + val a3err = solved.positions.getValue(a3).distanceTo(truth4.getValue(a3)) + assertTrue(a3err > 0.15, "rigid graph should absorb the bias by warping (A3 err=${a3err} m)") + // … but NOT into any edge residual, so detection correctly finds nothing. + val flagged = InterAnchorLinkReport.biasedEdges(solved.positions, biased) + assertTrue(flagged.isEmpty(), + "an exactly-rigid graph must flag NO biased edge (residual is absorbed); got $flagged") + } + + @Test + fun redundant_five_anchor_graph_flags_exactly_the_biased_edge() { + val biased = distances(truth5, a2 to a3, 0.6) + val solved = AnchorConstellationSolver.refine(truth5, biased, rootId = a1) + val flagged = InterAnchorLinkReport.biasedEdges(solved.positions, biased) + + val residuals = biased.entries.joinToString { (p, d) -> + "${p.first.value}-${p.second.value}=" + + "${abs(solved.positions.getValue(p.first).distanceTo(solved.positions.getValue(p.second)) - d)}" + } + assertTrue(canon(a2, a3) in flagged, + "the redundant solve must flag the biased A2-A3 edge. residuals=[$residuals] flagged=$flagged") + assertEquals(setOf(canon(a2, a3)), flagged.keys, + "ONLY the biased edge should be flagged. residuals=[$residuals] flagged=$flagged") + } + + // ── Part B: DOWN-WEIGHT (the required detect → down-weight → recover loop) ────────────────────── + + @Test + fun detect_then_downweight_recovers_the_biased_anchor() { + val biased = distances(truth5, a2 to a3, 0.6) + + // 1) UN-weighted solve: an over-determined graph still spreads a single bias into the frame. + val unweighted = AnchorConstellationSolver.refine(truth5, biased, rootId = a1) + val errUnweighted = unweighted.positions.getValue(a3).distanceTo(truth5.getValue(a3)) + assertTrue(errUnweighted > 0.10, "the un-weighted solve spreads the bias into A3 (err=${errUnweighted} m)") + + // 2) DETECT the biased edge from that solve (no ground truth used — pure graph residual). + val flagged = InterAnchorLinkReport.biasedEdges(unweighted.positions, biased) + assertTrue(flagged.isNotEmpty(), "detection must find the biased edge to drive the down-weight") + + // 3) DOWN-WEIGHT the flagged edges and re-solve → the good edges recover A3. + val weights = flagged.keys.associateWith { 0.01 } + val weighted = AnchorConstellationSolver.refine(truth5, biased, rootId = a1, weights = weights) + val errWeighted = weighted.positions.getValue(a3).distanceTo(truth5.getValue(a3)) + assertTrue(errWeighted < 0.05, + "detect + down-weight must recover A3 (err ${errWeighted} m vs un-weighted ${errUnweighted} m)") + } + + // ── Part A wiring: the flag surfaces on InterAnchorLinkReport ─────────────────────────────────── + + @Test + fun build_surfaces_the_biased_flag_and_count_and_is_a_noop_when_empty() { + val ids = truth5.keys.toList() + val samples = buildMap { + val d = distances(truth5, a2 to a3, 0.6) + for ((pair, dm) in d) put(canon(pair.first, pair.second), InterAnchorLinkSample(dm, null, 0L, 5)) + } + // Empty biasedPairs ⇒ unchanged report (no biased flags). + val plain = InterAnchorLinkReport.build(ids, samples, nowMicros = 0L) + assertEquals(0, plain.biased) + assertFalse(plain.hasBiasedLink) + assertTrue(plain.links.none { it.biased }) + + // With the flag set, exactly that link is marked and counted. + val flagged = InterAnchorLinkReport.build(ids, samples, nowMicros = 0L, biasedPairs = setOf(canon(a2, a3))) + assertEquals(1, flagged.biased) + assertTrue(flagged.hasBiasedLink) + val link = flagged.links.first { it.a == canon(a2, a3).first && it.b == canon(a2, a3).second } + assertTrue(link.biased, "the A2-A3 link must carry the biased flag") + assertEquals(1, flagged.links.count { it.biased }) + } + + // ── End-to-end through the real engine (report flag + refine down-weight wiring) ─────────────── + + private fun feedBiased(h: MofeTestHarness) { + val d = distances(truth5, a2 to a3, 0.6) + for ((pair, dm) in d) repeat(4) { h.engine.processInterAnchorRanging(pair.first, pair.second, dm) } + } + + private fun refs(h: MofeTestHarness): Map = + h.frameManager.getAllReferencePoints().associate { it.id to it.position } + + private fun maxPairwiseError(got: Map): Double { + val ids = truth5.keys.toList() + var maxErr = 0.0 + for (i in ids.indices) for (j in i + 1 until ids.size) { + val g = got.getValue(ids[i]).distanceTo(got.getValue(ids[j])) + val w = truth5.getValue(ids[i]).distanceTo(truth5.getValue(ids[j])) + maxErr = maxOf(maxErr, abs(g - w)) + } + return maxErr + } + + private fun seedAtTruth(h: MofeTestHarness) { + h.engine.initializeAsRoot(a1) + h.engine.registerMeshNode(a2, truth5.getValue(a2)) + h.engine.registerMeshNode(a3, truth5.getValue(a3)) + h.engine.registerMeshNode(a4, truth5.getValue(a4)) + h.engine.registerMeshNode(a5, truth5.getValue(a5)) + } + + @Test + fun engine_report_flags_the_biased_link_on_a_redundant_constellation() { + val h = MofeTestHarness(MofeConfig()).build() + seedAtTruth(h) + feedBiased(h) + + val report = h.engine.interAnchorLinkReport() + assertTrue(report.hasBiasedLink, "the engine report must flag the stable cross-room bias") + val biasedLinks = report.links.filter { it.biased }.map { canon(it.a, it.b) } + assertTrue(canon(a2, a3) in biasedLinks, "the flagged link must be A2-A3; got $biasedLinks") + } + + @Test + fun engine_downweight_prevents_the_bias_from_warping_the_frame() { + // Down-weight ON (default): refineAnchorConstellation holds the frame near truth. + val on = MofeTestHarness(MofeConfig()).build() + seedAtTruth(on); feedBiased(on) + on.engine.refineAnchorConstellation() + val errOn = maxPairwiseError(refs(on)) + + // Down-weight OFF: the same biased edge warps the constellation on refine. + val off = MofeTestHarness(MofeConfig(anchorConstellationDownWeightBiasedEnabled = false)).build() + seedAtTruth(off); feedBiased(off) + off.engine.refineAnchorConstellation() + val errOff = maxPairwiseError(refs(off)) + + assertTrue(errOff > 0.10, "without down-weighting the bias must warp the frame (maxErr=${errOff} m)") + assertTrue(errOn < errOff * 0.6, + "down-weighting must keep the frame closer to truth (on=${errOn} m vs off=${errOff} m)") + } +} -- 2.43.0