
    +jN                        d Z ddlZddl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fd
Z	 	 ddej        dej        dej        dej        dededededej        fdZ	de
dedede
dej        f
dZdS )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                 D   t          j        |                    t           j                            }t	          |dz                                            }|                     t           j                  |z  }|                    t           j                  |z  }||z  |z  }|t          j        ||          z
  }t           j                            ||j	        z  d          \  }	}
}|	
                                |j	        
                                }}t           j                            |          t           j                            |          z  dk     r|dddfxx         dz  cc<   ||j	        z  }t	          ||z                                            }|dk     rt          d          t	          ||z  |z                                            |z  }||||z  z  z
  |z  |z  }t          j        d	t           j        
          }||z  |ddddf<   ||dddf<   |S )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    WW^^BJ//00FVq[%%''((G	BJ		&	(B	BJ		&	(B;'
!CBHS&)))H	b8:oTBBIAr2"$))++#D	y}}TRY]]3///!33QQQT


d


suA8b=%%''((Eu}}PQQQ1x<"$))++,,u4E!b&))V3w>K
q
###A	Abqb"1"fIAbqb!eHH    	canonicalruntimec           	          t          t          j                            t	          | ||          dddf                             S )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   (   s>     B9gW^ _ _`bab`bde`e fgghhhr/        O@      ?landmarks_normalizedcanonical_verticesprocrustes_indicesprocrustes_weightsimage_widthimage_heightvertical_fov_degreesnearc                    dz  t          j        dt          j        |          z            z  }||z  |z  }	|                    t          j                  }
| |
         j                            t          j                                                  }||
         j                            t          j                                                  }|                    t          j                  }d|d         z
  |d<   |d         |	z  d|	z  z
  |d<   |d         |z  d|z  z
  |d<   |d         |	z  |d<   t          |d         
                                          dt          j        dt          d	t          j        ffd
}|                                }|dxx         dz  cc<   t          |||          }t          | |||          |          }t          | ||||z            |                              t          j                  S )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      ?r6      r   r   sr*   r   c                     |                                  } | d         z
  z   |z  | d<   | dxx         | d         z  z  cc<   | dxx         | d         z  z  cc<   | dxx         dz  cc<   | S )Nr   r   r@   r
   )r   )rA   r*   depth_offsetr>   s     r-   
_unprojectz6solve_facial_transformation_matrix.<locals>._unprojectL   s    FFHH!|#d*e3!	!!t	!!t	!r/   r
   )mathtanradiansr   r   int64r   r   r   r   meanndarrayr4   r.   float32)r7   r8   r9   r:   r;   r<   r=   r>   h_nearw_nearsubscreencanonr   rD   firsts1s2rC   s          `          @r-   "solve_facial_transformation_matrixrT   -   s     4Z$(36J)K)K#KLLLF6!L0F

#
#BH
-
-C!#&(//
;;@@BBFs#%,,RZ88==??E ''
33G fQiF1Iq	F"S6\1F1Iq	F"S6\1F1Iq	F"F1I))**Lbj  2:        KKMME	!HHHHHH	w	/	/B	

62 6 6	@	@B-eZZR5P5PRYZZaabdblmmmr/   	face_dictcanonical_datac                    | d         | d         }}t          j        |ddddf                             t           j                  t          j        |j        d         df          gd          }t           j                            ||                    t           j                  d          ^}}t          t           j        	                    |d                             }	|	d	k    r|	|z  nd
|z  }
t          j
        |j        d         dft           j                  }|dddf         |z  |dddf<   |dddf         |z  |dddf<   |dddf         |
z  |dddf<   t          ||d         |d         |d         ||          S )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ư>r6   r   r   r8   r9   r:   )r;   r<   )r   concatenater   r   onesshaper   lstsqr   r3   emptyrK   rT   )rU   r;   r<   rV   lmks_xylmks_3daugr,   _scale_xz_scale
normalizeds               r-   $transformation_matrix_from_detectionrh   [   s   
 !0)N2KWG
.'!!!RaR%.//
;;RWgmTUFVXYEZ=[=[\cd
e
e
eCIOOC
!;!;4OHHEABINN1Q4(())G'.~~g##3;LG7=+Q/rzBBBJqqq!t}{2Jqqq!tqqq!t}|3Jqqq!tqqq!t}w.Jqqq!t-N#78+,n=Q.Rl   r/   )r5   r6   )__doc__rE   numpyr   rJ   r.   r   r4   intrT   dictrh    r/   r-   <module>rn      s|   
     BJ RZ RTR\ acak    <irz iBJ i iX] i i i i #'+n +n*+n
+n 
+n 
	+n
 +n +n  +n +n Z+n +n +n +n\D s Z] os xz  yC      r/   