diff --git a/web/pose_3d.js b/web/pose_3d.js index 1e40d14..6bedc7e 100644 --- a/web/pose_3d.js +++ b/web/pose_3d.js @@ -222,6 +222,7 @@ const state = { joint_ucses: [], // array di THREE.Group, UCS per ogni link URDF (debug) clouds_cfg: null, // {nuwa:{...}, d435:{...}} dal config cloud_points: { nuwa: null, d435: null }, // THREE.Points obj per camera + align: { enabled: false, id: 0, matrix: null /* THREE.Matrix4 */ }, tag_locator: { // sezione "Tag locator" nella sidebar Pose 3D marker: null, // THREE.LineSegments (crocetta XYZ) o null position_m: null, // [x,y,z] in pelvis frame della crocetta corrente @@ -1605,8 +1606,12 @@ function buildOrUpdateCloud(camKey, pointsFlat) { `Y[${ymin.toFixed(2)},${ymax.toFixed(2)}]`, `Z[${zmin.toFixed(2)},${zmax.toFixed(2)}]`); // (Ri-)attacca al parent corretto e applica transform optical→link - if (pts.parent !== parent) parent.add(pts); - applyCloudTransform(camKey); + if (camKey === 'nuwa') { + applyNuwaCloudPlacement(); + } else { + if (pts.parent !== parent) parent.add(pts); + applyCloudTransform(camKey); + } applyCloudVisibility(); return pts; } @@ -1621,6 +1626,24 @@ function applyCloudVisibility() { // Applica position+rpy+mirror sui punti del cloud `camKey`. Il mirror (sign // flip per asse) avviene IN FRAME OTTICO grazie all'ordine T·R·S di Three.js // (lo scale e' la prima operazione applicata al punto locale). +// Matrix4 locale (rispetto al parent getCloudParent('d435')) usata dal cloud +// D435i: hand-eye (M·S) oppure URDF (T·R·S). Riusata per piazzare la Nuwa +// allineata nello stesso frame del D435i. +function d435LocalMatrix() { + const sv = readCloudScaleVec('d435'); + const S = new THREE.Matrix4().makeScale(sv[0], sv[1], sv[2]); + if (useD435Handeye()) { + const M = getD435HandeyeMatrix(); + if (M) return M.clone().multiply(S); + } + const t = getCloudTransform('d435'); + const T = new THREE.Matrix4().makeTranslation(t.offset_m[0], t.offset_m[1], t.offset_m[2]); + const e = new THREE.Euler( + t.rpy_deg[0]*Math.PI/180, t.rpy_deg[1]*Math.PI/180, t.rpy_deg[2]*Math.PI/180, 'XYZ'); + const R = new THREE.Matrix4().makeRotationFromEuler(e); + return T.multiply(R).multiply(S); +} + function applyCloudTransform(camKey) { const pts = state.cloud_points[camKey]; if (!pts) return; @@ -1652,6 +1675,34 @@ function applyCloudTransform(camKey) { pts.scale.set(sv[0], sv[1], sv[2]); } +// Piazza la nuvola Nuwa: in modalità allineata la renderizza nel frame del +// D435i (parent + pipeline D435i · matrice di allineamento); altrimenti usa il +// piazzamento URDF normale. +function applyNuwaCloudPlacement() { + const pts = state.cloud_points.nuwa; + if (!pts) return; + const aligned = state.align.enabled && state.align.matrix; + if (aligned) { + const parent = getCloudParent('d435'); + if (parent) { + if (pts.parent !== parent) parent.add(pts); + pts.matrixAutoUpdate = false; + pts.matrix.copy(d435LocalMatrix()).multiply(state.align.matrix); + pts.matrixWorldNeedsUpdate = true; + return; + } + } + if (state.align.enabled && !state.align.matrix) { + const s = $('p3AlignStatus'); + if (s) { s.textContent = 'nessuna calibrazione: premi Allinea'; s.style.color = 'var(--dim)'; } + } + // fallback URDF normale + const parent = getCloudParent('nuwa'); + if (parent && pts.parent !== parent) parent.add(pts); + pts.matrixAutoUpdate = true; + applyCloudTransform('nuwa'); +} + function readCloudFlip(camKey) { const tag = camKey === 'nuwa' ? 'Nuwa' : 'D435'; return [