Exemplo n.º 1
0
def test_line_scan_driver():
    j2000 = FrameNode(1)
    body_rotation = TimeDependentRotation(
        np.array([[0, 0, 0, 1], [0, 0, 0, 1]]),
        np.array([0, 1]),
        100,
        1
    )
    body_fixed = FrameNode(100, parent=j2000, rotation=body_rotation)
    spacecraft_rotation = TimeDependentRotation(
        np.array([[0, 0, 0, 1], [0, 0, 0, 1]]),
        np.array([0, 1]),
        1000,
        1
    )
    spacecraft = FrameNode(1000, parent=j2000, rotation=spacecraft_rotation)
    sensor_rotation = ConstantRotation(np.array([0, 0, 0, 1]), 1010, 1000)
    sensor = FrameNode(1010, parent=spacecraft, rotation=sensor_rotation)
    driver = TestLineScanner()
    driver.target_body_radii = (1100, 1000)
    driver.sensor_position = (
        [[0, 1, 2], [3, 4, 5]],
        [[0, -1, -2], [-3, -4, -5]],
        [800, 900]
    )
    driver.sun_position = (
        [[0, 1, 2], [3, 4, 5]],
        [[0, -1, -2], [-3, -4, -5]],
        [800, 900]
    )
    driver.sensor_frame_id = 1010
    driver.target_frame_id = 100
    driver.frame_chain = j2000
    driver.sample_summing = 2
    driver.line_summing = 4
    driver.focal_length = 500
    driver.detector_center_line = 0.5
    driver.detector_center_sample = 512
    driver.detector_start_line = 0
    driver.detector_start_sample = 8
    driver.focal2pixel_lines = [0.1, 0.2, 0.3]
    driver.focal2pixel_samples = [0.3, 0.2, 0.1]
    driver.usgscsm_distortion_model = {
        'radial' : {
            'coefficients' : [0.0, 1.0, 0.1]
        }
    }
    driver.image_lines = 10000
    driver.image_samples = 1024
    driver.platform_name = 'Test Platform'
    driver.sensor_name = 'Test Line Scan Sensor'
    driver.ephemeris_stop_time = 900
    driver.ephemeris_start_time = 800

    return driver
Exemplo n.º 2
0
    def frame_chain(self):
        """
        Return the root node of the rotation frame tree/chain.

        The root node is the J2000 reference frame. The other nodes in the
        tree can be accessed via the methods in the FrameNode class.

        This property expects the ephemeris_time property/attribute to be defined.
        It should be a list of the ephemeris seconds past the J2000 epoch for each
        exposure in the image.

        Returns
        -------
        FrameNode
            The root node of the frame tree. This will always be the J2000 reference frame.
        """
        if not hasattr(self, '_root_frame'):
            j2000_id = 1  #J2000 is our root reference frame
            self._root_frame = FrameNode(j2000_id)

            sensor_quats = np.zeros((len(self.ephemeris_time), 4))
            sensor_times = np.array(self.ephemeris_time)
            body_quats = np.zeros((len(self.ephemeris_time), 4))
            body_times = np.array(self.ephemeris_time)
            for i, time in enumerate(self.ephemeris_time):
                sensor2j2000 = spice.pxform(spice.frmnam(self.sensor_frame_id),
                                            spice.frmnam(j2000_id), time)
                q_sensor = spice.m2q(sensor2j2000)
                sensor_quats[i, :3] = q_sensor[1:]
                sensor_quats[i, 3] = q_sensor[0]

                body2j2000 = spice.pxform(spice.frmnam(self.target_frame_id),
                                          spice.frmnam(j2000_id), time)
                q_body = spice.m2q(body2j2000)
                body_quats[i, :3] = q_body[1:]
                body_quats[i, 3] = q_body[0]

            sensor2j2000_rot = TimeDependentRotation(sensor_quats,
                                                     sensor_times,
                                                     self.sensor_frame_id,
                                                     j2000_id)
            sensor_node = FrameNode(self.sensor_frame_id,
                                    parent=self._root_frame,
                                    rotation=sensor2j2000_rot)

            body2j2000_rot = TimeDependentRotation(body_quats, body_times,
                                                   self.target_frame_id,
                                                   j2000_id)
            body_node = FrameNode(self.target_frame_id,
                                  parent=self._root_frame,
                                  rotation=body2j2000_rot)
        return self._root_frame
Exemplo n.º 3
0
 def frame_chain(self):
     j2000 = FrameNode(1)
     body_rotation = TimeDependentRotation(
         np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 100, 1)
     body_fixed = FrameNode(100, parent=j2000, rotation=body_rotation)
     spacecraft_rotation = TimeDependentRotation(
         np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 1000, 1)
     spacecraft = FrameNode(1000,
                            parent=j2000,
                            rotation=spacecraft_rotation)
     sensor_rotation = ConstantRotation(np.array([0, 0, 0, 1]), 1010, 1000)
     sensor = FrameNode(1010, parent=spacecraft, rotation=sensor_rotation)
     return j2000
Exemplo n.º 4
0
    def frame_chain(self):
        frame_chain = FrameChain()

        body_rotation = TimeDependentRotation(
            np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 100, 1)
        frame_chain.add_edge(rotation=body_rotation)

        spacecraft_rotation = TimeDependentRotation(
            np.array([[0, 0, 0, 1], [0, 0, 0, 1]]), np.array([0, 1]), 1000, 1)
        frame_chain.add_edge(rotation=spacecraft_rotation)

        sensor_rotation = ConstantRotation(np.array([0, 0, 0, 1]), 1010, 1000)
        frame_chain.add_edge(rotation=sensor_rotation)
        return frame_chain
Exemplo n.º 5
0
def test_last_time_dependent_frame_between():
    """
    Test frame tree structure:

          1
         / \
        /   \
       2     4
      /       \
     /         \
    3           5

    The rotations from 3 to 2 and 1 to 4 are time dependent.
    All other rotations are constant.
    """
    frame_chain = FrameChain()

    rotations = [
        ConstantRotation(np.array([1, 0, 0, 0]), 2, 1),
        TimeDependentRotation(
            np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
            np.array([1]), 3, 2),
        TimeDependentRotation(
            np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
            np.array([1]), 4, 1),
        ConstantRotation(np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
                         5, 4)
    ]
    frame_chain.add_edge(rotation=rotations[0])
    frame_chain.add_edge(rotation=rotations[1])
    frame_chain.add_edge(rotation=rotations[2])
    frame_chain.add_edge(rotation=rotations[3])

    # last frame from node 1 to node 3
    s31, d31, _ = frame_chain.last_time_dependent_frame_between(1, 3)
    assert s31 == 2
    assert d31 == 3
    # last frame from node 3 to node 1
    s13, d13, _ = frame_chain.last_time_dependent_frame_between(3, 1)
    assert s13 == 3
    assert d13 == 2
    # last frame from node 3 to node 5
    s35, d35, _ = frame_chain.last_time_dependent_frame_between(3, 5)
    assert s35 == 1
    assert d35 == 4
    # last frame from node 5 to node 3
    s53, d53, _ = frame_chain.last_time_dependent_frame_between(5, 3)
    assert s53 == 2
    assert d53 == 3
Exemplo n.º 6
0
def create_rotations(rotation_table):
    """
    Convert an ISIS rotation table into rotation objects.

    Parameters
    ----------
    rotation_table : dict
                     The rotation ISIS table as a dictionary

    Returns
    -------
    : list
      A list of time dependent or constant rotation objects from the table. This
      list will always have either 1 or 2 elements. The first rotation will be
      time dependent and the second rotation will be constant. The rotations will
      be ordered such that the reference frame the first rotation rotates to is
      the reference frame the second rotation rotates from.
    """
    rotations = []
    root_frame = rotation_table['TimeDependentFrames'][-1]
    last_time_dep_frame = rotation_table['TimeDependentFrames'][0]
    # Case 1: It's a table of quaternions and times
    if 'J2000Q0' in rotation_table:
        # SPICE quaternions are (W, X, Y, Z) and ALE uses (X, Y, Z, W).
        quats = np.array([
            rotation_table['J2000Q1'], rotation_table['J2000Q2'],
            rotation_table['J2000Q3'], rotation_table['J2000Q0']
        ]).T
        time_dep_rot = TimeDependentRotation(quats, rotation_table['ET'],
                                             root_frame, last_time_dep_frame)
        rotations.append(time_dep_rot)
    # Case 2: It's a table of Euler angle coefficients
    elif 'J2000Ang1' in rotation_table:
        ephemeris_times = np.linspace(rotation_table['CkTableStartTime'],
                                      rotation_table['CkTableEndTime'],
                                      rotation_table['CkTableOriginalSize'])
        base_time = rotation_table['J2000Ang1'][-1]
        time_scale = rotation_table['J2000Ang2'][-1]
        scaled_times = (ephemeris_times - base_time) / time_scale
        coeffs = np.array([
            rotation_table['J2000Ang1'][:-1], rotation_table['J2000Ang2'][:-1],
            rotation_table['J2000Ang3'][:-1]
        ]).T
        angles = polyval(scaled_times, coeffs).T
        # ISIS is hard coded to ZXZ (313) Euler angle axis order.
        # SPICE also interprets Euler angle rotations as negative rotations,
        # so negate them before passing to scipy.
        time_dep_rot = TimeDependentRotation.from_euler(
            'zxz', -angles, ephemeris_times, root_frame, last_time_dep_frame)
        rotations.append(time_dep_rot)

    if 'ConstantRotation' in rotation_table:
        last_constant_frame = rotation_table['ConstantFrames'][0]
        rot_mat = np.reshape(np.array(rotation_table['ConstantRotation']),
                             (3, 3))
        constant_rot = ConstantRotation.from_matrix(rot_mat,
                                                    last_time_dep_frame,
                                                    last_constant_frame)
        rotations.append(constant_rot)
    return rotations
Exemplo n.º 7
0
def test_apply_at_single_time():
    test_quats = Rotation.from_euler('x', np.array([-90, 0, 45]),
                                     degrees=True).as_quat()
    rot = TimeDependentRotation(test_quats, [0, 1, 1.5], 1, 2)
    input_vec = np.asarray([1, 2, 3])
    rot_vec = rot.apply_at(input_vec, 0)
    np.testing.assert_almost_equal(rot_vec, np.asarray([[1, 3, -2]]))
Exemplo n.º 8
0
    def from_spice(cls,
                   *args,
                   sensor_frame,
                   target_frame,
                   center_ephemeris_time,
                   ephemeris_times=[],
                   **kwargs):
        frame_chain = cls()

        times = np.array(ephemeris_times)

        sensor_time_dependent_frames, sensor_constant_frames = cls.frame_trace(
            sensor_frame, center_ephemeris_time)
        target_time_dependent_frames, target_constant_frames = cls.frame_trace(
            target_frame, center_ephemeris_time)

        time_dependent_frames = list(
            zip(sensor_time_dependent_frames[:-1],
                sensor_time_dependent_frames[1:]))
        constant_frames = list(
            zip(sensor_constant_frames[:-1], sensor_constant_frames[1:]))
        target_time_dependent_frames = list(
            zip(target_time_dependent_frames[:-1],
                target_time_dependent_frames[1:]))
        target_constant_frames = list(
            zip(target_constant_frames[:-1], target_constant_frames[1:]))

        time_dependent_frames.extend(target_time_dependent_frames)
        constant_frames.extend(target_constant_frames)

        for s, d in time_dependent_frames:
            quats = np.zeros((len(times), 4))
            avs = np.zeros((len(times), 3))
            for j, time in enumerate(times):
                state_matrix = spice.sxform(spice.frmnam(s), spice.frmnam(d),
                                            time)
                rotation_matrix, avs[j] = spice.xf2rav(state_matrix)
                quat_from_rotation = spice.m2q(rotation_matrix)
                quats[j, :3] = quat_from_rotation[1:]
                quats[j, 3] = quat_from_rotation[0]

            rotation = TimeDependentRotation(quats, times, s, d, av=avs)
            frame_chain.add_edge(rotation=rotation)

        for s, d in constant_frames:
            quats = np.zeros(4)
            rotation_matrix = spice.pxform(spice.frmnam(s), spice.frmnam(d),
                                           times[0])
            quat_from_rotation = spice.m2q(rotation_matrix)
            quats[:3] = quat_from_rotation[1:]
            quats[3] = quat_from_rotation[0]

            rotation = ConstantRotation(quats, s, d)

            frame_chain.add_edge(rotation=rotation)

        return frame_chain
Exemplo n.º 9
0
def test_last_time_dependent_frame_between():
    """
    Test frame tree structure:

          1
         / \
        /   \
       2     4
      /       \
     /         \
    3           5

    The rotations from 3 to 2 and 1 to 4 are time dependent.
    All other rotations are constant.
    """
    rotations = [
        ConstantRotation(np.array([1, 0, 0, 0]), 2, 1),
        TimeDependentRotation(
            np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
            np.array([1]), 4, 1),
        TimeDependentRotation(
            np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
            np.array([1]), 5, 4),
        ConstantRotation(np.array([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]),
                         3, 2)
    ]
    root_node = FrameNode(1)
    child_node_1 = FrameNode(2, parent=root_node, rotation=rotations[0])
    child_node_2 = FrameNode(3, parent=child_node_1, rotation=rotations[1])
    child_node_3 = FrameNode(4, parent=root_node, rotation=rotations[2])
    child_node_4 = FrameNode(5, parent=child_node_3, rotation=rotations[3])
    last_frame_from_root_to_child_2 = root_node.last_time_dependent_frame_between(
        child_node_2)
    last_frame_from_child_2_to_root = child_node_2.last_time_dependent_frame_between(
        root_node)
    last_frame_from_child_2_to_child_4 = child_node_2.last_time_dependent_frame_between(
        child_node_4)
    last_frame_from_child_4_to_child_2 = child_node_4.last_time_dependent_frame_between(
        child_node_2)

    assert last_frame_from_root_to_child_2 == child_node_2
    assert last_frame_from_child_2_to_root == child_node_1
    assert last_frame_from_child_2_to_child_4 == child_node_3
    assert last_frame_from_child_4_to_child_2 == child_node_2
Exemplo n.º 10
0
def test_time_dependent_time_dependent_composition():
    # 90 degree rotation about the X-axis to a 180 degree rotation about the X-axis
    quats1_2 = [[1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], [1, 0, 0, 0]]
    times1_2 = [0, 1]
    rot1_2 = TimeDependentRotation(quats1_2, times1_2, 1, 2)
    # -90 degree rotation about the X-axis to a 90 degree rotation about the X-axis
    quats2_3 = [[1.0 / np.sqrt(2), 0, 0, -1.0 / np.sqrt(2)],
                [1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)]]
    times2_3 = [0, 2]
    rot2_3 = TimeDependentRotation(quats2_3, times2_3, 2, 3)
    # compose to get no rotation to a 180 degree rotation about the X-axis to no rotation
    rot1_3 = rot2_3 * rot1_2
    assert isinstance(rot1_3, TimeDependentRotation)
    assert rot1_3.source == 1
    assert rot1_3.dest == 3
    expected_times = np.array([0, 1])
    expected_quats = np.array([[0, 0, 0, -1], [-1, 0, 0, 0]])
    np.testing.assert_equal(rot1_3.times, expected_times)
    np.testing.assert_almost_equal(rot1_3.quats, expected_quats)
Exemplo n.º 11
0
def test_slerp_single_time():
    rot = TimeDependentRotation([[0, 0, 0, 1]], [0],
                                1,
                                2,
                                av=[[np.pi / 2, 0, 0]])
    new_rot, new_avs = rot._slerp([-1, 3])
    expected_quats = [[-1 / np.sqrt(2), 0, 0, 1 / np.sqrt(2)],
                      [1 / np.sqrt(2), 0, 0, -1 / np.sqrt(2)]]
    expected_av = [[np.pi / 2, 0, 0], [np.pi / 2, 0, 0]]
    np.testing.assert_almost_equal(new_rot.as_quat(), expected_quats)
    np.testing.assert_equal(new_avs, expected_av)
Exemplo n.º 12
0
def test_slerp():
    test_quats = Rotation.from_euler('x',
                                     np.array([-135, -90, 0, 45, 90]),
                                     degrees=True).as_quat()
    rot = TimeDependentRotation(test_quats, [-0.5, 0, 1, 1.5, 2], 1, 2)
    new_rots, new_avs = rot._slerp(np.arange(-3, 5))
    expected_rot = Rotation.from_euler(
        'x', [-360, -270, -180, -90, 0, 90, 180, 270], degrees=True)
    np.testing.assert_almost_equal(new_rots.as_quat(), expected_rot.as_quat())
    np.testing.assert_almost_equal(np.degrees(new_avs),
                                   np.repeat([[90, 0, 0]], 8, 0))
Exemplo n.º 13
0
def test_time_dependent_inverse():
    quats1_2 = [[1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], [1, 0, 0, 0]]
    times1_2 = [0, 1]
    rot1_2 = TimeDependentRotation(quats1_2, times1_2, 1, 2)
    rot2_1 = rot1_2.inverse()
    assert rot2_1.source == 2
    assert rot2_1.dest == 1
    expected_quats = np.array([[1.0 / np.sqrt(2), 0, 0, -1.0 / np.sqrt(2)],
                               [1, 0, 0, 0]])
    np.testing.assert_equal(rot2_1.times, np.array(times1_2))
    np.testing.assert_almost_equal(rot2_1.quats, expected_quats)
Exemplo n.º 14
0
def test_rotate_velocity_at():
    test_quats = Rotation.from_euler(
        'xyz', [[0, 0, 0], [-90, 0, 0], [-90, 180, 0], [-90, 180, 90]],
        degrees=True).as_quat()
    rot = TimeDependentRotation(test_quats, [0, 1, 2, 3], 1, 2)
    input_pos = [[1, 2, 3], [1, 2, 3], [1, 2, 3]]
    input_vel = [[-1, -2, -3], [-1, -2, -3], [-1, -2, -3]]
    input_times = [1, 2, 3]
    rot_vel = rot.rotate_velocity_at(input_pos, input_vel, input_times)
    np.testing.assert_almost_equal(
        rot_vel,
        [[-1, -3 + np.pi, 2 + 3 * np.pi / 2], [1 + 3 * np.pi, -3 + np.pi, -2],
         [3, 1 - np.pi, -2 - np.pi / 2]])
Exemplo n.º 15
0
def test_time_dependent_inverse():
    quats1_2 = [[1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], [1, 0, 0, 0]]
    times1_2 = [0, 1]
    av1_2 = [[np.pi / 2, 0, 0], [np.pi / 2, 0, 0]]
    rot1_2 = TimeDependentRotation(quats1_2, times1_2, 1, 2, av=av1_2)
    rot2_1 = rot1_2.inverse()
    assert rot2_1.source == 2
    assert rot2_1.dest == 1
    expected_quats = [[1.0 / np.sqrt(2), 0, 0, -1.0 / np.sqrt(2)],
                      [1, 0, 0, 0]]
    expected_av = [[-np.pi / 2, 0, 0], [-np.pi / 2, 0, 0]]
    np.testing.assert_equal(rot2_1.times, times1_2)
    np.testing.assert_almost_equal(rot2_1.quats, expected_quats)
    np.testing.assert_almost_equal(rot2_1.av, expected_av)
Exemplo n.º 16
0
def test_slerp_variable_velocity():
    test_quats = Rotation.from_euler(
        'xyz', [[0, 0, 0], [-90, 0, 0], [-90, 180, 0], [-90, 180, 90]],
        degrees=True).as_quat()
    rot = TimeDependentRotation(test_quats, [0, 1, 2, 3], 1, 2)
    new_rots, new_avs = rot._slerp([-0.5, 0.5, 1.5, 2.5, 3.5])
    expected_rot = Rotation.from_euler('xyz',
                                       [[45, 0, 0], [-45, 0, 0], [-90, 90, 0],
                                        [-90, 180, 45], [-90, 180, 135]],
                                       degrees=True)
    np.testing.assert_almost_equal(new_rots.as_quat(), expected_rot.as_quat())
    np.testing.assert_almost_equal(
        np.degrees(new_avs),
        [[-90, 0, 0], [-90, 0, 0], [0, 180, 0], [0, 0, 90], [0, 0, 90]])
Exemplo n.º 17
0
def test_time_dependent_constant_composition():
    rot1_2 = ConstantRotation([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], 1, 2)
    quats = [[1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], [1, 0, 0, 0]]
    times = [0, 1]
    av = [[np.pi / 2, 0, 0], [np.pi / 2, 0, 0]]
    rot2_3 = TimeDependentRotation(quats, times, 2, 3, av=av)
    rot1_3 = rot2_3 * rot1_2
    assert isinstance(rot1_3, TimeDependentRotation)
    assert rot1_3.source == 1
    assert rot1_3.dest == 3
    expected_quats = [[1, 0, 0, 0],
                      [1.0 / np.sqrt(2), 0, 0, -1.0 / np.sqrt(2)]]
    np.testing.assert_equal(rot1_3.times, times)
    np.testing.assert_almost_equal(rot1_3.quats, expected_quats)
    np.testing.assert_almost_equal(rot1_3.av, av)
Exemplo n.º 18
0
def test_time_dependent_constant_composition():
    # 90 degree rotation about the X-axis
    rot1_2 = ConstantRotation([1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], 1, 2)
    # 90 degree rotation about the X-axis to a 180 degree rotation about the X-axis
    quats = [[1.0 / np.sqrt(2), 0, 0, 1.0 / np.sqrt(2)], [1, 0, 0, 0]]
    times = [0, 1]
    rot2_3 = TimeDependentRotation(quats, times, 2, 3)
    # compose to get a 180 degree rotation about the X-axis to a 270 degree rotation about the X-axis
    rot1_3 = rot2_3 * rot1_2
    assert isinstance(rot1_3, TimeDependentRotation)
    assert rot1_3.source == 1
    assert rot1_3.dest == 3
    expected_quats = np.array([[1, 0, 0, 0],
                               [1.0 / np.sqrt(2), 0, 0, -1.0 / np.sqrt(2)]])
    np.testing.assert_equal(rot1_3.times, np.array(times))
    np.testing.assert_almost_equal(rot1_3.quats, expected_quats)
Exemplo n.º 19
0
    def frame_chain(self):
        if not hasattr(self, '_frame_chain'):
            nadir = self._props.get('nadir', False)
            self._frame_chain = FrameChain.from_spice(
                sensor_frame=self.sensor_frame_id,
                target_frame=self.target_frame_id,
                center_ephemeris_time=self.center_ephemeris_time,
                ephemeris_times=self.ephemeris_time,
                nadir=nadir)

            if nadir:
                # Logic for nadir calculation was taken from ISIS3
                #  SpiceRotation::setEphemerisTimeNadir
                rotation = self._frame_chain.compute_rotation(
                    self.target_frame_id, 1)
                p_vec, v_vec, times = self.sensor_position
                rotated_positions = rotation.apply_at(p_vec, times)
                rotated_velocities = rotation.rotate_velocity_at(
                    p_vec, v_vec, times)

                p_vec = rotated_positions
                v_vec = rotated_velocities

                velocity_axis = 2
                # Get the default line translation with no potential flipping
                # from the driver
                trans_x = np.array(
                    list(spice.gdpool('INS{}_ITRANSL'.format(self.ikid), 0,
                                      3)))

                if (trans_x[0] < trans_x[1]):
                    velocity_axis = 1

                quats = [
                    spice.m2q(
                        spice.twovec(-p_vec[i], 3, v_vec[i], velocity_axis))
                    for i, time in enumerate(times)
                ]
                quats = np.array(quats)[:, [1, 2, 3, 0]]

                rotation = TimeDependentRotation(quats, times, 1,
                                                 self.sensor_frame_id)
                self._frame_chain.add_edge(rotation)

        return self._frame_chain
Exemplo n.º 20
0
def test_reinterpolate():
    rot = TimeDependentRotation([[0, 0, 0, 1], [0, 0, 0, 1]], [0, 1], 1, 2)
    new_rot = rot.reinterpolate(np.arange(-3, 5))
    assert new_rot.source == rot.source
    assert new_rot.dest == rot.dest
    np.testing.assert_equal(new_rot.times, np.arange(-3, 5))
Exemplo n.º 21
0
def test_slerp_constant_rotation():
    rot = TimeDependentRotation([[0, 0, 0, 1]], [0], 1, 2)
    new_rot, new_avs = rot._slerp([-1, 3])
    np.testing.assert_equal(new_rot.as_quat(), [[0, 0, 0, 1], [0, 0, 0, 1]])
    np.testing.assert_equal(new_avs, [[0, 0, 0], [0, 0, 0]])