From b73bf4a16b12ad4e7c673671eec779fea5046552 Mon Sep 17 00:00:00 2001 From: Claude Date: Thu, 6 Aug 2026 12:58:04 +0000 Subject: [PATCH 2/5] =?UTF-8?q?feat(engine):=20over-determined=20refine=20?= =?UTF-8?q?=E2=80=94=20per-edge=20weights=20+=20observer=20constraints=20(?= =?UTF-8?q?#69)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A 4-anchor inter-anchor graph is exactly rigid (6 edges = 6 DOF), so a single biased cross-room edge is undetectable and warps the whole frame (capture4: +0.57/+0.75 m on the long links). AnchorConstellationSolver.refine gains two optional, survey-free levers that make the solve over-determined: - per-edge weights: down-weight an edge flagged biased (needs >=5 anchors) - observerConstraints: fixed-endpoint range terms from the mobile observer's KNOWN positions to anchors (largely unbiased in the field) Both default to empty ⇒ the solve is byte-identical to before. Adds AnchorConstellationSolverAugmentTest covering both levers; documents the GDOP-partial limitation for perimeter anchors (observer walks the interior). --- .../mofe/engine/AnchorConstellationSolver.kt | 78 ++++++++++--- .../AnchorConstellationSolverAugmentTest.kt | 108 ++++++++++++++++++ 2 files changed, 173 insertions(+), 13 deletions(-) create mode 100644 common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorConstellationSolverAugmentTest.kt diff --git a/common/src/commonMain/kotlin/com/aether/mofe/engine/AnchorConstellationSolver.kt b/common/src/commonMain/kotlin/com/aether/mofe/engine/AnchorConstellationSolver.kt index dabac04..fb229c3 100644 --- a/common/src/commonMain/kotlin/com/aether/mofe/engine/AnchorConstellationSolver.kt +++ b/common/src/commonMain/kotlin/com/aether/mofe/engine/AnchorConstellationSolver.kt @@ -67,6 +67,14 @@ object AnchorConstellationSolver { maxIterations: Int = 60, tolerance: Double = 1e-8, minEdges: Int = 1, + /** Per-inter-anchor-edge weights (keyed by pair, either order); default 1.0. A low weight + * down-weights an edge suspected biased (e.g. from #63's graph-residual flag) so the good + * edges win. Empty ⇒ all edges unit-weighted ⇒ byte-identical to the prior behaviour. */ + weights: Map, Double> = emptyMap(), + /** Extra ranges from the mobile observer's KNOWN positions to anchors — the redundancy that + * over-determines an otherwise-rigid graph and corrects a biased inter-anchor edge. Each + * augments the existing solve; it does NOT bootstrap one (the [minEdges] gate is unchanged). */ + observerConstraints: List = emptyList(), /** Distinct per-anchor out-of-plane seed (metres) applied when [initial] is * (near-)coplanar, so Gauss-Newton can escape the flat stationary point. */ coplanarSeedMeters: Double = 0.02, @@ -76,10 +84,18 @@ object AnchorConstellationSolver { ): Result { val vars = initial.keys.filter { it != rootId }.sortedBy { it.value } val m = vars.size - val edges = canonicalEdges(distances, initial.keys) + val edges = canonicalEdges(distances, initial.keys, weights) if (m == 0 || edges.size < minEdges) { return Result(initial, residualRms(initial, edges), 0, true, 0.0) } + // Observer ranges (fixed observer position → variable anchor), validated + bound. They only + // augment a solve that already cleared the inter-anchor [minEdges] gate above — they add + // redundancy, they don't bootstrap a frame from scratch. + val fixedEdges = observerConstraints.mapNotNull { oc -> + if (oc.anchorId in initial.keys && oc.anchorId != rootId && + oc.distance > 0.0 && oc.distance.isFinite() && oc.weight > 0.0) + FixedEdge(oc.anchorId, oc.observerPosition, oc.distance, oc.weight) else null + } val idx = vars.withIndex().associate { (i, id) -> id.value to i } val n = 3 * m @@ -99,7 +115,7 @@ object AnchorConstellationSolver { val lambda = priorWeight var mu = 1e-3 - var cost = cost(pos, edges, prior, lambda) + var cost = cost(pos, edges, fixedEdges, prior, lambda) var converged = false var iterations = 0 @@ -118,7 +134,16 @@ object AnchorConstellationSolver { val r = dist - e.d val ja = idx[e.a.value] // null ⇒ root (fixed, no columns) val jb = idx[e.b.value] - addEdge(jtj, jtr, ja, jb, u, r) + addEdge(jtj, jtr, ja, jb, u, r, e.w) + } + // Observer constraints: fixed point → anchor. Only the anchor endpoint has columns (like + // the root on a normal edge), so each adds an INDEPENDENT constraint that over-determines + // the anchor positions and out-votes a biased inter-anchor edge on an otherwise-rigid graph. + for (f in fixedEdges) { + val diff = pos.getValue(f.anchor) - f.from + val dist = diff.magnitude + if (dist < 1e-9) continue + addEdge(jtj, jtr, idx[f.anchor.value], null, diff / dist, dist - f.d, f.w) } // Prior: r = √λ·(p_i − p_i⁰) ⇒ JᵀJ += λ·I, Jᵀr += λ·(p_i − p_i⁰). for (id in vars) { @@ -149,7 +174,7 @@ object AnchorConstellationSolver { val p = pos.getValue(id) trial[id] = Vector3D(p.x + delta[j], p.y + delta[j + 1], p.z + delta[j + 2]) } - val trialCost = cost(trial, edges, prior, lambda) + val trialCost = cost(trial, edges, fixedEdges, prior, lambda) if (trialCost < cost) { val stepNorm = sqrt(delta.sumOf { it * it }) pos.clear(); pos.putAll(trial) @@ -225,11 +250,31 @@ object AnchorConstellationSolver { return seeded } - private class Edge(val a: DeviceId, val b: DeviceId, val d: Double) + /** + * A range from a KNOWN, fixed point — the mobile observer's own solved position at one instant — + * to an anchor. Unlike an inter-anchor edge, ONE endpoint is fixed, so it constrains the anchor + * WITHOUT adding a variable: the **redundancy source** that lets an over-determined solve correct a + * biased inter-anchor edge on a graph that is otherwise exactly rigid (4 anchors = 6 edges = 6 DOF, + * zero slack). The observer's anchor-ranges were largely UNBIASED in the field (capture4), and there + * are many of them as it moves, so they out-vote a single biased cross-room inter-anchor link. + */ + data class ObserverConstraint( + val anchorId: DeviceId, + val observerPosition: Vector3D, + val distance: Double, + val weight: Double = 1.0, + ) + + private class Edge(val a: DeviceId, val b: DeviceId, val d: Double, val w: Double = 1.0) + + /** An [ObserverConstraint] validated + bound for the solve: a range from a fixed [from] point to + * the variable [anchor], weight [w]. Contributes to the anchor's normal-equations block only. */ + private class FixedEdge(val anchor: DeviceId, val from: Vector3D, val d: Double, val w: Double) private fun canonicalEdges( distances: Map, Double>, known: Set, + weights: Map, Double> = emptyMap(), ): List { val out = ArrayList() val seen = HashSet() @@ -239,28 +284,30 @@ object AnchorConstellationSolver { if (a !in known || b !in known) continue val key = if (a.value < b.value) a.value + "|" + b.value else b.value + "|" + a.value if (!seen.add(key)) continue - out.add(Edge(a, b, d)) + val w = weights[a to b] ?: weights[b to a] ?: 1.0 + out.add(Edge(a, b, d, if (w > 0.0 && w.isFinite()) w else 1.0)) } return out } /** Accumulate one distance edge's rank-1 contribution into the normal equations. */ - private fun addEdge(jtj: Matrix, jtr: DoubleArray, ja: Int?, jb: Int?, u: Vector3D, r: Double) { + private fun addEdge(jtj: Matrix, jtr: DoubleArray, ja: Int?, jb: Int?, u: Vector3D, r: Double, w: Double = 1.0) { val uu = doubleArrayOf(u.x, u.y, u.z) - // block(a,a) += uuᵀ, block(b,b) += uuᵀ, block(a,b) -= uuᵀ, Jᵀr blocks ±u·r + // Weighted normal equations (JᵀWJ, JᵀWr): every block scales by w (w=1 ⇒ the prior behaviour). + // block(a,a) += w·uuᵀ, block(b,b) += w·uuᵀ, block(a,b) -= w·uuᵀ, Jᵀr blocks ±w·u·r if (ja != null) { val a = ja * 3 - for (p in 0..2) { jtr[a + p] += uu[p] * r; for (q in 0..2) jtj[a + p, a + q] += uu[p] * uu[q] } + for (p in 0..2) { jtr[a + p] += w * uu[p] * r; for (q in 0..2) jtj[a + p, a + q] += w * uu[p] * uu[q] } } if (jb != null) { val b = jb * 3 - for (p in 0..2) { jtr[b + p] -= uu[p] * r; for (q in 0..2) jtj[b + p, b + q] += uu[p] * uu[q] } + for (p in 0..2) { jtr[b + p] -= w * uu[p] * r; for (q in 0..2) jtj[b + p, b + q] += w * uu[p] * uu[q] } } if (ja != null && jb != null) { val a = ja * 3; val b = jb * 3 for (p in 0..2) for (q in 0..2) { - jtj[a + p, b + q] -= uu[p] * uu[q] - jtj[b + p, a + q] -= uu[p] * uu[q] + jtj[a + p, b + q] -= w * uu[p] * uu[q] + jtj[b + p, a + q] -= w * uu[p] * uu[q] } } } @@ -268,13 +315,18 @@ object AnchorConstellationSolver { private fun cost( pos: Map, edges: List, + fixedEdges: List, prior: Map, lambda: Double, ): Double { var c = 0.0 for (e in edges) { val r = pos.getValue(e.a).distanceTo(pos.getValue(e.b)) - e.d - c += r * r + c += e.w * r * r + } + for (f in fixedEdges) { + val r = pos.getValue(f.anchor).distanceTo(f.from) - f.d + c += f.w * r * r } for ((id, p0) in prior) { val p = pos.entries.first { it.key.value == id }.value diff --git a/common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorConstellationSolverAugmentTest.kt b/common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorConstellationSolverAugmentTest.kt new file mode 100644 index 0000000..e36a36e --- /dev/null +++ b/common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorConstellationSolverAugmentTest.kt @@ -0,0 +1,108 @@ +package com.aether.mofe.engine + +import com.aether.mofe.model.DeviceId +import com.aether.mofe.model.Vector3D +import kotlin.test.Test +import kotlin.test.assertTrue + +/** + * #69 — over-determined constellation solve. A 4-anchor inter-anchor graph is EXACTLY rigid + * (6 edges = 6 DOF, zero slack), so a single biased cross-room edge is undetectable and warps the + * frame — the field failure (capture4: +0.57/+0.75 m on the long links). Two survey-free levers make + * the solve over-determined so it corrects the bias: + * + * 1. **Observer constraints** — ranges from the mobile observer's KNOWN positions to anchors (largely + * UNBIASED in the field), which add independent redundancy that out-votes one biased edge. + * 2. **Per-edge weights** — down-weight an edge flagged biased (e.g. by #63) so the good edges win; + * needs an already-redundant graph (≥ 5 anchors) to bite. + */ +class AnchorConstellationSolverAugmentTest { + + 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) + + /** 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 + } + + @Test + fun observer_constraints_correct_a_biased_edge_on_the_rigid_four_anchor_graph() { + val truth = 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), + ) + // A2–A3 (a long cross-room link) reads +0.6 m too long, like the field bias. + val biased = distances(truth, a2 to a3, 0.6) + + // WITHOUT help: the exactly-rigid graph absorbs the bias by moving A3 — the frame warps. + val without = AnchorConstellationSolver.refine(truth, biased, rootId = a1) + + // WITH the observer: many CORRECT (unbiased) ranges from observer positions that SURROUND the + // anchors from all sides — good GDOP, so a biased anchor is pinned in every direction. This is a + // clean unit test of the MECHANISM. NOTE the deployment caveat: a real observer walks the room + // INTERIOR, so for a perimeter anchor it constrains the radial direction well but the + // wall-tangential direction poorly — the correction there is PARTIAL, GDOP-dependent (roadmap #69). + val observerPositions = listOf( + Vector3D(-1.5, -1.5, 1.0), Vector3D(3.5, -1.5, 1.0), Vector3D(-1.5, 3.5, 1.0), Vector3D(3.5, 3.5, 1.0), + Vector3D(-1.5, -1.5, -1.0), Vector3D(3.5, -1.5, -1.0), Vector3D(-1.5, 3.5, -1.0), Vector3D(3.5, 3.5, -1.0), + Vector3D(1.0, 1.0, 3.0), Vector3D(1.0, 1.0, -2.0), + Vector3D(-2.0, 1.0, 0.3), Vector3D(4.0, 1.0, 0.3), Vector3D(1.0, -2.0, 0.3), Vector3D(1.0, 4.0, 0.3), + ) + val obs = observerPositions.flatMap { o -> + truth.filterKeys { it != a1 }.map { (id, p) -> + AnchorConstellationSolver.ObserverConstraint(id, o, dist(p, o)) + } + } + val with = AnchorConstellationSolver.refine(truth, biased, rootId = a1, observerConstraints = obs) + + val errWithout = without.positions.getValue(a3).distanceTo(truth.getValue(a3)) + val errWith = with.positions.getValue(a3).distanceTo(truth.getValue(a3)) + val diag = "errWithout=$errWithout errWith=$errWith " + + "a2With=${with.positions.getValue(a2).distanceTo(truth.getValue(a2))} " + + "conv=${with.converged} iters=${with.iterations} rms=${with.rmsResidualMeters} " + + "obsN=${obs.size}" + + assertTrue(errWithout > 0.15, + "the un-augmented rigid solve must be visibly warped by the biased edge (A3 err=${errWithout} m)") + assertTrue(errWith < 0.10, + "observer constraints must recover A3 near truth. $diag") + assertTrue(errWith < errWithout * 0.4, + "observer constraints must sharply cut the A3 error (${errWith} vs ${errWithout} m)") + } + + @Test + fun per_edge_weight_down_weights_a_biased_edge_on_a_redundant_graph() { + // 5 anchors = 10 edges > 9 DOF ⇒ redundant, so down-weighting the one biased edge leaves + // enough good edges to recover — the signal #63's graph-residual flag will drive. + val truth = 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), + ) + val biased = distances(truth, a2 to a3, 0.6) + + val unweighted = AnchorConstellationSolver.refine(truth, biased, rootId = a1) + val weighted = AnchorConstellationSolver.refine( + truth, biased, rootId = a1, weights = mapOf((a2 to a3) to 0.01), + ) + + val errUnweighted = unweighted.positions.getValue(a3).distanceTo(truth.getValue(a3)) + val errWeighted = weighted.positions.getValue(a3).distanceTo(truth.getValue(a3)) + + assertTrue(errUnweighted > 0.10, + "the unit-weighted solve spreads the bias into A3 (A3 err=${errUnweighted} m)") + assertTrue(errWeighted < 0.05, + "down-weighting the biased edge must recover A3 (A3 err=${errWeighted} m)") + } +} -- 2.43.0