U
    NKLh                      @   s  d dl mZ d dlZedZdd ed ddD Zdd eD Zdd	d
ed  dddded dddded dddded dddded dddded ddd d!ed" dd#d$d%ed& dd'd(d)ed* dd+d,d-ed. dd/d0d1ed2 dd3d4d5ed6 dd7d8d9ed: dd;d<d=ed> dd?d@dAedB ddCdDdEedF ddGdHdIedJ ddKdLdMedN ddOdPdQedR ddSdTdUedV ddWdXdYedZ dd[Z	d ed  d\ded d\ded d\ded d\ded d\ded d\d"ed" d\d&ed& d\d*ed* d\d.ed. d\d2ed2 d\d6ed6 d\d:ed: d\d>ed> d\dBedB d\d]ed] d\d^ed^ d\d_ed_ d\d`ed` d\dFedF d\dJedJ d\dNedN d\dRedR d\dVedV d\dZedZ d\daZ
d ed  d\ded d\ded d\ded d\ded d\ded d\d"ed" d\d&ed& d\dBed* d\d*ed. d\d.ed2 d\d2ed6 d\d6ed: d\d:ed> d\d>edB d\dbZd ed  d\d:ed d\dBed d\d^ed d\d6ed d\d>ed" d\d]ed& d\dZed. d\dced2 d\dded6 d\dVed: d\deed> d\dfedB d\dgedF d\dhedN d\diedR d\dedZ d\djZd ed  d\ded  d\ded  d\ded  d\ded  d\d"ed  d\d&ed  d\d*ed  d\d.ed  d\d6ed  d\d:ed  d\d>ed  d\dBed  d\d]ed  d\d_ed  d\d`ed  d\dFed  d\deed  d\dced  d\dfed  d\dkZded d\dVedV d\dZedZ d\deede d\dcedc d\d`ed` d\dFedF d\dJedJ d\dNedN d\ded d\d ed  d\ded d\ded d\d"ed" d\d&ed& d\d.ed d\d2ed2 d\d6ed] d\d:edJ d\dBedB d\d]ed] d\d^ed^ d\d_ed_ d\dlZdmdndodndpdodqdmdodrdsdodsdtdodqdrdodqdudodqdvdodvdwdodwdxdodxdydodvdzdodzd{dod{d|dodyd}dodyd~dod~ddod|ddod|ddodddodZdmdndodndpdodqdmdodrdsdodsdtdodqdrdodqdudodqdvdodvdwdodwdxdodxdydodvdzdodzd{dod{d|dodZdmdndodndpdodrdsdodsdtdodududodmdrdodwdzdodmdwdodzdrdodwdxdodxdydodzd{dod{d|dodyd}dodyd~dod~dydod|ddod|ddodd|dodZdddodddodddodqddodddodddodddodddodddodddodqddodddodddodddodqddodddodqddodddodddodddodddodddodddodddodddodddodddodddodddodddodZdmdndodndpdoddmdodrdsdodsdtdoddrdoddudodddoddvdodvdwdodwdxdodxddodvdzdodzd{dod{ddodddodddodZdddoddqdodqddodddodddodddodmdndodndpdodpddoddmdodrdsdodsdtdodtddoddrdoddwdodwdxdodxdydodyddoddzdodzd{dod{d|dod|ddodZddpdddndddmdddtdddsdddrdddwdddxdddyddd~dddzddd{ddd|dddddZdpdpddndnddmdmddtdtddsdsddrdrddwdwddxdxdddyddd~ddzdzdd{d{ddd|dddddvdvdddqddZdpdpddndnddmdmddtdtddsdsddrdrddwdwddxdxdddyddd~ddzdzdd{d{ddd|dddddududdZdpdpddndnddmdmddtdtddsdsddrdrddwdwddxdxdddyddzdzdd{d{ddd|ddZdddodddodddodddodddodddodddodddodddodddodddodddodddodddodƜZdddodddodddodddodddodddodddodddodddodddodddodddodddodddodddodɜZdddodddodddodddodddodddodddodddodddodddodddodddodddodddodƜZdS )    Nrainbowc                 C   s   g | ]}t |qS  )cmap).0ir   r   6/home/daniela/catkin_ws/src/hpe/scripts/utils/links.py
<listcomp>   s     r         c                 C   s.   g | ]&}|d  d |d d |d d fqS )      r	   r   r   )r   cr   r   r   r      s     gߟfogߠg<	P?)XYZcolorgb,gg"2gTU?g^?gяgU]?r   g-X?g$e gw?   geO>?gppgY?   gm2iǿggsg""?   gt˿g%$;Yg΍c!?   gY'Ϳg;EYcg~:j?   g?pgugΡvt?   grN?g]gYI:Y?	   g9]?g%hhg#pN?
   gfj ?g-Wg>j%?   g]wgg?qKgWB?   g4|Ag :3ghn<?   g|L_g{g@}?   g?gǷrxgVi?   gFng6@:g75%?   g Mg|gM(?   g3?g&,gg2?   gĻ}Q?g;q`g3t?   gᕌ*?gʒ"_g#!ă?   )NoseNeck	RShoulderRElbowRWrist	LShoulderLElbowLWristMidHipRHipRKneeRAnkleLHipLKneeLAnkleLBigToe	LSmallToeLHeelRBigToe	RSmallToeRHeel)idxr               )r$   r%   r&   r'   r(   r)   r*   r+   r,   r-   r.   r/   r0   r1   r2   ZREyeZLEyeZREarZLEarr3   r4   r5   r6   r7   r8   )r$   r%   r&   r'   r(   r)   r*   r+   r,   r-   r.   r/   r0   r1   r2                         )r$   r&   r'   r(   r)   r*   r+   r-   r.   r/   r0   r1   r2   r3   r5   r6   r8   )ZHipr-   r.   RFootRFootTipr0   r1   LFootLFootTipr,   SpineThoraxr$   Headr)   r*   r+   r&   r'   r(   )Pelvisr-   r.   r/   rE   r0   r1   r2   rG   Spine2Spine3Spine4r%   rK   HeadTopr)   r*   r+   LHandr&   r'   r(   RHandr&   r'   )parentchildr(   r%   r)   r*   r+   r$   r,   r-   r.   r/   r0   r1   r2   r8   r6   r7   r5   r3   r4   )	right_armright_forearmright_blade:left_armleft_forearmleft_blade:neckbody:	right_hip
right_tigh
right_leg:left_hip	left_tigh	left_leg:
right_heel
right_footright_foot_fingers:	left_heel	left_footleft_foot_fingers:)rU   rV   rW   rX   rY   rZ   r[   r\   r]   r^   r_   r`   ra   rb   )rU   rV   rX   rY   nosebody_up:
body_down:zbody_right:z
body_left:r^   r_   ra   rb   rc   rd   re   rf   rg   rh   ZRightShoulderZRightArmZRightForeArmZ	RightHandZRightFingerBaseZRThumbZRightHandIndex1ZLeftShoulderZLeftArmZLeftForeArmZLeftHandZLeftFingerBaseZLThumbZLeftHandIndex1ZNeck1rK   ZSpine1rI   Z	LowerBackZHipsZ	RHipJointZ
RightUpLegZRightLegZ	RightFootZRightToeBaseZ	LHipJointZ	LeftUpLegZLeftLegZLeftFootZLeftToeBase)rU   rV   right_wristrW   Z
right_handZright_thumbZright_idx_fingerrX   rY   
left_wristrZ   Z	left_handZ
left_thumbZleft_idx_fingerr[   headZbody1body2body3zhip:r]   r^   r_   rc   rd   r`   ra   rb   rf   rg   rJ   rE   rG   rF   rH   )rU   rV   rW   rX   rY   rZ   r[   rj   rk   r]   r^   r_   r`   ra   rb   rd   rg   rP   rO   rN   rM   rL   rR   rQ   )rn   r[   Zbody4rp   ro   bodyZr_armZ	r_forearmZr_handZr_bladeZl_armZ	l_forearmZl_handZl_bladeZr_hipZr_tighZr_legZr_footZl_hipZl_tighZl_legZl_foot)Zground_truthZbody25)rl   right_elbowright_shoulderrm   
left_elbowleft_shoulderr]   
right_kneeright_ankle	right_toer`   	left_knee
left_ankleleft_toe)rl   rr   rs   rm   rt   ru   r]   rv   rw   rx   r`   ry   rz   r{   mid_hipthorax)rl   rr   rs   rm   rt   ru   r]   rv   rw   rx   r`   ry   rz   r{   ri   )rl   rr   rs   rm   rt   ru   r]   rv   rw   r`   ry   rz   rs   rr   rl   ru   rt   rm   r]   rv   rw   r`   ry   rz   rx   r{   )rU   rV   rX   rY   r^   r_   ra   rb   rd   rg   Zmiddle_armsZmiddle_legsZarms_2_legs_rightZarms_2_legs_leftr}   r|   )rU   rV   rX   rY   r^   r_   ra   rb   rd   rg   Zright_bladeZ
left_blader`   r]   spine)matplotlib.pyplotpyplotpltnumpynpget_cmapr   linspacecolorsZintial_estimate_jointsZ
joint_idxsZjoint_idxs_op_mpijoints_mediapipeZjoints_human36mjoints_mpi_inf_3dhpZlinks_openposeZlinks_op_mpilinks_mediapipeZlinks_ground_truthZlinks_human36mlinks_mpi_inf_3dhpZjoint_correspondenceZjoint_correspondence_human36mZ'joint_correspondence_human36m_mediapipeZ"joint_correspondence_mpi_mediapipeZlinks_evaluationZlinks_evaluation_human36mZ#links_evaluation_human36m_mediapiper   r   r   r   <module>   s  
                     B/6 "C
