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
This commit is contained in:
Jacquin Antoine
@ -70,6 +70,22 @@ func decodeResponse(pkt *pb.WsPacket, msg proto.Message) error {
|
||||
return proto.Unmarshal(pkt.GetData(), msg)
|
||||
}
|
||||
|
||||
// SendRaw sends a raw command with pre-serialized data (no inner proto).
|
||||
// Useful for testing with custom payloads.
|
||||
func (t *Telescope) SendRaw(cmd uint32, data []byte) (*pb.WsPacket, error) {
|
||||
return t.t.Send(ModuleIDForCmd(cmd), cmd, data, 10*time.Second)
|
||||
}
|
||||
|
||||
// --- Task Center commands ---
|
||||
|
||||
// SwitchShootingMode switches the telescope shooting mode.
|
||||
// Mode 1 = photo, 2 = video/preview. Required to activate RTSP streaming on
|
||||
// some firmwares — the camera won't stream until a shooting mode is active.
|
||||
func (t *Telescope) SwitchShootingMode(mode int32) error {
|
||||
_, err := t.send(CmdTaskSwitchMode, &pb.ReqSwitchShootingMode{Mode: mode})
|
||||
return err
|
||||
}
|
||||
|
||||
// --- Camera commands ---
|
||||
|
||||
// Camera identifies which camera to address.
|
||||
@ -87,15 +103,19 @@ func cameraCmd(tele, wide uint32, cam Camera) uint32 {
|
||||
return tele
|
||||
}
|
||||
|
||||
// OpenCamera opens the specified camera.
|
||||
// Some firmwares don't send a type=3 reply; fire-and-forget is safe.
|
||||
// OpenCamera opens the specified camera and starts RTSP streaming.
|
||||
// The rtsp_encode_type=1 field is REQUIRED for the RTSP server to activate
|
||||
// (confirmed from WsOpenCameraReq.java in the APK).
|
||||
func (t *Telescope) OpenCamera(cam Camera) error {
|
||||
return t.sendNotify(cameraCmd(CmdTeleOpenCamera, CmdWideOpenCamera, cam), &pb.ReqMotorServiceJoystickStop{})
|
||||
return t.sendNotify(cameraCmd(CmdTeleOpenCamera, CmdWideOpenCamera, cam), &pb.ReqOpenCamera{
|
||||
Binning: false,
|
||||
RtspEncodeType: 1,
|
||||
})
|
||||
}
|
||||
|
||||
// CloseCamera closes the specified camera.
|
||||
func (t *Telescope) CloseCamera(cam Camera) error {
|
||||
return t.sendNotify(cameraCmd(CmdTeleCloseCamera, CmdWideCloseCamera, cam), &pb.ReqMotorServiceJoystickStop{})
|
||||
return t.sendNotify(cameraCmd(CmdTeleCloseCamera, CmdWideCloseCamera, cam), &pb.ReqOpenCamera{})
|
||||
}
|
||||
|
||||
// TakePhoto captures a single photograph.
|
||||
@ -410,12 +430,6 @@ func (t *Telescope) GetDeviceState() (*pb.ResGetDeviceStateInfo, error) {
|
||||
return resp, decodeResponse(pkt, resp)
|
||||
}
|
||||
|
||||
// SwitchShootingMode switches the active shooting mode.
|
||||
func (t *Telescope) SwitchShootingMode(modeID int32) error {
|
||||
_, err := t.send(CmdTaskSwitchMode, &pb.ReqSwitchShootingMode{Mode: modeID})
|
||||
return err
|
||||
}
|
||||
|
||||
// --- Panorama commands ---
|
||||
|
||||
// StartPanoramaGrid starts a panorama grid scan (triggers motor home/reset).
|
||||
|
||||
@ -24,7 +24,7 @@ func NewClient(addr string) *Client {
|
||||
}
|
||||
return &Client{
|
||||
baseURL: "http://" + addr,
|
||||
http: &http.Client{Timeout: 30 * time.Second},
|
||||
http: &http.Client{Timeout: 60 * time.Second},
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -8,13 +8,10 @@ import (
|
||||
"context"
|
||||
"encoding/json"
|
||||
"fmt"
|
||||
"image"
|
||||
_ "image/jpeg"
|
||||
"io"
|
||||
"log"
|
||||
"net/http"
|
||||
"os"
|
||||
"os/exec"
|
||||
"os/signal"
|
||||
"sync"
|
||||
"syscall"
|
||||
@ -22,6 +19,7 @@ import (
|
||||
|
||||
"github.com/antitbone/dwarfctl/internal/api"
|
||||
"github.com/antitbone/dwarfctl/internal/odometry"
|
||||
"github.com/antitbone/dwarfctl/internal/rtspgrab"
|
||||
)
|
||||
|
||||
// DefaultAddr is the default HTTP listen address for the daemon.
|
||||
@ -29,14 +27,15 @@ const DefaultAddr = "127.0.0.1:7777"
|
||||
|
||||
// Config configures the daemon.
|
||||
type Config struct {
|
||||
IP string
|
||||
Addr string
|
||||
FoVH float64
|
||||
FoVV float64
|
||||
IP string
|
||||
Addr string
|
||||
FoVH float64
|
||||
FoVV float64
|
||||
GridSize int
|
||||
Camera string // "wide" or "tele"
|
||||
Camera string // "wide" or "tele"
|
||||
Interval time.Duration // odometry frame interval
|
||||
Debug bool
|
||||
SettleDelay time.Duration // pause after motor stop before resuming
|
||||
Debug bool
|
||||
}
|
||||
|
||||
// Server is the persistent telescope daemon.
|
||||
@ -45,11 +44,15 @@ type Server struct {
|
||||
scope *api.Telescope
|
||||
tracker *odometry.Tracker
|
||||
|
||||
mu sync.Mutex
|
||||
camera api.Camera
|
||||
camOpen bool
|
||||
stopCh chan struct{}
|
||||
wg sync.WaitGroup
|
||||
mu sync.Mutex
|
||||
camera api.Camera
|
||||
camOpen bool
|
||||
stopCh chan struct{}
|
||||
wg sync.WaitGroup
|
||||
|
||||
// paused disables odometry frame capture (e.g. during motor slew to avoid
|
||||
// motion blur which destroys phase correlation).
|
||||
paused bool
|
||||
}
|
||||
|
||||
// New creates a daemon server.
|
||||
@ -72,6 +75,9 @@ func New(cfg Config) *Server {
|
||||
if cfg.Interval <= 0 {
|
||||
cfg.Interval = 5 * time.Second
|
||||
}
|
||||
if cfg.SettleDelay <= 0 {
|
||||
cfg.SettleDelay = 3 * time.Second
|
||||
}
|
||||
return &Server{
|
||||
cfg: cfg,
|
||||
stopCh: make(chan struct{}),
|
||||
@ -97,11 +103,14 @@ func (s *Server) Run() error {
|
||||
s.camera = api.CameraWide
|
||||
}
|
||||
log.Printf("Opening %s camera...", s.cfg.Camera)
|
||||
// Switch to shooting mode 1 (photo/preview) first — required for streaming.
|
||||
s.scope.SwitchShootingMode(1)
|
||||
if err := s.scope.OpenCamera(s.camera); err != nil {
|
||||
return fmt.Errorf("open camera: %w", err)
|
||||
}
|
||||
s.camOpen = true
|
||||
log.Println("Camera opened.")
|
||||
log.Println("Camera opened. Waiting 5s for RTSP to initialize...")
|
||||
time.Sleep(5 * time.Second)
|
||||
|
||||
// 3. Start odometry loop in background.
|
||||
s.wg.Add(1)
|
||||
@ -157,7 +166,7 @@ func (s *Server) odometryLoop() {
|
||||
defer ticker.Stop()
|
||||
|
||||
// Prime with first frame.
|
||||
if img, err := grabFrame(url, tmpFile); err == nil {
|
||||
if img, err := rtspgrab.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 {
|
||||
@ -169,7 +178,16 @@ func (s *Server) odometryLoop() {
|
||||
case <-s.stopCh:
|
||||
return
|
||||
case <-ticker.C:
|
||||
img, err := grabFrame(url, tmpFile)
|
||||
// Skip frame capture while motors are moving (motion blur destroys
|
||||
// phase correlation). The slew handler sets paused=true/false.
|
||||
s.mu.Lock()
|
||||
if s.paused {
|
||||
s.mu.Unlock()
|
||||
continue
|
||||
}
|
||||
s.mu.Unlock()
|
||||
|
||||
img, err := rtspgrab.GrabFrame(url, tmpFile)
|
||||
if err != nil {
|
||||
if s.cfg.Debug {
|
||||
log.Printf("grab error: %v", err)
|
||||
@ -197,22 +215,6 @@ func (s *Server) rtspChannel() string {
|
||||
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) {
|
||||
@ -223,6 +225,8 @@ func (s *Server) registerRoutes(mux *http.ServeMux) {
|
||||
mux.HandleFunc("/camera", s.handleCamera)
|
||||
mux.HandleFunc("/slew", s.handleSlew)
|
||||
mux.HandleFunc("/stop", s.handleStop)
|
||||
mux.HandleFunc("/pause", s.handlePause)
|
||||
mux.HandleFunc("/resume", s.handleResume)
|
||||
mux.HandleFunc("/fov", s.handleFoV)
|
||||
}
|
||||
|
||||
@ -251,13 +255,10 @@ func (s *Server) handleGrab(w http.ResponseWriter, r *http.Request) {
|
||||
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 {
|
||||
if _, err := rtspgrab.GrabFrame(url, out); 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")
|
||||
@ -318,16 +319,47 @@ func (s *Server) handleSlew(w http.ResponseWriter, r *http.Request) {
|
||||
body, _ := io.ReadAll(r.Body)
|
||||
json.Unmarshal(body, &req)
|
||||
}
|
||||
// Pause odometry during slew to avoid motion blur.
|
||||
s.mu.Lock()
|
||||
s.paused = true
|
||||
s.mu.Unlock()
|
||||
|
||||
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})
|
||||
writeJSON(w, map[string]any{"angle": req.Angle, "length": req.Length, "odometry": "paused"})
|
||||
}
|
||||
|
||||
func (s *Server) handleStop(w http.ResponseWriter, _ *http.Request) {
|
||||
s.scope.MotorJoystickStop()
|
||||
writeJSON(w, map[string]string{"status": "motors stopped"})
|
||||
// Keep odometry paused — a settle delay prevents motion-blurred frames
|
||||
// from corrupting the tracker. The odometry loop auto-resumes after the
|
||||
// delay via a timer.
|
||||
s.mu.Lock()
|
||||
s.paused = true
|
||||
s.mu.Unlock()
|
||||
go func() {
|
||||
time.Sleep(s.cfg.SettleDelay)
|
||||
s.mu.Lock()
|
||||
s.paused = false
|
||||
s.mu.Unlock()
|
||||
}()
|
||||
writeJSON(w, map[string]string{"status": "motors stopped, settling..."})
|
||||
}
|
||||
|
||||
func (s *Server) handlePause(w http.ResponseWriter, _ *http.Request) {
|
||||
s.mu.Lock()
|
||||
s.paused = true
|
||||
s.mu.Unlock()
|
||||
writeJSON(w, map[string]string{"status": "odometry paused"})
|
||||
}
|
||||
|
||||
func (s *Server) handleResume(w http.ResponseWriter, _ *http.Request) {
|
||||
s.mu.Lock()
|
||||
s.paused = false
|
||||
s.mu.Unlock()
|
||||
writeJSON(w, map[string]string{"status": "odometry resumed"})
|
||||
}
|
||||
|
||||
func (s *Server) handleFoV(w http.ResponseWriter, r *http.Request) {
|
||||
|
||||
@ -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,
|
||||
}
|
||||
|
||||
53
dwarfctl/internal/odometry/rotate.go
Normal file
53
dwarfctl/internal/odometry/rotate.go
Normal file
@ -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
|
||||
}
|
||||
187
dwarfctl/internal/odometry/rotation.go
Normal file
187
dwarfctl/internal/odometry/rotation.go
Normal file
@ -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
|
||||
}
|
||||
@ -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,
|
||||
|
||||
79
dwarfctl/internal/rtspgrab/grabber.go
Normal file
79
dwarfctl/internal/rtspgrab/grabber.go
Normal file
@ -0,0 +1,79 @@
|
||||
// Package rtspgrab captures single frames from the DWARF telescope's RTSP
|
||||
// server. The telescope's RTSP implementation is non-standard: ffmpeg can
|
||||
// connect and capture frames but does not terminate cleanly (it hangs after
|
||||
// writing the output file). This package works around that by launching ffmpeg
|
||||
// in the background, watching for the output file to appear, then killing ffmpeg.
|
||||
package rtspgrab
|
||||
|
||||
import (
|
||||
"context"
|
||||
"fmt"
|
||||
"image"
|
||||
_ "image/jpeg"
|
||||
"os"
|
||||
"os/exec"
|
||||
"time"
|
||||
)
|
||||
|
||||
// GrabFrame connects to the RTSP URL, captures one MJPEG frame, saves it to
|
||||
// outFile, and returns the decoded image. It uses ffmpeg with low-latency
|
||||
// options required by the DWARF telescope's non-standard RTSP server, then
|
||||
// kills ffmpeg once the output file is written (ffmpeg hangs after capture).
|
||||
func GrabFrame(rtspURL, outFile string) (image.Image, error) {
|
||||
// Remove any stale output.
|
||||
os.Remove(outFile)
|
||||
|
||||
ctx, cancel := context.WithTimeout(context.Background(), 30*time.Second)
|
||||
defer cancel()
|
||||
|
||||
cmd := exec.CommandContext(ctx, "ffmpeg",
|
||||
"-rtsp_transport", "tcp",
|
||||
"-fflags", "nobuffer",
|
||||
"-flags", "low_delay",
|
||||
"-probesize", "4096",
|
||||
"-analyzeduration", "1000000",
|
||||
"-i", rtspURL,
|
||||
"-frames:v", "1", "-q:v", "2", "-y", outFile)
|
||||
cmd.Stdout = nil
|
||||
cmd.Stderr = nil
|
||||
|
||||
if err := cmd.Start(); err != nil {
|
||||
return nil, fmt.Errorf("start ffmpeg: %w", err)
|
||||
}
|
||||
|
||||
// Poll for the output file. Once it appears with non-zero size, kill ffmpeg.
|
||||
deadline := time.Now().Add(25 * time.Second)
|
||||
for time.Now().Before(deadline) {
|
||||
if info, err := os.Stat(outFile); err == nil && info.Size() > 0 {
|
||||
// File written — kill ffmpeg (it won't exit on its own).
|
||||
cmd.Process.Kill()
|
||||
cmd.Wait()
|
||||
break
|
||||
}
|
||||
select {
|
||||
case <-ctx.Done():
|
||||
cmd.Process.Kill()
|
||||
return nil, fmt.Errorf("timeout waiting for frame")
|
||||
case <-time.After(200 * time.Millisecond):
|
||||
}
|
||||
}
|
||||
|
||||
// Check if we got the file.
|
||||
info, err := os.Stat(outFile)
|
||||
if err != nil || info.Size() == 0 {
|
||||
cmd.Process.Kill()
|
||||
return nil, fmt.Errorf("no frame captured (ffmpeg produced no output)")
|
||||
}
|
||||
|
||||
// Decode the JPEG.
|
||||
f, err := os.Open(outFile)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
defer f.Close()
|
||||
img, _, err := image.Decode(f)
|
||||
if err != nil {
|
||||
return nil, fmt.Errorf("decode JPEG: %w", err)
|
||||
}
|
||||
return img, nil
|
||||
}
|
||||
@ -60,10 +60,41 @@ func (c *Client) Connect(ip string) error {
|
||||
return fmt.Errorf("ws dial %s: %w", url, err)
|
||||
}
|
||||
c.conn = conn
|
||||
// Keep the connection alive — the telescope closes idle connections
|
||||
// after ~30s. The PongHandler also resets the read deadline on each pong.
|
||||
conn.SetPingHandler(func(string) error {
|
||||
conn.WriteControl(websocket.PongMessage, nil, time.Now().Add(5*time.Second))
|
||||
return nil
|
||||
})
|
||||
go c.readLoop()
|
||||
go c.heartbeatLoop()
|
||||
return nil
|
||||
}
|
||||
|
||||
// heartbeatLoop sends periodic ping frames to keep the WebSocket alive.
|
||||
// The telescope drops idle connections after ~30s.
|
||||
func (c *Client) heartbeatLoop() {
|
||||
ticker := time.NewTicker(10 * time.Second)
|
||||
defer ticker.Stop()
|
||||
for {
|
||||
select {
|
||||
case <-c.done:
|
||||
return
|
||||
case <-ticker.C:
|
||||
c.mu.Lock()
|
||||
if c.conn == nil {
|
||||
c.mu.Unlock()
|
||||
return
|
||||
}
|
||||
err := c.conn.WriteControl(websocket.PingMessage, nil, time.Now().Add(5*time.Second))
|
||||
c.mu.Unlock()
|
||||
if err != nil && c.Debug {
|
||||
log.Printf("[WS] heartbeat ping error: %v", err)
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Close shuts down the connection.
|
||||
func (c *Client) Close() error {
|
||||
close(c.done)
|
||||
|
||||
Reference in New Issue
Block a user