o
    oi                     @  s  d dl mZ d dlmZ d dlZd dlm  m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mZ d dlmZ d dlmZ g d	ZdddZdddZdddZddddZ ddddZ!dd d!Z"dd#d$Z#dd%d&Z$dd'd(Z%dd*d+Z&ed+d,d-dd.d/Z'dd1d2Z(ed2d,d-dd3d4Z)ddd5d6Z*ddd9d:Z+dd;d<Z,dd=d>Z-ed>d,d-dd?d@Z.dddAdBZ/dddCdDZ0ddEdFZ1edFd,d-ddGdHZ2ddLdMZ3ddRdSZ4dddXdYZ5dddZd[Z6	ddd]d^Z7	ddd_d`Z8ddbdcZ9ddhdiZ:	j		dddodpZ;	j		dddqdrZ<ddsdtZ=ddvdwZ>ddzd{Z?dd}d~Z@dddZAdddZBdddZCdddZDdddZEdddZFdddZGdddZHdddZIdddZJdS )    )annotations)OptionalN)pi)	Tensorconcatenatecospadsinstacktensorwhere
zeros_like)KORNIA_CHECKKORNIA_CHECK_SHAPE)
deprecated)_torch_inverse_cast)-ARKitQTVecs_to_ColmapQTVecsRt_to_matrix4x4angle_axis_to_quaternionangle_axis_to_rotation_matrixangle_to_rotation_matrixaxis_angle_to_quaternionaxis_angle_to_rotation_matrix!camtoworld_graphics_to_vision_4x4 camtoworld_graphics_to_vision_Rtcamtoworld_to_worldtocam_Rt!camtoworld_vision_to_graphics_4x4 camtoworld_vision_to_graphics_Rtcart2pol"convert_affinematrix_to_homography$convert_affinematrix_to_homography3dconvert_points_from_homogeneousconvert_points_to_homogeneousdeg2raddenormalize_homographydenormalize_pixel_coordinatesdenormalize_pixel_coordinates3d"denormalize_points_with_intrinsicseuler_from_quaternionmatrix4x4_to_Rtnormal_transform_pixelnormal_transform_pixel3dnormalize_homographynormalize_homography3dnormalize_pixel_coordinatesnormalize_pixel_coordinates3d normalize_points_with_intrinsicsnormalize_quaternionpol2cartquaternion_exp_to_logquaternion_from_eulerquaternion_log_to_expquaternion_to_angle_axisquaternion_to_axis_anglequaternion_to_rotation_matrixrad2degrotation_matrix_to_angle_axisrotation_matrix_to_axis_anglerotation_matrix_to_quaternionvector_to_skew_symmetric_matrixworldtocam_to_camtoworld_Rtr   r   returnc                 C  s8   t | tstdt|  d|  t| j| j S )a  Convert angles from radians to degrees.

    Args:
        tensor: Tensor of arbitrary shape.

    Returns:
        Tensor with same shape as input.

    Example:
        >>> input = tensor(3.1415926535)
        >>> rad2deg(input)
        tensor(180.)

     Input type is not a Tensor. Got      f@
isinstancer   	TypeErrortyper   todevicedtyper    rJ   O/home/ubuntu/.local/lib/python3.10/site-packages/kornia/geometry/conversions.pyr9   P      
r9   c                 C  s8   t | tstdt|  | t| j| j d S )zConvert angles from degrees to radians.

    Args:
        tensor: Tensor of arbitrary shape.

    Returns:
        tensor with same shape as input.

    Examples:
        >>> input = tensor(180.)
        >>> deg2rad(input)
        tensor(3.1416)

    r@   rA   rB   rI   rJ   rJ   rK   r#   e   rL   r#   rhophituple[Tensor, Tensor]c                 C  sP   t | tt |t@ stdt|  dt| | t| }| t| }||fS )a  Convert polar coordinates to cartesian coordinates.

    Args:
        rho: Tensor of arbitrary shape.
        phi: Tensor of same arbitrary shape.

    Returns:
        - x: Tensor with same shape as input.
        - y: Tensor with same shape as input.

    Example:
        >>> rho = torch.rand(1, 3, 3)
        >>> phi = torch.rand(1, 3, 3)
        >>> x, y = pol2cart(rho, phi)

    r@   , )rC   r   rD   rE   r   r	   )rM   rN   xyrJ   rJ   rK   r2   z   s
   r2   :0yE>rQ   rR   epsfloatc                 C  s^   t | tt |t@ stdt|  dt| t| d |d  | }t|| }||fS )a  Convert cartesian coordinates to polar coordinates.

    Args:
        x: Tensor of arbitrary shape.
        y: Tensor of same arbitrary shape.
        eps: To avoid division by zero.

    Returns:
        - rho: Tensor with same shape as input.
        - phi: Tensor with same shape as input.

    Example:
        >>> x = torch.rand(1, 3, 3)
        >>> y = torch.rand(1, 3, 3)
        >>> rho, phi = cart2pol(x, y)

    r@   rP      )rC   r   rD   rE   torchsqrtatan2)rQ   rR   rT   rM   rN   rJ   rJ   rK   r      s
   r   pointsc                 C  s   t | tstdt|  t| jdk rtd| j | dddf }t||k}t	|d||  t
|}|| dddf  S )a  Convert points from homogeneous to Euclidean space.

    Args:
        points: the points to be transformed of shape :math:`(B, N, D)`.
        eps: to avoid division by zero.

    Returns:
        the points in Euclidean space :math:`(B, N, D-1)`.

    Examples:
        >>> input = tensor([[0., 0., 1.]])
        >>> convert_points_from_homogeneous(input)
        tensor([[0., 0.]])

    r@   rV   (Input must be at least a 2D tensor. Got .N      ?)rC   r   rD   rE   lenshape
ValueErrorrW   absr   	ones_like)rZ   rT   z_vecmaskscalerJ   rJ   rK   r!      s   
r!   c                 C  sL   t | tstdt|  t| jdk rtd| j t| ddgddS )af  Convert points from Euclidean to homogeneous space.

    Args:
        points: the points to be transformed with shape :math:`(*, N, D)`.

    Returns:
        the points in homogeneous coordinates :math:`(*, N, D+1)`.

    Examples:
        >>> input = tensor([[0., 0.]])
        >>> convert_points_to_homogeneous(input)
        tensor([[0., 0., 1.]])

    r@   rV   r[   r      constantr]   )rC   r   rD   rE   r^   r_   r`   r   )rZ   rJ   rJ   rK   r"      s
   
r"   Ac                 C  s(   t | g dddd}|d  d7  < |S )N)r   r   r   rf   rg           )value).r\   r\   r]   )r   )rh   HrJ   rJ   rK   (_convert_affinematrix_to_homography_impl   s   rl   c                 C  T   t | tstdt|  t| jdkr| jdd dks&td| j t| S )a  Convert batch of affine matrices.

    Args:
        A: the affine matrix with shape :math:`(B,2,3)`.

    Returns:
         the homography matrix with shape of :math:`(B,3,3)`.

    Examples:
        >>> A = tensor([[[1., 0., 0.],
        ...                    [0., 1., 0.]]])
        >>> convert_affinematrix_to_homography(A)
        tensor([[[1., 0., 0.],
                 [0., 1., 0.],
                 [0., 0., 1.]]])

    r@      N)rV   rn   z)Input matrix must be a Bx2x3 tensor. Got rC   r   rD   rE   r^   r_   r`   rl   rh   rJ   rJ   rK   r      s
   
 r   c                 C  rm   )a  Convert batch of 3d affine matrices.

    Args:
        A: the affine matrix with shape :math:`(B,3,4)`.

    Returns:
         the homography matrix with shape of :math:`(B,4,4)`.

    Examples:
        >>> A = tensor([[[1., 0., 0., 0.],
        ...                    [0., 1., 0., 0.],
        ...                    [0., 0., 1., 0.]]])
        >>> convert_affinematrix_to_homography3d(A)
        tensor([[[1., 0., 0., 0.],
                 [0., 1., 0., 0.],
                 [0., 0., 1., 0.],
                 [0., 0., 0., 1.]]])

    r@   rn   ro   N)rn      z)Input matrix must be a Bx3x4 tensor. Got rp   rq   rJ   rJ   rK   r      s
   
 r    
axis_anglec                 C  s   t | tstdt|  | jd dkstd| j dddd}ddd}| |  jdd}|| |}|| }|dkddd}t	|||}|S )a  Convert 3d vector of axis-angle rotation to 3x3 rotation matrix.

    Args:
        axis_angle: tensor of 3d vector of axis-angle rotations in radians with shape :math:`(N, 3)`.

    Returns:
        tensor of rotation matrices of shape :math:`(N, 3, 3)`.

    Example:
        >>> input = tensor([[0., 0., 0.]])
        >>> axis_angle_to_rotation_matrix(input)  # doctest: +ELLIPSIS
        tensor([[[1., ...0., 0.],
                 [0., 1., ...0.],
                 [...0., 0., 1.]]])

        >>> input = tensor([[1.5708, 0., 0.]])
        >>> axis_angle_to_rotation_matrix(input)
        tensor([[[ 1.0000e+00,  0.0000e+00,  0.0000e+00],
                 [ 0.0000e+00, -3.6200e-06, -1.0000e+00],
                 [ 0.0000e+00,  1.0000e+00, -3.6200e-06]]])

    r@   r\   rn   z(Input size must be a (*, 3) tensor. Got ư>rs   r   theta2rT   rU   r?   c                 S  sF  t |jdd}| |d|  }|jdd\}}}t |}t |}	d| }
|| }|| }|| }||| |
  }||
 ||	  }||	 ||
  }||	 ||
  }||| |
  }| |	 ||
  }| |	 ||
  }||	 ||
  }||| |
  }t jt j|||gddt j|||gddt j|||gddgdd}|S )N-q=minr\   rf   dimr]   )rW   rX   clamp	unsqueezeunbindr   r	   r
   )rs   ru   rT   thetawxyzwxwywz	cos_theta	sin_thetaone_minus_coswxwywxwzwywzr00r01r02r10r11r12r20r21r22rotrJ   rJ   rK   _compute_rotation_matrixA  s4   

	z?axis_angle_to_rotation_matrix.<locals>._compute_rotation_matrixc              
   S  sN   |  d\}}}t|}tj|| |||| | ||g	ddddd}|S )Nr\   ry   rn   )r}   rW   rb   r
   view)rs   rxryrzk_oner   rJ   rJ   rK   _compute_rotation_matrix_taylore  s$   

zFaxis_angle_to_rotation_matrix.<locals>._compute_rotation_matrix_taylorry   rf   N)rt   )rs   r   ru   r   rT   rU   r?   r   rs   r   r?   r   )
rC   r   rD   rE   r_   r`   sumr   rW   r   )rs   r   r   ru   
rot_normal
rot_taylorrd   rotation_matrixrJ   rJ   rK   r   $  s   

$
r   z0.7.0)replace_withversionc                 C     t | S N)r   rs   rJ   rJ   rK   r        r   r   c                 C  sN   t | tstdt|  | jdd dkstd| j t| }t|S )a  Convert 3x3 rotation matrix to Rodrigues vector in radians.

    Args:
        rotation_matrix: rotation matrix of shape :math:`(N, 3, 3)`.

    Returns:
        Rodrigues vector transformation of shape :math:`(N, 3)`.

    Example:
        >>> input = tensor([[1., 0., 0.],
        ...                       [0., 1., 0.],
        ...                       [0., 0., 1.]])
        >>> rotation_matrix_to_axis_angle(input)
        tensor([0., 0., 0.])

        >>> input = tensor([[1., 0., 0.],
        ...                       [0., 0., -1.],
        ...                       [0., 1., 0.]])
        >>> rotation_matrix_to_axis_angle(input)
        tensor([1.5708, 0.0000, 0.0000])

    r@   ro   Nrn   rn   +Input size must be a (*, 3, 3) tensor. Got )rC   r   rD   rE   r_   r`   r<   r7   )r   
quaternionrJ   rJ   rK   r;     s   
r;   c                 C  r   r   )r;   )r   rJ   rJ   rK   r:     r   r:   c           
        s\  t | tstdt|  | jdd dkstd| j dd
d
| jg | jdd dR  }tj|ddd\		 	 d 
f	dd}d 	
fdd}d 	
fdd}d 	
fdd}t		k| | }t	k	k@ | |}t	dk| |}	|	S )af  Convert 3x3 rotation matrix to 4d quaternion vector.

    The quaternion vector has components in (w, x, y, z) format.

    Args:
        rotation_matrix: the rotation matrix to convert with shape :math:`(*, 3, 3)`.
        eps: small value to avoid zero division.

    Return:
        the rotation in quaternion with shape :math:`(*, 4)`.

    Example:
        >>> input = tensor([[1., 0., 0.],
        ...                       [0., 1., 0.],
        ...                       [0., 0., 1.]])
        >>> rotation_matrix_to_quaternion(input, eps=torch.finfo(input.dtype).eps)
        tensor([1., 0., 0., 0.])

    r@   ro   Nr   r   	numeratorr   denominatorr?   c                 S  s    t | jj}| t j||d S )Nrw   )rW   finforH   tinyr{   )r   r   rT   rJ   rJ   rK   safe_zero_division  s   z9rotation_matrix_to_quaternion.<locals>.safe_zero_division	   r\   )chunksrz   c                    s\   t d   d } d|  } | } | } | }t||||fddS Nr]          @g      ?r\   ry   rW   rX   r   sqqwqxqyqz)	rT   m01m02m10m12m20m21r   tracerJ   rK   trace_positive_cond  s   z:rotation_matrix_to_quaternion.<locals>.trace_positive_condc                    sd   t d  	   d } 
 | }d|  }
 | }
 | }t||||fddS r   r   r   rT   m00r   r   r   m11r   r   r   m22r   rJ   rK   cond_1  s   z-rotation_matrix_to_quaternion.<locals>.cond_1c                    sd   t d  	   d } 
 | }
 | }d|  }
 | }t||||fddS r   r   r   r   rJ   rK   cond_2  s   z-rotation_matrix_to_quaternion.<locals>.cond_2c                    sd   t d	     d } 
 | }
 | }
 | }d|  }t||||fddS r   r   r   r   rJ   rK   cond_3  s   z-rotation_matrix_to_quaternion.<locals>.cond_3ri   )r   r   r   r   r?   r   )r?   r   )
rC   r   rD   rE   r_   r`   reshaperW   chunkr   )
r   rT   rotation_matrix_vecr   r   r   r   where_2where_1r   rJ   )rT   r   r   r   r   r   r   r   r   r   r   r   rK   r<     s    

""""r<   rv   r   c                 C  sL   t | tstdt|  | jd dkstd| j tj| dd|dS )a  Normalize a quaternion.

    The quaternion should be in (x, y, z, w) or (w, x, y, z) format.

    Args:
        quaternion: a tensor containing a quaternion to be normalized.
          The tensor can be of shape :math:`(*, 4)`.
        eps: small value to avoid division by zero.

    Return:
        the normalized quaternion of shape :math:`(*, 4)`.

    Example:
        >>> quaternion = tensor((1., 0., 1., 0.))
        >>> normalize_quaternion(quaternion)
        tensor([0.7071, 0.0000, 0.7071, 0.0000])

    r@   r\   rr   ,Input must be a tensor of shape (*, 4). Got r   )prz   rT   )rC   r   rD   rE   r_   r`   F	normalize)r   rT   rJ   rJ   rK   r1     s
   
r1   c                 C  sD  t | tstdt|  | jd dkstd| j t| }|d }|d }|d }|d }d	| }d	| }d	| }|| }	|| }
|| }|| }|| }|| }|| }|| }|| }td
}t|||  || ||
 || |||  ||	 ||
 ||	 |||  f	dd}g t	| jdd dd}|
|}|S )a  Convert a quaternion to a rotation matrix.

    The quaternion should be in (w, x, y, z) format.

    Args:
        quaternion: a tensor containing a quaternion to be converted.
          The tensor can be of shape :math:`(*, 4)`.

    Return:
        the rotation matrix of shape :math:`(*, 3, 3)`.

    Example:
        >>> quaternion = tensor((0., 0., 0., 1.))
        >>> quaternion_to_rotation_matrix(quaternion)
        tensor([[-1.,  0.,  0.],
                [ 0., -1.,  0.],
                [ 0.,  0.,  1.]])

    r@   r\   rr   r   .r   .rf   .rV   .rn   r   r]   ry   Nrn   )rC   r   rD   rE   r_   r`   r1   r   r
   listr   )r   quaternion_normwrQ   rR   ztxtytztwxtwytwztxxtxytxztyytyztzzonematrix_flatoutput_shapematrixrJ   rJ   rK   r8     sL   




r8   c                 C  s@  t | stdt|  | jd dkstd| j tg }tg }tg }tg }| d }| d }| d }| d }|| ||  ||  }t |}d	t|d
k t 	| | t 	|| }|| }d	t 
| }	t|d
k||	}
t | dddf }|d  ||
 7  < |d  ||
 7  < |d  ||
 7  < |S )a<  Convert quaternion vector to axis angle of rotation in radians.

    The quaternion should be in (w, x, y, z) format.

    Adapted from ceres C++ library: ceres-solver/include/ceres/rotation.h

    Args:
        quaternion: tensor with quaternions.

    Return:
        tensor with axis angle of rotation.

    Shape:
        - Input: :math:`(*, 4)` where `*` means, any number of dimensions
        - Output: :math:`(*, 3)`

    Example:
        >>> quaternion = tensor((1., 0., 0., 0.))
        >>> quaternion_to_axis_angle(quaternion)
        tensor([0., 0., 0.])

    r@   r\   rr   z.Input must be a tensor of shape Nx4 or 4. Got r   r   r   r   r   ri   .Nrn   )rW   	is_tensorrD   rE   r_   r`   r   rX   r   rY   rb   r   )r   q1q2q3r   sin_squared_thetar   	two_thetak_posk_negkrs   rJ   rJ   rK   r7   c  s2   

r7   c                 C  r   r   )r7   )r   rJ   rJ   rK   r6     r   r6   c                 C  s   t | tstdt|  | jd dkstd| j tj| ddddj|d}| t	| | }t
|}tg }t||fdd	}|S )
a  Apply exponential map to log quaternion.

    The quaternion should be in (w, x, y, z) format.

    Args:
        quaternion: a tensor containing a quaternion to be converted.
          The tensor can be of shape :math:`(*, 3)`.
        eps: a small number for clamping.

    Return:
        the quaternion exponential map of shape :math:`(*, 4)`.

    Example:
        >>> quaternion = tensor((0., 0., 0.))
        >>> quaternion_log_to_exp(quaternion, eps=torch.finfo(quaternion.dtype).eps)
        tensor([1., 0., 0., 0.])

    r@   r\   rn   z,Input must be a tensor of shape (*, 3). Got rV   Tr   rz   keepdimrw   ry   )rC   r   rD   rE   r_   r`   rW   normr{   r	   r   r   r   )r   rT   norm_qquaternion_vectorquaternion_scalarquaternion_exprJ   rJ   rK   r5     s   
r5   c                 C  s   t | tstdt|  | jd dkstd| j tg }tg }| dddf }| dddf }tj|ddd	d
j	|d}|t
tj	|ddd | }|S )a  Apply the log map to a quaternion.

    The quaternion should be in (w, x, y, z) format.

    Args:
        quaternion: a tensor containing a quaternion to be converted.
          The tensor can be of shape :math:`(*, 4)`.
        eps: a small number for clamping.

    Return:
        the quaternion log map of shape :math:`(*, 3)`.

    Example:
        >>> quaternion = tensor((1., 0., 0., 0.))
        >>> quaternion_exp_to_log(quaternion, eps=torch.finfo(quaternion.dtype).eps)
        tensor([0., 0., 0.])

    r@   r\   rr   r   .r   rf   rV   Tr   rw         r]   rx   max)rC   r   rD   rE   r_   r`   r   rW   r   r{   acos)r   rT   r   r   r   quaternion_logrJ   rJ   rK   r3     s   
r3   c                 C  sN  t | stdt|  | jd dkstd| j | dddf }| dddf }| dddf }|| ||  ||  }t |}|d	 }|d
k}t |}d	| }	t|| }
t	||
|	}t	|t
||}t jg | jdd dR | j| jd}|| |dddf< || |dddf< || |dddf< ||dddf< |S )a=  Convert an axis angle to a quaternion.

    The quaternion vector has components in (w, x, y, z) format.

    Adapted from ceres C++ library: ceres-solver/include/ceres/rotation.h

    Args:
        axis_angle: tensor with axis angle in radians.

    Return:
        tensor with quaternion.

    Shape:
        - Input: :math:`(*, 3)` where `*` means, any number of dimensions
        - Output: :math:`(*, 4)`

    Example:
        >>> axis_angle = tensor((0., 1., 0.))
        >>> axis_angle_to_quaternion(axis_angle)
        tensor([0.8776, 0.0000, 0.4794, 0.0000])

    r@   r\   rn   z.Input must be a tensor of shape Nx3 or 3. Got .r   rf   rV         ?ri   Nrr   )sizerH   rG   )rW   r   rD   rE   r_   r`   rX   rb   r	   r   r   zerosrH   rG   )rs   a0a1a2theta_squaredr~   
half_thetard   onesr   r   r   r   r   rJ   rJ   rK   r     s,   


(r   c                 C  r   r   )r   r   rJ   rJ   rK   r   ,  r   r   r   r   tuple[Tensor, Tensor, Tensor]c                 C  s   t | j|jk t |j|jk t |j|jk || }d| | ||   }dd|| |   }||}d| | ||   }|jddd}| }	d| | ||   }
dd|||    }|
|}||	|fS )a|  Convert a quaternion coefficients to Euler angles.

    Returned angles are in radians in XYZ convention.

    Args:
        w: quaternion :math:`q_w` coefficient.
        x: quaternion :math:`q_x` coefficient.
        y: quaternion :math:`q_y` coefficient.
        z: quaternion :math:`q_z` coefficient.

    Return:
        A tuple with euler angles`roll`, `pitch`, `yaw`.

    r   r]   r   r   )r   r_   rY   r{   asin)r   rQ   rR   r   yy	sinr_cosp	cosr_cosprollsinppitch	siny_cosp	cosy_cospyawrJ   rJ   rK   r(   4  s   


r(   r  r  r  %tuple[Tensor, Tensor, Tensor, Tensor]c                 C  s   t | j|jk t |j|jk | d }|d }|d }| }| }| }| }	| }
| }|| |
 ||	 |  }|| | ||	 |
  }|| | ||	 |
  }|| |
 ||	 |  }||||fS )a=  Convert Euler angles to quaternion coefficients.

    Euler angles are assumed to be in radians in XYZ convention.

    Args:
        roll: the roll euler angle.
        pitch: the pitch euler angle.
        yaw: the yaw euler angle.

    Return:
        A tuple with quaternion coefficients in order of `wxyz`.

    r   )r   r_   r   r	   )r  r  r  	roll_half
pitch_halfyaw_halfcysycpspcrsrr   r   r   r   rJ   rJ   rK   r4   X  s    r4   pixel_coordinatesheightintwidthc                 C  sr   | j d dkrtd| j  tt|| j| jdt|| j| jdg}td| j| jd|d | }||  d S )aM  Normalize pixel coordinates between -1 and 1.

    Normalized, -1 if on extreme left, 1 if on extreme right (x = w-1).

    Args:
        pixel_coordinates: the grid with pixel coordinates. Shape can be :math:`(*, 2)`.
        width: the maximum width in the x-axis.
        height: the maximum height in the y-axis.
        eps: safe division by zero.

    Return:
        the normalized pixel coordinates with shape :math:`(*, 2)`.

    Examples:
        >>> coords = tensor([[50., 100.]])
        >>> normalize_pixel_coordinates(coords, 100, 50)
        tensor([[1.0408, 1.0202]])

    r\   rV   5Input pixel_coordinates must be of shape (*, 2). Got rG   rH   r   rf   )r_   r`   r
   r   rG   rH   r{   r  r  r  rT   hwfactorrJ   rJ   rK   r.     s    r.   c                 C  sl   | j d dkrtd| j  tt|t|g| j| j}td|d | }td| | d  S )aH  Denormalize pixel coordinates.

    The input is assumed to be -1 if on extreme left, 1 if on extreme right (x = w-1).

    Args:
        pixel_coordinates: the normalized grid coordinates. Shape can be :math:`(*, 2)`.
        width: the maximum width in the x-axis.
        height: the maximum height in the y-axis.
        eps: safe division by zero.

    Return:
        the denormalized pixel coordinates with shape :math:`(*, 2)`.

    Examples:
        >>> coords = tensor([[-1., -1.]])
        >>> denormalize_pixel_coordinates(coords, 100, 50)
        tensor([[0., 0.]])

    r\   rV   r  r   rf   r]   r_   r`   r
   r   rF   rG   rH   r{   r!  rJ   rJ   rK   r%     s
   $r%   depthc                 C  sj   | j d dkrtd| j  tt|t|t|g| j| j}td|d | }||  d S )a  Normalize pixel coordinates between -1 and 1.

    Normalized, -1 if on extreme left, 1 if on extreme right (x = w-1).

    Args:
        pixel_coordinates: the grid with pixel coordinates. Shape can be :math:`(*, 3)`.
        depth: the maximum depth in the z-axis.
        height: the maximum height in the y-axis.
        width: the maximum width in the x-axis.
        eps: safe division by zero.

    Return:
        the normalized pixel coordinates.

    r\   rn   5Input pixel_coordinates must be of shape (*, 3). Got r   rf   r$  r  r%  r  r  rT   dhwr#  rJ   rJ   rK   r/     s   (r/   c                 C  sr   | j d dkrtd| j  tt|t|t|g| j| j}td|d | }td| | d  S )a  Denormalize pixel coordinates.

    The input is assumed to be -1 if on extreme left, 1 if on extreme right (x = w-1).

    Args:
        pixel_coordinates: the normalized grid coordinates. Shape can be :math:`(*, 3)`.
        depth: the maximum depth in the x-axis.
        height: the maximum height in the y-axis.
        width: the maximum width in the x-axis.
        eps: safe division by zero.

    Return:
        the denormalized pixel coordinates.

    r\   rn   r&  r   rf   r]   r$  r'  rJ   rJ   rK   r&     s   (r&   anglec                 C  sD   t | }t|}t|}t||| |gddjg | jddR  S )aP  Create a rotation matrix out of angles in degrees.

    Args:
        angle: tensor of angles in degrees, any shape :math:`(*)`.

    Returns:
        tensor of rotation matrices with shape :math:`(*, 2, 2)`.

    Example:
        >>> input = torch.rand(1, 3)  # Nx3
        >>> output = angle_to_rotation_matrix(input)  # Nx3x2x2

    r\   ry   rV   )r#   r   r	   r
   r   r_   )r)  ang_radcos_asin_arJ   rJ   rK   r     s   ,r   dst_pix_trans_src_pix	dsize_srctuple[int, int]	dsize_dstc                 C  s   t | tstdt|  t| jdks&| jdd dks&td| j |\}}|\}}t||| }t	|}t||| }	|	| |  }
|
S )a  Normalize a given homography in pixels to [-1, 1].

    Args:
        dst_pix_trans_src_pix: homography/ies from source to destination to be
          normalized. :math:`(B, 3, 3)`
        dsize_src: size of the source image (height, width).
        dsize_dst: size of the destination image (height, width).

    Returns:
        the normalized homography of shape :math:`(B, 3, 3)`.

    r@   rn   ro   Nr   8Input dst_pix_trans_src_pix must be a Bx3x3 tensor. Got 
rC   r   rD   rE   r^   r_   r`   r*   rF   r   )r-  r.  r0  src_hsrc_wdst_hdst_wsrc_norm_trans_src_pixsrc_pix_trans_src_normdst_norm_trans_dst_pixdst_norm_trans_src_normrJ   rJ   rK   r,     s   
 r,   +=rG   Optional[torch.device]rH   Optional[torch.dtype]c                 C  sz   t g dg dg dg||d}|dkr|n|d }| dkr |n| d }|d d | |d< |d	 d | |d	< |d
S )ac  Compute the normalization matrix from image size in pixels to [-1, 1].

    Args:
        height: image height.
        width: image width.
        eps: epsilon to prevent divide-by-zero errors
        device: device to place the result on.
        dtype: dtype of the result.

    Returns:
        normalized transform with shape :math:`(1, 3, 3)`.

    )r]   ri   r   )ri   r]   r   )ri   ri   r]   r   rf   r]   r   r   r   rf   rf   r   r   r|   )r  r  rT   rG   rH   tr_matwidth_denomheight_denomrJ   rJ   rK   r*   6  s    
r*   c           
      C  s   t g dg dg dg dg||d}|dkr|n|d }|dkr#|n|d }| dkr-|n| d }	|d d	 | |d< |d
 d	 | |d
< |d d	 |	 |d< |dS )a  Compute the normalization matrix from image size in pixels to [-1, 1].

    Args:
        depth: image depth.
        height: image height.
        width: image width.
        eps: epsilon to prevent divide-by-zero errors
        device: device to place the result on.
        dtype: dtype of the result.

    Returns:
        normalized transform with shape :math:`(1, 4, 4)`.

    )r]   ri   ri   r   )ri   r]   ri   r   )ri   ri   r]   r   )ri   ri   ri   r]   r   rf   r]   r>  r   r?  )rV   rV   r   r@  )
r%  r  r  rT   rG   rH   rA  rB  rC  depth_denomrJ   rJ   rK   r+   V  s   
r+   c                 C  s   t | tstdt|  t| jdks&| jdd dks&td| j |\}}|\}}t||| }t||| }t	|}	|	| |  }
|
S )a  De-normalize a given homography in pixels from [-1, 1] to actual height and width.

    Args:
        dst_pix_trans_src_pix: homography/ies from source to destination to be
          denormalized. :math:`(B, 3, 3)`
        dsize_src: size of the source image (height, width).
        dsize_dst: size of the destination image (height, width).

    Returns:
        the denormalized homography of shape :math:`(B, 3, 3)`.

    r@   rn   ro   Nr   r1  r2  )r-  r.  r0  r3  r4  r5  r6  r7  r9  dst_denorm_trans_dst_pixr:  rJ   rJ   rK   r$   ~  s   
 r$   tuple[int, int, int]c                 C  s   t | tstdt|  t| jdks&| jdd dks&td| j |\}}}|\}}}t|||| }	t	|	}
t|||| }|| |
  }|S )a  Normalize a given homography in pixels to [-1, 1].

    Args:
        dst_pix_trans_src_pix: homography/ies from source to destination to be
          normalized. :math:`(B, 4, 4)`
        dsize_src: size of the source image (depth, height, width).
        dsize_dst: size of the destination image (depth, height, width).

    Returns:
        the normalized homography.

    Shape:
        Output: :math:`(B, 4, 4)`

    r@   rn   ro   N)rr   rr   r1  )
rC   r   rD   rE   r^   r_   r`   r+   rF   r   )r-  r.  r0  src_dr3  r4  dst_dr5  r6  r7  r8  r9  r:  rJ   rJ   rK   r-     s   
 

r-   point_2dcamera_matrixc                 C  s   t | ddg t |g d |ddddf }|dddddf jddd	}t|jt| jk r<|d|d}}| | | }|S )
a  Normalize points with intrinsics. Useful for conversion of keypoints to be used with essential matrix.

    Args:
        point_2d: tensor containing the 2d points in the image pixel coordinates. The shape of the tensor can be
                  :math:`(*, 2)`.
        camera_matrix: tensor containing the intrinsics camera matrix. The tensor shape must be :math:`(*, 3, 3)`.

    Returns:
        tensor of (u, v) cam coordinates with shape :math:`(*, 2)`.

    Example:
        >>> _ = torch.manual_seed(0)
        >>> X = torch.rand(1, 2)
        >>> K = torch.eye(3)[None]
        >>> normalize_points_with_intrinsics(X, K)
        tensor([[0.4963, 0.7682]])

    *2rK  3rN  .NrV   ro   r\   )dim1dim2)r   diagonalr^   r_   r|   )rI  rJ  cxcyfxfyxyrJ   rJ   rK   r0     s    r0   point_2d_normc           
      C  s   t | ddg t |g d | d }| d }|d }|d }|d }|d	 }t|jt|jk rF|d
|d
|d
|d
f\}}}}|| | }|| | }	t||	gd
dS )a  Normalize points with intrinsics. Useful for conversion of keypoints to be used with essential matrix.

    Args:
        point_2d_norm: tensor containing the 2d points in the image pixel coordinates. The shape of the tensor can be
                       :math:`(*, 2)`.
        camera_matrix: tensor containing the intrinsics camera matrix. The tensor shape must be :math:`(*, 3, 3)`.

    Returns:
        tensor of (u, v) cam coordinates with shape :math:`(*, 2)`.

    Example:
        >>> _ = torch.manual_seed(0)
        >>> X = torch.rand(1, 2)
        >>> K = torch.eye(3)[None]
        >>> denormalize_points_with_intrinsics(X, K)
        tensor([[0.4963, 0.7682]])

    rK  rL  rM  r   r   ).r   r   ).rf   rf   ).r   rV   ).rf   rV   r\   ry   )r   r^   r_   r|   r
   )
rU  rJ  x_coordy_coordfxfycxr  u_coordv_coordrJ   rJ   rK   r'     s   ,r'   Rtc                 C  s4   t | g d t |g d t| |gdd}t|S )a  Combine 3x3 rotation matrix R and 1x3 translation vector t into 4x4 extrinsics.

    Args:
        R: Rotation matrix, :math:`(B, 3, 3).`
        t: Translation matrix :math:`(B, 3, 1)`.

    Returns:
        the extrinsics :math:`(B, 4, 4)`.

    Example:
        >>> R, t = torch.eye(3)[None], torch.ones(3).reshape(1, 3, 1)
        >>> Rt_to_matrix4x4(R, t)
        tensor([[[1., 0., 0., 1.],
                 [0., 1., 0., 1.],
                 [0., 0., 1., 1.],
                 [0., 0., 0., 1.]]])

    BrN  rN  r`  rN  1rV   ry   )r   r   r    )r]  r^  RtrJ   rJ   rK   r     s   r   
extrinsicsc                 C  sL   t | g d | ddddddf | ddddddf }}||fS )a  Convert 4x4 extrinsics into 3x3 rotation matrix R and 1x3 translation vector ts.

    Args:
        extrinsics: pose matrix :math:`(B, 4, 4)`.

    Returns:
        R: Rotation matrix, :math:`(B, 3, 3).`
        t: Translation matrix :math:`(B, 3, 1)`.

    Example:
        >>> ext = torch.eye(4)[None]
        >>> matrix4x4_to_Rt(ext)
        (tensor([[[1., 0., 0.],
                 [0., 1., 0.],
                 [0., 0., 1.]]]), tensor([[[0.],
                 [0.],
                 [0.]]]))

    r`  4rf  Nrn   )r   )rd  r]  r^  rJ   rJ   rK   r)   .  s   6r)   extrinsics_graphicsc                 C  B   t | g d tg dg dg dg dgg| j| jd}| | S )a  Convert graphics coordinate frame (e.g. OpenGL) to vision coordinate frame (e.g. OpenCV.).

    I.e. flips y and z axis. Graphics convention: [+x, +y, +z] == [right, up, backwards].
    Vision convention: [+x, +y, +z] == [right, down, forwards].

    Args:
        extrinsics_graphics: pose matrix :math:`(B, 4, 4)`.

    Returns:
        extrinsics: pose matrix :math:`(B, 4, 4)`.

    Example:
        >>> ext = torch.eye(4)[None]
        >>> camtoworld_graphics_to_vision_4x4(ext)
        tensor([[[ 1.,  0.,  0.,  0.],
                 [ 0., -1.,  0.,  0.],
                 [ 0.,  0., -1.,  0.],
                 [ 0.,  0.,  0.,  1.]]])

    re  rf   r   r   r   r   r\   r   r   r   r   r\   r   r   r   r   r]   rH   rG   r   r   rH   rG   )rg  	invert_yzrJ   rJ   rK   r   G     r   c                 C  2   t | g d t |g d tt| |}t|S )a  Convert graphics coordinate frame (e.g. OpenGL) to vision coordinate frame (e.g. OpenCV.).

    I.e. flips y and z axis. Graphics convention: [+x, +y, +z] == [right, up, backwards].
    Vision convention: [+x, +y, +z] == [right, down, forwards].

    Args:
        R: Rotation matrix, :math:`(B, 3, 3).`
        t: Translation matrix :math:`(B, 3, 1)`.

    Returns:
        R: Rotation matrix, :math:`(B, 3, 3).`
        t: Translation matrix :math:`(B, 3, 1)`.

    Example:
        >>> R, t = torch.eye(3)[None], torch.ones(3).reshape(1, 3, 1)
        >>> camtoworld_graphics_to_vision_Rt(R, t)
        (tensor([[[ 1.,  0.,  0.],
                 [ 0., -1.,  0.],
                 [ 0.,  0., -1.]]]), tensor([[[1.],
                 [1.],
                 [1.]]]))

    r_  ra  )r   r   r   r)   r]  r^  mat4x4rJ   rJ   rK   r   e     r   extrinsics_visionc                 C  rh  )a  Convert vision coordinate frame (e.g. OpenCV) to graphics coordinate frame (e.g. OpenGK.).

    I.e. flips y and z axis Graphics convention: [+x, +y, +z] == [right, up, backwards].
    Vision convention: [+x, +y, +z] == [right, down, forwards].

    Args:
        extrinsics_vision: pose matrix :math:`(B, 4, 4)`.

    Returns:
        extrinsics: pose matrix :math:`(B, 4, 4)`.

    Example:
        >>> ext = torch.eye(4)[None]
        >>> camtoworld_vision_to_graphics_4x4(ext)
        tensor([[[ 1.,  0.,  0.,  0.],
                 [ 0., -1.,  0.,  0.],
                 [ 0.,  0., -1.,  0.],
                 [ 0.,  0.,  0.,  1.]]])

    re  ri  rj  rk  rl  rm  rn  )ru  ro  rJ   rJ   rK   r     rp  r   c                 C  rq  )a  Convert graphics coordinate frame (e.g. OpenGL) to vision coordinate frame (e.g. OpenCV.).

    I.e. flips y and z axis. Graphics convention: [+x, +y, +z] == [right, up, backwards].
    Vision convention: [+x, +y, +z] == [right, down, forwards]

    Args:
        R: Rotation matrix, :math:`(B, 3, 3).`
        t: Translation matrix :math:`(B, 3, 1)`.

    Returns:
        R: Rotation matrix, :math:`(B, 3, 3).`
        t: Translation matrix :math:`(B, 3, 1)`.

    Example:
        >>> R, t = torch.eye(3)[None], torch.ones(3).reshape(1, 3, 1)
        >>> camtoworld_vision_to_graphics_Rt(R, t)
        (tensor([[[ 1.,  0.,  0.],
                 [ 0., -1.,  0.],
                 [ 0.,  0., -1.]]]), tensor([[[1.],
                 [1.],
                 [1.]]]))

    r_  ra  )r   r   r   r)   rr  rJ   rJ   rK   r     rt  r   c                 C  :   t | g d t |g d | dd}| | }||fS )a  Convert camtoworld to worldtocam frame used in Colmap.

    See
    long-url: https://colmap.github.io/format.html#output-format

    Args:
        R: Rotation matrix, :math:`(B, 3, 3).`
        t: Translation matrix :math:`(B, 3, 1)`.

    Returns:
        Rinv: Rotation matrix, :math:`(B, 3, 3).`
        tinv: Translation matrix :math:`(B, 3, 1)`.

    Example:
        >>> R, t = torch.eye(3)[None], torch.ones(3).reshape(1, 3, 1)
        >>> camtoworld_to_worldtocam_Rt(R, t)
        (tensor([[[1., 0., 0.],
                 [0., 1., 0.],
                 [0., 0., 1.]]]), tensor([[[-1.],
                 [-1.],
                 [-1.]]]))

    r_  ra  rf   rV   r   	transposer]  r^  R_invnew_trJ   rJ   rK   r     s
   
r   c                 C  rv  )aA  Convert worldtocam frame used in Colmap to camtoworld.

    Args:
        R: Rotation matrix, :math:`(B, 3, 3).`
        t: Translation matrix :math:`(B, 3, 1)`.

    Returns:
        Rinv: Rotation matrix, :math:`(B, 3, 3).`
        tinv: Translation matrix :math:`(B, 3, 1)`.

    Example:
        >>> R, t = torch.eye(3)[None], torch.ones(3).reshape(1, 3, 1)
        >>> worldtocam_to_camtoworld_Rt(R, t)
        (tensor([[[1., 0., 0.],
                 [0., 1., 0.],
                 [0., 0., 1.]]]), tensor([[[-1.],
                 [-1.],
                 [-1.]]]))

    r_  ra  rf   rV   rw  ry  rJ   rJ   rK   r>     s
   
r>   qvectvecc                 C  sF   t | }t||\}}t||\}}|ddd}t| }||fS )a  Convert output of Apple ARKit screen pose to the camera-to-world transformation, expected by Colmap.

    Both poses in quaternion representation.

    Args:
        qvec: ARKit rotation quaternion :math:`(B, 4)`, [w, x, y, z] format.
        tvec: translation vector :math:`(B, 3, 1)`, [x, y, z]

    Returns:
        qvec: Colmap rotation quaternion :math:`(B, 4)`, [w, x, y, z] format.
        tvec: translation vector :math:`(B, 3, 1)`, [x, y, z]

    Example:
        >>> q, t = tensor([0, 1, 0, 1.])[None], torch.ones(3).reshape(1, 3, 1)
        >>> ARKitQTVecs_to_ColmapQTVecs(q, t)
        (tensor([[0.7071, 0.0000, 0.7071, 0.0000]]), tensor([[[-1.0000],
                 [-1.0000],
                 [ 1.0000]]]))

    r\   rn   rf   )r8   r   r   r   r<   
contiguous)r|  r}  RcgRcvTcvR_colmapt_colmapq_colmaprJ   rJ   rK   r     s   r   vecc                 C  s   | j d dkst| j dkrtd| j  | d | d | d }}}t|}tt|| |gddt||| gddt| ||gddgd	d}|S )
aK  Convert a vector to a skew symmetric matrix.

    A vector :math:`(v1, v2, v3)` has a corresponding skew-symmetric matrix, which is of the form:

    .. math::
        \begin{bmatrix} 0 & -v3 & v2 \\
        v3 & 0 & -v1 \\
        -v2 & v1 & 0\end{bmatrix}

    Args:
        vec: tensor of shape :math:`(B, 3)`.

    Returns:
        tensor of shape :math:`(B, 3, 3)`.

    Example:
        >>> vec = torch.tensor([1.0, 2.0, 3.0])
        >>> vector_to_skew_symmetric_matrix(vec)
        tensor([[ 0., -3.,  2.],
                [ 3.,  0., -1.],
                [-2.,  1.,  0.]])

    r\   rn   rV   z2Input vector must be of shape (B, 3) or (3,). Got r   r   r   ry   ro   )r_   r^   r`   r   r
   )r  v1v2v3r   skew_symmetric_matrixrJ   rJ   rK   r=     s   :r=   )r   r   r?   r   )rM   r   rN   r   r?   rO   )rS   )rQ   r   rR   r   rT   rU   r?   rO   )rZ   r   rT   rU   r?   r   )rZ   r   r?   r   )rh   r   r?   r   r   )r   r   r?   r   )r   r   rT   rU   r?   r   )rv   )r   r   rT   rU   r?   r   )r   r   r?   r   )
r   r   rQ   r   rR   r   r   r   r?   r  )r  r   r  r   r  r   r?   r  )
r  r   r  r  r  r  rT   rU   r?   r   )r  r   r%  r  r  r  r  r  rT   rU   r?   r   )r)  r   r?   r   )r-  r   r.  r/  r0  r/  r?   r   )r;  NN)r  r  r  r  rT   rU   rG   r<  rH   r=  r?   r   )r%  r  r  r  r  r  rT   rU   rG   r<  rH   r=  r?   r   )r-  r   r.  rF  r0  rF  r?   r   )rI  r   rJ  r   r?   r   )rU  r   rJ  r   r?   r   )r]  r   r^  r   r?   r   )rd  r   r?   rO   )rg  r   r?   r   )r]  r   r^  r   r?   rO   )ru  r   r?   r   )r|  r   r}  r   r?   rO   )r  r   r?   r   )K
__future__r   typingr   rW   torch.nn.functionalnn
functionalr   kornia.constantsr   kornia.corer   r   r   r   r	   r
   r   r   r   kornia.core.checkr   r   kornia.utilsr   kornia.utils.helpersr   __all__r9   r#   r2   r   r!   r"   rl   r   r    r   r   r;   r:   r<   r1   r8   r7   r6   r5   r3   r   r   r(   r4   r.   r%   r/   r&   r   r,   r*   r+   r$   r-   r0   r'   r   r)   r   r   r   r   r   r>   r   r=   rJ   rJ   rJ   rK   <module>   s   ,
1


"




b

 K
 
H
:'
-
6

$($

'$
(
#
%
"
-






!
