python matrix to rotation

矫情吗；* 2024-04-19 12:26 98阅读 0赞

# # -----------------------Rotation-----------------------------------------
    # import warnings
    # import numpy as np
    # import re
    # _AXIS_TO_IND = {'x': 0, 'y': 1, 'z': 2}
    # def _elementary_basis_vector(axis):
    #     b = np.zeros(3)
    #     b[_AXIS_TO_IND[axis]] = 1
    #     return b
    # def _compute_euler_from_dcm(dcm, seq, extrinsic=False):
    #
    #     if extrinsic:
    #         seq = seq[::-1]
    #
    #     if dcm.ndim == 2:
    #         dcm = dcm[None, :, :]
    #     num_rotations = dcm.shape[0]
    #
    #     # Step 0
    #     # Algorithm assumes axes as column vectors, here we use 1D vectors
    #     n1 = _elementary_basis_vector(seq[0])
    #     n2 = _elementary_basis_vector(seq[1])
    #     n3 = _elementary_basis_vector(seq[2])
    #
    #     # Step 2
    #     sl = np.dot(np.cross(n1, n2), n3)
    #     cl = np.dot(n1, n3)
    #
    #     # angle offset is lambda from the paper referenced in [2] from docstring of
    #     # `as_euler` function
    #     offset = np.arctan2(sl, cl)
    #     c = np.vstack((n2, np.cross(n1, n2), n1))
    #
    #     # Step 3
    #     rot = np.array([
    #         [1, 0, 0],
    #         [0, cl, sl],
    #         [0, -sl, cl],
    #     ])
    #     res = np.einsum('...ij,...jk->...ik', c, dcm)
    #     dcm_transformed = np.einsum('...ij,...jk->...ik', res, c.T.dot(rot))
    #
    #     # Step 4
    #     angles = np.empty((num_rotations, 3))
    #     # Ensure less than unit norm
    #     positive_unity = dcm_transformed[:, 2, 2] > 1
    #     negative_unity = dcm_transformed[:, 2, 2] < -1
    #     dcm_transformed[positive_unity, 2, 2] = 1
    #     dcm_transformed[negative_unity, 2, 2] = -1
    #     angles[:, 1] = np.arccos(dcm_transformed[:, 2, 2])
    #
    #     # Steps 5, 6
    #     eps = 1e-7
    #     safe1 = (np.abs(angles[:, 1]) >= eps)
    #     safe2 = (np.abs(angles[:, 1] - np.pi) >= eps)
    #
    #     # Step 4 (Completion)
    #     angles[:, 1] += offset
    #
    #     # 5b
    #     safe_mask = np.logical_and(safe1, safe2)
    #     angles[safe_mask, 0] = np.arctan2(dcm_transformed[safe_mask, 0, 2],
    #                                       -dcm_transformed[safe_mask, 1, 2])
    #     angles[safe_mask, 2] = np.arctan2(dcm_transformed[safe_mask, 2, 0],
    #                                       dcm_transformed[safe_mask, 2, 1])
    #
    #     if extrinsic:
    #         # For extrinsic, set first angle to zero so that after reversal we
    #         # ensure that third angle is zero
    #         # 6a
    #         angles[~safe_mask, 0] = 0
    #         # 6b
    #         angles[~safe1, 2] = np.arctan2(
    #             dcm_transformed[~safe1, 1, 0] - dcm_transformed[~safe1, 0, 1],
    #             dcm_transformed[~safe1, 0, 0] + dcm_transformed[~safe1, 1, 1]
    #         )
    #         # 6c
    #         angles[~safe2, 2] = -np.arctan2(
    #             dcm_transformed[~safe2, 1, 0] + dcm_transformed[~safe2, 0, 1],
    #             dcm_transformed[~safe2, 0, 0] - dcm_transformed[~safe2, 1, 1]
    #         )
    #     else:
    #         # For instrinsic, set third angle to zero
    #         # 6a
    #         angles[~safe_mask, 2] = 0
    #         # 6b
    #         angles[~safe1, 0] = np.arctan2(
    #             dcm_transformed[~safe1, 1, 0] - dcm_transformed[~safe1, 0, 1],
    #             dcm_transformed[~safe1, 0, 0] + dcm_transformed[~safe1, 1, 1]
    #         )
    #         # 6c
    #         angles[~safe2, 0] = np.arctan2(
    #             dcm_transformed[~safe2, 1, 0] + dcm_transformed[~safe2, 0, 1],
    #             dcm_transformed[~safe2, 0, 0] - dcm_transformed[~safe2, 1, 1]
    #         )
    #
    #     # Step 7
    #     if seq[0] == seq[2]:
    #         # lambda = 0, so we can only ensure angle2 -> [0, pi]
    #         adjust_mask = np.logical_or(angles[:, 1] < 0, angles[:, 1] > np.pi)
    #     else:
    #         # lambda = + or - pi/2, so we can ensure angle2 -> [-pi/2, pi/2]
    #         adjust_mask = np.logical_or(angles[:, 1] < -np.pi / 2,
    #                                     angles[:, 1] > np.pi / 2)
    #
    #     # Dont adjust gimbal locked angle sequences
    #     adjust_mask = np.logical_and(adjust_mask, safe_mask)
    #
    #     angles[adjust_mask, 0] += np.pi
    #     angles[adjust_mask, 1] = 2 * offset - angles[adjust_mask, 1]
    #     angles[adjust_mask, 2] -= np.pi
    #
    #     angles[angles < -np.pi] += 2 * np.pi
    #     angles[angles > np.pi] -= 2 * np.pi
    #
    #     # Step 8
    #     if not np.all(safe_mask):
    #         warnings.warn("Gimbal lock detected. Setting third angle to zero since"
    #                       " it is not possible to uniquely determine all angles.")
    #
    #     # Reverse role of extrinsic and intrinsic rotations, but let third angle be
    #     # zero for gimbal locked cases
    #     if extrinsic:
    #         angles = angles[:, ::-1]
    #     return angles
    # def _elementary_basis_vector(axis):
    #     b = np.zeros(3)
    #     b[_AXIS_TO_IND[axis]] = 1
    #     return b
    # class Rotation(object):
    #     def __init__(self, quat, normalized=False, copy=True):
    #         self._single = False
    #         quat = np.asarray(quat, dtype=float)
    #
    #         if quat.ndim not in [1, 2] or quat.shape[-1] != 4:
    #             raise ValueError("Expected `quat` to have shape (4,) or (N x 4), "
    #                              "got {}.".format(quat.shape))
    #
    #         if quat.shape == (4,):
    #             quat = quat[None, :]
    #             self._single = True
    #
    #         if normalized:
    #             self._quat = quat.copy() if copy else quat
    #         else:
    #             self._quat = quat.copy()
    #             norms = scipy.linalg.norm(quat, axis=1)
    #
    #             zero_norms = norms == 0
    #             if zero_norms.any():
    #                 raise ValueError("Found zero norm quaternions in `quat`.")
    #
    #             self._quat[~zero_norms] /= norms[~zero_norms][:, None]
    #
    #     def __len__(self):
    #         return self._quat.shape[0]
    #
    #     @classmethod
    #     def from_dcm(cls,dcm):
    #         is_single = False
    #         dcm = np.asarray(dcm, dtype=float)
    #
    #         if dcm.ndim not in [2, 3] or dcm.shape[-2:] != (3, 3):
    #             raise ValueError("Expected `dcm` to have shape (3, 3) or "
    #                              "(N, 3, 3), got {}".format(dcm.shape))
    #
    #         # If a single dcm is given, convert it to 3D 1 x 3 x 3 matrix but set
    #         # self._single to True so that we can return appropriate objects in
    #         # the `to_...` methods
    #         if dcm.shape == (3, 3):
    #             dcm = dcm.reshape((1, 3, 3))
    #             is_single = True
    #
    #         num_rotations = dcm.shape[0]
    #
    #         decision_matrix = np.empty((num_rotations, 4))
    #         decision_matrix[:, :3] = dcm.diagonal(axis1=1, axis2=2)
    #         decision_matrix[:, -1] = decision_matrix[:, :3].sum(axis=1)
    #         choices = decision_matrix.argmax(axis=1)
    #
    #         quat = np.empty((num_rotations, 4))
    #
    #         ind = np.nonzero(choices != 3)[0]
    #         i = choices[ind]
    #         j = (i + 1) % 3
    #         k = (j + 1) % 3
    #
    #         quat[ind, i] = 1 - decision_matrix[ind, -1] + 2 * dcm[ind, i, i]
    #         quat[ind, j] = dcm[ind, j, i] + dcm[ind, i, j]
    #         quat[ind, k] = dcm[ind, k, i] + dcm[ind, i, k]
    #         quat[ind, 3] = dcm[ind, k, j] - dcm[ind, j, k]
    #
    #         ind = np.nonzero(choices == 3)[0]
    #         quat[ind, 0] = dcm[ind, 2, 1] - dcm[ind, 1, 2]
    #         quat[ind, 1] = dcm[ind, 0, 2] - dcm[ind, 2, 0]
    #         quat[ind, 2] = dcm[ind, 1, 0] - dcm[ind, 0, 1]
    #         quat[ind, 3] = 1 + decision_matrix[ind, -1]
    #
    #         quat /= np.linalg.norm(quat, axis=1)[:, None]
    #
    #         if is_single:
    #             return cls(quat[0], normalized=True, copy=False)
    #         else:
    #             return cls(quat, normalized=True, copy=False)
    #
    #     def as_dcm(self):
    #         x = self._quat[:, 0]
    #         y = self._quat[:, 1]
    #         z = self._quat[:, 2]
    #         w = self._quat[:, 3]
    #
    #         x2 = x * x
    #         y2 = y * y
    #         z2 = z * z
    #         w2 = w * w
    #
    #         xy = x * y
    #         zw = z * w
    #         xz = x * z
    #         yw = y * w
    #         yz = y * z
    #         xw = x * w
    #
    #         num_rotations = len(self)
    #         dcm = np.empty((num_rotations, 3, 3))
    #
    #         dcm[:, 0, 0] = x2 - y2 - z2 + w2
    #         dcm[:, 1, 0] = 2 * (xy + zw)
    #         dcm[:, 2, 0] = 2 * (xz - yw)
    #
    #         dcm[:, 0, 1] = 2 * (xy - zw)
    #         dcm[:, 1, 1] = - x2 + y2 - z2 + w2
    #         dcm[:, 2, 1] = 2 * (yz + xw)
    #
    #         dcm[:, 0, 2] = 2 * (xz + yw)
    #         dcm[:, 1, 2] = 2 * (yz - xw)
    #         dcm[:, 2, 2] = - x2 - y2 + z2 + w2
    #
    #         if self._single:
    #             return dcm[0]
    #         else:
    #             return dcm
    #
    #     def as_euler(self, seq, degrees=False):
    #         if len(seq) != 3:
    #             raise ValueError("Expected 3 axes, got {}.".format(seq))
    #
    #         intrinsic = (re.match(r'^[XYZ]{1,3}$', seq) is not None)
    #         extrinsic = (re.match(r'^[xyz]{1,3}$', seq) is not None)
    #         if not (intrinsic or extrinsic):
    #             raise ValueError("Expected axes from `seq` to be from "
    #                              "['x', 'y', 'z'] or ['X', 'Y', 'Z'], "
    #                              "got {}".format(seq))
    #
    #         if any(seq[i] == seq[i+1] for i in range(2)):
    #             raise ValueError("Expected consecutive axes to be different, "
    #                              "got {}".format(seq))
    #
    #         seq = seq.lower()
    #
    #         angles = _compute_euler_from_dcm(self.as_dcm(), seq, extrinsic)
    #         if degrees:
    #             angles = np.rad2deg(angles)
    #
    #         return angles[0] if self._single else angles
    # # -----------------------Rotation-----------------------------------------