File size: 7,081 Bytes
740910f
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
"""Gradio demo for both parts of the project.

1. Stereo vision: dense disparity + metric depth (StereoSGBM) on Middlebury pairs
   or your own rectified pair, scored against Middlebury ground truth.
2. Multi-agent pathfinding: Space-Time A* vs Conflict-Based Search on a 3D grid.
"""
import random
import tempfile
from pathlib import Path

import cv2
import gradio as gr
import matplotlib

matplotlib.use("Agg")
import numpy as np

from problem2 import run_problem2

ROOT = Path(__file__).parent / "Dataset"
SCENES = {
    "Adirondack": ROOT / "Adirondack-perfect" / "Adirondack-perfect",
    "Playroom": ROOT / "Playroom-perfect" / "Playroom-perfect",
}


def read_calib(path):
    vals = {}
    for line in Path(path).read_text().splitlines():
        if "=" in line:
            k, v = line.split("=", 1)
            vals[k.strip()] = v.strip()
    fx = float(vals["cam0"].strip("[]").split()[0])
    return fx, float(vals["baseline"]), float(vals["doffs"]), int(vals.get("ndisp", 256))


def read_pfm(path):
    """Read a Middlebury .pfm (OpenCV rescales these values, so parse it directly)."""
    with open(path, "rb") as f:
        channels = 3 if f.readline().strip() == b"PF" else 1
        w, h = map(int, f.readline().split())
        scale = float(f.readline())
        data = np.fromfile(f, ("<" if scale < 0 else ">") + "f4")
    shape = (h, w, channels) if channels == 3 else (h, w)
    return np.flipud(data.reshape(shape)).astype(np.float32)


def sgbm(left, right, num_disp, block):
    num_disp = max(16, int(np.ceil(num_disp / 16)) * 16)
    matcher = cv2.StereoSGBM_create(
        minDisparity=0, numDisparities=num_disp, blockSize=block,
        P1=8 * 3 * block**2, P2=32 * 3 * block**2, disp12MaxDiff=1,
        uniquenessRatio=10, speckleWindowSize=100, speckleRange=32,
        mode=cv2.STEREO_SGBM_MODE_SGBM_3WAY,
    )
    return matcher.compute(left, right).astype(np.float32) / 16.0


def colorize(arr, valid):
    v = arr[valid]
    lo, hi = (np.percentile(v, 2), np.percentile(v, 98)) if v.size else (0, 1)
    norm = np.clip((arr - lo) / max(hi - lo, 1e-6), 0, 1)
    img = cv2.applyColorMap((norm * 255).astype(np.uint8), cv2.COLORMAP_TURBO)[..., ::-1].copy()
    img[~valid] = 0
    return img


def run_stereo(scene, left_up, right_up, scale, block):
    gt = None
    if left_up is not None and right_up is not None:
        left, right = left_up, right_up
        fx = baseline = doffs = None
        ndisp = 256
    else:
        d = SCENES[scene]
        left = cv2.imread(str(d / "im0.png"))[..., ::-1]
        right = cv2.imread(str(d / "im1.png"))[..., ::-1]
        fx, baseline, doffs, ndisp = read_calib(d / "calib.txt")
        gt = read_pfm(d / "disp0.pfm") * scale  # disparities shrink with the image

    h, w = left.shape[:2]
    size = (int(w * scale), int(h * scale))
    l_s = cv2.resize(left, size, interpolation=cv2.INTER_AREA)
    r_s = cv2.resize(right, size, interpolation=cv2.INTER_AREA)
    disp = sgbm(cv2.cvtColor(l_s, cv2.COLOR_RGB2GRAY), cv2.cvtColor(r_s, cv2.COLOR_RGB2GRAY), ndisp * scale, block)
    valid = disp > 0

    notes = [f"Resolution {size[0]}x{size[1]}, valid pixels {100 * valid.mean():.1f}%"]
    depth_img = None
    if fx is not None:
        # Middlebury: Z = baseline * f / (d + doffs), in mm (all at full resolution)
        depth = np.zeros_like(disp)
        depth[valid] = baseline * fx / (disp[valid] / scale + doffs) / 1000.0
        depth_img = colorize(-depth, valid)  # near = warm
        notes.append(f"Median depth {np.median(depth[valid]):.2f} m")
    if gt is not None:
        gt = cv2.resize(gt, size, interpolation=cv2.INTER_NEAREST)
        m = valid & np.isfinite(gt) & (gt > 0)
        err = np.abs(disp[m] - gt[m])
        notes.append(
            f"vs ground truth: mean abs error {err.mean():.2f}px, "
            f"pixels off by >2px at full resolution: {100 * (err > 2 * scale).mean():.1f}%"
        )
    return colorize(disp, valid), depth_img, "  \n".join(notes)


def rand_point(n, rng):
    return tuple(int(v) for v in rng.integers(0, n, 3))


def run_mapf(agents, grid, weight_ratio, solver, seed):
    rng = np.random.default_rng(int(seed))
    reqs = [(rand_point(grid, rng), rand_point(grid, rng)) for _ in range(int(agents))]
    out = Path(tempfile.mkdtemp())
    for mode in ("fast", "cbs"):
        (out / mode).mkdir()  # problem2 writes into these but does not create them
    random.seed(int(seed))
    results = run_problem2(reqs, grid_size=int(grid), weight_ratio=weight_ratio, seed=int(seed),
                           solver=solver, output_dir=str(out))
    rows = []
    for mode, r in (results or {}).items():
        st = r.get("stats", {})
        rows.append([mode, st.get("status", "ok"), round(1000 * st.get("time", 0), 1),
                     st.get("cost", ""), r.get("makespan", "")])
    imgs = sorted(str(p) for p in out.rglob("*.png"))
    html = next(iter(out.rglob("*.html")), None)
    return imgs, rows, (str(html) if html else None)


with gr.Blocks(title="Stereo Vision & Multi-Agent Pathfinding") as demo:
    gr.Markdown("# Stereo Vision & Multi-Agent Pathfinding")
    with gr.Tab("Stereo depth"):
        gr.Markdown("Pick a Middlebury scene (scored against ground truth) or upload your own **rectified** pair.")
        with gr.Row():
            scene = gr.Dropdown(list(SCENES), value="Adirondack", label="Middlebury scene")
            scale = gr.Slider(0.15, 0.5, value=0.25, step=0.05, label="Scale (lower = faster)")
            block = gr.Slider(3, 15, value=7, step=2, label="SGBM block size")
        with gr.Accordion("Or upload a rectified stereo pair", open=False):
            with gr.Row():
                lu = gr.Image(type="numpy", label="Left")
                ru = gr.Image(type="numpy", label="Right")
        with gr.Row():
            disp_out = gr.Image(label="Disparity")
            depth_out = gr.Image(label="Depth (Middlebury scenes only)")
        stats = gr.Markdown()
        gr.Button("Compute", variant="primary").click(run_stereo, [scene, lu, ru, scale, block], [disp_out, depth_out, stats])
    with gr.Tab("Multi-agent pathfinding"):
        gr.Markdown("Random start/goal pairs on a weighted 3D grid; compare Space-Time A* with CBS.")
        with gr.Row():
            agents = gr.Slider(2, 8, value=3, step=1, label="Agents")
            grid = gr.Slider(11, 51, value=21, step=2, label="Grid size (N^3)")
            wr = gr.Slider(0.0, 0.5, value=0.1, step=0.05, label="Obstacle weight ratio")
            solver = gr.Radio(["fast", "cbs", "compare"], value="compare", label="Solver")
            seed = gr.Number(value=42, precision=0, label="Seed")
        table = gr.Dataframe(headers=["solver", "status", "time (ms)", "flowtime", "makespan"])
        gallery = gr.Gallery(label="Trajectories", columns=2, height=420)
        html_file = gr.File(label="Interactive 3D view (HTML)")
        gr.Button("Plan paths", variant="primary").click(run_mapf, [agents, grid, wr, solver, seed], [gallery, table, html_file])

if __name__ == "__main__":
    demo.queue(max_size=8).launch()