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

package odometry

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

// minConfidence is the threshold below which a frame is rejected (not
// accumulated into the orientation). Motion-blurred frames or frames with
// too little texture produce confidence near 0 and corrupt the estimate.
const minConfidence = 0.15

// 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
	rotDeg  float64 // cumulative field rotation

	// 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
	TiltDeg    float64
	RotDeg     float64 // cumulative field rotation (degrees)
	Frames     int
	Confidence float64
	Primed     bool
}

func (o Orientation) String() string {
	state := "priming"
	if o.Primed {
		state = "tracking"
	}
	return fmt.Sprintf("pan=%+.4f° tilt=%+.4f° rot=%+.4f° conf=%.3f frames=%d [%s]",
		o.PanDeg, o.TiltDeg, o.RotDeg, 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, confTrans := phaseCorrelation(t.prev, cur, t.size, t.size)
	rotDeg, confRot := EstimateRotation(t.prev, cur, t.size, t.size)

	// Use the higher confidence for the EMA.
	conf := confTrans
	if confRot > conf {
		conf = confRot
	}

	// Reject low-confidence frames (motion blur, bad texture, etc.).
	if conf < minConfidence {
		t.prev = cur // still update the reference frame
		return t.snapshot(), nil
	}

	// De-rotate for accurate translation if rotation is significant.
	if absf(rotDeg) > 0.3 {
		curDerot := rotateGray(cur, t.size, rotDeg)
		dx, dy, confTrans = phaseCorrelation(t.prev, curDerot, t.size, t.size)
	}

	// Convert pixel shift to degrees.
	panStep := -dx * t.fovXDeg / float64(t.size)
	tiltStep := -dy * t.fovYDeg / float64(t.size)

	t.panDeg += panStep
	t.tiltDeg += tiltStep
	t.rotDeg += rotDeg
	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.rotDeg = 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,
		RotDeg:     t.rotDeg,
		Frames:     t.frames,
		Confidence: t.confidence,
		Primed:     t.primed,
	}
}