
    i'iK                        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mZ ddlmZ ddlmZ ddlmZ ddlZdd	lmZ d
 Zd Zd Zd ZddlmZ ddlm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Z%dej"                  dej"                  dej"                  dej"                  fdZ&dej"                  d ej"                  dej"                  fd!Z'd"ej"                  d ej"                  dej"                  fd#Z(d$ej"                  d%ej"                  dej"                  fd&Z)dej"                  dej"                  dej"                  fd'Z*dej"                  dej"                  d ej"                  dej"                  fd(Z+dej"                  dej"                  d ej"                  dej"                  fd)Z,d-d*Z-y).z`Multi-view geometry & proejction code.. (most from vincent, some old nerf stuff hanging around)     N)	rearrangerepeat)
functional)ceillog2)einsum)Float)Tensor)rasterizationc                 d    t        j                  | t        j                  | ddgf         fd      S )N.r   )torchcat	ones_likexs    )/home/cameronsmith/repos/sire/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   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      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_flowr-      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)Rotationc                    | 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   dim*   )
n_clustersrandom_state)reshapesizecpunumpyr   tensorRfrom_matrixas_quatr2   r   r.   fitlabels_cluster_centers_rangewherenumelnormappendargminstack)posesr6   return_labelstranslations	rotationsquaternionsfeatureskmeanslabelscentersrepresentativesicluster_indicescluster_features	distancess                  r   cluster_and_representrX   #   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 )N   r    
lie_perpix   r   .r   r'   colorsz
kij,kj->kir   epsquatsscalesg?)r#   	opacitiesr   r   zRGB+D)render_modebackgrounds)r   eyeexpandshapecudakorniageometryconversionsquaternion_to_rotation_matrixlenr!   typefloatr-   r   r   r"   rotation_matrix_to_quaternionr   clip
zeros_like)
posetimestepimsizer   
splat_varsr%   flowcolors_imeans_iquats_is
             r   	do_renderr{   =   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      rJ   .r   g-C6?r^   r   lie_crdsrgb_crdsg      ?rZ   g?g{Gz?)r'   r]   r`   rb   ra   r[   )r#   rm   r   r   ri   rj   rk   rp   rf   flattennn	Parameteronesrh   )scenestride	poses_lies      r   format_splat_varsr   Q   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   r\   rh   device_typedtypeTư>minr   keepdim.)	set_tracezbik,bjk->bijzbpij,bij,bjk->bpikzbik,bpij,bij,bjk->bpikzbik,bjk,bpk,bcp,bcq->bpikr1   rZ   )rm   rg   efficient_nonrig_procrustesr   	unflattenr   autocastfloat32permuterq   sumpdbr   r   	unsqueezesvdre   r2   r   signdetbmmrf   r9   tosqueezesquaremean)S1S2weightsaff_embout
transposedr_   weights_normmu1mu2X1X2pdb_tmpr   UsVZr=   tS1_hatR_T_S_transfs                             r   r   r   d   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   r\   rZ   rh   r   Tr   r   r   r   r1   r   )rm   rg   efficient_procrustesr   r   r   r   r   r   r   rq   r   r   r   re   r2   r   r   r   r   rf   r9   r   r   r   r   )r   r   r   r   r   r_   r   r   r   r   r   r   r   r   r   r   r=   r   r   r   r   r   r   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                 T	   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                         j5                         |fcd d d        S # 1 sw Y   y xY wr   )rm   rg   
procrustesr   r   r   r   r   r   r   rq   r   
diag_embedr   r   r   re   r2   r   r   r   r   rf   r9   r   r   r   )r   r   r   r   r   r_   r   r   r   r   r   diagsr   r   r   r   r   r=   r   r   r   r   r   r   s                           r   r   r     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2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	!!#((*61i42 42 42s   '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   r1   r   r3   )r   r   r2   r   )r   r   s     r   homogenize_pointsr   K  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   r1   r   r3   )r   rr   r2   r   )r   zeross     r   homogenize_vecsr   X  s=     WS"1"W-gnnEE99gu%2..r   xy_pixz
intrinsicsreturnc                 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   e  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   y  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   r1   )r   )r   r   r   r2   )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 )NrZ   r   r   .r   T)r4   r   z... ch -> ... num_rays ch)num_rays)
r   re   rf   r9   r   r   rF   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 )NrZ   r   r   r\   )rm   rg   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   r\   )rotationscaletranslation)rg   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_procrustesr    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   )r   F)N)Tr   ).__doc__r   einopsr   r   torch.nnr   Fr;   r   mathr   r   r   r   	jaxtypingr	   r
   ri   gsplatr   r"   r   r$   r-   sklearn.clusterr.   scipy.spatial.transformr/   r=   rX   r{   r   r   r   r   r   r   r   r   r   r   r   r   r   r  r   r   r   <module>r     s   f  $ $          C2R # 1h4:(&R2lF2VC2L
-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r   