
    LFh'                     z   d dl Z d dlZd dl mZ d dlZd dlZd dlmZmZ d dlm	Z
 d dlZd dlZd dlZd dlZd dlZd dlmZ d dlmZ d dlZd dlmZ d dlmZmZmZmZmZmZ d d	lmZ d d
l m!Z!m"Z" d dl#Z#d dl$Z$d Z%ddZ&d Z'd Z(d Z)ddZ*d Z+d Z,d Z- G d dej\                        Z/ G d dej\                        Z0y)    N)nn)	rearrangerepeat)
functional)deepcopy)cm)tqdm)CallableListOptionalTuple	GeneratorDict)defaultdict)natural_cubic_spline_coeffsNaturalCubicSplinec                     t        | d      S )Nz... c x y -> ... (x y) c)r   xs    3/home/cameronsmith/repos/rotation_testing/models.py<lambda>r      s    9Q :;     c           	      f    t        | d|"t        | j                  d      dz              S |      S )Nz... (x y) c -> ... c x yg      ?r   )r   intsize)srcr   s     r   r   r      s2    Y-Gijir3sxxXZ|`cOdKez xyz r   c                 d    t        j                  | t        j                  | ddgf         fd      S )N.r   )torchcat	ones_liker   s    r   r   r      s)    		1eooaaSk:;R@ r   c                 .    | dd df   d| ddd f   z   z  S )N.r   gh㈵> r   s    r   r   r      s&    !C"H+#rs(!34 r   c                 @    t        j                  | |dz  dz
  ||      S )N      )modepadding_mode)Fgrid_sampler   ypadr(   s       r   r   r      s    AMM1a!eaidY\$] r   c                     t        | j                        dk(  rt        | |||      S t        | j                  dd      |j                  dd      ||      j	                  d| j                  d d       S )N   r   r'   r&   )lenshape
grid_samp_flatten	unflattenr,   s       r   r   r      s    WZ[\[b[bWcghWhAq#d9S nxyz  zC  zC  DE  GH  zI  KL  KT  KT  UV  XY  KZ  [^  _c  od  on  on  op  rs  ry  ry  z|  {|  r}  o~ r   c                 B    t        t        j                  d||             S )Nb...cij,b...ckj->b...cki)unhomr    einsum)crdsKs     r   r   r      s    %-GD QR r   c           	      d    t        t        j                  d|t        |             dd df   |      S )Nr7   .   )projectr    r9   hom)r:   posesr;   s      r   r   r      s6    gu||4NPUWZ[_W`'abegihigibi'jlmn r   c                 b    | t        j                  t        |             j                  |          S N)r    randpermr1   tor   s    r   r   r      s#    AennSV,//23 r   c                   ,     e Zd Zd fd	Zdi fdZ xZS )!CanonRobotTrajPredTransfEuclideanNc                 D   t         |           || _        t        dd      | _        d}dd lm} |j                  d      }t        j                  t        |j                               d d  | _        t        j                  d||||j                  rd	nd
g      | _        y )Nr=   T)in_chuse_first_pool   r   )
pretrainedr         	   )super__init__args	ResnetFPNimg_enctorchvision.modelsmodelsresnet18r   
Sequentiallistchildrenresnetmlp_helpersmake_net
noncanon6d	traj_pred)selfrQ   
latent_dimrU   rZ   	__class__s        r   rP   z*CanonRobotTrajPredTransfEuclidean.__init__!   s    	 !q>
+D1mmT&//*;%<Sb%AB$--s:jY]YhYhTUno.pqr   c                 .   | j                  |d         j                  d      j                  d      }| j                  |      }| j                  j                  s| j                  j
                  r|dd df   |ddd f   dS d|j                  dd      iS )Nimgr   .r=   )
pred_transpred_rot	pred_tris)r=   r=   )rZ   squeezer^   rQ   r]   
noncanon9dr5   )r_   model_input
track_idxsoutlatentr^   s         r   forwardz)CanonRobotTrajPredTransfEuclidean.forward.   s    [/088<DDRH..(	W[W`W`WkWkosoxox  pD  pD	#bqb& 1yQR?PS 	Ay222e<?	Ar   rB   )__name__
__module____qualname__rP   rm   __classcell__ra   s   @r   rF   rF       s    r 04 Ar   rF   c                   D     e Zd Z	 	 	 	 	 	 	 	 	 	 d fd	ZddZddZ xZS )rR   c                 t   t         |           dd}|| _        || _         ||	      }t	        d|d        t        t        j                  |      ||      | _        |
dk7  rt        j                  |
| j                  j                  j                  j                  d   | j                  j                  j                  | j                  j                  j                  | j                  j                  j                   | j                  j                  j"                        | j                  _        t        j$                         | j                  _        t        j$                         | j                  _        g d|   | _        || _        || _        || _        || _        | j5                  d	t7        j8                  d
d
d
d
      d       | j5                  dt7        j8                  dt6        j:                        d       t        j$                  t        j                  | j*                  dd
            | _        d| _        t        j@                  ddddd| j>                  ffD cg c]  \  }}t        j                  ||d
       c}}      jC                         | _"        t        j@                  dddd| j>                  fD cg c]  }t        j                  ||d
       c}      jC                         | _#        t        j                  |
| j>                  d
      jC                         | _$        y c c}}w c c}w )Nc                 :   | dk(  r(t        j                  t        j                  dd      }|S | dk(  r(t        j                  t        j                  dd      }|S | dk(  r&t        j                  t        j
                  |      }|S | dk(  rd}|S t        d	| z        )
ax  Return 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)affinetrack_running_statsinstanceFgroupnoneNz%normalization layer [%s] is not found)	functoolspartialr   BatchNorm2dInstanceNorm2d	GroupNormNotImplementedError)	norm_typegroup_norm_groups
norm_layers      r   get_norm_layerz*ResnetFPN.__init__.<locals>.get_norm_layerE   s     G#&..NN4T
  j(&..%%e
  g%&..r||=NO
  f$!

  *;iG r   zUsing torchvisionencoder)rK   r   r=   r   )r)   )r   @      rJ   rL   i   rl   r'   F)
persistentlatent_scalingr&   )dtyperL   r   )rJ   r   )r   r   )r   r   r   )ry       )%rO   rP   feature_scalerI   printgetattrtorchvisionrU   modelr   Conv2dconv1weightr2   kernel_sizestridepaddingr)   rW   fcavgpoollatent_size
num_layersindex_interpindex_paddingupsample_interpregister_bufferr    emptyfloat32rk   out_dim
ModuleListcudacombs_1combs_2last_conv_up)r_   backbonerK   r   r   r   r   r   rI   r   rH   r   r   d1d2dra   s                   r   rP   zResnetFPN.__init__6   s    		2 +,#I.
!8Y7:W[//:j]gh
A:!yy

  ''--a0

  ,,

  ''

  ((!ZZ--:: DJJ 

]]_

7
C$(*.Xu{{1aA'>5Qekk!5==Ae 	 	
 ==IId&&Q/
 }})T\]delnpquq}q}m~I  &A2ryyR';  &A  B  G  G  I}}C2bQUQ]Q]C^%_aryyAq'9%_`eeg))E4<<;@@B &A%_s   $!L/
L5c                 v   t        |j                        dkD  r6 | |j                  dd      |      j                  d|j                  d d       S | j                  dk7  rFt        j                  || j                  | j                  dkD  rdnd| j                  dkD  rdnd d	      }g }| j                  j                  |      }| j                  j                  |      }| j                  j                  |      }| j                  j                  |      }|j                  |       | j                  dkD  rS| j                  r| j                  j                  |      }| j                  j                  |      }|j                  |       | j                  dkD  r,| j                  j!                  |      }|j                  |       | j                  d
kD  r,| j                  j#                  |      }|j                  |       | j                  dkD  r,| j                  j%                  |      }|j                  |       |d   S )Nr0   r   r'   r&         ?bilinearareaTscale_factorr(   align_cornersrecompute_scale_factorr=   r   )r1   r2   r4   r5   r   r*   interpolater   r   maxpoolbn1reluappendr   rI   layer1layer2layer3layer4r   r   r   )r_   r   custom_sizelatentsr   	latent_sz	up_latents          r   rm   zResnetFPN.forward   s   qww<!D1a+$F$P$PQRTUT[T[\^]^T_$``$qt/A/A#'#5#5#;Z_c_q_qtw_wW[  ~B  [_aAJJQJJq!JJNN1JJOOAq??Q""JJ&&q)

!!!$ANN1??Q

!!!$ANN1??Q

!!!$ANN1??Q

!!!$ANN1 r{r   c                 :   t        |j                        dkD  r6 | |j                  dd      |      j                  d|j                  d d       S | j                  dk7  rFt        j                  || j                  | j                  dkD  rdnd| j                  dkD  rdnd d	      }| j                  j                  |      }| j                  j                  |      }| j                  j                  |      }|g}| j                  dkD  rS| j                  r| j                  j                  |      }| j                  j                  |      }|j                  |       | j                  dkD  r,| j                  j!                  |      }|j                  |       | j                  d
kD  r,| j                  j#                  |      }|j                  |       | j                  dkD  r,| j                  j%                  |      }|j                  |       | j&                  dk(  rd nd}|d   j                  dd  }t)        t        |            D ]/  }t        j                  ||   ||n|| j*                  |      ||<   1 t-        j.                  |d      | _        | j0                  j                  d   | j2                  d<   | j0                  j                  d   | j2                  d<   | j2                  | j2                  dz
  z  dz  | _        | j5                  | j0                        S )Nr0   r   r'   r&   r   r   r   Tr   r=   znearest r   )r(   r   )dimr   g       @)r1   r2   r4   r5   r   r*   r   r   r   r   r   r   rI   r   r   r   r   r   r   r   ranger   r    r!   rl   r   rk   )r_   r   r   r   r   r   is          r   forward_zResnetFPN.forward_   s   qww<!		!Q5??1772A;OO$!//#'#5#5#;Z&*&8&83&>dD'+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	 iiQ/!%!2!22!6A!%!2!22!6A"11T5H5H15LMPSSxx$$r   )
resnet34Tr0   r   borderr   r   Trv   r=   rB   )rn   ro   rp   rP   rm   r   rq   rr   s   @r   rR   rR   5   s6     "OCb)n1%r   rR   rB   )r   r   )1r    r   r   korniar|   einopsr   r   torch.nnr   r*   numpynpsysrandomtimeoscopyr   
matplotlibr   wandbr	   typingr
   r   r   r   r   r   collectionsr   torchcubicspliner   r   geometryr[   ch_secch_fstr?   r8   r3   	grid_sampr>   warpshuffleModulerF   rR   r$   r   r   <module>r      s        $ $        C C # M  
<	z@4]
 	
Rn
3A		 A*z%		 z%r   