h%<ddlZddlZddlmZddlZddlZddlmZmZddlm Z ddl Z ddl Z ddlZddlZddlZddlmZddlmZddlZddlmZddlmZmZmZmZmZmZdd lmZdd l m!Z!m"Z"ddl#Z#ddl$Z$d Z%dd Z&d Z'dZ(dZ)ddZ*dZ+dZ,dZ-Gddej\Z/Gddej\Z0Gddej\Z1Gddej\Z2y)N)nn) rearrangerepeat) functional)deepcopy)cm)tqdm)CallableListOptionalTuple GeneratorDict) defaultdict)natural_cubic_spline_coeffsNaturalCubicSplinect|dS)Nz... c x y -> ... (x y) c)rxs -/home/cameronsmith/repos/controll3r/models.pyrs9Q :;c ft|d|"t|jddzS|S)Nz... (x y) c -> ... c x yg?r)rintsize)srcrs rrrs2Y-Gijir3sxxXZ|`cOdKezxyzrcdtj|tj|ddgffdS)N.r)torchcat ones_likers rrrs) 1eooaaSk:;R@rc.|dddfd|dddfzz S)N.rgh㈵>rs rrrs&!C"H+#rs( !34rc@tj||dzdz ||S)N)mode padding_mode)F grid_samplerypadr(s rrrsAMM1a!eaidY\$]rct|jdk(rt||||St|jdd|jdd||j d|jddS)Nrr'r&)lenshape grid_samp_flatten unflattenr,s rrrsWZ[\[b[bWcghWhAq#d9SnxyzzCzCDEGHzIKLKTKTUVXYKZ[^_codononoprsryryz|{|r}o~rcBttjd||S)Nb...cij,b...ckj->b...cki)unhomr einsum)crdsKs rrrs% -GD QRrc dttjd|t|dddf|S)Nr7.)projectr r9hom)r:posesr;s rrrs6gu||4NPUWZ[_W`'abegihigibi'jlmnrcb|tjt|j|SN)r randpermr1tors rrrs#AennSV,//23rc8eZdZdfd ZdifdZdifdZxZS)!CanonRobotTrajPredTransfEuclideanNc t|||_ddlm}d}|j |j |_tdd|_ d}d}tjt|Dcgc]}tj||c}|_tj d |zd |z||dg|_ycc}w) Nr)AsymmetricCroCo3DStereoz)naver/DUSt3R_ViTLarge_BaseDecoder_512_dptr=Tin_chuse_first_pool@r&)super__init__args dust3r.modelrHfrom_pretrainedcudadust3r ResnetFPNimg_encr ModuleListrange mlp_helpers CrossAttn_ cross_attnsmake_netpointmap_decode)selfrPrH model_name latent_dimn_layer_transf_ __class__s rrOz*CanonRobotTrajPredTransfEuclidean.__init__!s   9@ -==jINNP !q>  ==afguav)w\]+*@*@J*W)wx*33Qz\!J,zZdef4gh*xs5Cc vddlm}||jtj|d|dfd}t t j|jddddjd|jdd jd\}}|jD] }|||} tj|dddft jt|d |jd ddfd} t|jt | | jd } |d | izS) Nr) set_tracetarg_img canon_imgr'ribilinearr(r&rirpointmap)pdbrerVr r!ch_secr* interpolater4r5r2unbindr[ch_fstr]r) r^ model_input track_idxsoutpdb_ img_feats tokens_targ tokens_canonattn comb_featsrls rforwardz)CanonRobotTrajPredTransfEuclidean.forward3sQ)$&LL%))[-D[Q\E],^_`"ac "(y7H7H17MeYc)d)n)nopqzrArABDCDrE*F#G#N#NOP#Q L$$SDT+l5SlSYY !A# f\RS>TU^UdUdegehUioy0z{|}~ $..vj/AB:??SUCVWH   rc \|jtj|d|dfd}tt j |j ddddjd|jddjd\}}|jD] }|||} tj|dddft j t|d |jd ddfd}t|jt||jd } |d | izS) Nrfrgr'rrhrjrkr&rirrl)rVr r!rnr*ror4r5r2rpr[rqr]r) r^rrrsrtrvrwrxryrzrls rforward_z*CanonRobotTrajPredTransfEuclidean.forward_DsJLL%))[-D[Q\E],^_`"ac "(y7H7H17MeYc)d)n)nopqzrArABDCDrE*F#G#N#NOP#Q L$$SDT+l5SlSYY !A# f\RS>TU^UdUdegehUioy0z{|}~ $..vj/AB:??SUCVWH   rrB__name__ __module__ __qualname__rOr{r} __classcell__rcs@rrFrF s!i$04 "15" rrFc8eZdZdfd ZdifdZdifdZxZS)%CanonRobotTrajPredTransfBaselineNaiveNc ht|||_tdd|_d}d}t j |||_d}t jt|Dcgc]}tj||c}|_ tj||||dg|_ycc}w)Nr=TrIr'rM )rNrOrPrUrVr Embeddingglobal_emb_latentrWrXrYrZr[r\ traj_decode)r^rPr` n_timestepsrarbrcs rrOz.CanonRobotTrajPredTransfBaselineNaive.__init__Vs  !q>   !#k:!F==afguav)w\]+*@*@J*W)wx&//JzR\]^0_`*xs&B/ct|j|ddddf}|jjdj t |dd}|j D] }|||} |j|jd}|dddf|dddf} }tj| |d} |d| izS)N start_imgrrr'.r= pred_tris)rnrVrweightexpandr1r[rsqueezegeometryrot_trans_to_trisr stackkornia conversionseuler_from_quaternionrotation_matrix_to_quaternionrotation_6d_to_matrix model_outputrp) r^rrrsrt img_tokens motion_tokensry pred_trajtransrot transf_tris rot_eulers rr{z-CanonRobotTrajPredTransfBaselineNaive.forwardfsDLL[)A!A#)FGH ..55d;BB3z?SUVXY $$TDd:m6TmT$$]3;;A> CF#Ys12v%6c..s59!< [   rc2t|j|ddddf}|jjdj t |dd}|j D] }|||} |j|}||dddf|dddfdzS)Nrrr.r=) pred_transpred_rot)rnrVrrrr1r[r)r^rrrsrtrrryrs rr}z.CanonRobotTrajPredTransfBaselineNaive.forward_}sDLL[)A!A#)FGH ..55d;BB3z?SUVXY $$TDd:m6TmT$$]3 'BQB/%c!"f-   rrBr~rs@rrrUs"a 04|.15" rrc,eZdZdfd ZdifdZxZS) CanonRobotTrajPredResnetBaselineNct|||_ddlm}|j d}t jt|jdd|_ tgd|_ y)NrT) pretrainedr)r<) rNrOrPtorchvision.modelsmodelsresnet18r Sequentiallistchildrenresnetr\ traj_pred)r^rPrrrcs rrOz)CanonRobotTrajPredResnetBaseline.__init__sZ   ,D1mmT&//*;%ffD cgc]\} }tj| |d c}} jC|_"tj@dddd|j>fDcgc]}tj||d c}jC|_#tj| |j>d jC|_$ycc}} wcc}w)Nc:|dk(r(tjtjdd}|S|dk(r(tjtjdd}|S|dk(r&tjtj |}|S|dk(rd}|St d |z) axReturn a normalization layer Parameters: norm_type (str) -- the name of the normalization layer: batch | instance | none For BatchNorm, we use learnable affine parameters and track running statistics (mean/stddev). For InstanceNorm, we do not use learnable affine parameters. We do not track running statistics. batchT)affinetrack_running_statsinstanceFgroupnoneNz%normalization layer [%s] is not found) functoolspartialr BatchNorm2dInstanceNorm2d GroupNormNotImplementedError) norm_typegroup_norm_groups norm_layers rget_norm_layerz*ResnetFPN.__init__..get_norm_layersG#&..NN4T  j(&..%%e  g%&..r||=NO   f$!  *;iGrzUsing torchvisionencoder)rrr=r)r))rrLrrrirr'F) persistentlatent_scalingr&)dtyperrL)rr)rrL)rLrLr)r )%rNrO feature_scalerKprintgetattr torchvisionrmodelrConv2dconv1rr2 kernel_sizestridepaddingr)rfcavgpool latent_size num_layers index_interp index_paddingupsample_interpregister_bufferr emptyfloat32rtout_dimrWrScombs_1combs_2 last_conv_up)r^backbonerrrrrrrKrrJrrd1d2drcs rrOzResnetFPN.__init__s  2+,#I.  !8Y7:W[//:j]gh A:!yy   ''--a0   ,,   ''   ((!ZZ--::  DJJ   ]]_ 7 C$(*. Xu{{1aA'>5Q  ekk!5==Ae  == IId&&Q /  }})T\]delnpquq}q}m~I&A2ryyR';&ABGGI }}C2bQUQ]Q]C^%_aryyAq'9%_`eeg ))E4<<;@@B&A%_s $!L/ L5c t|jdkDr6||jdd|jd|jddS|jdk7rFt j ||j|jdkDrdnd|jdkDrdndd }|g}|jj|}|jj|}|jj|}|jj|}|j||jdkDrS|jr|jj|}|jj|}|j||jdkDr,|jj!|}|j||jd kDr,|jj#|}|j||jdkDr,|jj%|}|j||j&d k(rdnd}|djd d}|j(d|j*dt j |d |d jd dd|d zj}|j(d|j*dt j ||djd dd|dzj}|j(d |j*d t j ||djd dd|dzj}|j(d|j*dt j ||djd dd|j-|dzj}|S)Nr0rr'r&?rjareaT scale_factorr( align_cornersrecompute_scale_factorr=nearest rrrk)r1r2r4r5rr*rorrmaxpoolbn1reluappendrrKlayer1layer2layer3layer4rrrrrXrr r!rrrt)r^r custom_sizelatentsr latent_sz up_latentis rr{zResnetFPN.forwards qwwdD'+ A JJ  Q  JJNN1  JJOOA # ??Q ""JJ&&q) !!!$A NN1  ??Q  !!!$A NN1  ??Q  !!!$A NN1  ??Q  !!!$A NN1  $ 1 1Z ?T AJ$$RS) s7|$ A (0 k))+ GAJ iiQ/ !%!2!22!6A!%!2!22!6A"11T5H5H15LMPSSxx $$r) resnet34Tr0rjborderrjrTrr=rBr~rs@rrUrUs6"OCb5%n1%rrUrB)r rj)3r rrrreinopsrrtorch.nnrr*numpynpsysrandomtimeoscopyr matplotlibrwandbr typingr r r r rr collectionsrtorchcubicsplinerrrrYrnrqr?r8r3 grid_sampr>warpshuffleModulerFrrrUr$rrrs $$ CC#M < z@4 ]   Rn 33  3 j8 BII8 r ryy ,z% z%r