dwarf-go/dwarfctl/internal/odometry/tracker.go

package odometry

import (
	"fmt"
	"image"
	"sync"
)

// Tracker is a stateful visual odometry integrator. It accumulates the
// frame-to-frame rotation measured by Estimate into a cumulative pointing
// orientation (pan/tilt) relative to wherever the first frame was captured.
//
// This is what lets a static, un-aligned alt-az telescope know "where it is
// pointing now" purely from the wide-angle camera — no absolute encoders, no
// plate solving, no polar alignment required. Every time the motors slew (to
// track an airplane / satellite), the scene shifts; the tracker measures that
// shift and integrates it into the running orientation.
//
// Convention: PanDeg positive = pointing further right (eastward for a level
// alt-az mount); TiltDeg positive = pointing further up. The sign follows the
// MOTOR motion, i.e. it is the negative of the raw scene shift, so the values
// describe where the optical axis now points.
type Tracker struct {
	mu sync.Mutex

	fovXDeg, fovYDeg float64
	size             int

	// Cumulative orientation in degrees, relative to the first frame.
	panDeg  float64
	tiltDeg float64

	// Number of frames integrated.
	frames int

	// Running confidence (exponential moving average of per-frame confidence).
	confidence float64

	// Last grayscale frame, kept so the next Update only needs one new image.
	prev [][]float64

	// Set once the first frame establishes the reference.
	primed bool
}

// NewTracker creates a visual-odometry tracker.
// fovXDeg/fovYDeg: wide camera horizontal/vertical field of view in degrees.
// size: FFT grid side (power of two, e.g. 256 or 512).
func NewTracker(fovXDeg, fovYDeg float64, size int) *Tracker {
	if size <= 0 {
		size = DefaultSize
	}
	if fovXDeg <= 0 {
		fovXDeg = DefaultFoVH
	}
	if fovYDeg <= 0 {
		fovYDeg = DefaultFoVV
	}
	return &Tracker{fovXDeg: fovXDeg, fovYDeg: fovYDeg, size: size}
}

// Orientation is a snapshot of the tracker's current state.
type Orientation struct {
	PanDeg     float64 // cumulative pan (right positive), degrees
	TiltDeg    float64 // cumulative tilt (up positive), degrees
	Frames     int     // number of frames integrated
	Confidence float64 // EMA of per-frame phase-correlation confidence [0,1]
	Primed     bool    // true once at least one frame has been processed
}

func (o Orientation) String() string {
	state := "priming"
	if o.Primed {
		state = "tracking"
	}
	return fmt.Sprintf("pan=%+.4f° tilt=%+.4f° conf=%.3f frames=%d [%s]",
		o.PanDeg, o.TiltDeg, o.Confidence, o.Frames, state)
}

// Update feeds the tracker a new frame and returns the updated orientation.
// The very first call establishes the reference origin (orientation 0,0) and
// performs no measurement. Subsequent calls measure the rotation since the
// previous frame and integrate it.
func (t *Tracker) Update(img image.Image) (Orientation, error) {
	if img == nil {
		return Orientation{}, fmt.Errorf("odometry: nil image")
	}
	t.mu.Lock()
	defer t.mu.Unlock()

	cur := toGrayDownscaled(img, t.size, t.size)

	if !t.primed {
		t.prev = cur
		t.primed = true
		return t.snapshot(), nil
	}

	dx, dy, conf := phaseCorrelation(t.prev, cur, t.size, t.size)

	// Convert pixel shift to degrees. The scene shifts OPPOSITE to the telescope
	// motion: if the telescope pans right, the background moves left in the
	// image. We negate so PanDeg/TiltDeg track the optical axis, not the scene.
	panStep := -dx * t.fovXDeg / float64(t.size)
	tiltStep := -dy * t.fovYDeg / float64(t.size)

	t.panDeg += panStep
	t.tiltDeg += tiltStep
	t.frames++

	// Exponential moving average of the confidence.
	if t.frames == 1 {
		t.confidence = conf
	} else {
		const alpha = 0.2
		t.confidence = alpha*conf + (1-alpha)*t.confidence
	}

	t.prev = cur
	return t.snapshot(), nil
}

// Orientation returns the current cumulative orientation without adding a frame.
func (t *Tracker) Orientation() Orientation {
	t.mu.Lock()
	defer t.mu.Unlock()
	return t.snapshot()
}

// Reset zeros the accumulated orientation and makes the next Update the new
// reference origin.
func (t *Tracker) Reset() {
	t.mu.Lock()
	defer t.mu.Unlock()
	t.panDeg = 0
	t.tiltDeg = 0
	t.frames = 0
	t.confidence = 0
	t.prev = nil
	t.primed = false
}

// SetFoV overrides the field of view (degrees). Call between frames; the next
// Update uses the new values.
func (t *Tracker) SetFoV(fovXDeg, fovYDeg float64) {
	t.mu.Lock()
	defer t.mu.Unlock()
	if fovXDeg > 0 {
		t.fovXDeg = fovXDeg
	}
	if fovYDeg > 0 {
		t.fovYDeg = fovYDeg
	}
}

func (t *Tracker) snapshot() Orientation {
	return Orientation{
		PanDeg:     t.panDeg,
		TiltDeg:    t.tiltDeg,
		Frames:     t.frames,
		Confidence: t.confidence,
		Primed:     t.primed,
	}
}