
    &:"h/i                        d Z ddlZddlmZmZ ddlmZ ddlZ	ddl
mZm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ZddlZd	 Zd
 Zd Zdej$                  dej$                  fdZdej$                  dej$                  fdZd ZddlmZ ddlmZ   ejB                  g dg dg dg      d   jE                         jG                         Z$d Z%d Z&d Z'd Z(d=dZ)d Z*d Z+d Z,d>dZ-d>d Z.d!ej$                  fd"Z/d#ej$                  fd$Z0d%ej$                  d&ej$                  d'ej$                  dej$                  fd(Z1d)ej$                  d*ej$                  dej$                  fd+Z2d,ej$                  d*ej$                  dej$                  fd-Z3d.ej$                  d/ej$                  dej$                  fd0Z4d%ej$                  d'ej$                  dej$                  fd1Z5d%ej$                  d'ej$                  d*ej$                  dej$                  fd2Z6d%ej$                  d'ej$                  d*ej$                  dej$                  fd3Z7d?d4Z8d5 Z9d6 Z:d7 Z;d@d8Z<ddlmZ  d9 Z=d: Z>d; Z?d< Z@y)Az<Multi-view geometry & proejction code.. (most from vincent)     N)	rearrangerepeat)
functional)ceillog2)einsum)Float)Tensorc                 d    t        j                  | t        j                  | ddgf         fd      S )N.r   )torchcat	ones_likexs    //home/cameronsmith/repos/controll3r/geometry.py<lambda>r      s)    eii5??1S!W:#> ?C     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                 B    t        t        j                  d||             S )Nzb...cij,b...ckj->b...cki)unhomr   r   )crdsKs     r   r   r      s    5.HD!QR r   d6returnc                    | dd df   | ddd f   }}t        j                  |d      }|||z  j                  dd      |z  z
  }t        j                  |d      }t        j                  ||d      }t        j
                  |||fd      S )N.   r   dimTkeepdim)F	normalizesumr   crossstack)r   a1a2b1b2b3s         r   rotation_6d_to_matrixr/      s    RaR["S!"W+B	
RR	 B	rBwmmBm-2	2B	
RR	 B	R	$B;;B|,,r   matrixc                     | j                         d d }| dd dd d f   j                         j                  |dz         S )Nr$   .   )   )sizeclonereshape)r0   	batch_dims     r   matrix_to_rotation_6dr8       sB    cr"I#rr1*##%--i$.>??r   c           
      ~   t        j                  d| d d |f   j                         t        |            dd df   }t        j                  d| d d t	        d|dz
        f   j                         t        |            dd df   }t        |d   |d dd f         d   }t        |d   |d dd f         d   }||z
  }|S )Nz
pij,pj->pi.r   r      NNr   r   )r   r   inversehommaxproject)	pose_perpixview_imeansr   pos_ipos_adjpos_i_2d
pos_adj_2dflow_2ds	            r   compute_flowrI   $   s    ll<AfH(=(E(E(GE
STWXZYZXZTZ[Ell<Ac!F1Ho4E(F(N(N(PQTUZQ[\]`acbcac]cdG+AbqbgJ7<H+AbqbgJ7<J!GNr   )KMeans)Rotation)r   r   r   r:   )r:   r   r   r:   )r   r:   r   r:   c                 b   t        t        dd df   t        dd df         d   }|dd ddf   }t        j                  t        j                  j                  j                  t
        j                  j                  j                  |d d d dd df         j                  d       d      }|||fS )N.r   r   )
procrustesunit_tri
transf_trir   r)   korniageometryconversionseuler_from_quaternionrotation_matrix_to_quaternionunbind)tristransf_recoveredtrans_recoveredrot_recovereds       r   tris_to_transfrZ   3   s   !(3rr6"2:c"1"f3EFrJ&s2A2by1OKK ; ; Q QSYSbSbSnSn  TM  TM  N^  _`  ac  bc  ac  df  ef  df  _f  Ng  Th  To  To  pr  Ts  !t  uw  xMOM99r   c                     t        j                  d| j                         t        j	                               dd df   }|| fS )Nbij,bkj->bki.r   )r   r   cudarN   float)transfrO   s     r   transf_to_trisr`   8   s<    nV[[]8>>;KLSQSRSQSVTJfr   c                    t        j                  d      d    j                  t        |       dd      j	                         }||d d d ddf<   | j                  d      dk(  rt        |       |d d d dd df<   n5t        j                  j                  j                  |       |d d d dd df<   t        j                  d|j	                         t        j                               dd df   }||fS )N   r   r   r3   r\   .)r   eyeexpandlenr]   r4   r/   rP   rQ   rR   quaternion_to_rotation_matrixr   rN   r^   )rottransr_   rO   s       r   rot_trans_to_trisri   ;   s    99Q<$$SXb499;FF1RaR7O
xx|Q(=c(Bq!BQBw //55SSTWX&2A2bqb/nV[[]8>>;KLSQSRSQSVTJfr   c                    t        j                  d      d    j                  t        |       dd      }||d d d ddf<   t        j
                  j                  j                  t        j                  t	        j
                  j                  j                  | j                  d       d            |d d d dd df<   |d d d ddf   }t        j                  t	        j
                  j                  j                  t        j
                  j                  j                  |d d d dd df         j                  d       d      }y )Nrb   r   r   r:   )r   rc   rd   re   rP   rQ   rR   rf   r)   quaternion_from_eulerrU   rS   rT   )rg   rh   r_   trans2rot2s        r   rot_trans_to_tris_rn   C   s   99Q<$$SXb4FF1RaR7OOO//MMekkZ`ZiZiZuZu  [L  [L  NQ  NX  NX  YZ  N[  [\  ]_  O`  aF1RaR7O!BQBr'?F	V__00FFHcHc  IB  IB  CI  JK  LN  MN  LN  OQ  PQ  OQ  JQ  CR  IS  IZ  IZ  []  I^  _  `b  
cDr   c                    | d d d d d ddf   j                  | j                  d      d      }| d d d d d dd df   j                  ddd      j                         j                         }t	        j
                  t        j                  |      j                         | j                        }|j                  | j                  d      d      }t	        j                  ||gd      j                         j                         }t        |d      j                  |      }|j                  }t	        j
                  |j                  | j                        }	g }
t        |      D ]  }t	        j                   t	        j
                  |      |k(        d   }|j#                         dk(  rE	 ||   }t	        j$                  t	        j
                  || j                        |	|   z
  d      }|
j'                  | ||j)                                       |st	        j*                  |
      S t	        j*                  |
      |fS #  Y xY w)	Nr   r   r   devicer:   r    *   )
n_clustersrandom_state)r6   r4   cpunumpyr   tensorRfrom_matrixas_quatrq   r   rJ   fitlabels_cluster_centers_rangewherenumelnormappendargminr)   )posesrs   return_labelstranslations	rotationsquaternionsfeatureskmeanslabelscentersrepresentativesicluster_indicescluster_features	distancess                  r   cluster_and_representr   L   s   Arr1%--ejjmR@LaBQBl#++B1599;AACI,,q}}Y7??A%,,WK%%ejjmR8Kyy,4!<@@BHHJH z;??IF^^Fll6225<<HG O: ++ell6&:a&?@C  "a'	'8

5<<0@#VY`abYc#cijkI""59I9I9K)L#MN 0=5;;'g5;;C_`fBgg 	xs   +A&IIc                     t        j                  d      d   j                  g |d   j                  d d dd j	                         }t
        j                  j                  j                  |d   dd df         |dd dd df<   |d   ddd f   |dd ddf<   t        | j                        dk(  r| d    n| |d d |f   j                         z  } t        |      t        k(  r	 t        |||d   |      }t        j                  |d	   |fd      }t        j                  d
| t!        |d               dd df   }t
        j                  j                  j#                  | d d d dd df   d      |d   z  }	t%        ||	|d   j'                  d      |d   |t        j                  d      j	                         d    ||d   |d   dt        j(                  |      d d dz         S )Nrb   r;   
lie_perpixr2   r   .r   rC   colorsz
kij,kj->kir   epsquatsscales皙?)r?   	opacitiesr:   r   zRGB+D)render_modebackgrounds)r   rc   rd   shaper]   rP   rQ   rR   rf   re   r=   typer^   rI   r   r   r>   rT   rasterizationclip
zeros_like)
posetimestepimsizer   
splat_varsrA   flowcolors_imeans_iquats_is
             r   	do_renderr   f   sG   0%))A,y)00[*\2J2P2PQSRS2T[UW[XZ[``bK#__88VVWabnWopstvuvtvpvWwxKBQBrr	'5c!"f=KBQBr	djj/1,DJ$+aj:Q:Y:Y:[[DH~u HZ-@CDyy*X.t4R8HLLd3z'/B+CDS!VLGOO''EEd1RaRPRQRPR7mX\E]^hip^qqG'7Jx,@,E,E",E,MzZeOfhprwr{r{|}r~  sD  sD  sF  GK  sL  NO  QW  XY  QZ  \b  cd  \e  ry((22A6q8: :r   c                 
   t        dt        | d   d         dz        }t        j                  t        j
                  j                  j                  | d   dd dd df   d	      | d   dd dd
f   fd
      }|j                  t        | d   d   j                  dd            d
d
      | d<   t        j                  j                  | d   d   j                  dd      d d |         t        j                  j                  | d   d   j                  dd      d d |   dz  dz         t        j                  j                  t        j                  t        | d   d   j                  dd      d d |         d      j                               t        j                  j                  t        j                  t        | d   d   j                  dd      d d |               j                         dz        t        j                  j                  t        j                  t        | d   d   j                  dd      d d |         d      j                         dz        t        j                  j                  | d   d d |         dS )Nr:   
world_crdsr      r   .r   g-C6?r   r   lie_crdsrgb_crds      ?rb   皙?g{Gz?)rC   r   r   r   r   r   )r?   re   r   r   rP   rQ   rR   rT   rd   flattennn	Parameteronesr]   )scenestride	poses_lies      r   format_splat_varsr   z   s   qU<(+,b01F 		6??66TTUZ[bUcdghjijhjkmlmkmdmUnswTxy~  @G  zH  IL  MO  NO  MO  PR  IR  zS  T  UW  XI!((U<-@-C-K-KAa-P)QRTUWXE*  ((,,U<-@-C-K-KAa-PQYSYQY-Z[((,,U:->q-A-I-I!A-NxQWx-XY[-[\^-^`((,,UZZE,<OPQ<R<Z<Z[\]^<_`hbh`h<i8jkl-m-r-r-tv((,,UZZE,<OPQ<R<Z<Z[\]^<_`hbh`h<i8j-k-p-p-rsv-vx((,,UZZE,<OPQ<R<Z<Z[\]^<_`hbh`h<i8jkl-m-r-r-tux-xz 88--eJ.?&.IJ r   c                 0	   t        | j                        dkD  rut        | j                  dd      |j                  dd      |j                  dd      |j                  dd            }|d   |d   j	                  d| j                  d d       fS 	 t        j                  dt
        j                        5  | j                  ddd      } |j                  ddd      }|j                  ddd      }d}d}|j                  |	      }||j                  d
d      |z   z  }| |z  j                  dd      }||z  j                  dd      }	|j                  |	      }| |z
  }
||	z
  }|dd df   }ddlm}  |        t        j                  d||      }t        j                  d|j                  d      |
j                  d      z  ||j                  ddd            }t        j                  d||
||j                  ddd            }t        j                  d|||
||j                  ddd             t        j                  |      \  }}}t        j                   |j                  d   | j"                        j                  d      }|j%                  |j                  d   dd      }|d d d
d
fxx   t        j&                  t        j(                  |j+                  |j                  ddd                        z  cc<   |j+                  |j+                  |j                  ddd                  }|	 |j*                  |      z
  } |j*                  |       |z   }t        j                   d      d    j-                  | j/                  d      d
d
      j1                  |       }||d d d dd df<   t        j                   d      d    j-                  | j/                  d      d
d
      j1                  |       }|j3                  d
      |d d d dd
f<   t        j                   d      d    j-                  | j/                  d      d
d
      j1                  |       }||z  |z  }||z
  j5                         j7                         |fcd d d        S # 1 sw Y   y xY w)Nr   r   r:   r2   r]   device_typedtypeTư>minr   r"   .)	set_tracezbik,bjk->bijzbpij,bij,bjk->bpikzbik,bpij,bij,bjk->bpikzbik,bjk,bpk,bcp,bcq->bpikrp   rb   )re   r   efficient_nonrig_procrustesr   	unflattenr   autocastfloat32permuter   r'   pdbr   r   	unsqueezesvdrc   rq   r   signdetbmmrd   r4   tosqueezesquaremean)S1S2weightsaff_embout
transposedr   weights_normmu1mu2X1X2pdb_tmpr   UsVZrx   tS1_hatR_T_S_r_   s                             r   r   r      s    bhh-/)"**Qq/"**Qq/'//Z[\]J^_f_n_nopqr_st1vc!f&&q"1666 
F%--	@ F2ZZ!AZZ!A!A&
 % Bt <S @A,##Ad#3,##Ad#3!&&3&/#X#X RaR)$&ll>'':
 LL-s}}Q/?Q/OQXZ\ZdZdefhiklZmnLL17BTUWXZ[I\] 	17GRRTR\R\]^`acdRef ))A,1a IIaggaj3==a@HHQWWQZ!$	!B(uzz%))AEE!))Aa2B,C"DEE EE!%%		!Aa()* EAEE#J  rQ 99Q<$$RWWQZ2699"=1RaR799Q<$$RWWQZ2699"=IIbM1RaR799Q<$$RWWQZ2699"=Brr	!!#((*61MF2 F2 F2s   4ORRc                 	   t        | j                        dk(  rht        | j                  dd      |j                  dd      ||j                  dd      nd       }|d   |d   j	                  d| j                  d d       fS t        | j                        dk(  rht        | j                  dd      |j                  dd      ||j                  dd      nd       }|d   |d   j	                  d| j                  d d       fS 	 t        j                  dt
        j                        5  | j                  ddd      } |j                  ddd      }||j                  ddd      }d}|t        j                  | d d d df         }d	}|j                  |
      }||j                  dd      |z   z  }| |z  j                  dd      }||z  j                  dd      }|j                  |
      }| |z
  }	||z
  }
|	|z  j                  |
j                  ddd            }t        j                  |      \  }}}t        j                  |j                  d   | j                        j!                  d      }|j#                  |j                  d   dd      }|d d ddfxx   t        j$                  t        j&                  |j                  |j                  ddd                        z  cc<   |j                  |j                  |j                  ddd                  }| |j                  |      z
  } |j                  |       |z   }t        j                  d      d    j)                  | j+                  d      dd      j-                  |       }||d d d dd df<   t        j                  d      d    j)                  | j+                  d      dd      j-                  |       }|j/                  d      |d d d ddf<   t        j                  d      d    j)                  | j+                  d      dd      j-                  |       }||z  |z  }||z
  j1                         j3                         |fcd d d        S # 1 sw Y   y xY w)N   r   r:   r2   rb   r]   r   Tr   r   r   r"   rp   r   )re   r   efficient_procrustesr   r   r   r   r   r   r   r   r'   r   r   rc   rq   r   r   r   r   rd   r4   r   r   r   r   )r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   rx   r   r   r   r   r   r_   s                          r   r   r      s    288}a"2::a?2::a?[b[n7??STUVCWtxy1vc!f&&q"1666
288}a"2::a?2::a?[b[n7??STUVCWtxy1vc!f&&q"1666 
F%--	@ 72ZZ!AZZ!AOOAa*G
?eoob2A2h&?G% Bt <S @A,##Ad#3,##Ad#3!&&3&/#X#X ZRZZ!A./ ))A,1a IIaggaj3==a@HHQWWQZ!$	!B(uzz%))AEE!))Aa2B,C"DEE EE!%%		!Aa()* EAEE#J  rQ 99Q<$$RWWQZ2699"=1RaR799Q<$$RWWQZ2699"=IIbM1RaR799Q<$$RWWQZ2699"=Brr	!!#((*61o72 72 72s   'M	Q::Rc                 8	   t        | j                        dk(  rht        | j                  dd      |j                  dd      ||j                  dd      nd       }|d   |d   j	                  d| j                  d d       fS t        | j                        dk(  rht        | j                  dd      |j                  dd      ||j                  dd      nd       }|d   |d   j	                  d| j                  d d       fS 	 t        j                  dt
        j                        5  | j                  ddd      } |j                  ddd      }||j                  ddd      }d}|t        j                  | d d d df         }d	}|j                  |
      }||j                  dd      |z   z  }| |z  j                  dd      }||z  j                  dd      }|j                  |
      }| |z
  }	||z
  }
t        j                  |j                  d            }|	|z  j                  |
j                  ddd            }t        j                  |      \  }}}t        j                   |j                  d   | j"                        j%                  d      }|j'                  |j                  d   dd      }|d d ddfxx   t        j(                  t        j*                  |j                  |j                  ddd                        z  cc<   |j                  |j                  |j                  ddd                  }| |j                  |      z
  } |j                  |       |z   }t        j                   d      d    j-                  | j/                  d      dd      j1                  |       }||d d d dd df<   t        j                   d      d    j-                  | j/                  d      dd      j1                  |       }|j                  d      |d d d ddf<   t        j                   d      d    j-                  | j/                  d      dd      j1                  |       }||z  |z  }||z
  j3                         |fcd d d        S # 1 sw Y   y xY w)Nr   r   r:   r2   rb   r]   r   Tg}:r   r   r"   rp   r   )re   r   rM   r   r   r   r   r   r   r   r   r'   
diag_embedr   r   r   rc   rq   r   r   r   r   rd   r4   r   abs)r   r   r   r   r   r   r   r   r   r   r   diagsr   r   r   r   r   rx   r   r   r   r   r   r_   s                           r   rM   rM   .  s    288}aAaAaQXQd19Mjno1vc!f&&q"1666
288}aAaAaQXQd19Mjno1vc!f&&q"1666 
F%--	@ 4(ZZ!AZZ!AOOAa*G
?eoob2A2h&?G% Bt <S @A,##Ad#3,##Ad#3!&&3&/#X#X  !34 XNN2::a!,- ))A,1a IIaggaj3==a@HHQWWQZ!$	!B(uzz%))AEE!))Aa2B,C"DEE EE!%%		!Aa()* EAEE#J  rQ 99Q<$$RWWQZ2699"=1RaR799Q<$$RWWQZ2699"=IIbM1RaR799Q<$$RWWQZ2699"=Brr	 'i4( 4( 4(s   'MRRpointsc                     t        j                  | dddf   | j                        }t        j                  | |fd      S )zAppends a "1" to the coordinates of a (batch of) points of dimension DIM.

    Args:
        points: points of shape (..., DIM)

    Returns:
        points_hom: points with appended "1" dimension.
    .Nr:   rp   r   r    )r   r   rq   r   )r   r   s     r   homogenize_pointsr   t  s9     ??6#rr'?6==AD99fd^,,r   vectorsc                     t        j                  | dddf   | j                        }t        j                  | |fd      S )zAppends a "0" to the coordinates of a (batch of) vectors of dimension DIM.

    Args:
        vectors: vectors of shape (..., DIM)

    Returns:
        vectors_hom: points with appended "0" dimension.
    .Nr:   rp   r   r    )r   r   rq   r   )r   zeross     r   homogenize_vecsr     s=     WS"1"W-gnnEE99gu%2..r   xy_pixz
intrinsicsc                 p    t        |       }t        j                  d|j                         |      }||z  }|S )aw  Unproject (lift) 2D pixel coordinates x_pix and per-pixel z coordinate
    to 3D points in camera coordinates.

    Args:
        xy_pix: 2D pixel coordinates of shape (..., 2)
        z: per-pixel depth, defined as z coordinate of shape (..., 1)
        intrinscis: camera intrinscics of shape (..., 3, 3)

    Returns:
        xyz_cam: points in 3D camera coordinates.
    ...ij,...kj->...ki)r   r   r   r=   )r   r   r   
xy_pix_homxyz_cams        r   	unprojectr     s8     #6*Jll/1C1C1EzRGqLGNr   xyz_world_hom	cam2worldc                 D    t        j                  |      }t        | |      S )a  Transforms points from 3D world coordinates to 3D camera coordinates.

    Args:
        xyz_world_hom: homogenized 3D points of shape (..., 4)
        cam2world: camera pose of shape (..., 4, 4)

    Returns:
        xyz_cam: points in camera coordinates.
    )r   r=   transform_rigid)r   r   	world2cams      r   transform_world2camr    s     i(I=)44r   xyz_cam_homc                     t        | |      S )a  Transforms points from 3D world coordinates to 3D camera coordinates.

    Args:
        xyz_cam_hom: homogenized 3D points of shape (..., 4)
        cam2world: camera pose of shape (..., 4, 4)

    Returns:
        xyz_world: points in camera coordinates.
    )r  )r  r   s     r   transform_cam2worldr    s     ;	22r   xyz_homTc                 0    t        j                  d||       S )zApply a rigid-body transform to a (batch of) points / vectors.

    Args:
        xyz_hom: homogenized 3D points of shape (..., 4)
        T: rigid-body transform matrix of shape (..., 4, 4)

    Returns:
        xyz_trans: transformed points.
    r   )r   r   )r  r  s     r   r  r    s     <<,a99r   c           	      j    t        | t        j                  | dd df   | j                        |      S )N.r:   rp   )r   )r   r   r   rq   )r   r   s     r   #get_unnormalized_cam_ray_directionsr    s5     sBQBw> r   c                 j   |Gt        j                  d      d    j                  | j                  d      dd      j	                  |       }|dd ddf   }t        | |      }||j                  dd      z  }t        |      }t        ||      }t        |d|j                  d	      
      }||dd df   fS )Nrb   r   r   .r   T)r!   r#   z... ch -> ... num_rays chr$   )num_rays)
r   rc   rd   r4   r   r  r   r   r  r   )r   r   r   cam_origin_worldray_dirs_cam
rd_cam_homrd_world_homs          r   get_world_rays_r    s     IIaL&--fkk!nRCFFvN	 !bqb"- 7vzJL,"3"3D"3"IIL !.J 'z9=L/1LWcWhWhikWln \#rr'222r   c           	      2   t        | j                        dk(  rnt        | j                  dd      |j                  dd      ||j                  dd      nd       }|D cg c]!  }|j	                  d| j                  d d       # c}S t        | ||      S c c}w )Nrb   r   r:   r2   )re   r   r  r   r   )r   r   r   r   r   s        r   get_world_raysr    s    
 6<<!fnnQq1*2D2DQq2Idmdy)J[J[\]^_J`  @D  E9<=AAfll2A./==6*Y77 >s   &Bc                    | j                   \  }}|j                   \  }}| j                  d      }|j                  d      }	| |z
  }
||	z
  }|
dz  j                         }|dz  j                         }t        j                  |      }t        j                  |      }|
|z  }
||z  }||k  r/t        j
                  |t        j                  |||z
        fd      }t        j                  |
j                  |      }t        j                  j                  |d      \  }}}|j                  }t        j                  ||j                        }|dk7  ret        j                  j                  |      dk  }||k7  r>|d d dfxx   dz  cc<   |dxx   dz  cc<   t        j                  ||j                        }|j                         }|r0||z  |z  }d|dz  z
  }||z  t        j                  ||      z  |z   }n2d}d||z  z   d|z  |z  |z  z
  }|t        j                  ||      z  |z   }||k  r|d |d d f   }||t        j                  |	|      z  z
  }|||d	}|||fS )
Nr   g       @F)full_matricesbestr   r:   r2   )rotationscaletranslation)r   r   r'   npsqrtconcatenater   dotr  linalgr   r   )XYscaling
reflectionnmnymymuXmuYX0Y0ssXssYnormXnormYAr   r   Vtr   r  have_reflectiontraceTAbdr   ctforms                                r   numpy_procrustesr8    sF   
''CAaGGEBr
&&)C
&&)C	
SB	
SBr6,,.Cr6,,.C GGCLEGGCLE %KB%KB	Av^^R!QrT!23A6 	rttRAYY]]15]1FAa
A
q!##AV ))--*Q. (adGrMGbERKEq!##AeegG eOe# 
N 'M"&&Q-'#- CK!g+-55"&&Q-#% 
Avcrc!eHasAA 1A6E a;r   c                 &   | j                   \  }}t        j                  t        j                  |      t        j                  |      d      \  }}| }	||z
  |	z  |z  }
||z
  |	z  |z  }t        j                  |
||	fd      j                  dd      S )Nxy)indexingr   )axisr   )r   r  meshgridaranger)   r6   )depthfxfycxcyhwr   jr   r   ys               r   depth_to_point_cloudrH  K  s    ;;DAq;;ryy|RYYq\DADAqA	
R1rA	
R1rA88Q1IB'//A66r   c                     | dt        j                  t        j                  |dz              z  z  }||| dz  |dz  f\  }}}}t        j                  d      }||||f\  |d<   |d<   |d<   |d<   |||||fS )Nr2   rb   r<   )r:   r:   )r   r2   )r:   r2   )r  tanradiansrc   )	widthheightfov_degfr@  rA  rB  rC  r   s	            r   get_camera_intrinsicsrP  R  s    RVVBJJw{3445AAuqy&1*,KBr"RffQiA%'R^#AcFAcFAcFAcFbBq=r   c                 $   | j                   |j                   k(  sJ | j                  d      }|j                  d      }| |z
  }||z
  }|j                  |z  }t        j                  j                  |      \  }}}	|	j                  |j                  z  }
t        j                  j                  |
      dk  r*|	dddfxx   dz  cc<   |	j                  |j                  z  }
||
|z  z
  }t        j                  d      }|
|ddddf<   ||dddf<   |S )z
    A: (N,3) source points (e.g., marker coordinates)
    B: (N,3) target points (e.g., lifted 3D keypoints)

    Returns: R (3x3), t (3,)
    r   r   Nrb   r   )r   r   r  r  r  r   r   rc   )r0  Bmu_Amu_B
A_centered
B_centeredHr   Sr1  rx   r   r  s                r   procrustes_umeyamarY  X  s     77agg 66!9D66!9D TJTJ 	z!A yy}}QHAq"
qss
A 
yy}}Q!
2q5	R	DD133Jq4xAffQiAAbqb!eHAbqbeHHr   c                    t        j                  g dg dg dg dg      |z  }t        j                  |t        j                  d      g      }| |j                  z  j                  ddddf   }| dddf   }|D ]  }	 |j
                  t        ||	      d|i   t        d	      D ]6  }
||
   }||
d
z   d	z     } |j
                  t        ||      ||
dk(  r|ndd 8 |S )u   
    Plot a camera frustum defined by camera->world pose T (4×4).
    The frustum is a pyramid whose apex is at the camera center and
    whose base is the square at z=scale in camera coords.
    )r:   r:   r:   )r   r:   r:   )r   r   r:   )r:   r   r:   )rb   r:   Nr   colorrb   r:   r   )r[  label)r  arrayhstackr   r  plotzipr~   )r  r  axr[  r\  cam_ptscam_h	world_ptsCPr   p1p2s                r   plot_frustumri  z  s     hh###% & )..G
 IIw/0EUWW2A2&I	"1"a%A 8Q8%88 1X Iq\!qy!REA4HI
 Ir   c                    |t        j                  |      z  t        j                  |       } | d d d ddf   }|j                  j	                        }| d d d dd df   }t        j                  |      j                         t        t                    D ]/  }t        j                  |   d         dk  s#|xx   dz  cc<   1 t        fdt        t                    D              }t         j                  j                  |      \  }}|d d t        j                  |      f   }	t        j                  |	      j                         }
t        j                  d      }|
|d dd df<   ||d ddf<   |S )Nr   r   r   c              3   b   K   | ]&  }|   t        j                  |   |         z   ( y wN)r  outer).0r   qsrE  s     r   	<genexpr>z$average_se3_split.<locals>.<genexpr>  s-     BaAaD288BqE2a5))Bs   ,/rb   )r  r'   asarrayr  r  rx   ry   rz   r~   re   r  eighargmax	from_quat	as_matrixrc   )Tscstst_meanRsr   Meigvalseigvecsq_meanR_meanTmro  rE  s               @@r   average_se3_splitr    sL   
RVVBZA	BB	Arr1HBTTXXa[F 
Arr2A2IB	
r		"	"	$B3r7^ 66"Q%A!#qERKE 	B5R>BBAyy~~a(GWQ		'**+F[[ **,F	BBrr2A2vJBrr1uIIr   c                    t         j                  j                  ddt         j                  z        }t         j                  j                  t         j                   dz  t         j                  dz        }d}dd|g}| t        j                  |      z  t        j                  |      z  | t        j                  |      z  t        j
                  |      z  | t        j
                  |      z  |z   g}t        j                  t         j                  j                  dd      t         j                  j                  dd      t         j                  j                  dd      g      dz  }t        j                  |      |z   }t        j                  |      |z   }t         j                  j                  d	d
      dz  t         j                  j                  d	d
      dz  dg}|j                         |j                         |fS )Nr   r2   r   r   g333333?r   r:   g?gr   )r  randomuniformpicossinr]  tolist)radiusazimuth_camelevation_camz_offsetlook_at_poscam_posrandom_translationrolls           r   sample_camera_positionsr    s   ))##Aq255y1KII%%ruufSj2558=M H a"K ..1DDfrvvVcOdFdgigmgmnygzFz  }C  FH  FL  FL  MZ  F[  }[  ^f  }f  hG BII$5$5b!$<bii>O>OPRTU>VXZXaXaXiXijlnoXp"rsvxx hhw"44G((;'*<<KIIc"%a'):):3r)B1)DaHD>>[//1477r   c                 ,   i }t        j                  |       \  }}t        j                  t        j                  |            j                  dd      }t        j                  d      }||ddddf<   ||dddf<   ||d<   t        t        j                  |             D ]  }t        j                  | |d      }|d   |d   }}t        j                  t        j                  |            j                  dd      }t        j                  d      }||ddddf<   ||dddf<   |||<    t        j                  |       }|D 	ch c]  }	|	d   t         j                  k(  s|	d	     }
}	t        d
 |D              r|
j                  d       ||
fS c c}	w )u   
    Returns:
      transforms: dict link_idx → 4×4 numpy.ndarray world→link
      visual_links: set of link indices (including -1 for base) that have visuals
    r   rb   Nr   T)computeForwardKinematicsr   r2   r:   c              3   ,   K   | ]  }|d    dk(    yw)r:   r   Nr   )rn  r5  s     r   rp  z*get_all_link_transforms.<locals>.<genexpr>  s     
+!1Q42:
+s   )pgetBasePositionAndOrientationr  r]  getMatrixFromQuaternionr6   rc   r~   getNumJointsgetLinkStategetVisualShapeData	GEOM_MESHanyadd)body_id
transformsposornrx   r  r   stvisual_datar5  visual_linkss              r   get_all_link_transformsr    s    J..w7HC
**3/0881=A
q	Aa1RaRU832A2a4JrN 1>>'*+ ^^GQFa5"Q%SHHQ..s34<<QqAFF1I!q!BQBxsQrr!tW
1 &&w/K"-EQ11DAaDELE

+{
++|## Fs   F!Fc                     t        j                  |       x}}t        |      D ]U  }t        j                  | |      }|d   |d   }}||k  s)t	        j
                  ||      }t        j                  | ||       W y )N   	   )r  r  r~   getJointInfor  r  resetJointState)rob_idn_joints
num_jointsr   infolowerupper
rand_angles           r   randomly_move_jointsr    ss    ..00HZ: 5~~fa(AwQu5=u5Jfa45r   )r   Frl  )Tr  )g      ?NredCam)A__doc__r   einopsr   r   torch.nnr   r%   rv   r  mathr   r   r  r   r   	jaxtypingr	   r
   rP   pybulletr  r>   r   r@   r/   r8   rI   sklearn.clusterrJ   scipy.spatial.transformrK   rx   rw   r]   r^   rN   rZ   r`   ri   rn   r   r   r   r   r   rM   r   r   r   r  r  r  r  r  r  r8  rH  rP  rY  ri  r  r  r  r  r   r   r   <module>r     s   B  $ $          C2R-ell -u|| -@%,, @5<< @ # 1 5<<+kEFtLQQSYY[:
ch4:(&R2lF2VC(L
-ell 
-
/U\\ 
/LL"\\7<||
\\(5<<5,1LL5
\\5 33*/,,3
\\3
:U\\ 
:ell 
:u|| 
:LL&+ll
\\3LL33 ||3 \\	368LL88 ||8 \\	8K\7!D< 2080$6	5r   