o i*@sdZddlZddlZddlmZddlmmZej ddZ ej ddZ dZ dZ d Zd Zd Zd Zd Zd ZgdZgdZgdZgdZgdZdZdZdZdZGdddejZedkre ej!j"#rjdndZ ededdZ$e$%e Z$e&dddd%e Z'e(ded%e Z)e*e$e'e+ddg%e e)d \Z,Z-Z.Z/Wdn1swYe0d!e,j1e0d"e-j1e0d#e.j1e0d$e/j1dSdS)%aeModel for trajectory volume prediction using DINOv2. Predicts a pixel-aligned volume: N_WINDOW x N_HEIGHT_BINS logits per pixel (cross-entropy). Gripper is per-pixel (N_WINDOW x N_GRIPPER_BINS per pixel): supervised at GT pixel during training, decoded at predicted pixel during inference (teacher forcing in train, argmax at pred pixel in val/inference). N DINO_REPO_DIRz;/Users/cameronsmith/Projects/robotics_testing/random/dinov3DINO_WEIGHTS_PATHzt/Users/cameronsmith/Projects/robotics_testing/random/dinov3/weights/dinov3_vits16plus_pretrain_lvd1689m-4057cbaa.pth)g ףp= ?gv/?gCl?)gZd;O?gy&1?g?gi1??)n! rr)n! @r r )r r r)rrr )rrg@ @csXeZdZdZdeedffdd ZfddZdd Zd d Z d d Z dddZ Z S)TrajectoryHeatmapPredictorzPredicts pixel-aligned volume (N_WINDOW x N_HEIGHT_BINS per pixel) and per-pixel gripper (N_WINDOW x N_GRIPPER_BINS per pixel).Fc st||_||_||_t|_tdtj j t ddt d|_ |r7|j D]}d|_q'|j tdntd|j j|_td|jtt|jd |_td |jd |j}ttj||d d dttj||d d dttj||d d dt|_td|tj||jtd d|_td|jdtd|d|d d}||_tj||j|d d|_tj||jd td d|_td|jd|d|d|d td|jdtd|d|d dS)NzLoading DINOv2 model...Zdinov3_vits16pluslocal)sourceweightsFu✓ Frozen DINOv2 backboneu ✓ DINOv2 backbone is trainableu✓ DINO embedding dim: g{Gz?u,✓ Learnable start keypoint embedding (dim=)) kernel_sizepaddingu1✓ Feature convs: 3× Conv2d(3×3) at pred_size=)ru✓ Volume head → (B, z, u✓ Gripper head → (B, u) [1×1 conv, CE 2-class]u✓ Rotation head → (B, z, 3, u) [1×1 conv, CE]) super__init__ target_size pred_sizen_windowDINO_PATCH_SIZEZ patch_sizeprinttorchhubloadrrdino parameters requires_gradeval embed_dimnn Parameterrandnstart_keypoint_embedding SequentialConv2dGELU feature_convs N_HEIGHT_BINS volume_headn_gripper_classes gripper_head N_ROT_BINS rotation_head)selfrrrfreeze_backboneparamDZN_GRIPPER_CLASSES __class__9/data/cameron/567_augmentation_viewpoint_project/model.pyr'sP    $$(z#TrajectoryHeatmapPredictor.__init__cs(t|t|dr|j||_|S)Nr")rtohasattrr")r5devicer9r;r<r=\s  zTrajectoryHeatmapPredictor.toc Cs|jd}|j|\}\}}|jjD]}|jjr!|jj||dnd}|||}q|jjr[|j|ddd|jjdf}|j|dd|jjddf} t j || gdd}n|j|}|dddf} |dd|jjddf} | ||||j } | dddd} | | fS)zExtract patch features and CLS token. Returns: patch_features: (B, D, H_p, W_p) cls_token: (B, D) r)HWNr)dimrr)shaper"Zprepare_tokens_with_masksblocksZ rope_embedZuntie_cls_and_patch_normsZcls_normZn_storage_tokensnormrcatreshaper&permute contiguous) r5xBZx_tokensH_pW_pZblkZ rope_sincosZ x_norm_clsZx_norm_patches cls_tokenZ patch_tokenspatch_featuresr;r;r<_extract_dino_featuresbs   $$ z1TrajectoryHeatmapPredictor._extract_dino_featuresc Cs||j\}}}}|jd}|dd|d}|dd|d} tj||jd|d||} || dd| |fS)zIndex spatial feature map at specified pixel locations. Args: feats: (B, D, H, W) query_pixels: (B, N, 2) pixel coords [x, y] in feats coordinate space Returns: indexed: (B, N, D) r.rr.rr?N)rClongclampraranger?viewexpand) r5feats query_pixelsrKr8r@rANpxpy batch_idxr;r;r<_index_featureszs   z*TrajectoryHeatmapPredictor._index_featurescCs|jd}|jd}|j}}|j}t}|dd|d} |dd|d} |||||||} tj ||j d|d ||} tj ||j dd| ||} | | | dd| | f}| |||d|||}|| | dddd| | f}||fS)alIndex dense gripper/rotation maps at specified pixel locations. Args: feats: (B, D, pred_size, pred_size) query_pixels: (B, N_WINDOW, 2) in pred_size coordinate space Returns: gripper_logits: (B, N_WINDOW, 2) logits for [open, close] rotation_logits: (B, N_WINDOW, 3, N_ROT_BINS) rrrQrRrSNr) rCrr1r3rTrUr2rWrrVr?rXr4)r5rYrZrKr[r@rAZNcZNrr\r]Zgrip_mapr^Ztime_idxgripper_logitsZrot_maprotation_logitsr;r;r<predict_at_pixelss    z,TrajectoryHeatmapPredictor.predict_at_pixelsNcCs@|jd}||\}}|j\}}} } |dkr"|d|d}|dddf| |jd| d} |dddf| |jd| d} tj ||j d} || dd| | f|j d7<t j ||j|jfddd}||}||}|||jt|j|j}|dur|||\}}nd}}||||fS) u+ Args: x: (B, 3, H, W) start_keypoint_2d: (B, 2) or (2,) current EEF pixel in image coords query_pixels: (B, N_WINDOW, 2) pixel coords in pred_size space to query for gripper/rotation. Pass GT pixels during training; pass predicted pixels (volume argmax) during inference. If None, gripper/rotation logits are not computed. Returns: volume_logits: (B, N_WINDOW, N_HEIGHT_BINS, pred_size, pred_size) gripper_logits: (B, N_WINDOW, 2) or None rotation_logits: (B, N_WINDOW, 3, N_ROT_BINS) or None feats: (B, D, pred_size, pred_size) — for predict_at_pixels at eval rrNrSbilinearF)sizemode align_corners)rCrPrB unsqueezerXrrTrUrrVr?r*F interpolaterr.r0rWrr/rb)r5rJstart_keypoint_2drZrKrOrN_r8rLrMZ start_patch_xZ start_patch_yZ batch_indicesrYvol volume_logitsr`rar;r;r<forwards"  **$   z"TrajectoryHeatmapPredictor.forward)N) __name__ __module__ __qualname____doc__ PRED_SIZEN_WINDOWrr=rPr_rbro __classcell__r;r;r9r<r $s 5!r __main__mpscpurT)rrr6rrgl@)rkrZzvolume_logits zgripper_logits razfeats )2rsosrtorch.nnr'torch.nn.functional functionalrienvirongetrrr IMAGENET_MEAN IMAGENET_STDru MIN_HEIGHT MAX_HEIGHT MIN_GRIPPER MAX_GRIPPERMIN_ROTMAX_ROTREF_ROTATION_QUATMIN_POSMAX_POSr/N_GRIPPER_BINSr3rtModuler rpr?backendsrx is_availablemodelr=r)rJzerosZ fake_queryno_gradtensorrmZgriprotrYrrCr;r;r;r<sP 8  (