Spaces:
Running
Running
feat: scan continuously while detecting motion
Browse filesReplace 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 +32 -23
- movement_tracker/main.py +85 -47
- movement_tracker/motion.py +156 -39
- movement_tracker/static/index.html +10 -4
- movement_tracker/static/main.js +1 -0
- movement_tracker/static/style.css +1 -1
- tests/test_motion.py +33 -1
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
|
| 19 |
-
background is relearned, since the robot's own motion changes the
|
| 20 |
-
|
| 21 |
-
|
| 22 |
-
|
| 23 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 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
|
|
|
|
| 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
|
| 37 |
-
size, so detection can be tuned live while watching the
|
| 38 |
-
until the app restarts; the defaults are the constants
|
| 39 |
-
`movement_tracker/motion.py`:
|
| 40 |
|
| 41 |
-
| Constant
|
| 42 |
-
|-----------------|------------------------------------------------------------------|
|
| 43 |
-
| `VAR_THRESHOLD`
|
| 44 |
-
| `MIN_BLOB_FRAC`
|
| 45 |
-
| `WARMUP`
|
|
|
|
|
|
|
| 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 |
-
| `
|
| 54 |
-
| `
|
|
|
|
| 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
|
| 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=
|
|
|
|
|
|
|
| 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.
|
| 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 |
-
|
| 78 |
-
|
| 79 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 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 |
-
|
| 90 |
-
|
| 91 |
-
|
| 92 |
-
|
| 93 |
-
|
| 94 |
-
|
| 95 |
-
|
| 96 |
-
|
| 97 |
-
|
| 98 |
-
|
| 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
|
| 113 |
-
|
| 114 |
-
|
| 115 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 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 |
-
|
| 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;
|
| 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 |
-
#
|
| 24 |
-
|
| 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
|
| 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 |
-
|
| 57 |
-
|
| 58 |
-
|
|
|
|
| 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 |
-
|
| 67 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 68 |
|
| 69 |
-
|
| 70 |
-
|
| 71 |
-
if
|
|
|
|
|
|
|
| 72 |
return None
|
| 73 |
|
| 74 |
-
|
| 75 |
-
|
| 76 |
-
|
| 77 |
-
|
| 78 |
-
|
| 79 |
-
|
| 80 |
-
|
| 81 |
-
|
| 82 |
-
|
| 83 |
-
|
| 84 |
-
|
| 85 |
-
|
| 86 |
-
|
| 87 |
-
|
| 88 |
-
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| 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.
|
| 30 |
-
raw mask is the background subtraction output;
|
| 31 |
-
the
|
|
|
|
| 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:
|
| 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
|