
    0`cgL                        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/cheaper_flowmap/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!   h㈵> 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	unflattenr1   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_rG   )ms    r   init_weights_normalz%make_net.<locals>.init_weights_normal%   sF    7biiq(#''CfS['\ $  r   r*   r!   )ranger5   appendr   rM   ReLU
Sequentialapply)dimsrR   layersinets        r   make_netr\   $   s    ] F3t9Q; !biiQQqS	23bggi ! --
%CII!"Jr   c                   P     e Zd Z fdZi fdZi fdZi fdZi fdZi fdZ xZS )FlowMapc           	         t         |           || _        |j                  | _        d}t	        j
                  d|      | _        t	        j
                  |d      | _        t        ||dg      | _	        t        |dz  ddg      | _
        d| _        t        ||| j                  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)      rA   T)in_chuse_first_pool)padding)super__init__argstime_strider   rM   fmap_downprojfmap_downproj2r\   	depth_estcorr_weighter_perpointn_rigrig_predictorrV   PixelNeRFEncoderConv2d
resnet_enc
depth_convaffinities_convgeneral_confidence_conv)selfri   fdimaffinity_dim	__class__s       r   rh   zFlowMap.__init__2   sF   	  ++YYs40YYtA.!4Q-0&.Qr!}&=#
%tD&<= &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   c                    |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| j                  	      }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  }|
|z   }t        j                   |d   |d   d       d   }|t#        ||d   j%                  d            j'                  d      z  }| j)                  |d   j                  dd            j                  d||f      j+                         j-                  d      }t        j.                  | j1                  |d   j                  dd            j                  d||f      d      }t        j2                  |      }t5        d       ddlm}  |        t        j:                  t<        d d dd d d f   j?                  | j@                  ddd      tB        d d d d d d f   j?                  | j@                  ddd      tD        d d d d d d f   tF        d d dd d d f   z  j-                  d            d   }tI        |dz
  dd      D ]:  }t        jJ                  |d d d |f   |d d |dz
  gf   |d d |d f   z  fd      }< t        jJ                  t        jL                  d      jO                  |      d   j?                  |j                  d      ddd      |fd      }t        jP                  d|jS                         tU        |d d dgf   j?                  d|dd                  dd df   }tW        ||d         j-                  dd      }||d   z
  |d   j%                  d      z  jY                         j[                         }tW        t        jP                  dt\        d d d df   jS                         t\        d d dd f   z  tU        t<        d d dd f               dd df   |d   d d dd f         |d   d d dd f   z
  }d|v r	 t        t        j^                  t        t<        |d         j                  dd      |d   j                  dd      j%                  d      dz  dz
  d       j                  d||f      d!      }t        j`                  |d   j                  d            d tc        d"|dz  d#z  z         |d$<   t        t        j^                  t        t\        j                  dd      |d         j                  dd      |d   j                  dd      j%                  d      dz  dz
  d       j                  d||f      d%d&      }te        |d d |d$   f   d'|	      jS                         te        |d d |d$   f   d(|	      z  }t        jP                  d)|tU        |d d |d$   f               dd df   }tg        t        jP                  d*|d   d d df   |            j-                  dd      |d+<   nt5        d,       |tF        j                  dd      ji                  d      jk                  d      d      d    }|t        tF        d-| j@                  .      tm        |      tm        |      |||d   t        tD        |d         |d/   t        jP                  dt\        tU        t<                    dd df   tm        |d         tF        j                  dd      d0   tn        d d d f   z  ji                  d      tp        j                  d|      d1z  S )2Nrgbr   r)   r*   r   @   r~   	depth_inpb (t s) c x y -> b t (s c) x ysr   r+   r-    (b t) (s c) x y -> b (t s) c x yr   bfmappred_tracks
intrinsics-C6?mindimzsanity affinity as ones)	set_tracer!   r4   NNbtpij,btpj->btpi.rA   pred_visibilityx_pixr,   r.   b t c p 1 -> b p t c@    
track_idxsb t (x y) p 1 -> b p t x yr   b p t x y -> b p s t x yb p t x y -> b p t s x ybksnij,bksj->bksnibij,bksnj->bksnitrack_reprojsskipping point tracks sup(b o) t xy 1 -> b t o xy 1obwd_flow.N	rig_masks	res_depthdepthflow_from_poserD   r   corr_weights	flow_inp_
world_crdsrgb_crdslie_crds
lie_perpix)9r6   r   r"   linspacelongch_fstr   rj   r/   interpolaters   r8   softplusrt   r9   geometryget_world_rays	grid_samp	unsqueezesqueezerv   sigmoidclip	normalizeru   r$   printpdbr   efficient_procrusteseye_surfexpandro   corresp_surfr   r   rS   r#   eyetor=   inverserC   rB   squaremeanpose_perpixr0   randpermr   r   r<   summaxch_sec	poses_lier   )rw   model_inputoutimsizer   _n_trgtn_samprand_subset	low_imresr   img_inpfmap_outr   r   	rds_trackeye_surf_trackgeneral_confaffinity_embpdb_rD   rZ   point_track_surf_reprojpoint_track_reprojpoint_track_lossadj_opt_flow
eye_trackspose_trackspose_all_to_all_perpixtrack_surf_reprojsbg_poses                                  r   forwardzFlowMap.forwardJ   s	   5!'', '--bq1+e2D2I2I!2L1fay"nnQvay':1'<VDIIK	;{3F1I>	 e$'"BTEUEUV==11Eb1H1K!LVYcd'1SVZVfVfijkF JJt{6/B/J/J1Q/OPZZ[\^_`f]ghijjklmm	I% ++K,F{S_G`aefghi	"Yu[5O5Y5YZ\5]%^%f%fgh%ii 33K4G4O4OPQRS4TU__`acdekblmuuw||  BF|  G {{D$8$8V9L9T9TUVWX9Y$Z$d$defhijpgq$rxy{|4U;T5U)$& ,,hqAv.>.E.EdjjQSTVWY.Z\hijklmnin\o\v\vw{  xB  xB  CE  FH  IK  ]La!e$YqAv%66<<<FIIJLvax"%dAuyy%"1"+eAqQRseGnUZ[\]^]_[_U`F`9abc/dud		599Q<??51)<CCEJJqMRTUWXZ[\abcde #(,,/A%--/RUVdefhigjejVkVrVrsuv|}  AC  WD  SE  #F  GJ  KM  LM  KM  GM  #N$%<[=VW\\]^_`a0;}3MMQ\]nQoQyQyz|Q}}  G  G  I  N  N  P -?AcrcE@R@Z@Z@\]hijklkmim]n@nors{|}~  A  }A  tB  pC  !D  EH  IK  JK  IK  EK  !L  NY  Zf  Ng  hi  jk  jl  hl  Nm  o  p{  |C  pD  EF  GH  GI  EI  pJ  J K'A vhvay/I/Q/QRSTU/VWbcpWqWyWyz{|}W~  XI  XI  JL  XM  NO  XO  PQ  XQ  _g ")Aqj)+ADJ %{=/I/N/Nr/R STqUXY]`fhi`ijo`oYpUq rC!!--{7J7J2b7QRXYZR[0\0d0defgh0iju  wD  kE  kM  kM  NO  PQ  kR  k\  k\  ]_  k`  ab  kb  cd  kd  s{  #|  #F  #F  GH  JK  LR  IS  #T  Vr  uv  wK%+K#l:K8K,LMgjp%q%y%y%{  }C  DO  PQ  RU  Vb  Rc  Pc  Dd  e  BH  }I  &I"!&.BCYZ]^hijkno{k|i|^}Z~!  AD  EG  FG  EG  AG  "H#(6HUaIbcdefcfIghz){#|  $B  $B  CD  EF  $GC -.	))!B/33B7;;;B1EFtL!),H4::Vy)5M)#K0"<q	:$Z0,,'9+c(mTUXY[Z[Y[U[\{512#++Aa0:Yqv=NNSSTUV#--b8
 
 	
r   c                 H  ) |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         }
t        j                  t        j                  |d   d d d df         |d   d	z  fd      }|d   }t        |d
| j                        }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  }|
|z   }|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
        j-                  d      j/                  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
        j-                  d      j/                  ddd      j!                  d||dz
  f      }| j1                  t        j                  |t#        |d         d d dd f   fd            j3                         j5                  d      }||dk(  j7                  d      j+                  d       j9                         z  }	 	 	 t        d| jM                  t#        |d               z  jO                  d      d      }	 t%        jZ                  |d d dd d d f   jU                  | jV                  ddd      |d d d d d d f   jU                  | jV                  ddd      |d d d d d d f   |d d dd d d f   z  j5                  d            d   }	 tG        |dz
  dd      D ]:  }t        j                  |d d d |f   |d d |dz
  gf   |d d |d f   z  fd      }< t        j                  t        j`                  d      je                  |      d   jU                  |j                  d      ddd      |fd      }t        j                  tf        j$                  jh                  jk                  |dd dd df   d      |dd ddf   fd      } || j+                  d      z  j!                  d|| jV                  f      jm                  d      }!t        j`                  d      d   jU                  |||!j                  d      dd      je                  |!      }"tf        j$                  jh                  jo                  |!dd df         |"dd dd df<   |!ddd f   |"dd ddf<   tq        t        jr                  d |"d d d df   ju                         |"d d dd f   z  tw        |d d dd f               dd df   |d   d d dd f         |d   d d dd f   z
  }#d!|v r	 t        t        j(                  t        ||d         j                  dd      |d!   j                  dd      j+                  d      dz  dz
  d"#      j!                  d||f      d$      }$t        jH                  |d!   j                  d            d ty        d%|dz  d&z  z         |d'<   t        t        j(                  t        |"j                  dd      |d         j                  dd      |d!   j                  dd      j+                  d      dz  dz
  d"#      j!                  d||f      d(d)      }%t{        |%d d |d'   f   d*|      ju                         t{        |%d d |d'   f   d+|      z  }&t        jr                  d,|&tw        |$d d |d'   f               dd df   }'t}        t        jr                  d-|d   d d df   |'            j5                  dd      |d.<   nt        d/       ||j                  dd      jm                  d      j?                  d      d      d    }(|t        |d0| jV                  1      t#        |      t#        |      |#||d   t        ||d         |d   t        jr                  d |"tw        |            dd df   t#        |d         |j                  dd      d2   | d d d f   z  jm                  d      |!j!                  d|      d3z  S )4Nr|   r   r)   r*   r   r}   r   r   g      I@r   r   r   r+   r   r   r   r   r   r!   r   r   r   r         ?b t x o -> (b o) t x 1rA   r4   r   .epsNNNr   r   r,   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   )@r6   r   r"   r   r   r   r#   
zeros_liker   rj   r/   r   rs   r8   r   rt   r9   r   r   r   r0   r   r   permutern   r   r   allfloatr$   roundr   aranger5   cudarS   r   stackrp   softmax
procrustesgatherr   ro   rl   r   	enumeratezipr   rT   r   korniaconversionsrotation_matrix_to_quaternionr   quaternion_to_rotation_matrixrB   r=   r   rC   r   r   r<   r   )*rw   r   r   r   r   r   r   r   r   r   r   flow_inpr   r   r   r   
corresp_uvrdsr   r   corresp_featr   rig_mask_inpr   r   rD   rig_inpsubset_rZ   rig_maskidxposer   r   r   r   r   r   r   r   r   idxss*                                            @r   forward_zFlowMap.forward_   si   5!'', '--bq1+e2D2I2I!2L1fay"nnQvay':1'<VDIIK	 ;{3F1I>	99e..{:/Fq!t/LMkZdNefiNijklm e$'"BTEUEUV==11Eb1H1K!LVYcd'1SVZVfVfijkFJJt{6/B/J/J1Q/OPZZ[\^_`f]ghijjklmm	I% "'*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#q(=(="(=(E(O(OPR(S'S&Z&Z&\\ & #c$*<*<VKPVDW=X*Y&Y%b%bce%f  iA  B	   44hqAv6F6M6MdjjY[\^_a6bdpqrstuvqvdwd~d~  @D  @J  @J  KM  NP  QS  eT%a!e,YqAv->>DDDNQQRT$ vax"%dAuyy%"1"+eAqQRseGnUZ[\]^]_[_U`F`9abc/dud		599Q<??51)<CCEJJqMRTUWXZ[\abcde IIv::XXY^_bcedecefhghfh_hYinrXstyz}  A  @A  A  BD  {D  uE  F  GI  J		 3 3B 77BB1a

^TXXYZ[
iil>299!F:??SUCVWYZ\]``akl!'!<!<!Z!Z[efijlkljlfl[m!nC2A2I!+CF!3C2I -?AcrcE@R@Z@Z@\]hijklkmim]n@nors{|}~  A  }A  tB  pC  !D  EH  IK  JK  IK  EK  !L  NY  Zf  Ng  hi  jk  jl  hl  Nm  o  p{  |C  pD  EF  GH  GI  EI  pJ  J K'A vhvay/I/Q/QRSTU/VWbcpWqWyWyz{|}W~  XI  XI  JL  XM  NO  XO  PQ  XQ  _g ")Aqj)+ADJ %{=/I/N/Nr/R STqUXY]`fhi`ijo`oYpUq rC!!--{7J7J2b7QRXYZR[0\0d0defgh0iju  wD  kE  kM  kM  NO  PQ  kR  k\  k\  ]_  k`  ab  kb  cd  kd  s{  #|  #F  #F  GH  JK  LR  IS  #T  Vr  uv  wK%+K#l:K8K,LMgjp%q%y%y%{  }C  DO  PQ  RU  Vb  Rc  Pc  Dd  e  BH  }I  &I"!&.BCYZ]^hijkno{k|i|^}Z~!  AD  EG  FG  EG  AG  "H#(6HUaIbcdefcfIghz){#|  $B  $B  CD  EF  $GC -.	))!B/33B7;;;B1EFtL!),H4::Vy)5M)#K0"<q	:$Z0,,'9+c(mTUXY[Z[Y[U[\{512#++Aa0:Yqv=NNSSTUV#--b8
 
 	
r   c                 T   |d   j                   dd  }|d   j                   d d |d   j                  d      c\  }}}d}t        j                  d|d   |d   z  dz
  |      j	                         }t        j
                  t        j                  |d   d d d df         |d   dz  fd      }	t        |d	   |d         }
t        j
                  |d   |
|	fd      }t        j
                  |d   |
d
z  dz
  fd      }t        |d| j                        }t        j                  dt        j                        5  t        j                  | j                  |j                  dd      dz  dz         |d      }d d d        t        j!                         d| j                  |      |d<   t        j"                  | j%                  |d   j                  dd            j'                  d||f      dz         }|
|z   }|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      j1                  d      dz  dz
        j3                  d      j5                  ddd      j'                  d||dz
  f      }t        j.                  |d   d d d df   j                  dd      |j                  dd      j1                  d      dz  dz
        j3                  d      j5                  ddd      j'                  d||dz
  f      }| j7                  t        j
                  |t)        |d         d d dd f   fd            j9                         j;                  d      }||dk(  j=                  d      j1                  d       j!                         z  t)        |d         z  }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        j
                  |d d d |f   |d d |dz
  gf   |d d |d f   z  fd      }< t        j
                  t        jB                  d      jE                  |      d   jG                  |j                  d      ddd      |fd      }tI        t        jJ                  d|d d d df   jM                         |d d dd f   z  tO        |d d dd f               dd d f   |d   d d dd f         |d   d d dd f   z
  }d!|v rR	 t        t        j.                  t        ||d         j                  dd      |d!   j                  dd      j1                  d      dz  dz
  d"#      j'                  d||f      d$      }tQ        |d%|      jM                         tQ        |d&|      z  }t        jR                  |d!   j                  d            d tU        d'|dz  d(z  z         |d)<   t        jJ                  d*|tO        |d d |d)   f               dd d f   }tW        t        jJ                  d+|d   d d df   |            j;                  dd      |d,<   |t)        |      t)        |      |||d	   t        ||d         |d   t        jJ                  d|tO        |            dd d f   t)        |d         d-	z  S # 1 sw Y   xY w).Nr|   r   r)   r*   i  r   r        @@r   
   r   r   r   )device_typedtyper   r+   r   r   r   r   r   r!   r   r   r   r   rig_flow_masksr4   r   zbtij,btpj->btpi.rA   r   r,   r   r   zb t x y -> b s t x yzb t x y -> b t s x yr   r   r   zbsnij,bksj->bksnir   r   )	r   r   r   rD   r   r   r   r   r   ),r6   r   r"   r   r   r#   r   r   r   rj   autocastfloat16r/   r   rs   r8   r   r   rt   r9   r   r   r   r0   r   r   r   rn   r   r   r   r   rS   r   r   r   rB   r=   r   rC   r   r   r   r<   )rw   r   r   r   r   r   r   r   r   r   r   rgbdfr   r   r   r   r   r   r   r   r  r   rD   rZ   r   r   pose_all_to_allr   s                               r   r	  zFlowMap.forward_)  s   5!'', '--bq1+e2D2I2I!2L1fnnQvay':1'<VDIIK 99e..{:/Fq!t/LMkZdNefiNijklm;{3F1I>			;u-iA!D))[/	"Q?B'"BTEUEUV^^emmD 	i}}T__W__Qq5I"5LR5O%PQW]ghH	i ((89[^b^n^nqrsFJJt{6/B/J/J1Q/OPZZ[\^_`f]ghijjk	I% "'*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#q(=(="(=(E(O(OPR(S'S&Z&Z&\\_efq  sC  gD  `E  E ##Xa;.>%?aPQR]oA^`lmnopq|m|`}  AB  Cvax"%dAuyy%"1"+eAqQRseGnUZ[\]^]_[_U`F`9abc/dud		599Q<??51)<CCEJJqMRTUWXZ[\abcde ->uQssU|?S?S?UV[\]^_^`\`Va?abefnopqrqsosftbu vwz{}|}{}w} ~  AL  MY  AZ  [\  ]^  ]_  [_  A`  b  cn  ov  cw  xy  z{  z|  x|  c}  } K'A vhvay/I/Q/QRSTU/VWbcpWqWyWyz{|}W~  XI  XI  JL  XM  NO  XO  PQ  XQAI"LLUIVWYZ[aXbLce{~J$U+AFKSSUV\]bcy  }C  WD  DO %{=/I/N/Nr/R STqUXY]`fhi`ijo`oYpUq rC!&.A/RUV`abcfgsctatVuRv!wx{|~}~|~x~!#(6HUaIbcdefcfIghz){#|  $B  $B  CD  EF  $GC y)5M)#K0"<q	:$Z0,,'8s8}McRTSTRTfU{512
 
 	
I	i 	is   9>\\'c                    |d   j                   dd  }|d   j                   d d |d   j                  d      c\  }}}d}t        j                  d|d   |d   z  dz
  |      j	                         }d}	t        |d   |d         }
t        j                  |d   |
d	z  dz
  fd      }t        |d
| j                        }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  }|
|z   }|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
        j+                  d      j-                  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
        j+                  d      j-                  ddd      j                  d||dz
  f      }| j/                  t        j                  |t!        |d         d d dd f   fd            j1                         j3                  d      }||dk(  j5                  d      j)                  d       j7                         z  }t        d| j9                  t!        |d               z  j;                  d      d      }t#        j<                  |d d dd |f   j?                  | j@                  ddd      |d d d d |f   j?                  | j@                  ddd      |d d d d |f   |d d dd |f   z  j3                  d            d   }tC        |dz
  dd      D ]:  }t        j                  |d d d |f   |d d |dz
  gf   |d d |d f   z  fd      }< t        j                  t        jD                  d      jG                  |      d   j?                  |j                  d      ddd      |fd      }t        j                  tH        j"                  jJ                  jM                  |dd dd df   d      |dd ddf   fd      }||j)                  d      z  j                  d|| j@                  f      jO                  d      }t        jD                  d      d    j?                  |||j                  d      dd      jG                  |      }tH        j"                  jJ                  jQ                  |dd df         |dd dd df<   |ddd f   |dd ddf<   tS        t        jT                  d!|d d d df   jW                         |d d dd f   z  tY        |d d dd f               dd df   |d   d d dd f         |d   d d dd f   z
  }d"|v r	 t        t        j&                  t        ||d         j                  dd      |d"   j                  dd      j)                  d      dz  dz
  d#$      j                  d||f      d%      }t        jZ                  |d"   j                  d            d t]        d&|dz  d'z  z         |d(<   t        t        j&                  t        |j                  dd      |d         j                  dd      |d"   j                  dd      j)                  d      dz  dz
  d#$      j                  d||f      d)d*      }t_        |d d |d(   f   d+|      jW                         t_        |d d |d(   f   d,|      z  }t        jT                  d-|tY        |d d |d(   f               dd df   }ta        t        jT                  d.|d   d d df   |            j3                  dd      |d/<   ||j                  dd      jO                  d      jc                  d      d      d    } |t        |d0| j@                  1      t!        |      t!        |      |||d   t        ||d         |d   t        jT                  d!|tY        |            dd df   t!        |d         |j                  dd      d2   |d d d f   z  jO                  d      |j                  d|      d3z  S )4Nr|   r   r)   r*   i  r   r}   r   r  r   r   r   r+   r   r   r   r   r   r!   r   r   r   r   r   r   r   r4   r   .rA   r   r   r   r   r,   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   )2r6   r   r"   r   r   r   r#   r   rj   r/   r   rs   r8   r   rt   r9   r   r   r   r0   r   r   r   rn   r   r   r   r   rp   r   r   r   ro   rS   r   r   r   r   r   r   r   rB   r=   r   rC   r   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   rD   rZ   r   r   r   r   r   r   r   r   r   s!                                    r   r	  zFlowMap.forward_j  s   5!'', '--bq1+e2D2I2I!2L1fnnQvay':1'<VDIIK	 ;{3F1I>	))[/	"Q?B'"BTEUEUV==11Eb1H1K!LVYcd'1SVZVfVfijkFJJt{6/B/J/J1Q/OPZZ[\^_`f]ghijjklmm	I% "'*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#q(=(="(=(E(O(OPR(S'S&Z&Z&\\ c$"4"4VK<O5P"QQZZ[]^`xy	 ##Xa;.>%?%F%FtzzRTUWXZ%[]ijklmnyjy]z  ^B  ^B  CG  CM  CM  NP  QS  TV  ^W&21Q{?&CIaPQPRS^N^D_&_%e%ejn%e%orrsuvax"%dAuyy%"1"+eAqQRseGnUZ[\]^]_[_U`F`9abc/dud		599Q<??51)<CCEJJqMRTUWXZ[\abcde IIv::XXY^_bcedecefhghfh_hYinrXstyz}  A  @A  A  BD  {D  uE  F  GI  J		 3 3B 77BB1a

^TXXYZ[
iil>299!F:??SUCVWYZ\]``akl!'!<!<!Z!Z[efijlkljlfl[m!nC2A2I!+CF!3C2I -?AcrcE@R@Z@Z@\]hijklkmim]n@nors{|}~  A  }A  tB  pC  !D  EH  IK  JK  IK  EK  !L0;L0I!AB$0ORR]^eRfghijikgkRlm K'A vhvay/I/Q/QRSTU/VWbcpWqWyWyz{|}W~  XI  XI  JL  XM  NO  XO  PQ  XQ  _g ")Aqj)+ADJ %{=/I/N/Nr/R STqUXY]`fhi`ijo`oYpUq rC!!--{7J7J2b7QRXYZR[0\0d0defgh0iju  wD  kE  kM  kM  NO  PQ  kR  k\  k\  ]_  k`  ab  kb  cd  kd  s{  #|  #F  #F  GH  JK  LR  IS  #T  Vr  uv  wK%+K#l:K8K,LMgjp%q%y%y%{  }C  DO  PQ  RU  Vb  Rc  Pc  Dd  e  BH  }I  &I"!&.BCYZ]^hijkno{k|i|^}Z~!  AD  EG  FG  EG  AG  "H#(6HUaIbcdefcfIghz){#|  $B  $B  CD  EF  $GC 	))!B/33B7;;;B1EFtL!),H4::Vy)5M)#K0"<q	:$Z0,,'9+c(mTUXY[Z[Y[U[\{512#++Aa0:Yqv=NNSSTUV#--b8
 
 	
r   c                    |d   j                   dd  }d}|d   j                   d d |d   j                  d      c\  }}}d}t        j                  |d   |d   z        d | }	| j                  j
                  r1t        j                  d|d   |d   z  dz
  |      j                         }	t        j                  t        j                  |d   d d d df         |d   d	z  fd      }
t        |d
   |d         }t        j                  |d   ||
fd      }t        |d| j                        }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         }||z   }|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      j1                  d      dz  dz
        j3                  d      j5                  ddd      j'                  d||dz
  f      }t        j.                  |d   d d d df   j!                  dd      |j!                  dd      j1                  d      dz  dz
        j3                  d      j5                  ddd      j'                  d||dz
  f      }| j7                  t        j                  |t)        |d         d d dd f   fd            j9                         j;                  d      }t        d| j=                  t)        |d               z  j?                  d      d      x}}t)        t        j                  t        ||d         j!                  dd      |      j'                  d|j                   d d             }d }tA         |ddtC        d            d| jD                  ||dz
        jG                         x}}t+        jH                  t        tK        t        |d d dd f   |d         t        |d|            d |j                  d      !      t        tK        t        ||d         t        |d|            d |j                  d      !      t        tK        t        ||d         t        |d|            d |j                  d      !      tK        t        |d d dd f   |d         |      j3                  d      z  j;                  d            d   }tM        |dz
  dd      D ]:  }t        j                  |d d d |f   |d d |dz
  gf   |d d |d f   z  fd      }< t        j                  t        jN                  d"      jQ                  |      d#   jS                  |j                  d      ddd      |fd      }t        j                  tT        j*                  jV                  jY                  |d$d d%d d%f   d&'      |d$d d%df   fd      }||j1                  d      z  j'                  d|| jD                  f      j[                  d      }t)        t        j                  t        ||d         j!                  dd      |d      j'                  d||f            }t        jN                  d"      d(   jS                  |||j                  d      dd      jQ                  |      }tT        j*                  jV                  j]                  |d$d d"f         |d$d d%d d%f<   |d$d"d f   |d$d d%df<   t_        t        j`                  d)|d d d df   jc                         |d d dd f   z  te        |d d dd f               d$d d%f   |d   d d dd f         |d   d d dd f   z
  } d*|v rt        t        j.                  t        ||d         j!                  dd      |d*   j!                  dd      j1                  d      dz  dz
  d+,      j'                  d||f      d-      }!t        t        j.                  t        |j!                  dd      |d         j!                  dd      |d*   j!                  dd      j1                  d      dz  dz
  d+,      j'                  d||f      d.d"/      }"tA        |"d0|      jc                         tA        |"d1|      z  }#t        j`                  d2|#te        |!            d$d d%f   }$tg        t        j`                  d3|d   d d df   |$            j;                  dd      |d4<   ||j!                  dd      j[                  d      ji                  d5      d      d    }%t        j`                  d)|te        |            d$d d%f   }&t        j`                  d)"j5                  dddd%d"      te        !j5                  dddd%                  d$d d%f   }'t        j.                  |d   j!                  dd      |d*   j!                  dd      j1                  d      dz  dz
  d+,      j'                  d||f      j5                  ddd%dd"      j3                  d      }(t        j                  t        jj                  g d6      jm                         jG                         d(   jS                  |||(j                  d      d      |!d d d d dgf   |!z
  j5                  dddd%      fd      j1                  d      })t)        |d         }*|i d7t        |d8| jD                  9      d:t)        |      d;t)        |      d<| d=t        j                  |j1                  d      |)fd>      d?t        j                  |&|'fd      d@t        j                  |*|(fd      dA|j'                  d|      dB|j'                  d>|      dC|dD|dE|%d
|d
   dFt        ||d         dE|%dG|d   z  S )HNr|   r   r}   r)   r*   i,  r   r   r  r   r   r   r   r+   r   r   r   r   r   r!   r   r   r          @r   c                     t        j                  t        j                  t        d      D cg c]  }t        j                  | dz   |dz
  |      ! c}       j                  dd      S c c}w )Nr)   g{Gz?r*   )r"   r   meshgridrS   r   r8   )r   enr   s       r   r   z"FlowMap.forward_.<locals>.<lambda>  sV    U^^ejklem=n`aennQsUSTUXSXYZ>[=n-o!p!x!xyz{|!} =ns   $A(g}U\6@zc p -> (b o) t p 1 c)r   r   tz(b s) t p 1 c -> b t (p s) 1 c)r   zb t c (p s) 1 -> (b s) t p c)pr4   r   .rA   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   	poses_allrD   r   r   )7r6   r   r"   r   ri   overfitr   r   r#   r   r   r   rj   r/   r   rs   r8   r   rt   r9   r   r   r   r0   r   r   r   rn   r   r   rp   r   r   r   ro   r   r   r   rS   r   r   r   r   r   r   r   r   rB   r=   r   rC   r<   r   tensorr   )+rw   r   r   r   r   r   r   r   r   r   r   r   r  r   r   r   r   r   r   r   r  r   affinity_maskaffinity_mask_unnormget_xpixsample_locsuniform_grid_pixrD   rZ   r   r   r   r   r   r   r   r   r   r   world_crds_tracks
rgb_tracks
lie_tracksr   s+                                              r   r	  zFlowMap.forward_  sG   5!'',	 '--bq1+e2D2I2I!2L1fnnVAYvay%89'6B99ENN1VAYvay=PQR=RSY,Z,_,_,ak 99e..{:/Fq!t/LMkZdNefiNijklm;{3F1I>			;u-iA!D>AQAQR==q1CB1Fr1I!J6Wab'1SVZVfVfijkF JJt{6/B/J/J1Q/OPZZ[\^_`f]ghijjk	I% "'*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 ,5s4;M;MfU`agUhNi;j7j6s6stv6w  zR  ,S  	S*Q]]6-q	+J+R+RSTUV+WXabllmno|  pC  pC  DF  EF  pG  H  I
 ~)/1S\0JKadhdndnqru{|}u})~  *D  *D  *F  	F& ##ix!"~fQi @;Wwz{A|}  ]  `k  `p  `p  qr  `s  ti|F1I >;Wwz{A|}  ]  `k  `p  `p  qr  `s  ti|F1I >;Wwz{A|}  ]  `k  `p  `p  qr  `s  tf1!AB$7	!EkRZZ[\]^_c_chl_c_m	
  vax"%dAuyy%"1"+eAqQRseGnUZ[\]^]_[_U`F`9abc/dud		599Q<??51)<CCEJJqMRTUWXZ[\abcde IIv::XXY^_bcedecefhghfh_hYinrXstyz}  A  @A  A  BD  {D  uE  F  GI  J	#I$7$7$;;FFq!DJJX\\]^_
AMM&IaL*I*Q*QRSTU*VW]cmnxxyz|}  E  |F  G  H
iil>299!F:??SUCVWYZ\]``akl!'!<!<!Z!Z[efijlkljlfl[m!nC2A2I!+CF!3C2I -?AcrcE@R@Z@Z@\]hijklkmim]n@nors{|}~  A  }A  tB  pC  !D  EH  IK  JK  IK  EK  !L0;L0I!AB$0ORR]^eRfghijikgkRlm K' vhvay/I/Q/QRSTU/VWbcpWqWyWyz{|}W~  XI  XI  JL  XM  NO  XO  PQ  XQ  _g ")Aqj)+ADJ!!--{7J7J2b7QRXYZR[0\0d0defgh0iju  wD  kE  kM  kM  NO  PQ  kR  k\  k\  ]_  k`  ab  kb  cd  kd19#;;D9Q&z;RTpstvK%+K8RU[%\%d%d%fgmny  {U  X^  h_  &_"!&.BCYZ]^hZi!jknoqpqoqkq!r#(6HUaIbcdefcfIghz){#|  $B  $B  CD  EF  $GC --a377;??A?FqIJ4P\\"4[XOPSTVUVTVPVW
!LL);K<O<OPQRSTUVWXY<Z[^_i_q_qrstuvwxy_z[{|  ~A  BD  CD  BD  ~D  E==U!3!;!;Aa!@]A[AcAcdefgAhArArsuAvwxAxyzAzIQSS\S\]^`abh_iSjkrkrstuvwxyz{|k}  F  F  GI  J 	YYY 7 = = ? D D F~ V ] ]^_`fgqgvgvwygz{} ~!+AaG!4Z!? H H1Qq Q ""$&&/im 	 +e,- 
	-0LtzzZ
 y)	

 F5M
 \
 uyy*"6"6r"::!FrJ
 J/@#A"E
 uyy(:!6r:
, --b8-
. +//6:/
0 x1
2 3
4 G5
6 K07
8 F<q	:9
: G;
< Z0=
 
 	
r   )__name__
__module____qualname__rh   r   r	  __classcell__rz   s   @r   r^   r^   1   sC    C0 (* O
f )+ G
X )+ >
B )+ b
H )+ x
r   r^   c                   <     e Zd Z	 	 	 	 	 	 	 	 	 	 d fd	ZddZ xZS )rq   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_layerX  s     G#&..NN4T
  j(&..%%e
  g%&..r||=NO

 	 f$!
  **QT]*]^^r   zUsing torchvisionencoder)
pretrainedr?  rA   r   r   )r   r~         ra   i   latentr*   F)
persistentlatent_scalingr)   )r  ra   )r4  r`   )rg   rh   feature_scalere   r   getattrtorchvisionmodelsmodelr   rr   conv1rG   r6   kernel_sizestriderf   r.   rV   fcavgpoollatent_size
num_layersindex_interpindex_paddingupsample_interpregister_bufferr"   emptyfloat32r   )rw   backbonerB  rS  rT  rU  rV  rH  re   r=  rd   r@  r?  rz   s                r   rh   zPixelNeRFEncoder.__init__I  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 )Nr4   r   r*   r)   r   r+   areaT)scale_factorr-   align_cornersrecompute_scale_factor)devicerA   znearest r   )r-   r^  r   r!   r  )r5   r6   r8   r9   rH  r/   r   r   rE  r`  rL  rM  bn1rH   rS  re   maxpoollayer1rT   layer2layer3layer4latentsrT  rS   rV  r"   r#   rG  r   )rw   r   custom_sizerg  r^  	latent_szrZ   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   )
resnet34Tr4   r+   r,   r+   r   Tr1  rA   N)r)  r*  r+  rh   r   r,  r-  s   @r   rq   rq   H  s/     "J
X0%r   rq   rk  )9r"   rJ  r   r   r7  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pathrT   cotracker.utils.visualizerr   r   r   torch_kmeansr   r   r   rC   r<   r7   r   rB   warpr\   Moduler^   rq   r'   r   r   <module>r     s        $  $        C C # " + , G    >uC2T
 C	RoU
bii U
n}%ryy }%r   