a
    d#                     @   s  d dl Z d dlmZmZ d dlZd dlmZmZ ddlm	Z	m
Z
 ddlmZ ddlmZ eejejf Zdejejejeeejd
ddZdejejejeeejd
ddZdejejeej ejdddZdejejejeeejdddZejejejdddZdS )    N)OptionalTuple)_extract_device_dtypesafe_inverse_with_mask   )convert_points_from_homogeneousconvert_points_to_homogeneous)normalize_points)transform_pointsT:0yE>)pts1pts2Hsquaredepsreturnc                 C   s   t |tjstdt| t|jdks>|jdd dksNtd|j | ddkrdt	| } |ddkrzt	|}t
|| }|| djdd	}|r|S ||  S )
ad  Return transfer error in image 2 for correspondences given the homography matrix.

    Args:
        pts1: correspondences from the left images with shape
          (B, N, 2 or 3). If they are homogeneous, converted automatically.
        pts2: correspondences from the right images with shape
          (B, N, 2 or 3). If they are homogeneous, converted automatically.
        H: Homographies with shape :math:`(B, 3, 3)`.
        squared: if True (default), the squared distance is returned.
        eps: Small constant for safe sqrt.

    Returns:
        the computed distance with shape :math:`(B, N)`.
    "H type is not a torch.Tensor. Got    Nr   r   "H must be a (*, 3, 3) tensor. Got    dim)
isinstancetorchTensor	TypeErrortypelenshape
ValueErrorsizer   r
   powsumsqrt)r   r   r   r   r   Z	pts1_in_2Zerror_squared r'   c/var/www/html/stable-diffusion-webui/venv/lib/python3.9/site-packages/kornia/geometry/homography.pyoneway_transfer_error   s     
r)   c                 C   s   t |tjstdt| t|jdks>|jdd dksNtd|j | ddkrdt	| } |ddkrzt	|}t
| jj}t|\}}t| ||d|}t|| |d|}	|dd	|}
||	 |
|j ||
 |j  }|r|S ||  S )
ac  Return Symmetric transfer error for correspondences given the homography matrix.

    Args:
        pts1: correspondences from the left images with shape
          (B, N, 2 or 3). If they are homogeneous, converted automatically.
        pts2: correspondences from the right images with shape
          (B, N, 2 or 3). If they are homogeneous, converted automatically.
        H: Homographies with shape :math:`(B, 3, 3)`.
        squared: if True (default), the squared distance is returned.
        eps: Small constant for safe sqrt.

    Returns:
        the computed distance with shape :math:`(B, N)`.
    r   r   r   Nr   r   r   Tr   )r   r   r   r   r   r    r!   r"   r#   r   Zfinfodtypemaxr   r)   viewZ	expand_astor&   )r   r   r   r   r   Zmax_numZH_invZgood_HZtherebackZgood_H_reshapeoutr'   r'   r(   symmetric_transfer_error5   s"     &r0   )points1points2weightsr   c              
   C   sb  | j |j krt| j t| j dkr2| j d dks<t| j | j d dk rTt| j t| |g\}}d}t| \}}t|\}}	tj|ddd\}
}tj|ddd\}}t|
t|
 }}tj	||||
 | | ||
 || |g	dd}tj	|
|||||| |
 | | | g	dd}tj	||fdd
|j d d|j d }|d	u r^|d
d| }nnt|j dkr|j | j d	d kst|j t|jddddd
|j d d}|d
d| | }zt|\}}}W n< ty   tdt tj|dddf||d Y S 0 |d ddd}|	 ||  }||ddd	dd	f |  }|S )a$  Compute the homography matrix using the DLT formulation.

    The linear system is solved by using the Weighted Least Squares Solution for the 4 Points algorithm.

    Args:
        points1: A set of points in the first image with a tensor shape :math:`(B, N, 2)`.
        points2: A set of points in the second image with a tensor shape :math:`(B, N, 2)`.
        weights: Tensor containing the weights per point correspondence with a shape of :math:`(B, N)`.

    Returns:
        the computed homography matrix with shape :math:`(B, 3, 3)`.
    r   r   r      r   )r   chunksr   r   Nr   zSVD did not converger   )devicer*   ).r   .)r!   AssertionErrorr    r   r	   r   chunkZ	ones_likeZ
zeros_likecatZreshapeZ	transposeZ
diag_embedZ	unsqueezerepeatZsvdRuntimeErrorwarningswarnRuntimeWarningemptyr#   r,   Zinverse)r1   r2   r3   r6   r*   r   Zpoints1_normZ
transform1Zpoints2_normZ
transform2x1y1Zx2y2ZoneszerosaxZayAZw_diag_Vr   ZH_normr'   r'   r(   find_homography_dlt`   s>    


..(
&
*"rH         @   )r1   r2   r3   soft_inl_thn_iterr   c           	      C   sT   t | ||}t|d D ]6}t| ||d}t| d|d   }t | ||}q|S )a  Compute the homography matrix using the iteratively-reweighted least squares (IRWLS).

    The linear system is solved by using the Reweighted Least Squares Solution for the 4 Points algorithm.

    Args:
        points1: A set of points in the first image with a tensor shape :math:`(B, N, 2)`.
        points2: A set of points in the second image with a tensor shape :math:`(B, N, 2)`.
        weights: Tensor containing the weights per point correspondence with a shape of :math:`(B, N)`.
          Used for the first iteration of the IRWLS.
        soft_inl_th: Soft inlier threshold used for weight calculation.
        n_iter: number of iterations.

    Returns:
        the computed homography matrix with shape :math:`(B, 3, 3)`.
    r   Fg       @r   )rH   ranger0   r   exp)	r1   r2   r3   rK   rL   r   rF   errorsZweights_newr'   r'   r(   find_homography_dlt_iterated   s    rP   )r1   r2   r   c                 C   s  | j |j krt| j t| j dkr2| j d dks<t| j | j d dkrTt| j | j}tjg dg dg dg dgtj|d	}t| }t|}|d
d
|f }|d
d
|f }t|dddd
d
f |dddd
d
f |dddd
d
f 	dddd 
 }t|dddd
d
f |dddd
d
f |dddd
d
f 	dddd 
 }	||	kddjddd }
|
S )a  Function, which implements oriented constraint check from :cite:`Marquez-Neila2015`.

    Analogous to https://github.com/opencv/opencv/blob/4.x/modules/calib3d/src/usac/degeneracy.cpp#L88

    Args:
        points1: A set of points in the first image with a tensor shape :math:`(B, 4, 2)`.
        points2: A set of points in the second image with a tensor shape :math:`(B, 4, 2)`.

    Returns:
        Mask with the minimal sample is good for homography estimation:math:`(B, 3, 3)`.
    r   r   r   r4   )r   r   r   )r   r   r   )r   r   r   )r   r   r   )r*   r6   N.r   r   r   )r!   r7   r    r6   r   Ztensorlongr   ZcrossZpermutesignr,   min)r1   r2   r6   Zidx_permZpoints_src_hZpoints_dst_hZsrc_permZdst_permZ	left_signZ
right_signZsample_is_validr'   r'   r(   sample_is_valid_for_homography   s$    


*PPrT   )Tr   )Tr   )N)rI   rJ   )r<   typingr   r   r   Zkornia.utilsr   r   Zconversionsr   r   Zepipolarr	   Zlinalgr
   r   ZTupleTensorboolfloatr)   r0   rH   intrP   rT   r'   r'   r'   r(   <module>   s8    ' , < 