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:
1 parent
f1f1277755
commit
8bfdfb194a
4 files changed
+239
-26
No files matched your search
+21
-26
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
Reference in new issue
Block a user