U
    Ya}f-                     @   s   d dl Z d dlZd dlmZ d dlmZmZmZm	Z	m
Z
mZmZmZmZ d dlmZ d dlmZmZ d dlmZ d dlmZmZ dd	 Zd
d Zdd ZdS )    N)projectToCamera)	links_ground_truthlinks_openpose
joint_idxsjoints_mediapipelinks_mediapipejoint_idxs_op_mpilinks_op_mpilinks_mpi_inf_3dhpjoints_mpi_inf_3dhp)TicToc)jitcuda)PrettyTable)StyleForec                 C   s*   t ||  d || d  || d  S )N   )mathsqrt)xiyizixfyfzf r   J/home/daniela/catkin_ws/src/hpe/scripts/optimization/objective_function.pypointsDistance   s    r   c                 C   s   t t| t t|  S )N)floatsumlen)lstr   r   r   average   s    r"   c           O      C   s8  | d }| d }| d }| d }| d }i }d}d}|d dkrFd	}n|d dkrVd	}d}	|d
 dkrtd}	t }
t}n2|d
 dkrd}	t}
t}|d	krt}t}
nt}
t}g }|
 D ]$\}}|	|d  |	|d  qt
t|}t }|  | D ]|\}}|d  D ]d\}}|d  D ]J\}}tjdtd}|| | d |d d< || | d |d d< || | d |d d< d|d d< t|d |}t|d |d |d |d |ddd d f \}}}|d d }|d d }|d d |d< |d d |d < |d! r&|d" }|d# }d$| d% | d& | }|d' d t|| d || d   ||< q&qq|  |d( stt
| d d) t
| dd  D ]N\}}t|t| dksڐq|| } || }!|  D ]}| | }"|!| }#d*| d% | d& | }|"d |"d |"d   }$}%}&|#d |#d |#d   }'}(})d}*| D ]X\}}|d | }+|d | },|+d | }"|,d | }#|"d! r|#d! sdd	}* qqdt|$|%|&|'|(|)d+ks|*d	krt|$|%|&|'|(|)||< nd||< qq|  | d, d- }-|d. s|d/ rLd0d1d2d3d4d5d6d7g}.t|.}/|
 D ]\}0}g }1g }2| D ]\}}|| |d  d8 }3|3d" |3d# |3d9   }4}5}6|| |d  d8 }7|7d" |7d# |7d9   }8}9}:|2	t|4|5|6|8|9|: ||d  };|;d |;d |;d   }4}5}6||d  }<|<d |<d |<d   }8}9}:t|4|5|6|8|9|:}=|1	|= qnt|1}>t|2}2g }?d}@d}A| D ]\}}||d  };|;d |;d |;d   }4}5}6||d  }<|<d |<d |<d   }8}9}:t|4|5|6|8|9|:}B|?	|B |@t|B|> 7 }@|Ad7 }Aq|@|A }C|d/ rt|-g}D|D	tjtj  d: tj!  |D	|d  |D	|d  |D	d;|2  |D	d;t|?  |D	d;t|2t|?   |D	d;t|C  |/"|D d<|d  d& |d  }|C||< qT|d/ r
| d, d= r
|d. s
t#|/ |d/ r4| d, d= r4| d, d- }-d>g}.|D ]}|.	| q8t|.}/i ||-< | D ]\}}i ||- |< |g}E|D ]}|| }Fi ||- | |< |Fd }G|Fd }H|Fd }I|| | d8 }J|Jd" }K|Jd# }L|Jd9 }Mtt$|G|H|Ig|K|L|Mg}N|N||- | | d?< |E	d;|N  q|/"|E qbt#|/ |S )@Nargscamerasposes3dgroundtrutherrorsFdataset_namempiT2d_detector	mediapipeopenposeparentchildframesjoints)      )dtypeXr   Yr2   Zr      
extrinsics
intrinsics
distortionwidthheightx_projy_projvalidxyprojection_sensor__frame__joint_
confidenceskip_frame_to_frame_residualsconsecutive_frame_   statusnum_iterationsskip_link_length_residualsdebugzIteration #FrameZParentZChildZGroundtruthZMeasuredErrorzStd DeviationposezzAVERAGE z%.1flink_length_joint_is_iterationzFrame #rmse)%r   r   r   r   r   r	   r
   r   itemsappendlistsetr   ticnpndarrayr   dotr   r   r   zipkeysintr   r   r"   absstrr   BLUEr   BRIGHT	RESET_ALLZadd_rowprintdist)Odatar#   r$   r%   r&   e	residualsr)   human36mdetectorlinksskeleton_jointsjoints_to_use_linkt
camera_keycamera	frame_keyframe	joint_keyjointpts_in_worldpts_in_sensorpts_in_imagexpix_projectedypix_projectedZxpix_detectedZypix_detectedresidual_keyinitial_frame_keyfinal_frame_keyinitial_poseZ
final_poseZinitial_jointZfinal_jointr   r   r   r   r   r   Zinvalid_jointZinitial_frameZfinal_frame	iterationZtable_headertablelink_keyZlink_lengthsZgroundtruth_lengthsZ	parent_gtZXpZYpZZpZchild_gtZXcZYcZZcZparent_poseZ
child_poselengthZlink_length_averageZlink_lengths_measuredZstdevn_framesZlink_lengthZlink_length_residualrowZavg_rowZjoint_detectedZx_detZy_detZz_detZjoint_gtZx_gtZy_gtZz_gtrT   r   r   r   objectiveFunction   s<    


4
"





"r   )r   numpyrZ   utils.projectionr   utils.linksr   r   r   r   r   r   r	   r
   r   pytictocr   numbar   r   Zprettytabler   coloramar   r   r   r"   r   r   r   r   r   <module>   s   ,