Joint solve lie

scripts/copy_images_in_range.py

@@ -0,0 +1,115 @@
+#!/usr/bin/env python3
+from __future__ import annotations
+
+import argparse
+import os
+import re
+import shutil
+import sys
+from pathlib import Path
+
+SUPPORTED_EXTENSIONS = (".png", ".jpg", ".jpeg", ".bmp", ".gif", ".tiff")
+CAMERA_DIRS = ("main_bot_left", "main_bot_right")
+
+
+def parse_args() -> argparse.Namespace:
+    parser = argparse.ArgumentParser(
+        description=(
+            "Copy timestamped images from main_bot_left/main_bot_right into a "
+            "new folder, keeping only images inside an inclusive timestamp range."
+        )
+    )
+    parser.add_argument("source", help="Source folder containing camera folders")
+    parser.add_argument("destination", help="Destination folder to copy into")
+    parser.add_argument("start_time", type=float, help="Minimum timestamp to copy")
+    parser.add_argument("end_time", type=float, help="Maximum timestamp to copy")
+    parser.add_argument(
+        "--flat",
+        action="store_true",
+        help="Copy directly into destination instead of preserving camera folders",
+    )
+    return parser.parse_args()
+
+
+def extract_timestamp(filename: str) -> float:
+    match = re.search(r"\d+(\.\d+)?", filename)
+    if not match:
+        raise ValueError(f"No timestamp found in filename: {filename}")
+    return float(match.group())
+
+
+def iter_image_paths(folder: Path):
+    for path in sorted(folder.iterdir()):
+        if path.is_file() and path.name.lower().endswith(SUPPORTED_EXTENSIONS):
+            yield path
+
+
+def copy_camera_folder(
+    source_dir: Path,
+    destination_dir: Path,
+    start_time: float,
+    end_time: float,
+) -> tuple[int, int]:
+    destination_dir.mkdir(parents=True, exist_ok=True)
+    copied = 0
+    skipped = 0
+
+    for image_path in iter_image_paths(source_dir):
+        try:
+            timestamp = extract_timestamp(image_path.name)
+        except ValueError:
+            skipped += 1
+            continue
+
+        if start_time <= timestamp <= end_time:
+            shutil.copy2(image_path, destination_dir / image_path.name)
+            copied += 1
+        else:
+            skipped += 1
+
+    return copied, skipped
+
+
+def main() -> int:
+    args = parse_args()
+    source = Path(args.source).expanduser()
+    destination = Path(args.destination).expanduser()
+
+    if args.start_time > args.end_time:
+        print("Error: start_time must be <= end_time", file=sys.stderr)
+        return 1
+
+    if not source.is_dir():
+        print(f"Error: source folder not found: {source}", file=sys.stderr)
+        return 1
+
+    camera_dirs = [source / camera_dir for camera_dir in CAMERA_DIRS]
+    if not all(camera_dir.is_dir() for camera_dir in camera_dirs):
+        missing = [str(camera_dir) for camera_dir in camera_dirs if not camera_dir.is_dir()]
+        print(
+            "Error: source must contain main_bot_left and main_bot_right. "
+            f"Missing: {', '.join(missing)}",
+            file=sys.stderr,
+        )
+        return 1
+
+    total_copied = 0
+    total_skipped = 0
+    for camera_dir in camera_dirs:
+        output_dir = destination if args.flat else destination / camera_dir.name
+        copied, skipped = copy_camera_folder(
+            camera_dir,
+            output_dir,
+            args.start_time,
+            args.end_time,
+        )
+        total_copied += copied
+        total_skipped += skipped
+        print(f"{camera_dir.name}: copied {copied}, skipped {skipped}")
+
+    print(f"Done. Copied {total_copied} images into {destination}")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

scripts/img_opener.py

@@ -1,10 +1,12 @@
 import os
 import sys
 import re
+from bisect import bisect_left
 from PIL import Image, ImageTk
 import tkinter as tk
 
 SUPPORTED_EXTENSIONS = (".png", ".jpg", ".jpeg", ".bmp", ".gif", ".tiff")
+CAMERA_DIRS = ("main_bot_left", "main_bot_right")
 
 
 def extract_timestamp(filename):
@@ -20,24 +22,31 @@ def __init__(self, folder, start_time=0.0):
         self.root = tk.Tk()
         self.root.title("Image Viewer (Press any key to advance)")
 
-        # Collect images
-        items = []
-        for f in os.listdir(folder):
-            if f.lower().endswith(SUPPORTED_EXTENSIONS):
-                try:
-                    ts = extract_timestamp(f)
-                    items.append((ts, os.path.join(folder, f)))
-                except ValueError:
-                    pass
+        folder = resolve_folder(folder)
+        camera_folders = [os.path.join(folder, cam) for cam in CAMERA_DIRS]
+        if all(os.path.isdir(camera_folder) for camera_folder in camera_folders):
+            self.load_stereo_images(camera_folders, start_time)
+        else:
+            self.load_single_folder(folder, start_time)
+
+        self.index = 0
+
+        self.label = tk.Label(self.root)
+        self.label.pack()
+
+        self.root.bind("<Key>", self.next_image)
+
+        self.show_image()
+        self.root.mainloop()
+
+    def load_single_folder(self, folder, start_time):
+        self.mode = "single"
+        items = collect_images(folder)
 
         if not items:
             print("No valid timestamped images found.")
             sys.exit(1)
 
-        # Sort numerically
-        items.sort(key=lambda x: x[0])
-
-        # --- FILTER BY START TIME ---
         items = [item for item in items if item[0] >= start_time]
 
         if not items:
@@ -46,22 +55,52 @@ def __init__(self, folder, start_time=0.0):
 
         self.timestamps = [x[0] for x in items]
         self.image_paths = [x[1] for x in items]
-
-        # --- NORMALIZE TO FIRST DISPLAYED FRAME ---
         self.t0 = self.timestamps[0]
         self.norm_timestamps = [t - self.t0 for t in self.timestamps]
 
-        self.index = 0
+    def load_stereo_images(self, camera_folders, start_time):
+        self.mode = "stereo"
+        left_items = collect_images(camera_folders[0])
+        right_items = collect_images(camera_folders[1])
 
-        self.label = tk.Label(self.root)
-        self.label.pack()
+        if not left_items or not right_items:
+            print("Both main_bot_left and main_bot_right need valid timestamped images.")
+            sys.exit(1)
 
-        self.root.bind("<Key>", self.next_image)
+        left_items = [item for item in left_items if item[0] >= start_time]
+        right_items = [item for item in right_items if item[0] >= start_time]
 
-        self.show_image()
-        self.root.mainloop()
+        if not left_items or not right_items:
+            print(f"No images found after start_time={start_time}")
+            sys.exit(1)
+
+        all_timestamps = sorted(set(t for t, _ in left_items + right_items))
+
+        left_timestamps = [x[0] for x in left_items]
+        right_timestamps = [x[0] for x in right_items]
+        pairs = []
+        last_paths = None
+        for timestamp in all_timestamps:
+            left = nearest_item(left_items, left_timestamps, timestamp)
+            right = nearest_item(right_items, right_timestamps, timestamp)
+            paths = (left[1], right[1])
+            if paths == last_paths:
+                continue
+            pairs.append((timestamp, left, right))
+            last_paths = paths
+
+        self.timestamps = [x[0] for x in pairs]
+        self.image_pairs = [(x[1], x[2]) for x in pairs]
+        self.t0 = self.timestamps[0]
+        self.norm_timestamps = [t - self.t0 for t in self.timestamps]
 
     def show_image(self):
+        if self.mode == "stereo":
+            self.show_stereo_image()
+        else:
+            self.show_single_image()
+
+    def show_single_image(self):
         path = self.image_paths[self.index]
         img = Image.open(path)
 
@@ -79,19 +118,93 @@ def show_image(self):
             f"({self.index+1}/{len(self.image_paths)})"
         )
 
+    def show_stereo_image(self):
+        (left_ts, left_path), (right_ts, right_path) = self.image_pairs[self.index]
+
+        screen_w = self.root.winfo_screenwidth()
+        screen_h = self.root.winfo_screenheight()
+        img = make_side_by_side(left_path, right_path, screen_w, screen_h)
+
+        self.tk_image = ImageTk.PhotoImage(img)
+        self.label.config(image=self.tk_image)
+
+        norm_time = self.norm_timestamps[self.index]
+        frame_time = self.timestamps[self.index]
+        delta_ms = abs(left_ts - right_ts) * 1000
+        self.root.title(
+            f"left {os.path.basename(left_path)} | right {os.path.basename(right_path)} | "
+            f"t = {norm_time:.3f}s ({frame_time:.3f}) | dt = {delta_ms:.1f}ms "
+            f"({self.index+1}/{len(self.image_pairs)})"
+        )
+
     def next_image(self, event=None):
         self.index += 1
-        if self.index >= len(self.image_paths):
+        if self.index >= len(self.timestamps):
             print("Reached end of images.")
             self.root.quit()
             return
 
         self.show_image()
 
 
+def resolve_folder(folder):
+    if os.path.isdir(folder):
+        return folder
+
+    scripts_parent_path = os.path.join(os.path.dirname(__file__), "..", folder)
+    if os.path.isdir(scripts_parent_path):
+        return scripts_parent_path
+
+    print(f"Folder not found: {folder}")
+    sys.exit(1)
+
+
+def collect_images(folder):
+    items = []
+    for f in os.listdir(folder):
+        if f.lower().endswith(SUPPORTED_EXTENSIONS):
+            try:
+                ts = extract_timestamp(f)
+                items.append((ts, os.path.join(folder, f)))
+            except ValueError:
+                pass
+
+    items.sort(key=lambda x: x[0])
+    return items
+
+
+def nearest_item(items, timestamps, timestamp):
+    index = bisect_left(timestamps, timestamp)
+    if index == 0:
+        return items[0]
+    if index == len(items):
+        return items[-1]
+
+    before = items[index - 1]
+    after = items[index]
+    if abs(before[0] - timestamp) <= abs(after[0] - timestamp):
+        return before
+    return after
+
+
+def make_side_by_side(left_path, right_path, screen_w, screen_h):
+    max_each_w = max(1, screen_w // 2)
+    left_img = Image.open(left_path).convert("RGB")
+    right_img = Image.open(right_path).convert("RGB")
+    left_img.thumbnail((max_each_w, screen_h))
+    right_img.thumbnail((max_each_w, screen_h))
+
+    width = left_img.width + right_img.width
+    height = max(left_img.height, right_img.height)
+    combined = Image.new("RGB", (width, height), "black")
+    combined.paste(left_img, (0, (height - left_img.height) // 2))
+    combined.paste(right_img, (left_img.width, (height - right_img.height) // 2))
+    return combined
+
+
 if __name__ == "__main__":
     if len(sys.argv) not in (2, 3):
-        print("Usage: python viewer.py <image_folder> [start_time]")
+        print("Usage: python img_opener.py <image_folder> [start_time]")
         sys.exit(1)
 
     folder = sys.argv[1]