rpx_benchmark.metrics.depth_robotics

  1"""Robotics-deployment depth metric calculators.
  2
  3Camera-frame Chamfer-L1 + grasp F-score, surface normals (MAE, %11.25°),
  4and scale-invariant boundary F1.  All are **pose-free** (intrinsics
  5only) — they measure shape/reachability, not pose drift.
  6"""
  7
  8from __future__ import annotations
  9
 10from typing import Dict
 11
 12import numpy as np
 13
 14from ..api import DepthGroundTruth, DepthPrediction, TaskType
 15from ..exceptions import MetricError
 16from .registry import MetricCalculator, register_metric
 17
 18# D435 intrinsics at 640×480
 19_FX: float = 615.0
 20_FY: float = 615.0
 21_CX: float = 320.0
 22_CY: float = 240.0
 23
 24# Max points for Chamfer/F-score (computational tractability)
 25_MAX_POINTS: int = 50_000
 26
 27
 28def _backproject(depth: np.ndarray, valid: np.ndarray) -> np.ndarray:
 29    """Backproject valid depth pixels to 3D in camera frame.
 30
 31    Returns (N, 3) float64 array of 3D points.
 32    """
 33    h, w = depth.shape
 34    u, v = np.meshgrid(np.arange(w, dtype=np.float64),
 35                        np.arange(h, dtype=np.float64))
 36    z = depth[valid].astype(np.float64)
 37    x = (u[valid] - _CX) * z / _FX
 38    y = (v[valid] - _CY) * z / _FY
 39    return np.stack([x, y, z], axis=-1)
 40
 41
 42def _subsample(pts: np.ndarray, max_n: int, rng: np.random.Generator) -> np.ndarray:
 43    if len(pts) <= max_n:
 44        return pts
 45    idx = rng.choice(len(pts), size=max_n, replace=False)
 46    return pts[idx]
 47
 48
 49def _nearest_distances(a: np.ndarray, b: np.ndarray, *, p: int = 2) -> np.ndarray:
 50    """Exact nearest-neighbour distances without quadratic temporary arrays."""
 51    from scipy.spatial import cKDTree
 52
 53    return cKDTree(b).query(a, k=1, p=p, workers=1)[0]
 54
 55
 56def _chamfer_l1(a: np.ndarray, b: np.ndarray) -> float:
 57    """Symmetric mean nearest-neighbour L1 distance (sum of both directions)."""
 58    return float(_nearest_distances(a, b, p=1).mean() + _nearest_distances(b, a, p=1).mean())
 59
 60
 61def _fscore(a: np.ndarray, b: np.ndarray, threshold: float) -> float:
 62    """Harmonic mean of bidirectional Euclidean matches below the threshold."""
 63    precision = float(np.mean(_nearest_distances(a, b) < threshold))
 64    recall = float(np.mean(_nearest_distances(b, a) < threshold))
 65    return 0.0 if precision + recall < 1e-12 else 2 * precision * recall / (precision + recall)
 66
 67
 68@register_metric(TaskType.MONOCULAR_DEPTH)
 69class DepthGrasp3D(MetricCalculator):
 70    """Camera-frame Chamfer-L1 and grasp F-score @ {1, 2, 5, 10 cm}.
 71
 72    Back-projects predicted and GT depth to 3D using D435 intrinsics
 73    only (no camera pose) — measures shape/reachability, not pose drift.
 74    """
 75
 76    name = "depth_grasp_3d"
 77
 78    def compute(
 79        self,
 80        prediction: DepthPrediction,
 81        ground_truth: DepthGroundTruth,
 82    ) -> Dict[str, float]:
 83        if not isinstance(prediction, DepthPrediction):
 84            raise MetricError(
 85                f"DepthGrasp3D expected DepthPrediction, got {type(prediction).__name__}",
 86            )
 87        if not isinstance(ground_truth, DepthGroundTruth):
 88            raise MetricError(
 89                f"DepthGrasp3D expected DepthGroundTruth, got {type(ground_truth).__name__}",
 90            )
 91
 92        pred = np.asarray(prediction.depth_map, dtype=np.float32)
 93        gt = np.asarray(ground_truth.depth_map, dtype=np.float32)
 94
 95        from .depth_alignment import default_valid_mask
 96
 97        valid = default_valid_mask(pred, gt)
 98        if valid.sum() < 10:
 99            return {
100                "chamfer_l1": float("nan"),
101                "fscore_1cm": 0.0,
102                "fscore_2cm": 0.0,
103                "fscore_5cm": 0.0,
104                "fscore_10cm": 0.0,
105            }
106
107        # Use independent RNGs so identical pred/gt get the same subset
108        rng_pred = np.random.default_rng(42)
109        rng_gt = np.random.default_rng(42)
110        pts_pred = _subsample(_backproject(pred, valid), _MAX_POINTS, rng_pred)
111        pts_gt = _subsample(_backproject(gt, valid), _MAX_POINTS, rng_gt)
112
113        chamfer = _chamfer_l1(pts_pred, pts_gt)
114
115        return {
116            "chamfer_l1": chamfer,
117            "fscore_1cm": _fscore(pts_pred, pts_gt, 0.01),
118            "fscore_2cm": _fscore(pts_pred, pts_gt, 0.02),
119            "fscore_5cm": _fscore(pts_pred, pts_gt, 0.05),
120            "fscore_10cm": _fscore(pts_pred, pts_gt, 0.10),
121        }
122
123
124# --------------------------------------------------------------------------- #
125# Surface normals from depth
126# --------------------------------------------------------------------------- #
127
128def _depth_to_normals(depth: np.ndarray, valid: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
129    """Compute surface normals from depth via finite differences + backprojection.
130
131    Returns (H, W, 3) normals and (H, W) bool mask of pixels where
132    normals are reliable (both neighbours valid and away from holes).
133    """
134    h, w = depth.shape
135    z = depth.astype(np.float64)
136
137    # Backproject to 3D (per-pixel camera-frame positions)
138    u, v = np.meshgrid(np.arange(w, dtype=np.float64),
139                        np.arange(h, dtype=np.float64))
140    x = (u - _CX) * z / _FX
141    y = (v - _CY) * z / _FY
142
143    # Finite-difference tangent vectors (central where possible)
144    dx = np.zeros((h, w, 3), dtype=np.float64)
145    dy = np.zeros((h, w, 3), dtype=np.float64)
146
147    dx[:, 1:-1, 0] = x[:, 2:] - x[:, :-2]
148    dx[:, 1:-1, 1] = y[:, 2:] - y[:, :-2]
149    dx[:, 1:-1, 2] = z[:, 2:] - z[:, :-2]
150
151    dy[1:-1, :, 0] = x[2:, :] - x[:-2, :]
152    dy[1:-1, :, 1] = y[2:, :] - y[:-2, :]
153    dy[1:-1, :, 2] = z[2:, :] - z[:-2, :]
154
155    normals = np.cross(dx, dy)
156    norm = np.linalg.norm(normals, axis=-1, keepdims=True)
157    norm = np.where(norm > 1e-8, norm, 1e-8)
158    normals = normals / norm
159
160    # Mask: valid depth at this pixel and all 4 finite-diff neighbours;
161    # exclude 1px border and k=2px around holes.
162    neighbour_valid = (
163        valid[1:-1, 1:-1]
164        & valid[:-2, 1:-1] & valid[2:, 1:-1]
165        & valid[1:-1, :-2] & valid[1:-1, 2:]
166    )
167    normal_valid = np.zeros((h, w), dtype=bool)
168    normal_valid[1:-1, 1:-1] = neighbour_valid
169
170    return normals, normal_valid
171
172
173@register_metric(TaskType.MONOCULAR_DEPTH)
174class DepthNormals(MetricCalculator):
175    """Surface normal MAE and %11.25° from depth-derived normals.
176
177    Normals are derived via finite differences on the back-projected
178    3D positions (intrinsics only, no pose). Pixels near depth holes
179    and image borders are masked out to avoid amplifying D435 noise.
180    """
181
182    name = "depth_normals"
183
184    def compute(
185        self,
186        prediction: DepthPrediction,
187        ground_truth: DepthGroundTruth,
188    ) -> Dict[str, float]:
189        if not isinstance(prediction, DepthPrediction):
190            raise MetricError(
191                f"DepthNormals expected DepthPrediction, got {type(prediction).__name__}",
192            )
193        if not isinstance(ground_truth, DepthGroundTruth):
194            raise MetricError(
195                f"DepthNormals expected DepthGroundTruth, got {type(ground_truth).__name__}",
196            )
197
198        pred = np.asarray(prediction.depth_map, dtype=np.float32)
199        gt = np.asarray(ground_truth.depth_map, dtype=np.float32)
200
201        from .depth_alignment import default_valid_mask
202
203        valid = default_valid_mask(pred, gt)
204
205        pred_normals, pred_nvalid = _depth_to_normals(pred, valid)
206        gt_normals, gt_nvalid = _depth_to_normals(gt, valid)
207
208        both_valid = pred_nvalid & gt_nvalid
209        if both_valid.sum() < 10:
210            return {"normal_mae": float("nan"), "normal_acc1125": 0.0}
211
212        # Cosine similarity → angular error (degrees)
213        cos_sim = np.sum(
214            pred_normals[both_valid] * gt_normals[both_valid], axis=-1
215        )
216        cos_sim = np.clip(cos_sim, -1.0, 1.0)
217        angles_deg = np.degrees(np.arccos(np.abs(cos_sim)))  # abs: sign ambiguity
218
219        normal_mae = float(np.mean(angles_deg))
220        normal_acc1125 = float(np.mean(angles_deg < 11.25))
221
222        return {
223            "normal_mae": normal_mae,
224            "normal_acc1125": normal_acc1125,
225        }
226
227
228# --------------------------------------------------------------------------- #
229# Scale-invariant boundary F1
230# --------------------------------------------------------------------------- #
231
232def _depth_ratio_contours(depth: np.ndarray, threshold_pct: float) -> np.ndarray:
233    """Binary occluding-contour map from depth ratios.
234
235    A pixel (i,j) is an occluding contour if any 4-connected neighbour
236    satisfies d(neighbour)/d(pixel) > 1 + threshold_pct/100.
237    """
238    t = 1.0 + threshold_pct / 100.0
239    h, w = depth.shape
240    contour = np.zeros((h, w), dtype=bool)
241
242    for dy, dx in [(0, 1), (0, -1), (1, 0), (-1, 0)]:
243        sy = slice(max(0, dy), h + min(0, dy))
244        sx = slice(max(0, dx), w + min(0, dx))
245        ny = slice(max(0, -dy), h + min(0, -dy))
246        nx = slice(max(0, -dx), w + min(0, -dx))
247        d_self = depth[sy, sx]
248        d_neigh = depth[ny, nx]
249        valid_pair = (d_self > 1e-6) & (d_neigh > 1e-6)
250        ratio = np.ones_like(d_self)
251        ratio[valid_pair] = d_neigh[valid_pair] / d_self[valid_pair]
252        contour[sy, sx] |= (ratio > t) & valid_pair
253
254    return contour
255
256
257@register_metric(TaskType.MONOCULAR_DEPTH)
258class DepthBoundaryF1(MetricCalculator):
259    """Scale-invariant occluding-contour F1 from depth ratios.
260
261    Averages F1 over thresholds t ∈ {5, 10, 15, 20, 25} (percent).
262    No GT boundary annotations needed — contours are derived from
263    depth ratios on both predicted and GT depth maps.
264    """
265
266    name = "depth_boundary_f1"
267
268    _thresholds = (5, 10, 15, 20, 25)
269
270    def compute(
271        self,
272        prediction: DepthPrediction,
273        ground_truth: DepthGroundTruth,
274    ) -> Dict[str, float]:
275        if not isinstance(prediction, DepthPrediction):
276            raise MetricError(
277                f"DepthBoundaryF1 expected DepthPrediction, got {type(prediction).__name__}",
278            )
279        if not isinstance(ground_truth, DepthGroundTruth):
280            raise MetricError(
281                f"DepthBoundaryF1 expected DepthGroundTruth, got {type(ground_truth).__name__}",
282            )
283
284        pred = np.asarray(prediction.depth_map, dtype=np.float32)
285        gt = np.asarray(ground_truth.depth_map, dtype=np.float32)
286
287        f1_scores = []
288        for t in self._thresholds:
289            pred_c = _depth_ratio_contours(pred, t)
290            gt_c = _depth_ratio_contours(gt, t)
291
292            tp = float((pred_c & gt_c).sum())
293            fp = float((pred_c & ~gt_c).sum())
294            fn = float((~pred_c & gt_c).sum())
295
296            if tp + fp < 1e-6 and tp + fn < 1e-6:
297                # No contours in either → perfect agreement
298                f1_scores.append(1.0)
299            elif tp < 1e-6:
300                f1_scores.append(0.0)
301            else:
302                prec = tp / (tp + fp)
303                rec = tp / (tp + fn)
304                f1_scores.append(2 * prec * rec / (prec + rec))
305
306        return {"boundary_f1": float(np.mean(f1_scores))}