Add camera streaming guide with all access methods
Comprehensive documentation for accessing the DWARF camera streams:
- RTSP URLs and channels (ch0=tele, ch1=wide)
- The critical prerequisite: camera must be opened via WebSocket first
- ffmpeg commands for frame capture, timelapse, and video recording
- mpv and VLC usage with TCP transport
- Python/OpenCV integration example
- Troubleshooting common issues (black image, connection refused, VLC delay)
- Comparison of RTSP vs MJPEG modes across device models
💘 Generated with Crush
Assisted-by: Crush:glm-5.2
This commit is contained in:
Jacquin Antoine
85
dwarfctl/internal/odometry/fft.go
Normal file
85
dwarfctl/internal/odometry/fft.go
Normal file
@ -0,0 +1,85 @@
|
||||
package odometry
|
||||
|
||||
import "math"
|
||||
|
||||
// fft is an in-place iterative radix-2 Cooley-Tukey FFT.
|
||||
// len(a) must be a power of two. If inverse is true the result is normalized
|
||||
// by 1/n (so ifft(fft(x)) == x).
|
||||
func fft(a []complex128, inverse bool) {
|
||||
n := len(a)
|
||||
if n <= 1 {
|
||||
return
|
||||
}
|
||||
|
||||
// Bit-reversal permutation.
|
||||
for i, j := 1, 0; i < n; i++ {
|
||||
bit := n >> 1
|
||||
for ; j&bit != 0; bit >>= 1 {
|
||||
j ^= bit
|
||||
}
|
||||
j ^= bit
|
||||
if i < j {
|
||||
a[i], a[j] = a[j], a[i]
|
||||
}
|
||||
}
|
||||
|
||||
// Butterfly stages.
|
||||
for length := 2; length <= n; length <<= 1 {
|
||||
// Standard forward DFT uses exp(-i*2π/length); inverse negates.
|
||||
angle := -2 * math.Pi / float64(length)
|
||||
if inverse {
|
||||
angle = -angle
|
||||
}
|
||||
wlen := complex(math.Cos(angle), math.Sin(angle))
|
||||
half := length / 2
|
||||
for i := 0; i < n; i += length {
|
||||
var w complex128 = 1
|
||||
for k := 0; k < half; k++ {
|
||||
u := a[i+k]
|
||||
v := a[i+k+half] * w
|
||||
a[i+k] = u + v
|
||||
a[i+k+half] = u - v
|
||||
w *= wlen
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if inverse {
|
||||
invN := complex(1/float64(n), 0)
|
||||
for i := range a {
|
||||
a[i] *= invN
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// fftRows applies a 1-D FFT to every row of m (in place).
|
||||
func fftRows(m [][]complex128, inverse bool) {
|
||||
for i := range m {
|
||||
fft(m[i], inverse)
|
||||
}
|
||||
}
|
||||
|
||||
// fftCols applies a 1-D FFT to every column of m (in place).
|
||||
func fftCols(m [][]complex128, inverse bool) {
|
||||
rows := len(m)
|
||||
if rows == 0 {
|
||||
return
|
||||
}
|
||||
cols := len(m[0])
|
||||
col := make([]complex128, rows)
|
||||
for j := 0; j < cols; j++ {
|
||||
for i := 0; i < rows; i++ {
|
||||
col[i] = m[i][j]
|
||||
}
|
||||
fft(col, inverse)
|
||||
for i := 0; i < rows; i++ {
|
||||
m[i][j] = col[i]
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// fft2 performs a forward 2-D FFT on the rows×cols complex matrix m.
|
||||
func fft2(m [][]complex128, inverse bool) {
|
||||
fftRows(m, inverse)
|
||||
fftCols(m, inverse)
|
||||
}
|
||||
284
dwarfctl/internal/odometry/odometry.go
Normal file
284
dwarfctl/internal/odometry/odometry.go
Normal file
@ -0,0 +1,284 @@
|
||||
// Package odometry estimates the pointing rotation of the telescope between two
|
||||
// wide-angle frames using image-based phase correlation.
|
||||
//
|
||||
// It is deliberately NOT star/plate-solving based: phase correlation works on
|
||||
// any textured scene (landscape, horizon, daytime sky, clouds, ...), which is
|
||||
// exactly what the wide camera sees. For small telescope rotations the image
|
||||
// content undergoes an almost pure translation (pan -> horizontal shift,
|
||||
// tilt -> vertical shift), so the recovered image-plane shift maps directly to
|
||||
// angular pan/tilt via the camera field of view.
|
||||
package odometry
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"image"
|
||||
_ "image/jpeg"
|
||||
_ "image/png"
|
||||
"math"
|
||||
"math/cmplx"
|
||||
"os"
|
||||
)
|
||||
|
||||
// Result holds the rotation 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 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.
|
||||
Confidence float64
|
||||
// Size is the FFT grid dimension actually used.
|
||||
Size int
|
||||
}
|
||||
|
||||
func (r Result) String() string {
|
||||
return fmt.Sprintf(
|
||||
"pan=%+.3fpx (%+.4f°) tilt=%+.3fpx (%+.4f°) conf=%.3f",
|
||||
r.PanPx, r.PanDeg, r.TiltPx, r.TiltDeg, r.Confidence)
|
||||
}
|
||||
|
||||
// DefaultSize is the default FFT grid (power of two). 256 is fast and accurate
|
||||
// to ~0.1px; 512 improves sub-pixel resolution at ~4x the CPU cost.
|
||||
const DefaultSize = 256
|
||||
|
||||
// DefaultFoVH/V are the documented DWARF Mini "wide" display FoV (degrees).
|
||||
// NOTE: firmware-reported live FoV can differ widely (see analysis/DEVICE_MODELS.md),
|
||||
// so these are only a starting point — calibrate with a known motor move.
|
||||
const (
|
||||
DefaultFoVH = 8.0
|
||||
DefaultFoVV = 6.5
|
||||
)
|
||||
|
||||
// Estimate computes the rotation between two decoded images.
|
||||
//
|
||||
// fovXDeg/fovYDeg are the camera horizontal/vertical field of view in degrees
|
||||
// (they only scale the pixel shift into degrees; pass DefaultFoVH/V if unsure).
|
||||
// size is the square FFT grid (must be a power of two, e.g. 256 or 512).
|
||||
func Estimate(img1, img2 image.Image, fovXDeg, fovYDeg float64, size int) (Result, error) {
|
||||
if img1 == nil || img2 == nil {
|
||||
return Result{}, fmt.Errorf("odometry: nil image")
|
||||
}
|
||||
if !isPow2(size) {
|
||||
return Result{}, fmt.Errorf("odometry: size %d is not a power of two", size)
|
||||
}
|
||||
if size < 8 {
|
||||
return Result{}, fmt.Errorf("odometry: size %d too small", size)
|
||||
}
|
||||
if fovXDeg <= 0 || fovYDeg <= 0 {
|
||||
return Result{}, fmt.Errorf("odometry: fov must be positive (got %.3f x %.3f)", fovXDeg, fovYDeg)
|
||||
}
|
||||
|
||||
g1 := toGrayDownscaled(img1, size, size)
|
||||
g2 := toGrayDownscaled(img2, size, size)
|
||||
|
||||
dx, dy, conf := phaseCorrelation(g1, g2, size, size)
|
||||
|
||||
r := Result{
|
||||
PanPx: dx,
|
||||
TiltPx: dy,
|
||||
PanDeg: dx * fovXDeg / float64(size),
|
||||
TiltDeg: dy * fovYDeg / float64(size),
|
||||
Confidence: conf,
|
||||
Size: size,
|
||||
}
|
||||
return r, nil
|
||||
}
|
||||
|
||||
// EstimateFiles decodes two image files and runs Estimate.
|
||||
func EstimateFiles(path1, path2 string, fovXDeg, fovYDeg float64, size int) (Result, error) {
|
||||
img1, err := decodeImage(path1)
|
||||
if err != nil {
|
||||
return Result{}, fmt.Errorf("decode %s: %w", path1, err)
|
||||
}
|
||||
img2, err := decodeImage(path2)
|
||||
if err != nil {
|
||||
return Result{}, fmt.Errorf("decode %s: %w", path2, err)
|
||||
}
|
||||
return Estimate(img1, img2, fovXDeg, fovYDeg, size)
|
||||
}
|
||||
|
||||
func decodeImage(path string) (image.Image, error) {
|
||||
f, err := os.Open(path)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
defer f.Close()
|
||||
img, _, err := image.Decode(f)
|
||||
return img, err
|
||||
}
|
||||
|
||||
// phaseCorrelation returns the integer+subpixel shift (dx,dy) of the scene from
|
||||
// g1 to g2, plus a confidence metric. The shift is expressed so that
|
||||
// g2(x,y) ≈ g1(x-dx, y-dy), i.e. positive dx => content moved to the right.
|
||||
func phaseCorrelation(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)
|
||||
|
||||
// Normalized cross-power spectrum. conj(A)*B peaks at the shift that maps
|
||||
// image A (g1) onto image B (g2): positive dx = content moved right.
|
||||
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) // inverse transform -> correlation surface
|
||||
|
||||
// Peak of the real part.
|
||||
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)
|
||||
|
||||
// Wrap-around to signed shift.
|
||||
dx = float64(signedShift(px, cols))
|
||||
dy = float64(signedShift(py, rows))
|
||||
|
||||
// Sub-pixel refinement via 3-point parabolic interpolation along each axis.
|
||||
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))
|
||||
|
||||
// Confidence: peak height relative to the surface mean (clamped to [0,1]).
|
||||
denom := peak - mean
|
||||
if denom > 1 {
|
||||
denom = 1
|
||||
}
|
||||
if denom < 0 {
|
||||
denom = 0
|
||||
}
|
||||
conf = denom
|
||||
return dx, dy, conf
|
||||
}
|
||||
|
||||
// windowedComplex converts a grayscale matrix to complex, subtracts the DC
|
||||
// component and applies a 2-D Hann window to reduce spectral leakage at the
|
||||
// image borders.
|
||||
func windowedComplex(g [][]float64, rows, cols int) [][]complex128 {
|
||||
// Mean (DC) removal.
|
||||
dc := 0.0
|
||||
for i := 0; i < rows; i++ {
|
||||
for j := 0; j < cols; j++ {
|
||||
dc += g[i][j]
|
||||
}
|
||||
}
|
||||
dc /= float64(rows * cols)
|
||||
|
||||
m := make([][]complex128, rows)
|
||||
for i := 0; i < rows; i++ {
|
||||
m[i] = make([]complex128, cols)
|
||||
// Hann along rows.
|
||||
wy := 0.5 * (1 - math.Cos(2*math.Pi*float64(i)/float64(rows-1)))
|
||||
for j := 0; j < cols; j++ {
|
||||
wx := 0.5 * (1 - math.Cos(2*math.Pi*float64(j)/float64(cols-1)))
|
||||
m[i][j] = complex((g[i][j]-dc)*wx*wy, 0)
|
||||
}
|
||||
}
|
||||
return m
|
||||
}
|
||||
|
||||
// at2 reads r[y][x] with horizontal (column) wrap-around.
|
||||
func at2(r [][]complex128, y, x, cols int) float64 {
|
||||
x = ((x % cols) + cols) % cols
|
||||
if y < 0 {
|
||||
y += len(r)
|
||||
}
|
||||
if y >= len(r) {
|
||||
y -= len(r)
|
||||
}
|
||||
return real(r[y][x])
|
||||
}
|
||||
|
||||
// parabola returns the sub-pixel offset of the true peak given the values at
|
||||
// (left, center, right). Standard 3-point parabolic interpolation.
|
||||
func parabola(left, center, right float64) float64 {
|
||||
denom := left - 2*center + right
|
||||
if math.Abs(denom) < 1e-12 {
|
||||
return 0
|
||||
}
|
||||
return 0.5 * (left - right) / denom
|
||||
}
|
||||
|
||||
// signedShift converts a correlation peak index in [0,n) to a signed shift in
|
||||
// [-n/2, n/2).
|
||||
func signedShift(idx, n int) int {
|
||||
if idx >= n/2 {
|
||||
return idx - n
|
||||
}
|
||||
return idx
|
||||
}
|
||||
|
||||
func isPow2(n int) bool {
|
||||
return n > 0 && (n&(n-1)) == 0
|
||||
}
|
||||
|
||||
// toGrayDownscaled converts an image to a rows×cols grayscale matrix using
|
||||
// area-averaging (box filter) downscaling. This preserves the full field of
|
||||
// view so pixel shifts scale directly to the original FoV.
|
||||
func toGrayDownscaled(img image.Image, rows, cols int) [][]float64 {
|
||||
b := img.Bounds()
|
||||
srcW := b.Dx()
|
||||
srcH := b.Dy()
|
||||
|
||||
out := make([][]float64, rows)
|
||||
for i := range out {
|
||||
out[i] = make([]float64, cols)
|
||||
}
|
||||
|
||||
xRatio := float64(srcW) / float64(cols)
|
||||
yRatio := float64(srcH) / float64(rows)
|
||||
|
||||
for oy := 0; oy < rows; oy++ {
|
||||
y0 := b.Min.Y + int(math.Floor(float64(oy)*yRatio))
|
||||
y1 := b.Min.Y + int(math.Floor(float64(oy+1)*yRatio))
|
||||
if y1 <= y0 {
|
||||
y1 = y0 + 1
|
||||
}
|
||||
for ox := 0; ox < cols; ox++ {
|
||||
x0 := b.Min.X + int(math.Floor(float64(ox)*xRatio))
|
||||
x1 := b.Min.X + int(math.Floor(float64(ox+1)*xRatio))
|
||||
if x1 <= x0 {
|
||||
x1 = x0 + 1
|
||||
}
|
||||
sum := 0.0
|
||||
cnt := 0
|
||||
for y := y0; y < y1 && y < b.Max.Y; y++ {
|
||||
for x := x0; x < x1 && x < b.Max.X; x++ {
|
||||
r, g, bl, _ := img.At(x, y).RGBA()
|
||||
// 16-bit RGBA -> 8-bit luminance (Rec. 601).
|
||||
lum := 0.299*float64(r) + 0.587*float64(g) + 0.114*float64(bl)
|
||||
sum += lum / 257.0
|
||||
cnt++
|
||||
}
|
||||
}
|
||||
if cnt > 0 {
|
||||
out[oy][ox] = sum / float64(cnt)
|
||||
}
|
||||
}
|
||||
}
|
||||
return out
|
||||
}
|
||||
236
dwarfctl/internal/odometry/odometry_test.go
Normal file
236
dwarfctl/internal/odometry/odometry_test.go
Normal file
@ -0,0 +1,236 @@
|
||||
package odometry
|
||||
|
||||
import (
|
||||
"image"
|
||||
"math"
|
||||
"testing"
|
||||
)
|
||||
|
||||
// makeTextured makes a deterministic, highly-textured synthetic image of the
|
||||
// given size. It mixes multiple sine gratings at different orientations so that
|
||||
// phase correlation has rich frequency content to lock onto.
|
||||
func makeTextured(w, h int) *image.RGBA {
|
||||
img := image.NewRGBA(image.Rect(0, 0, w, h))
|
||||
for y := 0; y < h; y++ {
|
||||
for x := 0; x < w; x++ {
|
||||
// Sum of 3 gratings — pseudo-random but deterministic texture.
|
||||
v := 0.0
|
||||
v += 0.5 + 0.5*math.Sin(float64(x)*0.13+float64(y)*0.07)
|
||||
v += 0.5 + 0.5*math.Sin(float64(x)*0.05-float64(y)*0.11+1.3)
|
||||
v += 0.5 + 0.5*math.Sin(float64(x)*0.21+2.1)
|
||||
v /= 3.0
|
||||
b := uint8(v * 255)
|
||||
// Add some discrete structure (a grid of bright dots) too.
|
||||
if x%32 < 3 && y%32 < 3 {
|
||||
b = 255
|
||||
}
|
||||
off := img.PixOffset(x, y)
|
||||
img.Pix[off+0] = b
|
||||
img.Pix[off+1] = b
|
||||
img.Pix[off+2] = b
|
||||
img.Pix[off+3] = 255
|
||||
}
|
||||
}
|
||||
return img
|
||||
}
|
||||
|
||||
// shiftImage returns a copy of src translated by (dx,dy) pixels, with wrapped
|
||||
// (toroidal) borders — this models a pure translation that phase correlation
|
||||
// can recover exactly (no edge artifacts).
|
||||
func shiftImage(src *image.RGBA, dx, dy int) *image.RGBA {
|
||||
w, h := src.Bounds().Dx(), src.Bounds().Dy()
|
||||
dst := image.NewRGBA(src.Bounds())
|
||||
for y := 0; y < h; y++ {
|
||||
for x := 0; x < w; x++ {
|
||||
sx := ((x-dx)%w + w) % w
|
||||
sy := ((y-dy)%h + h) % h
|
||||
so := src.PixOffset(sx, sy)
|
||||
do := dst.PixOffset(x, y)
|
||||
dst.Pix[do+0] = src.Pix[so+0]
|
||||
dst.Pix[do+1] = src.Pix[so+1]
|
||||
dst.Pix[do+2] = src.Pix[so+2]
|
||||
dst.Pix[do+3] = 255
|
||||
}
|
||||
}
|
||||
return dst
|
||||
}
|
||||
|
||||
func TestFFT_RoundTrip(t *testing.T) {
|
||||
// FFT followed by inverse FFT must recover the original signal.
|
||||
const n = 64
|
||||
a := make([]complex128, n)
|
||||
for i := range a {
|
||||
a[i] = complex(math.Sin(float64(i)*0.3)+0.5*math.Cos(float64(i)*0.7), 0)
|
||||
}
|
||||
orig := make([]complex128, n)
|
||||
copy(orig, a)
|
||||
|
||||
fft(a, false)
|
||||
fft(a, true)
|
||||
|
||||
for i := range a {
|
||||
if math.Abs(real(a[i])-real(orig[i])) > 1e-9 || math.Abs(imag(a[i])-imag(orig[i])) > 1e-9 {
|
||||
t.Fatalf("FFT round-trip failed at %d: got %v want %v", i, a[i], orig[i])
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func TestIsPow2(t *testing.T) {
|
||||
cases := map[int]bool{0: false, 1: true, 2: true, 3: false, 4: true, 16: true, 255: false, 256: true, 257: false}
|
||||
for n, want := range cases {
|
||||
if got := isPow2(n); got != want {
|
||||
t.Errorf("isPow2(%d)=%v want %v", n, got, want)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func TestEstimate_KnownShift(t *testing.T) {
|
||||
const size = 128
|
||||
const fovX, fovY = 10.0, 8.0
|
||||
|
||||
base := makeTextured(size, size)
|
||||
|
||||
cases := []struct{ dx, dy int }{
|
||||
{5, 0},
|
||||
{0, 4},
|
||||
{-6, 3},
|
||||
{10, -7},
|
||||
{0, 0}, // identical => zero shift
|
||||
}
|
||||
for _, c := range cases {
|
||||
shifted := shiftImage(base, c.dx, c.dy)
|
||||
r, err := Estimate(base, shifted, fovX, fovY, size)
|
||||
if err != nil {
|
||||
t.Fatalf("Estimate(dx=%d,dy=%d): %v", c.dx, c.dy, err)
|
||||
}
|
||||
// We expect the scene shift to equal the translation we applied. Positive
|
||||
// PanPx = content moved right. shifting the image by +dx moves content
|
||||
// to the right by dx pixels, so PanPx should be ≈ +dx.
|
||||
tolPx := 1.5 // sub-pixel estimator can be off by ~1px on a 128 grid.
|
||||
if math.Abs(r.PanPx-float64(c.dx)) > tolPx {
|
||||
t.Errorf("dx=%d dy=%d: PanPx=%.3f want ≈%d (±%.1f)", c.dx, c.dy, r.PanPx, c.dx, tolPx)
|
||||
}
|
||||
if math.Abs(r.TiltPx-float64(c.dy)) > tolPx {
|
||||
t.Errorf("dx=%d dy=%d: TiltPx=%.3f want ≈%d (±%.1f)", c.dx, c.dy, r.TiltPx, c.dy, tolPx)
|
||||
}
|
||||
// Degree conversion: px * fov / size.
|
||||
wantPanDeg := float64(c.dx) * fovX / size
|
||||
if math.Abs(r.PanDeg-wantPanDeg) > (tolPx*fovX/size) {
|
||||
t.Errorf("dx=%d: PanDeg=%.4f want ≈%.4f", c.dx, r.PanDeg, wantPanDeg)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func TestEstimate_IdenticalImages_HighConfidence(t *testing.T) {
|
||||
const size = 128
|
||||
a := makeTextured(size, size)
|
||||
r, err := Estimate(a, a, 10, 8, size)
|
||||
if err != nil {
|
||||
t.Fatal(err)
|
||||
}
|
||||
if math.Abs(r.PanPx) > 0.5 || math.Abs(r.TiltPx) > 0.5 {
|
||||
t.Errorf("identical images should give ~0 shift, got pan=%.2f tilt=%.2f", r.PanPx, r.TiltPx)
|
||||
}
|
||||
}
|
||||
|
||||
func TestEstimate_RejectsBadSize(t *testing.T) {
|
||||
img := makeTextured(16, 16)
|
||||
if _, err := Estimate(img, img, 10, 8, 100); err == nil { // 100 not pow2
|
||||
t.Error("expected error for non-power-of-two size")
|
||||
}
|
||||
if _, err := Estimate(img, img, 0, 0, 64); err == nil { // zero fov
|
||||
t.Error("expected error for zero fov")
|
||||
}
|
||||
}
|
||||
|
||||
func TestTracker_IntegratesRotation(t *testing.T) {
|
||||
const size = 128
|
||||
const fovX, fovY = 10.0, 8.0
|
||||
|
||||
tr := NewTracker(fovX, fovY, size)
|
||||
|
||||
base := makeTextured(size, size)
|
||||
|
||||
// Frame 0: primes the reference; orientation should be zero.
|
||||
if _, err := tr.Update(base); err != nil {
|
||||
t.Fatal(err)
|
||||
}
|
||||
o0 := tr.Orientation()
|
||||
if o0.PanDeg != 0 || o0.TiltDeg != 0 || !o0.Primed {
|
||||
t.Fatalf("after prime: %+v", o0)
|
||||
}
|
||||
|
||||
// Frame 1: shift right by 5 px (content moves right). The telescope is
|
||||
// static, so the motor-equivalent pan is the NEGATIVE of the scene shift.
|
||||
// Scene +5px right => tracker pan = -5*fov/size.
|
||||
f1 := shiftImage(base, 5, 0)
|
||||
if _, err := tr.Update(f1); err != nil {
|
||||
t.Fatal(err)
|
||||
}
|
||||
o1 := tr.Orientation()
|
||||
wantPan := -5.0 * fovX / size
|
||||
if math.Abs(o1.PanDeg-wantPan) > 0.3 {
|
||||
t.Errorf("frame1 pan=%.4f want≈%.4f", o1.PanDeg, wantPan)
|
||||
}
|
||||
|
||||
// Frame 2: shift another 4 px right (cumulative content shift 9 px).
|
||||
f2 := shiftImage(base, 9, 0)
|
||||
if _, err := tr.Update(f2); err != nil {
|
||||
t.Fatal(err)
|
||||
}
|
||||
o2 := tr.Orientation()
|
||||
wantPan2 := -9.0 * fovX / size
|
||||
if math.Abs(o2.PanDeg-wantPan2) > 0.5 {
|
||||
t.Errorf("frame2 cumulative pan=%.4f want≈%.4f", o2.PanDeg, wantPan2)
|
||||
}
|
||||
if o2.Frames != 2 {
|
||||
t.Errorf("frames=%d want 2", o2.Frames)
|
||||
}
|
||||
}
|
||||
|
||||
func TestTracker_Reset(t *testing.T) {
|
||||
tr := NewTracker(10, 8, 64)
|
||||
base := makeTextured(64, 64)
|
||||
tr.Update(base)
|
||||
tr.Update(shiftImage(base, 4, 0))
|
||||
if tr.Orientation().PanDeg == 0 {
|
||||
t.Fatal("expected nonzero pan before reset")
|
||||
}
|
||||
tr.Reset()
|
||||
o := tr.Orientation()
|
||||
if o.PanDeg != 0 || o.Primed || o.Frames != 0 {
|
||||
t.Errorf("after reset: %+v", o)
|
||||
}
|
||||
}
|
||||
|
||||
func TestPhaseCorrelation_NoTexture(t *testing.T) {
|
||||
// Flat images: no signal. Should not panic and should return low confidence.
|
||||
const n = 64
|
||||
flat := make([][]float64, n)
|
||||
for i := range flat {
|
||||
flat[i] = make([]float64, n)
|
||||
}
|
||||
dx, dy, conf := phaseCorrelation(flat, flat, n, n)
|
||||
if conf > 0.01 {
|
||||
t.Errorf("flat images confidence=%.4f, expected ~0", conf)
|
||||
}
|
||||
if dx != 0 || dy != 0 {
|
||||
t.Errorf("flat images shift=(%.1f,%.1f) want (0,0)", dx, dy)
|
||||
}
|
||||
}
|
||||
|
||||
func TestSignedShift(t *testing.T) {
|
||||
cases := map[int]int{0: 0, 1: 1, 3: 3, 4: -4, 5: -3, 7: -1, 8: 0}
|
||||
for in, want := range cases {
|
||||
if got := signedShift(in, 8); got != want {
|
||||
t.Errorf("signedShift(%d,8)=%d want %d", in, got, want)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Ensure float comparison helpers behave sensibly on this toolchain.
|
||||
func TestFloatHelpers(t *testing.T) {
|
||||
if math.Abs(-1.5) != 1.5 {
|
||||
t.Fatal("math.Abs broken")
|
||||
}
|
||||
}
|
||||
164
dwarfctl/internal/odometry/tracker.go
Normal file
164
dwarfctl/internal/odometry/tracker.go
Normal file
@ -0,0 +1,164 @@
|
||||
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,
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user