
    g*x                     4   d dl Z d dlZd dlZd dlmZ d dlZd dlZd dlmZm	Z	 d dl
m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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l+m,Z, dd	Z-d
 Z.ddZ/ddZ0ddZ1ddZ2y)    N)cm)	make_griddraw_keypointsc                 l    t        j                  d|z  | j                         j                               S )Nz&/nobackup/users/camsmith/img/tmp%s.png)pltimsavecpunumpyxys     1/home/cameronsmith/repos/flowmap++/vis_scripts.py<lambda>r      s+    

CklmCmnonsnsnun{n{n}8~     )	rearrangerepeat)spatial)defaultdict)IsolationForestc           	      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    9S)Cabajc#((SU,Y[J\F]r pqr r   c                     t        | d      S )Nz... c x y -> ... (x y) c)r   r   s    r   r   r      s    9Q9: r   c           	          t        t        dt        |d                  j                  |       }|j	                  |       }|j                  |       dk7  S )N{Gz?g\(\?)contamination)	r   maxminfitdecision_functionpredicttorchquantileall)
points
iqr_factor
iso_forestoutlier_scoresq1q3iqrlower_boundupper_boundmasks
             r   filter_pointsr4   !   sL     s3s:c7J/KLPPQWXJ11&9Nf%++r   c
                 R   %&'( t        d       t        t              }
|D ].  }|j                         D ]  \  }}|
|   j	                  |        0 d }|dz  dk(  r*|
j                         D ci c]  \  }}| |||       }}}(d } |d      (d<   |d   j                  dd      j                  d	d
      j                  ddd      d    (d<   |d   (d<   |d   (d<   (j                         D ci c]]  \  }}t        |      t        j                  k(  s#t        |j                        dkD  s<||d   j                         j                         _ c}}(d   j                  d      }t        j                   d|      }t#        |      D cg c]
  } ||       c}&t        j$                  t'        j(                  &      j+                  d
d            dd df   &t-        t/        j0                  t-        d   dd      |d   j                  d	d        d|d   j                  d      |d   j                  d            }&d d d d d f   j3                         |z  j5                  d      j                  dd      }t6        j8                  j:                  j=                  d   dd df         j                  dd      j                  dd      dz  dz   }d   ddd f   j                  dd      j                  dd      dz  dz   }(d   j                  dd      dz  dz   |j                  dd      |j                  dd      |j                  dd      d%%j                         D ci c]C  \  }}||dz  j                         j                         j?                         jA                         E c}}%(d   j                  dd      jA                         x}}%d   }(d   jC                  d(d   j                  d	d        dz  dz   dz  j?                         jA                         'd9&'( fd}|dk(  rg  _"        d _#        |dz  dk(  rMj                         D ]:  \  }} jD                  j	                   jH                  jK                  |              <  jH                  jM                  d!dt        '      dz
  dd"       _'         jH                  jQ                  d#d%jS                         $       _*         jH                  jW                  d%d&      x _,        } jH                  jM                  d'd(dd)d"      x _-        } jH                  jM                  d*dt        |      dt        |      "      x _.        } jH                  jM                  d+dt        '      dt_        t        '      d,      "      x _0        } jH                  jM                  d-d.d/d.d)"      x _1        } t&        jd                  jg                  |j                  d   |jh                  d01      x _5        }!| _6        | _7         jp                  js                  d2||!   ||!   | jh                  3      x _:        }"g  _;         jT                  jx                  d9% fd4       }# jb                  jx                  d9 fd5       }#n5|dz  dk(  r8t{        j                               D ]  \  }\  }}| jD                  |   _>          jX                  jh                  rgt        d6       t'        j~                  |j                  d         t        t        j$                  |       jZ                  jh                            _A        n't'        j~                  |j                  d          _A        t&        jd                  jg                   j                  t_        t         j                        dz
   j\                  jh                        d1       _5        t        d7t         jj                                | jN                  jh                        j                  dd      j                         j                         jA                          jj                      jt                  _6        % jT                  jh                      jj                      jt                  _7         jb                  jh                   jt                  _B        |		 |dk(  ri  _C        |	j                         D ci c]c  \  }}||j                         j                         dz  j                  ddd      j                  t        j                        jA                         e c}}j                         D ]f  \  }}t        j                  |      }$| j                  vr* jH                  jK                  |$        j                  |<   S|$ j                  |   _>        h t        d8       y c c}}w c c}}w c c}w c c}}w c c}}w ):Nzdoing viserc           
          t        j                  |t        t        t	        |                  d| d|       }d|j
                  j                  _        |j                  t        dddd             |S )Nr   r   )r   r   labelstitleT   )lrtb)margin)
pxlinelistrangelenlayoutr8   
automarginupdate_layoutdict)klossfigs      r   make_loss_figz#viser_update.<locals>.make_loss_fig<   s^    ggeCI&6!7cPQ@RZ[\&*

#$rR2">@
r   
   r   c                     t        j                  d|d   d d | gf   j                         |d   z  j                  dd      t	        j
                  |d               dd df   S )Nzbtpij,btpj->btpipose_perpix      eye_surf.)r'   einsuminverseflattenmodelshom)r<   ss     r   r   zviser_update.<locals>.<lambda>L   s    -?=AQRSUVTWRWAXA`A`AbcdercsAs@|@|}~  @A  AB  CI  CM  CM  NO  PZ  N[  C\  !]  ^a  bd  cd  bd  ^d  !e r   
world_crdsrgb   r   r!   rO   
intrinsics	rig_masksviridis   .rP   b t o (x y) 1 -> (b t o) 1 x y@   r   z(b t o) 1 x y -> b t o (x y) 1)r=   r<   
lie_perpixr      )rY   	rig_grouprot_lietrans   	flow_inp_c                     j                   D ]  } | j                           j                   j                          dfd}t        t	        j                         j                  d      d   j                         j                         j                  d                  D ]K  \  }\  }}t        |      D ]C  \  }}t        j                  j                  |j                         j                               }j                  j                  d| d| |j                         j                   |j#                         dd|dk(        } j                   j%                  |        j&                  d	   j&                  d
   }	}d   j                         d   |z  }
d	}j                  j)                  d| d| dd
t+        j,                  |d
z  |
      z  |	|z  d|dz  j/                               } |||        F |dk(  scj0                  rqt3        j5                               sj5                         d   }t        j                  j7                  t        j8                  |j:                  j                         |j:                  j<                        }t        j                  j7                  t        j8                   j                         | j<                        t        j                  j?                  t+        j@                  g d            z  }|j                         |z  }|t        j                  jC                  |jE                               z  }|jG                         5  |j                         j                   |j:                  _        |j#                         |j:                  _        d d d        |jI                          | j<                  |j:                  _%        d_        N y # 1 sw Y   ?xY w)Nc                 4    | j                   dfd       }y )Nc                     j                         j                         D ]8  }j                  |j                  _        j                  |j                  _        : y N)get_clientsvalueswxyzcameraposition)_clientframeservers     r   rr   zJviser_update.<locals>.visualize_frames.<locals>.attach_callback.<locals>._o   sD    $00299; <F).FMM&-2^^FMM*<r   returnN)on_click)frustumrt   rr   ru   s    ` r   attach_callbackz?viser_update.<locals>.visualize_frames.<locals>.attach_callbackn   s    < <r   r   	poses_allz	/flowmap/_frame_皙?g{Gzt?)ro   rq   axes_lengthaxes_radius	show_axesrZ   rO   r[   )r   rZ   rZ   z/frustum皙?rg   )fovaspectscalecolor)        r   g      Trv   )&framesremoveclear	enumeratezipr	   unbinddetachtfSE3from_matrixrS   r
   scene	add_framerotationro   translationappendshapeadd_camera_frustumnparctan2r   cam_setrC   rm   from_rotation_and_translationSO3rp   rq   from_translationarrayexplogatomicflushlook_at)rt   rz   src_i
pose_colorposesframe_iposeT_world_cameraHWfydownsample_factorry   rs   T_world_currentT_world_targetT_current_targetT_world_setcolors_per_rigimagesmodel_outputr   ru   s                     r   visualize_framesz&viser_update.<locals>.visualize_framesi   s   ]]2EELLN2	< )2#n6H6H6J6Q6QRS6TVbcnVoVvVvVxV|V|V~  WF  WF  GH  WI  3J  )K  	$$E$:e )% 0 0!#!3!3DLLN4H4H4J!K..)E7''0SZhZqZqZsZxZx  DR  D^  D^  D` #%( / M$$U+ ||Av||A1<(..079"#! ,,99wggYh?QTUXYTY[]I^E^ghklgltx &c>..0	 :   /%0( ax3v7I7I7K3L))+A."$&&"F"Fv}}OaOaHbdjdqdqdzdz"{!#!E!ErvvejjGY[`[i[i!jmomsms  nE  nE  FH  FN  FN  O_  F`  na  "a#2#:#:#<~#M -

<L<P<P<R0SS]]_ G)4)=)=)?)D)DFMM&-8-D-D-FFMM*G (-%#A 	$6G Gs   ,A	O33O<	F)figureTimestep)r#   r"   stepinitial_valuez	Color Vis)r   optionszOutlier Filtering)r   zOutlier filter thresholdgh㈵>r   z
Max pointsz
Max framesd   z
Point sizegMbP?r   T)replacez/flowmap/pcd)namer*   colors
point_sizec                 V    j                   j                     j                  _        y rl   )gui_color_choicevaluepoint_cloudr   )rr   color_choicesru   s    r   rr   zviser_update.<locals>._   s"    (5f6M6M6S6S(TF%r   c                 P    j                   j                  j                  _        y rl   )gui_point_sizer   r   r   )rr   ru   s    r   rr   zviser_update.<locals>._   s    ,2,A,A,G,GF)r   zfiltering points for viszlen pointmaskz
done viserrv   )Hprintr   rA   itemsr   rT   permutetyper'   TensorrC   r   r   r	   r   r   get_cmaprB   
from_numpyr   concatenatereshaper   Finterpolatecudasumkorniageometryconversionsquaternion_to_axis_angler   r
   	unflatten
loss_plotsr   gui
add_plotly
add_slidergui_timestep_slideradd_dropdownkeysr   add_checkboxgui_outlier_filtergui_outlier_slider
gui_pointsr#   
gui_framesr   randomchoicer   
point_maskr*   r   r   add_point_cloudr   r   	on_updater   r   aranger4   valid_pointsr   wandb_plotstouint8r?   imshow))ru   loss_r   model_inputground_truth
resolutionprefixsuffixr   
wandb_imgsrI   r   rH   vrK   	loss_figsget_world_pixn_rigcmapir\   colors_per_rig_perpixrot_vis	trans_visr*   points3dr   r   r   r   r   r   r   r   r   rr   rJ   r   r   r   r   s)   ` `                                  @@@@r   viser_updater  5   s
   	-	T	D 779 	CAaGNN1	 Bwz59ZZ\BcaQ}Qq))B	B E eM%a5E,u%--a2::2bAII!AaPQUVE%L%l3E,%l3E,,1KKM
fSQqT!Well=RWZ[\[b[bWcdeWe1QqT[[]  
fE%**1-E<<	5)D',U|4!d1g4N%%bnn^&D&L&LRPQ&RSTWXZYZXZTZ[N!--	,{2KLloq(rs~  @E  tF  tL  tL  MO  MP  tQ  R  Ss  vA  BG  vH  vM  vM  NO  vP  S^  _d  Se  Sj  Sj  kl  Sm  nI+Da,<=BBDYNSSTUV^^_`abcOO''@@lA[\_`bab`b\bAcdllmnopqyyz{|}~  @B  B  CE  EG\*3rs73;;Aa@HH1MaOPRRI <//!4R7:G\GdGdefghGit{  uD  uD  EF  GH  uI  R[  Rc  Rc  de  fg  Rh  iMGTGZGZG\]!Q#~~'++-11399;;]Ml+33Aa8>>@@FX5!FU|%%ak(:(@(@(EFrI"LcQVVX^^`F-$ -$^ Qw7A: ( J!!!(()>)>a)>)HIJ9?9N9NPZ`agjkqgrstgt{|  MN9N  :O"9?9P9PR]mr  |I  |N  |N  |P9P  :Q9?9P9PReuz9P9||!$69?9N9NPjpv|}  EH  XY9N  :[  	[!$69?9N9NP\bcilmuiv}~  OR  S[  O\9N  :^  	^J9?9N9NP\bcilmsit{|  MP  QT  U[  Q\  ^a  Mb9N  :d  	dJ9?9N9NP\bgmqx}  NR9N  :S  	SN131A1A&,,q/S]ScScmq1A1rrJ28,,2N2NUclrs}l~  HN  OY  HZ  gu  g{  g{2N  3}  	}K		 	 	*	*	U 
+	U 
			(	(	H 
)	H< 7A:$Y__%67 .%1Q,-!!!$+.
 $$**,-"$))FLLO"<]5K[K[\bKcdjd}d}  eD  eD  >E  #FF$&IIfll1o$>f!II,,V-@-@#c&J]J]F^_`F`agararaxaxBy  DI,  Joc&"3"345$1&2L2L2R2RS_$`$h$hijkl$m$t$t$v$z$z$|  %C  %C  %E  FL  FW  FW  %X!$1&2I2I2O2O$PQWQbQb$c!(.(=(=(C(C% !7b6%_i_o_o_qrX[XYZ[Aqxxz~~'+44Qq;>>u{{KQQSSrxxz 	2CAa))A,C***&**BWBW_bBWBcF,>,>q,A.1##A&-	2
 
,U C  g 5 ^R ss,   2h9$hh7$hh6AhA(h#c                 d+   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dd      j'                  d|	      j)                         |      |d<   d|v rt*        j,                  j.                  j1                  |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 rt*        j,                  j.                  j1                  |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 rt%        t3        |d   d d!"      j)                         |d   j	                  d            |d#<   t%        t3        |d   j                  dd      t5        j6                  |d   j                  d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%        t3        |d'   d d!"      j)                         |d   j	                  d            |d(<   d)|v r+t%        t3        |d)   d*      j)                         d      |d+<   d,|v rdt;        |d,         D ]S  \  }}t%        t3        |d-|d   j	                  d      "      j)                         |j	                  d            |d.|z  <   U d/|v rPt;        |d/         D ]?  \  }}t%        t3        |d0      j)                         |j	                  d            |d1|z  <   A d2|v r=t%        t3        |d2   d3      j)                         |d2   j	                  d            |d4<   d5|v rpt%        |d5   d d d df   j=                         j)                         d6      |d7<   t%        |d5   d d dd f   j=                         j)                         d6      |d8<   d9|v r@t%        t3        |d9   d:   d;      j)                         |d9   j	                  d            |d<<   d=|v rt3        |d=   j                  dd      d>      }|j                  \  }}}}|j?                  ||d      }|jA                  dd?@      }||z
  }t        jB                  ||jE                  dd            ||z  dz
  z  }t        jF                  |      \  }}}d}t        jB                  |d d d d d |f   jE                  dd      |      }|j?                  ||||      }t%        |j)                         |d=   j	                  d            |dA<   |j                         D ]D  \  }}dB|vrt%        t3        |dC      j)                         |j	                  d            |dD|z  <   F |j                         D ]G  \  }}dE|vrt%        t3        |dFz  dG      j)                         |j	                  d            |dD|z  <   I dH|v r`t%        |dH   j                         j                  dd      j                  ddd      j'                  d|	      j)                         |      |dI<   dJ|v r`t%        |dJ   j                         j                  dd      j                  ddd      j'                  d|	      j)                         |      |dK<   dL|v r`t%        |dL   j                         j                  dd      j                  ddd      j'                  d|	      j)                         |      |dM<   dN|v rNt%        |dN   j                         j                  dd      j)                         jI                         |d      |dO<   dP|v r@t%        |dP   j                  dd      j                         j)                         d?|Q      |dR<   dS|v r9tK        jL                  t%        |dS   j                  dd      |            dFz  |dT<   dU|v rDt%        |dU   j                  dd      j                  ddd      j'                  d|	      |      |dV<   dW|v rt        jN                  |dW         jQ                         sjtK        jL                  t%        |dW   j                  dXdY      j                  dd      j                  ddd      j'                  d|	      |            dFz  |dZ<   d[|v rtK        jL                  t%        t3        |d[   d\      j)                         t        |d[                     dFz  |d]<   t%        t3        |dS   |d[   z
  j=                         jS                  d^      d_      j)                         t        |d[               |d`<   da|v rBt        jT                  t        |da   j                         d         d      }tW        jX                  dbc      }|j[                  dddef      } t]        ||da   dd d d d d ddf   j)                         j                               D ]K  \  }!}" | j^                  |"j                         j`                  j!                         dg|!j!                         i M | jb                  je                  g        | jf                  je                  g        | jh                  je                  g        | jk                  dhdij       |jm                          |jn                  jq                          ts        jt                  |jn                  jw                         tr        jx                  k      }#|#j{                  |jn                  j}                         d d d   dlz         }#t        j                  |#      j                  ddd      dFz  }$tW        j~                          |$|dm<   t        d      D ]  }%t        d      D ]  }&  dn|v r	 doD ]  }g }?dp|vr|dq   j9                  d      n|dp   }@t        t        |@            D ]  }t        t        @d               D ]  }A@||Ad d d ddf   j)                         j                         }"t        |      rWt-        j                  |"|dn   d d dgf   j                         |dn   z  d d d d d ddf   j                         |         d   }Bn:|dn   d d dgf   j                         |dn   z  |d d d ddf   j                         }Bt        j                  |dn   |d d d ddf   j                         j!                         |"j!                               d d D cg c]  }t        j                  |       c}\  }C}D|C|Dz
  j                         jA                         j                         }Et        dr|E       t        j                  ds|Ei|t       tW        jX                         }|j[                  dddef      }  | j^                  |"j                         j                  d      dudvdw  | j^                  Bj                         j                  d      dxdydw t        |"j                         |Bj                               t        |"j                         |Bj                               }G}F| j                  |F|G       | j                  |F|G       | j                  |F|G       tW        j                          tW        jl                          |jn                  jq                          ts        jt                  |jn                  jw                         tr        jx                  k      }#|#j{                  |jn                  j}                         d d d   dlz         }#|?j                  t        j                  |#      j                  ddd      dFz         tW        j~                          t        j                  |?d      |dz|z   <      |dq   dd d d ddf   j)                         j                         }"|dn   dd d d ddf   j)                         j                         }Bt        j                  |Bj                         j!                         |"j!                               d d D cg c]  }t        j                  |       c}\  }B}"tW        jX                  dbc      }|j[                  dddef      }  | j^                  |"j                         j`                  j!                         dudvdw  | j^                  |Bj                         j`                  j!                         dxdydw | jb                  je                  g        | jf                  je                  g        | jh                  je                  g        | jk                  dhdij       |jm                          |jn                  jq                          ts        jt                  |jn                  jw                         tr        jx                  k      }#|#j{                  |jn                  j}                         d d d   dlz         }#t        j                  |#      j                  ddd      dFz  }$tW        j~                          |$|d{<   	 t        j                  |j                         D ci c]z  \  }}||z   |z   t        j                  |j                  ddd      jI                         j)                         j                  dd      j                         j!                               | c}}       |S c c}w c c}w c c}}w )|NrY   r   rZ   rO   rP   r   c                     d| dz   z  S )NrZ   g:0yE> r   s    r   r   zwandb_summary.<locals>.<lambda>   s    Q$Z r   333333?)r#   depth_rawrawmagma.visr!   )nrowz
ref/rgb_gt	poses_lier^   r   rb   rc   F)r  	normalizezest/poses_lie_rotzest/poses_lie_transra   zest/poses_lie_rot_perpixzest/poses_lie_trans_perpixr\   r_   r`   r   zest/rig_masks)r`   r`   zbt o (x y) c -> (bt o) c x yzest/rig_masks_rgbrig_masks_unnormzest/rig_masks_unnormlevel_scoreszb t l x y -> (b t l) 1 x yzest/level_scorescomposite_weightszb t o (x y) -> (b t o) 1 x yzest/composite_weights_%imask_spatial_scoreszb t o x y -> (b t o) 1 x yzest/mask_spatial_scores_%i	is_staticzb t 1 x y -> (b t) 1 x yzest/is_statictraj_top_comps)r  zest/top_comps1zest/top_comps2affinity_sim)r   r   zx1 y1 x y -> (x1 y1) 1 x yzest/affinity_simaffinity_embzbt c x y -> 1 c (bt x) yT)dimkeepdimzest/affinity_emball_maskzt m x y -> (t m) 1 x yzref/%s	masks_visrg   zt m 1 x y c -> (t m) c x yzoe_depthviszest/zoe_depthres_depthviszest/res_depthdepthvisz	est/depthnot_skyzref/not_skycorr_weights)r  r  zest/corr_weightsbwd_flowzref/flow_gt_bwd
warp_rgbs_zest/warp_rgbflow_from_posegr}   zest/flow_est_poseflow_from_pose_uncompzm bt c x y -> (bt m) c x yzest/flow_est_pose_uncomp)r  zm bt x y -> (bt m) 1 x yzest/flow_est_pose_uncomp_err	rig_poses)      )figsizeo   3d)
projectioncg     K@i)elevazim)dtype)rP   zest/rig_posesc2w)_aligned intermed_poses_r   zATE:zmetrics/ATE)r   redzEstimated Trajectory)r*  labelblackzGT Trajectoryzest/pose_estzest/pose_est_pretty)[rA   rT   r   r   r   r   r   rC   clipr'   r   r   r   r#   itemr	   r
   squeezer   r   r   r   r   r   r   r   r   r   	unsqueezer   absviewmeanbmm	transposesvdfloatflow_vis_torchflow_to_colorisnananyr   randr   r   add_subplotr   plotTxaxisset_ticklabelsyaxiszaxis	view_inittight_layoutcanvasdrawr   
frombuffertostring_rgbr   r   get_width_heightcloserB   r"   gatherexpandr4   tensormmr   scatternumpy_procrustesrS   r   
procrustessquaresqrtr   wandbr   r   set_xlimset_ylimset_zlimlegendr   catr   Image)HrI   r   r   r   r   r   r   r   r  imslinvrH   r   	wandb_outr   r   r   r   featuresBCr   r   features_mean
covarianceUSVnum_componentstransformed_features
rig_colorsrJ   axr   our_posimage_from_plotpose_imgcolor_iview_ipts_allrig_selectionptsr3   r   rig_ir#  pose_ir   frustum_pointsfrustum_worldcamera_centerbottom_leftbottom_right	top_righttop_leftfrustum_linesr@   rY   stride	pt_colorssc	view_name
color_name	pose_imgsr   jgt_pospos_gt_pos_est_atemin_max_sH                                                                           r   wandb_summaryr     s   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&|E':'>'>'@'H'H1'M'U'UVWXYZ['\'f'fgijn'o'v'v'x  C  DIll"++DD\R]E^_bcedece_eEfgoopqrst||}~  @A  BC  DE  F  GI  I  JL  L -c"#g6>>qCKKAaPQRSTUVVWYY	(1'..2BW\(]	%&*3I4D4D4FD[`*a	'(|#++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	./ l"$-i[8QRruw.x..  /B  HT  U`  Ha  Hf  Hf  gh  Hi  %j	/"(1)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0NQS3UU[U[U]cop{c|  dB  dB  CD  dE)F	%&\)+4Y|L^?_  aA  DF  6G  6N  6N  6P  Vb  cn  Vo  Vt  Vt  uv  Vw  ,x	()%'0<;WXt1v1}1}1  GH  (I	$%l*\*=>? 	cCAa5>yKilwx}l~  mD  mD  EG  mH  @I  @P  @P  @R  YZ  Y_  Y_  `a  Yb  6cI023	c,\*?@A 	ECAa7@1MiAjAqAqAsz{  {A  {A  BC  {D  8EI2145	El"$-i[8QRl.m.t.t.v  ~J  KV  ~W  ~\  ~\  ]^  ~_  %`	/"<'%.|<L/MaPRQRPRd/S/W/W/Y/`/`/bmr%s	"#%.|<L/MaPQPRd/S/W/W/Y/`/`/bmr%s	"#%(1)L<XY\<]^z2{  3C  3C  3E  LX  Yg  Lh  Lm  Lm  no  Lp  )q	$%%<7??!DE_`^^
1a==Ar* !T:m+YYx););Aq)ABa!eaiP
))J'1a$yy1a.+@)A)K)KAq)QS[\388NAqQ'01E1L1L1NUabpUqUvUvwxUy'z	$%  " h!Qx(15M)N)U)U)W]^]c]cde]fg	(1*h   " p!a(1S59U)V)])])_efekeklmeno	(1*p %)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 K)KPYDZD^D^D`DhDhijklDmDtDtDvD|D|D~  EI  TY  ;Z=!9%yQ]^lQmQuQuvwxyQzQ~Q~  RA  RH  RH  RJ  UY  _c  Hdy1C'D[ @\@\]fgrs}g~  hG  hG  HI  JK  hL  RV  ^W  AX  Y\  A\),=">|#	,WcJdJlJlmnopJqJyJyz{|}~  KA  KK  KK  LN  OS  KT  Z^  A_Y~%><'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  Gv  *w  x{  *{	%&,.0>0L0LYW`am  oF  bG  Hd  Xe  Xl  Xl  XnUXYef}Y~UNA 1B CF1F	,-4=iU_I`am  oF  bG  JG  IL  IL  IN  IR  IR  WX  IR  IY  [u  ?v  ?}  ?}  ?&=>?5A	01 l"::c,{";"?"?"A!"DEaH
JJv&COOCDO,Qr L,Ea!BQBrk,R,Y,Y,[,_,_,ab 	>ME'BGGW[[]__**,=u{{}=	>
#
#
#
#C(

--

(?(?(AR)11#**2M2M2OPTRTPT2UX\2\]!!/2::1QqA#E		%-	/" Qx 7	hG( 6h6h7	hr % 	NFI:KS_:_L)33A6eq  sD  fEE3u:& Ns58}- NA!!Aa2+.557;;=G6{8+D+DWkZ_N`abdecfafNgNoNoNqt  AF  uG  OG  IJ  KL  MO  NO  MO  PR  IR  NS  NW  NW  NY  Z[  N\  ,]  ^_  ,`F"-e"4QsU";"C"C"ETYHZ"Z\]^_`bab`bce\e!f!j!j!l&ELEWEWXcdiXjklmnoqpqoqrtktXuXyXyX{  YB  YB  YD  EL  ER  ER  ET  FU  VX  WX  FY  (Z(8(8(;  (Z$GH )11388:??AC&#&II}c2=

C#//#$/*ORBGGW[[]11!4uCYZBGGVZZ\003gOT!'++-

=c'++-PVPZPZP\>]DKKd+BKKd,CBKKPTVZD[JJL!1!1!3CJJOO4E&(mmCJJ4K4K4MUWU]U]&^O&5&=&=cjj>Y>Y>[\`^`\`>adh>h&iO$$U%5%5o%F%N%NqQRST%UVY%YZIIK7<yy17MInV34-NN		N: W%a"1"Ri0779==?5!!Abqb),33599;7>7I7I&**,J\J\J^_f_l_l_n7oprqr7st!%**1-twJJv&COOCDO,Qr&&(57MN%%''H 	#
#
#
#C(

--

(?(?(AR)11#**2M2M2OPTRTPT2UX\2\]!!/2::1QqA#E		+3	'( 	 
IIs|  tC  tC  tE  Flolmnovaxu{{199Q1+=+C+C+E+L+L+N+S+STUVW+X+\+\+^+d+d+fgg  F  Gm (Z( uB Fs   8AV"
K:AV'TA?AV,
c                     t        j                  d|d   d d d ddf   j                         j                         d      }t        j                  d|i       y )Nz
lidar/betar   rP   r!   )r   r*   zcamera positions)r]  Object3Dr	   r
   r   )rI   r   r   r   r   r   point_scenes          r   pose_summaryr    sU    ..(2A2b1557==?" K 
II!;/0r   )r   rl   )r  )r0  r0  r   N)r0  r0  r   )r0  )3osr   r]  
matplotlibr   cv2torchvisiontorchvision.utilsr   r   torch.nn.functionalnn
functionalr   r   r
   r   r'   flow_visr@  matplotlib.pyplotpyplotr   r   einopsr   r   rU   piqaimageioscipyr   plotly.expressexpressr?   plotly.graph_objectsgraph_objectsgocollectionsr   viser.transforms
transformsr   sklearn.ensembler   ch_fstch_secr4   r  r  r  r  r   r   <module>r     sy    	    
  6        *~ $      ! #  ,	r	:(xt@D1r   