
    hS                     4   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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c 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m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%m&Z' d dl(m)Z* d dl+m,Z, dd	Z-d
 Z.ddd g fdZ/y)    N)cm)tqdm)	make_griddraw_keypoints)	rearrangerepeat)Image)spatial)defaultdictc           	      f    t        | d|"t        | j                  d      dz              S |      S )Nz... (x y) c -> ... c x y      ?x)r   intsize)srcr   s     9/home/cameronsmith/repos/old_contrastive_pointrack/vis.py<lambda>r      s2    9S)Cabajc#((SU,Y[J\F]r pqr     c                     t        | d      S )Nz... c x y -> ... (x y) cr   r   s    r   r   r      s    9Q9: r    c	                 00  8 |||fD 	
cg c]@  }	|	j                         D 
ci c]"  \  }
}|
t        |j                        r|d d n|$ c}}
B c}}
}	\  }}}t        |d   j	                  dd      j                  dddd      j                        }|d   j                  d      |d   j                  d      z  |d<   |d   j                  d      }|d   j                  dd  }d }t        |j                               D ]  \  }
}t        |      t        k7  r't        |j                        r|j                  d	
      }d|
v r|||
dz   <   d|
v sPd|
vsUt        j                   t        j                  d      |j                         j                         |j                         j!                         z              j#                  d      dd df   ||
dz   <    i }t%        |d   j                         j	                  dd      j'                         dz  dz   |      |d<   t%        |d   j                         j	                  dd      j'                         dz  dz   d      |d<   d|v rEt%        |d   j                         j	                  dd      j'                         dz  dz   d      |d<   d|v rt(        j*                  j,                  j/                  |d   dd df         j	                  dd      j                  dddd      dz  dz   }|d   ddd f   j	                  dd      j                  dddd      dz  dz   }t%        |j'                         |d      |d<   t%        |j'                         |d      |d<   d |v r0t%        t1        |d    |d         j	                  dd      d!      |d"<   d#|v r0t%        t1        |d#   |d         j	                  dd      d!      |d$<   d%|v r,|}t%        t3        |d%   d&|d   '      j'                         |d%   j                  d            |d(<   t%        t5        j6                  |d)   d   |      j9                  d      t1        |d%   ddd f   |d         z  j	                  dd      j'                         |d%   j                  d            |d*<   t%        t5        j6                  |d   ddd f   |      j9                  d      t1        |d%   ddd f   |d         z  t5        j6                  |d)   d   |      j9                  d      z  j	                  dd      j'                         |d%   j                  d            |d+<   t%        t3        |d%   j	                  dd      t5        j6                  |d   j	                  dd      |      j	                  dd,      j                  ddd      j9                  d      dz  dz   z  d-|d   '      j'                         |d%   j                  d            |d.<   d/|v r`t%        |d/   j                         j	                  dd      j                  ddd      j;                  d,|      j'                         |      |d0<   d1|v r`t%        |d1   j                         j	                  dd      j                  ddd      j;                  d,|      j'                         |      |d2<   d3|v r`t%        |d3   j                         j	                  dd      j                  ddd      j;                  d,|      j'                         |      |d4<   d5|v rt%        |d5   j                         j	                  dd      j                  ddd      j;                  d,|      j'                         |d6      |d7<   t%         ||d5         j                         j	                  dd      j                  ddd      j;                  d,|      j'                         |d6      |d8<   d)|v r@t%        |d)   j	                  dd      j                         j'                         d6|9      |d:<   d;|v r@t%        |d;   j	                  dd      j                         j'                         d6|9      |d<<   d=|v r<t=        j>                  t%        |d=   j	                  dd      |dz
              d>z  |d?<   d@|v r/t%        t3        |d@   dA      |d@   j                  d            |dB<   dC|v rt        j@                  |dC         jC                         smt=        j>                  t%        |dC   j                  dDdE      j	                  dd      j                  ddd      j;                  d,|      |dz
              d>z  |dF<   dG|v r|dG   }t3        |j	                  dd      dH      }|j                  \  }}}}|jE                  ||d,      }|jG                  dd6I      }||z
  }t        jH                  ||jK                  dd            ||z  dz
  z  }t        jL                  |      \  }}}t        dJ|      }t        jH                  |d d d d d |f   jK                  dd      |      }t%        t3        |d d d df   j'                         dK|j                  d,      |j                  d      L      |dG   j                  d      d6      |dM<   |dkD  ret%        t3        |d d dd f   j'                         dK|j                  d,      |j                  d      L      |dG   j                  d      d6      |dN<   dO|v r:dPdQgd d D ]/  } |dO| z      }!|!j                  d      dk  r[t        jN                  |!t        jP                  |!d d d d dgf         jS                  d,d,d|!j                  d      z
  d,d,      fd      }!t3        |!j	                  dd      dH      }|j                  \  }}}}|jE                  ||d,      }|jG                  dd6I      }||z
  }t        jH                  ||jK                  dd            ||z  dz
  z  }t        jL                  |      \  }}}t        dJ|      }t        jH                  |d d d d d |f   jK                  dd      |      }t%        t3        |d d d df   j'                         dK|!j                  d,      |!j                  d      L      |dO   j                  d      d6      |dR| z   <   |dkD  st%        t3        |d d dd f   j'                         dK|!j                  d,      |!j                  d      L      |dO   j                  d      d6      |dS| z   <   2 dT|v r~	 |dT   j                  d      dUz  dk(  rdVndW8tU        |dT   j                  d      8dz  z        }"88f}tV        jX                  d|d   d|d   f   j[                  t\              jK                  ddd      }#t        j                  tW        j^                  |#d,X      ja                               jc                         }#|#t        jd                  |d   dz
  |d   dz
  g      z  }#8fdY}$t=        j>                  t%         |$|dT   |dZ   j9                  d,      z        |#j                  ddd      d    jg                         z
   |$|dZ   j9                  d,            z  |"            d>z  |d[<   d\|v r,	 t%        |d\   j	                  dd      d d d f   d]d6      |d^<   d_|v r	 |d_   |dT   z
  |dZ   j9                  d,      z  ji                         }%t%        t3        |%d`88L      "      dz  }&|&dg   jS                  dd,d,      |da<   |&dg   jS                  dd,d,      |db<   t=        j>                  t%         $|d_   |dZ   j9                  d,      z        #j                  ddd      d    jg                         z
   |$|dZ   j9                  d,            z  |"            d>z  |dc<   dd|v rt%         |$|dd   de         |"      |df<   dg|v r	 dh|v rt3        |dh   di88L      dg   }|jG                  dd6I      }||z
  }t        jH                  ||jK                  dd            |j                  d,      dz
  z  }t        jL                  |      \  }}}t        d|j                  d            }t        jH                  |d d d d d |f   jK                  dd      |      }t%        t3        |j'                         dj88L      d6k      |dl<   dm|v rt        jN                  t(        j*                  j,                  jm                  |dm   dd dd df   dno      |dm   dd dd,f   fd,      }(t(        j*                  j,                  j/                  |(dd df         dz  dz   }|(ddd f   dz  dz   }t%        t3        |j'                         dp88L      |(j                  d      d      |dq<   t%        t3        |j'                         dp88L      |(j                  d      d      |dr<   t        jn                  t3        |ds   j'                         dt88L      d d df   dz  dz   t3        |j'                         du88L      d d dd,f   t3        |j'                         du88L      d d dd,f   fd      j	                  dd      })t%        |)dd      |dv<   dw|v r	 dx}g }*dy|v r	 |dz   j9                  d      }+d},tq        t        |+d               D ]  }-ts        |-       |+|,|-d d d dd,f   j'                         j                         }.t+        jt                  |.|dw   d d dgf   jw                         |dw   z  d d d d d dd,f   j                         |,         d   }/ty        jz                  |dw   |,d d d dd,f   j                         j!                         |.j!                               d d D 	cg c]  }	t        j                  |	       c}	\  }0}1|0|1z
  j}                         jG                         j                         }2ts        d{|, d||2       t        j                  d}|2i|~       t        j                         }3|3j                  dd      }4 |4j                  |.j                         j                  d      ddd  |4j                  |/j                         j                  d      ddd t        |.j                         |/j                               t        |.j                         |/j                               }6}5|4j                  |5|6       |4j                  |5|6       |4j                  |5|6       t        j                          t        j                          |3j                  j                          tW        j                  |3j                  j                         tV        j                        }7|7j                  |3j                  j                         d d d,   dz         }7|*j                  t        j                  |7      j                  ddd      d>z         t        j                          t        jN                  |*d      |d|z   <    	 ts        dts        t        |                   |j                         D ]  \  }
}ts        |
|j                          t        j                  |j                         D 
ci c]w  \  }
}||
z   t        j                  |j                  ddd      j]                         j'                         j                  dd      j                         j!                               y c}}
       ts        d       |S c c}}
w c c}}
}	w c c}	w c c}}
w )N    rgbr            r   c                     d| dz   z  S )Nr   g:0yE> r   s    r   r   zwandb_summary.<locals>.<lambda>(   s    Q$Z r   gMbP?)mindepth_rawrawmagma.visr   )nrowz
ref/rgb_gtimgszref/rgb_warpimgs_premaskzref/rgb_warp_premask
lie_perpix      F)r(   	normalizezest/poses_lie_rot_perpixzest/poses_lie_trans_perpixseg_imgsd   zref/seg_imgsseg_estzest/seg_imgs	rig_maskszb t o (x y) 1 -> (b t o) 1 x yr   zest/rig_maskscorr_weightszest/rig_masks_corr_weightedzest/rig_masks_corr_weighted_rgbzbt o (x y) c -> (bt o) c x yzest/rig_masks_rgbdepth_inpviszest/depth_inpres_depthviszest/res_depthdepthvisz	est/depth	depth_rawTzest/depth_rawzest/depth_raw_inv)r/   r(   zest/corr_weightscorr_weights_staticzest/corr_weights_staticbwd_flow   zref/flow_gt_bwdrig_flow_maskszb t o x y -> (b t o) 1 x yzref/rig_flow_masksflow_from_posegg?zest/flow_est_posefmapzbt c x y -> 1 c (bt x) y)dimkeepdim   z1 c (bt x y) -> (bt) c x yyr   zest/fmap_emb0to3zest/fmap_emb3to6affinity_embr   _unnormzest/affinity_emb0to3zest/affinity_emb3to6pred_tracksi   @   *   )axisc                 "    t        | d      S )Nb t (x y s) c -> (b t s) c x yrC   r   )r   sls    r   r   zwandb_summary.<locals>.<lambda>   s    i*JRRTU r   pred_visibilityzref/track_flow_gtaff_sim_grid   zest/aff_gridpoint_track_reprojrL   zmetrics/track_err_xzmetrics/track_err_yzest/track_flow_estrig_pertrack)N.Nzest/rig_sampsaff_simaff_emb_pertrackzb (x y s) c -> s c (b x y)z1 c (b x y) -> b c x y)r/   zest/affinity_emb_track	poses_allgh㈵>)epszb (x y s) t c -> (b s t) c x y zest/poses_lie_rot_allzest/poses_lie_trans_allrgb_pertrackzb (x y s) c -> b s c x y zb (x y s) t c -> b s t c x y zest/rgb_camimgsc2w_alignedpose_clustersposeszATE :zmetrics/ATE)stepo   3d)
projectionredzEstimated Trajectory)clabelblackzGT Trajectory)dtype)r   zest/pose_estzlogging imageszdone logging images)Yitemslenshapelistflattenpermuter   typecliptorch
from_numpyr   get_cmapr"   itemcpunumpysqueezer   detachkorniageometryconversionsquaternion_to_axis_anglech_fstr   Finterpolate	unsqueeze	unflattenflow_vis_torchflow_to_colorisnananyviewmeanbmm	transposesvdcat
zeros_likeexpandr   npmgridastypefloatflipcopylongtensorcudaabsrandpermrotation_matrix_to_quaternionstackrangeprintnumpy_procrustesinverser
   
procrustessquaresqrtwandblogpltfigureadd_subplotplotunbindmaxset_xlimset_ylimset_zlimlegendtight_layoutcanvasdraw
frombuffertostring_rgbuint8reshapeget_width_heightappendcloseimsavezzr	   )9lossmodel_outputmodel_inputground_truth
resolutionprefixsuffixr]   
losses_aggr   kvr(   imslinv	wandb_outrot_vis	trans_vislow_resr?   featuresBCHWfeatures_mean
covarianceUSVnum_componentstransformed_featuressuffaff_embnrow_uv	track_unppoint_track_errerr_imgaff_grid	poses_lie
rgbcamimgs	pose_imgsr[   ijour_posgt_pospos_gt_pos_est_atefigaxmin_max_image_from_plotrM   s9                                                           @r   wandb_summaryr   !   s   p|  ~I  JV  pW  -X  -Xjk[\[b[b[d-eTWTUVWa3qww<3BQ.N-e  -X)L\k%(0015==a!AFLLMJu%**1-l5.A.F.Fq.IIJqM	U		 	 	#D	U		!	!"#	&D
C L&&() _!7D=S\QVVV-=1a<!ah/a<ENEDTDTUiUWU`U`ahUijkjojojqjvjvjxyzy~y~  zA  zG  zG  zI  kI  VJ  EK  ES  ES  TV  EW  X[  \^  ]^  \^  X^  E_L5,A	_ I '{5'9'='='?'G'G!'L'S'S'UVX'XY['[aefIl(f)=)A)A)C)K)KAa)P)W)W)YZ\)\]_)_efgIn%)T`aoTpTtTtTvT~T~  @A  BC  UD  UK  UK  UM  NP  UP  QS  US  YZ  K[y1G'H|#++DD\R^E_`cdfefdf`fEghppqrstu}}~  AB  CD  EF  G  HJ  J  KM  M .s23w7??!DLLQqQRSTUVWWXZZ	/89It^c/d	,-1:9;K;K;MSWbg1h	./[ #,VK
4KDQRG-T-\-\]^_`-agj#k	.!L #,VL4KDQRG-T-\-\]^_`-agj#k	.!l"$-i[8QRru|}~u  /A  /H  /H  /J  P\  ]h  Pi  Pn  Pn  op  Pq  %r	/"2;Q]]<XfKghiKjkr=s=}=}~  >A  BH  IU  Va  Ib  cd  ef  eg  cg  Ih  ip  qr  is  Bt  >tFMgaPQlSYSYS[amnyazaa  AB  bC3D	/06?{[`OabcdedfbfOghoApAzAz{|A}  E  FR  S^  F_  `a  bc  bd  `d  Fe  fm  no  fp  q  BqDEMMR^_mRnopRqryDz  EE  EE  FG  EHBHFMgaPQlSYSYS[amnyazaa  AB  bC7D	34 )2)L<U<]<]^_`a<bdedqdqr}  D  sE  sM  sM  NO  PQ  sR  SZ  e[  ec  ec  df  gi  ej  er  er  st  uv  wx  ey  eC  eC  DE  eF  GI  eI  JL  eL  =M0NQXYZQ[3]]c]c]ekw  yD  lE  lJ  lJ  KL  lM)N	%&%)LYgLhLlLlLnLvLvwxyzL{  MD  MD  EF  GH  IJ  MK  MU  MU  VX  Y]  M^  Me  Me  Mg  mq  Cry'A%)LYgLhLlLlLnLvLvwxyzL{  MD  MD  EF  GH  IJ  MK  MU  MU  VX  Y]  M^  Me  Me  Mg  mq  Cry'A\!)LQ[D\D`D`DbDjDjklmnDoDwDwxyz{|}D~  EI  EI  JL  MQ  ER  EY  EY  E[  ae  ;f9[#9l"#,\+-F-J-J-L-T-TUVWX-Y-a-abcdefg-h-r-rsuvz-{  .C  .C  .E  KO  Z^  $_	/"'0\+5N1O1S1S1U1]1]^_`a1b1j1jklmnop1q1{1{|~  @D  2E  2L  2L  2N  TX  cg  (h	%&%yQ]^lQmQuQuvwxyQzQ~Q~  RA  RH  RH  RJ  UY  _c  Hdy1C'D,U^_k  mB  `C  `K  `K  LM  NO  `P  `T  `T  `V  `]  `]  `_  jn  tx  Vyi8Q.R[ @\@\]fgrs}g~  hG  hG  HI  JK  hL  RV  WX  RX  ^Y  AZ  [^  A^),=">;&)29[IY=Z[w3x  J  K[  \  a  a  bc  d  *e	&'
 <'LIY<Z0[0_0_0a)7)E)EiP\]mPnPsPstwxzP{  QD  QD  EF  GH  QI  QQ  QQ  RS  TU  VW  QX  Qb  Qb  ce  fj  Qk  qu  vw  qw  Gx  *y  z}  *}	%&F#4<<!,-GH^^
1a==Ar* !T:m+YYx););Aq)ABa!eaiP
))J'1a1Qx$yy1a.+@)A)K)KAq)QS[\'0;OPQRTSTRTPT;U;\;\;^_{  C  H  H  IK  L  OS  OX  OX  Y[  O\  2]  dp  qw  dx  d}  d}  ~  d@  KO  (P	$%Q3yCWXYZ[Z\X\C]CdCdCf  hD  GK  GP  GP  QS  GT  W[  W`  W`  ac  Wd  :e  lx  y  l@  lE  lE  FG  lH  SW  0X	,- %	N2A& 	sD">$#67G||Aq EIIwu?O?OPWXYZ[]^\_X_P`?a?h?hiklnopqxq}q}~  rA  pA  BD  EG  @H  7I  JK  -L'wq34NOH!JAq!Q}}Q2.H$MMaM>M-/H8X-?-?1-EF!a%RS)TJii
+GAq!q8N#(99Qq!_n_/D-E-O-OPQST-UW_#` 4=iH\]^_a`a_a]aHbHiHiHk  mI  LS  LX  LX  Y[  L\  _f  _k  _k  ln  _o  ?p  wC  DR  wS  wX  wX  YZ  w[  fj  5kI,T12sIiPdefghgieiPjPqPqPs  uQ  T[  T`  T`  ac  Td  gn  gs  gs  tv  gw  Gx  K  LZ  [  `  `  ab  c  nr  =sI4T9:#	s. # ]+004U:A=22+m,11"5r1u<=BXXa'!*na'!*n45<<UCMMaQRTUVbggbr2779:??A%,,
1gajl;<<U	)7)E)EiQZ[fgt[u  wB  CT  wU  w_  w_  `b  wc  \c  RdLNJJWXYZ[\L]^bLcLhLhLjRklu  wB  CT  wU  w_  w_  `b  wc  mdQd joGp *q ru*u	%& %!$-l>.J.R.RSTUV.WXYZ^X^._efqu$v	.!|+)*>?+mB\\`kl}`~  aI  aI  JL  aM  M  S  S  UIo6VY[^`aglmnoo+2A3<+>+>qB+G	'(+2A3<+>+>qB+G	'(*8*F*FyR[\hi}\~  @K  L]  @^  @h  @h  ik  @l  ]l  SmLNJJWXYZ[\L]^bLcLhLhLjSklu  wB  CT  wU  w_  w_  `b  wc  mdRd joHp +q ru+u	&'\)	R[\hiw\x  zG  ]H  SI  OT  IU9_+E $ - .@!AB^acfhikljmnH$MMaM>M-/H8X-?-?1-EF(--XZJ[^_J_`Jii
+GAq!qq!12N#(99Qq!_n_/D-E-O-OPQST-UW_#` 1:9EYE`E`Ebc{  A  DF  <G  SW  2XI./,&		6??#>#>#\#\]iju]vwz{}|}{}  A  @A  A  xA  ^B  GK#\  $L  MY  Ze  Mf  gj  km  lm  km  np  gp  Mq  #r  su  vIOO//HHSVWYXYWYSYIZ[\^^_aaG!#bc'*1,R/I099W^^EUVwz|  @B  <C  JS  JX  JX  YZ  J[  fk  1lI-.2;YyGWGWGYZ{  A  DF  >G  NW  N\  N\  ]^  N_  jo  3pI/0,~6==?@[^`cefghijgjklnnoqq'..*+JRRTUVWXYZ\V\])**,-LrTVWXYZ[\^X^_& 	 WQq\	 
 +4ZaRW*XI'( 	l*q"7+55a8e
s58}% 	FA!H!Aa2+&--/335G,,Wk%6HA36O6W6W6Y\ghm\n6npqrstvuvtvwypy5z5~5~  6A  BC  6D  E  FG  HF >E=O=OP[\aPbcdefgihigijlclPmPqPqPsPyPyP{  }D  }J  }J  }L  >M  NP  OP  >Q   R 0 0 3   RGH!))+002779CD1+s#II}c*5

C#//#$/"GRBGGW[[]))!,u;QRBGGVZZ\((+gOL'++-

5c'++-

6UDKKd#BKKd$;BKKd<SJJL))+CJJOO,= mmCJJ,C,C,ERXXVO-55cjj6Q6Q6STXVXTX6Y\`6`aOU--o>FFq1McQRIIK/4yy1/EInV+,-	F. 	 

5Y01 1!q!11	IIlul{l{l}~ehefghvaxAIIaA$6$<$<$>$E$E$G$L$LQq$Q$U$U$W$]$]$_``~	
 c .f  -Xj  R4 s,   A`'A` 
A`QA`]/A<A`
` A`)N)0osioshutilrw   r   
matplotlibr   cv2r   torchvisiontimetorchvision.utilsr   r   torch.nn.functionalnn
functionalr{   rv   rs   r   rn   flow_visr   matplotlib.pyplotpyplotr   einopsr   r   modelspiqaimageioPILr	   scipyr
   plotly.expressexpresspxplotly.graph_objectsgraph_objectsgocollectionsr   rz   ch_secr   r!   r   r   <module>r     s}         
    6         $       ! # 
s	:SU]_efrt Rh/r   