U h0@sdZddlmZddlmZddlmZddlZddlZddlmZm Z ddZ d d Z d d Z d dZ ddZd'ddZddZd(ddZd)ddZddZdd Zd!d"Zd#d$Zd%d&ZdS)*a[ Functions about transforming mesh(changing the position: modify vertices). 1. forward: transform(transform, camera, project). 2. backward: estimate transform matrix from correspondences. Preparation knowledge: transform&camera model: https://cs184.eecs.berkeley.edu/lecture/transforms-2 Part I: camera geometry and single view geometry in MVGCV )absolute_import)division)print_functionN)cossincCst|dt|dt|d}}}tdddgdt|t| gdt|t|gg}tt|dt|gdddgt| dt|gg}tt|t| dgt|t|dgdddgg}|||}|tjS)a7 get rotation matrix from three rotation angles(degree). right-handed. Args: angles: [3,]. x, y, z angles x: pitch. positive for looking down. y: yaw. positive for looking left. z: roll. positive for tilting head right. Returns: R: [3, 3]. rotation matrix. r)npdeg2radarrayrrdotastypefloat32anglesxyzZRxZRyZRzRrV/tmp/pip-unpacked-wheel-5oclok7i/insightface/thirdparty/face3d/mesh_numpy/transform.py angle2matrixs . rcCs|d|d|d}}}tdddgdt|t|gdt| t|gg}tt|dt| gdddgt|dt|gg}tt|t|dgt| t|dgdddgg}|||}|tjS)z get rotation matrix from three rotation angles(radian). The same as in 3DDFA. Args: angles: [3,]. x, y, z angles x: pitch. y: yaw. z: roll. Returns: R: 3x3. rotation matrix. rrr)r r rrr r rrrrrangle2matrix_3ddfa/s  rcCst|}||j}|S)a? rotate vertices. X_new = R.dot(X). X: 3 x 1 Args: vertices: [nver, 3]. rx, ry, rz: degree angles rx: pitch. positive for looking down ry: yaw. positive for looking left rz: roll. positive for tilting head right Returns: rotated vertices: [nver, 3] )rr T)verticesrrZrotated_verticesrrrrotateNs  rcCs<ttj|tjd}|||j|tjddf}|S)a9 similarity transform. dof = 7. 3D: s*R.dot(X) + t Homo: M = [[sR, t],[0^T, 1]]. M.dot(X) Args:(float32) vertices: [nver, 3]. s: [1,]. scale factor. R: [3,3]. rotation matrix. t3d: [3,]. 3d translation vector. Returns: transformed vertices: [nver, 3] dtypeN)r Zsqueezer rr rnewaxis)rsrZt3dtransformed_verticesrrrsimilarity_transform_s "r!cCs0d}ttj|ddd}t||}||S)Ng-q=rr)Zaxis)r sqrtsummaximum)repsilonnormrrr normalizess r'c Cs|dkrtdddgtj}|dkr8tdddgtj}t|tj}t|tj}t|| }tt||}t||}t|||f}||}||j}|S)a 'look at' transformation: from world space to camera space standard camera space: camera located at the origin. looking down negative z-axis. vertical vector is y-axis. Xcam = R(X - C) Homo: [[R, -RC], [0, 1]] Args: vertices: [nver, 3] eye: [3,] the XYZ world space position of the camera. at: [3,] a position along the center of the camera's gaze. up: [3,] up direction Returns: transformed_vertices: [nver, 3] Nrr) r r rr r'crossstackr r) rZeyeatZupZz_aixsZx_aixsZy_axisrr rrr lookat_camerays  r+cCs|S)a scaled orthographic projection(just delete z) assumes: variations in depth over the object is small relative to the mean distance from camera to object x -> x*f/z, y -> x*f/z, z -> f. for point i,j. zi~=zj. so just delete z ** often used in face Homo: P = [[1,0,0,0], [0,1,0,0], [0,0,1,0]] Args: vertices: [nver, 3] Returns: projected_vertices: [nver, 3] if isKeepZ=True. [nver, 2] if isKeepZ=False. copy)rrrrorthographic_projects r.?皙?@@c Cst|}|t|}| }||}| }t||dddgd||ddgdd|| ||d||||gddddgg} t|t|jddff} | | j} | | ddddf} | ddddf} | dddf | dddf<| S)a: perspective projection. Args: vertices: [nver, 3] fovy: vertical angular field of view. degree. aspect_ratio : width / height of field of view near : depth of near clipping plane far : depth of far clipping plane Returns: projected_vertices: [nver, 3] rrNr) r r tanr hstackonesshaper r) rZfovyZ aspect_ratioZnearZfartopZbottomrightleftPZ vertices_homoZprojected_verticesrrrperspective_projects (  r=FcCs|}|rT|dddf|d|dddf<|dddf|d|dddf<|dddf|d|dddf<|dddf|d|dddf<||dddfd|dddf<|S)a* change vertices to image coord system 3d system: XYZ, center(0, 0, 0) 2d image: x(u), y(v). center(w/2, h/2), flip y-axis. Args: vertices: [nver, 3] h: height of the rendering w : width of the rendering Returns: projected_vertices: [nver, 3] Nrrrr,)rhwZis_perspectiveZimage_verticesrrrto_images $$$$$r@cCs6t|t|jddgf}tj||dj}|S)z Using least-squares solution Args: X: [n, 3]. 3d points(fixed) Y: [n, 3]. corresponding 3d points(moving). Y = PX Returns: P_Affine: (3, 4). Affine camera matrix (the third row is [0, 0, 0, 1]). rr)r r6r7r8linalgZlstsqr)XYX_homor<rrrestimate_affine_matrix_3d23dsrEcCs|j}|j}|jd|jdks$t|jd}|dks:tt|d}|t|ddtjfd|g}ttt|dd}td|}||}tj dtj d}||d<|d <| ||dddf<d|d <t |t d|ff}t|d}|t|ddtjfd|g}|dd ddf|}ttt|dd}td |}||}tj d tj d} || d<| d <| d <| || dd d f<d| d<tj |dd ftj d} t |t d|ffj}|| d|ddf<|| |dddf<t |ddg} tj| | } tj dtj d} | dddf| dddf<| dddf| dddf<d| d<tj|| | }|S)a Using Golden Standard Algorithm for estimating an affine camera matrix P from world to image correspondences. See Alg.7.2. in MVGCV Code Ref: https://github.com/patrikhuber/eos/blob/master/include/eos/fitting/affine_camera_estimation.hpp x_homo = X_homo.dot(P_Affine) Args: X: [n, 3]. corresponding 3d points(fixed) x: [n, 2]. n>=4. 2d points(moving). x = PX Returns: P_Affine: [3, 4]. Affine camera matrix rNrr)r4r4rrrrrrrr4)rFrFr3)r4rF)r3r3)rr8AssertionErrorr meanZtilerr"r#zerosrZvstackr7ZreshaperAZpinvr inv)rBrnrLZ average_normZscalerrDmUAbZp_8r<ZP_Affinerrrestimate_affine_matrix_3d22dsJ    " "rTc Cs|dddf}|ddddf}|ddddf}tj|tj|d}|tj|}|tj|}t||}t|||fd}|||fS)z decompositing camera matrix P Args: P: (3, 4). Affine Camera Matrix. Returns: s: scale factor. R: (3, 3). rotation matrix. t: (3,). translation. Nr4rrrg@)r rAr&r(Z concatenate) r<tZR1ZR2rZr1Zr2Zr3rrrrP2sRt/s  rVcCs>t|}t||}tjd|jd}tj||}|dkS)zN checks if a matrix is a valid rotation matrix(whether orthogonal or not) r4rư>)r Z transposer identityrrAr&)rZRtZshouldBeIdentityIrOrrrisRotationMatrixDs   rZc Cststt|d|d|d|d}|dk}|svt|d|d}t|d |}t|d|d}n,t|d |d}t|d |}d }|d tj|d tj|d tj}}}|||fS) z get three Euler angles from Rotation Matrix Args: R: (3,3). rotation matrix Returns: x: pitch y: yaw z: roll rG)rrrW)rrrI)rr)rrrHr)rZrKmathr"atan2r pi) rZsyZsingularrrrrxZryZrzrrr matrix2angleMs &.r`)NN)r/r0r1)F)__doc__ __future__rrrZnumpyr r\rrrrrr!r'r+r.r=r@rErTrVrZr`rrrrs(    " #  >