a þd+ã @sBdZddlmZmZddlZddlmZmZddlm Z ddl m Z m Z ddl mZgd ¢Zejejejejd œd d „Zejeejejejfd œdd„Zejejejejejdœdd„Zejeejejfd œdd„Zdejejejejejeejeejejejfdœdd„Zejejejejeejejfdœdd„ZdS)z;Module containing functionalities for the Essential matrix.é)ÚOptionalÚTupleN)Úeye_likeÚvec_likeé)Úcross_product_matrix)Údepth_from_pointÚprojection_from_KRt)Útriangulate_points)Úessential_from_fundamentalÚdecompose_essential_matrixÚessential_from_RtÚmotion_from_essentialÚ%motion_from_essential_choose_solutionÚrelative_camera_motion)ÚF_matÚK1ÚK2ÚreturncCsÖt|jƒdkr |jdd…dks*t|jƒ‚t|jƒdkrJ|jdd…dksTt|jƒ‚t|jƒdkrt|jdd…dks~t|jƒ‚t|jdd…ƒt|jdd…ƒkr¼t|jdd…ƒksÂnt‚| dd¡||S)a¿Get Essential matrix from Fundamental and Camera matrices. Uses the method from Hartley/Zisserman 9.6 pag 257 (formula 9.12). Args: F_mat: The fundamental matrix with shape of :math:`(*, 3, 3)`. K1: The camera matrix from first camera with shape :math:`(*, 3, 3)`. K2: The camera matrix from second camera with shape :math:`(*, 3, 3)`. Returns: The essential matrix with shape :math:`(*, 3, 3)`. ééþÿÿÿN©éréÿÿÿÿ©ÚlenÚshapeÚAssertionErrorÚ transpose)rrr©rúk/var/www/html/stable-diffusion-webui/venv/lib/python3.9/site-packages/kornia/geometry/epipolar/essential.pyr s      @r )ÚE_matrc Cst|jƒdkr|jdd…s&t|jƒ‚t |¡\}}}| dd¡}t |¡}|ddd…fd9<| dd¡}t t |¡dkd|||¡}t t |¡dkd|||¡}t t  gd ¢g¡  |¡ƒ}|d d 7<|||}|| dd¡|} |} | } |ddd…f} | | | fS) aõDecompose an essential matrix to possible rotations and translation. This function decomposes the essential matrix E using svd decomposition [96] and give the possible solutions: :math:`R1, R2, t`. Args: E_mat: The essential matrix in the form of :math:`(*, 3, 3)`. Returns: A tuple containing the first and second possible rotation matrices and the translation vector. The shape of the tensors with be same input :math:`[(*, 3, 3), (*, 3, 3), (*, 3, 1)]`. rrNr.gð¿ç).NN)r"r"çð?).rrr#) rrrÚtorchZsvdrZ ones_likeÚwhereZdetrZtensorZtype_as) r!ÚUÚ_ÚVZVtÚmaskZmasktÚWZU_W_VtZU_Wt_VtÚR1ÚR2ÚTrrr r /s"        r )r+Út1r,Út2rcCsÎt|jƒdkr |jdd…dks*t|jƒ‚t|jƒdkrJ|jdd…dksTt|jƒ‚t|jƒdkrt|jdd…dks~t|jƒ‚t|jƒdkrž|jdd…dks¨t|jƒ‚t||||ƒ\}}t|dƒ}||S)aGet the Essential matrix from Camera motion (Rs and ts). Reference: Hartley/Zisserman 9.6 pag 257 (formula 9.12) Args: R1: The first camera rotation matrix with shape :math:`(*, 3, 3)`. t1: The first camera translation vector with shape :math:`(*, 3, 1)`. R2: The second camera rotation matrix with shape :math:`(*, 3, 3)`. t2: The second camera translation vector with shape :math:`(*, 3, 1)`. Returns: The Essential matrix with the shape :math:`(*, 3, 3)`. rrNr©rr).r)rrrrr)r+r.r,r/ÚRÚtZTxrrr r Zs         r cCspt|jƒdkr |jdd…dks*t|jƒ‚t|ƒ\}}}tj||||gdd}tj|| || gdd}||fS)uóGet Motion (R's and t's ) from Essential matrix. Computes and return four possible poses exist for the decomposition of the Essential matrix. The possible solutions are :math:`[R1,t], [R1,−t], [R2,t], [R2,−t]`. Args: E_mat: The essential matrix in the form of :math:`(*, 3, 3)`. Returns: The rotation and translation containing the four possible combination for the retrieved motion. The tuple is as following :math:`[(*, 4, 3, 3), (*, 4, 3, 1)]`. rrNréýÿÿÿ)Údim)rrrr r$Ústack)r!r+r,r2ÚRsZTsrrr rzs  r)r!rrÚx1Úx2r)rcCsvt|jƒdkr |jdd…dks*t|jƒ‚t|jƒdkrJ|jdd…dksTt|jƒ‚t|jƒdkrt|jdd…dks~t|jƒ‚t|jƒdkrš|jddks¤t|jƒ‚t|jƒdkrÀ|jddksÊt|jƒ‚t|jdd…ƒt|jdd…ƒkr t|jdd…ƒksnt‚|durVt|jƒdkr6t|jƒ‚|j|jdd…krVt|jƒ‚t|jƒdk}|r¤|d}|d}|d}|d}|d}|dur¤|d}t|ƒ\}}td|ƒ} td|ƒ} | dd…df dddd¡} | dd…df dddd¡} |dd…df dddd¡}t|| | ƒ} |} |} |dd…df dddd¡}t|| | ƒ}|dd…df dddd¡}|dd…df dddd¡}t| |||ƒ}t | | |ƒ}t | | |ƒ}|d k|d k@}|durÚ||  d¡M}t j |  d¡dd d d}|dd…|fdd…d d f}|dd…|fdd…d d f}|dd…|fdd…d d f}|rl|d }|d }|d }|||fS) aºRecover the relative camera rotation and the translation from an estimated essential matrix. The method checks the corresponding points in two images and also returns the triangulated 3d points. Internally uses :py:meth:`~kornia.geometry.epipolar.decompose_essential_matrix` and then chooses the best solution based on the combination that gives more 3d points in front of the camera plane from :py:meth:`~kornia.geometry.epipolar.triangulate_points`. Args: E_mat: The essential matrix in the form of :math:`(*, 3, 3)`. K1: The camera matrix from first camera with shape :math:`(*, 3, 3)`. K2: The camera matrix from second camera with shape :math:`(*, 3, 3)`. x1: The set of points seen from the first camera frame in the camera plane coordinates with shape :math:`(*, N, 2)`. x2: The set of points seen from the first camera frame in the camera plane coordinates with shape :math:`(*, N, 2)`. mask: A boolean mask which can be used to exclude some points from choosing the best solution. This is useful for using this function with sets of points of different cardinality (for instance after filtering with RANSAC) while keeping batch semantics. Mask is of shape :math:`(*, N)`. Returns: The rotation and translation plus the 3d triangulated points. The tuple is as following :math:`[(*, 3, 3), (*, 3, 1), (*, N, 3)]`. rrNrrrrér"T)r4Zkeepdimr)rrrrrrÚexpandr r rZ unsqueezer$ÚmaxÚsum)r!rrr7r8r)Z unbatchedr6Útsr+r.ZP1r,r/ZP2ÚXÚd1Zd2Z depth_maskZ mask_indicesZR_outZt_outZ points3d_outrrr r”sl        D            rcCsÌt|jƒdkr |jdd…dks*t|jƒ‚t|jƒdkrJ|jdd…dksTt|jƒ‚t|jƒdkrt|jdd…dks~t|jƒ‚t|jƒdkrž|jdd…dks¨t|jƒ‚|| dd¡}|||}||fS)uCompute the relative camera motion between two cameras. Given the motion parameters of two cameras, computes the motion parameters of the second one assuming the first one to be at the origin. If :math:`T1` and :math:`T2` are the camera motions, the computed relative motion is :math:`T = T_{2}T^{−1}_{1}`. Args: R1: The first camera rotation matrix with shape :math:`(*, 3, 3)`. t1: The first camera translation vector with shape :math:`(*, 3, 1)`. R2: The second camera rotation matrix with shape :math:`(*, 3, 3)`. t2: The second camera translation vector with shape :math:`(*, 3, 1)`. Returns: A tuple with the relative rotation matrix and translation vector with the shape of :math:`[(*, 3, 3), (*, 3, 1)]`. rrNrr0rr)r+r.r,r/r1r2rrr rs         r)N)Ú__doc__Útypingrrr$Z kornia.utilsrrÚnumericrZ projectionrr Z triangulationr Ú__all__ZTensorr r r rrrrrrr Ús0   "+  úù pþ