liyang
ae28d55f81
Source: RGB-D -> Dyn-HaMR -> L20 retargeting -> FoundationPose -> reference repair -> SPIDER, documented in docs/PIPELINE_LATEST.md and docs/SETUP_AND_WEIGHTS.md. Adds FoundationPose and nvdiffrast upstream snapshots, requirements/pipeline_venv.txt and the FoundationPose weight manifest/downloader. Assets (Git LFS): weights/ (WiLoR detector, HandFlow denoiser, UniDepth-L), FoundationPose checkpoints, HaMeR checkpoint, Dyn-HaMR HMP model and BMC constraints, L20 URDF/meshes, the 20260915_171525 D405 recording and the two box CADs. MANO models are not redistributed (third_party/hamer/_DATA/data/mano/README.txt). Environments, caches and run outputs excluded. Co-Authored-By: Claude Opus 5 (1M context) <noreply@anthropic.com>
54 lines
4.0 KiB
Python
54 lines
4.0 KiB
Python
"""Audit RGB-D input, preserve object meshes and estimate static-background camera motion."""
|
|
import os
|
|
os.environ.setdefault('OMP_NUM_THREADS','4')
|
|
from pathlib import Path
|
|
import json
|
|
import shutil
|
|
import cv2
|
|
import numpy as np
|
|
import open3d as o3d
|
|
|
|
ROOT=Path(__file__).resolve().parents[1]
|
|
SRC=ROOT/'docs/20260915_171525';OUT=ROOT/'output/20260915_171525';OUT.mkdir(exist_ok=True)
|
|
meta=json.loads((SRC/'intrinsics.json').read_text())
|
|
times=np.asarray(meta['timestamps_ms'])/1000;times-=times[0]
|
|
assert len(times)==meta['frames'] and np.all(np.diff(times)>0)
|
|
assert len(list((SRC/'depth').glob('*.png')))==len(times)
|
|
assets=OUT/'object_assets';assets.mkdir(exist_ok=True)
|
|
meshes=[]
|
|
for source,name in [('上半.stl','upper.stl'),('下半.stl','lower.stl')]:
|
|
shutil.copy2(ROOT/'docs'/source,assets/name)
|
|
mesh=o3d.io.read_triangle_mesh(str(assets/name));v=np.asarray(mesh.vertices)
|
|
meshes.append(dict(file=name,vertices=len(v),triangles=len(mesh.triangles),bounds=[v.min(0).tolist(),v.max(0).tolist()],extent_m=np.ptp(v,axis=0).tolist()))
|
|
(assets/'manifest.json').write_text(json.dumps(dict(units='assumed meters from numeric dimensions; compare observed depth before claiming registration',parts=meshes,preserve_shared_coordinates=True),indent=2))
|
|
cap=cv2.VideoCapture(str(SRC/'color.mp4'));K=o3d.camera.PinholeCameraIntrinsic(424,240,meta['fx']/2,meta['fy']/2,(meta['cx']+.5)/2-.5,(meta['cy']+.5)/2-.5)
|
|
option=o3d.pipelines.odometry.OdometryOption();option.depth_max=1.5;option.depth_min=.10;option.depth_diff_max=.025
|
|
option.iteration_number_per_pyramid_level=o3d.utility.IntVector([15,8,5])
|
|
c2w=[np.eye(4)];valid=[True];rows=[];previous=None
|
|
for i in range(len(times)):
|
|
ok,frame=cap.read();assert ok
|
|
depth=cv2.imread(str(SRC/'depth'/f'{i:06d}.png'),-1);assert depth.dtype==np.uint16 and depth.shape==frame.shape[:2]
|
|
color=cv2.resize(frame,(424,240),interpolation=cv2.INTER_AREA)
|
|
z=cv2.resize(depth,(424,240),interpolation=cv2.INTER_NEAREST).astype(np.float32)*meta['depth_scale_m']
|
|
hsv=cv2.cvtColor(color,cv2.COLOR_BGR2HSV);ycc=cv2.cvtColor(color,cv2.COLOR_BGR2YCrCb)
|
|
skin=cv2.inRange(ycc,np.array([0,133,77]),np.array([255,173,127]))>0
|
|
colored=(hsv[:,:,1]>85)&(((hsv[:,:,0]<14)|(hsv[:,:,0]>165))|((hsv[:,:,0]>90)&(hsv[:,:,0]<135)))
|
|
dynamic=skin|colored;dynamic[160:]=True
|
|
dynamic=cv2.dilate(dynamic.astype(np.uint8),np.ones((9,9),np.uint8))>0
|
|
z[dynamic]=0
|
|
rgbd=o3d.geometry.RGBDImage.create_from_color_and_depth(o3d.geometry.Image(cv2.cvtColor(color,cv2.COLOR_BGR2RGB)),o3d.geometry.Image(z),depth_scale=1.,depth_trunc=1.5,convert_rgb_to_intensity=True)
|
|
if previous is not None:
|
|
success,T,info=o3d.pipelines.odometry.compute_rgbd_odometry(previous,rgbd,K,np.eye(4),o3d.pipelines.odometry.RGBDOdometryJacobianFromHybridTerm(),option)
|
|
angle=np.arccos(np.clip((np.trace(T[:3,:3])-1)/2,-1,1))
|
|
accepted=bool(success and np.isfinite(T).all() and np.linalg.norm(T[:3,3])<.06 and angle<.12)
|
|
valid.append(accepted)
|
|
c2w.append(c2w[-1]@np.linalg.inv(T) if accepted else c2w[-1].copy())
|
|
rows.append(dict(frame=i,accepted=accepted,translation_step_m=float(np.linalg.norm(T[:3,3])),rotation_step_rad=float(angle)))
|
|
previous=rgbd
|
|
if i%50==0:print('RGBD camera',i,len(times),flush=True)
|
|
cap.release()
|
|
np.savez_compressed(OUT/'rgbd_camera.npz',c2w=np.asarray(c2w),intrinsics=np.array([meta[k] for k in ['fx','fy','cx','cy']]),time=times,valid=np.asarray(valid),method='Open3D adjacent RGBD odometry; mask skin/red/blue/bottom third; first camera world; no loop closure')
|
|
report=dict(frames=len(times),time_span_s=float(times[-1]),color_depth_timestamp_max_difference_ms=float(np.max(np.abs(np.array(meta['timestamps_ms'])-meta['depth_timestamps_ms']))),camera_valid_steps=int(np.sum(valid)),camera_rejected_frames=np.flatnonzero(~np.asarray(valid)).tolist(),steps=rows,boundary='Odometry estimates, not external tracking truth; drift possible; failed steps held and explicitly invalid')
|
|
(OUT/'rgbd_preflight.json').write_text(json.dumps(report,indent=2))
|
|
print(json.dumps({k:v for k,v in report.items() if k!='steps'},indent=2))
|