Image Segmentation
PyTorch
English
painting-vision-robotics-kit
semantic-segmentation
robotics
edge-ai
construction-ai
autonomous-painting
wall-painting-robot
paint-coverage-estimation
building-facade
drywall
skirting-detection
window-detection
lidar
depth-validation
segformer
deeplabv3
Eval Results (legacy)
Download depth_fusion.py from constructelligence/painting-vision-robotics-kit: direct link, hf CLI and curl.
- Browser
- Download file 18.4 kB
-
https://huggingface.co/constructelligence/painting-vision-robotics-kit/resolve/main/depth_fusion.py
- Command line
-
hf download hf://constructelligence/painting-vision-robotics-kit/depth_fusion.py
-
curl -L -o depth_fusion.py https://huggingface.co/constructelligence/painting-vision-robotics-kit/resolve/main/depth_fusion.py
18.4 kB
| """Depth fusion: stop lines, corners and area from fitted 3D planes instead of image edges. | |
| ``wall_measure.py`` measures from one RGB frame: good when the mask is clean, | |
| but the stop lines are only as good as the segmentation and the scale needs an | |
| assumed or measured length. With an aligned depth map (LiDAR, ToF, stereo) the | |
| robot does what survey software does instead: | |
| 1. **Planes.** RANSAC fits the wall plane(s) to depth points the model labels | |
| wall; a second wall plane, crossing the first at a real corner angle, is an | |
| inside corner. Ceiling and floor planes are fitted to the non-wall points | |
| above and below the wall, and must stand roughly square to it. | |
| 2. **Stop lines are plane intersections:** ceiling line = wall ∩ ceiling, floor | |
| line = wall ∩ floor, inside corner = wall ∩ wall. The top of the skirting is | |
| the skirting pixels' height above the floor line, measured on the wall plane. | |
| Lines come out in the camera frame in millimetres (what a robot controller | |
| consumes) and projected back to image pixels. | |
| 3. **Area on a metric grid.** Each face is sampled on a 1 cm grid in its own | |
| plane coordinates; every cell is projected into the image and takes that | |
| pixel's label. No perspective guess and no assumed ceiling height. Windows | |
| count as openings even though glass returns bad depth, because labels come | |
| from the image and geometry from the plane. | |
| 4. **Honest fallbacks.** No ceiling or floor plane in view -> that line is the | |
| wall points' extent and is marked ``depth_extent``; a wall running out of the | |
| image keeps ``frame`` (no stop) at that end. | |
| The output has the same face fields as ``wall_measure.measure_wall`` (plus | |
| ``stop_lines_camera_mm`` and plane quality), so the paint estimate, overlay, | |
| safety gate and ``robot_planner.py --measurements`` work unchanged. | |
| Camera convention: OpenCV pinhole (x right, y down, z forward), depth = z in | |
| metres, intrinsics ``fx, fy, cx, cy`` for the RGB frame. A depth map of another | |
| resolution is assumed to cover the same field of view (aligned, as phone LiDAR | |
| and most RGB-D SDKs deliver it). numpy + scipy only. | |
| """ | |
| from __future__ import annotations | |
| import numpy as np | |
| from scipy import ndimage | |
| from wall_measure import (DOOR, OBSTACLE, OTHER, PAINTED, SILHOUETTE_CLASSES, SKIRTING, SWITCH, AC, UNCERTAIN, | |
| UNPAINTED, WALL_CLASSES, WINDOW) | |
| M_TO_FT = 3.28084 | |
| FT2_PER_M2 = M_TO_FT ** 2 | |
| def fit_plane(points, tol=0.015, iterations=400, sample=30000, seed=0): | |
| """RANSAC + least-squares refit. Returns ``(normal, offset, inlier_mask, rms_m)`` with n·p + d = 0, | |
| normal oriented away from the camera (d <= 0).""" | |
| points = np.asarray(points, float) | |
| rng = np.random.default_rng(seed) | |
| pool = points[rng.choice(len(points), min(sample, len(points)), replace=False)] | |
| best, best_count = None, -1 | |
| for _ in range(iterations): | |
| a, b, c = pool[rng.choice(len(pool), 3, replace=False)] | |
| n = np.cross(b - a, c - a) | |
| norm = np.linalg.norm(n) | |
| if norm < 1e-9: | |
| continue | |
| n /= norm | |
| d = -n @ a | |
| count = int((np.abs(pool @ n + d) < tol).sum()) | |
| if count > best_count: | |
| best, best_count = (n, d), count | |
| if best is None: | |
| return None | |
| n, d = best | |
| for _ in range(2): # refit on all inliers, twice | |
| inliers = np.abs(points @ n + d) < tol | |
| if inliers.sum() < 3: | |
| return None | |
| p = points[inliers] | |
| centroid = p.mean(0) | |
| _, vecs = np.linalg.eigh((p - centroid).T @ (p - centroid)) | |
| n = vecs[:, 0] | |
| d = -n @ centroid | |
| if d > 0: | |
| n, d = -n, -d | |
| inliers = np.abs(points @ n + d) < tol | |
| rms = float(np.sqrt(np.mean((points[inliers] @ n + d) ** 2))) if inliers.any() else float("inf") | |
| return n, float(d), inliers, rms | |
| def plane_intersection(p1, p2): | |
| """Line (point, unit direction) where two planes (n, d) meet.""" | |
| (n1, d1), (n2, d2) = p1, p2 | |
| direction = np.cross(n1, n2) | |
| norm = np.linalg.norm(direction) | |
| if norm < 1e-6: | |
| return None | |
| a = np.array([n1, n2, direction]) | |
| point = np.linalg.solve(a, np.array([-d1, -d2, 0.0])) | |
| return point, direction / norm | |
| def _rays(xs, ys, k): | |
| fx, fy, cx, cy = k | |
| return np.stack([(np.asarray(xs, float) - cx) / fx, (np.asarray(ys, float) - cy) / fy, | |
| np.ones(np.shape(xs))], axis=-1) | |
| def _project(points, k): | |
| fx, fy, cx, cy = k | |
| p = np.atleast_2d(points) | |
| return np.c_[fx * p[:, 0] / p[:, 2] + cx, fy * p[:, 1] / p[:, 2] + cy] | |
| def _resize_nearest(labels, shape): | |
| h, w = labels.shape | |
| ys = np.minimum((np.arange(shape[0]) * h / shape[0]).astype(int), h - 1) | |
| xs = np.minimum((np.arange(shape[1]) * w / shape[1]).astype(int), w - 1) | |
| return labels[ys][:, xs] | |
| class _Frame: | |
| """Wall-plane coordinates: origin on the floor line, u along the wall (image right), v up.""" | |
| def __init__(self, plane, up_hint): | |
| n, d = plane | |
| self.n, self.d = np.asarray(n, float), float(d) | |
| v = up_hint - (up_hint @ self.n) * self.n | |
| self.v = v / np.linalg.norm(v) | |
| u = np.cross(self.v, self.n) | |
| self.u = u / np.linalg.norm(u) | |
| self.origin = -self.d * self.n # foot of the camera on the plane | |
| def to_uv(self, points): | |
| rel = np.atleast_2d(points) - self.origin | |
| return np.c_[rel @ self.u, rel @ self.v] | |
| def to_xyz(self, uv): | |
| uv = np.atleast_2d(uv) | |
| return self.origin + uv[:, :1] * self.u + uv[:, 1:2] * self.v | |
| def hit(self, rays): | |
| """Ray (from the camera centre) intersections with the plane.""" | |
| t = -self.d / (rays @ self.n) | |
| return rays * t[..., None] | |
| def _line_v(frame, line, u): | |
| """v of a 3D line (point, dir) where it crosses wall-plane column u (None if parallel).""" | |
| point, direction = line | |
| uv_p = frame.to_uv(point)[0] | |
| uv_dir = np.array([direction @ frame.u, direction @ frame.v]) | |
| if abs(uv_dir[0]) < 1e-6: | |
| return None | |
| s = (u - uv_p[0]) / uv_dir[0] | |
| return float(uv_p[1] + s * uv_dir[1]) | |
| def _line_u(frame, line, v): | |
| point, direction = line | |
| uv_p = frame.to_uv(point)[0] | |
| uv_dir = np.array([direction @ frame.u, direction @ frame.v]) | |
| if abs(uv_dir[1]) < 1e-6: | |
| return None | |
| s = (v - uv_p[1]) / uv_dir[1] | |
| return float(uv_p[0] + s * uv_dir[0]) | |
| def measure_with_depth(labels, depth, intrinsics, plane_tol=0.015, min_face_share=0.12, cell_m=0.01): | |
| """Stop lines, corners and areas for each wall face from labels + aligned depth (metres). | |
| Returns a dict shaped like ``wall_measure.measure_wall`` output, or | |
| ``{"status": "depth_failed", "reason": ...}`` when no wall plane is found. | |
| """ | |
| labels = np.asarray(labels) | |
| depth = np.asarray(depth, float) | |
| H, W = labels.shape | |
| hd, wd = depth.shape | |
| k_rgb = tuple(float(v) for v in intrinsics) | |
| sx, sy = wd / W, hd / H | |
| k_d = (k_rgb[0] * sx, k_rgb[1] * sy, (k_rgb[2] + 0.5) * sx - 0.5, (k_rgb[3] + 0.5) * sy - 0.5) | |
| lab_d = _resize_nearest(labels, (hd, wd)) | |
| valid = np.isfinite(depth) & (depth > 0.05) | |
| ys, xs = np.nonzero(valid) | |
| pts = _rays(xs, ys, k_d) * depth[ys, xs][:, None] | |
| lab_pts = lab_d[ys, xs] | |
| # 1. Wall planes, largest first; a second plane must cross the first at a corner angle. | |
| wall_pts_idx = np.flatnonzero(np.isin(lab_pts, WALL_CLASSES)) | |
| if len(wall_pts_idx) < 200: | |
| return {"status": "depth_failed", "reason": "too few depth points on predicted wall", "faces": []} | |
| planes, remaining = [], wall_pts_idx | |
| while len(remaining) >= max(200, min_face_share * len(wall_pts_idx)) and len(planes) < 3: | |
| fit = fit_plane(pts[remaining], tol=plane_tol, seed=len(planes)) | |
| if fit is None: | |
| break | |
| n, d, inl, rms = fit | |
| if inl.sum() < max(200, min_face_share * len(wall_pts_idx)): | |
| break | |
| if any(abs(n @ p["n"]) > 0.85 for p in planes): # parallel to an existing wall: not a new face | |
| remaining = remaining[~inl] | |
| continue | |
| planes.append({"n": n, "d": d, "idx": remaining[inl], "rms_mm": round(rms * 1000, 2), | |
| "inlier_share": round(float(inl.mean()), 3)}) | |
| remaining = remaining[~inl] | |
| if not planes: | |
| return {"status": "depth_failed", "reason": "no wall plane fits the depth", "faces": []} | |
| # 2. Ceiling and floor: non-wall points above / below the wall, square to it. | |
| wall_rows = np.flatnonzero(np.isin(lab_d, WALL_CLASSES).any(1)) | |
| top_row, bottom_row = (wall_rows[0], wall_rows[-1]) if len(wall_rows) else (0, hd - 1) | |
| other_idx = np.flatnonzero(np.isin(lab_pts, (OTHER,))) | |
| horizontal = {} | |
| for name, rows in (("ceiling", ys[other_idx] < top_row + 0.25 * (bottom_row - top_row)), | |
| ("floor", ys[other_idx] > bottom_row - 0.25 * (bottom_row - top_row))): | |
| idx = other_idx[rows] | |
| if len(idx) < 150: | |
| continue | |
| fit = fit_plane(pts[idx], tol=plane_tol * 1.5, seed=7) | |
| if fit and fit[2].sum() >= 150 and all(abs(fit[0] @ p["n"]) < 0.3 for p in planes): | |
| horizontal[name] = {"n": fit[0], "d": fit[1], "rms_mm": round(fit[3] * 1000, 2), | |
| "points": int(fit[2].sum())} | |
| # "Up": the floor/ceiling normal when seen (true vertical), else image-up on the wall. | |
| up = np.array([0.0, -1.0, 0.0]) | |
| for name in ("floor", "ceiling"): | |
| if name in horizontal: | |
| n = horizontal[name]["n"] | |
| up = n if n @ up > 0 else -n | |
| break | |
| # 3. Faces, ordered left to right in the image. | |
| for p in planes: | |
| p["frame"] = _Frame((p["n"], p["d"]), up) | |
| # u must run to the image right, so "left" and "right" ends mean what they say. | |
| centre = pts[p["idx"]].mean(0) | |
| a, b = _project(np.array([centre, centre + 0.1 * p["frame"].u]), k_d) | |
| if b[0] < a[0]: | |
| p["frame"].u = -p["frame"].u | |
| p["x_med"] = float(np.median(xs[p["idx"]] / sx)) | |
| planes.sort(key=lambda p: p["x_med"]) | |
| corners = [] | |
| for a, b in zip(planes, planes[1:]): | |
| line = plane_intersection((a["n"], a["d"]), (b["n"], b["d"])) | |
| corners.append(line) | |
| silhouette = ndimage.binary_fill_holes(np.isin(labels, SILHOUETTE_CLASSES)) | |
| def outline_u(frame, face_px, side): | |
| """u of the wall's visible end from the label outline (pixel edges hit on the plane).""" | |
| rows = np.flatnonzero(face_px.any(1)) | |
| if len(rows) < 3: | |
| return None | |
| xs_edge = np.array([np.flatnonzero(face_px[r])[0] - 0.5 if side == "left" else | |
| np.flatnonzero(face_px[r])[-1] + 0.5 for r in rows], float) | |
| hits = frame.hit(_rays(xs_edge, rows + 0.0, k_rgb)) | |
| return float(np.median(frame.to_uv(hits)[:, 0])) | |
| # Image region of each face: the outline, split at the corner lines' image positions. | |
| face_regions = [] | |
| yy_img, xx_img = np.mgrid[0:H, 0:W] | |
| for i in range(len(planes)): | |
| region = silhouette.copy() | |
| for j, side in ((i - 1, "right_of"), (i, "left_of")): | |
| if 0 <= j < len(corners) and corners[j] is not None: | |
| point, direction = corners[j] | |
| a, b = _project(np.array([point - direction, point + direction]), k_rgb) | |
| # Side of the projected corner line each pixel is on. | |
| cross = (b[0] - a[0]) * (yy_img - a[1]) - (b[1] - a[1]) * (xx_img - a[0]) | |
| ref = (b[0] - a[0]) * (H / 2 - a[1]) - (b[1] - a[1]) * (0 - a[0]) # image left side | |
| left_side = np.sign(cross) == np.sign(ref) | |
| region &= ~left_side if side == "right_of" else left_side | |
| face_regions.append(region) | |
| faces = [] | |
| for i, p in enumerate(planes): | |
| f = p["frame"] | |
| uv = f.to_uv(pts[p["idx"]]) | |
| lines, trust = {}, {} | |
| for name in ("ceiling", "floor"): | |
| if name in horizontal: | |
| lines[name] = plane_intersection((p["n"], p["d"]), (horizontal[name]["n"], horizontal[name]["d"])) | |
| u_mid = float(np.median(uv[:, 0])) | |
| v_floor = _line_v(f, lines["floor"], u_mid) if "floor" in lines else None | |
| v_ceiling = _line_v(f, lines["ceiling"], u_mid) if "ceiling" in lines else None | |
| trust["floor"] = "depth_planes" if v_floor is not None else "depth_extent" | |
| trust["top"] = "depth_planes" if v_ceiling is not None else "depth_extent" | |
| if v_floor is None: | |
| v_floor = float(np.percentile(uv[:, 1], 0.5)) | |
| if v_ceiling is None: | |
| v_ceiling = float(np.percentile(uv[:, 1], 99.5)) | |
| # Ends: inside corners where a neighbouring plane meets this one, else the wall points' extent. | |
| ends, u_ends, u_at = {}, {}, {} | |
| region = face_regions[i] | |
| for side, j, pick in (("left", i - 1, 0.5), ("right", i, 99.5)): | |
| corner = corners[j] if 0 <= j < len(corners) else None | |
| if corner is not None: | |
| ends[side] = "inside_corner" | |
| # The corner line itself, at ceiling and floor height (it need not be exactly vertical). | |
| u_at[side] = (_line_u(f, corner, v_ceiling), _line_u(f, corner, v_floor)) | |
| u_ends[side] = _line_u(f, corner, 0.5 * (v_floor + v_ceiling)) | |
| else: | |
| cols = np.flatnonzero(region.any(0)) | |
| at_edge = (cols[0] <= 1) if side == "left" else (cols[-1] >= W - 2) | |
| ends[side] = "frame" if at_edge else "wall_end" | |
| u = outline_u(f, region, side) | |
| u_ends[side] = u if u is not None else float(np.percentile(uv[:, 0], pick)) | |
| u_at[side] = (u_ends[side], u_ends[side]) | |
| u0, u1 = u_ends["left"], u_ends["right"] | |
| # 4. Metric grid on the face: label each 1 cm cell from the image. | |
| nu = max(1, int(round((u1 - u0) / cell_m))) | |
| nv = max(1, int(round((v_ceiling - v_floor) / cell_m))) | |
| gu, gv = np.meshgrid(u0 + (np.arange(nu) + 0.5) * cell_m, v_floor + (np.arange(nv) + 0.5) * cell_m) | |
| xyz = f.to_xyz(np.c_[gu.ravel(), gv.ravel()]) | |
| px = _project(xyz, k_rgb) | |
| cx_, cy_ = np.rint(px[:, 0]).astype(int), np.rint(px[:, 1]).astype(int) | |
| seen = (cx_ >= 0) & (cx_ < W) & (cy_ >= 0) & (cy_ < H) & (xyz[:, 2] > 0) | |
| cell_labels = np.full(len(xyz), 255, np.int64) | |
| cell_labels[seen] = labels[cy_[seen], cx_[seen]] | |
| counts = np.bincount(cell_labels[cell_labels < 255], minlength=10)[:10] * cell_m ** 2 * FT2_PER_M2 | |
| outside_ft2 = float((~seen).sum() * cell_m ** 2 * FT2_PER_M2) | |
| # Skirting top: skirting cells' height above the floor line. | |
| skirt = cell_labels.reshape(nv, nu) == SKIRTING | |
| skirting_m = 0.0 | |
| if skirt.any(): | |
| rows_with = np.flatnonzero(skirt.mean(1) > 0.3) | |
| skirting_m = float((rows_with.max() + 1) * cell_m) if len(rows_with) else 0.0 | |
| trust["bottom"] = "depth_planes" if skirting_m and trust["floor"] == "depth_planes" else trust["floor"] | |
| v_paint_bottom = v_floor + skirting_m | |
| def seg(v_a, v_b, u_a, u_b): | |
| ends3d = f.to_xyz(np.array([[u_a, v_a], [u_b, v_b]])) | |
| return ends3d | |
| lines3d = {"top": seg(v_ceiling, v_ceiling, u0, u1), "bottom": seg(v_paint_bottom, v_paint_bottom, u0, u1), | |
| "floor": seg(v_floor, v_floor, u0, u1), | |
| "left": seg(v_ceiling, v_floor, *u_at["left"]), "right": seg(v_ceiling, v_floor, *u_at["right"])} | |
| width_m, height_m = u1 - u0, v_ceiling - v_floor | |
| quad3d = f.to_xyz(np.array([[u0, v_ceiling], [u1, v_ceiling], [u1, v_floor], [u0, v_floor]])) | |
| paint3d = f.to_xyz(np.array([[u0, v_ceiling], [u1, v_ceiling], [u1, v_paint_bottom], [u0, v_paint_bottom]])) | |
| clipped = [s for s in ("left", "right") if ends[s] == "frame"] | |
| to_px = lambda pts3d: [[round(float(x), 1), round(float(y), 1)] for x, y in _project(pts3d, k_rgb)] | |
| faces.append({ | |
| "index": i, | |
| "stop_lines_px": {k: to_px(v) for k, v in lines3d.items()}, | |
| "stop_lines_camera_mm": {k: np.round(v * 1000, 1).tolist() for k, v in lines3d.items()}, | |
| "ends": ends, | |
| "snapped_to_image_edge": {}, | |
| "stop_line_trust": trust, | |
| "corners_px": to_px(quad3d), | |
| "paint_region_corners_px": to_px(paint3d), | |
| "corners_seen": {"top_left": "left" not in clipped, "top_right": "right" not in clipped, | |
| "bottom_right": "right" not in clipped, "bottom_left": "left" not in clipped}, | |
| "clipped_sides": clipped, | |
| "width_ft": round(width_m * M_TO_FT, 3), "height_ft": round(height_m * M_TO_FT, 3), | |
| "width_mm": round(width_m * 1000, 1), "height_mm": round(height_m * 1000, 1), | |
| "skirting_height_in": round(skirting_m * 39.3701, 2), | |
| "paint_height_ft": round((height_m - skirting_m) * M_TO_FT, 3), | |
| "aspect": round(width_m / max(height_m, 1e-9), 4), "aspect_source": "depth_planes", | |
| "focal_px": round(k_rgb[0], 1), "scale_source": "depth", | |
| "plane": {"normal": np.round(p["n"], 5).tolist(), "distance_mm": round(-p["d"] * 1000, 1), | |
| "rms_mm": p["rms_mm"], "inlier_share": p["inlier_share"], "points": int(len(p["idx"]))}, | |
| "area_ft2": {"gross_floor_to_ceiling": round(width_m * height_m * FT2_PER_M2, 2), | |
| "wall_visible": round(float(counts[list(WALL_CLASSES)].sum()), 2), | |
| "openings": round(float(counts[[DOOR, WINDOW]].sum()), 2), | |
| "fixtures": round(float(counts[[SWITCH, AC, OBSTACLE]].sum()), 2), | |
| "skirting": round(float(counts[SKIRTING]), 2), | |
| "hidden_behind_objects": round(float(counts[OTHER]), 2), | |
| "outside_frame": round(outside_ft2, 2), | |
| "painted": round(float(counts[PAINTED]), 2), "unpainted": round(float(counts[UNPAINTED]), 2), | |
| "uncertain": round(float(counts[UNCERTAIN]), 2)}, | |
| }) | |
| return {"status": "measured", "source": "depth", "creases": [], "faces": faces, | |
| "horizontal_planes": {k: {"rms_mm": v["rms_mm"], "points": v["points"]} for k, v in horizontal.items()}, | |
| "assumption": None} | |