"""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}