#!/usr/bin/env python3
"""sparsemap — live sparse feature map from your webcam.
The main window shows the webcam feed with sparse tracked feature points
(Good-Features-to-Track) and Lucas-Kanade optical-flow vectors. A second
window draws a SLAM-style top-down map: the camera trajectory plus the
sparse landmarks implied by those tracks (planar-motion assumption).
Keys:
r re-detect / reset tracks
f toggle flow vectors
p toggle feature points
m toggle map window
s toggle stats overlay
space pause / resume
q,Esc quit
"""
import argparse
import math
import sys
import time
from collections import deque
import cv2
import numpy as np
# --------------------------------------------------------------------------
# sparse LK tracker with forward-backward check
# --------------------------------------------------------------------------
class SparseTracker:
"""Tracks a sparse set of points frame-to-frame with Lucas-Kanade.
Keeps per-track ids/ages, prunes bad tracks with a forward-backward
consistency check, and re-detects new corners when the pool runs low.
"""
def __init__(self, max_corners=200):
self.max_corners = max_corners
self.lk = dict(
winSize=(21, 21),
maxLevel=3,
criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01),
)
self.prev_gray = None
self.pts = np.zeros((0, 1, 2), np.float32) # (N,1,2)
self.ids = np.zeros(0, np.int64)
self.ages = np.zeros(0, np.int32)
self.next_id = 0
self.fb_thresh = 1.5 # px, forward-backward consistency
self.min_dist = 12 # px, min spacing between corners
self.redetect_ratio = 0.35 # re-detect when alive < ratio * max
def reset(self):
self.prev_gray = None
self.pts = np.zeros((0, 1, 2), np.float32)
self.ids = np.zeros(0, np.int64)
self.ages = np.zeros(0, np.int32)
def _detect_new(self, gray):
"""Detect fresh corners, avoiding spots already tracked."""
h, w = gray.shape
mask = np.full((h, w), 255, np.uint8)
if self.pts.size:
for p in self.pts[:, 0]:
cv2.circle(mask, (int(p[0]), int(p[1])), self.min_dist, 0, -1)
return cv2.goodFeaturesToTrack(
gray, maxCorners=self.max_corners, qualityLevel=0.01,
minDistance=self.min_dist, blockSize=7, mask=mask)
def process(self, gray):
"""Advance one frame. Returns (prev_pts, curr_pts, accepted_mask)."""
if self.prev_gray is None or self.pts.size == 0:
self.prev_gray = gray
fresh = self._detect_new(gray)
if fresh is not None:
n = len(fresh)
self.pts = fresh
self.ids = np.arange(self.next_id, self.next_id + n, dtype=np.int64)
self.next_id += n
self.ages = np.zeros(n, np.int32)
return (self.pts.copy(), self.pts.copy(),
np.zeros(len(self.pts), bool))
# forward track
curr, st, _ = cv2.calcOpticalFlowPyrLK(self.prev_gray, gray, self.pts, None, **self.lk)
st = (st[:, 0] == 1) if st is not None else np.zeros(len(self.pts), bool)
# backward check (catches drift / bad matches)
if st.any():
idx = np.nonzero(st)[0]
back, st2, _ = cv2.calcOpticalFlowPyrLK(gray, self.prev_gray, curr[idx], None, **self.lk)
err = np.linalg.norm(back[:, 0] - self.pts[idx, 0], axis=1)
st[idx] = (st2[:, 0] == 1) & (err < self.fb_thresh)
prev_pts = self.pts[st]
curr_pts = curr[st]
self.ids = self.ids[st]
self.ages = self.ages[st] + 1
self.pts = curr_pts
self.prev_gray = gray
st = st[st] # keep the mask aligned with the survivor rows (all True)
# top-up the pool when it gets thin
if len(self.pts) < self.redetect_ratio * self.max_corners:
fresh = self._detect_new(gray)
if fresh is not None:
room = max(0, 2 * self.max_corners - len(self.pts))
fresh = fresh[:room]
n = len(fresh)
self.pts = np.vstack([self.pts, fresh])
self.ids = np.concatenate(
[self.ids, np.arange(self.next_id, self.next_id + n, dtype=np.int64)])
self.next_id += n
self.ages = np.concatenate([self.ages, np.zeros(n, np.int32)])
pad = fresh.copy() # zero-motion placeholder: no flow line drawn
prev_pts = np.vstack([prev_pts, pad])
curr_pts = np.vstack([curr_pts, fresh])
st = np.concatenate([st, np.zeros(n, bool)])
return prev_pts, curr_pts, st
# --------------------------------------------------------------------------
# SLAM-style pose + landmark accumulation (planar assumption)
# --------------------------------------------------------------------------
class PoseMapper:
"""Chains per-frame rigid 2D transforms into a camera pose (x, y, theta)
and accumulates sparse landmarks: each live track is projected onto a
world plane and frozen in place when its track dies."""
def __init__(self, min_inliers=8):
self.min_inliers = min_inliers
self.x = 0.0
self.y = 0.0
self.theta = 0.0
self.inliers = 0
self.landmarks = {} # id -> (wx, wy, age)
self.trail = deque(maxlen=2400)
self.trail.append((0.0, 0.0))
def _project(self, img_pt, frame_w, frame_h, scale):
u = (img_pt[0] - frame_w / 2.0) * scale
v = (img_pt[1] - frame_h / 2.0) * scale
c, s = math.cos(self.theta), math.sin(self.theta)
return (self.x + c * u - s * v, self.y + s * u + c * v)
def update(self, prev_pts, curr_pts, ok, tracker, frame_w, frame_h, scale):
"""Estimate pose delta from the previous frame and refresh landmarks.
prev_pts/curr_pts cover ALL live tracks; ok marks rows with a valid
previous position (used for the RANSAC pose estimate)."""
n = int(ok.sum())
if n >= 8:
M, inl = cv2.estimateAffinePartial2D(
prev_pts[ok], curr_pts[ok], method=cv2.RANSAC, ransacReprojThreshold=2.0)
self.inliers = int(inl.sum()) if inl is not None else 0
if M is not None and self.inliers >= self.min_inliers:
dtheta = math.atan2(M[1, 0], M[0, 0])
t = M[:2, 2]
self.theta += dtheta
c, s = math.cos(self.theta), math.sin(self.theta)
self.x -= c * t[0] - s * t[1]
self.y -= s * t[0] + c * t[1]
self.trail.append((self.x, self.y))
else:
self.inliers = 0
# live landmarks follow the current pose estimate
for i in range(len(tracker.ids)):
wx, wy = self._project(curr_pts[i, 0], frame_w, frame_h, scale)
self.landmarks[int(tracker.ids[i])] = (wx, wy, int(tracker.ages[i]))
while len(self.landmarks) > 6000:
self.landmarks.pop(next(iter(self.landmarks)))
# --------------------------------------------------------------------------
# drawing
# --------------------------------------------------------------------------
def age_color(age, hue0=40, step=2, span=180):
h = (hue0 + int(age) * step) % span
px = np.uint8([[[h, 255, 255]]])
b, g, r = cv2.cvtColor(px, cv2.COLOR_HSV2BGR)[0, 0]
return int(b), int(g), int(r)
def draw_overlay(frame, flow_prev, flow_curr, args, opts):
fps, tracker, pose, stats_on, flow_on, points_on, paused = opts
if flow_on:
for p, q in zip(flow_prev[:, 0], flow_curr[:, 0]):
cv2.line(frame, (int(p[0]), int(p[1])), (int(q[0]), int(q[1])),
(255, 255, 0), 1, cv2.LINE_AA)
if points_on:
for i, q in enumerate(tracker.pts[:, 0]):
age = int(tracker.ages[i])
cv2.circle(frame, (int(q[0]), int(q[1])), 3, age_color(age), -1, cv2.LINE_AA)
if stats_on:
h, w = frame.shape[:2]
lines = [
f"{fps:5.1f} fps | tracks {len(tracker.pts):3d}/{args.features} | "
f"landmarks {len(pose.landmarks):4d} | inliers {pose.inliers}",
f"pose x {pose.x:8.1f} y {pose.y:8.1f} theta {math.degrees(pose.theta) % 360:6.1f} deg",
"r reset | f flow | p points | m map | s stats | space pause | q quit",
]
if paused:
lines.insert(0, "PAUSED")
y = 26
for line in lines:
cv2.putText(frame, line, (10, y), cv2.FONT_HERSHEY_SIMPLEX,
0.55, (0, 0, 0), 3, cv2.LINE_AA)
cv2.putText(frame, line, (10, y), cv2.FONT_HERSHEY_SIMPLEX,
0.55, (80, 255, 80), 1, cv2.LINE_AA)
y += 22
return frame
def draw_map(size, pose, stats_on):
canvas = np.full((size, size, 3), (28, 28, 28), np.uint8)
pts = list(pose.trail) + [(wx, wy) for wx, wy, _ in pose.landmarks.values()]
pad = 34
if len(pts) >= 2:
xs = np.array([p[0] for p in pts], np.float64)
ys = np.array([p[1] for p in pts], np.float64)
cx, cy = (xs.min() + xs.max()) / 2, (ys.min() + ys.max()) / 2
rng = max(xs.max() - xs.min(), ys.max() - ys.min(), 1e-6)
scale = min((size - 2 * pad) / rng, 4.0)
else:
cx, cy, scale = 0.0, 0.0, 1.0
def to_map(x, y):
return (int(size / 2 + (x - cx) * scale), int(size / 2 + (cy - y) * scale))
# world origin (where the camera started)
ox, oy = to_map(0, 0)
cv2.line(canvas, (ox - 5, oy), (ox + 5, oy), (160, 120, 90), 1, cv2.LINE_AA)
cv2.line(canvas, (ox, oy - 5), (ox, oy + 5), (160, 120, 90), 1, cv2.LINE_AA)
# landmarks, colored by track age
for wx, wy, age in pose.landmarks.values():
mx, my = to_map(wx, wy)
cv2.circle(canvas, (mx, my), 2, age_color(age), -1, cv2.LINE_AA)
# camera trajectory
if len(pose.trail) >= 2:
poly = np.array([to_map(x, y) for x, y in pose.trail], np.int32)
cv2.polylines(canvas, [poly], False, (75, 180, 60), 2, cv2.LINE_AA)
# camera marker (heading triangle)
mx, my = to_map(pose.x, pose.y)
a = pose.theta
d = np.array([math.cos(a), -math.sin(a)]) # map Y is flipped
p = np.array([mx, my], np.float64)
tip = p + d * 11
b1 = p - d * 7 + np.array([-d[1], d[0]]) * 6
b2 = p - d * 7 - np.array([-d[1], d[0]]) * 6
cv2.fillPoly(canvas, [np.array([tip, b1, b2], np.int32)], (60, 60, 230), cv2.LINE_AA)
if stats_on:
cv2.putText(canvas, f"map | {len(pose.landmarks)} landmarks | trail {len(pose.trail)}",
(8, size - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 0, 0), 2, cv2.LINE_AA)
cv2.putText(canvas, f"map | {len(pose.landmarks)} landmarks | trail {len(pose.trail)}",
(8, size - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (200, 200, 200), 1, cv2.LINE_AA)
return canvas
# --------------------------------------------------------------------------
# main
# --------------------------------------------------------------------------
def parse_args():
ap = argparse.ArgumentParser(description="Live sparse feature map from your webcam.")
ap.add_argument("--camera", type=int, default=0, help="webcam index (default 0)")
ap.add_argument("--width", type=int, default=1280, help="requested camera width")
ap.add_argument("--height", type=int, default=720, help="requested camera height")
ap.add_argument("--features", type=int, default=200, help="max tracked corners (default 200)")
ap.add_argument("--scale", type=float, default=1.0,
help="processing downscale factor, e.g. 0.5 for speed (default 1.0)")
ap.add_argument("--map-size", type=int, default=560, help="map window pixels (default 560)")
ap.add_argument("--no-map", action="store_true", help="start with the map window hidden")
return ap.parse_args()
def main():
args = parse_args()
cap = cv2.VideoCapture(args.camera)
if not cap.isOpened():
print(f"error: could not open camera {args.camera}", file=sys.stderr)
return 1
cap.set(cv2.CAP_PROP_FRAME_WIDTH, args.width)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, args.height)
tracker = SparseTracker(max_corners=args.features)
pose = PoseMapper()
flow_on, points_on, stats_on = True, True, True
show_map = not args.no_map
paused = False
if show_map:
cv2.namedWindow("map", cv2.WINDOW_NORMAL)
cv2.resizeWindow("map", args.map_size, args.map_size)
cv2.namedWindow("sparsemap", cv2.WINDOW_NORMAL)
cv2.resizeWindow("sparsemap", 960, 540)
fps = 0.0
last = time.perf_counter()
ret, frame = cap.read()
if not ret:
print("error: camera opened but no frames arrived", file=sys.stderr)
return 1
while True:
now = time.perf_counter()
fps = 0.9 * fps + 0.1 * (1.0 / max(now - last, 1e-6))
last = now
if not paused:
ret, frame = cap.read()
if not ret:
print("warning: lost camera stream, stopping", file=sys.stderr)
break
proc = frame
if args.scale != 1.0:
proc = cv2.resize(frame, None, fx=args.scale, fy=args.scale,
interpolation=cv2.INTER_AREA)
gray = cv2.cvtColor(proc, cv2.COLOR_BGR2GRAY)
if paused:
flow_prev = flow_curr = tracker.pts
ok = np.zeros(len(tracker.pts), bool)
else:
prev_pts, curr_pts, ok = tracker.process(gray)
pose.update(prev_pts, curr_pts, ok, tracker,
gray.shape[1], gray.shape[0], args.scale)
flow_prev, flow_curr = prev_pts[ok], curr_pts[ok]
disp = draw_overlay(proc.copy(), flow_prev, flow_curr, args,
(fps, tracker, pose, stats_on, flow_on, points_on, paused))
cv2.imshow("sparsemap", disp)
if show_map:
cv2.imshow("map", draw_map(args.map_size, pose, stats_on))
key = cv2.waitKey(1) & 0xFF
if key in (ord("q"), 27):
break
elif key == ord("r"):
tracker.reset()
pose.landmarks.clear()
elif key == ord("f"):
flow_on = not flow_on
elif key == ord("p"):
points_on = not points_on
elif key == ord("m"):
show_map = not show_map
if show_map:
cv2.namedWindow("map", cv2.WINDOW_NORMAL)
cv2.resizeWindow("map", args.map_size, args.map_size)
else:
cv2.destroyWindow("map")
elif key == ord("s"):
stats_on = not stats_on
elif key == 32:
paused = not paused
cap.release()
cv2.destroyAllWindows()
return 0
if __name__ == "__main__":
sys.exit(main())