Project

General

Profile

Download (16.4 KB)

User Story #79 » 0002-feat-engine-over-determined-refine-per-edge-weights-.patch

knight8241, 08/07/2026 18:33

View differences:

common/src/commonMain/kotlin/com/aether/mofe/engine/AnchorConstellationSolver.kt
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<Pair<DeviceId, DeviceId>, 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<ObserverConstraint> = 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,
......
): 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
......
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
......
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) {
......
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)
......
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<Pair<DeviceId, DeviceId>, Double>,
known: Set<DeviceId>,
weights: Map<Pair<DeviceId, DeviceId>, Double> = emptyMap(),
): List<Edge> {
val out = ArrayList<Edge>()
val seen = HashSet<String>()
......
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]
}
}
}
......
private fun cost(
pos: Map<DeviceId, Vector3D>,
edges: List<Edge>,
fixedEdges: List<FixedEdge>,
prior: Map<String, Vector3D>,
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
common/src/commonTest/kotlin/com/aether/mofe/engine/AnchorConstellationSolverAugmentTest.kt
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<DeviceId, Vector3D>, biasPair: Pair<DeviceId, DeviceId>, bias: Double,
): Map<Pair<DeviceId, DeviceId>, Double> {
val ids = pts.keys.toList()
val out = HashMap<Pair<DeviceId, DeviceId>, 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)")
}
}
    (1-1/1)