原路径：/SSD2/yangzijing/egoview-v9-foundationhand-s9l10-20260901/v9_fusion/s9_pose_initialization.py
SHA256：1c0918a857343c621a02ec442a8251ef813567f473d96d22eab2f7b641ba595a
1 """Calibrated rigid palm initialization from S9 geometry and RGB evidence only.""" 2 from __future__ import annotations 3 4 import cv2 5 import numpy as np 6 7 8 def surface_wrist_seed(relative_joints, surface_depth, depth_weight, previous_wrist, 9 keypoints, usable, k, joint_below_surface_m=.01): 10 """Translate a posed palm from observed surface Z without flattening its offsets. 11 12 Input geometry is wrist-relative in camera axes, after the chosen palm 13 rotation. The observations constrain joint Z, not wrist Z. No scale, pose, 14 temporal interpolation, or validity is changed here. 15 """ 16 rel=np.asarray(relative_joints,dtype=np.float64);depth=np.asarray(surface_depth) 17 ok=(np.asarray(depth_weight)>0)&np.isfinite(depth)&np.isfinite(rel).all(-1) 18 if ok.sum()<3:return np.array(previous_wrist,copy=True),False 19 candidates=depth[ok]+joint_below_surface_m-rel[ok,2] 20 z=float(np.median(candidates)) 21 if not .1<=z<=4 or np.median(abs(candidates-z))>.03: 22 return np.array(previous_wrist,copy=True),False 23 observed=np.asarray(usable)&np.isfinite(keypoints).all(-1)&(rel[:,2]+z>.02) 24 if observed.sum()<4:return np.array(previous_wrist,copy=True),False 25 k=np.asarray(k) 26 xy=(np.asarray(keypoints)[observed]-k[:2,2])*(rel[observed,2:]+z)/np.diag(k)[:2]-rel[observed,:2] 27 return np.r_[np.median(xy,axis=0),z].astype(np.asarray(previous_wrist).dtype),True 28 29 30 def initialize_palm_orientation(joints, rotations, wrist, keypoints, usable, scores, k, 31 *, local_rotation_radius_rad=.7, fixed_wrist_z=True, 32 confidence_guard=False): 33 """Resolve large palm turns before allowing finger articulation to explain RGB. 34 35 Wrist and four finger MCPs form the rigid palm. PnP proposes only proper 36 global rotations; it never changes local finger pose, shape, or wrist Z. 37 Evaluate each proposal at the unchanged network depth against all usable 38 RGB joints, including thumb evidence that disambiguates a palm flip. 39 """ 40 palm = np.array([0, 5, 9, 13, 17]) 41 # A near-planar palm fitted to weak keypoints can select the opposite PnP 42 # branch. Require independently confident finger evidence to resolve it. 43 # This changes initialization only, never observation validity or fitting 44 # weights. The legacy path remains identical unless explicitly enabled. 45 usable = np.array(usable, dtype=bool, copy=True) 46 if confidence_guard: 47 usable &= np.isfinite(scores) & (np.asarray(scores) >= .6) 48 nonpalm = np.setdiff1d(np.arange(21), palm) 49 certified = (usable[..., palm].sum(-1) >= 4) & (usable[..., nonpalm].sum(-1) >= 2) 50 usable &= certified[..., None] 51 result = np.array(rotations, copy=True) 52 roots = np.array(wrist, copy=True) 53 changed = np.zeros(joints.shape[:2], bool) 54 before = np.full(joints.shape[:2], np.nan) 55 after = before.copy() 56 k = np.asarray(k, np.float64) 57 for f, s in np.argwhere(usable[..., palm].sum(-1) >= 4): 58 frame_k = k[f] if k.ndim == 3 else k 59 ok = usable[f, s] 60 indices = palm[ok[palm]] 61 local = (joints[f, s] - joints[f, s, :1]) @ rotations[f, s, 0] 62 points = np.ascontiguousarray(local[indices], np.float64) 63 target = np.ascontiguousarray(keypoints[f, s, indices], np.float64) 64 if np.linalg.svd(target - target.mean(0), compute_uv=False)[1] < 1: 65 continue 66 try: 67 solved = cv2.solvePnPGeneric(points, target, frame_k, None, flags=cv2.SOLVEPNP_SQPNP) 68 except cv2.error: 69 continue 70 candidates = [rotations[f, s, 0]] 71 depths = [wrist[f,s,2]] 72 if solved[0]: 73 candidates.extend(cv2.Rodrigues(rv)[0] for rv in solved[1]) 74 depths.extend(float(tv[2,0]) for tv in solved[2]) 75 errors, translations = [], [] 76 for candidate_index, rotation in enumerate(candidates): 77 camera = local @ rotation.T 78 candidate_depth = wrist[f,s,2] if fixed_wrist_z else depths[candidate_index] 79 camera[:, 2] += candidate_depth 80 if not fixed_wrist_z and not .1 <= candidate_depth <= 4: 81 errors.append(np.inf);translations.append(roots[f,s,:2]);continue 82 if (camera[ok, 2] <= .02).any(): 83 errors.append(np.inf) 84 translations.append(roots[f, s, :2]) 85 continue 86 desired = (keypoints[f, s, ok] - frame_k[:2, 2]) * camera[ok, 2:] / np.diag(frame_k)[:2] 87 xy = np.median(desired - camera[ok, :2], axis=0) 88 camera[:, :2] += xy 89 uv = camera[:, :2] / camera[:, 2:] * np.diag(frame_k)[:2] + frame_k[:2, 2] 90 residual = np.linalg.norm(uv[ok] - keypoints[f, s, ok], axis=-1) 91 weight = scores[f, s, ok] ** 2 92 errors.append(np.sum(residual * weight) / weight.sum()) 93 translations.append(xy) 94 best = int(np.argmin(errors)) 95 before[f, s], after[f, s] = errors[0], errors[best] 96 correction = np.arccos(np.clip((np.trace(candidates[best] @ candidates[0].T)-1)/2, -1, 1)) 97 # Small rotations already lie in the local optimizer's trust region. 98 # Do not replace those seeds with noisier per-frame PnP estimates. 99 if best and ((correction > local_rotation_radius_rad and errors[best] < .75 * errors[0]) 100 or (not fixed_wrist_z and errors[best] < .9*errors[0])): 101 result[f, s, 0] = candidates[best] 102 roots[f, s, :2] = translations[best] 103 if not fixed_wrist_z:roots[f,s,2]=depths[best] 104 changed[f, s] = True 105 return result, roots, {"changed": changed, "before_px": before, "after_px": after}