Improve odometry with rotation estimation, daemon stability, and RTSP fixes

- Add rotation estimation via log-polar phase correlation (EstimateRotation, rotateGray) — de-rotate before translation to avoid aliasing - Track cumulative field rotation in Tracker, reject low-confidence frames - Extract RTSP grabber into internal/rtspgrab module - Daemon: pause odometry during slew (motion blur), auto-resume after settle delay, add /pause and /resume HTTP endpoints, switch shooting mode before opening camera, use ReqOpenCamera with rtsp_encode_type=1 - WebSocket heartbeat (ping/pong every 10s) to prevent idle disconnects - FFmpeg low-latency flags (-fflags nobuffer, -probesize, -analyzeduration) - Add SendRaw API for custom payloads, increase daemon HTTP timeout to 60s 💘 Generated with Crush Assisted-by: Crush:/models/Qwen3.6-27B-uncensored-heretic-v2-Native-MTP-Preserved-Q4_K_M.gguf
2026-07-13 23:00:30 +02:00
parent 8daa9b4d58
commit d319ef6b4a
12 changed files with 522 additions and 143 deletions
--- a/dwarfctl/internal/odometry/odometry.go
+++ b/dwarfctl/internal/odometry/odometry.go
@ -19,20 +19,19 @@ import (
 	"os"
 )

-// Result holds the rotation estimated between two frames.
+// Result holds the motion estimated between two frames.
 type Result struct {
-	// PanPx / TiltPx are the image-plane shift (in resized pixels) of the scene
-	// from frame 1 to frame 2. Positive PanPx = scene moved right; positive
-	// TiltPx = scene moved down. To point back at the original target, slew the
-	// motors in the opposite direction.
+	// PanPx / TiltPx are the image-plane translation (in resized pixels).
 	PanPx  float64
 	TiltPx float64
 	// PanDeg / TiltDeg convert the pixel shift to degrees using the supplied FoV.
 	PanDeg  float64
 	TiltDeg float64
-	// Confidence in [0,1]: the normalized phase-correlation peak height. Higher
-	// is better; below ~0.05 the frames are likely too flat or the motion too
-	// large to be trusted.
+	// RotDeg is the estimated field rotation (degrees). Positive = scene rotated
+	// counter-clockwise. Dominant when the telescope points near zenith on an
+	// alt-az mount (azimuth rotation → field rotation, not translation).
+	RotDeg float64
+	// Confidence in [0,1].
 	Confidence float64
 	// Size is the FFT grid dimension actually used.
 	Size int
@ -78,13 +77,31 @@ func Estimate(img1, img2 image.Image, fovXDeg, fovYDeg float64, size int) (Resul
 	g1 := toGrayDownscaled(img1, size, size)
 	g2 := toGrayDownscaled(img2, size, size)

-	dx, dy, conf := phaseCorrelation(g1, g2, size, size)
+	// Pass 1: estimate rotation (log-polar phase correlation).
+	rotDeg, confRot := EstimateRotation(g1, g2, size, size)
+
+	// Pass 2: de-rotate g2, then estimate translation on de-rotated images.
+	// This removes the rotation-induced aliasing that corrupts translation.
+	var dx, dy, confTrans float64
+	if absf(rotDeg) > 0.3 {
+		g2Derot := rotateGray(g2, size, rotDeg)
+		dx, dy, confTrans = phaseCorrelation(g1, g2Derot, size, size)
+	} else {
+		dx, dy, confTrans = phaseCorrelation(g1, g2, size, size)
+	}
+
+	// Use the higher-confidence metric as the overall confidence.
+	conf := confTrans
+	if confRot > conf {
+		conf = confRot
+	}

 	r := Result{
 		PanPx:      dx,
 		TiltPx:     dy,
 		PanDeg:     dx * fovXDeg / float64(size),
 		TiltDeg:    dy * fovYDeg / float64(size),
+		RotDeg:     rotDeg,
 		Confidence: conf,
 		Size:       size,
 	}
--- a/dwarfctl/internal/odometry/rotate.go
+++ b/dwarfctl/internal/odometry/rotate.go
@ -0,0 +1,53 @@
+package odometry
+
+import "math"
+
+// rotateGray rotates a grayscale matrix by angleDeg (positive = clockwise)
+// around its center, using bilinear interpolation. The output has the same
+// dimensions as the input.
+func rotateGray(g [][]float64, size int, angleDeg float64) [][]float64 {
+	theta := angleDeg * math.Pi / 180.0
+	cosT := math.Cos(theta)
+	sinT := math.Sin(theta)
+	cy := float64(size-1) / 2.0
+	cx := float64(size-1) / 2.0
+
+	out := make([][]float64, size)
+	for i := range out {
+		out[i] = make([]float64, size)
+	}
+
+	for oy := 0; oy < size; oy++ {
+		for ox := 0; ox < size; ox++ {
+			// Map output pixel to input coordinates via inverse rotation.
+			dy := float64(oy) - cy
+			dx := float64(ox) - cx
+			sx := cx + dx*cosT + dy*sinT
+			sy := cy - dx*sinT + dy*cosT
+
+			// Bilinear interpolation with clamp at borders.
+			x0 := int(math.Floor(sx))
+			y0 := int(math.Floor(sy))
+			fx := sx - float64(x0)
+			fy := sy - float64(y0)
+
+			x0 = clampInt(x0, 0, size-1)
+			y0 = clampInt(y0, 0, size-1)
+			x1 := clampInt(x0+1, 0, size-1)
+			y1 := clampInt(y0+1, 0, size-1)
+
+			out[oy][ox] = (1-fy)*(1-fx)*g[y0][x0] +
+				(1-fy)*fx*g[y0][x1] +
+				fy*(1-fx)*g[y1][x0] +
+				fy*fx*g[y1][x1]
+		}
+	}
+	return out
+}
+
+func absf(x float64) float64 {
+	if x < 0 {
+		return -x
+	}
+	return x
+}
--- a/dwarfctl/internal/odometry/rotation.go
+++ b/dwarfctl/internal/odometry/rotation.go
@ -0,0 +1,187 @@
+package odometry
+
+import (
+	"math"
+	"math/cmplx"
+)
+
+// EstimateRotation estimates the rotation angle (in degrees) between two
+// grayscale matrices using the log-polar phase correlation method:
+//
+//  1. Compute the magnitude spectra |F1|, |F2| (rotation in spatial domain =
+//     rotation in frequency domain).
+//  2. Apply a high-pass filter to suppress the DC-dominated center.
+//  3. Resample to log-polar coordinates: rotation becomes a shift along the
+//     angular axis, scale becomes a shift along the log-radius axis.
+//  4. Phase-correlate in log-polar space → the peak along the angular axis
+//     gives the rotation angle.
+//
+// Returns the rotation angle in degrees (positive = counter-clockwise rotation
+// of the scene from g1 to g2), and a confidence in [0,1].
+func EstimateRotation(g1, g2 [][]float64, rows, cols int) (angleDeg float64, conf float64) {
+	// 1. FFT both images.
+	a := windowedComplex(g1, rows, cols)
+	b := windowedComplex(g2, rows, cols)
+	fft2(a, false)
+	fft2(b, false)
+
+	// 2. Magnitude spectra with high-pass filtering.
+	magA := make([][]float64, rows)
+	magB := make([][]float64, rows)
+	cy, cx := float64(rows)/2, float64(cols)/2
+	maxR := math.Min(cy, cx)
+	for i := 0; i < rows; i++ {
+		magA[i] = make([]float64, cols)
+		magB[i] = make([]float64, cols)
+		for j := 0; j < cols; j++ {
+			// Shift zero-frequency to center (fftshift).
+			si := i
+			sj := j
+			if i < rows/2 {
+				si = i + rows/2
+			} else {
+				si = i - rows/2
+			}
+			if j < cols/2 {
+				sj = j + cols/2
+			} else {
+				sj = j - cols/2
+			}
+			ma := cmplx.Abs(a[si][sj])
+			mb := cmplx.Abs(b[si][sj])
+			// High-pass: suppress frequencies near DC.
+			dist := math.Sqrt(float64((float64(i)-cy)*(float64(i)-cy)) + float64((float64(j)-cx)*(float64(j)-cx)))
+			hp := 1.0 - math.Exp(-(dist*dist)/(2*(maxR*0.1)*(maxR*0.1)))
+			magA[i][j] = ma * hp
+			magB[i][j] = mb * hp
+		}
+	}
+
+	// 3. Resample magnitude spectra to log-polar coordinates.
+	nAngles := 256   // angular resolution
+	nRadii := 128    // radial resolution
+	lpA := logPolarResample(magA, rows, cols, nAngles, nRadii, maxR)
+	lpB := logPolarResample(magB, rows, cols, nAngles, nRadii, maxR)
+
+	// 4. Phase-correlate in log-polar space.
+	dx, dy, confRaw := phaseCorrelationFloat(lpA, lpB, nAngles, nRadii)
+
+	// dy is the shift along the angular axis → rotation angle.
+	// Each row in lpA/lpB corresponds to 360/nAngles degrees.
+	angleDeg = -float64(dy) * 360.0 / float64(nAngles)
+	// Wrap to [-180, 180)
+	for angleDeg < -180 {
+		angleDeg += 360
+	}
+	for angleDeg >= 180 {
+		angleDeg -= 360
+	}
+
+	_ = dx // log-radius shift (scale change) — not used for rotation-only estimation
+	conf = confRaw
+	return angleDeg, conf
+}
+
+// logPolarResample converts a Cartesian matrix to log-polar coordinates.
+// The output has nAngles rows (angular samples) and nRadii columns (log-radius
+// samples). This transforms a rotation in Cartesian space into a cyclic shift
+// along the angular (row) axis.
+func logPolarResample(mag [][]float64, rows, cols, nAngles, nRadii int, maxR float64) [][]float64 {
+	out := make([][]float64, nAngles)
+	cy, cx := float64(rows)/2, float64(cols)/2
+
+	logMinR := math.Log(2.0)
+	logMaxR := math.Log(maxR)
+	rStep := (logMaxR - logMinR) / float64(nRadii-1)
+
+	for a := 0; a < nAngles; a++ {
+		theta := 2 * math.Pi * float64(a) / float64(nAngles)
+		out[a] = make([]float64, nRadii)
+		for r := 0; r < nRadii; r++ {
+			radius := math.Exp(logMinR + float64(r)*rStep)
+			y := cy + radius*math.Sin(theta)
+			x := cx + radius*math.Cos(theta)
+			// Bilinear interpolation.
+			y0 := int(math.Floor(y))
+			x0 := int(math.Floor(x))
+			fy := y - float64(y0)
+			fx := x - float64(x0)
+			y0 = clampInt(y0, 0, rows-1)
+			x0 = clampInt(x0, 0, cols-1)
+			y1 := clampInt(y0+1, 0, rows-1)
+			x1 := clampInt(x0+1, 0, cols-1)
+			v := (1-fy)*(1-fx)*mag[y0][x0] +
+				(1-fy)*fx*mag[y0][x1] +
+				fy*(1-fx)*mag[y1][x0] +
+				fy*fx*mag[y1][x1]
+			out[a][r] = v
+		}
+	}
+	return out
+}
+
+// phaseCorrelationFloat is a float-based phase correlation (same algorithm as
+// phaseCorrelation but operates on float64 matrices instead of complex, using
+// its own internal FFT buffers).
+func phaseCorrelationFloat(g1, g2 [][]float64, rows, cols int) (dx, dy, conf float64) {
+	a := windowedComplex(g1, rows, cols)
+	b := windowedComplex(g2, rows, cols)
+
+	fft2(a, false)
+	fft2(b, false)
+
+	r := make([][]complex128, rows)
+	for i := 0; i < rows; i++ {
+		r[i] = make([]complex128, cols)
+		for j := 0; j < cols; j++ {
+			cross := cmplx.Conj(a[i][j]) * b[i][j]
+			mag := cmplx.Abs(cross)
+			if mag > 1e-12 {
+				r[i][j] = cross / complex(mag, 0)
+			}
+		}
+	}
+
+	fft2(r, true)
+
+	px, py, peak := 0, 0, math.Inf(-1)
+	mean := 0.0
+	for i := 0; i < rows; i++ {
+		for j := 0; j < cols; j++ {
+			v := real(r[i][j])
+			mean += v
+			if v > peak {
+				peak = v
+				px, py = j, i
+			}
+		}
+	}
+	mean /= float64(rows * cols)
+
+	dx = float64(signedShift(px, cols))
+	dy = float64(signedShift(py, rows))
+
+	// Sub-pixel refinement.
+	dx += parabola(at2(r, py, px-1, cols), peak, at2(r, py, px+1, cols))
+	dy += parabola(at2(r, py-1, px, rows), peak, at2(r, py+1, px, rows))
+
+	denom := peak - mean
+	if denom > 1 {
+		denom = 1
+	}
+	if denom < 0 {
+		denom = 0
+	}
+	conf = denom
+	return dx, dy, conf
+}
+
+func clampInt(v, lo, hi int) int {
+	if v < lo {
+		return lo
+	}
+	if v > hi {
+		return hi
+	}
+	return v
+}
--- a/dwarfctl/internal/odometry/tracker.go
+++ b/dwarfctl/internal/odometry/tracker.go
@ -6,6 +6,11 @@ import (
 	"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.
@ -29,6 +34,7 @@ type Tracker struct {
 	// Cumulative orientation in degrees, relative to the first frame.
 	panDeg  float64
 	tiltDeg float64
+	rotDeg  float64 // cumulative field rotation

 	// Number of frames integrated.
 	frames int
@ -61,11 +67,12 @@ func NewTracker(fovXDeg, fovYDeg float64, size int) *Tracker {

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

 func (o Orientation) String() string {
@ -73,8 +80,8 @@ func (o Orientation) String() string {
 	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)
+	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.
@ -96,16 +103,34 @@ func (t *Tracker) Update(img image.Image) (Orientation, error) {
 		return t.snapshot(), nil
 	}

-	dx, dy, conf := phaseCorrelation(t.prev, cur, t.size, t.size)
+	dx, dy, confTrans := phaseCorrelation(t.prev, cur, t.size, t.size)
+	rotDeg, confRot := EstimateRotation(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.
+	// 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.
@ -134,6 +159,7 @@ func (t *Tracker) Reset() {
 	defer t.mu.Unlock()
 	t.panDeg = 0
 	t.tiltDeg = 0
+	t.rotDeg = 0
 	t.frames = 0
 	t.confidence = 0
 	t.prev = nil
@ -157,6 +183,7 @@ 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,