fix: use color intrinsics for back-projection when depth is aligned

The depth stream is aligned to the color frame via rs.align(rs.stream.color),
so the depth map lives on the color pixel grid. /api/measure, /api/pointcloud
and /api/pointcloud/roi were de-projecting those pixels with the *depth* (ir)
intrinsics, inflating X/Y by fx_rgb/fx_ir (~1.54x on the D435i). Z was correct
since it only depends on the depth value, not the intrinsics.

A 103mm ArUco marker at ~0.73m measured ~157mm; back-projected point clouds did
not overlap other calibrated cameras.

Fix: centralize intrinsics selection in _backprojection_intrinsics(), keyed on
the new RealSenseD435.is_depth_aligned_to_color(). Aligned (USB 3 default) ->
rgb intrinsics; no alignment (USB 2 fallback, where color is resized onto the
depth grid) -> ir intrinsics. An unconditional ir->rgb swap would have broken
the USB 2 path symmetrically.

Verified live: /api/measure?u=377&v=68 now returns x=+0.0686 (rgb) instead of
+0.1092 (ir). Documented root cause + non-regression check in README and
bug_d435i_intrinsics_mismatch.md.

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
henrytherobotandClaude Opus 4.7 committed 2026-05-27 16:29:30 +02:00
1 parent f1f1277755
commit 8bfdfb194a
4 files changed
+239 -26

No files matched your search

+21 -26
View File
@@ -117,6 +117,24 @@ def _calibration_payload() -> dict | None:
}
def _backprojection_intrinsics() -> tuple[float, float, float, float]:
"""(fx, fy, cx, cy) for back-projecting a depth pixel to 3D.
Picks the intrinsics that match the grid the depth map lives on:
- depth aligned to color (USB 3 default) -> color (rgb) intrinsics
- no alignment (USB 2 fallback) -> depth (ir) intrinsics
Using the ir intrinsics on an aligned (color-grid) depth map inflates
X/Y by fx_rgb/fx_ir (~1.54x on the D435i).
"""
if _camera and _camera.intrinsics:
i = _camera.intrinsics
if _camera.is_depth_aligned_to_color():
return i.fx_rgb, i.fy_rgb, i.cx_rgb, i.cy_rgb
return i.fx_ir, i.fy_ir, i.cx_ir, i.cy_ir
# Default intrinsics for 640x480 when calibration is unavailable.
return 445.0, 445.0, 320.0, 240.0
@router.get("/status")
async def status():
"""Camera status and intrinsics."""
@@ -267,16 +285,7 @@ async def get_pointcloud(
depth = frame.depth
rgb = frame.rgb # BGR format
# Use intrinsics if available, otherwise use typical HP60C defaults
if _camera.intrinsics:
fx = _camera.intrinsics.fx_ir
fy = _camera.intrinsics.fy_ir
cx = _camera.intrinsics.cx_ir
cy = _camera.intrinsics.cy_ir
else:
# Default intrinsics for 640x480 HP60C
fx, fy = 445.0, 445.0
cx, cy = 320.0, 240.0
fx, fy, cx, cy = _backprojection_intrinsics()
h, w = depth.shape
points = []
@@ -338,14 +347,7 @@ async def get_pointcloud_roi(
if u1 <= u0 or v1 <= v0:
return fail("INVALID_ROI", f"Empty ROI ({u0},{v0})-({u1},{v1}) in {w}x{h}")
if _camera.intrinsics:
fx = _camera.intrinsics.fx_ir
fy = _camera.intrinsics.fy_ir
cx = _camera.intrinsics.cx_ir
cy = _camera.intrinsics.cy_ir
else:
fx, fy = 445.0, 445.0
cx, cy = 320.0, 240.0
fx, fy, cx, cy = _backprojection_intrinsics()
# Vectorized: restrict both arrays to the ROI and filter by depth mask.
roi_depth = depth[v0:v1:step, u0:u1:step].astype(np.float32)
@@ -431,14 +433,7 @@ async def measure_point(
v_found = int(abs_ys[idx])
z_mm = int(depth[v_found, u_found])
if _camera.intrinsics:
fx = _camera.intrinsics.fx_ir
fy = _camera.intrinsics.fy_ir
cx = _camera.intrinsics.cx_ir
cy = _camera.intrinsics.cy_ir
else:
fx, fy = 445.0, 445.0
cx, cy = 320.0, 240.0
fx, fy, cx, cy = _backprojection_intrinsics()
z_m = z_mm / 1000.0
x_m = (u_found - cx) * z_m / fx
+10
View File
@@ -132,6 +132,16 @@ class RealSenseD435:
"""False when the pipeline runs depth-only (USB 2.x fallback)."""
return self._color_available
def is_depth_aligned_to_color(self) -> bool:
"""True when depth is hardware-aligned to the color frame.
When aligned (USB 3 default), the depth map lives on the **color**
pixel grid, so back-projecting a pixel must use the color (rgb)
intrinsics. On USB 2 the align step is skipped and color is resized
onto the depth grid instead, so the depth (ir) intrinsics apply.
"""
return self._align is not None
def usb_type(self) -> str:
return self._usb_type