Python scipy.spatial.transform.Rotation.from_euler() Examples

def draw_model_axes(self, face: Face, length: float, lw: int = 2) -> None:
        assert self.image is not None
        assert face is not None
        assert face.head_pose_rot is not None
        assert face.head_position is not None
        assert face.landmarks is not None
        # Get the axes of the model coordinate system
        axes3d = np.eye(3, dtype=np.float) @ Rotation.from_euler(
            'XYZ', [0, np.pi, 0]).as_matrix()
        axes3d = axes3d * length
        axes2d = self._camera.project_points(axes3d,
                                             face.head_pose_rot.as_rotvec(),
                                             face.head_position)
        center = face.landmarks[MODEL3D.NOSE_INDEX]
        center = self._convert_pt(center)
        for pt, color in zip(axes2d, AXIS_COLORS):
            pt = self._convert_pt(pt)
            cv2.line(self.image, center, pt, color, lw, cv2.LINE_AA) 

def debugGatePoses(p_o_b, r, theta, psi):
    # get relative vector in the base frame coordinates
    t_b_g_body = polarTranslation(r, theta, psi)
    # transform relative vector from base frame to the world frame
    t_b_g = convert_t_body_2_world(t_b_g_body, p_o_b.orientation)
    # get the gate coord in world coordinates from origin
    t_o_g = p_o_b.position + t_b_g
    # check if gate is at least half outside the ground
    # create rotation of gate
    phi_quad_ref = np.arctan2(p_o_b.position.y_val, p_o_b.position.x_val)
    phi_rel = np.pi/2
    phi_gate = phi_quad_ref + phi_rel
    rot_gate = Rotation.from_euler('ZYX', [phi_gate, 0, 0])
    q = rot_gate.as_quat()
    p_o_g = Pose(t_o_g, Quaternionr(q[0], q[1], q[2], q[3]))
    return p_o_g, r, theta, psi, phi_rel 

def to_quaternion(self):
        return RotationQuaternion(*R.from_euler(self._axes[1:],self._array,degrees=False).as_quat()) 

def to_matrix(self):
        mat = np.eye(4)
        mat[:3, :3] = R.from_euler(self.axes[1:],self._array,degrees=False).as_dcm() # scipy as_matrix() not available
        return mat 

def initVisualizeBoundary(self):
        with open(CAMERA_INFO_PATH, 'r') as stream:
            try:
                contents = yaml.load(stream)
                camera_matrix = np.array(contents['camera_matrix']['data']).reshape((3, 3))
                distortion_coefficients = np.array(contents['distortion_coefficients']['data']).reshape((1, 5))
            except yaml.YAMLError as exc:
                print(exc)

        # camera installation info
        r = R.from_euler('xyz', CAMERA_ROT_EULER_COORD_GROUND, degrees=True)
        camera_rot_mat_coord_ground = r.as_dcm()

        pos_transformer = PosTransformer(camera_matrix, distortion_coefficients, CAMERA_POS_COORD_GROUND,
                                         camera_rot_mat_coord_ground)

        self.boundary_coner_pixel_pos = np.zeros((2, 4))
        # assume that image is undistorted
        self.boundary_coner_pixel_pos[:, 0] = \
            pos_transformer.phyPosGround2PixelPos([MIN_X, MIN_Y], return_distort_image_pos=False).squeeze()
        self.boundary_coner_pixel_pos[:, 1] = \
            pos_transformer.phyPosGround2PixelPos([MAX_X, MIN_Y], return_distort_image_pos=False).squeeze()
        self.boundary_coner_pixel_pos[:, 2] = \
            pos_transformer.phyPosGround2PixelPos([MAX_X, MAX_Y], return_distort_image_pos=False).squeeze()
        self.boundary_coner_pixel_pos[:, 3] = \
            pos_transformer.phyPosGround2PixelPos([MIN_X, MAX_Y], return_distort_image_pos=False).squeeze()

        # transform the corresponding points into cropped image
        self.boundary_coner_pixel_pos = self.boundary_coner_pixel_pos - (np.array(ORIGIN_SIZE) -
                                                                         np.array(CROPPED_SIZE)).reshape(2, 1) / 2.0
        
        # transform the corresponding points into resized image (RENDER_WIDHT, RENDER_HEIGHT)
        self.boundary_coner_pixel_pos[0, :] *= RENDER_WIDTH/CROPPED_SIZE[0]
        self.boundary_coner_pixel_pos[1, :] *= RENDER_HEIGHT/CROPPED_SIZE[1]
        
        self.boundary_coner_pixel_pos = np.around(self.boundary_coner_pixel_pos).astype(np.int) 

def default_scene(obj_file, width=640, height=480, use_distortion=True):

    mesh_trimesh = trimesh.load(obj_file)

    mesh = ColoredTriMesh.from_trimesh(mesh_trimesh)

    # rot = Rotation.from_euler("xyz", [180, 0, 0], degrees=True).as_matrix()
    rot = Rotation.from_euler("xyz", [180, 0, 0], degrees=True).as_matrix()

    camera = differentiable_renderer.default_camera(
        width, height, 80, mesh.vertices, rot
    )
    if use_distortion:
        camera.distortion = np.array([-0.5, 0.5, 0, 0, 0])

    bg_color = np.array((0.8, 0.8, 0.8))
    scene = differentiable_renderer.Scene3D()
    light_ambient = 0
    light_directional = 0.3 * np.array([1, -1, 0])
    scene.set_light(light_directional=light_directional, light_ambient=light_ambient)
    scene.set_mesh(mesh)
    background_image = np.ones((height, width, 3)) * bg_color
    scene.set_background(background_image)
    return scene, camera 

def from_hpr(h, p, r):
    """Create a direction cosine matrix from heading, pitch and roll angles.

    The sequence of elemental rotations is as follows::

           -heading   pitch   roll
        N ---------> ------> -----> B
              3         1       2

    Here N denotes the local level wander-azimuth frame and B denotes the body
    frame. The resulting DCM projects from B frame to N frame.

    Parameters
    ----------
    h, p, r : float or array_like with shape (n,)
        Heading, pitch and roll.

    Returns
    -------
    dcm : ndarray, shape (3, 3) or (n, 3, 3)
        Direction cosine matrices.
    """
    h = np.asarray(h)
    p = np.asarray(p)
    r = np.asarray(r)
    if h.ndim == 0 and p.ndim == 0 and r.ndim == 0:
        return Rotation.from_euler('yxz', [r, p, -h], degrees=True).as_matrix()

    h = np.atleast_1d(h)
    p = np.atleast_1d(p)
    r = np.atleast_1d(r)

    n = max(len(h), len(p), len(r))

    angles = np.empty((n, 3))
    angles[:, 0] = r
    angles[:, 1] = p
    angles[:, 2] = -h
    return Rotation.from_euler('yxz', angles, degrees=True).as_matrix() 

def euler_to_quat(euler):
    """
    Convert euler angles into a quaternion.

    Args:
        euler (np.ndarray):  euler angles to be converted

    Returns:
        Quaternion in format [w, x, y, z]

    """
    if len(euler.shape) < 2:
        return R.from_euler('xyz', euler).as_quat()[[3, 0, 1, 2]]
    else:
        return R.from_euler('xyz', euler.T).as_quat()[:, [3, 0, 1, 2]].T 

def randomQuadPose(x_range, y_range, z_range, yaw_range, pitch_range, roll_range):
    x = randomSample(x_range)
    y = randomSample(y_range)
    z = randomSample(z_range)
    yaw = randomSample(yaw_range)
    pitch = randomSample(pitch_range)
    roll = randomSample(roll_range)
    q = Rotation.from_euler('ZYX', [yaw, pitch, roll])  # capital letters denote intrinsic rotation (lower case would be extrinsic)
    q = q.as_quat()
    t_o_b = Vector3r(x,y,z)
    q_o_b = Quaternionr(q[0], q[1], q[2], q[3])
    return Pose(t_o_b, q_o_b), yaw 

def randomGatePose(p_o_b, phi_base, r_range, cam_fov, correction):
    gate_ok = False
    while not gate_ok:
        # create translation of gate
        r = randomSample(r_range)
        alpha = cam_fov/180.0*np.pi/2.0  # alpha is half of fov angle
        theta_range = [-alpha, alpha]
        theta = randomSample(theta_range)
        # need to make projection on geodesic curve! not equal FOV in theta and psi
        alpha_prime = np.arctan(np.cos(np.abs(theta)))
        psi_range = [-alpha_prime, alpha_prime]
        psi_range = [x * correction for x in psi_range]
        psi = randomSample(psi_range) + np.pi/2.0
        # get relative vector in the base frame coordinates
        t_b_g_body = polarTranslation(r, theta, psi)

        # transform relative vector from base frame to the world frame
        t_b_g = convert_t_body_2_world(t_b_g_body, p_o_b.orientation)

        # get the gate coord in world coordinates from origin
        t_o_g = p_o_b.position + t_b_g

        # check if gate is at least half outside the ground
        if t_o_g.z_val >= 0.0:
            continue

        # create rotation of gate
        eps = 0  # np.pi/15.0
        phi_rel_range = [-np.pi + eps, 0 - eps]
        phi_rel = randomSample(phi_rel_range)
        phi_quad_ref = get_yaw_base(p_o_b)
        phi_gate = phi_quad_ref + phi_rel
        rot_gate = Rotation.from_euler('ZYX', [phi_gate, 0, 0])
        q = rot_gate.as_quat()
        p_o_g = Pose(t_o_g, Quaternionr(q[0], q[1], q[2], q[3]))

        return p_o_g, r, theta, psi, phi_rel 

def debugRelativeOrientation(p_o_b, p_o_g, phi_rel):
    phi_quad_ref = get_yaw_base(p_o_b)
    phi_gate = phi_quad_ref + phi_rel
    rot_gate = Rotation.from_euler('ZYX', [phi_gate, 0, 0])
    q = rot_gate.as_quat()
    p_o_g = Pose(p_o_g.position, Quaternionr(q[0], q[1], q[2], q[3]))
    return p_o_g 

def getGatePoseWorld(p_o_b, r, theta, psi, phi_rel):
    # get relative vector in the base frame coordinates
    t_b_g_body = polarTranslation(r, theta, psi)
    # transform relative vector from base frame to the world frame
    t_b_g = convert_t_body_2_world(t_b_g_body, p_o_b.orientation)
    # get the gate coord in world coordinates from origin
    t_o_g = p_o_b.position + t_b_g
    # create rotation of gate
    phi_quad_ref = get_yaw_base(p_o_b)
    phi_gate = phi_quad_ref + phi_rel
    rot_gate = Rotation.from_euler('ZYX', [phi_gate, 0, 0])
    q = rot_gate.as_quat()
    p_o_g = Pose(t_o_g, Quaternionr(q[0], q[1], q[2], q[3]))
    return p_o_g 

def generate_new_templates(self, base_rotation=3, scale_std=.1, translate_std=20.):
        """Creates new templates by rotation, scaling and translation.

        params:
        -------
        base_rotation: int
            Fraction of pi to be used for base rotation value. Multiples of
            this amount are used, up to 2 * pi to rotate templates spatially.
        """
        new_temps = np.zeros_like(self.wave_forms)
        scales = np.random.normal(1., scale_std, [self.n_unit, self.n_channel])
        max_rotation = base_rotation * 2
        rotations = np.random.randint(0, max_rotation, self.n_unit) * max_rotation
        translates = np.random.normal(0, translate_std, [self.n_unit, 2])
        for unit in range(self.n_unit):
            rot_matrix = Rotation.from_euler("x", rotations[unit]).as_dcm()[1:, 1:]
            x = np.matmul(self.geom.geom, rot_matrix) + translates[unit]
            # Find mapping of new geometry and the original geometry
            c_dist = cdist(x, self.geom.geom)
            new = np.array(c_dist.argmin(axis=1))
            seen = np.zeros(self.n_channel, dtype=bool)
            for i in c_dist.min(axis=1).argsort()[::-1]:
                if seen[new[i]]:
                    continue
                new_temps[unit, i] = self.wave_forms[unit, new[i]] * scales[unit, i] 
                seen[new[i]] = True
        return new_temps 

def generate_new_templates(self, base_rotation=3, scale_std=.1, translate_std=20.):
        """Creates new templates by rotation, scaling and translation.

        params:
        -------
        base_rotation: int
            Fraction of pi to be used for base rotation value. Multiples of
            this amount are used, up to 2 * pi to rotate templates spatially.
        """
        new_temps = np.zeros_like(self.wave_forms)
        scales = np.random.normal(1., scale_std, [self.n_unit, self.n_channel])
        max_rotation = base_rotation * 2
        rotations = np.random.randint(0, max_rotation, self.n_unit) * max_rotation
        translates = np.random.normal(0, translate_std, [self.n_unit, 2])
        for unit in range(self.n_unit):
            rot_matrix = Rotation.from_euler("x", rotations[unit]).as_dcm()[1:, 1:]
            x = np.matmul(self.geom.geom, rot_matrix) + translates[unit]
            # Find mapping of new geometry and the original geometry
            c_dist = cdist(x, self.geom.geom)
            new = np.array(c_dist.argmin(axis=1))
            seen = np.zeros(self.n_channel, dtype=bool)
            for i in c_dist.min(axis=1).argsort()[::-1]:
                if seen[new[i]]:
                    continue
                new_temps[unit, i] = self.wave_forms[unit, new[i]] * scales[unit, i] 
                seen[new[i]] = True
        return new_temps 

def from_llw(lat, lon, wan=0):
    """Create a direction cosine matrix from latitude and longitude and wander angle.

    The sequence of elemental rotations is as follows::

           pi/2+lon    pi/2-lan     wan
        E ----------> ----------> ------> N
               3           1         3

    Here E denotes the ECEF frame and N denotes the local level wander-angle
    frame. The resulting DCM projects from N frame to E frame.

    If ``wan=0`` then the 2nd axis of N frame points to North.

    Parameters
    ----------
    lat, lon : float or array_like with shape (n,)
        Latitude and longitude.
    wan : float or array_like with shape (n,), optional
        Wander angle, default is 0.

    Returns
    -------
    dcm : ndarray, shape (3, 3) or (n, 3, 3)
        Direction Cosine Matrices.
    """
    lat = np.asarray(lat)
    lon = np.asarray(lon)
    wan = np.asarray(wan)

    if lat.ndim == 0 and lon.ndim == 0 and wan.ndim == 0:
        return Rotation.from_euler('ZXZ', [90 + lon, 90 - lat, wan], degrees=True).as_matrix()

    lat = np.atleast_1d(lat)
    lon = np.atleast_1d(lon)
    wan = np.atleast_1d(wan)

    n = max(len(lat), len(lon), len(wan))

    angles = np.empty((n, 3))
    angles[:, 0] = 90 + lon
    angles[:, 1] = 90 - lat
    angles[:, 2] = wan
    return Rotation.from_euler('ZXZ', angles, degrees=True).as_matrix()