File size: 18,394 Bytes
bf88271
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
 
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
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
"""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}