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
1 parent
f1f1277755
commit
8bfdfb194a
4 files changed
+239
-26
No files matched your search
@@ -193,6 +193,14 @@ frontend Nuwa, ma su D4xx significano:
|
||||
- `rgb` = intrinseci del **sensore color**
|
||||
- `extrinsics_ir_to_rgb` = trasformazione **depth → color** (translation in mm)
|
||||
|
||||
**Quali intrinseci usa la back-projection** (`/api/measure`, `/api/pointcloud`,
|
||||
`/api/pointcloud/roi`): dipende dall'allineamento depth→color (vedi
|
||||
[bug intrinsics mismatch](#punti-3d-stirati-del-54-su-xy-intrinsics-mismatch)).
|
||||
Su USB 3 (default) la depth è allineata al frame color, quindi si usano gli
|
||||
intrinseci **`rgb`**; su USB 2 (no alignment) si usano gli **`ir`**. La scelta
|
||||
è centralizzata in `_backprojection_intrinsics()` e il campo `intrinsics` nella
|
||||
risposta riporta sempre quelli effettivamente usati.
|
||||
|
||||
---
|
||||
|
||||
## Configurazione (`.env`)
|
||||
@@ -333,6 +341,62 @@ Su G1 questo significa quasi sempre **una di due cose**:
|
||||
Se invece stai lavorando sul repo H1, su USB 2 questo errore è quasi
|
||||
sempre un mismatch di profilo color (BGR8 vs YUYV) — vedi STATUS.md.
|
||||
|
||||
### Punti 3D stirati del ~54% su XY (intrinsics mismatch)
|
||||
|
||||
Riguarda `/api/measure`, `/api/pointcloud`, `/api/pointcloud/roi`.
|
||||
|
||||
**Sintomo.** Le coordinate `(X, Y, Z)` restituite dalla back-projection sono
|
||||
gonfiate lateralmente di un fattore ~1.54 su X e Y, mentre Z (depth) è corretto.
|
||||
Un marker ArUco da 103 mm a ~0.73 m veniva misurato ~157 mm; i point cloud del
|
||||
D435i non si sovrapponevano a quelli di altre camere calibrate. Diagnosticato e
|
||||
fixato il 2026-05-27 (vedi `bug_d435i_intrinsics_mismatch.md`).
|
||||
|
||||
**Root cause.** La pipeline allinea la depth al frame **color** con
|
||||
`rs.align(rs.stream.color)` ([`services/camera.py`](src/backend/services/camera.py)),
|
||||
quindi dopo l'alignment la depth vive sulla **griglia di pixel del color**. Ma
|
||||
i tre endpoint di back-projection usavano gli intrinseci del **sensore depth**
|
||||
(`fx_ir`, `cx_ir`, …) per de-proiettare quei pixel. Risultato: X e Y inflati
|
||||
esattamente del rapporto delle focali
|
||||
|
||||
```
|
||||
fx_rgb / fx_ir = 603.88 / 392.31 ≈ 1.539
|
||||
```
|
||||
|
||||
Z resta corretto perché dipende solo dal valore di depth letto al pixel, non
|
||||
dagli intrinseci. Verifica numerica sul pixel `(377, 68)`, `z = 0.732 m`:
|
||||
|
||||
```
|
||||
IR (errato): X = (377 − 318.48) × 0.732 / 392.31 = 0.1092 m ← valore restituito (buggy)
|
||||
RGB (corretto): X = (377 − 320.62) × 0.732 / 603.88 = 0.0683 m ← valore atteso
|
||||
```
|
||||
|
||||
**Fix.** La scelta degli intrinseci è ora condizionale all'allineamento, in un
|
||||
unico helper `_backprojection_intrinsics()` ([`api/routes.py`](src/backend/api/routes.py))
|
||||
usato da `/api/measure`, `/api/pointcloud`, `/api/pointcloud/roi`:
|
||||
|
||||
- depth allineata al color (USB 3, default) → intrinseci **`rgb`**;
|
||||
- nessun alignment (fallback USB 2, dove il color è invece *resize*-ato sulla
|
||||
griglia depth) → intrinseci **`ir`** (comportamento corretto per quel ramo).
|
||||
|
||||
Lo stato di allineamento è esposto dal metodo `is_depth_aligned_to_color()`
|
||||
(`= self._align is not None`) su `RealSenseD435`. Un fix incondizionato IR→RGB
|
||||
avrebbe rotto il percorso USB 2 in modo speculare: per questo è condizionale.
|
||||
|
||||
**Verifica di non-regressione.** Dopo il fix, sul pixel `(377, 68)`:
|
||||
|
||||
```bash
|
||||
curl -sS "http://localhost:8878/api/measure?u=377&v=68&radius=8"
|
||||
# point_m.x ≈ +0.069 (RGB, corretto) — non più +0.109 (IR, buggy)
|
||||
```
|
||||
|
||||
In generale, per ogni pixel `(u, v)` con depth `z` deve valere
|
||||
`x ≈ (u − cx_rgb)·z / fx_rgb` (entro la tolleranza di distorsione), e un marker
|
||||
di lato noto back-proiettato deve dare lo stesso lato ±5 mm a 0.7 m.
|
||||
|
||||
> **Nota lato client.** Il workaround temporaneo nel viewport 3D di presa-mano
|
||||
> (campo `D435i scale XY` con default `0.65 = fx_ir/fx_rgb`) va riportato a
|
||||
> `1.0` ora che il server è corretto.
|
||||
|
||||
### `RuntimeError: Failed to resolve request`
|
||||
|
||||
Pipeline non riesce a fare match tra config richiesta e device. Su G1
|
||||
|
||||
@@ -0,0 +1,144 @@
|
||||
# Bug report — D435i camera service: intrinsics mismatch nel back-projection 3D
|
||||
|
||||
**Servizio:** `front_camera` (RealSense D435i) — `http://192.168.123.99:8878`
|
||||
**Severità:** alta — punti 3D ritornati con XY sovrastimati di ~54%
|
||||
**Endpoint affetti:** almeno `/api/measure`; molto probabilmente anche `/api/pointcloud` e `/api/pointcloud/roi`
|
||||
**Endpoint NON affetti:** `/api/intrinsics` (espone i parametri corretti)
|
||||
**Data test:** 2026-05-27
|
||||
**Reporter:** sviluppo presa-mano
|
||||
|
||||
---
|
||||
|
||||
## 1. Sintomo
|
||||
|
||||
Le coordinate 3D `(X, Y, Z)` ritornate dal back-projection lato server sono **stirate** lateralmente di un fattore **~1.54** sugli assi X e Y, mentre Z (depth) è plausibile.
|
||||
|
||||
Conseguenza: oggetti di dimensioni note appaiono ~50% più grandi del reale in qualsiasi misurazione 3D, e i point cloud non si sovrappongono ad altre camere correttamente calibrate.
|
||||
|
||||
---
|
||||
|
||||
## 2. Test di verifica
|
||||
|
||||
Marker ArUco fisico di **lato 103.0 mm** (`id=20`, dictionary DICT_4X4_50) posizionato davanti al D435i a ~735 mm.
|
||||
|
||||
### 2.1 Intrinsics riportate dal servizio (`GET /api/intrinsics`)
|
||||
|
||||
```json
|
||||
{
|
||||
"ir": { "fx": 392.30963134765625, "fy": 392.30963134765625,
|
||||
"cx": 318.48004150390625, "cy": 242.43441772460938 },
|
||||
"rgb": { "fx": 603.88037109375, "fy": 603.6130981445312,
|
||||
"cx": 320.6182556152344, "cy": 250.35804748535156 },
|
||||
"extrinsics_ir_to_rgb": { "rotation": [...], "translation": [14.68, -0.07, 0.29] }
|
||||
}
|
||||
```
|
||||
|
||||
### 2.2 Detect del marker (su RGB image del D435i)
|
||||
|
||||
Marker-finder ritorna le 4 corner del tag nel sistema **pixel del frame RGB**:
|
||||
|
||||
```
|
||||
corners (RGB px): [(377, 68), (372, 152), (289, 147), (294, 63)]
|
||||
```
|
||||
|
||||
### 2.3 `GET /api/measure?u=&v=&radius=3` per ciascuna corner
|
||||
|
||||
```
|
||||
pixel (377, 68) → x=+0.1092 y=-0.3255 z=0.732 m
|
||||
pixel (372, 152) → x=+0.0999 y=-0.1687 z=0.732 m
|
||||
pixel (289, 147) → x=-0.0554 y=-0.1793 z=0.737 m
|
||||
pixel (294, 63) → x=-0.0461 y=-0.3375 z=0.738 m
|
||||
```
|
||||
|
||||
### 2.4 Misurazioni geometriche dai punti 3D ritornati
|
||||
|
||||
| Misura | Atteso | Ottenuto | Errore |
|
||||
|-----------------|---------------|---------------|----------|
|
||||
| Lato C0–C1 | 103.00 mm | 157.13 mm | **+52.6%** |
|
||||
| Lato C1–C2 | 103.00 mm | 155.71 mm | **+51.2%** |
|
||||
| Lato C2–C3 | 103.00 mm | 158.48 mm | **+53.9%** |
|
||||
| Lato C3–C0 | 103.00 mm | 155.92 mm | **+51.4%** |
|
||||
| Diagonale 0–2 | 145.66 mm | 220.19 mm | **+51.2%** |
|
||||
| Diagonale 1–3 | 145.66 mm | 223.32 mm | **+53.3%** |
|
||||
| Z medio (depth) | — | 735 mm | plausibile |
|
||||
|
||||
L'errore è **omogeneo ~52%** su tutte le coppie. Z (depth) è coerente — l'errore è confinato a X, Y.
|
||||
|
||||
---
|
||||
|
||||
## 3. Diagnosi (root cause)
|
||||
|
||||
Il rapporto fra le focali è esattamente:
|
||||
|
||||
```
|
||||
fx_rgb / fx_ir = 603.88037 / 392.30963 = 1.5393
|
||||
```
|
||||
|
||||
L'errore osservato sui lati è **1.524 ± 0.014**.
|
||||
|
||||
Verifica numerica della corner `(u=377, v=68)`, depth `z=0.732 m`:
|
||||
|
||||
```
|
||||
Con RGB intrinsics (corretto): X = (377 − 320.62) × 0.732 / 603.88 = 0.0683 m
|
||||
Con IR intrinsics (errato): X = (377 − 318.48) × 0.732 / 392.31 = 0.1091 m
|
||||
Server ritorna: = 0.1092 m ← coincide con IR
|
||||
```
|
||||
|
||||
**Conclusione:** il servizio sta usando le **intrinsics IR** (`fx=392.31, cx=318.48`) per back-projettare pixel che sono in **coordinate RGB** (allineamento depth-to-color attivo). Il risultato è X e Y inflati di `fx_rgb/fx_ir ≈ 1.54×`.
|
||||
|
||||
Il Z è corretto perché dipende solo dal valore di depth letto al pixel, non dalle intrinsics.
|
||||
|
||||
---
|
||||
|
||||
## 4. Fix consigliato
|
||||
|
||||
Nella routine di back-projection (probabilmente in `/api/measure`, `/api/pointcloud`, `/api/pointcloud/roi`):
|
||||
|
||||
```python
|
||||
# ERRATO (sospetto attuale):
|
||||
X = (u - cx_ir) * z / fx_ir
|
||||
Y = (v - cy_ir) * z / fy_ir
|
||||
|
||||
# CORRETTO (depth allineata al frame color → usa RGB intrinsics):
|
||||
X = (u - cx_rgb) * z / fx_rgb
|
||||
Y = (v - cy_rgb) * z / fy_rgb
|
||||
```
|
||||
|
||||
Se la pipeline NON sta facendo `align_to(color)` sul depth stream, allora il fix opposto: i pixel passati devono essere quelli del frame IR e si usano le intrinsics IR. Ma la semantica attuale (`/api/rgb` espone il frame color, `/api/measure` accetta `(u,v)` con range coerente col color frame ~640×480) suggerisce che l'API è progettata per RGB-aligned depth — quindi è il back-projection lato server che ha l'intrinsics sbagliata.
|
||||
|
||||
---
|
||||
|
||||
## 5. Test di non-regressione consigliati
|
||||
|
||||
Dopo il fix, verificare che:
|
||||
|
||||
1. **Tag di dimensione nota** (es. 103mm): le 4 corner back-proiettate formano un quadrato con lato `103.00 ± 5 mm` a 0.7m di distanza.
|
||||
2. **Coerenza con `/api/intrinsics`**: per ogni pixel `(u, v)` con depth `z`, vale:
|
||||
```
|
||||
x_returned ≈ (u - cx_rgb) * z / fx_rgb (entro tolleranza distorsione)
|
||||
y_returned ≈ (v - cy_rgb) * z / fy_rgb
|
||||
```
|
||||
3. **Cross-camera consistency**: il point cloud del D435i di un oggetto sovrapposto al cloud Nuwa (eye-in-hand polso DX) dello stesso oggetto deve avere errore residuo `< 30 mm` dopo hand-eye calibration.
|
||||
|
||||
---
|
||||
|
||||
## 6. Workaround temporaneo (lato client)
|
||||
|
||||
Nel viewport 3D di presa-mano è stato aggiunto un campo `D435i scale XY` con default **0.65** (`= fx_ir / fx_rgb`) che riscala il cloud per compensare il bug. Da rimuovere/portare a 1.0 quando il server è fixato.
|
||||
|
||||
---
|
||||
|
||||
## 7. Comandi per riprodurre
|
||||
|
||||
```bash
|
||||
# 1) Verifica intrinsics
|
||||
curl -sS http://192.168.123.99:8878/api/intrinsics
|
||||
|
||||
# 2) Detect marker (presuppone marker-finder a :8082 e tag 20 visibile)
|
||||
curl -sS -X POST http://localhost:8082/detect \
|
||||
-F "image_url=http://192.168.123.99:8878/api/rgb" \
|
||||
-F 'markers=[{"id":20,"size_mm":103}]'
|
||||
|
||||
# 3) Per ogni corner (u,v) ritornata, misura 3D
|
||||
curl -sS "http://192.168.123.99:8878/api/measure?u=377&v=68&radius=3"
|
||||
```
|
||||
+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