U
    Nf&Q                     @   s   d dl m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 d dlZd dlmZmZ d dlZd dlmZmZmZmZmZ d!d	d
Zd"ddZd#ddZd$ddZdd Zdd Zdd Zdd Z dd Z!dd Z"dd  Z#dS )%    )cameraN)Path)deepcopy)FuncAnimationwriters)drawSquare2DdrawCross2D
drawCircledrawDiagonalCross2Ddraw3Dcoordinatesystemffffff?Fc              	      s  t d  fddtdd|jD }dd |D }| }|j}	|	g}
|
r|
 }|j| }t	| t
| |df t
| |df f}|d k	r| |df nd }|d ks||krtj||d|| d	tjd
 |rt|||tjdd |j| D ]}||jks|j| dk rq|
| |j| }t
| |df t
| |df f}|rX| |df nd }|d ksv||kr||krtj||||| dtjd qqJt|d| ||d}|S )Nrainbowc                    s   g | ]} |qS  r   ).0icmapr   4/home/daniela/catkin_ws/src/hpe/scripts/utils/vis.py
<listcomp>   s     z$vis_2d_keypoints.<locals>.<listcomp>r      c                 S   s.   g | ]&}|d  d |d d |d d fqS )      r   r   r   r   cr   r   r   r      s     r      )radiuscolor	thicknesslineType      ?r   r   r   )r   r   r   g      ?)pltget_cmapnplinspaceZkeypoint_numcopyrootpopkeypoint2indexprintintcv2circleLINE_AAputTextFONT_HERSHEY_SIMPLEXchildrenappendlineaddWeighted)	keypointsimgskeletonZ	kp_threshalphaoutput_fileZ	show_namecolorsmaskr'   stackparentp_idxp_posZp_scorechildc_idxc_posZc_scoreZ
vis_resultr   r   r   vis_2d_keypoints   sZ    

     

 
    
rC      c              	   C   s  t | d d df t | d d df  }}t | d d df t | d d df  }}t | d d df t | d d df  }}	t|| || ||	 d }
t }|jddd}|j||d ||
 |
g ||
 |
g |	dd|
 g |j
}|g}|r| }|j| }| | }|j| D ]}|j|ddkrNq2|| |j| }| | }||jkr|d	}n||jkrd
}nd}|j|d |d g|d |d g|d |d g|ddd}q2qd S )Nr   r   r   o   3d
projectionelevazimr   brk.zxsyszsr   markerzdir)r$   maxminr"   figureadd_subplot	view_init
set_xlim3d
set_ylim3d
set_zlim3dr'   r(   r)   r1   getr2   left_jointsright_jointsplot)r5   r7   azimuthrJ   x_maxx_miny_maxy_minz_maxz_minr   figaxr'   r<   r=   r>   r?   r@   rA   rB   r   r3   r   r   r   vis_3d_keypointsC   sH    ...


  rl      c                    s  | t d d d d df t d d d d df  }}t d d d d df t d d d d df  }}	t d d d d df t d d d d df  }
}t|| ||	 |
| d }t }|jddd  j||d  | |g  | |g  	dd| g dg  fd	d
}t
||jd d| d}|rt|}|j st|j |jdkrtd }||i dd}|j||d n&|jdkr|j|ddd ntd|S )Nr   r   r   rE   rF   rG   rI   Fc              	      s  sj }|g}|r| }j| }d|f }j| D ]}j|ddkrTq<|| j| }d|f }|jkrd}	n|jkrd}	nd}	 j|d |d g|d |d g|d |d g|	dd	d
}
|
 q<qdn
d}j }|g}|r| }j| }| |f }j| D ]}j|ddkrBq&|| j| }| |f }|jkrtd}	n|jkrd}	nd}	| d 	|d |d g | d 
|d |d g | d |d |d g |d7 }q&qd S )Nr   r   rL   rM   rN   r   r   rO   rP   rQ   T)r'   r(   r)   r1   r_   r2   r`   ra   rb   	set_xdata	set_ydataset_3d_properties)framer'   r<   r=   r>   r?   r@   rA   rB   r   r3   line_idxrk   initializedkps_sequencelinesr7   r   r   update   sd    




  


z)vis_3d_keypoints_sequence.<locals>.update  rj   funcframesinterval.mp4ffmpeg  fpsmetadatabitratewriter.gifP   imagemagickdpir   9Unsupported output format.Only mp4 and gif are supported.)r$   rW   rX   r"   rY   rZ   r[   r\   r]   r^   r   shaper   r=   existsosmakedirssuffixr   save
ValueError)Zkeypoints_sequencer7   rc   r   rJ   r9   rd   re   rf   rg   rh   ri   r   rj   rw   animWriterr   r   rs   r   vis_3d_keypoints_sequencem   s@    :::7   r   c                    s  D ]}t |d d d d df t |d d d d df  }}t |d d d d df t |d d d d df  }	}
t |d d d d df t |d d d d df  }}t|| |	|
 || d }qt }|jddd  j||d  | |g  | |g  	dd| g dg  fd	d
}dd t
jd< t|||jd d| d}|rt|}|j st|j |jdkrtd }||i dd}|j||d n&|jdkr|j|ddd ntd|S )Nr   r   r   rE   rF   rG   rI   Fc                    s^  dddg}dddg}sVd}D ] }j }|g}d}|r<| }j| }	|d|	f }
j| D ]}j|d	d	kr~qf|| j| }|d|f }|| }|dkr j|
d |d g|
d |d g|
d
 |d
 g|dd|| d}n: j|
d |d g|
d |d g|
d
 |d
 g|ddd}| d}qfq8|d }q"   dnd}d}D ]}j }|g}|rN| }j| }	|| |	f }
j| D ]}j|d	d	krq|| j| }|| |f }|| }| d |
d |d g | d 	|
d |d g | d 
|
d
 |d
 g |d7 }qqr|d }qbd S )NrL   grM   zcamera 2zcamera 3zcamera 4r   r   r   r   rO   rP   )rR   rS   rT   r   rU   rV   labelrQ   T)r'   r(   r)   r1   r_   r2   rb   legendrn   ro   rp   )rq   r:   labelsr   ru   r'   r<   firstr=   r>   r?   r@   rA   rB   r   r3   rr   rk   rt   keypoints_sequencesrv   r7   r   r   rw      s~    





     




z3vis_3d_keypoints_sequence_multi_cam.<locals>.update   zanimation.embed_limitrx   ry   r}   r~   r   r   r   r   r   r   r   r   )r$   rW   rX   r"   rY   rZ   r[   r\   r]   r^   
matplotlibrcParamsr   r   r   r=   r   r   r   r   r   r   r   )r   r7   rc   r   rJ   r9   ru   rd   re   rf   rg   rh   ri   r   rj   rw   r   r   r   r   r   r   #vis_3d_keypoints_sequence_multi_cam   sB    :::K   r   c           
      C   s   |  D ]\}}| d |d  }| d |d  }|d r|d sBqt|d t|d  }}t|d t|d  }}	t| d ||f||	fdd	 qd S )
Njointsr=   r@   validxy	image_gui)r   r   r   r   )itemsr+   r,   r3   )
rq   links	link_namelinkjoint0joint1x0y0x1y1r   r   r   draw_links_on_imageB  s    r   c           
   
   C   s   |  D ]\}}| d   D ]\}}t|d t|d  }}|d }|d sdt| d|  d}dd	|d
   }	t| d |||ftjdd t| d |||	|dd qqd S )Nr   r   r   r   r   z is not valid in camera )r   r   r      rD   
confidencer   r    r!   r   r   r   )r   r+   r*   r,   r/   FONT_HERSHEY_PLAINr   )
rq   cameras
camera_keyr   Z	joint_idxjointr   r   r   Zsquare_sizer   r   r   draw_2d_detectionsL  s    r   c                 C   sH  |  D ]8}||  D ]$\}}t|| | d d g|| | d d g|| | d d gdgg}t|d |}	|rtt|d d |d |d	 |	d d
d d f \}
}}nBtt|d t|d |d |d	 |	d d
d d f \}
}}|
d d }|
d d }t|t| }}t| d ||ddd
d qqd S )Nposer   r   rP   r   
extrinsics
intrinsicswidthheightr   
distortionr   r   
   r   r   r   r   )valuesr   r$   arraydotprojectToCamerar+   r
   )rq   ground_truthr   this_frame_keympir   	joint_keyr   Zpts_in_worldZpts_in_sensorZpts_in_image_Zxpix_projectedZypix_projectedr   r   r   r   r   draw_ground_truth_projectionY  s4      
r   c                 C   s"  i || < t  ||  d< ||  d jd|  dd ||  d jddd||  d< ||  d d	d
 ||  d d	d
 ||  d dd ||  d jddd ||  d jddd ||  d j	ddd t j
||  d  dd t j
||  d  dd t j
||  d  dd |S )NrY   Frame #   fontsizerE   rF   rG   axesrx   r     r      r   rP   Fvisible)r"   rY   suptitlerZ   r\   r]   r^   
set_xlabel
set_ylabel
set_zlabelsetpget_xticklabelsget_yticklabelsget_zticklabels)initial_frame_keyplot_handlesr   r   r   setup_3d_visualizationr  s    r   c                 C   s  | | d    | | d jd| dd | | d jddd| | d< | | d d	d
 | | d d	d
 | | d dd | | d jddd | | d jddd | | d jddd t	j
| | d  dd t	j
| | d  dd t	j
| | d  dd g g g g f\}}	}
}|d  D ]l\}}|| | }||d  |	|d  |
|d  tdd |d D \}}}||||f q6|d r||  D ]Z\}}|| | d }|t|d  |	t|d  |
t|d  |d q| | d j||	|
|d| | d< t| | d g ddddd | | d!< |d r,i | | d"< | D ]\}}i | | d" |< || |d#  d }|| |d$  d }|d |d |d   }}}|d |d |d   }}}| | d j||g||g||gdd| | d" | d%< qz| D ]\}}i | | |< || |d#  }|| |d$  }|d |d |d   }}}|d |d |d   }}}| | d j||g||g||gd&d| | | d'< q4t	  t	d( d S ))Nr   rY   r   r   r   rE   rF   rG   r   rx   r   r   r   r   r   rP   Fr   r   XYZc                 s   s   | ]}|d  V  qdS )r   Nr   r   r   r   r   	<genexpr>  s     z*update_3d_visualization.<locals>.<genexpr>r   has_ground_truthr   r   r   Zjoint_handleg?)xcyczcsizeZcoordinate_systemr   r=   r@   Zlink_handle_gtr    r    r    Zlink_handleg-C6?)clearr   rZ   r\   r]   r^   r   r   r   r"   r   r   r   r   r   r2   tuplefloatscatterr   rb   drawpause)r   r   poses3dr   r   rq   r   argsX_vecY_vecZ_vecjoint_colorsr   r   pointrL   r   rM   r   r   r   r   X0Y0Z0X1Y1Z1r   r   r   update_3d_visualization  s^    
"$
:6r   c           
      C   s   dd ||   D }dd ||   D }dd ||   D }dd ||   D }	|r|dd ||   D 7 }|dd ||   D 7 }|dd ||   D 7 }|	d	gt||  7 }	| j||||	d
 d S )Nc                 S   s   g | ]}|d  qS )r   r   r   r   r   r   r   r     s     z"draw_3d_points.<locals>.<listcomp>c                 S   s   g | ]}|d  qS )r   r   r   r   r   r   r     s     c                 S   s   g | ]}|d  qS )r   r   r   r   r   r   r     s     c                 S   s:   g | ]2}|d  d d |d  d d |d  d d fqS )r   r   r   r   r   r   r   r   r   r   r     s     c                 S   s   g | ]}|d  d qS )r   r   r   r   r   r   r   r     s     c                 S   s   g | ]}|d  d qS )r   r   r   r   r   r   r   r     s     c                 S   s   g | ]}|d  d qS )r   rP   r   r   r   r   r   r     s     r   r   )r   lenr   )
rk   r   r   r   r   r   r   r   r   r   r   r   r   draw_3d_points  s    r  c                 C   s&  |  D ]\}}|| |d  || |d   }}	|d |d |d   }
}}|	d |	d |	d   }}}| j|
|g||g||gdd q|r"|  D ]\}}|| |d  d || |d  d  }}	|d	 |d
 |d   }
}}|	d	 |	d
 |	d   }}}| j|
|g||g||gdd qd S )Nr=   r@   r   r   r   r   r   r   r   r   rP   r   )r   rb   )rk   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   draw_3d_links  s    " *r  )r   NF)rD   )rm   rD   N)rm   rD   N)$utilsr   r,   numpyr$   r   pathlibr   r&   r   matplotlib.pyplotpyplotr"   Zmpl_toolkits.mplot3d.axes3dmpl_toolkitsmatplotlib.animationr   r   r   
utils.drawr   r   r	   r
   r   rC   rl   r   r   r   r   r   r   r   r  r  r   r   r   r   <module>   s6   
0
,     
b     
t
8