
    74wgA                        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
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$ ejJ                  jM                  d
       d dl'm(Z(m)Z) d dl*Z*d dl+Z+d dl+m,Z, d Z-ddZ.d Z/d Z0d Z1d Z2d Z3d Z4d Z5 G d dejl                        Z7 G d dejl                        Z8y)    N)nn)	rearrangerepeat)
functional)deepcopy)cm)tqdm)CallableListOptionalTuple	GeneratorDict)defaultdictz./third_party/co-tracker/)
Visualizerread_video_from_path)KMeansc                     t        | d      S )Nz... c x y -> ... (x y) c)r   xs    2/home/cameronsmith/repos/point_track_sfm/models.py<lambda>r      s    i"<=     c           	      f    t        | d|"t        | j                  d      dz              S |      S )Nz... (x y) c -> ... c x y      ?r   )r   intsize)srcr   s     r   r   r      s2    i,FdedmSXXVX\\^M_I`u stu 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)    eii5??1S!W:#> ?C r   c                 .    | dd df   d| ddd f   z   z  S )N.r!   gh㈵> r   s    r   r   r      s%    aCRCj$qRSz/2 r   c                 @    t        j                  | |dz  dz
  dd      S )N      bilinearborder)modepadding_mode)Fgrid_sampler   ys     r   r   r      s    a!A:8T 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	unflattenr0   s     r   r   r       so    3qww<?
1Q 
199UVWX>Z[ZcZcdefgZh@i@s@stuvwv}v}  A  @A  wB  AC r   c                 B    t        t        j                  d||             S )Nb...cij,b...ckj->b...cki)unhomr"   einsum)crdsKs     r   r   r   !   s    5.HD!QR r   c           	      d    t        t        j                  d|t        |             dd df   |      S )Nr:   .   )projectr"   r<   hom)r=   posesr>   s      r   r   r   "   s6    %,,7QRWX[\`Xa*bcfgihigici*jlm!o r   c           	      6   d }g }t        t        |       dz
        D ]S  }|j                  t        j                  | |   | |dz                   |j                  t        j
                                U t        j                  |d d  }|j                  |       |S )Nc                     t        |       t        j                  k(  r;t        | d      r.t        j                  j                  | j                  ddd       y y y )Nweightg        relufan_in)anonlinearityr,   )typer   Linearhasattrinitkaiming_normal_rF   )ms    r   init_weights_normalz%make_net.<locals>.init_weights_normal%   sF    7biiq(#''CfS['\ $  r   r)   r!   )ranger4   appendr   rL   ReLU
Sequentialapply)dimsrQ   layersinets        r   make_netr[   $   s    ] F3t9Q; !biiQQqS	23bggi ! --
%CII!"Jr   c                   6     e Zd Z fdZdi fdZdi fdZ xZS )FlowMapc           	         t         |           || _        |j                  | _        d}t	        ||dg      | _        t        j                  t        d| j                  z  d      t        j                  d|| j                  z  dd            | _
        t        j                  |ddd      | _        d}t        j                  ||dd      | _        t        j                  |ddd      | _        y )	N    r)   r@   T)in_chuse_first_pool   )padding   )super__init__argstime_strider[   	depth_estr   rU   PixelNeRFEncoderConv2d
resnet_enc
depth_convaffinities_convgeneral_confidence_conv)selfrg   fdimaffinity_dim	__class__s       r   rf   zFlowMap.__init__2   s    	  ++!4Q-0&6Qt?O?O=O_c&degenenorswx|  yI  yI  tI  JK  TU  fV  W		$q15YYtL1E%'YYtAa%B$r   Nc                 "   t        j                  |d   j                  d            }g }t        d       t        j                         5  t        |j                  t        |      dz              D ]  \  }}|j                   | ||               	 d d d        t        d       |d   }t        j                  |D cg c]  }|d   	 c}d	      |d<   t        j                  |D cg c]  }|d
   	 c}d      |d
<   t        j                  |D cg c]  }|d   	 c}d      |d<   t        j                  |D cg c]  }|d   	 c}d      |d<   t        j                  |d   d      |d<   |S # 1 sw Y   xY wc c}w c c}w c c}w c c}w )Npred_tracksr   zcollecting perpoint queries,  )
track_idxsz done collecting perpoint queriesr   point_track_reprojr(   aff_simr)   	poses_allworldcrds_pertrack   )
n_clusterspose_clusters)r"   aranger   printno_grad	enumeratechunkr4   rS   r#   geometrycluster_and_represent)	rp   model_input
sample_ptsouttrack_idxs_alloutsrY   rw   r   s	            r   forward_allptszFlowMap.forward_allpts@   st   k-&@&E&Eb&IJ 	+,]]_ 	G ).*>*>s>?RTW?W*X Y G*T+DFG	G 	01G$)IIQU.WA2F0G.WYZ$[ !4#AaQy\#A1EI 99t%E!q~%EqIK$)IIQU.WA2F0G.WXY$Z !  (==c+>NZ\]O
	G 	G /X#A%E.Ws%   	AE1=E='FF;F1E:c                 v   |d   j                   dd  }|d   j                   d d |d   j                  d      c\  }}}|d   |d   z  }t        j                  d|d   |d   z  dz
  |      j	                         }	d}
t        |d   |d         }d}t        |d	   d d d d dgf   |d
   d d dd f   j                  d            j                  d      j                  d      j                         }t        j                  |d   d d dd f   |t        j                  |            }|j                  d      d   }|dz  dz   }t        d       |.t        j                  |d
   j                  d            d |rdnd }t        |d   d| j                         }d|vst        j"                         rYt%        j&                  | j)                  |j+                  dd      dz  dz         |d      }t        |d| j                   |      |d<   t%        j,                  | j/                  |d   j+                  dd            j1                  d||f      dz         }||z   }t3        j4                  |d
   |d   d       d   }|t        ||d
   j                  d            j                  d      z  }| j7                  |d   j+                  dd            j1                  d||f      }t%        j8                  |d      }t;        t        ||d
   j                  d                  }||d   j                  d      z  j=                  d      |d   j                  d      j=                  d      j?                  d      z  }t        j@                  d|d d |f   |      }| jC                  |d   j+                  dd            j1                  d||f      jE                         j?                  d      }t        ||d
   j                  d            j                  d      |d   j                  d      z  }|d
   j                  d      dz  }d}t        j                  |jG                         d d |d f   jI                  dd|j                  d            t        j                  |      |      }t3        jJ                  |d d dd d d |f   jI                  |j                  d      ddd      |d d d dd d |f   jI                  |j                  d      ddd      |d d d dd d |f   jI                  |j                  d      ddd      |d d d d d d |f   j+                  dd      d d d d d d f   z  j?                  d            d   }tM        |dz
  dd      D ]:  }t        jN                  |d d d |f   |d d |dz
  gf   |d d |d f   z  fd      }< t        jN                  t        jP                  d      jS                  |      d   jI                  |j                  d      ddd      |fd      }|rJt        jT                  |d
   j                  d            }|jI                  |j                  d      ddd      }tW        |jY                         d|      tW        |d |      z  } t        j@                  d!| t[        tW        |d d d d |f   j                  d      d"|                  d d#d d$f   }!t]        |!|d         j?                  dd      }"|"|d
   d d d d d |f   z
  |d   d d d d d |d f   z  j_                         ja                         }#t;        t        |d   |d
   j                  d                  }$|$|d   j                  d      z  j=                  d      |d   j                  d      j=                  d      j?                  d      z  }$t        j@                  d%|t[        |dd d |f               d#d d$f   }%|%|d   d d d d |f   j                  d      z  j=                  d      |d   d d d d |f   j                  d      j=                  d      j?                  d      z  }%||%|$|t;        |      t;        |      |tc        |      dz  g   ||d   |#|"d d df   ||||d&z  S )'Nrgbr   r(   r)   r   )@   r   	depth_inpFrig_flow_masksru   r!   pred_visibilitydimz
rig sanityrv   zb (t s) c x y -> b t (s c) x y)sfmapr   r*   )r,   z (b t) (s c) x y -> b (t s) c x y)r   b
intrinsics)minzb p c, b q c -> b p qg-C6?i  r3   )NNzp t x y -> p s t x yzp t x y -> p t s x yzpstij,stpj->stpizt p c -> t s p c.r@   zptij,tpj->tpi)r{   rgb_pertrackrig_pertrack	res_depthdepthrC   rz   r   point_track_lossrx   corr_weightsry   affinity_embaffinity_emb_unnorm)2r5   r   r"   linspacelongch_fst	grid_samp	unsqueezesqueezeroundwherer$   r   r   randpermr   rh   is_grad_enabledr.   interpolaterl   r7   softplusrm   r8   r   get_world_raysrn   	normalizech_secsumclipr<   ro   sigmoidboolexpandefficient_procrustesrR   r#   eyetor   r   inverserB   rA   squaremeanr4   )&rp   r   rw   r   imsizer   _n_trgtn_samprand_subset	low_imresr   static_solverig_sampimg_inpfmap_outr   r   	rds_trackeye_surf_trackr   r   aff_emb_pertrack_allframeaff_emb_pertrackry   general_confgeneral_conf_tracksolve_stridedirect_pose_regressionaff_sim_rigrC   rY   poses_all_to_allpoint_track_surf_reprojrx   r   r   r{   s&                                         r   forwardzFlowMap.forwardW   s   5!'', '--bq1+e2D2I2I!2L1fay"nnQvay':1'<VDIIK	;{3F1I>	 [)9:1Qs7CKP]D^_`abac_cDdDnDnoqDrs{{|}~  G  G  HJ  K  Q  Q  S;;{+<=adCHU__]eMfg<<A<&q)A:a<l 3 ENN;};U;Z;Z[];^,_`{frabx{,|z +e,-MPTP`P`a$(=(=(?}}T__W__Qq5I"5LR5O%PQW]ghH"+H5WZ^ZjZjmn"oKJJt{6/B/J/J1Q/OPZZ[\^_`f]ghijjk	I% ++K,F{S_G`aefghi	"Yu[5O5Y5YZ\5]%^%f%fgh%ii #22;v3F3N3NqQR3ST^^_`bcdjakl {{#6A>$*9\+mB\BfBfgiBj+k$l!5kBS6T6^6^_a6bbgglmgnoz  |M  pN  pX  pX  Y[  p\  p`  p`  ef  p`  pg  pl  pl  qr  pl  ps  s,,68H:8VXhi 33K4G4O4OPQRS4TU__`acdekblmuuw||  BF|  G&|K4N4X4XY[4\]eefghkv  xI  lJ  lT  lT  UW  lX  X !/44R8$>$ KK*T0A B I I"RPWP\P\]_P` achcrcrszc{  ~E  F,,nQqr.L.=P.Q.X.XY`YeYefgYhiklnoq.r  uC  DE  FI  GI  FI  JX  LX  JX  DX  uY  u`  u`  ah  am  am  no  ap  qs  tv  wy  uz&83B3~~9M&N&U&UV]VbVbcdVefhikln&op{|}~  AO  CO  AO  }O  qP  qX  qX  YZ  [\  q]  ^_  `d  ef  gk  ^k  ql  'l  &r  &r  w{  &r  &|~ @Avax"%dAuyy%"1"+eAqQRseGnUZ[\]^]_[_U`F`9abc/dud 		599Q<??51)<CCEJJqMRTUWXZ[\abcde ell;}3M3R3RSU3V&W^c^j^jkyk~k~  @B  lC  DF  GI  JL  _MX] "%--/2H6RSYZ_`vy  TA  A"',,/ABRSVW]^lmnopq{m{^|  _E  _E  FG  _H  I[  ^d  Xe  Tf  #g  hl  mp  qs  rs  qs  hs  #t$%<[=VW\\]^_`a0;}3MaPTUVWaNa3bbfq  sD  gE  FG  HL  MN  OY  Z^  F^  g_  _  h  h  j  o  o  q iE(:;};U;_;_`b;cde$[1B%C%M%Mb%QQVV[\V]^ij{^|  _G  _G  HJ  _K  _O  _O  TU  _O  _V  _[  _[  `a  _[  _b  b"\\/%NSTUVWaSaDb@cdehikjkikekl0=N1OPQRST^P^1_1i1ijl1mmrrwxry  {F  GX  {Y  Z[  \]  ^h  Zh  {i  {s  {s  tv  {w  {{  {{  @A  {{  {B  {G  {G  LM  {G  {N  N!3'#y)5M3u:q=/*#K0/"4QqS"9((#6
 
 	
r   )__name__
__module____qualname__rf   r   r   __classcell__rs   s   @r   r]   r]   1   s"    C 6:r . /3 j
r   r]   c                   <     e Zd Z	 	 	 	 	 	 	 	 	 	 d fd	ZddZ xZS )rj   c           	      r   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
            | _        y )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_layerz1PixelNeRFEncoder.__init__.<locals>.get_norm_layer   s     G#&..NN4T
  j(&..%%e
  g%&..r||=NO

 	 f$!
  **QT]*]^^r   zUsing torchvisionencoder)
pretrainedr   r@   r   )r-   )r   r         rb   i   latentr)   F)
persistentlatent_scalingr(   )dtyperb   )r   r_   )re   rf   feature_scalera   r   getattrtorchvisionmodelsmodelr   rk   conv1rF   r5   kernel_sizestriderc   r-   rU   fcavgpoollatent_size
num_layersindex_interpindex_paddingupsample_interpregister_bufferr"   emptyfloat32r   )rp   backboner   r   r   r   r   r   ra   r   r`   r   r   rs   s                r   rf   zPixelNeRFEncoder.__init__   s    		0 +,#I.
!8Y7:W[//:!j

 A:!yy

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

  ,,

  ''

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

]]_

7
C$(*.Xu{{1aA'>5Qekk!5==Ae 	 	
 ==IId&&Q/
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                  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  }t1        t        |            D ]/  }t        j                  ||   ||n|| j2                  |      ||<   1 t5        j6                  |d      | _        | j                  j                  d   | j8                  d<   | j                  j                  d   | j8                  d<   | j8                  | j8                  dz
  z  dz  | _        | j;                  | j                        S )Nr3   r   r)   r(         ?r*   areaT)scale_factorr,   align_cornersrecompute_scale_factor)devicer@   znearest r   )r,   r  r   r!   g       @)r4   r5   r7   r8   r   r.   r   r   r   r
  r   r   bn1rG   r   ra   maxpoollayer1rS   layer2layer3layer4latentsr   rR   r   r"   r#   r   r   )rp   r   custom_sizer  r  	latent_szrY   s          r   r   zPixelNeRFEncoder.forward  s    qww<>$qyy1~k"B"L"LQqwwWYXY{"[[$!//#'#5#5#;Z&*&8&83&>dD'+A DD**D+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   )
resnet34Tr3   r*   r+   r*   r  Tr   r@   N)r   r   r   rf   r   r   r   s   @r   rj   rj      s/     "J
X0%r   rj   r  )9r"   r   r   korniar   einopsr   r   torchvision.opsopstorch.nnr   r.   numpynpsysrandomtimeoscopyr   
matplotlibr   wandbr	   typingr
   r   r   r   r   r   collectionsr   torchvision.transforms
transformsTpathrS   cotracker.utils.visualizerr   r   r   torch_kmeansr   r   r   rB   r;   r6   r   rA   warpr[   Moduler]   rj   r&   r   r   <module>r.     s        $  $        C C # " + , G    >uC2T
 C	RoP
bii P
d}%ryy }%r   