o oi!@sfddlmZddlZddlmZmZddlmZmZm Z m Z ddl m Z ddd dZ GdddZdS)) annotationsN)Tensoreye)Se2Se3So2So3) QuaternionRmatrixr matrix_typestrreturnNonec Csg}|dkrddgddgg}n |dkrddgddgg}t|jdks4t|jdks4t|jdd|vrRt|d|d d |d d |d d |d d |j dS)z"Verify matrix shape based on type.r RTNz must be either rxz or z, got )lenshapelist ValueError)r r target_shapesrH/home/ubuntu/.local/lib/python3.10/site-packages/kornia/geometry/pose.pycheck_matrix_shapes 2rc@seZdZdZd,d-d d Zd.d dZd/ddZed0ddZed1ddZ ed2ddZ ed.ddZ ed.ddZ e  d,d3d"d#Ze d,d4d%d&Zd5d'd(Zd6d*d+ZdS)7 NamedPoseaClass to represent a named pose between two frames. Internally represented by either Se2 or Se3. Example: >>> b_from_a = NamedPose(Se3.identity(), frame_src="frame_a", frame_dst="frame_b") >>> b_from_a NamedPose(dst_from_src=rotation: tensor([1., 0., 0., 0.]) translation: x: 0.0 y: 0.0 z: 0.0, frame_src: frame_a -> frame_dst: frame_b) N dst_from_src Se2 | Se3 frame_src str | None frame_dstrrcCs*||_|p tj|_|ptj|_dS)zConstruct NamedPose. Args: dst_from_src: Pose from source frame to destination frame. frame_src: Name of frame a. frame_dst: Name of frame b. N) _dst_from_srcuuiduuid4hex _frame_src _frame_dst)selfr r"r$rrr__init__9s zNamedPose.__init__r cCsd|jd|jd|jdS)NzNamedPose(dst_from_src=z , frame_src: z -> frame_dst: ))r%r)r*r+rrr__repr__Fs zNamedPose.__repr__othercCs|j|jkrtd|d|t|jtr#t|j|j|j|jSt|jt r6t|j |j|j|jStdt |j)aGCompose two NamedPoses. Args: other: NamedPose to compose with. Returns: Composed NamedPose. Example: >>> b_from_a = NamedPose(Se3.identity(), frame_src="frame_a", frame_dst="frame_b") >>> c_from_b = NamedPose(Se3.identity(), frame_src="frame_b", frame_dst="frame_c") >>> c_from_b * b_from_a NamedPose(dst_from_src=rotation: tensor([1., 0., 0., 0.]) translation: x: 0.0 y: 0.0 z: 0.0, frame_src: frame_a -> frame_dst: frame_c) zCannot compose z with z$Pose must be either Se2 or Se3, got ) r)r*r isinstanceposerrr%_mul_se2r_mul_se3type)r+r0rrr__mul__Ls   zNamedPose.__mul__cC|jS)z-Pose from source frame to destination frame .r%r.rrrr2izNamedPose.pose So3 | So2cC|jjS)zRotation part of the pose.)r%rotationr.rrrr<nzNamedPose.rotationrcCr;)zTranslation part of the pose.)r% translationr.rrrr>sr=zNamedPose.translationcCr7)zName of the source frame.)r)r.rrrr"xr9zNamedPose.frame_srccCr7)zName of the destination frame.)r*r.rrrr$}r9zNamedPose.frame_dstr<So3 | So2 | Tensor | Quaternionr>NamedPose | NonecCst|ttfr|t||||St|tr|t||||St|trgt||jd}t |d|j |j d}||dd|d|f<||dd||f<|dkrX|t |||S|dkre|t |||SdSt dt|) aXConstruct NamedPose from rotation and translation. Args: rotation: Rotation part of the pose. translation: Translation part of the pose. frame_src: Name of the source frame. frame_dst: Name of the destination frame. Returns: NamedPose constructed from rotation and translation. Example: >>> b_from_a_rot = So3.identity() >>> b_from_a_trans = torch.tensor([1., 2., 3.]) >>> b_from_a = NamedPose.from_rt(b_from_a_rot, b_from_a_trans, frame_src="frame_a", frame_dst="frame_b") >>> b_from_a NamedPose(dst_from_src=rotation: tensor([1., 0., 0., 0.]) translation: Parameter containing: tensor([1., 2., 3.], requires_grad=True), frame_src: frame_a -> frame_dst: frame_b) r)devicedtype.Nrrz6R must be either So2, So3, Quaternion, or Tensor, got )r1rr rrrrrrrrBrC from_matrixrr5)clsr<r>r"r$dimrrrrfrom_rts    zNamedPose.from_rtr cCsNt|dd|jd}|dkr|t|||S|dkr%|t|||SdS)aConstruct NamedPose from a matrix. Args: matrix: Matrix representation of the pose. frame_src: Name of the source frame. frame_dst: Name of the destination frame. Returns: NamedPose constructed from a matrix. Example: >>> b_from_a_matrix = Se3.identity().matrix() >>> b_from_a = NamedPose.from_matrix(b_from_a_matrix, frame_src="frame_a", frame_dst="frame_b") >>> b_from_a NamedPose(dst_from_src=rotation: tensor([1., 0., 0., 0.]) translation: Parameter containing: tensor([0., 0., 0.], requires_grad=True), frame_src: frame_a -> frame_dst: frame_b) r)r rArrN)rrrrDr)rEr r"r$rFrrrrDs  zNamedPose.from_matrixcCst|j|j|jS)aInverse of the NamedPose. Returns: Inverse of the NamedPose. Example: >>> b_from_a = NamedPose(Se3.identity(), frame_src="frame_a", frame_dst="frame_b") >>> b_from_a.inverse() NamedPose(dst_from_src=rotation: tensor([1., -0., -0., -0.]) translation: x: 0.0 y: 0.0 z: 0.0, frame_src: frame_b -> frame_dst: frame_a) )rr%inverser*r)r.rrrrHszNamedPose.inverse points_in_srccCs |j|S)aTransform points from source frame to destination frame. Args: points_in_src: Points in source frame. Returns: Points in destination frame. Example: >>> b_from_a = NamedPose(Se3.identity(), frame_src="frame_a", frame_dst="frame_b") >>> b_from_a.transform_points(torch.tensor([1., 2., 3.])) tensor([1., 2., 3.]) r8)r+rIrrrtransform_pointss zNamedPose.transform_points)NN)r r!r"r#r$r#rr)rr )r0rrr)rr!)rr:)rr) r<r?r>rr"r#r$r#rr@)r rr"r#r$r#rr@)rr)rIrrr)__name__ __module__ __qualname____doc__r,r/r6propertyr2r<r>r"r$ classmethodrGrDrHrJrrrrr)s0       / r)r )r rr r rr) __future__rr& kornia.corerrkornia.geometry.liegrouprrrrkornia.geometry.quaternionr rrrrrrs