
    +jN                        S r SSKrSSKrS\R                  S\R                  S\R                  S\R                  4S jrS\R                  S	\R                  S\R                  S\4S
 jr  SS\R                  S\R                  S\R                  S\R                  S\S\S\S\S\R                  4S jjr	S\
S\S\S\
S\R                  4
S jrg)zPure-numpy port of MediaPipe's face_geometry (FACE_LANDMARK_PIPELINE mode)
+ weighted Procrustes solver. Computes the 4x4 facial transformation matrix.
    Nsrctgtweightsreturnc                 
   [         R                  " UR                  [         R                  5      5      n[	        US-  R                  5       5      nU R                  [         R                  5      U-  nUR                  [         R                  5      U-  nXS-  U-  nU[         R                  " Xs5      -
  n[         R                  R                  XhR                  -  SS9u  pnU	R                  5       UR                  R                  5       p[         R                  R                  U5      [         R                  R                  U5      -  S:  a  USS2S4==   S-  ss'   XR                  -  n[	        X-  R                  5       5      nUS:  a  [        S5      e[	        X-  U-  R                  5       5      U-  nUUX-  -  -
  U-  U-  n[         R                  " S	[         R                  S
9nUU-  USS2SS24'   UUSS2S4'   U$ )u   Weighted orthogonal Procrustes (similarity). Returns 4x4 M with
`target ≈ M @ homogeneous(source)` in the weighted LS sense. fp64 for
SVD stability. Port of procrustes_solver.cc.   T)full_matricesr   N      g-q=z5Procrustes denominator collapsed (degenerate source).   dtype   )npsqrtastypefloat64floatsumouterlinalgsvdTcopydet
ValueErroreye)r   r   r   sqrt_ww_totalwswtc_wcenteredU_SVtpostpreRdenomscaletranslationMs                      D/home/wildlama/comfy/ComfyUI/comfy_extras/mediapipe/face_geometry.py"_solve_weighted_orthogonal_problemr.   
   s    WWW^^BJJ/0FVq[%%'(G	BJJ	&	(B	BJJ	&	(B;'
!CBHHS))H		b::oTBIA2"$$))+#	yy}}TRYY]]3//!3QT
d
uuA8=%%'(Eu}PQQ1<"$))+,u4E!&))V3w>K
q

#A	Abqb"1"fIAbqb!eHH    	canonicalruntimec           	      t    [        [        R                  R                  [	        XU5      SS2S4   5      5      $ )uL   scale = ‖first column of M[:3]‖ per geometry_pipeline.cc::EstimateScale.Nr   r   )r   r   r   normr.   )r0   r1   r   s      r-   _estimate_scaler4   (   s5     B9W^ _`bab`bde`e fghhr/   landmarks_normalizedcanonical_verticesprocrustes_indicesprocrustes_weightsimage_widthimage_heightvertical_fov_degreesnearc                   ^^ ST-  [         R                  " S[         R                  " U5      -  5      -  nXH-  U-  n	UR                  [        R
                  5      n
X
   R                  R                  [        R                  5      R                  5       nX   R                  R                  [        R                  5      R                  5       nUR                  [        R                  5      nSUS   -
  US'   US   U	-  SU	-  -
  US'   US   U-  SU-  -
  US'   US   U	-  US'   [        US   R                  5       5      mS[        R                  S[        S	[        R                  4UU4S
 jjnUR                  5       nUS==   S-  ss'   [        XU5      n[        X" UU5      U5      n[        X" UUU-  5      U5      R                  [        R                  5      $ )z4x4 facial transformation matrix via two-pass scale recovery
`landmarks_normalized` is (N, 3) in MediaPipe normalized convention: x, y
in [0,1] with TOP-LEFT origin, z in width-scaled units.
g       @g      ?      ?   r   r   sr*   r   c                    > U R                  5       n U S   T-
  T-   U-  U S'   U S==   U S   T-  -  ss'   U S==   U S   T-  -  ss'   U S==   S-  ss'   U $ )Nr   r   r?   r
   )r   )r@   r*   depth_offsetr<   s     r-   
_unproject6solve_facial_transformation_matrix.<locals>._unprojectL   sh    FFH!|#d*e3!	!!t	!!t	!r/   r
   )mathtanradiansr   r   int64r   r   r   r   meanndarrayr4   r.   float32)r5   r6   r7   r8   r9   r:   r;   r<   h_nearw_nearsubscreencanonr   rC   firsts1s2rB   s          `          @r-   "solve_facial_transformation_matrixrT   -   s     4Z$((36J)K#KLLF!L0F

#
#BHH
-C!&((//

;@@BF#%%,,RZZ8==?E ''

3G fQiF1Iq	F"S6\1F1Iq	F"S6\1F1Iq	F"F1I)*Lbjj  2::   KKME	!HH	w	/B	
62 6	@B-eZR5PRYZaabdblblmmr/   	face_dictcanonical_datac           	         U S   U S   pT[         R                  " USS2SS24   R                  [         R                  5      [         R                  " UR
                  S   S45      /SS9n[         R                  R                  XdR                  [         R                  5      SS9tpx[        [         R                  R                  US   5      5      n	U	S	:  a  X-  OS
U-  n
[         R                  " UR
                  S   S4[         R                  S9nUSS2S4   U-  USS2S4'   USS2S4   U-  USS2S4'   USS2S4   U
-  USS2S4'   [        XS   US   US   XS9$ )zAdapt a FaceLandmarker face dict to MP's normalized convention and solve.
FaceMesh emits (x, y, z) in 192-canonical units; MP's geometry expects
z_norm = z_canonical * scale_x / image_widthlandmarks_xylandmarks_3dNr   r   r?   )axis)rcondgư>r>   r   r   r6   r7   r8   )r9   r:   )r   concatenater   r   onesshaper   lstsqr   r3   emptyrK   rT   )rU   r9   r:   rV   lmks_xylmks_3daugr,   _scale_xz_scale
normalizeds               r-   $transformation_matrix_from_detectionrh   [   sQ   
 !0)N2KW
..'!RaR%.//

;RWWgmmTUFVXYEZ=[\cd
eCIIOOC

!;4OHEABIINN1Q4()G'.~g#3;LG7==+Q/rzzBJq!t}{2Jq!tq!t}|3Jq!tq!t}w.Jq!t-#78+,n=Q.R r/   )g     O@r>   )__doc__rE   numpyr   rJ   r.   r   r4   intrT   dictrh    r/   r-   <module>rn      s7  
  BJJ RZZ RTR\R\ acakak <irzz iBJJ i iX] i #'+n**+n

+n 

+n 

	+n
 +n +n  +n +n ZZ+n\D s Z] os xz  yC  yC r/   