"""Two figures of where the CPU section's remaining errors sit (v5.5, 256 layers, N 128, the nearest rule, NT 512): failures_points.png: three objects, one view each: the mesh with every point answered wrongly farther than 0.1 % of the diagonal from the surface (of 2M), coloured by that distance; failures_collide.png: three pairs, one view each, the worst wrong pose of the densest sampling: a false contact shows the FCP samples found inside the other object (the meshes do not touch); a missed contact shows where the true meshes overlap (FCP found no sample inside). Writes into ~/code/data/meshes/fcp_report/sections/collision/. FCP_API= FCP_VTAG=v55k1_ env -u PYTHONPATH .venv/bin/python scripts/failures.py """ import sys from pathlib import Path import matplotlib matplotlib.use("Agg") import matplotlib.pyplot as plt # noqa: E402 import numpy as np # noqa: E402 import open3d as o3d # noqa: E402 import trimesh # noqa: E402 from matplotlib.lines import Line2D # noqa: E402 from mpl_toolkits.mplot3d.art3d import Poly3DCollection # noqa: E402 sys.path.insert(0, str(Path(__file__).parent)) from common import RULE, descriptor, load_mesh # noqa: E402 from fcpq import FCPQuery, collide_cpu_batch, relative_maps, table_args # noqa: E402 from household import NT, OUT # noqa: E402 from slice_errors import BANDS, COLORS # noqa: E402 from sweep import points as point_sets # noqa: E402 LAYERS, N = 256, 128 OBJ_VIEWS = [("mug", 25, -60), ("dino", 12, -60), ("spring", 20, -60), ("knot", 35, -60)] # (object, elevation, azimuth): the most telling view PAIRS = [("dino-horse", "false"), ("bowl-bowl_dog", "miss"), ("knot-spring", "false")] A_COL, B_COL = np.array([0.62, 0.72, 0.84]), np.array([0.86, 0.75, 0.62]) FALSE_C, MISS_C = "#d7301f", "#1d1d1b" INK = "#1d1d1b" def decimated(m, faces=25000): if len(m.faces) <= faces: return m o = o3d.geometry.TriangleMesh(o3d.utility.Vector3dVector(m.vertices), o3d.utility.Vector3iVector(m.faces)) o = o.simplify_quadric_decimation(faces) return trimesh.Trimesh(np.asarray(o.vertices), np.asarray(o.triangles), process=False) def shaded(m, base, alpha): light = np.array([0.3, -0.5, 0.8]) / np.linalg.norm([0.3, -0.5, 0.8]) sh = 0.5 + 0.5 * np.clip(np.asarray(m.face_normals) @ light, 0, 1) return np.column_stack([np.outer(sh, base), np.full(len(sh), alpha)]) def box(ax, c, r): ax.set_xlim(c[0] - r, c[0] + r) ax.set_ylim(c[1] - r, c[1] + r) ax.set_zlim(c[2] - r, c[2] + r) ax.set_box_aspect((1, 1, 1)) ax.set_axis_off() def points_figure(): fig = plt.figure(figsize=(20, 6.2), dpi=100, facecolor="white") for i, (name, elev, azim) in enumerate(OBJ_VIEWS): m = load_mesh(name) d = descriptor(name, LAYERS, N) q = FCPQuery(d, nt=NT) bad_all, dist_all, P_all = [], [], [] for P, gt, dist in point_sets(m).values(): ins = q.inside_cpu(np.ascontiguousarray(d.pose.to_canonical(P), np.float32), RULE) b = (ins != gt) & (dist > BANDS[0][0]) P_all.append(P[b]), dist_all.append(dist[b]) P, dist = np.concatenate(P_all), np.concatenate(dist_all) mm = decimated(m) ax = fig.add_subplot(1, len(OBJ_VIEWS), i + 1, projection="3d") ax.add_collection3d(Poly3DCollection(np.asarray(mm.vertices)[np.asarray(mm.faces)], facecolors=shaded(mm, np.array([0.86, 0.86, 0.88]), 0.35), edgecolors="none")) for j, ((lo, hi), col) in enumerate(zip(BANDS, COLORS)): s = (dist > lo) & (dist <= hi) ax.scatter(*P[s].T, s=10 + 10 * j, color=col, depthshade=False) lo_, hi_ = m.bounds box(ax, (lo_ + hi_) / 2, (hi_ - lo_).max() / 2) ax.view_init(elev=elev, azim=azim) ax.set_title(f"{name.replace('_', ' ')}: {len(P)} of 2M points wrong beyond 0.1 %\n" f"(beyond 0.2 %: {int((dist > 0.2).sum())})", fontsize=13, color=INK, loc="left") handles = [Line2D([], [], ls="", marker="o", ms=9, color=c, label=f"{a}–{b} % of the diagonal" if np.isfinite(b) else f"over {a} %") for (a, b), c in zip(BANDS, COLORS)] fig.legend(handles=handles, loc="lower center", ncol=4, fontsize=12, frameon=False, title="wrong answers, by distance to the true surface", title_fontsize=12) fig.tight_layout(rect=(0, 0.08, 1, 1)) fig.savefig(OUT / "failures_points.png", facecolor="white", bbox_inches="tight") plt.close(fig) def collide_figure(): from bound import truth fig = plt.figure(figsize=(20, 7.4), dpi=100, facecolor="white") for i, (pair, kind) in enumerate(PAIRS): T = truth(pair) nA, nB = pair.split("-") mA, mB = load_mesh(nA), load_mesh(nB) dA, dB = descriptor(nA, LAYERS, N), descriptor(nB, LAYERS, N) qA, qB = FCPQuery(dA, nt=NT), FCPQuery(dB, nt=NT) R, t, gt, diag = T["R"], T["t"], T["gt"], float(T["diag"]) maps = relative_maps(qA, qB, R, t) ls, vs = LAYERS // 128, NT // 128 PA, PB = qA.surface_points(ls, vs), qB.surface_points(ls, vs) hits = np.zeros(len(R), np.bool_) collide_cpu_batch(PA, PB, *maps, table_args(qA), table_args(qB), hits) wrong = np.flatnonzero((gt & ~hits) if kind == "miss" else (~gt & hits)) k = wrong[np.argmax(T["depth"][wrong] if kind == "miss" else T["sep"][wrong])] Rk, tk = R[k], t[k] place = lambda X: X @ Rk.T + tk # B's world -> placed if kind == "false": # the samples FCP found inside the other object MAB, tAB, MBA, tBA = (x[k] for x in maps) a_in = qB.inside_cpu(np.ascontiguousarray(PA @ MAB.T + tAB, np.float32), RULE) b_in = qA.inside_cpu(np.ascontiguousarray(PB @ MBA.T + tBA, np.float32), RULE) mark = np.vstack([dA.pose.to_world(PA[a_in]), place(dB.pose.to_world(PB[b_in]))]) what = f"false contact: the meshes are {100 * T['sep'][k] / diag:.3f} % of the diagonal apart" else: # where the true meshes overlap SA, SB = mA.sample(60000), mB.sample(60000) sA, sB = o3d.t.geometry.RaycastingScene(), o3d.t.geometry.RaycastingScene() sA.add_triangles(o3d.core.Tensor(np.asarray(mA.vertices, np.float32)), o3d.core.Tensor(np.asarray(mA.faces, np.uint32))) sB.add_triangles(o3d.core.Tensor(np.asarray(mB.vertices, np.float32)), o3d.core.Tensor(np.asarray(mB.faces, np.uint32))) a_in = sB.compute_occupancy(o3d.core.Tensor(((SA - tk) @ Rk).astype(np.float32)), nsamples=3).numpy() > 0 b_in = sA.compute_occupancy(o3d.core.Tensor(place(SB).astype(np.float32)), nsamples=3).numpy() > 0 mark = np.vstack([SA[a_in], place(SB[b_in])]) what = f"missed contact: {100 * T['depth'][k] / diag:.4f} % of the diagonal deep" ax = fig.add_subplot(1, 3, i + 1, projection="3d") for m, col, tr in ((mA, A_COL, None), (mB, B_COL, place)): mm = decimated(m) V = np.asarray(mm.vertices) if tr is None else tr(np.asarray(mm.vertices)) ax.add_collection3d(Poly3DCollection(V[np.asarray(mm.faces)], facecolors=shaded(mm if tr is None else trimesh.Trimesh(V, mm.faces, process=False), col, 0.45), edgecolors="none")) col = FALSE_C if kind == "false" else MISS_C ax.scatter(*mark.T, s=140, color=col, edgecolors="white", linewidths=1.5, depthshade=False, zorder=10) ax.scatter(*mark.mean(0), s=2600, facecolors="none", edgecolors=col, linewidths=2.2, depthshade=False, zorder=11) c = mark.mean(0) if len(mark) else (mA.bounds.mean(0)) box(ax, c, 0.09 * min(np.linalg.norm(mA.extents), np.linalg.norm(mB.extents))) ax.view_init(elev=20, azim=-60) ax.set_title(f"{nA.replace('_', ' ')} – {nB.replace('_', ' ')}\n{what}", fontsize=13, color=INK, loc="left") print(pair, kind, "pose", k, what, "marked", len(mark), flush=True) handles = [Line2D([], [], ls="", marker="s", ms=12, color=A_COL, label="object A"), Line2D([], [], ls="", marker="s", ms=12, color=B_COL, label="object B (placed)"), Line2D([], [], ls="", marker="o", ms=9, color=FALSE_C, label="false contact: FCP samples found inside the other object"), Line2D([], [], ls="", marker="o", ms=9, color=MISS_C, label="missed contact: where the true meshes overlap")] fig.legend(handles=handles, loc="lower center", ncol=4, fontsize=12, frameon=False) fig.tight_layout(rect=(0, 0.07, 1, 1)) fig.savefig(OUT / "failures_collide.png", facecolor="white", bbox_inches="tight") plt.close(fig) if __name__ == "__main__": points_figure() collide_figure()