
    $iK                        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 ej@                  jC                  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 Z+d Z,ddZ-d Z.d Z/d Z0d Z1dZ2dZ3dZ4 G d dejj                        Z6 G d dejj                        Z7y)    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    ;/home/cameronsmith/repos/multivid_point_track_sfm/models.py<lambda>r      s    9Q :;     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    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 )Nr9   .   )projectr"   r;   hom)r<   posesr=   s      r   r   r   "   s6    gu||4NPUWZ[_W`'abegihigibi'jlmn r   c                 b    | t        j                  t        |             j                  |          S N)r"   randpermr3   tor   s    r   r   r   #   s#    AennSV,//23 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_rI   )ms    r   init_weights_normalz%make_net.<locals>.init_weights_normal&   sD    7biiq(#RWW%<%<ahh#\biq%<%r#  r   r)   r!   )ranger3   appendr   rO   ReLU
Sequentialapply)dimsrT   layersinets        r   make_netr^   %   s    s F3t9q=! !biiQa!e56bggi ! --
%CII!"Jr   Tc                   \     e Zd Zd fd	Zdi fdZdi fdZdi fdZdi fdZdi fdZ xZS )	FlowMapNc                    t         |           || _        d| _        d}t	        ||dg      | _        t        j                  |ddd      | _        d}t        j                  ||dd      | _	        t        j                  |ddd      | _
        t        j                  |ddd      | _        t	        |dz  ddg      | _        t        dt        dz  z   | j                  z  d	
      | _        d| _        y )Nr)   @   r?   )padding   r(      r   F)in_chuse_first_pool)super__init__argstime_strider^   	depth_estr   Conv2d
depth_convaffinities_convgeneral_confidence_convgeneral_confidence_conv_staticcorr_weighter_perpoint	ResnetFPNuse_depth_inpimg_encstep)selfrj   fdimaffinity_dim	__class__s       r   ri   zFlowMap.__init__6   s    	 !4q/2))D!Q:!yy|QJ')yyq!Q'G$.0iiaA.N+&.q"a/@&A# !-/(9T=M=M'M^cd 	r   c                 F   t        j                  |d   j                  d            }g }t        d       t	        t        |j                  t        |      dz        dd            D ]  \  }} | ||      }t        |      r-|j                         D 	ci c]  \  }}	|g dd d  v s||	 }}}	d	|v r`|t        j                  d
t        |j                  t        |      dz              d      j                         v r|d	   d d d df   |d	<   n|d	= |j                  |        t        d       |d
   }d	|v rt        j                  |D 
cg c]  }
|
d   	 c}
d      |d<   t        j                  |D 
cg c]  }
d	|
v s|
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      |d<   |S c c}	}w c c}
w c c}
w c c}
w c c}
w )Npred_tracksr   zcollecting perpoint queriesi,  F)leavedesc)
track_idxs)	poses_allpoint_track_reprojworldcrds_pertrackaff_simr   r   d   2   r)   z done collecting perpoint queriesr   r(   r   r      )
n_clusterspose_clusters)r"   aranger   print	enumerater	   chunkr3   itemslinspacelongrV   r#   geometrycluster_and_represent)rw   model_input
sample_ptsouttrack_idxs_alloutsr\   r   kvr   s              r   forward_allptszFlowMap.forward_allptsP   sJ    k-&@&E&Eb&IJ+,&n.B.B3~CVZ]C].^fk  sP  )R  S 	MAz{z:C4y#&99;}CAa!7yz{7|2|AaC}s}Cq#n.B.B3~CVX[C[.\*]_abggii|  AJ  }K  LM  NP  OP  NP  LP  }Qknoxkyi.KK	 	011g(-		UY2[PQA6J4K2[]^(_C$%"YYt'V!yTU~9'VXYZC	N$yy4)Ia1[>)I1MC(-		UY2[PQA6J4K2[\](^C$%#+#A#A#kBRSTBUac#dC 
 ~ 3\'V)I2[s*   H	'H	H7	HH&HHc                 h   t        j                         r| xj                  dz  c_        |d   j                  dd  }|d   j                  d d |d   j	                  d      c\  }}}|d   |d   z  }t        j
                  d|d   j	                  d      dz
  |      j                         }	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                  |            x}}|j                  d      d   }|*t        j                  |d   j	                  d            d d }|d   }t        |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            j/                  d||f      dz         dz   x}}d|v r| j                  dkD  s 	 t7        j8                  |d   |d   d       d   }|t        ||d   j                  d            j                  d      z  }	 | j;                  |d   j)                  dd            j/                  d||f      j=                         j?                  d      }t        ||d   j                  d            j                  d      |d
   j                  d	      z  }	 t7        j@                  |d d d dd |	f   |d d d d d	|	f   |d d d d d	|	f   j?                  d            d   }tC        |dz
  dd	      D ]C  }t        jD                  |d d d d d |f   |d d d d |dz
  gf   |d d d d |d f   z  fd      }E t        jD                  t        jF                  d      jI                  |      d   jK                  |j	                  d      |j	                  d      d	d	d	      |fd      }|jK                  d	tO        |      d	d	d	      }tQ        |jS                         d|      tQ        |d|      z  }t        jT                  d|tW        tQ        |d d d d |f   d |                  d!d d"f   }tY        ||d         j?                  dd      }|d
   t        jD                  t        j                  |d d d df         |fd      z  }||d   d d d d d |f   z
  |d d d d d |d f   z  j[                         j)                         j]                         }||||d d df   |d d df   t_        |      t_        |      d#z  }| ja                  |d   j)                  dd            j/                  d||f      }t#        jb                  |d      }t_        t        ||d   j                  d                  } | |d
   j                  d	      z  je                  d      |d
   j                  d	      je                  d      j?                  d      z  }!t        jT                  d$|!d d |f   |!      }"| jg                  |d   j)                  dd            j/                  d||f      j=                         j?                  d      }#t        |#|d   j                  d            j                  d      |d
   j                  d	      z  }$|d   j	                  d      d%z  }%t        j                  |j5                         d d |d f   jK                  d	d	|"j	                  d	            t        j                  |"      |"      }&	 t7        j@                  |d d d dd d d |%f   jK                  d	|"j	                  d      d	d	d	      |d d d d d	d d |%f   jK                  d	|"j	                  d      d	d	d	      |$d d d d d	d d |%f   jK                  d	|"j	                  d      d	d	d	      |&d d d d d d d |%d f   z  j?                  d            d   }'tC        |dz
  dd	      D ]C  }t        jD                  |'d d d d d |f   |'d d d d |dz
  gf   |'d d d d |d f   z  fd      }'E 	 t        jD                  t        jF                  d      jI                  |'      d   jK                  |'j	                  d      |'j	                  d      d	d	d	      |'fd      }'tQ        |'jS                         d|      tQ        |'d|      z  }(t        jT                  d|(tW        tQ        |d d d d |f   d |                  d!d d"f   })tY        |)|d         j?                  dd      }*|*|d   d d d d d |f   z
  |d
   d d d d d |d f   z  j[                         j)                         j]                         }+t        jh                         5  |d   j	                  d      d&z  dk(  rd'nd(\  },}-t        |!d)|,|,*      d d df   }.t        jT                  d$t_        |.d!d d |-d d |-f         t_        |.            j/                  d|,|-z  |,|-z  f      j/                  d	|,|,f      }/t_        t        |d   |d   j                  d                  }0|0|d
   j                  d	      z  je                  d      |d
   j                  d	      je                  d      j?                  d      z  }0t        jT                  d+|'tW        |d d d d |f               d!d d"f   }1|1|d
   d d d d |f   j                  d	      z  je                  d      |d
   d d d d |f   j                  d	      je                  d      j?                  d      z  }1d d d        |10||'|+|*d d df   |#|"/|||!t_        |      d,z  S # 1 sw Y   ,xY w)-Nr)   rgbr   r(   r   r|   rb   rb   rig_flow_masksr!   pred_visibilitydim   b (t s) c x y -> b t (s c) x ysfmapr   bilinearr*    (b t) (s c) x y -> b (t s) c x yr   b	depth_inp  
intrinsics-C6?minr2   NNNb p t x y -> b p s t x yb p t x y -> b p t s x ybpstij,bstpj->bstpib t p c -> b t s p c.r?   corr_weights_staticpoint_track_loss_staticpoint_track_reproj_staticrB   	res_depthdepthb p c, b q c -> b p q  i   )rb   r2   *   r?   b (x y s) c -> b s c x yr/   r   bptij,btpj->btpir   rgb_pertrackrig_pertrackr   point_track_lossr   corr_weightsr   aff_sim_gridaffinity_embaffinity_emb_unnormaff_emb_pertrackr   )5r"   is_grad_enabledrv   r4   r   r   r   	grid_samp	unsqueezesqueezeroundwherer$   r   rE   r   rk   r,   interpolateru   r6   softplusrn   r7   ch_fstfloatboolr   get_world_raysrq   sigmoidclipefficient_procrustesrU   r#   eyerF   expandr   r3   r   inverser;   rA   r@   squaremeanch_secro   	normalizesumrp   no_grad)2rw   r   r   r   imsizer   _n_trgtn_samprand_subset	low_imresrig_samprig_samp_allframeimg_inpfmap_outr   r   r   
depth_mask	rds_trackeye_surf_trackgeneral_conf_staticgeneral_conf_track_staticstatic_posesr\   static_poses_all_to_allstatic_point_track_surf_reprojstatic_point_track_reprojvis_and_rig_maskstatic_point_track_lossr   r   aff_emb_pertrack_allframer   r   general_confgeneral_conf_tracksolve_strideaff_sim_rigrB   poses_all_to_allpoint_track_surf_reprojr   r   track_sldslsrc_tracks_0r   r   r   s2                                                     r   forwardzFlowMap.forwardn   s'     "499a<9U#))"#.$U+11"15{57I7N7Nq7QAVAY&nnQM(B(G(G(KA(MvV[[]	
 ,< =aQCi H+VcJdefhihjejJkJuJuvxJy{  E  E  FG  H  Q  Q  RT  U  \  \  ^',{{K@Q4RSTVWVXSX4Y[cejetetu}e~'$<<A<&q) ENN;};U;Z;Z[];^,_`dad,ez e$W&F$JZJZ[$(=(=(? }}dll7??1a3H33NQT3T&UW]dnpH"+X7Y]a]m]mqr"sK JJF8K8S8STUWX8Y(Z(d(defijlrhs(twx(xyz{{{	+%499T>A  ++[-GUaIbdhijkl	"Y{=7Q7[7[\^7_%`%h%hij%kk $($G$GTZH[HcHcdeghHi$j$u$uvwz{  ~D  zE  %F  %O  %O  %Q  %W  %W  \`  %W  %a(13FTaHbHlHlmoHp(q(y(yz{(|  @K  L]  @^  @h  @h  ik  @l  )l%'<<nQPTVWVXZeMe>fhvwxz~  AD  BD  AD  FQ  xQ  iR  Tm  no  qu  wz  xz  wz  |G  nG  TH  TM  TM  RV  TM  TW   Y  Z[   \vz1b1  aA%))l[\^_acbcac[cNdfrstvwz{~z  zA  tA  gB  EQ  RS  UV  XY  XZ  RZ  E[  g[  N\  ^`  Ca<  a$yyEIIaLOOL,IJZ,[,b,bcoctctuvcw  zF  zK  zK  LM  zN  PR  TV  XZ  -[  ]i  +k  mo   q (..C
ORRPL '-l.B.B.DF`dj&knt  vB  D^  bh  oi  'i#-2\\;PRiknpv  yG  HI  KL  NX  HX  yY  [q  u{  q|  l}  .  @C  EG  FG  EG  @G  .H*(/1OQ\]iQj(k(p(pqrtu(v% +,= >EOO\mnoprqrprnr\sLt  vG  LH  IJ  BK  !K,ETaHbcdfjlmoycyHz,z  O  PQ  SW  YZ  \f  hl  Pl  m  +m  )u  )u  )w  )  )  )A  )F  )F  )H#':+B-Fq!t-L%ac*#I. C #22K4G4O4OPQST4UV``abefhndop {{#6A>$*IlKP]D^DhDhikDl,m$n!6EV9W9a9abd9eejjopjqt  AR  uS  u]  u]  ^`  ua  ue  ue  jk  ue  ul  uq  uq  wx  uq  uy  y,, 79I!Z-9XZjk 55k&6I6Q6QRSUV6WXccdehikqgrs||~  E  E  JN  E  O&k-6P6Z6Z[]6^_gghijmx  zK  nL  nV  nV  WY  nZ  Z %]388<Dkk8==?1j$3F#G#N#NrSUW^WcWcdfWg#hjojyjy  {B  kC  EL  N11>!T12~Q]~B]3^3e3efhjqjvjvwxjy{}  @B  DF  4G3A!T3B3P^R^P^B^3_3f3fgikrkwkwxykz|~  AC  EG  4H"4Qcrc>\>5Q"R"Y"YZ\^e^j^jkl^moqsuwy"z  ~I  JK  MN  PT  Vd  Xd  Vd  fj  Jj  ~k  #k  !q  !q  vz  !q  !{} ~@E 6A:q"-  Auyy5ArPQr?TYZ[]^abefaf`gZgThkpqrtuwxwyqykzTzB{}  8Au  A 		UYYq\__U34DELLUZZXY]\a\f\fgh\ikmoqsuvx}  BD  F "5==?4NRXY\bch  kE  IO  ]P  P"',,0EGWY\^dftuvxy  |F  vF  gG  I_  ci  _j  Zk  #m  nq  su  tu  su  nu  #v$&={<?XY^^_`bcd!3k-6PQRTXZ[]gQg6h!hlw  yJ  mK  LM  OS  UV  Xb  dh  Lh  mi   i  q  q  s  {  {  }  B  B  D]]_ 		L#.}#=#B#B2#Fu#La#OU[LHS"#34NQY\defghifijL <<)@&VYZ_\_Z_`ebe`eVeIfBgiop|i}~  I  I  JK  MU  WZ  MZ  [c  eh  [h  Li  j  t  t  uw  yA  BJ  xK  LL!9[-?]A[AeAefhAi#jkL)K8I,J,T,TUW,XX]]bc]dgr  tE  hF  hP  hP  QS  hT  hX  hX  ]^  hX  h_  hd  hd  jk  hd  hl  lL!&/A5#n]^`acm]mNnJo!pqtvxwxvxqx!y#5DU8VWXZ[]gWg8h8r8rsu8v#v&)cacj;?P3QRSUVXbRb3c3m3moq3r3v3v|}3v3~  4D  4D  JK  4D  4L"L		L "4($ 0"4QT":(((#6 0E]
 
 	
		L 		Ls   ?F>p((p1c                 h   t        j                         r| xj                  dz  c_        |d   j                  dd  }|d   j                  d d |d   j	                  d      c\  }}}|*t        j
                  |d   j	                  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                  |            x}}	|j                  d      d   }|d   }
t        |
d| j                        }
	 d|vst        j                         r| j                  t!        j"                  |d   dz  dz   j%                  dd      |d   dz  dz  |d   dz  dz  fd            x|d<   }t!        j"                  ||d      j'                  d||f      t(        sdndz  x|d<   }dt!        j"                  | j+                  |d         |d      j'                  d||f      dz   z  x}}d|v r	 t9        j:                  |d   |d   d       d   }|t        ||d   j                  d            j                  d      z  }| j=                  |d   j%                  dd            j'                  d||f      }t!        j>                  |d      }tA        t        ||d   j                  d                  }||d
   j                  d	      z  jC                  d      |d
   j                  d	      jC                  d      jE                  d      z  }t        jF                  d|d d |f   |      }t        j                  |j7                         d d |d f   jI                  d	d	|j	                  d	            t        j                  |      |      }| jK                  |d   j%                  dd            j'                  d||f      jM                         jE                  d      }| j                  dk  snt        j                  |      }t        ||d   j                  d            j                  d      |d
   j                  d	      z  }tN        r	 t9        jP                  |d d d dd f   |d d d d df   jI                  d	d	|dz
  d	d	      |d d d d d	f   |d d d d d d d f   z  jE                  d            d   }tW        d       |jI                  d	tY        |      d	d	d	      }n9|d   j	                  d      dz  }t9        jP                  |d d d dd d d |f   jI                  d	|j	                  d      d	d	d	      |d d d d d	d d |f   jI                  d	|j	                  d      d	d	d	      |d d d d d	d d |f   jI                  d	|j	                  d      d	d	d	      |d d d d d d d |d f   z  jE                  d            d   }tS        |dz
  dd	      D ]C  }t        jT                  |d d d d d |f   |d d d d |dz
  gf   |d d d d |d f   z  fd      }E t        jT                  t        jZ                  d       j]                  |      d!   jI                  |j	                  d      |j	                  d      d	d	d	      |fd      }t_        |ja                         d"|      t_        |d#|      z  }t        jF                  d$|tc        t_        |d d d d |f   d%|                  d&d d'f   }te        ||d         jE                  dd      }||d   d d d d d |f   z
  |d
   d d d d d |d f   z  jg                         j%                         ji                         } t        jj                         5  d(\  }!}"t        |d)|!|!*      d d df   }#t        jF                  dtA        |#d&d d |"d d |"f         tA        |#            j'                  d|!|"z  |!|"z  f      j'                  d	|!|!f      }$tA        t        |d   |d   j                  d                  }%|%|d
   j                  d	      z  jC                  d      |d
   j                  d	      jC                  d      jE                  d      z  }%t        jF                  d+|tc        |d d d d |f               d&d d'f   }&|&|d
   d d d d |f   j                  d	      z  jC                  d      |d
   d d d d |f   j                  d	      jC                  d      jE                  d      z  }&d d d        |&%||| |d d df   ||$|||tA        |      d,z  S # 1 sw Y   ,xY w)-Nr)   r   r   r(   r|   r   r   r   r!   r   r   r   r   r   r       r   r   midas_featsr   g     @@g?r   r   r   r   r     r   z#doing direct pose regression staticr   r2   r   r   r   r   r   .r?   r   r   r   r   r   )6r"   r   rv   r4   r   rE   r   r   r   r   r   r$   r   r   rk   midasr,   r   r6   r7   scratch_model	midas_outru   r   rn   r   r   r   r   r   ro   r   r   r   r   r;   r   rp   r   static_solver   rU   r#   r   r3   r   rF   r   r   rA   r@   r   r   r   )'rw   r   r   r   r   r   r   r   r   r   r   r  r   r   r   r   r   r   r   r   r   r   r   r   r   r   rB   r\   r   r   r   r   r   r  r  r  r   r   r   s'                                          r   forward_zFlowMap.forward_  s     "499a<9U#))"#.$U+11"15{57I7N7Nq7QA ENN;};U;Z;Z[];^,_`dad,ez ,< =aQCi H+VcJdefhihjejJkJuJuvxJy{  E  E  FG  H  Q  Q  RT  U  \  \  ^',{{K@Q4RSTVWVXSX4Y[cejetetu}e~'$<<A<&q) e$W&F$JZJZ[[(E,A,A,C9=AMMS^_dSefhShikSkRtRtuvwxRy  |B  CD  |E  GI  |I  JL  |L  MS  TU  MV  XZ  MZ  []  M]  {^  dn  Eo  :p  pM*;-.]];vS]-^-h-hijlmntku-v  DQx{  WX  .Y  YF#H #Q]]4>>+mB\3]^djt%u%%  AB  DE  FL  CM  &N  OS  &S  !T  TEI +%! ++[-GUaIbdhijkl	"Y{=7Q7[7[\^7_%`%h%hij%kk #22K4G4O4OPQST4UV``abefhndop {{#6A>$*IlKP]D^DhDhikDl,m$n!6EV9W9a9abd9eejjopjqt  AR  uS  u]  u]  ^`  ua  ue  ue  jk  ue  ul  uq  uq  wx  uq  uy  y,, 79I!Z-9XZjk kk8==?1j$3F#G#N#NrSUW^WcWcdfWg#hjojyjy  {B  kC  EL  N 55k&6I6Q6QRSUV6WXccdehikqgrs||~  E  E  JN  E  O99S=Aeool.K|&k-6P6Z6Z[]6^_gghijmx  zK  nL  nV  nV  WY  nZ  Z !55~aqrl7TVdefhlnpopnpepVqVxVxy{|~  @F  GH  @H  IK  LN  WO  Rd  ef  hl  nq  oq  nq  eq  Rr  s{  |}  ~B  CG  HI  JN  |N  sO  RO  QU  QU  Z^  QU  Q_  a  bc  d;<LL"c*or2rBE(7<<R@DHL11>!T12P^R^P^B^3_3f3fgikrkwkwxykz|~  AC  EG  4H3A!T3B3P^R^P^B^3_3f3fgikrkwkwxykz|~  AC  EG  4H/A!T3B3P^R^P^B^/_/f/fgikrkwkwxykz|~  AC  EG  0H:EaDTbVbTbdhFh:i0jkokotxkoky| }~E 6A:q"-  Auyy5ArPQr?TYZ[]^abefaf`gZgThkpqrtuwxwyqykzTzB{}  8Au  A		UYYq\__U34DELLUZZXY]\a\f\fgh\ikmoqsuvx}  BD  F "5==?4NRXY\bch  kE  IO  ]P  P"',,0EGWY\^dftuvxy  |F  vF  gG  I_  ci  _j  Zk  #m  nq  su  tu  su  nu  #v$&={<?XY^^_`bcd!3k-6PQRTXZ[]gQg6h!hlw  yJ  mK  LM  OS  UV  Xb  dh  Lh  mi   i  q  q  s  {  {  }  B  B  D ]]_ 		L*0LHS"#34NQY\defghifijL <<)@&VYZ_\_Z_`ebe`eVeIfBgiop|i}~  I  I  JK  MU  WZ  MZ  [c  eh  [h  Li  j  t  t  uw  yA  BJ  xK  LL!9[-?]A[AeAefhAi#jkL)K8I,J,T,TUW,XX]]bc]dgr  tE  hF  hP  hP  QS  hT  hX  hX  ]^  hX  h_  hd  hd  jk  hd  hl  lL!&/A5#n]^`acm]mNnJo!pqtvxwxvxqx!y#5DU8VWXZ[]gWg8h8r8rsu8v#v&)cacj;?P3QRSUVXbRb3c3m3moq3r3v3v|}3v3~  4D  4D  JK  4D  4L"L		L "4($ 0"4QT":(((#6 0E]
 
 	
		L 		Ls   F"f((f1c                 f   t        j                         r| xj                  dz  c_        |d   j                  dd  }|d   j                  d d |d   j	                  d      c\  }}}|d   |d   z  }t        j
                  d|d   j	                  d      dz
  |      j                         }	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                  |            x}}|j                  d      d   }|*t        j                  |d   j	                  d            d d }|d   }t        |d| j                         }d|vst        j                         r| j#                  |j%                  dd      dz  dz         x|d<   }t'        j(                  | j#                  |j%                  dd      dz  dz         |d      }t        |d| j                   |      |d<   t'        j*                  | j-                  |d   j%                  dd            j/                  d||f      dz         dz   x}}d|v r	 	 t1        |d   |d         dz  }|dk7  j3                         }|d
xx   t        ||d   j                  d      dd      j                  d      j                  d	      j5                         z  cc<   t7        j8                  |d   |d   d       d   }|t        ||d   j                  d            j                  d      z  }	 | j;                  |d   j%                  dd            j/                  d||f      j=                         j?                  d      }t        ||d   j                  d            j                  d      |d
   j                  d	      z  }	 t7        j@                  |d d d dd |	f   |d d d d d	|	f   |d d d d d	|	f   j?                  d            d   }tC        |dz
  dd	      D ]C  }t        jD                  |d d d d d |f   |d d d d |dz
  gf   |d d d d |d f   z  fd      }E t        jD                  t        jF                  d      jI                  |      d   jK                  |j	                  d      |j	                  d      d	d	d	      |fd      }|jK                  d	tO        |      d	d	d	      }tQ        |jS                         d |      tQ        |d!|      z  }t        jT                  d"|tW        tQ        |d d d d |f   d#|                  d$d d%f   }tY        ||d         j?                  dd      }|d
   t        jD                  t        j                  |d d d df         |fd      z  }||d   d d d d d |f   z
  |d d d d d |d f   z  j[                         j%                         j]                         }||||d d df   |d d df   t_        |      t_        |      d&z  }| ja                  |d   j%                  dd            j/                  d||f      }t'        jb                  |d      }t_        t        ||d   j                  d                  } | |d
   j                  d	      z  je                  d      |d
   j                  d	      je                  d      j?                  d      z  }!t        jT                  d'|!d d |f   |!      }"| jg                  |d   j%                  dd            j/                  d||f      j=                         j?                  d      }#t        |#|d   j                  d            j                  d      |d
   j                  d	      z  }$|d   j	                  d      d(z  }%t        j                  |j5                         d d |d f   jK                  d	d	|"j	                  d	            t        j                  |"      |"      }&	 t7        j@                  |d d d dd d d |%f   jK                  d	|"j	                  d      d	d	d	      |d d d d d	d d |%f   jK                  d	|"j	                  d      d	d	d	      |$d d d d d	d d |%f   jK                  d	|"j	                  d      d	d	d	      |&d d d d d d d |%d f   z  j?                  d            d   }'tC        |dz
  dd	      D ]C  }t        jD                  |'d d d d d |f   |'d d d d |dz
  gf   |'d d d d |d f   z  fd      }'E 	 t        jD                  t        jF                  d      jI                  |'      d   jK                  |'j	                  d      |'j	                  d      d	d	d	      |'fd      }'tQ        |'jS                         d |      tQ        |'d!|      z  }(t        jT                  d"|(tW        tQ        |d d d d |f   d#|                  d$d d%f   })tY        |)|d         j?                  dd      }*|*|d   d d d d d |f   z
  |d
   d d d d d |d f   z  j[                         j%                         j]                         }+t_        t        |d   |d   j                  d                  },|,|d
   j                  d	      z  je                  d      |d
   j                  d	      je                  d      j?                  d      z  },t        jT                  d)|'tW        |d d d d |f               d$d d%f   }-|-|d
   d d d d |f   j                  d	      z  je                  d      |d
   d d d d |f   j                  d	      je                  d      j?                  d      z  }-t        jh                         5  d*}.t        |!d+|.|.,      d d df   }/t        jT                  d't_        |/d$d d dd d df         t_        |/            j/                  d|.dz  |.dz  f      j/                  d	|.|.f      }0d d d        ||-|,||'|+|*d d df   |#|"0|||!t_        |      d-z  S # 1 sw Y   ,xY w).Nr)   r   r   r(   r   r|   r   r   r!   r   r   r   r   r   r   r   r   r   r   r   r   g     j@nearestzeros)r*   r0   r   r   r   r   r2   r   r   r   r   r   .r?   r   r   r   r   rb   r   r   r   )5r"   r   rv   r4   r   r   r   r   r   r   r   r   r$   r   rE   r   rk   ru   r6   r,   r   r   rn   r7   r   r   r   r   r   rq   r   r   r   rU   r#   r   rF   r   r   r3   r   r   r;   rA   r@   r   r   r   ro   r   r   rp   r   )1rw   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r\   r   r   r   r   r   r   r   r   r   r   r   r   r   r   rB   r   r   r   r   r   r   r  r  r   s1                                                    r   r  zFlowMap.forward_r  s     "499a<9U#))"#.$U+11"15{57I7N7Nq7QAVAY&nnQM(B(G(G(KA(MvV[[]	 ,< =aQCi H+VcJdefhihjejJkJuJuvxJy{  E  E  FG  H  Q  Q  RT  U  \  \  ^',{{K@Q4RSTVWVXSX4Y[cejetetu}e~'$<<A<&q) ENN;};U;Z;Z[];^,_`dad,ez e$W&F$JZJZ[$(=(=(?-1\\'//!Q:ORU:UX[:[-\\K(}}dll7??1a3H33NQT3T&UW]dnpH"+X7Y]a]m]mqr"sK JJF8K8S8STUWX8Y(Z(d(defijlrhs(twx(xyz{{{	+%!
 {;7q	B3F#Qh--/
-.)ZUbIcImImnpIq  xA  FM  3N  3V  3V  WX  3Y  3a  3a  bd  3e  3j  3j  3l  l. ++[-GUaIbdhijkl	"Y{=7Q7[7[\^7_%`%h%hij%kk $($G$GTZH[HcHcdeghHi$j$u$uvwz{  ~D  zE  %F  %O  %O  %Q  %W  %W  \`  %W  %a(13FTaHbHlHlmoHp(q(y(yz{(|  @K  L]  @^  @h  @h  ik  @l  )l%'<<nQPTVWVXZeMe>fhvwxz~  AD  BD  AD  FQ  xQ  iR  Tm  no  qu  wz  xz  wz  |G  nG  TH  TM  TM  RV  TM  TW   Y  Z[   \vz1b1  aA%))l[\^_acbcac[cNdfrstvwz{~z  zA  tA  gB  EQ  RS  UV  XY  XZ  RZ  E[  g[  N\  ^`  Ca<  a$yyEIIaLOOL,IJZ,[,b,bcoctctuvcw  zF  zK  zK  LM  zN  PR  TV  XZ  -[  ]i  +k  mo   q (..C
ORRPL '-l.B.B.DF`dj&knt  vB  D^  bh  oi  'i#-2\\;PRiknpv  yG  HI  KL  NX  HX  yY  [q  u{  q|  l}  .  @C  EG  FG  EG  @G  .H*(/1OQ\]iQj(k(p(pqrtu(v% +,= >EOO\mnoprqrprnr\sLt  vG  LH  IJ  BK  !K,ETaHbcdfjlmoycyHz,z  O  PQ  SW  YZ  \f  hl  Pl  m  +m  )u  )u  )w  )  )  )A  )F  )F  )H#':+B-Fq!t-L%ac*#I. C #22K4G4O4OPQST4UV``abefhndop {{#6A>$*IlKP]D^DhDhikDl,m$n!6EV9W9a9abd9eejjopjqt  AR  uS  u]  u]  ^`  ua  ue  ue  jk  ue  ul  uq  uq  wx  uq  uy  y,, 79I!Z-9XZjk 55k&6I6Q6QRSUV6WXccdehikqgrs||~  E  E  JN  E  O&k-6P6Z6Z[]6^_gghijmx  zK  nL  nV  nV  WY  nZ  Z %]388<Dkk8==?1j$3F#G#N#NrSUW^WcWcdfWg#hjojyjy  {B  kC  EL  N11>!T12~Q]~B]3^3e3efhjqjvjvwxjy{}  @B  DF  4G3A!T3B3P^R^P^B^3_3f3fgikrkwkwxykz|~  AC  EG  4H"4Qcrc>\>5Q"R"Y"YZ\^e^j^jkl^moqsuwy"z  ~I  JK  MN  PT  Vd  Xd  Vd  fj  Jj  ~k  #k  !q  !q  vz  !q  !{} ~@E 6A:q"-  Auyy5ArPQr?TYZ[]^abefaf`gZgThkpqrtuwxwyqykzTzB{}  8Au  A 		UYYq\__U34DELLUZZXY]\a\f\fgh\ikmoqsuvx}  BD  F "5==?4NRXY\bch  kE  IO  ]P  P"',,0EGWY\^dftuvxy  |F  vF  gG  I_  ci  _j  Zk  #m  nq  su  tu  su  nu  #v$&={<?XY^^_`bcd!3k-6PQRTXZ[]gQg6h!hlw  yJ  mK  LM  OS  UV  Xb  dh  Lh  mi   i  q  q  s  {  {  }  B  B  D yU);[=W=a=abd=efg%4E(F(P(PQS(TTYY^_Y`cn  pA  dB  dL  dL  MO  dP  dT  dT  YZ  dT  d[  d`  d`  fg  d`  dh  h"\\+=uc.YZ\]_iYiJjFklmprtstrtmtu1K@Q4RSTVWYcSc4d4n4noq4rr"%#!#*{;L/MaQRT^N^/_/i/ikm/n/r/rxy/r/z//  FG/  0HH ]]_ 	DH"#34NQY\defghifijL !<<)@&VYZ]\]Z]^a`a^aVaIbBceklxeyz  E  E  FG  IQ  ST  IT  U]  _`  U`  Ha  b  l  l  mo  qy  zB  pC  DL	D "4($ 0"4QT":(((#6 0E]
 
 	
	D 	Ds   A8r''r0c           	      	   t        j                         r| xj                  dz  c_        |d   j                  dd  }|d   j                  d d |d   j	                  d      c\  }}}d}t        j
                  |d   |d   z        d | }	d}
|d   }t        |d| j                  	      }d
|vst        j                         r	 | j                  |j                  dd      dz  dz         x|d
<   }t        |d| j                  |      |d
<   t        j                  | j                  |d
   j                  dd            j                  d||f      dz         dz   x}}|d   d d d df   t%        |d         z   }t'        j(                  |d   |d   d       d   }|t%              z  }t        j*                  t-        ||d         d d d df   j                  dd      |j                  dd      j/                  d      dz  dz
        j1                  d      j3                  ddd      j                  d||dz
  f      }t        j*                  |d
   d d d df   j                  dd      |j                  dd      j/                  d      dz  dz
        j1                  d      j3                  ddd      j                  d||dz
  f      }| j5                  t        j6                  |t%        |d
         d d dd f   fd            j9                         j;                  d      }| j                  dk  snt        j<                  |      }t'        j>                  |d d dd |	f   |d d d d |	f   |d d d d |	f         d   }|}tA        |dz
  dd      D ]:  }t        j6                  |d d d |f   |d d |dz
  gf   |d d |d f   z  fd      }< t        j6                  t        jB                  d      jE                  |      d   jG                  |j	                  d      ddd      |fd      }tI        |d d dd f   |d d d df   jK                         |d d dd f   z  |d   d d dd f         |d   d d dd f   z
  }|j;                  dd      t%        |d         j;                  dd      z
  jM                         jO                         }|t-        ||d         |||t%        |      dz  }tQ        d       |S )Nr)   r   r   r(   r   r   r   r   r   r   r   r   r   x_pixr!   bwd_flowr   r   r   r  r2   )NNgɿg?)r   flow_from_pose_static_lossflow_from_poserB   r   zjust doing static)8r"   r   rv   r4   r   rE   r   rk   ru   r6   r,   r   rn   r7   r	  r   r
  r  r   r   r   r-   r   r   r   permuterr   r#   r   r   r$   
procrustesrU   r   rF   r   warpr   r   r   r   ro   r   r   r   r;   rp   r   r   r   r   r   r   rA   r@   r   )-rw   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r  
corresp_uvrdseye_surfcorresp_surfcorresp_featr   
adj_transfrB   r\   adj_opt_flowr  r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r  r  r   s-                                                r   r  zFlowMap.forward_  s     "499a<9U#))"#.$U+11"15{57I7N7Nq7QAnnVAYvay%89'6B	 e$W&F$JZJZ[$(=(=(?15gooaQR>SVY>Y\_>_1``F#h&/;]aeaqaquv&wF#$%JJF@S@[@[\]_`@a0b0l0lmnqrtzp{0|  @A  1A  %B  CD  %D  D	 "'*1SbS51&Z9P2QQ
%%k'&:;|;TUYZ[\]VE]"}}VHVAY%?#2#%F%N%NqQR%ST^TfTfghijTkTuTuvwTxyzTz{|T|}  F  F  GI  J  R  R  ST  UV  WX  Y  c  c  de  gh  io  pq  iq  fr  s }}[%83B3%?%G%G!%LZM_M_`abcMdMnMnopMqrsMstuMuv~~  @B  C  K  K  LM  NO  PQ  R  \  \  ]^  `a  bh  ij  bj  _k  l22599l6R]^dReKfghijikgkKl=mnp3qrzz|  B  B  GK  B  L99S=Aeool.K| ((!AB{2B)C\RSTUVaRaEbdpqrst  vA  rA  eB  D  EF  G
vax"%dAuyy%"1"+eAqQRseGnUZ[\]^]_[_U`F`9abc/dud		599Q<??51)<CCEJJqMRTUWXZ[\abcde HQqrTN53B3<+?+?+A%!"++MkZfNghijkjlhlNmnoz  |C  pD  EF  GH  GI  EI  pJ  J'3'8'8R'@6+V`JaCbCgCghklnCo'o%x%x%z%%  &B""<q	:*D*E]
 	
 	!"#:r   rD   )__name__
__module____qualname__ri   r   r  r  __classcell__rz   s   @r   r`   r`   5   sK    6 '+< 04 V
p 15" j
X 15" ^
@ 15" z
r   r`   c                   D     e Zd Z	 	 	 	 	 	 	 	 	 	 d fd	ZddZddZ xZS )rs   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_layer  s     G#&..NN4T
  j(&..%%e
  g%&..r||=NO
  f$!

  *;iG r   zUsing torchvisionencoder)
pretrainedr6  r?   r   )r+   )r   rb            i   latentr)   F)
persistentlatent_scalingr(   )dtyper<  rb   )r;  r:  )r:  rb   r   r:  )r+  r  )%rh   ri   feature_scalerg   r   getattrtorchvisionmodelsmodelr   rm   conv1rI   r4   kernel_sizestriderc   r+   rX   fcavgpoollatent_size
num_layersindex_interpindex_paddingupsample_interpregister_bufferr"   emptyfloat32r   out_dim
ModuleListcudacombs_1combs_2last_conv_up)rw   backboner9  rL  rM  rN  rO  rA  rg   r4  rf   r7  r6  d1d2drz   s                   r   ri   zResnetFPN.__init__  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           
         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                  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  } | j(                  d    | j*                  d   t        j                  |d   |d   j                  dd  d            |d   z   j                               } | j(                  d    | j*                  d   t        j                  ||d   j                  dd  d            |d   z   j                               } | j(                  d
    | j*                  d
   t        j                  ||d   j                  dd  d            |d   z   j                               } | j(                  d    | j*                  d   t        j                  ||d   j                  dd  d            | j-                  |d         z   j                               }|S )Nr2   r   r)   r(         ?r   areaTscale_factorr*   align_cornersrecompute_scale_factorr?   nearest r   r!   r   r   )r3   r4   r6   r7   rA  r,   r   rE  rF  bn1rJ   rV   rL  rg   maxpoollayer1layer2layer3layer4rM  rW  rV  rX  )rw   r   custom_sizelatentsrb  	latent_sz	up_latents          r   r  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JJNN1JJOOAq??Q""JJ&&q)

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

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

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

!!!$ANN1 $ 1 1Z ?TAJ$$RS)	#DLLOodll1oammGBKPWXZP[PaPabdbePflv6w&x  zA  BD  zE  'E  &K  &K  &M  O	#DLLOodll1oammIgVXkN_N_`b`cNdjt6u&vw~  @B  xC  'C  &I  &I  &K  M	#DLLOodll1oammIgVXkN_N_`b`cNdjt6u&vw~  @B  xC  'C  &I  &I  &K  M	#DLLOodll1oammIgVXkN_N_`b`cNdjt6u&vw{  xI  xI  JQ  RT  JU  xV  'V  &\  &\  &^  `	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 )Nr2   r   r)   r(   r^  r   r_  Tr`  r?   rd  r   )r*   rb  r   r!   g       @)r3   r4   r6   r7   rA  r,   r   rE  rF  rg  rJ   rL  rg   rh  ri  rV   rj  rk  rl  rM  rU   rO  r"   r#   r=  r?  r   )rw   r   rm  rn  rb  ro  r\   s          r   r  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   )
resnet34Tr2   r   borderr   r^  Tr(  r?   rD   )r   r!  r"  ri   r  r  r#  r$  s   @r   rs   rs     s6     "OCb%f1%r   rs   rD   )rs  r   )8r"   rC  r   korniar.  einopsr   r   torch.nnr   r,   numpynpsysrandomtimeoscopyr   
matplotlibr   wandbr	   typingr
   r   r   r   r   r   collectionsr   pathrV   cotracker.utils.visualizerr   r   r   torch_kmeansr   r   r   rA   r:   r5   r   r@   r  shuffler^   r  rt   r
  Moduler`   rs   r&   r   r   <module>r     s        $ $        C C # + , G    
<	z@4]
 	
Rn
3
 W	
bii W	
vv%		 v%r   