Merge remote-tracking branch 'origin/main'
This commit is contained in:
jedarden
commit
eb4ad31aae
4 changed files with 291 additions and 2 deletions
|
|
@ -47,6 +47,7 @@ import (
|
|||
"github.com/spaxel/mothership/internal/health"
|
||||
featurehelp "github.com/spaxel/mothership/internal/help"
|
||||
"github.com/spaxel/mothership/internal/ingestion"
|
||||
"github.com/spaxel/mothership/internal/fusion"
|
||||
"github.com/spaxel/mothership/internal/learning"
|
||||
"github.com/spaxel/mothership/internal/loadshed"
|
||||
"github.com/spaxel/mothership/internal/localization"
|
||||
|
|
@ -980,9 +981,20 @@ func main() {
|
|||
var selfImprovingLocalizer *localization.SelfImprovingLocalizer
|
||||
var weightStore *localization.WeightStore
|
||||
|
||||
// fusionEngine is the 3D Fresnel fusion engine (internal/fusion) that
|
||||
// produces the blobs consumed by the live 10 Hz tracker loop via
|
||||
// pm.GetTrackedBlobs()/SetTrackedBlobs (bf-3f6q). It is a function-local
|
||||
// variable (mirroring selfImprovingLocalizer above) retained for the life of
|
||||
// main: the startup seeding loop below calls SetNodePosition on it, the
|
||||
// follow-up node-seeding bead can do the same, and the live-loop goroutine
|
||||
// closure captures it to run Fuse each tick. Before bf-3f6q this engine was
|
||||
// only ever constructed in tests, so pm.trackedBlobs was always nil (bf-2fz8).
|
||||
var fusionEngine *fusion.Engine
|
||||
|
||||
// Get room configuration from fleet registry
|
||||
roomWidth := 10.0
|
||||
roomDepth := 10.0
|
||||
roomHeight := 2.5
|
||||
originX := 0.0
|
||||
originZ := 0.0
|
||||
if fleetReg != nil {
|
||||
|
|
@ -990,6 +1002,7 @@ func main() {
|
|||
if roomErr == nil && room != nil {
|
||||
roomWidth = room.Width
|
||||
roomDepth = room.Depth
|
||||
roomHeight = room.Height
|
||||
originX = room.OriginX
|
||||
originZ = room.OriginZ
|
||||
}
|
||||
|
|
@ -1004,6 +1017,23 @@ func main() {
|
|||
|
||||
selfImprovingLocalizer = localization.NewSelfImprovingLocalizer(silConfig)
|
||||
|
||||
// bf-3f6q: Construct the 3D Fresnel fusion engine (assigned to the
|
||||
// function-local fusionEngine declared above, mirroring selfImprovingLocalizer).
|
||||
// This is the engine that produces the blobs consumed by the live 10 Hz
|
||||
// tracker loop through
|
||||
// pm.GetTrackedBlobs()/SetTrackedBlobs (see the tracker goroutine below).
|
||||
// Before this it was only ever constructed in tests (bf-2fz8), so
|
||||
// pm.trackedBlobs was always nil and no reader ever saw a blob. The Config
|
||||
// mirrors the room geometry used by the 2D self-improving localizer above.
|
||||
fusionEngine = fusion.NewEngine(&fusion.Config{
|
||||
Width: roomWidth,
|
||||
Height: roomHeight,
|
||||
Depth: roomDepth,
|
||||
OriginX: originX,
|
||||
OriginY: 0, // grid origin at floor level (height axis)
|
||||
OriginZ: originZ,
|
||||
})
|
||||
|
||||
// Load persisted weights
|
||||
weightStore, err = localization.NewWeightStore(filepath.Join(cfg.DataDir, "weights.db"))
|
||||
if err != nil {
|
||||
|
|
@ -1025,6 +1055,9 @@ func main() {
|
|||
nodes, _ := fleetReg.GetAllNodes()
|
||||
for _, node := range nodes {
|
||||
selfImprovingLocalizer.SetNodePosition(node.MAC, node.PosX, node.PosY, node.PosZ)
|
||||
// Seed the 3D fusion engine's node registry (bf-3f6q). Without
|
||||
// positioned nodes, Fuse skips every link and emits no blobs.
|
||||
fusionEngine.SetNodePosition(node.MAC, node.PosX, node.PosY, node.PosZ)
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -1032,6 +1065,8 @@ func main() {
|
|||
selfImprovingLocalizer.Start()
|
||||
log.Printf("[INFO] Self-improving localization started (room: %.1fx%.1fm, interval: %v)",
|
||||
roomWidth, roomDepth, silConfig.AdjustmentInterval)
|
||||
log.Printf("[INFO] 3D fusion engine ready (room: %.1fx%.1fx%.1fm, origin %.1f,%.1f, %d nodes positioned)",
|
||||
roomWidth, roomDepth, roomHeight, originX, originZ, fusionEngine.NodeCount())
|
||||
|
||||
// Phase 6: Ground truth store for self-improving localization weights
|
||||
var groundTruthStore *localization.GroundTruthStore
|
||||
|
|
@ -1854,6 +1889,13 @@ func main() {
|
|||
go func() {
|
||||
ticker := time.NewTicker(100 * time.Millisecond) // 10 Hz
|
||||
defer ticker.Stop()
|
||||
|
||||
// Per-session blob ID bookkeeping: associates each tick's fusion peaks to
|
||||
// stable tracked IDs via greedy nearest-neighbour matching (1.0 m gate),
|
||||
// assigning fresh monotonically-increasing IDs to unmatched peaks. IDs are
|
||||
// in-memory only and reset on restart. Mirrors plan.md Component 4.
|
||||
blobTracker := newBlobTracker()
|
||||
|
||||
for {
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
|
|
@ -1861,9 +1903,17 @@ func main() {
|
|||
case <-ticker.C:
|
||||
shedder.BeginIteration()
|
||||
|
||||
// Stage 1: Get tracked blobs from fusion/tracker
|
||||
// Stage 1: Run the 3D Fresnel fusion engine and derive tracked
|
||||
// blobs from its peaks (bf-3f6q). Before this, pm.trackedBlobs was
|
||||
// always nil (bf-2fz8) so GetTrackedBlobs() returned nothing and no
|
||||
// downstream consumer (identity matching, fall detection,
|
||||
// /api/blobs) ever saw a blob. We feed link motion through Fuse,
|
||||
// convert the peaks to TrackedBlobs, and publish them via
|
||||
// SetTrackedBlobs so every reader of that path sees live blobs.
|
||||
st1 := shedder.BeginStage("fusion_track")
|
||||
blobs := pm.GetTrackedBlobs()
|
||||
result := fusionEngine.Fuse(gatherFusionLinks(pm))
|
||||
blobs := blobTracker.track(result)
|
||||
pm.SetTrackedBlobs(blobs)
|
||||
shedder.EndStage(st1)
|
||||
|
||||
if len(blobs) == 0 {
|
||||
|
|
@ -5142,3 +5192,123 @@ func splitLinkID(linkID string) []string {
|
|||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
// gatherFusionLinks converts the processor manager's current per-link motion
|
||||
// states into fusion.LinkMotion values for the 3D engine. This mirrors the
|
||||
// conversion the 2D self-improving localizer loop performs, but targets the
|
||||
// internal/fusion package (bf-3f6q). Link IDs without a valid "mac-peer" split
|
||||
// are skipped.
|
||||
func gatherFusionLinks(pm *sigproc.ProcessorManager) []fusion.LinkMotion {
|
||||
states := pm.GetAllMotionStates()
|
||||
if len(states) == 0 {
|
||||
return nil
|
||||
}
|
||||
links := make([]fusion.LinkMotion, 0, len(states))
|
||||
for _, s := range states {
|
||||
parts := splitLinkID(s.LinkID)
|
||||
if len(parts) != 2 {
|
||||
continue
|
||||
}
|
||||
health := s.AmbientConfidence
|
||||
if health <= 0 {
|
||||
health = s.BaselineConf
|
||||
}
|
||||
links = append(links, fusion.LinkMotion{
|
||||
NodeMAC: parts[0],
|
||||
PeerMAC: parts[1],
|
||||
DeltaRMS: s.SmoothDeltaRMS,
|
||||
Motion: s.MotionDetected,
|
||||
HealthScore: health,
|
||||
})
|
||||
}
|
||||
return links
|
||||
}
|
||||
|
||||
// blobAssocDistM is the maximum distance (metres) at which a new fusion peak is
|
||||
// associated to an existing tracked blob rather than spawning a fresh ID.
|
||||
const blobAssocDistM = 1.0
|
||||
|
||||
// blobTracker assigns stable per-session IDs to the 3D fusion engine's peaks so
|
||||
// that downstream consumers keyed on blob.ID (identity matching, zone tracking,
|
||||
// /api/blobs) see continuity across ticks. It is the bridge from the
|
||||
// internal/fusion engine's stateless peaks to the stateful sigproc.TrackedBlob
|
||||
// list consumed by the live loop (bf-3f6q / bf-2fz8).
|
||||
type blobTracker struct {
|
||||
nextID int
|
||||
prev map[int]sigproc.TrackedBlob // last known blob per tracked ID
|
||||
lastAt time.Time
|
||||
}
|
||||
|
||||
func newBlobTracker() *blobTracker {
|
||||
return &blobTracker{nextID: 1, prev: make(map[int]sigproc.TrackedBlob)}
|
||||
}
|
||||
|
||||
// track converts a fusion Result's peaks into TrackedBlobs, associating each
|
||||
// peak to the nearest previous blob within blobAssocDistM (greedy) and issuing
|
||||
// a new ID otherwise. Velocity is derived from the previous position of the
|
||||
// same tracked ID. A nil/empty result yields nil (clearing tracked blobs).
|
||||
func (bt *blobTracker) track(result *fusion.Result) []sigproc.TrackedBlob {
|
||||
if result == nil || len(result.Blobs) == 0 {
|
||||
return nil
|
||||
}
|
||||
|
||||
now := time.Now()
|
||||
dt := 0.1
|
||||
if !bt.lastAt.IsZero() {
|
||||
dt = now.Sub(bt.lastAt).Seconds()
|
||||
if dt < 0.01 {
|
||||
dt = 0.01
|
||||
} else if dt > 2.0 {
|
||||
dt = 2.0
|
||||
}
|
||||
}
|
||||
bt.lastAt = now
|
||||
|
||||
used := make(map[int]bool, len(bt.prev))
|
||||
out := make([]sigproc.TrackedBlob, 0, len(result.Blobs))
|
||||
next := make(map[int]sigproc.TrackedBlob, len(result.Blobs))
|
||||
|
||||
for _, pk := range result.Blobs {
|
||||
// Greedy nearest-neighbour association against previous blobs.
|
||||
bestID := 0
|
||||
bestDist := blobAssocDistM
|
||||
for id, pb := range bt.prev {
|
||||
if used[id] {
|
||||
continue
|
||||
}
|
||||
dx := pk.X - pb.X
|
||||
dy := pk.Y - pb.Y
|
||||
dz := pk.Z - pb.Z
|
||||
d := math.Sqrt(dx*dx + dy*dy + dz*dz)
|
||||
if d < bestDist {
|
||||
bestDist = d
|
||||
bestID = id
|
||||
}
|
||||
}
|
||||
|
||||
id := bestID
|
||||
if id == 0 {
|
||||
id = bt.nextID
|
||||
bt.nextID++
|
||||
} else {
|
||||
used[id] = true
|
||||
}
|
||||
|
||||
b := sigproc.TrackedBlob{
|
||||
ID: id,
|
||||
X: pk.X,
|
||||
Y: pk.Y,
|
||||
Z: pk.Z,
|
||||
Weight: pk.Confidence,
|
||||
}
|
||||
if pb, ok := bt.prev[id]; ok {
|
||||
b.VX = (pk.X - pb.X) / dt
|
||||
b.VY = (pk.Y - pb.Y) / dt
|
||||
b.VZ = (pk.Z - pb.Z) / dt
|
||||
}
|
||||
out = append(out, b)
|
||||
next[id] = b
|
||||
}
|
||||
bt.prev = next
|
||||
return out
|
||||
}
|
||||
|
|
|
|||
|
|
@ -137,6 +137,27 @@ func (e *Engine) RemoveNode(mac string) {
|
|||
e.mu.Unlock()
|
||||
}
|
||||
|
||||
// NodeCount returns the number of positioned nodes currently registered.
|
||||
func (e *Engine) NodeCount() int {
|
||||
e.mu.RLock()
|
||||
defer e.mu.RUnlock()
|
||||
return len(e.nodePos)
|
||||
}
|
||||
|
||||
// NodePositions returns a snapshot of all registered node positions.
|
||||
// The slice is a copy; callers may mutate it freely. Used by startup
|
||||
// assertions and tests to confirm the engine holds distinct, non-default
|
||||
// positions after seeding (no all-(0,0,1) collapse).
|
||||
func (e *Engine) NodePositions() []NodePosition {
|
||||
e.mu.RLock()
|
||||
defer e.mu.RUnlock()
|
||||
out := make([]NodePosition, 0, len(e.nodePos))
|
||||
for _, p := range e.nodePos {
|
||||
out = append(out, p)
|
||||
}
|
||||
return out
|
||||
}
|
||||
|
||||
// Fuse performs a single fusion step over the provided link motion states.
|
||||
// It returns a Result containing detected blob positions and confidence scores.
|
||||
// Each link's contribution is weighted by its HealthScore (0-1). A link with
|
||||
|
|
|
|||
|
|
@ -314,6 +314,73 @@ func TestEngine_RemoveNode(t *testing.T) {
|
|||
}
|
||||
}
|
||||
|
||||
// seedPosKey is the distinctness key for a 3D position (see TestEngine_SeedNodePositions).
|
||||
type seedPosKey struct{ x, y, z float64 }
|
||||
|
||||
// TestEngine_SeedNodePositions locks in the bf-6s3d startup-seeding invariant.
|
||||
// At startup main.go iterates fleetReg.GetAllNodes() and calls
|
||||
// SetNodePosition(node.MAC, node.PosX, node.PosY, node.PosZ) per node, reading the
|
||||
// DB pos_x/pos_y/pos_z columns (the columns the nodes schema defaults to 0/0/1 —
|
||||
// internal/db/migrations.go and fleet/registry.go). After that seeding the engine
|
||||
// must hold: NodeCount() == number of fleet nodes, and NodePositions() holding a
|
||||
// distinct, non-(0,0,1) position for each node (nodes must NOT collapse to the
|
||||
// co-located DB default). This test replays that seeding pattern against the engine
|
||||
// so a future refactor cannot silently regress the acceptance criteria.
|
||||
func TestEngine_SeedNodePositions(t *testing.T) {
|
||||
e := makeEngine(6, 2.5, 5)
|
||||
|
||||
// Distinct, realistic fleet placements (meters) — mirrors what
|
||||
// fleetReg.GetAllNodes() returns for a positioned fleet; none sit at the
|
||||
// (0,0,1) column default.
|
||||
seeded := []NodePosition{
|
||||
{MAC: "AA:00:00:00:00:01", X: 0.2, Y: 2.1, Z: 0.2},
|
||||
{MAC: "AA:00:00:00:00:02", X: 5.8, Y: 2.0, Z: 0.2},
|
||||
{MAC: "AA:00:00:00:00:03", X: 0.2, Y: 0.3, Z: 4.8},
|
||||
{MAC: "AA:00:00:00:00:04", X: 5.8, Y: 0.3, Z: 4.8},
|
||||
}
|
||||
for _, n := range seeded {
|
||||
e.SetNodePosition(n.MAC, n.X, n.Y, n.Z) // mirrors main.go seeding loop
|
||||
}
|
||||
|
||||
// Acceptance criterion: NodeCount() equals the number of fleet nodes.
|
||||
if got := e.NodeCount(); got != len(seeded) {
|
||||
t.Fatalf("NodeCount() = %d, want %d (must equal fleet node count from GetAllNodes)", got, len(seeded))
|
||||
}
|
||||
|
||||
// Acceptance criterion: a distinct, non-(0,0,1) position for each registered node.
|
||||
pos := e.NodePositions()
|
||||
if len(pos) != len(seeded) {
|
||||
t.Fatalf("NodePositions() returned %d entries, want %d", len(pos), len(seeded))
|
||||
}
|
||||
byMAC := make(map[string]NodePosition, len(seeded))
|
||||
for _, n := range seeded {
|
||||
byMAC[n.MAC] = n
|
||||
}
|
||||
seen := make(map[seedPosKey]bool, len(pos))
|
||||
for _, p := range pos {
|
||||
want, ok := byMAC[p.MAC]
|
||||
if !ok {
|
||||
t.Errorf("NodePositions() returned unknown MAC %q", p.MAC)
|
||||
continue
|
||||
}
|
||||
// Coordinates must round-trip exactly from what the seeding loop set.
|
||||
if p.X != want.X || p.Y != want.Y || p.Z != want.Z {
|
||||
t.Errorf("node %q position = (%.2f,%.2f,%.2f), want (%.2f,%.2f,%.2f)",
|
||||
p.MAC, p.X, p.Y, p.Z, want.X, want.Y, want.Z)
|
||||
}
|
||||
// Must NOT be the co-located DB default.
|
||||
if p.X == 0 && p.Y == 0 && p.Z == 1 {
|
||||
t.Errorf("node %q seeded at DB default (0,0,1) — positions must come from DB columns", p.MAC)
|
||||
}
|
||||
// Must be distinct from every other node.
|
||||
k := seedPosKey{p.X, p.Y, p.Z}
|
||||
if seen[k] {
|
||||
t.Errorf("node %q duplicates another node's position (%.2f,%.2f,%.2f) — positions must be distinct", p.MAC, p.X, p.Y, p.Z)
|
||||
}
|
||||
seen[k] = true
|
||||
}
|
||||
}
|
||||
|
||||
// TestEngine_HealthWeight verifies that links with lower health scores contribute less to fusion.
|
||||
// Per spec: "each link's contribution to the 3D occupancy grid is multiplied by its health_score"
|
||||
func TestEngine_HealthWeight(t *testing.T) {
|
||||
|
|
|
|||
|
|
@ -599,6 +599,35 @@ type TrackedBlob struct {
|
|||
}
|
||||
|
||||
// SetTrackedBlobs stores the latest tracked blobs from the fusion engine.
|
||||
//
|
||||
// INVESTIGATION NOTE (bf-2fz8 / bf-3gw1 umbrella): this setter is the ONLY
|
||||
// writer of pm.trackedBlobs, and as of this writing it has NO callers anywhere
|
||||
// in the repo (verified by grep across all non-test and test code). Therefore
|
||||
// pm.trackedBlobs is always nil and GetTrackedBlobs() always returns an empty
|
||||
// slice. Every reader sees zero blobs, so IO-6 ("walker produces a tracked
|
||||
// blob") cannot pass. Verified read sites of GetTrackedBlobs:
|
||||
// - live 10 Hz loop: cmd/mothership/main.go:1866
|
||||
// - /api/blobs REST handler: cmd/mothership/main.go:4056
|
||||
// - anomalyPositionAdapter: cmd/mothership/main.go:4840
|
||||
// - API-layer wrappers: internal/api/status.go:84, internal/api/tracks.go:99
|
||||
//
|
||||
// The intended producer is internal/fusion.NewEngine (the 3D Fresnel engine,
|
||||
// internal/fusion/fusion.go:93), but that constructor is NEVER called in
|
||||
// non-test code — the internal/fusion package is imported by exactly one file,
|
||||
// internal/localizer/fusion/timing_budget_test.go (a test); the only NewEngine
|
||||
// call sites are in that test and internal/fusion/fusion_test.go. A second
|
||||
// engine, internal/localization.NewEngine (2D Fresnel,
|
||||
// internal/localization/fusion.go:53), IS constructed in the live path via
|
||||
// NewSelfImprovingLocalizer (main.go:1005) and runs Fuse (main.go:2593), but
|
||||
// the returned *FusionResult is DISCARDED — nothing reads .Peaks; only
|
||||
// GetLearnedWeights() is read afterward (main.go:2599) for weight persistence.
|
||||
// No signal.TrackedBlob literal exists outside test code.
|
||||
//
|
||||
// CONCLUSION: no engine currently feeds the live blob loop. To satisfy IO-6,
|
||||
// internal/fusion.NewEngine MUST be newly constructed in non-test code and
|
||||
// wired (Fuse -> TrackedBlob -> SetTrackedBlobs), OR the existing
|
||||
// localization.Engine FusionResult.Peaks must be converted to TrackedBlobs
|
||||
// and stored here via SetTrackedBlobs.
|
||||
func (pm *ProcessorManager) SetTrackedBlobs(blobs []TrackedBlob) {
|
||||
pm.mu.Lock()
|
||||
defer pm.mu.Unlock()
|
||||
|
|
@ -606,6 +635,8 @@ func (pm *ProcessorManager) SetTrackedBlobs(blobs []TrackedBlob) {
|
|||
}
|
||||
|
||||
// GetTrackedBlobs returns the latest tracked blobs from the fusion engine.
|
||||
// See SetTrackedBlobs for the bf-2fz8 finding: until that setter is wired to
|
||||
// an engine, this always returns nil.
|
||||
func (pm *ProcessorManager) GetTrackedBlobs() []TrackedBlob {
|
||||
pm.mu.RLock()
|
||||
defer pm.mu.RUnlock()
|
||||
|
|
|
|||
Loading…
Add table
Reference in a new issue