o
    \үi                     @   s   d dl Zd dlm  mZ 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d Zdd Zd%ddZdd Zd&ddZdd Zdd  Zd!d" Zd#d$ ZdS )'    Nc                 C   sZ   | j d d }tj|d | jd}| |dd ddf< | |dd dd df< d|d< |S )N   r   dtype.      .r   r   )shapenpzerosr   	as_matrix)posrotr
   mat r   A/data/cameron/vidgen/unified_video_action/umi/common/pose_util.pypos_rot_to_mat   s   r   c                 C   sF   | dd ddf j | d j  j }tj| dd dd df }||fS )N.r   r	   )TstRotationfrom_matrix)r   r   r   r   r   r   mat_to_pos_rot   s    r   c                 C   sJ   | j d d }tj|d | jd}| |dd df< | |ddd f< |S )Nr      r   .r   )r
   r   r   r   	as_rotvec)r   r   r
   poser   r   r   pos_rot_to_pose   s
   r   c                 C   s0   | dd df }t j| ddd f }||fS )N.r   )r   r   from_rotvec)r   r   r   r   r   r   pose_to_pos_rot   s   r   c                 C      t t|  S N)r   r   )r   r   r   r   pose_to_mat"      r"   c                 C   r    r!   )r   r   )r   r   r   r   mat_to_pose&   r#   r$   c                 C   s   t |}| | }t|}|S )zD
    tx: tx_new_old
    pose: tx_old_obj
    result: tx_new_obj
    )r"   r$   )txr   pose_matZtf_pose_matZtf_poser   r   r   transform_pose*   s   r'   c                 C   s*   || d dd df j  | d ddf  S Nr   r   )r%   pointr   r   r   transform_point6   s   *r+   c                 C   s,   || j  }|dd df |ddgf  }|S )N.   r)   )kr*   xuvr   r   r   project_point:   s   
r0   c                 C   sj   t | }| d d |d d  |d d< tj| dd  }tj|dd  }||  |dd < |S r(   )r   
zeros_liker   r   r   r   )r   Z
delta_poseZnew_poser   drotr   r   r   apply_delta_pose@   s   
 r3   Hz>c                 C   s   | t t j| | S r!   )r   maximumlinalgnorm)vectolr   r   r   	normalizeN   s   r:   c                 C   sN   t | } t |}t| |}t |}tt| |}|| }tj|}|S r!   )r:   r   crossarccosdotr   r   r   )Zfrom_vecZto_vecaxisanglerotvecr   r   r   r   rot_from_directionsR   s   rA   -q=c                 C   s,   t jj| dd}t ||}| j| j}|S )Nr   r>   )r   r6   r7   r5   r   )r8   epsr7   outr   r   r   r:   ]   s   c                 C   sv   | dd df | ddd f }}t |}|tj|| ddd|  }t |}tj||dd}tj|||fdd}|S )N.r   r   T)r>   keepdimsrC   )r:   r   sumr;   stack)d6a1a2b1b2b3rE   r   r   r   rot6d_to_matd   s   "rP   c                 C   s6   | j d d }| dd dd d f  |d }|S )NrG   .r,   r   )r
   copyreshape)r   	batch_dimrE   r   r   r   mat_to_rot6dn   s   $rT   c                 C   sF   | dd ddf }| dd dd df }t |}tj||gdd}|S )N.r   r   rC   )rT   r   concatenate)r   r   rotmatrJ   d10r   r   r   mat_to_pose10dt   s
   rX   c                 C   sz   | dd df }| ddd f }t |}tj| jd d d | jd}||dd dd df< ||dd ddf< d|d< |S )N.r   r   r   r   r   r	   )rP   r   r   r
   r   )rW   r   rJ   rV   rE   r   r   r   pose10d_to_mat|   s   rY   )r4   )rB   )numpyr   Zscipy.spatial.transformspatial	transformr   r   r   r   r   r"   r$   r'   r+   r0   r3   r:   rA   rP   rT   rX   rY   r   r   r   r   <module>   s&    	


