U
    y-d                     @   s0   d dl Z d dlZd dlmZ dd Zdd ZdS )    N)projectToCamerac                 C   s*   t ||  d || d  || d  S )N   )mathsqrt)xiyizixfyfzf r   C/home/daniela/catkin_ws/src/hpe/scripts/utils/objective_function.pypointsDistance   s    r   c           !      C   s  | d }| d }| d }i }ddl m} | D ]t\}}|d  D ]\\}}	|	d  D ]D\}
}|d spq\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 }|d }d| d | d |
 }|d t	|| d || d   ||< q\qFq0|d st
t| d d t| dd  D ]\}}t|t| dksq|| }|| }| D ]|}
||
 }||
 }d| d | d |
 }|d |d |d   }}}|d |d |d   }}} t|||||| ||< qq|S ) Nargscamerasposes3dr   )TicTocframesjointsvalid)      )dtypeXYr   Zr      
extrinsics
intrinsics
distortionwidthheightx_projy_projxyprojection__
confidenceskip_frame_to_frame_residualsframe_)Zpytictocr   itemsnpndarrayfloatdotr   r   r   ziplistkeysintr   )!datar   r   r   	residualsr   
camera_keycamera	frame_keyframe	joint_keyjointZpts_in_worldZpts_in_sensorZpts_in_imager'   Zxpix_projectedZ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   r   r   r   objectiveFunction   sX     


2rA   )r   numpyr-   Zutils.projectionr   r   rA   r   r   r   r   <module>   s   