a
    d|                    @   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Zd dl	m
Z
mZmZmZmZmZmZmZmZmZ d dlmZmZmZ ejdd Zejdd	 ZG d
d dZG dd dZG dd dZG dd dZG dd dZG dd dZ G dd dZ!G dd dZ"dd Z#ej$%dg ddd Z&ej$%dg dd d! Z'G d"d# d#Z(G d$d% d%Z)G d&d' d'Z*G d(d) d)Z+G d*d+ d+Z,G d,d- d-Z-G d.d/ d/Z.G d0d1 d1Z/G d2d3 d3Z0G d4d5 d5Z1dS )6    )partialN)	gradcheck)
ARKitQTVecs_to_ColmapQTVecsQuaternionCoeffOrderRt_to_matrix4x4!camtoworld_graphics_to_vision_4x4 camtoworld_graphics_to_vision_Rtcamtoworld_to_worldtocam_Rt!camtoworld_vision_to_graphics_4x4 camtoworld_vision_to_graphics_Rtmatrix4x4_to_Rtworldtocam_to_camtoworld_Rt)assert_closecreate_eye_batchtensor_to_gradcheck_varc                 C   s   d| j v r|tjkrdS dS z&Lower tolerance for cuda-float16 only.ZcudagMbP?-C6?typetorchfloat16devicedtype r   g/var/www/html/stable-diffusion-webui/venv/lib/python3.9/site-packages/test/geometry/test_conversions.pyatol   s    r   c                 C   s   d| j v r|tjkrdS dS r   r   r   r   r   r   rtol    s    r   c                   @   s   e Zd Zdd Zdd Zejdddd Zejd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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+S ),TestAngleAxisToQuaternionc                 C   s`   t jd||d}tt$ tjjj|t	j
d}W d    n1 sD0    Y  |jdks\J d S N   r   r   order)   r   zerospytestwarnsUserWarningkorniageometryconversionsangle_axis_to_quaternionr   XYZWshapeselfr   r   
angle_axis
quaternionr   r   r   test_smoke_xyzw-   s    $z)TestAngleAxisToQuaternion.test_smoke_xyzwc                 C   s6   t jd||d}tjjj|tjd}|jdks2J d S r   	r   r&   r*   r+   r,   r-   r   WXYZr/   r0   r   r   r   
test_smoke5   s    z$TestAngleAxisToQuaternion.test_smoke
batch_size   r       c                 C   sf   t j|d||d}tt$ tjjj|t	j
d}W d    n1 sF0    Y  |j|dfksbJ d S Nr    r   r"   r$   r%   r1   r8   r   r   r2   r3   r   r   r   test_smoke_batch_xyzw:   s    $z/TestAngleAxisToQuaternion.test_smoke_batch_xyzwc                 C   s<   t j|d||d}tjjj|tjd}|j|dfks8J d S r<   r5   r=   r   r   r   test_smoke_batchC   s    z*TestAngleAxisToQuaternion.test_smoke_batchc                 C   sr   t jd||d}t jd||d}tt$ tjjj|t	j
d}W d    n1 sT0    Y  t||||d d S )N        rA   rA   r   rA   rA   rA         ?r"   r   r   )r   tensorr'   r(   r)   r*   r+   r,   r-   r   r.   r   r1   r   r   r   r   r2   expectedr3   r   r   r   test_zero_angle_xyzwI   s    $z.TestAngleAxisToQuaternion.test_zero_angle_xyzwc                 C   sH   t jd||d}t jd||d}tjjj|tjd}t||||d d S )Nr@   r   rC   rA   rA   rA   r"   rD   )	r   rE   r*   r+   r,   r-   r   r6   r   rF   r   r   r   test_zero_angleR   s    z)TestAngleAxisToQuaternion.test_zero_anglec           	      C   s   d}t j|ddf||d}t jt|d ddt|d f||d}tt$ tj	j
j|tjd}W d    n1 sz0    Y  t||||d d S N{Gz?rA   r          @r"   rD   r   rE   npsincosr'   r(   r)   r*   r+   r,   r-   r   r.   r   	r1   r   r   r   r   thetar2   rG   r3   r   r   r   test_small_angle_x_xyzwX   s    ,$z1TestAngleAxisToQuaternion.test_small_angle_x_xyzwc           	      C   sn   d}t j|ddf||d}t jt|d t|d ddf||d}tjjj|t	j
d}t||||d d S rK   r   rE   rO   rQ   rP   r*   r+   r,   r-   r   r6   r   rR   r   r   r   test_small_angle_xb   s
    ,z,TestAngleAxisToQuaternion.test_small_angle_xc           	      C   s   d}t jd|df||d}t jdt|d dt|d f||d}tt$ tj	j
j|tjd}W d    n1 sz0    Y  t||||d d S rK   rN   rR   r   r   r   test_small_angle_y_xyzwi   s    ,$z1TestAngleAxisToQuaternion.test_small_angle_y_xyzwc           	      C   sn   d}t jd|df||d}t jt|d dt|d df||d}tjjj|t	j
d}t||||d d S rK   rU   rR   r   r   r   test_small_angle_ys   s
    ,z,TestAngleAxisToQuaternion.test_small_angle_yc           	      C   s   d}t jdd|f||d}t jddt|d t|d f||d}tt$ tj	j
j|tjd}W d    n1 sz0    Y  t||||d d S rK   rN   rR   r   r   r   test_small_angle_z_xyzwz   s    ,$z1TestAngleAxisToQuaternion.test_small_angle_z_xyzwc           	      C   sn   d}t jdd|f||d}t jt|d ddt|d f||d}tjjj|t	j
d}t||||d d S rK   rU   rR   r   r   r   test_small_angle_z   s
    ,z,TestAngleAxisToQuaternion.test_small_angle_zc           	      C   s   dt d }tjtjd ddf||d}tj|dd|f||d}tt$ tj	j
j|tjd}W d    n1 sv0    Y  t||||d d S N      ?rM   rA   r   r"   rD   rO   sqrtr   rE   r*   pir'   r(   r)   r+   r,   r-   r   r.   r   	r1   r   r   r   r   Z
half_sqrt2r2   rG   r3   r   r   r   test_x_rotation_xyzw   s    $z.TestAngleAxisToQuaternion.test_x_rotation_xyzwc           	      C   sj   dt d }tjtjd ddf||d}tj||ddf||d}tjjj|t	j
d}t||||d d S r[   rO   r^   r   rE   r*   r_   r+   r,   r-   r   r6   r   r`   r   r   r   test_x_rotation   s
    z)TestAngleAxisToQuaternion.test_x_rotationc           	      C   s   dt d }tjdtjd df||d}tjd|d|f||d}tt$ tj	j
j|tjd}W d    n1 sv0    Y  t||||d d S r[   r]   r`   r   r   r   test_y_rotation_xyzw   s    $z.TestAngleAxisToQuaternion.test_y_rotation_xyzwc           	      C   sj   dt d }tjdtjd df||d}tj|d|df||d}tjjj|t	j
d}t||||d d S r[   rb   r`   r   r   r   test_y_rotation   s
    z)TestAngleAxisToQuaternion.test_y_rotationc           	      C   s   dt d }tjddtjd f||d}tjdd||f||d}tt$ tj	j
j|tjd}W d    n1 sv0    Y  t||||d d S r[   r]   r`   r   r   r   test_z_rotation_xyzw   s    $z.TestAngleAxisToQuaternion.test_z_rotation_xyzwc           	      C   sj   dt d }tjddtjd f||d}tj|dd|f||d}tjjj|t	j
d}t||||d d S r[   rb   r`   r   r   r   test_z_rotation   s
    z)TestAngleAxisToQuaternion.test_z_rotationc                 C   sz   t |j}t jd||d| }t|}tt4 tt	t
jjjtjd|fddsXJ W d    n1 sl0    Y  d S Nr@   r   r"   TZraise_exception)r   finfoepsrE   r   r'   r(   r)   r   r   r*   r+   r,   r-   r   r.   r1   r   r   rk   r2   r   r   r   test_gradcheck_xyzw   s    z-TestAngleAxisToQuaternion.test_gradcheck_xyzwc                 C   sP   t |j}t jd||d| }t|}tttjj	j
tjd|fddsLJ d S rh   )r   rj   rk   rE   r   r   r   r*   r+   r,   r-   r   r6   rl   r   r   r   test_gradcheck   s    z(TestAngleAxisToQuaternion.test_gradcheckN)__name__
__module____qualname__r4   r7   r'   markparametrizer>   r?   rH   rJ   rT   rV   rW   rX   rY   rZ   ra   rc   rd   re   rf   rg   rm   rn   r   r   r   r   r   ,   s,   

	





r   c                   @   s   e Zd Zdd Zdd Zejdddd Zejd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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+S ),TestQuaternionToAngleAxisc                 C   s`   t jd||d}tt$ tjjj|t	j
d}W d    n1 sD0    Y  |jdks\J d S Nr$   r   r"   )r    r   r&   r'   r(   r)   r*   r+   r,   quaternion_to_angle_axisr   r.   r/   r1   r   r   r3   r2   r   r   r   r4      s    $z)TestQuaternionToAngleAxis.test_smoke_xyzwc                 C   s6   t jd||d}tjjj|tjd}|jdks2J d S ru   	r   r&   r*   r+   r,   rw   r   r6   r/   rx   r   r   r   r7      s    z$TestQuaternionToAngleAxis.test_smoker8   r9   c                 C   sf   t j|d||d}tt$ tjjj|t	j
d}W d    n1 sF0    Y  |j|dfksbJ d S Nr$   r   r"   r    rv   r1   r8   r   r   r3   r2   r   r   r   r>      s    $z/TestQuaternionToAngleAxis.test_smoke_batch_xyzwc                 C   s<   t j|d||d}tjjj|tjd}|j|dfks8J d S rz   ry   r{   r   r   r   r?      s    z*TestQuaternionToAngleAxis.test_smoke_batchc                 C   sr   t jd||d}t jd||d}tt$ tjjj|t	j
d}W d    n1 sT0    Y  t||||d d S )NrB   r   r@   r"   rD   )r   rE   r'   r(   r)   r*   r+   r,   rw   r   r.   r   r1   r   r   r   r   r3   rG   r2   r   r   r   test_unit_quaternion_xyzw   s    $z3TestQuaternionToAngleAxis.test_unit_quaternion_xyzwc                 C   sH   t jd||d}t jd||d}tjjj|tjd}t||||d d S )NrI   r   r@   r"   rD   )	r   rE   r*   r+   r,   rw   r   r6   r   r|   r   r   r   test_unit_quaternion   s    z.TestQuaternionToAngleAxis.test_unit_quaternionc                 C   sz   t jd||d}t jtjddf||d}tt$ tjjj	|t
jd}W d    n1 s\0    Y  t||||d d S )NrI   r   rA   r"   rD   r   rE   r*   r_   r'   r(   r)   r+   r,   rw   r   r.   r   r|   r   r   r   ra     s    $z.TestQuaternionToAngleAxis.test_x_rotation_xyzwc                 C   sP   t jd||d}t jtjddf||d}tjjj|tjd}t	||||d d S NrA   rC   rA   rA   r   rA   r"   rD   
r   rE   r*   r_   r+   r,   rw   r   r6   r   r|   r   r   r   rc     s    z)TestQuaternionToAngleAxis.test_x_rotationc                 C   sz   t jd||d}t jdtjdf||d}tt$ tjjj	|t
jd}W d    n1 s\0    Y  t||||d d S r   r   r|   r   r   r   rd     s    $z.TestQuaternionToAngleAxis.test_y_rotation_xyzwc                 C   sP   t jd||d}t jdtjdf||d}tjjj|tjd}t	||||d d S )NrA   rA   rC   rA   r   rA   r"   rD   r   r|   r   r   r   re     s    z)TestQuaternionToAngleAxis.test_y_rotationc                 C   s   t jdddtdd f||d}t jddtjd f||d}tt$ tj	j
j|tjd}W d    n1 sr0    Y  t||||d d S )NrA   r\         @rM   r   r"   rD   )r   rE   rO   r^   r*   r_   r'   r(   r)   r+   r,   rw   r   r.   r   r|   r   r   r   rf      s    "$z.TestQuaternionToAngleAxis.test_z_rotation_xyzwc                 C   sf   t jtdd dddf||d}t jddtjd f||d}tjjj|t	j
d}t||||d d S )Nr   rM   rA   r\   r   r"   rD   )r   rE   rO   r^   r*   r_   r+   r,   rw   r   r6   r   r|   r   r   r   rg   )  s    "z)TestQuaternionToAngleAxis.test_z_rotationc           	      C   s   d}t jt|d ddt|d f||d}t j|ddf||d}tt$ tj	j
j|tjd}W d    n1 sz0    Y  t||||d d S NrL   rM   rA   r   r"   rD   r   rE   rO   rP   rQ   r'   r(   r)   r*   r+   r,   rw   r   r.   r   	r1   r   r   r   r   rS   r3   rG   r2   r   r   r   rT   /  s    ,$z1TestQuaternionToAngleAxis.test_small_angle_x_xyzwc           	      C   sn   d}t jt|d t|d ddf||d}t j|ddf||d}tjjj|t	j
d}t||||d d S r   r   rE   rO   rQ   rP   r*   r+   r,   rw   r   r6   r   r   r   r   r   rV   9  s
    ,z,TestQuaternionToAngleAxis.test_small_angle_xc           	      C   s   d}t jdt|d dt|d f||d}t jd|df||d}tt$ tj	j
j|tjd}W d    n1 sz0    Y  t||||d d S NrL   rA      r   r"   rD   r   r   r   r   r   rW   @  s    ,$z1TestQuaternionToAngleAxis.test_small_angle_y_xyzwc           	      C   sn   d}t jt|d dt|d df||d}t jd|df||d}tjjj|t	j
d}t||||d d S NrL   r   rA   r   r"   rD   r   r   r   r   r   rX   J  s
    ,z,TestQuaternionToAngleAxis.test_small_angle_yc           	      C   s   d}t jddt|d t|d f||d}t jdd|f||d}tt$ tj	j
j|tjd}W d    n1 sz0    Y  t||||d d S r   r   r   r   r   r   rY   Q  s    ,$z1TestQuaternionToAngleAxis.test_small_angle_z_xyzwc           	      C   sn   d}t jt|d ddt|d f||d}t jdd|f||d}tjjj|t	j
d}t||||d d S r   r   r   r   r   r   rZ   [  s
    ,z,TestQuaternionToAngleAxis.test_small_angle_zc                 C   sz   t |j}t jd||d| }t|}tt4 tt	t
jjjtjd|fddsXJ W d    n1 sl0    Y  d S NrB   r   r"   Tri   )r   rj   rk   rE   r   r'   r(   r)   r   r   r*   r+   r,   rw   r   r.   r1   r   r   rk   r3   r   r   r   rm   b  s    z-TestQuaternionToAngleAxis.test_gradcheck_xyzwc                 C   sP   t |j}t jd||d| }t|}tttjj	j
tjd|fddsLJ d S )NrI   r   r"   Tri   )r   rj   rk   rE   r   r   r   r*   r+   r,   rw   r   r6   r   r   r   r   rn   n  s    z(TestQuaternionToAngleAxis.test_gradcheckN)ro   rp   rq   r4   r7   r'   rr   rs   r>   r?   r}   r~   ra   rc   rd   re   rf   rg   rT   rV   rW   rX   rY   rZ   rm   rn   r   r   r   r   rt      s,   

				


rt   c                   @   s   e Zd Zejdddd Zejd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d Zdd Zdd Zejjejddddd Zejjejdddd d! Zd"S )#TestRotationMatrixToQuaternionr8   r9   c                 C   sh   t j|dd||d}tt$ tjjj|t	j
d}W d    n1 sH0    Y  |j|dfksdJ d S r<   )r   r&   r'   r(   r)   r*   r+   r,   rotation_matrix_to_quaternionr   r.   r/   r1   r8   r   r   matrixr3   r   r   r   r>   {  s    $z4TestRotationMatrixToQuaternion.test_smoke_batch_xyzwc                 C   s>   t j|dd||d}tjjj|tjd}|j|dfks:J d S r<   )	r   r&   r*   r+   r,   r   r   r6   r/   r   r   r   r   r?     s    z/TestRotationMatrixToQuaternion.test_smoke_batchc                 C   sr   t jd||d}t jd||d}tt$ tjjj|t	j
d}W d    n1 sT0    Y  t||||d d S )NrC   rA   rA   rA   rC   rA   rA   rA   rC   r   rB   r"   rD   )r   rE   r'   r(   r)   r*   r+   r,   r   r   r.   r   r1   r   r   r   r   r   rG   r3   r   r   r   test_identity_xyzw  s    $z1TestRotationMatrixToQuaternion.test_identity_xyzwc                 C   sH   t jd||d}t jd||d}tjjj|tjd}t||||d d S )Nr   r   rI   r"   rD   )	r   rE   r*   r+   r,   r   r   r6   r   r   r   r   r   test_identity  s    z,TestRotationMatrixToQuaternion.test_identityc           	      C   s   t jd||d}t tjd j||d}t j|dd|f||d}tt$ tj	j
j|tjd}W d    n1 sv0    Y  t||||d d S Nr   rA   rA         r   r   g      @rA   r"   rD   )r   rE   rQ   r*   r_   tor'   r(   r)   r+   r,   r   r   r.   r   	r1   r   r   r   r   r   Zpi_half2rG   r3   r   r   r   test_rot_x_45_xyzw  s    $z1TestRotationMatrixToQuaternion.test_rot_x_45_xyzwc           	      C   sj   t jd||d}t tjd j||d}t j||ddf||d}tjjj|t	j
d}t||||d d S r   )r   rE   rQ   r*   r_   r   r+   r,   r   r   r6   r   r   r   r   r   test_rot_x_45  s
    z,TestRotationMatrixToQuaternion.test_rot_x_45c           	      C   s   t |j}t jd||d}tt: tjj	j
||tjd}tjj	j|tjd}W d    n1 sf0    Y  t||||d d S Nr   r   rk   r#   r"   rD   )r   rj   rk   rE   r'   r(   r)   r*   r+   r,   r   r   r.   quaternion_to_rotation_matrixr   	r1   r   r   r   r   rk   r   r3   Z
matrix_hatr   r   r   test_back_and_forth_xyzw  s    $z7TestRotationMatrixToQuaternion.test_back_and_forth_xyzwc           	      C   sZ   t |j}t jd||d}tjjj||tj	d}tjjj
|tj	d}t||||d d S r   )r   rj   rk   rE   r*   r+   r,   r   r   r6   r   r   r   r   r   r   test_back_and_forth  s    z2TestRotationMatrixToQuaternion.test_back_and_forthc           	      C   s   t |j}t jd||d}t jd||d}tt& tjj	j
||tjd}W d    n1 sb0    Y  t jdd t||||d d S )N)gx`gbg	?)gg6?g,gK)g ?gS@Yg??r   )  ?   7ܿ  @A??r   
   Z	precisionrD   )r   rj   rk   rE   r'   r(   r)   r*   r+   r,   r   r   r.   set_printoptionsr   	r1   r   r   r   r   rk   r   Zquaternion_truer3   r   r   r   test_corner_case_xyzw  s    	$z4TestRotationMatrixToQuaternion.test_corner_case_xyzwc           	      C   sb   t |j}t jd||d}t jd||d}tjjj||tj	d}t j
dd t||||d d S )Nr   r   )r   r   r   r   r   r   r   rD   )r   rj   rk   rE   r*   r+   r,   r   r   r6   r   r   r   r   r   r   test_corner_case  s    	z/TestRotationMatrixToQuaternion.test_corner_casec                 C   sx   t |j}t jd||d}t|}tt6 tt	t
jjj|tjd|fddsVJ W d    n1 sj0    Y  d S Nr    r   r   Tri   )r   rj   rk   eyer   r'   r(   r)   r   r   r*   r+   r,   r   r   r.   r1   r   r   rk   r   r   r   r   rm     s    z2TestRotationMatrixToQuaternion.test_gradcheck_xyzwc                 C   sN   t |j}t jd||d}t|}tttjj	j
|tjd|fddsJJ d S r   )r   rj   rk   r   r   r   r   r*   r+   r,   r   r   r6   r   r   r   r   rn     s    z-TestRotationMatrixToQuaternion.test_gradcheck1.6JIT Enum not handled.reasonc                 C   s   t jjj}tj|}tjd||d}t	t
 ||}W d    n1 sN0    Y  t	t
 ||}W d    n1 s0    Y  t|| d S )Nr   r   )r*   r+   r,   quaternion_log_to_expr   jitscriptrE   r'   r(   r)   r   )r1   r   r   op	op_scriptr3   actualrG   r   r   r   test_jit_xyzw  s    
&&z,TestRotationMatrixToQuaternion.test_jit_xyzwc           	      C   s`   t |j}tjjj}t j|}t j	d||d}|||t
jd}|||t
jd}t|| d S )Nr   r   r   )r   rj   rk   r*   r+   r,   r   r   r   rE   r   r6   r   )	r1   r   r   rk   r   r   r3   r   rG   r   r   r   test_jit  s    
z'TestRotationMatrixToQuaternion.test_jitN)ro   rp   rq   r'   rr   rs   r>   r?   r   r   r   r   r   r   r   r   rm   rn   skipifr   __version__
startswithr   r   r   r   r   r   r   z  s$   

	


r   c                   @   s   e Zd Zejdddd Zejd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d Zdd Zdd Zejjejddddd Zejjejdddd d! Zd"S )#TestQuaternionToRotationMatrixr8   r9   c                 C   sh   t j|d||d}tt$ tjjj|t	j
d}W d    n1 sF0    Y  |j|ddfksdJ d S rz   )r   r&   r'   r(   r)   r*   r+   r,   r   r   r.   r/   r1   r8   r   r   r3   r   r   r   r   r>     s    $z4TestQuaternionToRotationMatrix.test_smoke_batch_xyzwc                 C   s>   t j|d||d}tjjj|tjd}|j|ddfks:J d S rz   )	r   r&   r*   r+   r,   r   r   r6   r/   r   r   r   r   r?   %  s    z/TestQuaternionToRotationMatrix.test_smoke_batchc                 C   sr   t jd||d}t jd||d}tt$ tjjj|t	j
d}W d    n1 sT0    Y  t||||d d S )NrB   r   r   r"   rD   r   rE   r'   r(   r)   r*   r+   r,   r   r   r.   r   r1   r   r   r   r   r3   rG   r   r   r   r   r}   +  s    $z8TestQuaternionToRotationMatrix.test_unit_quaternion_xyzwc                 C   sH   t jd||d}t jd||d}tjjj|tjd}t||||d d S )NrI   r   r   r"   rD   	r   rE   r*   r+   r,   r   r   r6   r   r   r   r   r   r~   4  s    z3TestQuaternionToRotationMatrix.test_unit_quaternionc                 C   sr   t jd||d}t jd||d}tt$ tjjj|t	j
d}W d    n1 sT0    Y  t||||d d S )NrI   r   r   rA   r   rA   r   r"   rD   r   r   r   r   r   ra   :  s    $z3TestQuaternionToRotationMatrix.test_x_rotation_xyzwc                 C   sH   t jd||d}t jd||d}tjjj|tjd}t||||d d S )Nr   r   r   r"   rD   r   r   r   r   r   rc   C  s    z.TestQuaternionToRotationMatrix.test_x_rotationc                 C   sr   t jd||d}t jd||d}tt$ tjjj|t	j
d}W d    n1 sT0    Y  t||||d d S )Nr   r   r   rA   rA   r   r   r"   rD   r   r   r   r   r   rd   I  s    $z3TestQuaternionToRotationMatrix.test_y_rotation_xyzwc                 C   sH   t jd||d}t jd||d}tjjj|tjd}t||||d d S )Nr   r   r   r"   rD   r   r   r   r   r   re   S  s    z.TestQuaternionToRotationMatrix.test_y_rotationc                 C   sr   t jd||d}t jd||d}tt$ tjjj|t	j
d}W d    n1 sT0    Y  t||||d d S )Nr   r   r   r   r   r"   rD   r   r   r   r   r   rf   Y  s    $z3TestQuaternionToRotationMatrix.test_z_rotation_xyzwc                 C   sH   t jd||d}t jd||d}tjjj|tjd}t||||d d S )NrB   r   r   r"   rD   r   r   r   r   r   rg   b  s    z.TestQuaternionToRotationMatrix.test_z_rotationc                 C   sj   t jd||d}t|}tt4 tttj	j
jtjd|fddsHJ W d    n1 s\0    Y  d S r   )r   rE   r   r'   r(   r)   r   r   r*   r+   r,   r   r   r.   r1   r   r   r3   r   r   r   rm   h  s    z2TestQuaternionToRotationMatrix.test_gradcheck_xyzwc                 C   s@   t jd||d}t|}tttjjjt	j
d|fdds<J d S r   )r   rE   r   r   r   r*   r+   r,   r   r   r6   r   r   r   r   rn   s  s    z-TestQuaternionToRotationMatrix.test_gradcheckr   r   r   c                 C   sf   t jjj}tj|}tjd||d}t	t
" t|||| W d    n1 sX0    Y  d S )Nr   r   )r*   r+   r,   r   r   r   r   rE   r'   r(   r)   r   r1   r   r   r   op_jitr3   r   r   r   r   }  s
    
z,TestQuaternionToRotationMatrix.test_jit_xyzwc                 C   sH   t jjj}tj|}tjd||d}t||t	j
d||t	j
d d S )NrB   r   r"   )r*   r+   r,   r   r   r   r   rE   r   r   r6   r   r   r   r   r     s    
z'TestQuaternionToRotationMatrix.test_jitN)ro   rp   rq   r'   rr   rs   r>   r?   r}   r~   ra   rc   rd   re   rf   rg   rm   rn   r   r   r   r   r   r   r   r   r   r   r     s$   

		
	

r   c                   @   s   e Zd Zejdddd Zejd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d Zdd Zdd Zdd Zdd Zejjejdd d!d"d# Zejjejdd d!d$d% Zd&S )'TestQuaternionLogToExpr8   r9   c                 C   sf   t j|d||d}tt$ tjjj|t	j
d}W d    n1 sF0    Y  |j|dfksbJ d S r<   )r   r&   r'   r(   r)   r*   r+   r,   r   r   r.   r/   r1   r8   r   r   quaternion_logquaternion_expr   r   r   r>     s    $z,TestQuaternionLogToExp.test_smoke_batch_xyzwc                 C   s<   t j|d||d}tjjj|tjd}|j|dfks8J d S r<   )	r   r&   r*   r+   r,   r   r   r6   r/   r   r   r   r   r?     s
    z'TestQuaternionLogToExp.test_smoke_batchc           	      C   s   t |j}t jd||d}t jd||d}tt& tjj	j
||tjd}W d    n1 sb0    Y  t||||d d S )Nr@   r   rB   r   rD   )r   rj   rk   rE   r'   r(   r)   r*   r+   r,   r   r   r.   r   	r1   r   r   r   r   rk   r   rG   r   r   r   r   r}     s    $z0TestQuaternionLogToExp.test_unit_quaternion_xyzwc           	      C   sV   t |j}t jd||d}t jd||d}tjjj||tj	d}t
||||d d S )Nr@   r   rI   r   rD   )r   rj   rk   rE   r*   r+   r,   r   r   r6   r   r   r   r   r   r~     s    z+TestQuaternionLogToExp.test_unit_quaternionc           
      C   s   t |j}t jd||d}t jd||d}t jt |ddt |f||d}tt& t	j
jj||tjd}	W d    n1 s0    Y  t|	|||d d S NrC   r   r   rA   r   rD   r   rj   rk   rE   rP   rQ   r'   r(   r)   r*   r+   r,   r   r   r.   r   
r1   r   r   r   r   rk   Zoner   rG   r   r   r   r   test_pi_quaternion_x_xyzw  s    $$z0TestQuaternionLogToExp.test_pi_quaternion_x_xyzwc           
      C   sz   t |j}t jd||d}t jd||d}t jt |t |ddf||d}tjjj	||t
jd}	t|	|||d d S r   r   rj   rk   rE   rQ   rP   r*   r+   r,   r   r   r6   r   r   r   r   r   test_pi_quaternion_x  s    $z+TestQuaternionLogToExp.test_pi_quaternion_xc           
      C   s   t |j}t jd||d}t jd||d}t jdt |dt |f||d}tt& t	j
jj||tjd}	W d    n1 s0    Y  t|	|||d d S NrC   r   r   rA   r   rD   r   r   r   r   r   test_pi_quaternion_y_xyzw  s    $$z0TestQuaternionLogToExp.test_pi_quaternion_y_xyzwc           
      C   sz   t |j}t jd||d}t jd||d}t jt |dt |df||d}tjjj	||t
jd}	t|	|||d d S r   r   r   r   r   r   test_pi_quaternion_y  s    $z+TestQuaternionLogToExp.test_pi_quaternion_yc           
      C   s   t |j}t jd||d}t jd||d}t jddt |t |f||d}tt& t	j
jj||tjd}	W d    n1 s0    Y  t|	|||d d S NrC   r   r   rA   r   rD   r   r   r   r   r   test_pi_quaternion_z_xyzw  s    $$z0TestQuaternionLogToExp.test_pi_quaternion_z_xyzwc           
      C   sz   t |j}t jd||d}t jd||d}t jt |ddt |f||d}tjjj	||t
jd}	t|	|||d d S r   r   r   r   r   r   test_pi_quaternion_z  s    $z+TestQuaternionLogToExp.test_pi_quaternion_zc           	      C   s   t |j}t jd||d}tt& tjj	j
||tjd}W d    n1 sR0    Y  tt& tjj	j||tjd}W d    n1 s0    Y  t||||d d S Nr   r   r   rD   )r   rj   rk   rE   r'   r(   r)   r*   r+   r,   r   r   r.   quaternion_exp_to_logr   	r1   r   r   r   r   rk   r   r   Zquaternion_log_hatr   r   r   r     s    $$z/TestQuaternionLogToExp.test_back_and_forth_xyzwc           	      C   s\   t |j}t jd||d}tjjj||tj	d}tjjj
||tj	d}t||||d d S r   )r   rj   rk   rE   r*   r+   r,   r   r   r6   r   r   r   r   r   r   r     s    z*TestQuaternionLogToExp.test_back_and_forthc                 C   sx   t |j}t jd||d}t|}tt6 tt	t
jjj|tjd|fddsVJ W d    n1 sj0    Y  d S Nr   r   r   Tri   )r   rj   rk   rE   r   r'   r(   r)   r   r   r*   r+   r,   r   r   r.   r   r   r   r   rm     s    z*TestQuaternionLogToExp.test_gradcheck_xyzwc                 C   sN   t |j}t jd||d}t|}tttjj	j
|tjd|fddsJJ d S r   )r   rj   rk   rE   r   r   r   r*   r+   r,   r   r   r6   r   r   r   r   rn     s    z%TestQuaternionLogToExp.test_gradcheckr   r   r   c                 C   sr   t jjj}tj|}tjd||d}t	t
. t||tjd||tjd W d    n1 sd0    Y  d S Nr   r   r"   )r*   r+   r,   r   r   r   r   rE   r'   r(   r)   r   r   r.   r   r   r   r   r   %  s    
z$TestQuaternionLogToExp.test_jit_xyzwc                 C   sH   t jjj}tj|}tjd||d}t||t	j
d||t	j
d d S r   )r*   r+   r,   r   r   r   r   rE   r   r   r6   r   r   r   r   r   /  s    
zTestQuaternionLogToExp.test_jitNro   rp   rq   r'   rr   rs   r>   r?   r}   r~   r   r   r   r   r   r   r   r   rm   rn   r   r   r   r   r   r   r   r   r   r   r     s(   


	



	r   c                   @   s   e Zd Zejdddd Zejd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d Zdd Zdd Zdd Zdd Zejjejdd d!d"d# Zejjejdd d!d$d% Zd&S )'TestQuaternionExpToLogr8   r9   c                 C   st   t |j}t j|d||d}tt& tjj	j
||tjd}W d    n1 sT0    Y  |j|dfkspJ d S Nr$   r   r   r    )r   rj   rk   r&   r'   r(   r)   r*   r+   r,   r   r   r.   r/   r1   r8   r   r   rk   r   r   r   r   r   r>   :  s    $z,TestQuaternionExpToLog.test_smoke_batch_xyzwc                 C   sJ   t |j}t j|d||d}tjjj||tj	d}|j
|dfksFJ d S r   )r   rj   rk   r&   r*   r+   r,   r   r   r6   r/   r   r   r   r   r?   D  s    z'TestQuaternionExpToLog.test_smoke_batchc           	      C   s   t |j}t jd||d}t jd||d}tt& tjj	j
||tjd}W d    n1 sb0    Y  t||||d d S )NrB   r   r@   r   rD   )r   rj   rk   rE   r'   r(   r)   r*   r+   r,   r   r   r.   r   	r1   r   r   r   r   rk   r   rG   r   r   r   r   r}   M  s    $z0TestQuaternionExpToLog.test_unit_quaternion_xyzwc           	      C   sV   t |j}t jd||d}t jd||d}tjjj||tj	d}t
||||d d S )NrI   r   r@   r   rD   )r   rj   rk   rE   r*   r+   r,   r   r   r6   r   r   r   r   r   r~   W  s    z+TestQuaternionExpToLog.test_unit_quaternionc           	      C   s   t |j}t jd||d}t jtjd ddf||d}tt& tj	j
j||tjd}W d    n1 sn0    Y  t||||d d S )NrI   r   rM   rA   r   rD   r   rj   rk   rE   r*   r_   r'   r(   r)   r+   r,   r   r   r.   r   r   r   r   r   r   `  s    $z0TestQuaternionExpToLog.test_pi_quaternion_x_xyzwc           	      C   sb   t |j}t jd||d}t jtjd ddf||d}tjjj||t	j
d}t||||d d S )Nr   r   rM   rA   r   rD   r   rj   rk   rE   r*   r_   r+   r,   r   r   r6   r   r   r   r   r   r   j  s    z+TestQuaternionExpToLog.test_pi_quaternion_xc           	      C   s   t |j}t jd||d}t jdtjd df||d}tt& tj	j
j||tjd}W d    n1 sn0    Y  t||||d d S )Nr   r   rA   rM   r   rD   r   r   r   r   r   r   s  s    $z0TestQuaternionExpToLog.test_pi_quaternion_y_xyzwc           	      C   sb   t |j}t jd||d}t jdtjd df||d}tjjj||t	j
d}t||||d d S Nr   r   rA   rM   r   rD   r   r   r   r   r   r   }  s    z+TestQuaternionExpToLog.test_pi_quaternion_yc           	      C   s   t |j}t jd||d}t jddtjd f||d}tt& tj	j
j||tjd}W d    n1 sn0    Y  t||||d d S r   r   r   r   r   r   r     s    $z0TestQuaternionExpToLog.test_pi_quaternion_z_xyzwc           	      C   sb   t |j}t jd||d}t jddtjd f||d}tjjj||t	j
d}t||||d d S )NrB   r   rA   rM   r   rD   r   r   r   r   r   r     s    z+TestQuaternionExpToLog.test_pi_quaternion_zc           	      C   s   t |j}t jd||d}tt& tjj	j
||tjd}W d    n1 sR0    Y  tt& tjj	j||tjd}W d    n1 s0    Y  t||||d d S )NrI   r   r   rD   )r   rj   rk   rE   r'   r(   r)   r*   r+   r,   r   r   r.   r   r   	r1   r   r   r   r   rk   r   r   Zquaternion_exp_hatr   r   r   r     s    $$z/TestQuaternionExpToLog.test_back_and_forth_xyzwc           	      C   s\   t |j}t jd||d}tjjj||tj	d}tjjj
||tj	d}t||||d d S )Nr   r   r   rD   )r   rj   rk   rE   r*   r+   r,   r   r   r6   r   r   r   r   r   r   r     s    z*TestQuaternionExpToLog.test_back_and_forthc                 C   sx   t |j}t jd||d}t|}tt6 tt	t
jjj|tjd|fddsVJ W d    n1 sj0    Y  d S )NrI   r   r   Tri   )r   rj   rk   rE   r   r'   r(   r)   r   r   r*   r+   r,   r   r   r.   r   r   r   r   rm     s    z*TestQuaternionExpToLog.test_gradcheck_xyzwc                 C   sN   t |j}t jd||d}t|}tttjj	j
|tjd|fddsJJ d S )Nr   r   r   Tri   )r   rj   rk   rE   r   r   r   r*   r+   r,   r   r   r6   r   r   r   r   rn     s    z%TestQuaternionExpToLog.test_gradcheckr   r   r   c                 C   sl   t jjj}tj|}tjd||d}t	t
( t||||||d W d    n1 s^0    Y  d S )Nr   r   rD   )r*   r+   r,   r   r   r   r   rE   r'   r(   r)   r   )r1   r   r   r   r   r   r   r3   r   r   r   r     s
    
z$TestQuaternionExpToLog.test_jit_xyzwc           
      C   sj   t jjj}tj|}tjd||d}||t|j	t
jd}||t|j	t
jd}	t||	||d d S )NrB   r   r   rD   )r*   r+   r,   r   r   r   r   rE   rj   rk   r   r6   r   )
r1   r   r   r   r   r   r   r3   r   rG   r   r   r   r     s    
zTestQuaternionExpToLog.test_jitNr   r   r   r   r   r   9  s(   
	

	
	
	
	
r   c                   @   s*   e Zd Zejdddd Zdd ZdS )TestAngleAxisToRotationMatrixr8   r:   r      c           
      C   sz   t j|d||d}t|d||d}tjj|}t ||dd}	t	|	|||d t
|}ttjjj|fddsvJ d S )Nr    r   rD   Tri   )r   randr   r*   r+   r,   angle_axis_to_rotation_matrixmatmul	transposer   r   r   )
r1   r8   r   r   r   r   r2   	eye_batchrotation_matrixrotation_matrix_eyer   r   r   test_rand_angle_axis_gradcheck  s    z<TestAngleAxisToRotationMatrix.test_rand_angle_axis_gradcheckc                 C   s   t jd||d}t jd||d}t jd||d}t jd||d}t j||fdd}	t j||fdd}
ttjj|
|	||d d S 	N))g]>qӿgG1ng36zĭ)gEO%^ghr(?gV)g}?gW`ÿg{#cr   )gfG?g	S g}x?))gx3YI?g@^g?'.)g菛gJug7gN?)gRlڿg\i˿gXpu5)gIrtg3$w?g=@,Z%?r   )ZdimrD   )r   rE   stackr   r*   r+   r,   r   r1   r   r   r   r   Zrmat_1Zrvec_1Zrmat_2Zrvec_2ZrmatZrvecr   r   r   "test_angle_axis_to_rotation_matrix  s    		z@TestAngleAxisToRotationMatrix.test_angle_axis_to_rotation_matrixN)ro   rp   rq   r'   rr   rs   r   r   r   r   r   r   r     s   
r   c                   @   s*   e Zd Zejdddd Zdd ZdS )TestRotationMatrixToAngleAxisr8   r   c           
      C   s   t j|d||d}tjj|d }tjjj|tjd}t	|d||d}t 
||dd}	t|	||d |d d	 t|}ttjjj|fd
dsJ d S )Nr$   r   gư>)r3   r#   r    r   r   g      $@rD   Tri   )r   r   r*   r+   r,   Znormalize_quaternionr   r   r6   r   r   r   r   r   r   rotation_matrix_to_angle_axis)
r1   r8   r   r   r   r   r3   r   r   r   r   r   r   test_rand_quaternion_gradcheck
  s    z<TestRotationMatrixToAngleAxis.test_rand_quaternion_gradcheckc                 C   s   t jd||d}t jd||d}t jd||d}t jd||d}t j||fdd}	t j||fdd}
ttjj|	|
||d d S r   )r   rE   r   r   r*   r+   r,   r  r   r   r   r   "test_rotation_matrix_to_angle_axis  s    		z@TestRotationMatrixToAngleAxis.test_rotation_matrix_to_angle_axisN)ro   rp   rq   r'   rr   rs   r  r  r   r   r   r   r  	  s   
r  c                   C   s   t tjj d d S )Ngz !	@)r   r*   	constantsr_   itemr   r   r   r   test_pi:  s    r  batch_shape)r   r    r:   r   r    )r   r    r    )r   r   r    c                 C   s`   t jjtj| ||d }t jj|}t jj|}t	|| t
t jjjt|fdds\J d S Nr   Tri   )r*   r  r_   r   r   r+   r,   rad2degdeg2radr   r   r   )r  r   r   x_radx_degZx_deg_to_radr   r   r   test_rad2deg>  s
    
r  c                 C   sb   dt j| ||d }tjj|}tjj|}t||||d ttjjjt	|fdds^J d S )Ng     f@r   rD   Tri   )
r   r   r*   r+   r,   r  r  r   r   r   )r  r   r   r   r   r  r  Zx_rad_to_degr   r   r   test_deg2radN  s
    r  c                   @   sH   e Zd Zdd Zejdg ddd Zejdg ddd Zd	S )
TestPolCartConversionsc                 C   sJ   t jdddd||d}tjj||d us.J tjj||d usFJ d S )Nr:   r   )r   onesr*   r+   r,   pol2cartcart2pol)r1   r   r   xr   r   r   r7   ]  s    z!TestPolCartConversions.test_smoker  r	  c           
      C   s   t j||d}tjjt j||d }||}||}tjj||\}}tjj	||d\}}	t
|| t
||	 ttjjjt|t|fddsJ d S Nr   r   Tri   )r   r   r*   r  r_   r   r+   r,   r  r  r   r   r   )
r1   r  r   r   rhophiZ
x_pol2cartZ
y_pol2cartZrho_pol2cartZphi_pol2cartr   r   r   test_pol2cartb  s    



z$TestPolCartConversions.test_pol2cartc           
      C   s   t j||d}t j||d}||}||}tjj||d\}}tjj||\}}	t|| t||	 t	tjjjt
|t
|fddsJ d S r  )r   r   r   r*   r+   r,   r  r  r   r   r   )
r1   r  r   r   r  yZrho_cart2polZphi_cart2polZ
x_cart2polZ
y_cart2polr   r   r   test_cart2polw  s    



z$TestPolCartConversions.test_cart2polN)	ro   rp   rq   r7   r'   rr   rs   r  r  r   r   r   r   r  \  s
   
r  c                   @   s>   e Zd Zdd Zdd Zejdg ddd Zd	d
 Z	dS )TestConvertPointsToHomogeneousc                 C   s~   t jg dg dg dg dg dg||d}t jg dg dg d	g d
g dg||d}tjj|}t||ddd d S )NrC   rM   rC   rA   rC   rM   rM   rC   rA   r          r   rA   rC   r#  r   )rC   rM   rC   rC   rA   rC   rM   rC   rM   rC   rA   rC   )r   r#  r   rC   rA   rC   r#  rC   r   rD   r   rE   r*   r+   r,   convert_points_to_homogeneousr   r1   r   r   points_hrG   pointsr   r   r   test_convert_points  s"     z2TestConvertPointsToHomogeneous.test_convert_pointsc                 C   sr   t jg dgg dgg dgg||d}t jg dgg dgg dgg||d}tjj|}t||ddd	 d S )
Nr!  r   r$  r   r&  r%  r'  r   rD   r(  r*  r   r   r   test_convert_points_batch  s    (z8TestConvertPointsToHomogeneous.test_convert_points_batchr  r	  c                 C   s6   t j|||d}t|}ttjjj|fdds2J d S r  )r   r   r   r   r*   r+   r,   r)  r1   r  r   r   r+  r   r   r   rn     s    z-TestConvertPointsToHomogeneous.test_gradcheckc                 C   s@   t jjj}tj|}tjddd||d}t|||| d S Nr:   r   r    r   )	r*   r+   r,   r)  r   r   r   r&   r   r1   r   r   r   r   r+  r   r   r   r     s    
z'TestConvertPointsToHomogeneous.test_jitN)
ro   rp   rq   r-  r.  r'   rr   rs   rn   r   r   r   r   r   r    s
   
r  c                   @   s6   e Zd Zdd Zejdddgdd Zdd	 Zd
S )TestConvertAtoHc                 C   sn   t jg dg dg||dddd}t jg dg dg dg||dddd}tjj|}t|| d S )Nr   r   r   r:   r   r    r   )r   rE   viewr*   r+   r,   "convert_affinematrix_to_homographyr   )r1   r   r   ArG   Hr   r   r   r-    s    &"z#TestConvertAtoH.test_convert_pointsr  )r   r   r    )   r   r    c                 C   s6   t j|||d}t|}ttjjj|fdds2J d S r  )r   r   r   r   r*   r+   r,   r4  r/  r   r   r   rn     s
    zTestConvertAtoH.test_gradcheckc                 C   s@   t jjj}tj|}tjddd||d}t|||| d S r0  )	r*   r+   r,   r4  r   r   r   r&   r   r1  r   r   r   r     s    
zTestConvertAtoH.test_jitN)	ro   rp   rq   r-  r'   rr   rs   rn   r   r   r   r   r   r2    s   
	r2  c                   @   sZ   e Zd Zejdg ddd Zdd Zdd Zd	d
 Z	ej
ddd Zdd ZdS ) TestConvertPointsFromHomogeneousr  r	  c                 C   s>   t j|||d}tjj|}|j|jd d d ks:J d S )Nr   r   )r   )r   r   r*   r+   r,   convert_points_from_homogeneousr/   )r1   r   r   r  r+  r,  r   r   r   test_cardinality  s    z1TestConvertPointsFromHomogeneous.test_cardinalityc                 C   s~   t jg dg dg dg dg dg||d}t jddgd	d
gddgddgd	dgg||d}tjj|}t||ddd d S )Nr  r   r!  r"  r$  r   rC   rM   rA   r\         r   rD   r   rE   r*   r+   r,   r9  r   r*  r   r   r   test_points  s     $z,TestConvertPointsFromHomogeneous.test_pointsc                 C   sr   t jg dgg dgg dgg||d}t jddggddggdd	ggg||d}tjj|}t||d
d
d d S )Nr!  r   r$  r   rM   rC   rA   r\   r;  r   rD   r<  r*  r   r   r   test_points_batch  s    ((z2TestConvertPointsFromHomogeneous.test_points_batchc                 C   s:   t jddd||d}t|}ttjjj|fdds6J d S )Nr:   r   r    r   Tri   )r   r  r   r   r*   r+   r,   r9  r1   r   r   r+  r   r   r   rn      s    z/TestConvertPointsFromHomogeneous.test_gradcheckzERuntimeError: Jacobian mismatch for output 0 with respect to input 0,c                 C   sH   t jg dg dg dg||d}t|}ttjjj|fddsDJ d S )N)rC   rM   rA   )rA   rC   皙?)rM   rC   r@  r   Tri   )r   rE   r   r   r*   r+   r,   r9  r?  r   r   r   test_gradcheck_zvec_zeros  s    "z:TestConvertPointsFromHomogeneous.test_gradcheck_zvec_zerosc                 C   s@   t jjj}tj|}tjddd||d}t|||| d S r0  )	r*   r+   r,   r9  r   r   r   r&   r   r1  r   r   r   r     s    
z)TestConvertPointsFromHomogeneous.test_jitN)ro   rp   rq   r'   rr   rs   r:  r=  r>  rn   skiprA  r   r   r   r   r   r8    s   



r8  c                   @   s$   e Zd Zdd Zdd Zdd ZdS )TestNormalizePixelCoordinatesc                 C   sv   t |j}d\}}tjj||d|dj|d}tjj||d|dj|d}	tjjj	||||d}
t
|
|	||d d S )Nr    r$   FZnormalized_coordinatesr   r  Trk   rD   )r   rj   rk   r*   utilscreate_meshgridr   r+   r,   normalize_pixel_coordinatesr   r1   r   r   r   r   rk   heightwidthgridrG   	grid_normr   r   r   test_tensor_bhw2  s    z.TestNormalizePixelCoordinates.test_tensor_bhw2c                 C   s   t |j}d\}}tjj||d|dj|d}| dd}tjj||d|dj|d}	|	 dd}	tj	j
j||||d}
t|
|	||d	 d S )
NrD  FrE  r  r   r   TrF  rD   )r   rj   rk   r*   rG  rH  r   
contiguousr3  r+   r,   rI  r   rJ  r   r   r   	test_list%  s    z'TestNormalizePixelCoordinates.test_listc           
      C   s`   t jjj}tj|}d\}}t jj||d|dj	|d}||||}||||}	t
||	 d S NrD  TrE  r  )r*   r+   r,   rI  r   r   r   rG  rH  r   r   
r1   r   r   r   r   rK  rL  rM  r   rG   r   r   r   r   4  s    
z&TestNormalizePixelCoordinates.test_jitNro   rp   rq   rO  rQ  r   r   r   r   r   rC    s   rC  c                   @   s$   e Zd Zdd Zdd Zdd ZdS )TestDenormalizePixelCoordinatesc                 C   sf   d\}}t jj||d|dj|d}t jj||d|dj|d}t jj|||}t||ddd d S )NrD  TrE  r  Fr   rD   )r*   rG  rH  r   r+   r,   denormalize_pixel_coordinatesr   r1   r   r   rK  rL  rM  rG   rN  r   r   r   rO  A  s    z0TestDenormalizePixelCoordinates.test_tensor_bhw2c                 C   s   d\}}t jj||d|dj|d}| dd}t jj||d|dj|d}| dd}t jj|||}t	||ddd	 d S )
NrD  TrE  r  r   r   Fr   rD   )
r*   rG  rH  r   rP  r3  r+   r,   rV  r   rW  r   r   r   rQ  M  s    z)TestDenormalizePixelCoordinates.test_listc           
      C   s`   t jjj}tj|}d\}}t jj||d|dj	|d}||||}||||}	t
||	 d S rR  )r*   r+   r,   rV  r   r   r   rG  rH  r   r   rS  r   r   r   r   [  s    
z(TestDenormalizePixelCoordinates.test_jitNrT  r   r   r   r   rU  @  s   rU  c                   @   s&   e Zd Zejdg ddd ZdS )TestRt2Extrinsicsr8   r
  c           	      C   s   t j|dd||d}t j|dd||d}t||}|j|ddfksFJ t|\}}|j|ddfksfJ |j|ddfkszJ t||ddd t||ddd ttjj	jt
|t
|fdd	sJ d S )
Nr    r!   r:   r$   r   h㈵>r   r   Tri   )r   r   r   r/   r   r   r   r*   r+   r,   r   )	r1   r8   r   r   RtZRtZR2t2r   r   r   test_everythingi  s    
z!TestRt2Extrinsics.test_everythingNro   rp   rq   r'   rr   rs   r^  r   r   r   r   rX  h  s   rX  c                   @   s&   e Zd Zejdg ddd ZdS )TestCamtoworldGraphicsToVisionr8   r
  c                 C   s  t jg d||dddd|dd}t jdtjd dg||dd  }tj||dd}t||}t	|}t jg dg d	g d
g dg||dd  |dd}	t
||	ddd t||\}
}t jg dg dg dg||dd  |dd}t jg d||dddd|dd}t
||ddd t
|
|ddd t|}t
||ddd t|
|\}}t
||ddd t
||ddd ttt|ddsJ tt	t|ddsJ d S )Nr   r    r$   r   r:   r    r   rM   rA   )r   r   r   r   )r   r   r   r    )r   r   r   r$   )r   r   r   r:   r   rY  rZ  r   r   r   )r   r   r   )r   r   r   Tri   )r   rE   r3  repeatr*   r_   r+   r   r   r
   r   r   Zreshaper   r   r   r   )r1   r8   r   r   Zt_visanglesZR_visZK_visZK_grafrG   ZR_grafZt_grafZ
expected_R
expected_tZ	Kvis_backZ
R_vis_backZ
t_vis_backr   r   r   r^    s6    ( 
&(z.TestCamtoworldGraphicsToVision.test_everythingNr_  r   r   r   r   r`    s   r`  c                   @   s&   e Zd Zejdg ddd ZdS )TestCamtoworldRtToPoseRtr8   r
  c                 C   sV  t jg d||dddd|dd}t jdtjd dg||dd  }tj||dd}t||\}}t jg dg d	g d
g||dd  |dd}	t jg d||dddd|dd}
t	||	ddd t	||
ddd t
||\}}t	||ddd t	||ddd ttt|t|fdds2J tt
t|t|fddsRJ d S )Nra  r   r:   r    r   rM   rA   rb  )r   r:   r   r:   r   r   )r$   r   r   rY  rZ  Tri   )r   rE   r3  rc  r*   r_   r+   r   r	   r   r   r   r   )r1   r8   r   r   r\  rd  r[  ZRptpZexpected_RpZexpected_tpZRbackZtbackr   r   r   r^    s&    ( &(z(TestCamtoworldRtToPoseRt.test_everythingNr_  r   r   r   r   rf    s   rf  c                   @   s   e Zd Zdd ZdS )TestCARKitToColmapc                 C   s   t jg d||dddd}t jg d||dd  }tjj|}tjj|tj	d}t
||\}}tjjj|tj	d}	tjj|	}	t jg dg||d}
t jdgd	gd
ggg||d}t|	|
ddd t||ddd d S )Nrg  r   r:   r    )-   g      N@rA   r"   )g 8]@rA   gE(QgXgڬ\m?g}гY?r   rY  rZ  )r   rE   r3  r*   r+   r,   r  r-   r   r6   r   rw   r  r   )r1   r   r   r\  Zang_degZang_radZqvecZq_colmapZt_colmapZangles_colmapZexpected_anglesre  r   r   r   r^    s    z"TestCARKitToColmap.test_everythingN)ro   rp   rq   r^  r   r   r   r   rj    s   rj  )2	functoolsr   numpyrO   r'   r   Ztorch.autogradr   r*   Zkornia.geometry.conversionsr   r   r   r   r   r	   r
   r   r   r   Zkornia.testingr   r   r   Zfixturer   r   r   rt   r   r   r   r   r   r  r  rr   rs   r  r  r  r  r2  r8  rC  rU  rX  r`  rf  rj  r   r   r   r   <module>   sN   0

 + % "t + $-1

148)("