blambert commited on
Commit
a9f58f8
·
1 Parent(s): eafea2f

feat: scan continuously while detecting motion

Browse files

Replace step-and-pause scanning with a smooth head sweep. While the head
moves, a new ScanDetector cancels the camera's own motion by fitting a
homography to tracked background features, warping the older frame onto
the newer one, and differencing them. The settings page gets a slider for
its threshold and shows the aligned difference while scanning.

README.md CHANGED
@@ -15,43 +15,52 @@ tags:
15
 
16
  A Reachy Mini app that looks at whatever is moving. It uses OpenCV's MOG2
17
  background subtractor to find pixels that differ from the learned background,
18
- groups them into blobs, and turns its head toward the largest one. After each move the
19
- background is relearned, since the robot's own motion changes the whole view.
20
-
21
- If nothing moves for a few seconds, it scans: it sweeps left and right in
22
- steps, pausing at each position to watch for motion, and goes back to tracking
23
- as soon as it sees something.
 
 
 
 
 
24
 
25
  ## Motion Map
26
 
27
  The settings page at `http://<robot>:8042` shows a live motion map for checking
28
  detection: the camera view with the largest blob boxed (green if the robot will
29
- turn toward it, red if it is below the minimum blob size), the raw
30
- background-subtraction mask, and the cleaned mask used to pick the blob. Above
 
31
  it is what the robot is doing (learning the background, watching, tracking, or
32
  scanning).
33
 
34
  ## Tuning
35
 
36
- The settings page has sliders for the change threshold and the minimum blob
37
- size, so detection can be tuned live while watching the motion map. Changes last
38
- until the app restarts; the defaults are the constants at the top of
39
- `movement_tracker/motion.py`:
40
 
41
- | Constant | Meaning |
42
- |-----------------|------------------------------------------------------------------|
43
- | `VAR_THRESHOLD` | MOG2 change threshold; raise it to ignore noise and flicker. |
44
- | `MIN_BLOB_FRAC` | Smallest blob, as a fraction of the image, that triggers a turn. |
45
- | `WARMUP` | Frames used to learn the background before reacting. |
 
 
46
 
47
  Behavior constants live at the top of `movement_tracker/main.py`:
48
 
49
- | Constant | Meaning |
50
- |----------------|-------------------------------------------------------|
51
- | `DEADBAND` | How far from the image center motion must be to turn. |
52
- | `IDLE_TIMEOUT` | Seconds without motion before scanning starts. |
53
- | `SCAN_DWELL` | Seconds spent watching at each scan position. |
54
- | `SCAN_RANGE` | How far the head sweeps each way, relative to body. |
 
55
 
56
  ## Deployment
57
 
 
15
 
16
  A Reachy Mini app that looks at whatever is moving. It uses OpenCV's MOG2
17
  background subtractor to find pixels that differ from the learned background,
18
+ groups them into blobs, and turns its head toward the largest one. After each
19
+ move the background is relearned, since the robot's own motion changes the
20
+ whole view.
21
+
22
+ If nothing moves for a while, it scans: the head sweeps smoothly left and right
23
+ while watching for motion. MOG2 needs a still camera, so scanning uses a second
24
+ detector that cancels out the head's own motion: it tracks background features
25
+ between frames, fits the homography that explains how they moved (a turning
26
+ head is close to a pure rotation, which a homography models exactly), warps the
27
+ older frame to line up with the newer one, and looks for what still differs.
28
+ When it sees motion, the robot turns toward it and goes back to tracking.
29
 
30
  ## Motion Map
31
 
32
  The settings page at `http://<robot>:8042` shows a live motion map for checking
33
  detection: the camera view with the largest blob boxed (green if the robot will
34
+ turn toward it, red if it is below the minimum blob size), the raw detector
35
+ output (the background-subtraction mask while still, the aligned frame
36
+ difference while scanning), and the cleaned mask used to pick the blob. Above
37
  it is what the robot is doing (learning the background, watching, tracking, or
38
  scanning).
39
 
40
  ## Tuning
41
 
42
+ The settings page has sliders for the change thresholds (still and scanning)
43
+ and the minimum blob size, so detection can be tuned live while watching the
44
+ motion map. Changes last until the app restarts; the defaults are the constants
45
+ at the top of `movement_tracker/motion.py`:
46
 
47
+ | Constant | Meaning |
48
+ |-------------------|-------------------------------------------------------------------|
49
+ | `VAR_THRESHOLD` | MOG2 change threshold; raise it to ignore noise and flicker. |
50
+ | `MIN_BLOB_FRAC` | Smallest blob, as a fraction of the image, that triggers a turn. |
51
+ | `WARMUP` | Frames used to learn the background before reacting. |
52
+ | `DIFF_THRESHOLD` | Change, after cancelling head motion, that counts while scanning. |
53
+ | `FRAME_GAP` | Frames apart when comparing while scanning; higher sees slower. |
54
 
55
  Behavior constants live at the top of `movement_tracker/main.py`:
56
 
57
+ | Constant | Meaning |
58
+ |----------------|-----------------------------------------------------------|
59
+ | `DEADBAND` | How far from the image center motion must be to turn. |
60
+ | `IDLE_TIMEOUT` | Seconds without motion before scanning starts. |
61
+ | `SCAN_RANGE` | How far the head sweeps each way, relative to body. |
62
+ | `SCAN_SPEED` | Sweep speed; faster blurs frames and breaks alignment. |
63
+ | `SCAN_CONFIRM` | Consecutive detections needed to stop a sweep. |
64
 
65
  ## Deployment
66
 
movement_tracker/main.py CHANGED
@@ -1,4 +1,3 @@
1
- import itertools
2
  import threading
3
  import time
4
 
@@ -9,17 +8,16 @@ from pydantic import BaseModel, Field
9
  from reachy_mini import ReachyMini, ReachyMiniApp
10
  from reachy_mini.utils import create_head_pose
11
 
12
- from movement_tracker.motion import WARMUP, MotionDetector
13
 
14
  DEADBAND = 0.08 # ignore motion this close to image center (fraction of size)
15
  MOVE_DURATION = 1.0
16
  SETTLE = 0.3 # extra wait after a move before frames are trusted again
17
 
18
  IDLE_TIMEOUT = 15.0 # seconds without motion before scanning starts
19
- SCAN_DWELL = 2.5 # seconds to watch at each scan position; must exceed detector warmup
20
- # sweep as fractions of the scan range: out to the left, back, out to the right
21
- SCAN_PATTERN = (0.5, 1, 0.5, 0, -0.5, -1, -0.5, 0)
22
  SCAN_RANGE = np.deg2rad(50) # relative to the body, which allows up to 65
 
 
23
 
24
 
25
  class StateResp(BaseModel):
@@ -28,6 +26,7 @@ class StateResp(BaseModel):
28
 
29
  class Sensitivity(BaseModel):
30
  var_threshold: float = Field(ge=1, le=100)
 
31
  min_blob_frac: float = Field(ge=0, le=0.1)
32
 
33
 
@@ -43,8 +42,10 @@ class MovementTracker(ReachyMiniApp):
43
 
44
  def __init__(self, *args, **kwargs) -> None:
45
  super().__init__(*args, **kwargs)
46
- # created here so the settings page can show its debug view
47
  self.detector = MotionDetector()
 
 
48
  if self.settings_app is None:
49
  return
50
 
@@ -54,29 +55,38 @@ class MovementTracker(ReachyMiniApp):
54
 
55
  @self.settings_app.get("/sensitivity")
56
  def get_sensitivity() -> Sensitivity:
57
- d = self.detector
58
  return Sensitivity(
59
- var_threshold=d.var_threshold, min_blob_frac=d.min_blob_frac
 
 
60
  )
61
 
62
  @self.settings_app.post("/sensitivity")
63
  def set_sensitivity(req: Sensitivity) -> Sensitivity:
64
  self.detector.set_var_threshold(req.var_threshold)
 
65
  self.detector.min_blob_frac = req.min_blob_frac
 
66
  return req
67
 
68
  @self.settings_app.get("/motion.jpg")
69
  def motion_jpg() -> Response:
70
- img = self.detector.debug_image()
71
  if img is None:
72
  return Response(status_code=204)
73
  _, jpg = cv2.imencode(".jpg", img)
74
  return Response(jpg.tobytes(), media_type="image/jpeg")
75
 
76
  def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
77
- detector = self.detector
78
- scan = itertools.cycle(SCAN_PATTERN)
79
- last_active, scanning = time.monotonic(), False
 
 
 
 
 
 
80
 
81
  while not stop_event.is_set():
82
  frame = reachy_mini.media.get_frame()
@@ -86,48 +96,76 @@ class MovementTracker(ReachyMiniApp):
86
 
87
  target = detector.detect(frame)
88
  if target is None:
89
- idle = time.monotonic() - last_active
90
- if idle < (SCAN_DWELL if scanning else IDLE_TIMEOUT):
91
- learning = detector.n < WARMUP
92
- self.state = "learning background" if learning else "watching"
93
- time.sleep(0.05)
94
- continue
95
- self.scan_to(reachy_mini, next(scan))
96
- scanning = True
97
- else:
98
- scanning = False
99
- h, w = frame.shape[:2]
100
- u, v = target
101
- if abs(u / w - 0.5) < DEADBAND and abs(v / h - 0.5) < DEADBAND:
102
- self.state = "watching motion at center"
103
- last_active = time.monotonic() # already looking at it
104
- continue
105
- self.track(reachy_mini, u, v, w, h)
106
-
107
- # the camera moved, so the learned background is stale; wait it out and relearn
108
- time.sleep(MOVE_DURATION + SETTLE)
109
- detector.reset()
110
  last_active = time.monotonic()
111
 
112
- def track(
113
- self, reachy_mini: ReachyMini, u: float, v: float, w: int, h: int
114
- ) -> None:
115
- """Turn the head toward the image point (u, v)."""
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
116
  # look_at_image requires coordinates strictly inside the image
117
  u, v = int(np.clip(u, 1, w - 1)), int(np.clip(v, 1, h - 1))
118
  self.state = f"tracking motion at ({u}, {v})"
119
  print(self.state)
120
  reachy_mini.look_at_image(u, v, duration=MOVE_DURATION)
121
-
122
- def scan_to(self, reachy_mini: ReachyMini, frac: float) -> None:
123
- """Look level at a fraction of the scan range (positive = left)."""
124
- # scan relative to the body so the head stays within its yaw limit
125
- body_yaw = reachy_mini.get_current_joint_positions()[0][0]
126
- yaw = body_yaw + frac * SCAN_RANGE
127
- self.state = f"scanning {fmt_lr(yaw)}"
128
- print(self.state)
129
- head = create_head_pose(yaw=yaw, degrees=False)
130
- reachy_mini.goto_target(head=head, duration=MOVE_DURATION)
131
 
132
 
133
  def fmt_lr(angle: float) -> str:
 
 
1
  import threading
2
  import time
3
 
 
8
  from reachy_mini import ReachyMini, ReachyMiniApp
9
  from reachy_mini.utils import create_head_pose
10
 
11
+ from movement_tracker.motion import WARMUP, MotionDetector, ScanDetector, debug_image
12
 
13
  DEADBAND = 0.08 # ignore motion this close to image center (fraction of size)
14
  MOVE_DURATION = 1.0
15
  SETTLE = 0.3 # extra wait after a move before frames are trusted again
16
 
17
  IDLE_TIMEOUT = 15.0 # seconds without motion before scanning starts
 
 
 
18
  SCAN_RANGE = np.deg2rad(50) # relative to the body, which allows up to 65
19
+ SCAN_SPEED = np.deg2rad(15) # per second; faster blurs frames and breaks alignment
20
+ SCAN_CONFIRM = 3 # consecutive detections needed to stop a sweep
21
 
22
 
23
  class StateResp(BaseModel):
 
26
 
27
  class Sensitivity(BaseModel):
28
  var_threshold: float = Field(ge=1, le=100)
29
+ diff_threshold: float = Field(ge=1, le=100)
30
  min_blob_frac: float = Field(ge=0, le=0.1)
31
 
32
 
 
42
 
43
  def __init__(self, *args, **kwargs) -> None:
44
  super().__init__(*args, **kwargs)
45
+ # created here so the settings page can show their debug views
46
  self.detector = MotionDetector()
47
+ self.scan_detector = ScanDetector()
48
+ self.active: MotionDetector | ScanDetector = self.detector
49
  if self.settings_app is None:
50
  return
51
 
 
55
 
56
  @self.settings_app.get("/sensitivity")
57
  def get_sensitivity() -> Sensitivity:
 
58
  return Sensitivity(
59
+ var_threshold=self.detector.var_threshold,
60
+ diff_threshold=self.scan_detector.diff_threshold,
61
+ min_blob_frac=self.detector.min_blob_frac,
62
  )
63
 
64
  @self.settings_app.post("/sensitivity")
65
  def set_sensitivity(req: Sensitivity) -> Sensitivity:
66
  self.detector.set_var_threshold(req.var_threshold)
67
+ self.scan_detector.diff_threshold = req.diff_threshold
68
  self.detector.min_blob_frac = req.min_blob_frac
69
+ self.scan_detector.min_blob_frac = req.min_blob_frac
70
  return req
71
 
72
  @self.settings_app.get("/motion.jpg")
73
  def motion_jpg() -> Response:
74
+ img = debug_image(self.active.snapshot)
75
  if img is None:
76
  return Response(status_code=204)
77
  _, jpg = cv2.imencode(".jpg", img)
78
  return Response(jpg.tobytes(), media_type="image/jpeg")
79
 
80
  def run(self, reachy_mini: ReachyMini, stop_event: threading.Event):
81
+ while not stop_event.is_set():
82
+ self.watch(reachy_mini, stop_event)
83
+ self.scan(reachy_mini, stop_event)
84
+
85
+ def watch(self, reachy_mini: ReachyMini, stop_event: threading.Event) -> None:
86
+ """Track motion with the head still between moves, until it stays quiet."""
87
+ detector = self.active = self.detector
88
+ detector.reset()
89
+ last_active = time.monotonic()
90
 
91
  while not stop_event.is_set():
92
  frame = reachy_mini.media.get_frame()
 
96
 
97
  target = detector.detect(frame)
98
  if target is None:
99
+ if time.monotonic() - last_active > IDLE_TIMEOUT:
100
+ return
101
+ learning = detector.n < WARMUP
102
+ self.state = "learning background" if learning else "watching"
103
+ time.sleep(0.05)
104
+ continue
105
+
106
+ if self.turn_toward(reachy_mini, target, frame.shape):
107
+ # the camera moved, so the learned background is stale; relearn it
108
+ detector.reset()
 
 
 
 
 
 
 
 
 
 
 
109
  last_active = time.monotonic()
110
 
111
+ def scan(self, reachy_mini: ReachyMini, stop_event: threading.Event) -> None:
112
+ """Sweep the head smoothly side to side until motion is seen, then face it."""
113
+ # scan relative to the body so the head stays within its yaw limit
114
+ body_yaw = reachy_mini.get_current_joint_positions()[0][0]
115
+ rot = reachy_mini.get_current_head_pose()[:3, :3]
116
+ offset = np.clip(
117
+ np.arctan2(rot[1, 0], rot[0, 0]) - body_yaw, -SCAN_RANGE, SCAN_RANGE
118
+ )
119
+
120
+ # level the head first so the sweep is a pure yaw rotation
121
+ head = create_head_pose(yaw=body_yaw + offset, degrees=False)
122
+ reachy_mini.goto_target(head=head, duration=MOVE_DURATION)
123
+ time.sleep(MOVE_DURATION)
124
+
125
+ detector = self.active = self.scan_detector
126
+ detector.reset()
127
+ direction = 1.0 if offset <= 0 else -1.0 # head toward the far side first
128
+ t, hits = time.monotonic(), 0
129
+
130
+ while not stop_event.is_set():
131
+ now = time.monotonic()
132
+ offset += direction * SCAN_SPEED * (now - t)
133
+ t = now
134
+ if abs(offset) >= SCAN_RANGE:
135
+ offset, direction = np.sign(offset) * SCAN_RANGE, -direction
136
+ yaw = body_yaw + offset
137
+ reachy_mini.set_target(head=create_head_pose(yaw=yaw, degrees=False))
138
+ self.state = f"scanning {fmt_lr(yaw)}"
139
+
140
+ frame = reachy_mini.media.get_frame()
141
+ if frame is None:
142
+ time.sleep(0.05)
143
+ continue
144
+
145
+ # require a few detections in a row so one bad alignment can't stop the sweep
146
+ target = detector.detect(frame)
147
+ hits = hits + 1 if target else 0
148
+ if hits >= SCAN_CONFIRM:
149
+ self.turn_toward(reachy_mini, target, frame.shape)
150
+ return
151
+
152
+ def turn_toward(
153
+ self, reachy_mini: ReachyMini, target: tuple[float, float], shape: tuple
154
+ ) -> bool:
155
+ """Turn the head toward an image point unless it is already centered."""
156
+ h, w = shape[:2]
157
+ u, v = target
158
+ if abs(u / w - 0.5) < DEADBAND and abs(v / h - 0.5) < DEADBAND:
159
+ self.state = "watching motion at center"
160
+ return False
161
+
162
  # look_at_image requires coordinates strictly inside the image
163
  u, v = int(np.clip(u, 1, w - 1)), int(np.clip(v, 1, h - 1))
164
  self.state = f"tracking motion at ({u}, {v})"
165
  print(self.state)
166
  reachy_mini.look_at_image(u, v, duration=MOVE_DURATION)
167
+ time.sleep(MOVE_DURATION + SETTLE)
168
+ return True
 
 
 
 
 
 
 
 
169
 
170
 
171
  def fmt_lr(angle: float) -> str:
movement_tracker/motion.py CHANGED
@@ -1,12 +1,22 @@
 
 
 
1
  import cv2
2
  import numpy as np
3
 
4
  WIDTH = 320 # frames are downscaled to this width before processing
5
- # defaults; both can be changed live from the settings page
6
  VAR_THRESHOLD = 16 # MOG2 sensitivity; higher ignores subtler changes
7
  MIN_BLOB_FRAC = 0.0002 # smallest blob, as a fraction of the image, worth reacting to
8
  WARMUP = 15 # frames MOG2 needs to learn the background before its mask is useful
9
 
 
 
 
 
 
 
 
10
  KERNEL = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
11
  BLOB_COLOR = (0, 255, 0)
12
  SMALL_BLOB_COLOR = (0, 0, 255) # largest blob, but below MIN_BLOB_FRAC
@@ -14,15 +24,26 @@ LABEL_COLOR = (0, 200, 255)
14
  DEBUG_WIDTH = 1280 # panels are upscaled so labels and boxes stay crisp
15
 
16
 
 
 
 
 
 
 
 
 
 
 
 
 
17
  class MotionDetector:
18
  """Find the largest moving blob in a stream of frames from a still camera."""
19
 
20
  def __init__(self) -> None:
21
  self.var_threshold = VAR_THRESHOLD
22
  self.min_blob_frac = MIN_BLOB_FRAC
23
- # latest (frame, raw mask, cleaned mask, blob box, big enough) for the debug
24
- # view; replaced as a whole so readers on other threads see a consistent set
25
- self.snapshot: tuple | None = None
26
  self.reset()
27
 
28
  def set_var_threshold(self, value: float) -> None:
@@ -41,51 +62,147 @@ class MotionDetector:
41
 
42
  def detect(self, frame: np.ndarray) -> tuple[float, float] | None:
43
  """Return the (x, y) full-res pixel center of the largest moving blob, or None."""
44
- scale = frame.shape[1] / WIDTH
45
- small = cv2.resize(
46
- frame, None, fx=1 / scale, fy=1 / scale, interpolation=cv2.INTER_AREA
47
- )
48
- small = cv2.GaussianBlur(small, (5, 5), 0)
49
-
50
  raw = self.sub.apply(small)
51
  self.n += 1
52
  if self.n < WARMUP:
53
- self.snapshot = (small, raw, None, None, False)
54
  return None
55
 
56
- # drop speckle, then join fragments of the same object
57
- mask = cv2.morphologyEx(raw, cv2.MORPH_OPEN, KERNEL)
58
- mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, KERNEL, iterations=2)
 
59
 
60
- blob = largest_blob(mask)
61
- ok = blob is not None and blob[2] >= self.min_blob_frac * mask.size
62
- self.snapshot = (small, raw, mask, blob and blob[0], ok)
63
- if not ok:
64
- return None
65
 
66
- x, y = blob[1]
67
- return float(x * scale), float(y * scale)
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
68
 
69
- def debug_image(self) -> np.ndarray | None:
70
- """Render the latest frame above the raw and cleaned masks."""
71
- if self.snapshot is None:
 
 
72
  return None
73
 
74
- small, raw, mask, box, ok = self.snapshot
75
- frame = upscale(small, DEBUG_WIDTH, cv2.INTER_LINEAR)
76
- if box is not None:
77
- f = DEBUG_WIDTH / small.shape[1]
78
- x, y, w, h = (round(v * f) for v in box)
79
- color = BLOB_COLOR if ok else SMALL_BLOB_COLOR
80
- cv2.rectangle(frame, (x, y), (x + w, y + h), color, 3)
81
-
82
- # during warmup there is no cleaned mask yet
83
- mask = np.zeros_like(raw) if mask is None else mask
84
- masks = [
85
- label(upscale(cv2.cvtColor(m, cv2.COLOR_GRAY2BGR), DEBUG_WIDTH // 2), text)
86
- for text, m in (("raw mask", raw), ("cleaned mask", mask))
87
- ]
88
- return np.vstack([label(frame, "camera"), np.hstack(masks)])
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
89
 
90
 
91
  def upscale(img: np.ndarray, width: int, interp: int = cv2.INTER_NEAREST) -> np.ndarray:
 
1
+ from collections import deque
2
+ from dataclasses import dataclass
3
+
4
  import cv2
5
  import numpy as np
6
 
7
  WIDTH = 320 # frames are downscaled to this width before processing
8
+ # defaults; these can be changed live from the settings page
9
  VAR_THRESHOLD = 16 # MOG2 sensitivity; higher ignores subtler changes
10
  MIN_BLOB_FRAC = 0.0002 # smallest blob, as a fraction of the image, worth reacting to
11
  WARMUP = 15 # frames MOG2 needs to learn the background before its mask is useful
12
 
13
+ # moving-camera detection
14
+ FRAME_GAP = 6 # compare frames this far apart so slow motion still shows up
15
+ DIFF_THRESHOLD = 20 # grayscale change, after alignment, that counts as motion
16
+ MIN_FEATURES = 30 # tracked points needed to trust the alignment
17
+ MIN_INLIER_FRAC = 0.5 # fraction of points that must agree on the camera motion
18
+ MAX_MOTION_FRAC = 0.25 # more of the image than this "moving" means alignment failed
19
+
20
  KERNEL = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5))
21
  BLOB_COLOR = (0, 255, 0)
22
  SMALL_BLOB_COLOR = (0, 0, 255) # largest blob, but below MIN_BLOB_FRAC
 
24
  DEBUG_WIDTH = 1280 # panels are upscaled so labels and boxes stay crisp
25
 
26
 
27
+ @dataclass(frozen=True)
28
+ class Snapshot:
29
+ """One frame's detection state, for the debug view."""
30
+
31
+ frame: np.ndarray # downscaled BGR frame
32
+ raw: np.ndarray # detector output before cleaning
33
+ raw_label: str
34
+ mask: np.ndarray | None # cleaned mask; None until the detector is ready
35
+ box: tuple[int, ...] | None # largest blob
36
+ ok: bool # largest blob is big enough to track
37
+
38
+
39
  class MotionDetector:
40
  """Find the largest moving blob in a stream of frames from a still camera."""
41
 
42
  def __init__(self) -> None:
43
  self.var_threshold = VAR_THRESHOLD
44
  self.min_blob_frac = MIN_BLOB_FRAC
45
+ # replaced as a whole so readers on other threads see a consistent set
46
+ self.snapshot: Snapshot | None = None
 
47
  self.reset()
48
 
49
  def set_var_threshold(self, value: float) -> None:
 
62
 
63
  def detect(self, frame: np.ndarray) -> tuple[float, float] | None:
64
  """Return the (x, y) full-res pixel center of the largest moving blob, or None."""
65
+ small, scale = shrink(frame)
 
 
 
 
 
66
  raw = self.sub.apply(small)
67
  self.n += 1
68
  if self.n < WARMUP:
69
+ self.snapshot = Snapshot(small, raw, "raw mask", None, None, False)
70
  return None
71
 
72
+ self.snapshot, target = pick(
73
+ small, raw, "raw mask", clean(raw), scale, self.min_blob_frac
74
+ )
75
+ return target
76
 
 
 
 
 
 
77
 
78
+ class ScanDetector:
79
+ """Find the largest moving blob while the camera turns, by cancelling its motion.
80
+
81
+ Consecutive frames are aligned with a homography fitted to tracked background
82
+ features (a turning head is close to a pure rotation, which a homography models
83
+ exactly), so after warping only independently moving things differ.
84
+ """
85
+
86
+ def __init__(self) -> None:
87
+ self.diff_threshold = DIFF_THRESHOLD
88
+ self.min_blob_frac = MIN_BLOB_FRAC
89
+ self.snapshot: Snapshot | None = None
90
+ self.reset()
91
+
92
+ def reset(self) -> None:
93
+ """Drop frame history, e.g. when a new sweep starts."""
94
+ self.history: deque[np.ndarray] = deque(maxlen=FRAME_GAP + 1)
95
+
96
+ def detect(self, frame: np.ndarray) -> tuple[float, float] | None:
97
+ """Return the (x, y) full-res pixel center of the largest moving blob, or None."""
98
+ small, scale = shrink(frame)
99
+ gray = cv2.cvtColor(small, cv2.COLOR_BGR2GRAY)
100
+ if self.history and np.array_equal(gray, self.history[-1]):
101
+ return None # repeated frame; comparing it would hide motion
102
+ self.history.append(gray)
103
+ if len(self.history) <= FRAME_GAP:
104
+ return None
105
 
106
+ old = self.history[0]
107
+ H = align(old, gray)
108
+ if H is None:
109
+ blank = np.zeros_like(gray)
110
+ self.snapshot = Snapshot(small, blank, "no alignment", blank, None, False)
111
  return None
112
 
113
+ h, w = gray.shape
114
+ warped = cv2.warpPerspective(old, H, (w, h))
115
+ # pixels warped in from outside the old frame have no data; trim their edges
116
+ valid = cv2.warpPerspective(np.full_like(old, 255), H, (w, h))
117
+ valid = cv2.erode(valid, KERNEL, iterations=2) > 0
118
+
119
+ diff = cv2.absdiff(gray, warped)
120
+ # auto-exposure shifts the whole image while panning; remove that offset
121
+ diff = cv2.subtract(diff, int(np.median(diff[valid])))
122
+ diff[~valid] = 0
123
+
124
+ _, mask = cv2.threshold(diff, self.diff_threshold, 255, cv2.THRESH_BINARY)
125
+ mask = clean(mask)
126
+ if mask.mean() > MAX_MOTION_FRAC * 255:
127
+ mask[:] = 0 # everything "moving" means the alignment was off
128
+
129
+ self.snapshot, target = pick(
130
+ small, diff, "aligned difference", mask, scale, self.min_blob_frac
131
+ )
132
+ return target
133
+
134
+
135
+ def debug_image(snap: Snapshot | None) -> np.ndarray | None:
136
+ """Render a snapshot's frame above its raw and cleaned masks."""
137
+ if snap is None:
138
+ return None
139
+
140
+ frame = upscale(snap.frame, DEBUG_WIDTH, cv2.INTER_LINEAR)
141
+ if snap.box is not None:
142
+ f = DEBUG_WIDTH / snap.frame.shape[1]
143
+ x, y, w, h = (round(v * f) for v in snap.box)
144
+ color = BLOB_COLOR if snap.ok else SMALL_BLOB_COLOR
145
+ cv2.rectangle(frame, (x, y), (x + w, y + h), color, 3)
146
+
147
+ # during warmup there is no cleaned mask yet
148
+ mask = np.zeros_like(snap.raw) if snap.mask is None else snap.mask
149
+ masks = [
150
+ label(upscale(cv2.cvtColor(m, cv2.COLOR_GRAY2BGR), DEBUG_WIDTH // 2), text)
151
+ for text, m in ((snap.raw_label, snap.raw), ("cleaned mask", mask))
152
+ ]
153
+ return np.vstack([label(frame, "camera"), np.hstack(masks)])
154
+
155
+
156
+ def shrink(frame: np.ndarray) -> tuple[np.ndarray, float]:
157
+ """Downscale and blur a frame; return it with the factor back to full res."""
158
+ scale = frame.shape[1] / WIDTH
159
+ small = cv2.resize(
160
+ frame, None, fx=1 / scale, fy=1 / scale, interpolation=cv2.INTER_AREA
161
+ )
162
+ return cv2.GaussianBlur(small, (5, 5), 0), scale
163
+
164
+
165
+ def clean(mask: np.ndarray) -> np.ndarray:
166
+ """Drop speckle, then join fragments of the same object."""
167
+ mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, KERNEL)
168
+ return cv2.morphologyEx(mask, cv2.MORPH_CLOSE, KERNEL, iterations=2)
169
+
170
+
171
+ def pick(
172
+ small: np.ndarray,
173
+ raw: np.ndarray,
174
+ raw_label: str,
175
+ mask: np.ndarray,
176
+ scale: float,
177
+ min_blob_frac: float,
178
+ ) -> tuple[Snapshot, tuple[float, float] | None]:
179
+ """Choose the largest blob in a cleaned mask; return a snapshot and its target."""
180
+ blob = largest_blob(mask)
181
+ ok = blob is not None and blob[2] >= min_blob_frac * mask.size
182
+ snap = Snapshot(small, raw, raw_label, mask, blob and blob[0], ok)
183
+ if not ok:
184
+ return snap, None
185
+
186
+ x, y = blob[1]
187
+ return snap, (float(x * scale), float(y * scale))
188
+
189
+
190
+ def align(old: np.ndarray, new: np.ndarray) -> np.ndarray | None:
191
+ """Estimate the homography mapping old onto new from tracked features."""
192
+ pts = cv2.goodFeaturesToTrack(old, maxCorners=300, qualityLevel=0.01, minDistance=8)
193
+ if pts is None or len(pts) < MIN_FEATURES:
194
+ return None
195
+
196
+ nxt, status, _ = cv2.calcOpticalFlowPyrLK(old, new, pts, None)
197
+ found = status.ravel() == 1
198
+ if found.sum() < MIN_FEATURES:
199
+ return None
200
+
201
+ # RANSAC ignores points on moving objects, but a bad fit can still slip through
202
+ H, inliers = cv2.findHomography(pts[found], nxt[found], cv2.RANSAC, 2.0)
203
+ if H is None or inliers.mean() < MIN_INLIER_FRAC:
204
+ return None
205
+ return H
206
 
207
 
208
  def upscale(img: np.ndarray, width: int, interp: int = cv2.INTER_NEAREST) -> np.ndarray:
movement_tracker/static/index.html CHANGED
@@ -14,10 +14,15 @@
14
  <h2>Sensitivity</h2>
15
  <p class="legend">Lower values make detection more sensitive.</p>
16
  <div class="slider">
17
- <label for="var_threshold">Change threshold</label>
18
  <input type="range" id="var_threshold" min="4" max="64" step="1">
19
  <output id="var_threshold_out"></output>
20
  </div>
 
 
 
 
 
21
  <div class="slider">
22
  <label for="min_blob_frac">Minimum blob size</label>
23
  <input type="range" id="min_blob_frac" min="0.0001" max="0.02" step="0.0001">
@@ -26,9 +31,10 @@
26
 
27
  <h2>Motion map</h2>
28
  <p class="legend">The camera view boxes the largest moving blob: green if the
29
- robot will turn toward it, red if it is smaller than the minimum blob size. The
30
- raw mask is the background subtraction output; the cleaned mask is what picks
31
- the blob.</p>
 
32
  <div id="state"></div>
33
  <img id="motion" alt="Motion map">
34
  <script src="/static/main.js"></script>
 
14
  <h2>Sensitivity</h2>
15
  <p class="legend">Lower values make detection more sensitive.</p>
16
  <div class="slider">
17
+ <label for="var_threshold">Change threshold (still)</label>
18
  <input type="range" id="var_threshold" min="4" max="64" step="1">
19
  <output id="var_threshold_out"></output>
20
  </div>
21
+ <div class="slider">
22
+ <label for="diff_threshold">Change threshold (scanning)</label>
23
+ <input type="range" id="diff_threshold" min="4" max="64" step="1">
24
+ <output id="diff_threshold_out"></output>
25
+ </div>
26
  <div class="slider">
27
  <label for="min_blob_frac">Minimum blob size</label>
28
  <input type="range" id="min_blob_frac" min="0.0001" max="0.02" step="0.0001">
 
31
 
32
  <h2>Motion map</h2>
33
  <p class="legend">The camera view boxes the largest moving blob: green if the
34
+ robot will turn toward it, red if it is smaller than the minimum blob size. While
35
+ still, the raw mask is the background subtraction output; while scanning, it is
36
+ the difference between frames after cancelling out the head's own motion. The
37
+ cleaned mask is what picks the blob.</p>
38
  <div id="state"></div>
39
  <img id="motion" alt="Motion map">
40
  <script src="/static/main.js"></script>
movement_tracker/static/main.js CHANGED
@@ -18,6 +18,7 @@ setInterval(async () => {
18
  // sensitivity sliders: show values while dragging, send them on release
19
  const sliders = {
20
  var_threshold: (v) => `${v}`,
 
21
  min_blob_frac: (v) => `${(v * 100).toFixed(2)}% of image`,
22
  };
23
  const input = (k) => document.getElementById(k);
 
18
  // sensitivity sliders: show values while dragging, send them on release
19
  const sliders = {
20
  var_threshold: (v) => `${v}`,
21
+ diff_threshold: (v) => `${v}`,
22
  min_blob_frac: (v) => `${(v * 100).toFixed(2)}% of image`,
23
  };
24
  const input = (k) => document.getElementById(k);
movement_tracker/static/style.css CHANGED
@@ -30,7 +30,7 @@ body {
30
  }
31
 
32
  .slider label {
33
- width: 10em;
34
  }
35
 
36
  .slider input {
 
30
  }
31
 
32
  .slider label {
33
+ width: 14em;
34
  }
35
 
36
  .slider input {
tests/test_motion.py CHANGED
@@ -1,6 +1,7 @@
 
1
  import numpy as np
2
 
3
- from movement_tracker.motion import MotionDetector
4
 
5
  rng = np.random.default_rng(0)
6
  BG = rng.integers(60, 120, (720, 1280, 3), dtype=np.uint8)
@@ -49,3 +50,34 @@ def test_detects_object_slightly_darker_than_background():
49
  frame[200:500, 400:700] = 110
50
  x, y = d.detect(frame)
51
  assert abs(x - 550) < 10 and abs(y - 350) < 10
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
+ import cv2
2
  import numpy as np
3
 
4
+ from movement_tracker.motion import MotionDetector, ScanDetector
5
 
6
  rng = np.random.default_rng(0)
7
  BG = rng.integers(60, 120, (720, 1280, 3), dtype=np.uint8)
 
50
  frame[200:500, 400:700] = 110
51
  x, y = d.detect(frame)
52
  assert abs(x - 550) < 10 and abs(y - 350) < 10
53
+
54
+
55
+ def textured_world(width: int) -> np.ndarray:
56
+ """A scene with enough texture for the scan detector to track features."""
57
+ rng = np.random.default_rng(1)
58
+ world = rng.integers(0, 255, (720, width, 3), dtype=np.uint8)
59
+ world = cv2.GaussianBlur(world, (0, 0), 6)
60
+ return cv2.normalize(world, None, 30, 220, cv2.NORM_MINMAX)
61
+
62
+
63
+ def pan(world: np.ndarray, frames: int, obj_x=None) -> list:
64
+ """Detections while the camera pans right 6px per frame across the world."""
65
+ d, hits = ScanDetector(), []
66
+ for i in range(frames):
67
+ w = world.copy()
68
+ if obj_x is not None:
69
+ x = obj_x(i)
70
+ w[250:480, x : x + 90] = 240
71
+ hits.append(d.detect(w[:, i * 6 : i * 6 + 1280]))
72
+ return hits
73
+
74
+
75
+ def test_scan_ignores_camera_motion():
76
+ assert not any(pan(textured_world(1800), 60))
77
+
78
+
79
+ def test_scan_finds_moving_object():
80
+ # object walks right 8px/frame; in view coordinates its center is 545 + 2i
81
+ hits = pan(textured_world(1800), 40, obj_x=lambda i: 500 + 8 * i)
82
+ x, y = hits[-1]
83
+ assert abs(x - (545 + 2 * 39)) < 60 and abs(y - 365) < 30