diff --git a/analysis/API_REFERENCE.md b/analysis/API_REFERENCE.md index d493a0f..6feeef6 100644 --- a/analysis/API_REFERENCE.md +++ b/analysis/API_REFERENCE.md @@ -98,6 +98,34 @@ rtsp://:554//stream0 - Codec: MJPEG over RTSP (not H.264). - Port 8092 (MJPEG HTTP `/mainstream` and `/secondstream`) exists but is **inactive** on the DWARF Mini — the Mini uses RTSP exclusively. +### 2.4 Visual odometry (image-based orientation) + +The wide-angle RTSP frames can be used to estimate the telescope's pointing +rotation **without star identification or plate solving**, by comparing +consecutive frames with FFT phase correlation. This is implemented in +`dwarfctl` as the `orient` command (`internal/odometry/`). + +**Principle:** for small rotations of a static mount, the scene undergoes an +almost pure 2-D translation in the image plane. Phase correlation recovers that +translation with sub-pixel accuracy, which maps directly to pan/tilt degrees via +the camera field of view. Accumulating frame-to-frame shifts gives cumulative +pointing orientation. + +**Use cases on the DWARF Mini:** +- **Daytime airplane tracking** — the wide cam provides sky/horizon texture +- **Nighttime satellite tracking** — star fields serve as correlation texture + +**Pipeline:** +1. Open the wide camera (`CMD_CAMERA_WIDE_OPEN_CAMERA` 12000) — or let `orient + live` do it automatically +2. Grab frames via `ffmpeg -rtsp_transport tcp -i rtsp:///ch1/stream0` +3. Compare consecutive frames (`Estimate`) or integrate continuously (`Tracker`) + +**FoV caveat:** the DWARF Mini's display FoV is 8.0°×6.5° (see +`DEVICE_MODELS.md`), but the firmware-reported live FoV can differ. The pixel +shift is always correct; only the degree conversion depends on the FoV +parameter. Calibrate by slewing a known motor angle and comparing. + --- ## 3. Wire format — `WsPacket` envelope diff --git a/analysis/CAMERA_STREAMING.md b/analysis/CAMERA_STREAMING.md new file mode 100644 index 0000000..2f36e8f --- /dev/null +++ b/analysis/CAMERA_STREAMING.md @@ -0,0 +1,238 @@ +# Guide d'accès aux flux caméra — DWARF Mini + +Ce document décrit comment accéder aux flux vidéo des caméras du télescope +DWARF Mini (et DWARF II/3), que ce soit pour capturer une image unique, +afficher un flux continu, ou enregistrer une vidéo. + +--- + +## 1. Architecture du flux caméra + +Le télescope possède **deux caméras** accessibles via des protocoles distincts : + +| Caméra | Canal RTSP | Usage | +|--------|-----------|-------| +| **Tele** | `ch0` | Caméra principale (longue focale) | +| **Wide** | `ch1` | Caméra grand angle (repérage) | + +Le flux vidéo est accessible via le protocole **RTSP** sur le port **554** : + +``` +rtsp://:554/ch0/stream0 # Téléobjectif (Tele) +rtsp://:554/ch1/stream0 # Grand angle (Wide) +``` + +### Caractéristiques techniques + +- **Codec** : MJPEG (Motion JPEG) +- **Résolution** : 1920×1080 (les deux caméras) +- **Transport** : TCP (`rtsp_transport=tcp` obligatoire) +- **FPS** : Variable (généralement 15-30 FPS selon l'éclairage) + +### ⚠️ Prérequis absolu : ouvrir la caméra via WebSocket + +**Le flux RTSP n'est actif qu'après avoir ouvert la caméra via l'API de contrôle +WebSocket** (port 9900). Sans cette étape, le serveur RTSP accepte la connexion +TCP mais n'envoie **aucune frame vidéo**. + +La procédure complète est : +1. Se connecter au WebSocket `ws://:9900/?client_id=mon_client` +2. Envoyer la commande `CMD_CAMERA_TELE_OPEN_CAMERA` (10000) ou + `CMD_CAMERA_WIDE_OPEN_CAMERA` (12000) via le protocole `WsPacket` +3. Attendre ~2 secondes que le serveur RTSP démarre +4. Se connecter au flux RTSP avec `ffmpeg`, `mpv`, `VLC` ou tout lecteur RTSP + +--- + +## 2. Accès avec `dwarfctl` (le plus simple) + +Le CLI `dwarfctl` inclut une commande `preview grab` qui automatise toute la +séquence (ouverture caméra + capture RTSP) : + +```bash +# Capturer une frame de la caméra grand angle +dwarfctl --ip 192.168.88.1 preview grab --cam wide /tmp/wide.jpg + +# Capturer une frame du téléobjectif +dwarfctl --ip 192.168.88.1 preview grab --cam tele /tmp/tele.jpg +``` + +--- + +## 3. Accès avec `ffmpeg` (capture manuelle) + +### Capture d'une seule image (frame) + +```bash +# 1. Ouvrir la caméra via WebSocket +dwarfctl --ip 192.168.88.1 camera open --cam wide + +# 2. Attendre 2 secondes +sleep 2 + +# 3. Capturer une frame +ffmpeg -rtsp_transport tcp \ + -i "rtsp://192.168.88.1:554/ch1/stream0" \ + -frames:v 1 -q:v 2 \ + -y wide_frame.jpg +``` + +### Capture de plusieurs frames (time-lapse) + +```bash +# Capturer 1 frame par seconde pendant 60 secondes +ffmpeg -rtsp_transport tcp \ + -i "rtsp://192.168.88.1:554/ch1/stream0" \ + -r 1 -t 60 \ + -q:v 2 \ + wide_timelapse_%03d.jpg +``` + +### Enregistrement vidéo (MP4) + +```bash +# Enregistrer 30 secondes de vidéo +ffmpeg -rtsp_transport tcp \ + -i "rtsp://192.168.88.1:554/ch1/stream0" \ + -t 30 -c:v libx264 -preset fast \ + wide_video.mp4 +``` + +--- + +## 4. Accès avec `mpv` (flux continu) + +`mpv` est idéal pour afficher le flux en direct avec faible latence : + +```bash +# 1. Ovrir la caméra +dwarfctl --ip 192.168.88.1 camera open --cam wide + +# 2. Afficher le flux en plein écran +mpv --rtsp-transport=tcp "rtsp://192.168.88.1:554/ch1/stream0" +``` + +### Options utiles pour mpv +```bash +# Faible latence +mpv --rtsp-transport=tcp --profile=low-latency \ + "rtsp://192.168.88.1:554/ch1/stream0" + +# Double flux (Tele + Wide côte à côte) +dwarfctl --ip 192.168.88.1 camera open --cam tele & +dwarfctl --ip 192.168.88.1 camera open --cam wide & +sleep 2 +mpv --rtsp-transport=tcp "rtsp://192.168.88.1:554/ch0/stream0" & +mpv --rtsp-transport=tcp "rtsp://192.168.88.1:554/ch1/stream0" +``` + +--- + +## 5. Accès avec VLC + +```bash +# Ouvrir depuis la ligne de commande +vlc "rtsp://192.168.88.1:554/ch1/stream0" --rtsp-tcp + +# Ou depuis l'interface VLC : +# Média → Ouvrir un flux réseau → rtsp://192.168.88.1:554/ch1/stream0 +# (Cliquer sur "Afficher les options" → ajouter :rtsp-tcp) +``` + +--- + +## 6. Accès programmatique (Python / Go) + +### Python (avec OpenCV) + +```python +import cv2 +import subprocess +import time + +# 1. Ouvrir la caméra via dwarfctl +subprocess.run(["dwarfctl", "--ip", "192.168.88.1", "camera", "open", "--cam", "wide"]) +time.sleep(2) + +# 2. Ouvrir le flux RTSP avec OpenCV +cap = cv2.VideoCapture("rtsp://192.168.88.1:554/ch1/stream0") +cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) + +while True: + ret, frame = cap.read() + if not ret: + break + cv2.imshow("DWARF Wide", frame) + if cv2.waitKey(1) & 0xFF == ord('q'): + break + +cap.release() +cv2.destroyAllWindows() +``` + +### Go (avec `gortsplib` ou `ffmpeg` exec) + +```go +// Voir la commande `preview grab` dans: +// dwarfctl/cmd/dwarfctl/main.go → func cmdPreview() +// Elle utilise exec.Command("ffmpeg", ...) pour capturer une frame. +``` + +--- + +## 7. Comparatif des modes de preview + +Le télescope supporte deux modes de preview selon les modèles : + +| Mode | Protocole | Port | DWARF Mini | DWARF II/3 | +|------|-----------|------|:----------:|:----------:| +| **RTSP** | `rtsp://` | 554 | ✅ Actif | ✅ Actif | +| **MJPEG** | HTTP multipart | 8092 | ❌ Inactif | ⚠️ Non testé | + +- **RTSP (streamType=1)** : flux MJPEG encapsulé en RTSP, port 554, channel + `ch0` (tele) ou `ch1` (wide), path `/stream0`. +- **MJPEG HTTP (streamType=2)** : flux multipart/x-mixed-replace sur port 8092, + endpoints `/mainstream` (tele) et `/secondstream` (wide). + Le DWARF Mini n'envoie pas de frames sur ce port — le mode RTSP est utilisé. + +Le choix entre RTSP et MJPEG est décidé par le télescope via la notification +`CMD_NOTIFY_STREAM_TYPE` (15234) qui transporte `StreamType{stream_type, cam_id}`. +L'app Android s'y adapte automatiquement. + +--- + +## 8. Dépannage + +### Le flux RTSP ne donne aucune image + +**Cause** : la caméra n'est pas ouverte via WebSocket. + +**Solution** : +```bash +dwarfctl --ip 192.168.88.1 camera open --cam wide +sleep 2 +ffmpeg -rtsp_transport tcp -i "rtsp://192.168.88.1:554/ch1/stream0" -frames:v 1 test.jpg +``` + +### ffmpeg affiche "Connection refused" sur le port 554 + +**Cause** : le télescope vient de démarrer, le serveur RTSP n'est pas encore prêt. + +**Solution** : attendre 10-15 secondes après l'allumage du télescope. + +### L'image est noire + +**Cause** : le télescope pointe vers le sol ou dans l'obscurité. + +**Solution** : vérifier la position du télescope, ou augmenter le gain/exposure : +```bash +dwarfctl --ip 192.168.88.1 camera exp --cam wide 200 # exposition longue +dwarfctl --ip 192.168.88.1 camera gain --cam wide 100 # gain élevé +``` + +### VLC met très longtemps à afficher l'image + +**Cause** : VLC utilise UDP par défaut pour RTSP. + +**Solution** : forcer TCP — ajouter `:rtsp-tcp` dans les options ou utiliser +`--rtsp-transport=tcp` en ligne de commande. diff --git a/analysis/DEVICE_MODELS.md b/analysis/DEVICE_MODELS.md index e0df2bb..4e268cc 100644 --- a/analysis/DEVICE_MODELS.md +++ b/analysis/DEVICE_MODELS.md @@ -119,3 +119,7 @@ supports the older sun/moon tracking flow without extra confirmation. 3. Gate features client-side: warn when attempting panorama on a Mini, hide auto-shutdown, etc. 4. Log the reported FoV in `health` output to help identify the model. +5. **Visual odometry calibration:** the Mini's wide-cam FoV is uncertain + (8.0°×6.5° display vs 42.65°×24.45° live-reported — see above). When using + `dwarfctl orient`, the pixel shift is always correct; to get accurate degree + values, calibrate `--fov-h`/`--fov-v` by slewing a known motor angle. diff --git a/dwarfctl/README.md b/dwarfctl/README.md index 6f770e1..ecf25e0 100644 --- a/dwarfctl/README.md +++ b/dwarfctl/README.md @@ -71,6 +71,11 @@ dwarfctl power off # Monitor notifications (live status events) dwarfctl monitor + +# Visual odometry — image-based orientation (no star/plate solving) +dwarfctl orient compare before.jpg after.jpg # measure rotation between 2 frames +dwarfctl orient live --cam wide # live pointing tracker from RTSP +dwarfctl orient live --fov-h 42 --fov-v 24 # override FoV with measured values ``` ## Architecture @@ -81,9 +86,13 @@ dwarfctl/ ├── proto/dwarf.pb.go # generated Go bindings (24948 lines) ├── internal/ │ ├── transport/client.go # WebSocket client + WsPacket envelope -│ └── api/ -│ ├── commands.go # 323 command IDs + module routing -│ └── client.go # typed Telescope API (camera/motor/astro/...) +│ ├── api/ +│ │ ├── commands.go # 323 command IDs + module routing +│ │ └── client.go # typed Telescope API (camera/motor/astro/...) +│ └── odometry/ # FFT phase-correlation visual odometry +│ ├── fft.go # radix-2 Cooley-Tukey FFT (1D + 2D) +│ ├── odometry.go # Estimate() — shift between 2 frames → degrees +│ └── tracker.go # Tracker — cumulative orientation integrator └── cmd/dwarfctl/main.go # cobra CLI ``` @@ -114,9 +123,47 @@ protoc --go_out=. --go_opt=paths=source_relative dwarf.proto ## Roadmap +- [x] **Visual odometry** — image-based orientation via FFT phase correlation (`orient`) - [ ] BLE discovery + handshake (DwarfPing/DwarfEcho) - [ ] RTSP preview viewer - [ ] Interactive REPL mode - [ ] Schedule plan management - [ ] OTA firmware update - [ ] Full Notify event parsing (typed) +- [ ] Closed-loop tracking: feed `orient` output into motor corrections + +## Visual odometry (`orient`) + +The `orient` command estimates the telescope's pointing rotation by comparing +wide-angle frames using **FFT phase correlation**. It does NOT use star +identification or plate solving — it works on any textured scene (sky, horizon, +clouds, daytime landscape), making it suitable for: + +- **Daytime airplane tracking** — the wide cam sees sky/horizon texture +- **Nighttime satellite tracking** — the wide cam sees star fields as texture + +### How it works + +1. Grab two wide-angle frames (via RTSP `ffmpeg` grab, same as `preview grab`) +2. Downscale both to a square grid (default 256×256, power of two) +3. Compute the 2-D FFT of each, then the normalized cross-power spectrum +4. The inverse FFT gives a correlation surface; its peak = image-plane shift +5. Sub-pixel refinement via parabolic interpolation +6. Convert pixel shift → degrees using the camera field of view + +For a static alt-az mount, the accumulated shifts give the cumulative pointing +orientation relative to the starting frame — no absolute encoders or polar +alignment needed. + +### Field of view calibration + +The default FoV (8.0°×6.5°) comes from the DWARF Mini's display values in +`DEVICE_MODELS.md`, but the firmware-reported live FoV can differ. To calibrate: +slew the motors by a known angle and compare with the measured rotation. Override +with `--fov-h` / `--fov-v` once you have measured values. + +### Prerequisites + +- **ffmpeg** must be installed (for RTSP frame capture) +- The wide camera must be opened first (`camera open --cam wide`) — `orient live` + does this automatically. diff --git a/dwarfctl/cmd/dwarfctl/main.go b/dwarfctl/cmd/dwarfctl/main.go index b5c5dc2..6767205 100644 --- a/dwarfctl/cmd/dwarfctl/main.go +++ b/dwarfctl/cmd/dwarfctl/main.go @@ -3,6 +3,9 @@ package main import ( "bufio" "fmt" + "image" + _ "image/jpeg" + _ "image/png" "math" "os" "os/exec" @@ -11,6 +14,8 @@ import ( "time" "github.com/antitbone/dwarfctl/internal/api" + "github.com/antitbone/dwarfctl/internal/daemon" + "github.com/antitbone/dwarfctl/internal/odometry" pb "github.com/antitbone/dwarfctl/proto" "github.com/spf13/cobra" "google.golang.org/protobuf/proto" @@ -20,6 +25,7 @@ var ( flagIP string flagClientID string flagDebug bool + flagDaemon string // daemon addr, e.g. "127.0.0.1:7777" ) func main() { @@ -31,8 +37,10 @@ func main() { rootCmd.PersistentFlags().StringVarP(&flagIP, "ip", "i", "", "telescope IP address (required)") rootCmd.PersistentFlags().StringVar(&flagClientID, "client-id", "dwarfctl", "WebSocket client_id") rootCmd.PersistentFlags().BoolVarP(&flagDebug, "debug", "d", false, "verbose WebSocket traffic log") + rootCmd.PersistentFlags().StringVar(&flagDaemon, "daemon", "", "daemon addr (e.g. 127.0.0.1:7777)") rootCmd.AddCommand( + cmdServe(), cmdHealth(), cmdState(), cmdCamera(), @@ -46,9 +54,15 @@ func main() { cmdInteractive(), cmdPano(), cmdPreview(), + cmdOrient(), ) - rootCmd.MarkPersistentFlagRequired("ip") + // --ip is required only when not using --daemon + rootCmd.PersistentPreRun = func(cmd *cobra.Command, _ []string) { + if flagDaemon == "" && flagIP == "" && cmd.Name() != "serve" && cmd.Name() != "help" && cmd.Name() != "completion" { + die("--ip is required (or use --daemon to connect to a running daemon)") + } + } if err := rootCmd.Execute(); err != nil { os.Exit(1) } @@ -687,8 +701,8 @@ func cmdPreview() *cobra.Command { out = args[0] } scope, cleanup := dial() + defer cleanup() must(scope.OpenCamera(cam)) - cleanup() time.Sleep(2 * time.Second) url := fmt.Sprintf("rtsp://%s:554/%s/stream0", flagIP, ch) ffmpegCmd := exec.Command("ffmpeg", "-rtsp_transport", "tcp", @@ -817,6 +831,222 @@ func cmdInteractive() *cobra.Command { } } +// --- orient (visual odometry) --- + +func cmdOrient() *cobra.Command { + c := &cobra.Command{ + Use: "orient", + Short: "Image-based orientation (visual odometry via wide-angle frames)", + Long: "Estimate telescope pointing rotation by comparing wide-angle frames.\n" + + "Uses FFT phase correlation — works on any scene (sky, horizon, clouds)\n" + + "without star identification or plate solving.\n\n" + + "Designed for a static alt-az mount tracking airplanes (day) or satellites (night).", + } + c.AddCommand( + &cobra.Command{ + Use: "status", + Short: "Get current orientation from the daemon (requires 'serve' running)", + Run: func(_ *cobra.Command, _ []string) { + dc := daemonClient() + if dc == nil { + die("no daemon: use --daemon ADDR or start 'dwarfctl serve'") + } + o, err := dc.Orientation() + must(err) + fmt.Println(o) + }, + }, + &cobra.Command{ + Use: "reset", + Short: "Reset the odometry tracker (requires daemon)", + Run: func(_ *cobra.Command, _ []string) { + dc := daemonClient() + if dc == nil { + die("no daemon: use --daemon ADDR or start 'dwarfctl serve'") + } + must(dc.Reset()) + fmt.Println("orientation reset") + }, + }, + &cobra.Command{ + Use: "grab [output.jpg]", + Short: "Grab a frame via the daemon (camera stays open)", + Args: cobra.MaximumNArgs(1), + Run: func(_ *cobra.Command, args []string) { + dc := daemonClient() + if dc == nil { + die("no daemon: use --daemon ADDR or start 'dwarfctl serve'") + } + out := "" + if len(args) > 0 { + out = args[0] + } + path, err := dc.Grab(out) + must(err) + fmt.Printf("frame saved: %s\n", path) + }, + }, + &cobra.Command{ + Use: "compare ", + Short: "Measure rotation between two image files", + Args: cobra.ExactArgs(2), + Run: func(cmd *cobra.Command, args []string) { + fovH, _ := cmd.Flags().GetFloat64("fov-h") + fovV, _ := cmd.Flags().GetFloat64("fov-v") + size, _ := cmd.Flags().GetInt("size") + r, err := odometry.EstimateFiles(args[0], args[1], fovH, fovV, size) + must(err) + fmt.Printf("Scene shift (img1 -> img2):\n %s\n", r) + fmt.Printf(" grid=%dx%d fov=%.2f°x%.2f°\n", r.Size, r.Size, fovH, fovV) + if r.Confidence < 0.05 { + fmt.Println(" ⚠ low confidence (frames too similar or motion too large)") + } + }, + }, + &cobra.Command{ + Use: "live", + Short: "Continuously track pointing from RTSP frames (Ctrl-C to stop)", + Run: func(cmd *cobra.Command, _ []string) { + cam := mustCam(cmd.Flag("cam").Value.String()) + fovH, _ := cmd.Flags().GetFloat64("fov-h") + fovV, _ := cmd.Flags().GetFloat64("fov-v") + size, _ := cmd.Flags().GetInt("size") + interval, _ := cmd.Flags().GetDuration("interval") + + ch := "ch0" + if cam == api.CameraWide { + ch = "ch1" + } + + scope, cleanup := dial() + defer cleanup() + must(scope.OpenCamera(cam)) + time.Sleep(2 * time.Second) + + url := fmt.Sprintf("rtsp://%s:554/%s/stream0", flagIP, ch) + tracker := odometry.NewTracker(fovH, fovV, size) + tmpA := "/tmp/dwarf_orient_a.jpg" + tmpB := "/tmp/dwarf_orient_b.jpg" + useA := true + + fmt.Printf("Live visual odometry on %s (fov %.1f°x%.1f°, grid %d, every %s)\n", + url, fovH, fovV, size, interval) + fmt.Println("Ctrl-C to stop.") + ticker := time.NewTicker(interval) + defer ticker.Stop() + + grab := func(out string) error { + return exec.Command("ffmpeg", "-rtsp_transport", "tcp", + "-i", url, "-frames:v", "1", "-q:v", "2", "-y", out).Run() + } + + // Prime with first frame. + first := "/tmp/dwarf_orient_prime.jpg" + if err := grab(first); err != nil { + die("ffmpeg grab: %v (is ffmpeg installed?)", err) + } + img, err := decodeJPEG(first) + if err != nil { + die("decode: %v", err) + } + o, _ := tracker.Update(img) + fmt.Printf("[prime] %s\n", o) + + for range ticker.C { + out := tmpA + if !useA { + out = tmpB + } + useA = !useA + if err := grab(out); err != nil { + fmt.Fprintf(os.Stderr, " grab error: %v\n", err) + continue + } + img, err := decodeJPEG(out) + if err != nil { + fmt.Fprintf(os.Stderr, " decode error: %v\n", err) + continue + } + o, err := tracker.Update(img) + if err != nil { + fmt.Fprintf(os.Stderr, " odometry error: %v\n", err) + continue + } + ts := time.Now().Format("15:04:05") + fmt.Printf("[%s] %s\n", ts, o) + } + }, + }, + ) + for _, sub := range c.Commands() { + sub.Flags().Float64("fov-h", odometry.DefaultFoVH, "horizontal field of view (degrees)") + sub.Flags().Float64("fov-v", odometry.DefaultFoVV, "vertical field of view (degrees)") + sub.Flags().Int("size", odometry.DefaultSize, "FFT grid size (power of 2: 128/256/512)") + if sub.Name() == "live" { + sub.Flags().String("cam", "wide", "camera: tele|wide") + sub.Flags().Duration("interval", 3*time.Second, "time between frames") + } + } + return c +} + +// decodeJPEG reads a JPEG/PNG file into an image.Image. +func decodeJPEG(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 +} + +// --- serve (daemon) --- + +func cmdServe() *cobra.Command { + c := &cobra.Command{ + Use: "serve", + Short: "Start persistent telescope daemon (keeps WS + camera + odometry alive)", + Run: func(cmd *cobra.Command, _ []string) { + addr, _ := cmd.Flags().GetString("addr") + cam, _ := cmd.Flags().GetString("cam") + fovH, _ := cmd.Flags().GetFloat64("fov-h") + fovV, _ := cmd.Flags().GetFloat64("fov-v") + size, _ := cmd.Flags().GetInt("size") + interval, _ := cmd.Flags().GetDuration("interval") + + srv := daemon.New(daemon.Config{ + IP: flagIP, + Addr: addr, + FoVH: fovH, + FoVV: fovV, + GridSize: size, + Camera: cam, + Interval: interval, + Debug: flagDebug, + }) + if err := srv.Run(); err != nil { + die("daemon: %v", err) + } + }, + } + c.Flags().String("addr", daemon.DefaultAddr, "HTTP listen address") + c.Flags().String("cam", "wide", "camera to open: tele|wide") + c.Flags().Float64("fov-h", odometry.DefaultFoVH, "horizontal FoV (degrees)") + c.Flags().Float64("fov-v", odometry.DefaultFoVV, "vertical FoV (degrees)") + c.Flags().Int("size", odometry.DefaultSize, "FFT grid size") + c.Flags().Duration("interval", 5*time.Second, "odometry frame interval") + return c +} + +// daemonClient returns a daemon client if --daemon is set, else nil. +func daemonClient() *daemon.Client { + if flagDaemon == "" { + return nil + } + return daemon.NewClient(flagDaemon) +} + // --- helpers --- func flagCam(cmd *cobra.Command) string { diff --git a/dwarfctl/cmd/rtsp-test/main.go b/dwarfctl/cmd/rtsp-test/main.go new file mode 100644 index 0000000..98555a9 --- /dev/null +++ b/dwarfctl/cmd/rtsp-test/main.go @@ -0,0 +1,71 @@ +package main + +import ( + "fmt" + "os" + "os/exec" + "time" + + "github.com/antitbone/dwarfctl/internal/api" +) + +func main() { + ip := "192.168.88.1" + if len(os.Args) > 1 { + ip = os.Args[1] + } + + scope := api.New("rtsp-test") + scope.SetDebug(true) + fmt.Println("Connecting to", ip) + if err := scope.Connect(ip); err != nil { + die("connect: %v", err) + } + defer scope.Close() + + fmt.Println("Opening wide camera...") + if err := scope.OpenCamera(api.CameraWide); err != nil { + die("open camera: %v", err) + } + fmt.Println("Wide camera opened. Waiting 3s for stream to initialize...") + time.Sleep(3 * time.Second) + + url := fmt.Sprintf("rtsp://%s:554/ch1/stream0", ip) + out := "/tmp/rtsp_test_frame.jpg" + fmt.Println("Grabbing frame via ffmpeg...") + cmd := exec.Command("ffmpeg", "-rtsp_transport", "tcp", + "-i", url, "-frames:v", "1", "-q:v", "2", "-y", out) + cmd.Stdout = os.Stdout + cmd.Stderr = os.Stderr + if err := cmd.Run(); err != nil { + die("ffmpeg: %v", err) + } + + info, _ := os.Stat(out) + if info != nil { + fmt.Printf("SUCCESS: frame saved (%d bytes)\n", info.Size()) + } + + // Also try tele + fmt.Println("\nOpening tele camera...") + if err := scope.OpenCamera(api.CameraTele); err != nil { + die("open tele: %v", err) + } + time.Sleep(3 * time.Second) + url0 := fmt.Sprintf("rtsp://%s:554/ch0/stream0", ip) + out0 := "/tmp/rtsp_test_tele.jpg" + fmt.Println("Grabbing tele frame...") + cmd0 := exec.Command("ffmpeg", "-rtsp_transport", "tcp", + "-i", url0, "-frames:v", "1", "-q:v", "2", "-y", out0) + cmd0.Stdout = os.Stdout + cmd0.Stderr = os.Stderr + cmd0.Run() + if i, _ := os.Stat(out0); i != nil { + fmt.Printf("SUCCESS: tele frame saved (%d bytes)\n", i.Size()) + } +} + +func die(format string, args ...any) { + fmt.Fprintf(os.Stderr, "ERROR: "+format+"\n", args...) + os.Exit(1) +} diff --git a/dwarfctl/deploy/dwarfctl.service b/dwarfctl/deploy/dwarfctl.service new file mode 100644 index 0000000..0700654 --- /dev/null +++ b/dwarfctl/deploy/dwarfctl.service @@ -0,0 +1,16 @@ +[Unit] +Description=DWARF Mini telescope daemon (persistent WS + RTSP + visual odometry) +After=network-online.target +Wants=network-online.target + +[Service] +Type=simple +ExecStart=/usr/local/bin/dwarfctl serve --ip 192.168.88.1 --cam wide --addr 127.0.0.1:7777 +Restart=on-failure +RestartSec=3 +# Logs go to journald (Captured by systemd). +StandardOutput=journal +StandardError=journal + +[Install] +WantedBy=default.target diff --git a/dwarfctl/internal/daemon/client.go b/dwarfctl/internal/daemon/client.go new file mode 100644 index 0000000..4700dcd --- /dev/null +++ b/dwarfctl/internal/daemon/client.go @@ -0,0 +1,112 @@ +package daemon + +import ( + "bytes" + "encoding/json" + "fmt" + "io" + "net/http" + "time" + + "github.com/antitbone/dwarfctl/internal/odometry" +) + +// Client is a thin HTTP client that talks to the daemon. +type Client struct { + baseURL string + http *http.Client +} + +// NewClient creates a daemon client for the given address (e.g. "127.0.0.1:7777"). +func NewClient(addr string) *Client { + if addr == "" { + addr = DefaultAddr + } + return &Client{ + baseURL: "http://" + addr, + http: &http.Client{Timeout: 30 * time.Second}, + } +} + +// IsAlive checks if the daemon is reachable. +func (c *Client) IsAlive() bool { + resp, err := c.http.Get(c.baseURL + "/health") + return err == nil && resp.StatusCode == 200 +} + +// Orientation returns the current cumulative orientation from the daemon. +func (c *Client) Orientation() (odometry.Orientation, error) { + resp, err := c.http.Get(c.baseURL + "/orientation") + if err != nil { + return odometry.Orientation{}, err + } + defer resp.Body.Close() + var o odometry.Orientation + return o, json.NewDecoder(resp.Body).Decode(&o) +} + +// Grab captures a frame. If path is empty, the daemon picks a temp path. +// Returns the saved file path. +func (c *Client) Grab(path string) (string, error) { + url := c.baseURL + "/grab" + if path != "" { + url += "?path=" + path + } + resp, err := c.http.Get(url) + if err != nil { + return "", err + } + defer resp.Body.Close() + if resp.StatusCode != 200 { + body, _ := io.ReadAll(resp.Body) + return "", fmt.Errorf("daemon: %s", body) + } + var result map[string]string + json.NewDecoder(resp.Body).Decode(&result) + return result["path"], nil +} + +// Reset zeros the odometry. +func (c *Client) Reset() error { + _, err := c.http.Post(c.baseURL+"/reset", "", nil) + return err +} + +// Slew sends a joystick vector. +func (c *Client) Slew(angle, length float64) error { + body, _ := json.Marshal(map[string]float64{"angle": angle, "length": length}) + resp, err := c.http.Post(c.baseURL+"/slew", "application/json", bytes.NewReader(body)) + if err != nil { + return err + } + resp.Body.Close() + return nil +} + +// StopMotors stops all motors. +func (c *Client) StopMotors() error { + _, err := c.http.Post(c.baseURL+"/stop", "", nil) + return err +} + +// Camera opens or closes a camera. +func (c *Client) Camera(action, cam string) error { + url := fmt.Sprintf("%s/camera?action=%s&cam=%s", c.baseURL, action, cam) + resp, err := c.http.Post(url, "", nil) + if err != nil { + return err + } + resp.Body.Close() + return nil +} + +// SetFoV overrides the field of view. +func (c *Client) SetFoV(h, v float64) error { + body, _ := json.Marshal(map[string]float64{"h": h, "v": v}) + resp, err := c.http.Post(c.baseURL+"/fov", "application/json", bytes.NewReader(body)) + if err != nil { + return err + } + resp.Body.Close() + return nil +} diff --git a/dwarfctl/internal/daemon/server.go b/dwarfctl/internal/daemon/server.go new file mode 100644 index 0000000..bdb3349 --- /dev/null +++ b/dwarfctl/internal/daemon/server.go @@ -0,0 +1,346 @@ +// Package daemon implements a persistent telescope service that keeps the +// WebSocket connection open, manages cameras, grabs RTSP frames, and runs the +// visual odometry tracker — exposing everything via a local HTTP API so that +// short-lived CLI invocations can control a long-lived session. +package daemon + +import ( + "context" + "encoding/json" + "fmt" + "image" + _ "image/jpeg" + "io" + "log" + "net/http" + "os" + "os/exec" + "os/signal" + "sync" + "syscall" + "time" + + "github.com/antitbone/dwarfctl/internal/api" + "github.com/antitbone/dwarfctl/internal/odometry" +) + +// DefaultAddr is the default HTTP listen address for the daemon. +const DefaultAddr = "127.0.0.1:7777" + +// Config configures the daemon. +type Config struct { + IP string + Addr string + FoVH float64 + FoVV float64 + GridSize int + Camera string // "wide" or "tele" + Interval time.Duration // odometry frame interval + Debug bool +} + +// Server is the persistent telescope daemon. +type Server struct { + cfg Config + scope *api.Telescope + tracker *odometry.Tracker + + mu sync.Mutex + camera api.Camera + camOpen bool + stopCh chan struct{} + wg sync.WaitGroup +} + +// New creates a daemon server. +func New(cfg Config) *Server { + if cfg.Addr == "" { + cfg.Addr = DefaultAddr + } + if cfg.FoVH <= 0 { + cfg.FoVH = odometry.DefaultFoVH + } + if cfg.FoVV <= 0 { + cfg.FoVV = odometry.DefaultFoVV + } + if cfg.GridSize <= 0 { + cfg.GridSize = odometry.DefaultSize + } + if cfg.Camera == "" { + cfg.Camera = "wide" + } + if cfg.Interval <= 0 { + cfg.Interval = 5 * time.Second + } + return &Server{ + cfg: cfg, + stopCh: make(chan struct{}), + tracker: odometry.NewTracker(cfg.FoVH, cfg.FoVV, cfg.GridSize), + } +} + +// Run starts the daemon: connects to telescope, opens camera, starts odometry, +// and serves HTTP until SIGTERM/SIGINT is received (systemd-compatible). +func (s *Server) Run() error { + // 1. Connect to telescope. + s.scope = api.New("dwarfctl-daemon") + s.scope.SetDebug(s.cfg.Debug) + log.Printf("Connecting to telescope at %s ...", s.cfg.IP) + if err := s.scope.Connect(s.cfg.IP); err != nil { + return fmt.Errorf("connect: %w", err) + } + log.Println("Connected.") + + // 2. Open camera. + s.camera = api.CameraTele + if s.cfg.Camera == "wide" { + s.camera = api.CameraWide + } + log.Printf("Opening %s camera...", s.cfg.Camera) + if err := s.scope.OpenCamera(s.camera); err != nil { + return fmt.Errorf("open camera: %w", err) + } + s.camOpen = true + log.Println("Camera opened.") + + // 3. Start odometry loop in background. + s.wg.Add(1) + go s.odometryLoop() + + // 4. HTTP server. + mux := http.NewServeMux() + s.registerRoutes(mux) + srv := &http.Server{Addr: s.cfg.Addr, Handler: mux} + + // 5. Signal handling for systemd (SIGTERM/SIGINT → clean shutdown). + go func() { + sigCh := make(chan os.Signal, 1) + signal.Notify(sigCh, syscall.SIGTERM, syscall.SIGINT) + sig := <-sigCh + log.Printf("Received %s — shutting down...", sig) + close(s.stopCh) + ctx, cancel := context.WithTimeout(context.Background(), 5*time.Second) + defer cancel() + srv.Shutdown(ctx) + }() + + log.Printf("Daemon listening on http://%s (Ctrl-C to stop)", s.cfg.Addr) + if err := srv.ListenAndServe(); err != nil && err != http.ErrServerClosed { + return err + } + + // Cleanup. + s.scope.CloseCamera(s.camera) + s.scope.Close() + s.wg.Wait() + log.Println("Daemon stopped.") + return nil +} + +// Stop shuts down the daemon. +func (s *Server) Stop() { + close(s.stopCh) + if s.scope != nil { + s.scope.CloseCamera(s.camera) + s.scope.Close() + } +} + +// odometryLoop continuously grabs RTSP frames and feeds the tracker. +func (s *Server) odometryLoop() { + defer s.wg.Done() + ch := s.rtspChannel() + url := fmt.Sprintf("rtsp://%s:554/%s/stream0", s.cfg.IP, ch) + tmpFile := "/tmp/dwarf_daemon_frame.jpg" + + ticker := time.NewTicker(s.cfg.Interval) + defer ticker.Stop() + + // Prime with first frame. + if img, err := grabFrame(url, tmpFile); err == nil { + s.tracker.Update(img) + log.Printf("Odometry primed (grid=%d, fov=%.1fx%.1f)", s.cfg.GridSize, s.cfg.FoVH, s.cfg.FoVV) + } else { + log.Printf("Odometry prime failed: %v", err) + } + + for { + select { + case <-s.stopCh: + return + case <-ticker.C: + img, err := grabFrame(url, tmpFile) + if err != nil { + if s.cfg.Debug { + log.Printf("grab error: %v", err) + } + continue + } + o, err := s.tracker.Update(img) + if err != nil { + log.Printf("odometry error: %v", err) + continue + } + if s.cfg.Debug { + log.Printf("[odometry] %s", o) + } + } + } +} + +func (s *Server) rtspChannel() string { + s.mu.Lock() + defer s.mu.Unlock() + if s.camera == api.CameraWide { + return "ch1" + } + return "ch0" +} + +// grabFrame uses ffmpeg to grab a single RTSP frame, then decodes it. +func grabFrame(url, tmpFile string) (image.Image, error) { + cmd := exec.Command("ffmpeg", "-rtsp_transport", "tcp", + "-i", url, "-frames:v", "1", "-q:v", "2", "-y", tmpFile) + if err := cmd.Run(); err != nil { + return nil, fmt.Errorf("ffmpeg: %w", err) + } + f, err := os.Open(tmpFile) + if err != nil { + return nil, err + } + defer f.Close() + img, _, err := image.Decode(f) + return img, err +} + +// --- HTTP API --- + +func (s *Server) registerRoutes(mux *http.ServeMux) { + mux.HandleFunc("/health", s.handleHealth) + mux.HandleFunc("/orientation", s.handleOrientation) + mux.HandleFunc("/grab", s.handleGrab) + mux.HandleFunc("/reset", s.handleReset) + mux.HandleFunc("/camera", s.handleCamera) + mux.HandleFunc("/slew", s.handleSlew) + mux.HandleFunc("/stop", s.handleStop) + mux.HandleFunc("/fov", s.handleFoV) +} + +func writeJSON(w http.ResponseWriter, v any) { + w.Header().Set("Content-Type", "application/json") + json.NewEncoder(w).Encode(v) +} + +func (s *Server) handleHealth(w http.ResponseWriter, _ *http.Request) { + writeJSON(w, map[string]any{ + "status": "ok", + "ip": s.cfg.IP, + "camera": s.cfg.Camera, + "cam_open": s.camOpen, + }) +} + +func (s *Server) handleOrientation(w http.ResponseWriter, _ *http.Request) { + writeJSON(w, s.tracker.Orientation()) +} + +func (s *Server) handleGrab(w http.ResponseWriter, r *http.Request) { + ch := s.rtspChannel() + url := fmt.Sprintf("rtsp://%s:554/%s/stream0", s.cfg.IP, ch) + out := "/tmp/dwarf_daemon_grab.jpg" + if q := r.URL.Query().Get("path"); q != "" { + out = q + } + cmd := exec.Command("ffmpeg", "-rtsp_transport", "tcp", + "-i", url, "-frames:v", "1", "-q:v", "2", "-y", out) + if err := cmd.Run(); err != nil { + http.Error(w, err.Error(), http.StatusInternalServerError) + return + } + // Return the image bytes or just the path. + if r.URL.Query().Get("raw") == "1" { + data, _ := os.ReadFile(out) + w.Header().Set("Content-Type", "image/jpeg") + w.Write(data) + return + } + writeJSON(w, map[string]string{"path": out}) +} + +func (s *Server) handleReset(w http.ResponseWriter, _ *http.Request) { + s.tracker.Reset() + writeJSON(w, map[string]string{"status": "reset"}) +} + +func (s *Server) handleCamera(w http.ResponseWriter, r *http.Request) { + action := r.URL.Query().Get("action") // open/close + cam := r.URL.Query().Get("cam") // wide/tele + + s.mu.Lock() + target := s.camera + if cam == "wide" { + target = api.CameraWide + } else if cam == "tele" { + target = api.CameraTele + } + + var err error + switch action { + case "open": + err = s.scope.OpenCamera(target) + if err == nil { + s.camera = target + s.camOpen = true + } + case "close": + err = s.scope.CloseCamera(target) + if err == nil { + s.camOpen = false + } + default: + err = fmt.Errorf("action must be 'open' or 'close'") + } + s.mu.Unlock() + + if err != nil { + http.Error(w, err.Error(), http.StatusBadRequest) + return + } + writeJSON(w, map[string]any{"camera": cam, "action": action, "ok": true}) +} + +func (s *Server) handleSlew(w http.ResponseWriter, r *http.Request) { + var req struct { + Angle float64 `json:"angle"` + Length float64 `json:"length"` + } + if r.Body != nil { + body, _ := io.ReadAll(r.Body) + json.Unmarshal(body, &req) + } + if err := s.scope.SlewJoystick(req.Angle, req.Length); err != nil { + http.Error(w, err.Error(), http.StatusInternalServerError) + return + } + writeJSON(w, map[string]any{"angle": req.Angle, "length": req.Length}) +} + +func (s *Server) handleStop(w http.ResponseWriter, _ *http.Request) { + s.scope.MotorJoystickStop() + writeJSON(w, map[string]string{"status": "motors stopped"}) +} + +func (s *Server) handleFoV(w http.ResponseWriter, r *http.Request) { + if r.Method == http.MethodPost { + var req struct { + H float64 `json:"h"` + V float64 `json:"v"` + } + body, _ := io.ReadAll(r.Body) + json.Unmarshal(body, &req) + s.tracker.SetFoV(req.H, req.V) + s.cfg.FoVH = req.H + s.cfg.FoVV = req.V + } + writeJSON(w, map[string]float64{"fov_h": s.cfg.FoVH, "fov_v": s.cfg.FoVV}) +} diff --git a/dwarfctl/internal/odometry/fft.go b/dwarfctl/internal/odometry/fft.go new file mode 100644 index 0000000..8a07687 --- /dev/null +++ b/dwarfctl/internal/odometry/fft.go @@ -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) +} diff --git a/dwarfctl/internal/odometry/odometry.go b/dwarfctl/internal/odometry/odometry.go new file mode 100644 index 0000000..10c8138 --- /dev/null +++ b/dwarfctl/internal/odometry/odometry.go @@ -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 +} diff --git a/dwarfctl/internal/odometry/odometry_test.go b/dwarfctl/internal/odometry/odometry_test.go new file mode 100644 index 0000000..ce99639 --- /dev/null +++ b/dwarfctl/internal/odometry/odometry_test.go @@ -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") + } +} diff --git a/dwarfctl/internal/odometry/tracker.go b/dwarfctl/internal/odometry/tracker.go new file mode 100644 index 0000000..8e3f477 --- /dev/null +++ b/dwarfctl/internal/odometry/tracker.go @@ -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, + } +}