From 51c259e5237e665d9818131f7b896b27de21ebed Mon Sep 17 00:00:00 2001 From: ykcheungab Date: Mon, 30 Mar 2026 19:56:22 +0800 Subject: [PATCH] add optical flow debug script --- test_3Dflow.py | 449 +++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 449 insertions(+) create mode 100644 test_3Dflow.py diff --git a/test_3Dflow.py b/test_3Dflow.py new file mode 100644 index 0000000..53bc26f --- /dev/null +++ b/test_3Dflow.py @@ -0,0 +1,449 @@ + +""" +Prototype tactile 2.5D motion analysis on top of the existing dense flow output. + +This script compares several motion representations derived from optical flow: +1. Raw flow arrows / magnitude +2. Radial and tangential flow around an estimated contact center +3. Differential flow features: divergence, curl, strain magnitude +4. Pseudo-height reconstructed from divergence (qualitative only) +""" +import os +import sys +import argparse +from pathlib import Path +import time + +os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0" + +import cv2 +import numpy as np + +WINDOW_TITLE = "Tactile 2.5D Motion Prototype" + +_local_src = os.path.join(os.path.dirname(__file__), "src") +if _local_src not in sys.path: + sys.path.insert(0, _local_src) + +import orisys + + +def parse_args(): + parser = argparse.ArgumentParser() + parser.add_argument("--video", "-v", type=str, default="0", help="Camera ID / video file path") + parser.add_argument("--config", "-c", type=str, default="./config/ddjx01.json", help="Configuration file path") + parser.add_argument("--cal", "-cal", type=str, default="./config/ddjx01.npy", help="Calibration file path") + parser.add_argument("--verbose", action="store_true", help="Print detailed sensor logs") + parser.add_argument("--no-display", action="store_true", help="Disable OpenCV windows") + parser.add_argument("--save", type=str, default="", help="Optional output video path for the comparison panel") + parser.add_argument("--max-frames", type=int, default=0, help="Stop after N frames (0 = unlimited)") + parser.add_argument("--grid-spacing", type=int, default=20, help="Arrow visualization grid spacing") + parser.add_argument("--flow-threshold", type=float, default=2.0, help="Arrow visualization magnitude threshold") + parser.add_argument("--print-every", type=int, default=1, help="Print scalar metrics every N frames") + parser.add_argument("--panel-size", type=int, default=320, help="Width/height of each panel tile") + parser.add_argument("--plane-width-mm", type=float, default=20.0, help="Physical tactile imaging plane width in mm") + parser.add_argument("--plane-height-mm", type=float, default=20.0, help="Physical tactile imaging plane height in mm") + parser.add_argument("--camera-distance-mm", type=float, default=10.0, help="Distance from camera plane to imaging plane in mm") + parser.add_argument("--display-max-width", type=int, default=1800, help="Maximum preview window width in pixels") + parser.add_argument("--display-max-height", type=int, default=1000, help="Maximum preview window height in pixels") + return parser.parse_args() + + +def create_sensor(args): + try: + video_input = int(args.video) + except ValueError: + video_input = args.video + + return orisys.Sensor( + video_input, + config_name=args.config, + cal_path=args.cal, + verbose=args.verbose, + ) + + +def ensure_bgr(image): + if image.ndim == 2: + return cv2.cvtColor(image, cv2.COLOR_GRAY2BGR) + return image + + +def resize_panel(image, panel_size): + return cv2.resize(ensure_bgr(image), (panel_size, panel_size), interpolation=cv2.INTER_LINEAR) + + +def fit_display_image(image, max_width, max_height): + h, w = image.shape[:2] + scale = min(max_width / max(float(w), 1.0), max_height / max(float(h), 1.0), 1.0) + if scale >= 0.999: + return image + new_size = (max(1, int(w * scale)), max(1, int(h * scale))) + return cv2.resize(image, new_size, interpolation=cv2.INTER_AREA) + + +def signed_heatmap(field, colormap=cv2.COLORMAP_TURBO): + max_abs = float(np.max(np.abs(field))) + if max_abs <= 1e-9: + norm = np.full(field.shape, 127, dtype=np.uint8) + else: + norm = np.clip((field / max_abs + 1.0) * 127.5, 0, 255).astype(np.uint8) + return cv2.applyColorMap(norm, colormap) + + +def magnitude_heatmap(field, colormap=cv2.COLORMAP_TURBO): + max_val = float(np.max(field)) + if max_val <= 1e-9: + norm = np.zeros(field.shape, dtype=np.uint8) + else: + norm = np.clip(field / max_val * 255.0, 0, 255).astype(np.uint8) + return cv2.applyColorMap(norm, colormap) + + +def add_title(image, title): + panel = image.copy() + cv2.rectangle(panel, (0, 0), (panel.shape[1], 28), (0, 0, 0), -1) + cv2.putText(panel, title, (8, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 2, cv2.LINE_AA) + return panel + + +def make_panel(image, title, panel_size): + return add_title(resize_panel(image, panel_size), title) + + +def stack_panels(panels, columns=4): + filler = np.zeros_like(panels[0]) + rows = [] + for idx in range(0, len(panels), columns): + row = panels[idx:idx + columns] + if len(row) < columns: + row = row + [filler] * (columns - len(row)) + rows.append(cv2.hconcat(row)) + return cv2.vconcat(rows) + + +def estimate_contact_center(divergence, flow_magnitude): + positive = np.maximum(divergence, 0.0) + weights = positive * (flow_magnitude + 1e-6) + total = float(weights.sum()) + h, w = divergence.shape + + if total <= 1e-9: + return (w // 2, h // 2) + + ys, xs = np.indices(divergence.shape, dtype=np.float32) + cx = int(np.clip((weights * xs).sum() / total, 0, w - 1)) + cy = int(np.clip((weights * ys).sum() / total, 0, h - 1)) + return (cx, cy) + + +def prepare_support_mask(support_map): + if support_map is None: + return None + + support = np.abs(support_map).astype(np.float32) + max_support = float(np.max(support)) + if max_support > 0.0: + support /= max_support + support = (support > 0.05).astype(np.float32) + if float(np.mean(support)) < 0.002: + return None + return support + + +def decompose_radial_tangential(flow, center): + h, w = flow.shape[:2] + ys, xs = np.indices((h, w), dtype=np.float32) + cx, cy = center + + rx = xs - float(cx) + ry = ys - float(cy) + r_norm = np.sqrt(rx * rx + ry * ry) + r_norm = np.where(r_norm < 1e-6, 1.0, r_norm) + + rhat_x = rx / r_norm + rhat_y = ry / r_norm + that_x = -rhat_y + that_y = rhat_x + + u = flow[:, :, 0] + v = flow[:, :, 1] + radial = u * rhat_x + v * rhat_y + tangential = u * that_x + v * that_y + return radial, tangential + + +def compute_flow_jacobian_features(flow): + u = flow[:, :, 0] + v = flow[:, :, 1] + + du_dy, du_dx = np.gradient(u) + dv_dy, dv_dx = np.gradient(v) + + divergence = du_dx + dv_dy + curl = dv_dx - du_dy + strain_xx = du_dx + strain_yy = dv_dy + strain_xy = 0.5 * (du_dy + dv_dx) + strain_mag = np.sqrt(strain_xx * strain_xx + strain_yy * strain_yy + 2.0 * strain_xy * strain_xy) + + return { + "divergence": divergence, + "curl": curl, + "strain_mag": strain_mag, + } + + +def reconstruct_pseudo_height(divergence, flow_magnitude=None, support_map=None): + rhs = divergence.astype(np.float32).copy() + + # No-contact guard: tiny flow noise becomes large low-frequency drift after Poisson solve. + if flow_magnitude is not None: + mean_mag = float(np.mean(flow_magnitude)) + max_mag = float(np.max(flow_magnitude)) + if mean_mag < 0.03 and max_mag < 0.15: + return np.zeros_like(rhs, dtype=np.float32) + + support = prepare_support_mask(support_map) + if support_map is not None: + if support is None: + return np.zeros_like(rhs, dtype=np.float32) + rhs *= support + + noise_floor = max(float(np.std(rhs)) * 1.5, 1e-4) + rhs[np.abs(rhs) < noise_floor] = 0.0 + if not np.any(rhs): + return np.zeros_like(rhs, dtype=np.float32) + + # Zero-pad before FFT solve to reduce periodic wrap-around ghosts, then crop back. + h, w = rhs.shape + pad_y = max(8, h // 2) + pad_x = max(8, w // 2) + rhs_padded = np.pad(rhs, ((pad_y, pad_y), (pad_x, pad_x)), mode="constant") + rhs_padded = rhs_padded - float(rhs_padded.mean()) + + hp, wp = rhs_padded.shape + fy = np.fft.fftfreq(hp).reshape(-1, 1) + fx = np.fft.fftfreq(wp).reshape(1, -1) + denom = (2.0 * np.pi * fx) ** 2 + (2.0 * np.pi * fy) ** 2 + denom[0, 0] = 1.0 + + rhs_hat = np.fft.fft2(rhs_padded) + height_hat = -rhs_hat / denom + height_hat[0, 0] = 0.0 + height_full = np.real(np.fft.ifft2(height_hat)).astype(np.float32) + height = height_full[pad_y:pad_y + h, pad_x:pad_x + w] + height = cv2.GaussianBlur(height, (0, 0), 1.0) + return height + + +def build_visualization(img, flow, center, features, radial, tangential, pseudo_height, depth_map, args): + arrows = orisys.util.draw_arrows( + img, + flow, + threshold=args.flow_threshold, + grid_spacing=args.grid_spacing, + arrow_scale=1.0, + ) + + contact_view = ensure_bgr(img) + cv2.circle(contact_view, center, 6, (0, 255, 0), -1) + cv2.circle(contact_view, center, 12, (0, 255, 0), 2) + + panel_images = [ + (contact_view, "Image / center"), + (arrows, "Raw flow"), + (signed_heatmap(radial), "Radial flow"), + (signed_heatmap(tangential), "Tangential flow"), + (signed_heatmap(features["divergence"]), "Divergence"), + (signed_heatmap(features["curl"]), "Curl"), + (magnitude_heatmap(features["strain_mag"]), "Strain mag"), + (signed_heatmap(depth_map), "Depth map"), + (signed_heatmap(pseudo_height), "Pseudo height"), + ] + panels = [make_panel(image, title, args.panel_size) for image, title in panel_images] + return stack_panels(panels) + + +def make_writer(path, frame_size, fps): + if not path: + return None + parent = Path(path).parent + if parent and not parent.exists(): + parent.mkdir(parents=True, exist_ok=True) + return cv2.VideoWriter(path, cv2.VideoWriter_fourcc(*"mp4v"), fps, frame_size) + + +def approximate_raw_flow_to_mm(flow_px, plane_width_mm, plane_height_mm, camera_distance_mm, image_shape): + h, w = image_shape[:2] + fx = camera_distance_mm * w / max(plane_width_mm, 1e-6) + fy = camera_distance_mm * h / max(plane_height_mm, 1e-6) + flow_mm = flow_px.copy().astype(np.float32) + flow_mm[:, :, 0] *= camera_distance_mm / max(fx, 1e-6) + flow_mm[:, :, 1] *= camera_distance_mm / max(fy, 1e-6) + return flow_mm + + +def compute_metrics(flow_magnitude, raw_flow_mm, radial, tangential, features, depth_map, pseudo_height): + return { + "flow_mean": float(flow_magnitude.mean()), + "flow_mean_mm": float(np.linalg.norm(raw_flow_mm, axis=2).mean()), + "radial_abs_mean": float(np.mean(np.abs(radial))), + "tangential_abs_mean": float(np.mean(np.abs(tangential))), + "divergence_mean": float(features["divergence"].mean()), + "curl_abs_mean": float(np.mean(np.abs(features["curl"]))), + "strain_mean": float(features["strain_mag"].mean()), + "depth_peak": float(np.max(np.abs(depth_map))), + "pseudo_height_peak": float(np.max(np.abs(pseudo_height))), + } + + +def format_metrics(frame_count, fps, metrics): + line = ( + f"frame={frame_count:04d} fps={fps:6.2f} " + f"flow_mean={metrics['flow_mean']:.4f} " + f"flow_mean_mm={metrics['flow_mean_mm']:.4f} " + f"radial_abs={metrics['radial_abs_mean']:.4f} " + f"tangential_abs={metrics['tangential_abs_mean']:.4f} " + f"div_mean={metrics['divergence_mean']:.4f} " + f"curl_abs={metrics['curl_abs_mean']:.4f} " + f"strain={metrics['strain_mean']:.4f} " + f"depth_peak={metrics['depth_peak']:.4f} " + f"height_peak={metrics['pseudo_height_peak']:.4f}" + ) + t = metrics.get("_timings") + if t: + line += ( + f" | analyze {t['total']:.2f}ms" + f" (mag:{t['magnitude']:.2f} jac:{t['jacobian']:.2f}" + f" rad_tan:{t['rad_tan']:.2f} pseudo_h:{t['pseudo_h']:.2f}" + f" metrics:{t['metrics']:.2f} vis:{t['visualize']:.2f})" + ) + return line + + +def read_sensor_frame(sensor): + sensor.compute_flow() + sensor.compute_hhd() + sensor.compute_contact() + return sensor.read_info( + sensor.info.FPS, + sensor.info.VRAW, + sensor.info.IMG, + sensor.info.DEPTH, + ) + + +def analyze_frame(img, flow, depth_map, args): + t = time.perf_counter() + + flow_magnitude = np.linalg.norm(flow, axis=2) + t_mag = time.perf_counter() + + features = compute_flow_jacobian_features(flow) + t_jac = time.perf_counter() + + center = estimate_contact_center(features["divergence"], flow_magnitude) + radial, tangential = decompose_radial_tangential(flow, center) + t_decomp = time.perf_counter() + + pseudo_height = reconstruct_pseudo_height( + features["divergence"], + flow_magnitude=flow_magnitude, + support_map=depth_map, + ) + t_height = time.perf_counter() + + raw_flow_mm = approximate_raw_flow_to_mm( + flow, + args.plane_width_mm, + args.plane_height_mm, + args.camera_distance_mm, + img.shape, + ) + metrics = compute_metrics(flow_magnitude, raw_flow_mm, radial, tangential, features, depth_map, pseudo_height) + t_metrics = time.perf_counter() + + canvas = build_visualization( + img, + flow, + center, + features, + radial, + tangential, + pseudo_height, + depth_map, + args, + ) + t_vis = time.perf_counter() + + metrics["_timings"] = { + "magnitude": (t_mag - t) * 1000, + "jacobian": (t_jac - t_mag) * 1000, + "rad_tan": (t_decomp - t_jac) * 1000, + "pseudo_h": (t_height - t_decomp) * 1000, + "metrics": (t_metrics - t_height) * 1000, + "visualize": (t_vis - t_metrics) * 1000, + "total": (t_vis - t) * 1000, + } + return canvas, metrics + + +def main(): + args = parse_args() + sensor = create_sensor(args) + writer = None + frame_count = 0 + print_every = max(args.print_every, 1) + + try: + print("\nPress 'q' to quit, 'r' to reset tracking.") + + while True: + img = sensor.get_img() + if img is None: + print("No more frames returned from sensor.") + break + + fps, flow, img, depth_map = read_sensor_frame(sensor) + canvas, metrics = analyze_frame(img, flow, depth_map, args) + + if writer is None and args.save: + writer = make_writer(args.save, (canvas.shape[1], canvas.shape[0]), max(float(fps), 1.0)) + + if writer is not None: + writer.write(canvas) + + frame_count += 1 + + if frame_count % print_every == 0: + print(format_metrics(frame_count, fps, metrics)) + + if not args.no_display: + display_canvas = fit_display_image( + canvas, + max_width=args.display_max_width, + max_height=args.display_max_height, + ) + cv2.imshow(WINDOW_TITLE, display_canvas) + key = cv2.waitKey(1) & 0xFF + if key == ord("q"): + print("\nExiting...") + break + if key == ord("r"): + sensor.reset() + print("Sensor reset.") + + if args.max_frames > 0 and frame_count >= args.max_frames: + print(f"Reached max_frames={args.max_frames}.") + break + finally: + if writer is not None: + writer.release() + sensor.disconnect() + if not args.no_display: + cv2.destroyAllWindows() + + +if __name__ == "__main__": + main()