#!/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())