feat(right_arm): force, camera↔polso offset, cluster filter sul centroide
Tre miglioramenti al move-toward dopo il primo uso live:
1. checkbox "force" accanto a "Muovi": bypassa max_step_m quando
l'utente sa che il delta richiesto e' grande ma corretto (es. primo
avvicinamento all'oggetto).
2. camera_offset_wrist_mm: il modello assumeva camera e TCP coincidenti
(sole sign flips). La camera Nuwa e' invece montata con traslazione
costante rispetto al polso. La formula diventa:
delta_wrist = sign * (centroid_cam - offset_cam) + t_wrist_cam
Esposto in config (default 0,0,0), GUI (3 input "Camera rispetto a
polso (mm)") e in /state. Il termine compensa il fatto che muovere
il polso porta la camera, non il TCP, sul punto di presa.
3. filter_nearest_cluster: il centroide era media di tutti i punti
SAM21 della mask. In presenza di sfondo lontano visibile attraverso
buchi della depth del primo piano la media veniva trascinata a
metri di distanza. Ora si tiene il cluster piu' vicino:
z_near = p5(Z)
keep = punti con Z <= z_near + cluster_depth_window_m
Default window 0.20 m, configurabile. La risposta di /detect/wrist
espone n_sam21_points, n_centroid_points e z_near/z_cutoff per
diagnosi.
Aggiornata anche la persistenza: /offset accetta camera_offset_wrist_mm.
GUI: i campi Offset/Sign/CamOff rispettano "user touched" (refresh non
li cancella mentre l'utente digita).
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
1 parent
fcd921ff32
commit
1fa7d12ad7
8 files changed
+260
-20
No files matched your search
@@ -0,0 +1,56 @@
|
||||
"""Unit test del filtro cluster sui punti SAM21."""
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
from presa_mano.right_arm.cluster import filter_nearest_cluster
|
||||
|
||||
|
||||
def test_empty_input_returns_empty():
|
||||
out, diag = filter_nearest_cluster(np.zeros((0, 3)), window_m=0.2)
|
||||
assert out.shape == (0, 3)
|
||||
assert diag["n_input"] == 0 and diag["n_kept"] == 0
|
||||
assert diag["z_near_m"] is None
|
||||
|
||||
|
||||
def test_bimodal_keeps_only_near_cluster():
|
||||
# 100 punti vicini (Z ~0.25 m) + 50 background lontano (Z ~4.0 m).
|
||||
near = np.column_stack([np.zeros(100), np.zeros(100),
|
||||
np.random.uniform(0.22, 0.28, 100)])
|
||||
far = np.column_stack([np.zeros(50), np.zeros(50),
|
||||
np.random.uniform(3.8, 4.2, 50)])
|
||||
pts = np.vstack([near, far])
|
||||
out, diag = filter_nearest_cluster(pts, window_m=0.20)
|
||||
# Tutti i punti vicini, nessun punto lontano
|
||||
assert diag["n_kept"] == 100
|
||||
assert out[:, 2].max() < 0.30
|
||||
assert diag["z_near_m"] < 0.25 # p5 ~= 0.22
|
||||
|
||||
|
||||
def test_single_cluster_keeps_all():
|
||||
# Unica nuvola compatta: tutto rientra nella window.
|
||||
pts = np.column_stack([np.zeros(100), np.zeros(100),
|
||||
np.random.uniform(0.20, 0.30, 100)])
|
||||
out, diag = filter_nearest_cluster(pts, window_m=0.20)
|
||||
assert diag["n_kept"] == 100
|
||||
|
||||
|
||||
def test_window_too_small_keeps_only_nearest():
|
||||
pts = np.array([
|
||||
[0.0, 0.0, 0.20],
|
||||
[0.0, 0.0, 0.30],
|
||||
[0.0, 0.0, 0.40],
|
||||
[0.0, 0.0, 4.00],
|
||||
])
|
||||
out, diag = filter_nearest_cluster(pts, window_m=0.05)
|
||||
# z_near = p5 di [0.20,0.30,0.40,4.00] ≈ 0.215 → cutoff ≈ 0.265 → solo 0.20
|
||||
assert out.shape[0] == 1
|
||||
assert out[0, 2] == 0.20
|
||||
|
||||
|
||||
def test_diag_reports_inputs_and_kept():
|
||||
pts = np.array([[0.0, 0.0, 0.25]] * 20 + [[0.0, 0.0, 4.0]] * 5)
|
||||
out, diag = filter_nearest_cluster(pts, window_m=0.10)
|
||||
assert diag["n_input"] == 25
|
||||
assert diag["n_kept"] == 20
|
||||
assert diag["z_cutoff_m"] == diag["z_near_m"] + 0.10
|
||||
Reference in new issue
Block a user