o
    i                     @   s   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lm	Z	 d dl
mZmZ d dlmZ dd Zdd	 ZG d
d dZG dd dejZG dd dejZdd ZG dd dejZG dd dejZG dd deZdefddZdS )    N)tqdm)override)ComfyExtensionio)LotusConditioningc           
      C   s   t j| t jd}t j|t jd}t|dkro|d |d  d }|d dkr5|d dkr5t|d |d nd}t j|d|dd}t |d|}t g d	}t g d
}| | }}	|| ||< || |	|< ||	}}||fS )a<  Insert neck keypoint and remap from MMPose to OpenPose ordering.

    Returns (kp, sc) where kp has shape (134, 2) and sc has shape (134,).
    Layout:
      0-17   body  (18 kp, OpenPose order)
      18-23  feet  (6 kp)
      24-91  face  (68 kp)
      92-112 right hand (21 kp)
      113-133 left hand (21 kp)
    dtype            333333?r   axis)r	   r      
      	                  r            )r   r   r   r   r   r   r   r   r   r   r   r   r   r   r	   )nparrayfloat32lenmininsertcopy)
kp_rawsc_rawkpscneck
neck_score
mmpose_idxopenpose_idxtmp_kptmp_sc r-   7/mnt/c/Users/fbmor/ComfyUI/comfy_extras/nodes_sdpose.py_preprocess_keypoints   s   .
r/   c                 C   s  dd }g }t t| D ]~}g }t| | || D ]g\}}	t||	\}
}tj|
dd |
ddg gdd}tj|dd |ddg gdd}|||
dd	 |dd	 ||
d	d |d	d |||||
dd
 |dd
 ||
d
d |d
d d q||||d q|S )a  Convert raw keypoint lists to a list of OpenPose-style frame dicts.

    Each frame dict contains:
      canvas_width, canvas_height, people: list of person dicts with keys:
        pose_keypoints_2d       - 18 body kp  as flat [x,y,score,...] (absolute pixels)
        foot_keypoints_2d       -  6 foot kp  as flat [x,y,score,...] (absolute pixels)
        face_keypoints_2d       - 70 face kp  as flat [x,y,score,...] (absolute pixels)
                                   indices 0-67: 68 face landmarks
                                   index  68:    right eye (body[14])
                                   index  69:    left  eye (body[15])
        hand_right_keypoints_2d - 21 right-hand kp (absolute pixels)
        hand_left_keypoints_2d  - 21 left-hand  kp (absolute pixels)
    c                 S   s4   t j| d d df | d d df |gdd  S )Nr   r   r   )r   stackflattentolist)kp_slicesc_slicer-   r-   r.   _flatten5   s   4z%_to_openpose_frames.<locals>._flatten   \   r   r   r   r      q      )pose_keypoints_2dfoot_keypoints_2dface_keypoints_2dhand_right_keypoints_2dhand_left_keypoints_2d)canvas_widthcanvas_heightpeople)ranger   zipr/   r   concatenateappend)all_keypoints
all_scoresheightwidthr5   framesimg_idxrB   r#   r$   r%   r&   face_kpface_scr-   r-   r.   _to_openpose_frames'   s"   ""
rO   c                   @   sZ   e Zd ZdZdd Zedd ZedddZed	d
 ZedddZ			dddZ
dS )KeypointDrawzP
    Pose keypoint drawing class that supports both numpy and cv2 backends.
    c                 C   s  z	dd l }|| _W n ty   | | _Y nw ddgddgddgddgddgddgddgdd	gdd
gd
dgddgddgddgddgddgddgddgddgddgddgg| _ddgddgddgddgddgdd	gdd
gd
dgddgddgddgddgddgddgddgddgddgg| _g dg dg dg dg dg dg dg dg dg dg d g d!g d"g d#g d$g d%g d&g d'g| _d S )(Nr   r   r   r   r   r
   r   r   r   r   r      r   r   r   r   r   r	   r8         )   r   r   )rT   U   r   )rT      r   )rT   rT   r   )rV   rT   r   )rU   rT   r   )r   rT   r   )r   rT   rU   )r   rT   rV   )r   rT   rT   )r   rV   rT   )r   rU   rT   r   r   rT   )rU   r   rT   )rV   r   rT   )rT   r   rT   )rT   r   rV   )rT   r   rU   )cv2drawImportError
hand_edgesbody_limbSeqcolors)selfrX   r-   r-   r.   __init__O   s*   

0
*
zKeypointDraw.__init__c                 K   s   |\}}| j dd \}}tt|}	td||	 t|||	 d }
}td||	 t|||	 d }}||
ks>||kr@dS tj|
|||f \}}|| d || d  |d k}|| |
|||f |< dS )z7Draw a filled circle using NumPy vectorized operations.Nr   r   r   )shapeintr   ceilmaxr    ogrid)	canvas_npcenterradiuscolorkwargscxcyhw
radius_inty_miny_maxx_minx_maxyxmaskr-   r-   r.   circlen   s   "" zKeypointDraw.circler   c           !      K   sT  g ||R \}}}}	| j dd \}
}t|| t|	| }}||k r'dnd||	k r.dnd}}|| ||g f\}}}}	 |||f ||krO||	krOn d| }|| kra|| || }}||k rn|| || }}q?|dkr|d d tt|d d }}|D ]U\}}td|| t|
|| d td|| t||| d f\}}}}||kr||krtj||||f \}}|| ||||f || d || d  |d k< qdS t	|}|dddf dk|dddf |
k @ |dddf dk@ |dddf |k @ }||  } j
r(|| | dddf | dddf f< dS dS )	z<Draw line using Bresenham's algorithm with NumPy operations.Nr   r   Tg       @      ?r   )r`   absrF   ra   r   rb   rc   r    rd   r   size)!re   pt1pt2rh   	thicknessri   x0y0x1y1rl   rm   dxdysxsyerrrt   rs   line_pointse2rg   rn   pxpyro   rp   rq   rr   yyxxvalidvalid_pointsr-   r-   r.   line   s:   "

$D4
P(zKeypointDraw.linec              	   K   s  t |dk rdS tj|tjd}| jdd \}}td|dddf  t||dddf  d td|dddf  t||dddf  d f\}}}}	||ks[|	|kr]dS tj||||	f \}
}tj|| |	| ft	d}t
t |D ]X}|| ||d t |  }}|d |d }}||krq}||kr|||d |d f\}}}}|
|k|
|k @  } sq}||||d |
| |d |d   ||   k@ N }q}|| ||||	f |< dS )z1Fill polygon using vectorized scanline algorithm.r   Nr   r   r   r   )r   r   r   int32r`   rc   r    mgridzerosboolrC   any)re   ptsrh   ri   rl   rm   ro   rp   rq   rr   r   r   ru   ip1p2r   y2	edge_maskr-   r-   r.   fillConvexPoly   s(   t6zKeypointDraw.fillConvexPolyc              	   K   s  |d d |d d f}|d }||kr||}}|dk r*|d |d }}|dk s|dkr;|d |d }}|dks.|| dkrEd\}}t |}t |t |}}	g }
t||| |D ]@}t t||}|d t | |d t | }}|
tt| d ||  ||	  tt| d ||	  ||  g q_g t	dt	df}}|
D ]}t
| }|kr|| |}qt|dkr|S | d | d g| d | d ggS )z*Python implementation of cv2.ellipse2Poly.r   rx   r   h  )r   r   inf)mathradianscossinrC   r    rF   ra   roundfloattupler   )rf   axesangle	arc_startarc_enddeltari   	angle_radalphabetar   r   	theta_radrt   rs   
unique_ptsprev_ptptpt_tupler-   r-   r.   ellipse2Poly   s4   

&H
0zKeypointDraw.ellipse2PolyNr   Tr   r   c           &   	   C   s  |j \}}}|rt|dkrt| jD ]\}}|d d |d d }}|dks,|dkr-q|dur>|| |k s=|| |k r>q|| d || d g}|| d || d g}|d |d  d |d |d  d }}t|d |d  d |d |d  d  }|dk rqtt|d |d  |d |d  }| j	t
|t
|ft
|d |	ft
|ddd}| j||| j|t| j   q|r#t|dkr#tdD ]N}|dur|| |k rqt
|| d t
|| d }}d|  kr|k r"n qd|  kr|k r"n q| jj|||fd| j|t| j  dd	 q|rt|d
krtdd
D ]U}|durB|| |k rBq2t
|| d t
|| d }}d|  kr_|k rn q2d|  krn|k rn q2| jj|||fd| j|t| j  dd	 q2|rt|dkrd}t| jD ]\}}d|d  d|d  }}|dur|| |k s|| |k rqt
|| d t
|| d }}t
|| d t
|| d } }!||krb||krb| |krb|!|krbd|  kr|k rbn qd|  kr|k rbn qd|   kr|k rbn qd|!  kr.|k rbn qt|tt| j dd\}"}#}$t
|"d t
|#d t
|$d f}%| jj|||f| |!f|%dd	 qtddD ]W}|dury|| |k ryqit
|| d t
|| d }}||kr||krd|  kr|k rn qid|  kr|k rn qi| jj|||fdddd	 qi|rt|dkrd}t| jD ]\}}d|d  d|d  }}|dur|| |k s|| |k rqt
|| d t
|| d }}t
|| d t
|| d } }!||kr||kr| |kr|!|krd|  kr:|k rn qd|  krI|k rn qd|   krX|k rn qd|!  krg|k rn qt|tt| j dd\}"}#}$t
|"d t
|#d t
|$d f}%| jj|||f| |!f|%dd	 qtddD ]^}|dur|t|k r|| |k rqt
|| d t
|| d }}||kr||krd|  kr|k rn qd|  kr|k rn q| jj|||fdddd	 q|rjt|dkrjd}td
dD ]W}|dur"|| |k r"qt
|| d t
|| d }}||krh||krhd|  krI|k rhn qd|  krX|k rhn q| jj|||f|
ddd	 q|S )a  
        Draw wholebody keypoints (134 keypoints after processing) in DWPose style.

        Expected keypoint format (after neck insertion and remapping):
        - Body: 0-17 (18 keypoints in OpenPose format, neck at index 1)
        - Foot: 18-23 (6 keypoints)
        - Face: 24-91 (68 landmarks)
        - Right hand: 92-112 (21 keypoints)
        - Left hand: 113-133 (21 keypoints)

        Args:
            canvas: The canvas to draw on (numpy array)
            keypoints: Array of keypoint coordinates
            scores: Optional confidence scores for each keypoint
            threshold: Minimum confidence threshold for drawing keypoints

        Returns:
            canvas: The canvas with keypoints drawn
        r8   r   r   Nr   r   r   rw   )r}   r6   r9   {Gz?r7         ?rT   rW   r:   )rT   rT   rT   )r`   r   	enumerater\   r   sqrtdegreesatan2rY   r   ra   r   r]   rC   rv   r[   colorsys
hsv_to_rgbr   r   )&r^   canvas	keypointsscores	threshold	draw_body	draw_feet	draw_face
draw_handsstick_widthface_point_sizeHWCr   limbidx1idx2YXmXmYlengthr   polygonrt   rs   epsieedger   r   x2r   rgbrh   r-   r-   r.   draw_wholebody_keypoints   s   *.*2""6*"<*
""(x"""P
""(x""&"P"Pz%KeypointDraw.draw_wholebody_keypoints)r   )Nr   TTTTr   r   )__name__
__module____qualname____doc__r_   staticmethodrv   r   r   r   r   r-   r-   r-   r.   rP   K   s    

rP   c                   @   ,   e Zd Zedd ZedejfddZdS )SDPoseDrawKeypointsc                 C   s   t jddg dt ddt jjdddt jjd	ddt jjd
ddt jjdddt jjddddddt jjddddddt jjddddddgt j gdS )Nr   image/preprocessors)openposepose detectionpreprocessorr   posePOSE_KEYPOINTr   r   T)defaultr   r   r   Fr   r   r   r   r   r    rc   stepr   r   score_thresholdr   g        r   r   node_idcategorysearch_aliasesinputsoutputs)	r   SchemaCustomInputBooleanIntFloatImageOutputclsr-   r-   r.   define_schema_  s    z!SDPoseDrawKeypoints.define_schemareturnc	           !      C   s  |st tjdtjdS |d d }	|d d }
dd }dd	 }g }t }t|d
dD ]}tj|	|
dftjd}|d D ]l}||d d\}}|	d}|rT||dn|d\}}||d d\}}|d d |d d }}||d d\}}||d d\}}tj
|||||gdd}tj
|||||gdd}|j||||||||||d
}q=|| q,t|dkrt|nt|d d}t| d } t | S )N)r   @   r   r   r   r   rA   r@   c                 S   s<   t j| t jd|d}|d d d df |d d df fS )Nr   r   r   )r   r   r   reshape)flatnarrr-   r-   r.   _parse{  s   $z+SDPoseDrawKeypoints.execute.<locals>._parsec                 S   s$   t j| dft jdt j| t jdfS )Nr   r   )r   r   r   )r   r-   r-   r.   _zeros  s   $z+SDPoseDrawKeypoints.execute.<locals>._zeroszDrawing keypoints on framesdescr   rB   r;   r8   r<   r   r=   F   D   r>      r?   r   )r   r   r   r   r   r   r   r   g     o@)r   
NodeOutputtorchr   r   rP   r   r   uint8getrE   r   rF   r   r0   expand_dims
from_numpyr   )!r   r   r   r   r   r   r   r   r   rI   rJ   r  r  pose_outputsdrawerframer   personbody_kpbody_scfoot_rawfoot_kpfoot_scrM   rN   rhand_kprhand_sclhand_kplhand_scr%   r&   pose_outputs_npfinal_pose_outputr-   r-   r.   executet  s>   
&
zSDPoseDrawKeypoints.executeNr   r   r   classmethodr   r   r  r  r-   r-   r-   r.   r   ^  s
    
r   c                   @   s.   e Zd Zedd ZeddejfddZdS )SDPoseKeypointExtractorc                 C   sp   t jddg ddt jdt jdt jdt jjdd	d
dd
dt jjdddddgt dj	dddgdS )Nr   r   )r   r   r   r   sdposezxExtract pose keypoints from images using the SDPose model: https://huggingface.co/Comfy-Org/SDPose/tree/main/checkpointsmodelvaeimage
batch_sizer   r   i'  r   bboxesTzZOptional bounding boxes for more accurate detections. Required for multi-person detection.)optionalforce_inputtooltipr   r   zHKeypoints in OpenPose frame format (canvas_width, canvas_height, people)r)  r   r   r   descriptionr   r   )
r   r   Modelr   Vaer   r   BoundingBoxr   r   r   r-   r-   r.   r     s   


z%SDPoseKeypointExtractor.define_schemaNr   c           1   
      sV  |j d |j d }}t  jd  fdd}| dd|giijd< t|jjd	s2t	d
|jjj
|j d }	d  tjd d }
tjd d } fdd}g }g }tj|	}|d urt|tsr|gg}nt|dkr}d g|	 }tt|	ddD ]N}|||d  }|r|t|t|d  nd }g }g }|r|D ]}tdt|d }tdt|d }t|t|d |d  }t|t|d |d  }||ks||krq|| || }}|d d ||||d d f }t|
| || }tt|| tt|| }}|
| d || d } }!|dddd }"tjj|"||ddd}#tjd|#j d |
||#j|#jd}$|#|$d d d d | | | |!|!| f< |$dddd}%||%}&||&\}'}(|'d |(d })}*t|)t j!r|)" nt j#|)t j$d})|)d |! | | |)d< |)d |  | | |)d< |%|) |%|* qn||}+||+\}'}(|%|'d  |%|(d  |%| |%| |&d qnIt|	dd},td|	|D ]<}-t|-| |	}.|||-|. }/||/\}'}(t'|'|(D ]\})}*|%|)g |%|*g |,&d q|&|.|-  qt(||||}0t)*|0S ) Nr   c                    s   | j d dkr|   | |fS )Nr   i  )r`   clone)rl   hsptransformer_options)captured_featr-   r.   output_patch  s   z5SDPoseKeypointExtractor.execute.<locals>.output_patchpatchesoutput_block_patchr4  heatmap_headzThe provided model does not have a heatmap_head. Please use SDPose model from here https://huggingface.co/Comfy-Org/SDPose/tree/main/checkpoints.r   r   c                    s4   d t jjt| dddd| ddd} S )zLRun one forward pass and return (keypoints_list, scores_list) for the batch.Nr   r   eulersimpleT)
noisestepscfgsampler_name	schedulerpositivenegativelatent_imagedisable_noisedisable_pbar)comfysampler	  
zeros_like)latent_batch_r5  contextheadmodel_cloner-   r.   _run_on_latent  s   z7SDPoseKeypointExtractor.execute.<locals>._run_on_latentzExtracting keypoints from cropsr  rt   rs   rJ   rI   r   r   bilineardisabledupscale_methodcropr   devicer   ).r   ).r   zExtracting keypoints)totalr  )+r`   r   r  resultr2  model_optionshasattrr"  diffusion_model
ValueErrorr9  ra   heatmap_sizerF  utilsProgressBar
isinstancelistr   r   rC   r    rc   r   permuter   common_upscaler	  r   r   rV  encoder   ndarrayr"   r   r   rF   updaterD   rO   r   r  )1r   r"  r#  r$  r%  r&  rI   rJ   r6  total_imagesmodel_hmodel_wrO  rG   rH   pbarrL   img
img_bboxesimg_keypoints
img_scoresbboxr   r   r   r   	crop_h_px	crop_w_pxrT  scalescaled_hscaled_wpad_toppad_leftcrop_chwscaledpaddedcrop_resizedlatent_cropkp_batchsc_batchr%   r&   
latent_img	tqdm_pbarbatch_start	batch_endrI  openpose_framesr-   rK  r.   r    s   






 " (
&

$

7
zSDPoseKeypointExtractor.executeNr  r-   r-   r-   r.   r     s
    
r   c                 C   s  |\}}|   dd  ||f }tj|dd\}}tj|dd\}}	|| }
|	| }|
dks2|dkr6g dS |
| }|| }t||
|  }t|||
  }||
 d }|| d }t|| d}t|| |}t|d|  d}t|	| |}t|t|t|t|gS )Nr   r   r   )r   r   r   r   r   r   r   )r"   r   r    rc   r   ra   )kp2dsrr  image_shaperl   rm   
kp2ds_facemin_xmin_ymax_xmax_yinitial_widthinitial_heightinitial_areaexpanded_area	new_width
new_heightdelta_widthdelta_heightexpanded_min_xexpanded_max_xexpanded_min_yexpanded_max_yr-   r-   r.   get_face_bboxes2  s&   r  c                   @   r   )SDPoseFaceBBoxesc                 C   sZ   t jddg dt ddt jjdddd	d
ddt jjddddgt jjdddgdS )Nr  r   )z	face bboxzface bounding boxr   r   r   r   rr  g      ?r   g      $@皙?z?Multiplier for the bounding box area around each detected face.r   r    rc   r   r)  force_squareTzAExpand the shorter bbox axis so the crop region is always square.)r   r)  r&  zTFace bounding boxes per frame, compatible with SDPoseKeypointExtractor bboxes input.r*  r   )r   r   r   r   r   r   r/  r   r   r-   r-   r.   r   R  s   zSDPoseFaceBBoxes.define_schemar   c              	   C   s  g }|D ]}|d }|d }g }|d D ]}	|	 dg }
|
sqtj|
tjddd}|d d d df }|tj||gtjd }ttjd	tjd|g}t||||f\}}}}||kr||kr|r|| || }}||krt||}|| d || d }}|d }td
|| }td
|| }t	||| }t	||| }td
|| }td
|| }|
|||| || d q|
| qt|S )NrA   r@   rB   r=   r   rw   r   r   )r   r   r   )rt   rs   rJ   rI   )r  r   r   r   r   vstackr   r  rc   r    rF   r   r  )r   r   rr  r  
all_bboxesr  rl   rm   frame_bboxesr  	face_flatface_arrface_xykp_norm	kp_paddedr   r   r   r   bwbhsiderj   rk   halfr-   r-   r.   r  b  s>   

zSDPoseFaceBBoxes.executeNr  r-   r-   r-   r.   r  P  s
    
r  c                   @   s.   e Zd Zedd ZeddejfddZdS )	CropByBBoxesc                 C   s   t jddg ddt jdt jjdddt jjd	d
dddddt jjdd
dddddt jjdddddddt jjdddgdddgt jjddgdS )Nr  r   )rT  z	face cropz	bbox cropr   zbounding boxzTCrop and resize regions from the input image batch based on provided bounding boxes.r$  r&  T)r(  output_widthi   r   i   r   zWidth each crop is resized to.r  output_heightzHeight each crop is resized to.paddingr   i   r   zGExtra padding in pixels added on each side of the bbox before cropping.keep_aspectstretchpadzfWhether to stretch the crop to fit the output size, or pad with black pixels to preserve aspect ratio.)optionsr   r)  z,All crops stacked into a single image batch.r*  r+  )r   r   r   r   r/  r   Combor   r   r-   r-   r.   r     s   
	zCropByBBoxes.define_schemar  r   c           #      C   s*  |j d }|j d }|j d }	|j d }
t|ts|gg}nt|dkr)t|S g }t|D ]J}|t|t|d  }|s@q/|| ddd	d}tdd |D }tdd |D }t
dd |D }t
d	d |D }|dkrt
d|| }t
d|| }t|	|| }t||| }t
d|t|	|}}t
d|t||}}||ks||krtt||	d
 }t
d|	| d }t
dt|d }t|	|| }t||| }||ks||kr|tjd|
|||j|jd q/||||f\}}}}|d d d d ||||f }|dkrj|| || }}t|| || }tt|| }tt|| }tjj|||ddd}|| d }|| d } tjd|
|||j|jd}!||!d d d d | | | ||| f< ntjj|||ddd}!||! q/|st|S tj|dddddd}"t|"S )Nr   r   r   r   c                 s       | ]	}t |d  V  qdS )rt   Nra   .0r   r-   r-   r.   	<genexpr>      z'CropByBBoxes.execute.<locals>.<genexpr>c                 s   r  )rs   Nr  r  r-   r-   r.   r    r  c                 s   $    | ]}t |d  |d  V  qdS )rt   rJ   Nr  r  r-   r-   r.   r       " c                 s   r  )rs   rI   Nr  r  r-   r-   r.   r    r  r   r  rU  r  rP  rQ  rR  )dim)r`   r`  ra  r   r   r  rC   r    rb  	unsqueezerc   ra   rF   r	  r   r   rV  r   rF  r^  rc  cat)#r   r$  r&  r  r  r  r  total_framesimg_himg_wnum_chcrops	frame_idxr  	frame_chwr   r   r   r   fallback_sizefb_x1fb_y1fb_x2fb_y2rw  crop_hcrop_wrr  rt  rs  rx  rv  ru  resized
out_imagesr-   r-   r.   r    sh   






  
*

zCropByBBoxes.executeN)r  r  r-   r-   r-   r.   r    s
    
r  c                   @   s(   e Zd Zedeeej  fddZdS )SDPoseExtensionr   c                    s   t tttgS r  )r   r   r  r  )r^   r-   r-   r.   get_node_list  s   zSDPoseExtension.get_node_listN)	r   r   r   r   ra  typer   	ComfyNoder  r-   r-   r-   r.   r    s    r  r   c                      s   t  S r  )r  r-   r-   r-   r.   comfy_entrypoint  s   r  )r	  comfy.utilsrF  numpyr   r   r   r   typing_extensionsr   comfy_api.latestr   r   comfy_extras.nodes_lotusr   r/   rO   rP   r  r   r   r  r  r  r  r  r-   r-   r-   r.   <module>   s,    $  B :\
