Example #1
0
def test_spline_properties():
    times = np.array([0, 5, 15, 27])
    angles = [[-5, 10, 27], [3, 5, 38], [-12, 10, 25], [-15, 20, 11]]

    rotations = Rotation.from_euler('xyz', angles, degrees=True)
    spline = RotationSpline(times, rotations)

    assert_allclose(spline(times).as_euler('xyz', degrees=True), angles)
    assert_allclose(spline(0).as_euler('xyz', degrees=True), angles[0])

    h = 1e-8
    rv0 = spline(times).as_rotvec()
    rvm = spline(times - h).as_rotvec()
    rvp = spline(times + h).as_rotvec()
    assert_allclose(rv0, 0.5 * (rvp + rvm), rtol=1e-15)

    r0 = spline(times, 1)
    rm = spline(times - h, 1)
    rp = spline(times + h, 1)
    assert_allclose(r0, 0.5 * (rm + rp), rtol=1e-14)

    a0 = spline(times, 2)
    am = spline(times - h, 2)
    ap = spline(times + h, 2)
    assert_allclose(a0, am, rtol=1e-7)
    assert_allclose(a0, ap, rtol=1e-7)
Example #2
0
  def _interpolate_rotation(self):
    quats = [p[-1] for p in self.poses]
    self.rots = Rotation.from_quat(quats)

    # Import here since older versions of scipy don't have RotationSpline.
    from scipy.spatial.transform import RotationSpline  # pylint: disable=g-import-not-at-top
    self.rot_spline = RotationSpline(self.times, self.rots)
Example #3
0
def stationary_rotation(dt, lat, alt, Cnb, Cbs=None):
    """Simulate readings on a stationary bench.

    Parameters
    ----------
    dt : float
        Time step.
    lat : float
        Latitude of the place.
    alt : float
        Altitude of the place.
    Cnb : ndarray, shape (n_points, 3, 3)
        Body attitude matrix.
    Cbs : ndarray with shape (3, 3) or (n_points, 3, 3) or None
        Sensor assembly attitude matrix relative to the body axes. If None,
        (default) identity attitude is assumed.

    Returns
    -------
    gyro, accel : ndarray, shape (n_points - 1, 3)
        Gyro and accelerometer readings.
    """
    n_points = Cnb.shape[0]

    time = dt * np.arange(n_points)
    lon_inertial = np.rad2deg(earth.RATE) * time
    lat = np.full_like(lon_inertial, lat)
    Cin = dcm.from_llw(lat, lon_inertial)

    R = transform.lla_to_ecef(lat, lon_inertial, alt)
    v_s = CubicSpline(time, R).derivative()

    if Cbs is None:
        Cns = Cnb
    else:
        Cns = util.mm_prod(Cnb, Cbs)

    Cis = util.mm_prod(Cin, Cns)
    Cib_spline = RotationSpline(time, Rotation.from_matrix(Cis))
    a = Cib_spline.interpolator.c[2]
    b = Cib_spline.interpolator.c[1]
    c = Cib_spline.interpolator.c[0]

    g = earth.gravitation_ecef(lat, lon_inertial, alt)
    a_s = v_s.derivative()
    d = a_s.c[1] - g[:-1]
    e = a_s.c[0] - np.diff(g, axis=0) / dt

    d = util.mv_prod(Cis[:-1], d, at=True)
    e = util.mv_prod(Cis[:-1], e, at=True)

    gyros, accels = _compute_readings(dt, a, b, c, d, e)

    return gyros, accels
Example #4
0
def test_constant_attitude():
    times = np.arange(10)
    rotations = Rotation.from_rotvec(np.ones((10, 3)))
    spline = RotationSpline(times, rotations)

    times_check = np.linspace(-1, 11)
    assert_allclose(spline(times_check).as_rotvec(), 1, rtol=1e-15)
    assert_allclose(spline(times_check, 1), 0, atol=1e-17)
    assert_allclose(spline(times_check, 2), 0, atol=1e-17)

    assert_allclose(spline(5.5).as_rotvec(), 1, rtol=1e-15)
    assert_allclose(spline(5.5, 1), 0, atol=1e-17)
    assert_allclose(spline(5.5, 2), 0, atol=1e-17)
Example #5
0
def test_spline_2_rotations():
    times = [0, 10]
    rotations = Rotation.from_euler('xyz', [[0, 0, 0], [10, -20, 30]],
                                    degrees=True)
    spline = RotationSpline(times, rotations)

    rv = (rotations[0].inv() * rotations[1]).as_rotvec()
    rate = rv / (times[1] - times[0])
    times_check = np.array([-1, 5, 12])
    dt = times_check - times[0]
    rv_ref = rate * dt[:, None]

    assert_allclose(spline(times_check).as_rotvec(), rv_ref)
    assert_allclose(spline(times_check, 1), np.resize(rate, (3, 3)))
    assert_allclose(spline(times_check, 2), 0, atol=1e-16)
Example #6
0
def test_error_handling():
    raises(ValueError, RotationSpline, [1.0], Rotation.random())

    r = Rotation.random(10)
    t = np.arange(10).reshape(5, 2)
    raises(ValueError, RotationSpline, t, r)

    t = np.arange(9)
    raises(ValueError, RotationSpline, t, r)

    t = np.arange(10)
    t[5] = 0
    raises(ValueError, RotationSpline, t, r)

    t = np.arange(10)

    s = RotationSpline(t, r)
    raises(ValueError, s, 10, -1)

    raises(ValueError, s, np.arange(10).reshape(5, 2))
Example #7
0
    def getGS2RSFlow(depth_rs, cam, anchors_t_r):
        num_anchor = int(anchors_t_r.shape[0] / 6)
        h, w = depth_rs.shape[:2]
        flow_gs2rs = np.empty([h, w, 2], dtype=np.float32)
        flow_gs2rs[:] = np.nan

        tm = np.arange(num_anchor + 1) / num_anchor
        ts, rs = [[0, 0, 0]], [[0, 0, 0]]
        for i in range(num_anchor):
            ts.append(list(anchors_t_r[(3 * i):(3 * i + 3)]))
            rs.append(
                list(anchors_t_r[(3 * num_anchor + 3 * i):(3 * num_anchor +
                                                           3 * i + 3)]))
        t_spline = CubicSpline(tm, ts)
        R_spline = RotationSpline(tm, Rotation.from_rotvec(rs))

        K = np.array([[cam[0], 0, cam[2]], [0, cam[1], cam[3]], [0, 0, 1]])
        K_i = LA.inv(K)

        # Project from rs to gs
        for v_rs in range(h):
            tm = v_rs / (h - 1)
            KRK_i = np.matmul(np.matmul(K, R_spline(tm).as_matrix()), K_i)
            Kt = np.matmul(K, t_spline(tm))
            for u_rs in range(w):
                if np.isnan(depth_rs[v_rs, u_rs]):
                    continue

                p_gs = depth_rs[v_rs, u_rs] * \
                    np.matmul(KRK_i, np.array([u_rs, v_rs, 1])) + Kt
                u_gs, v_gs = p_gs[0] / p_gs[2], p_gs[1] / p_gs[2]
                if not np.isnan(u_gs):
                    u_gsi, v_gsi = int(u_gs + 0.5), int(v_gs + 0.5)
                    if 0 <= u_gsi < w and 0 <= v_gsi < h:
                        flow_gs2rs[v_gsi, u_gsi, 0] = u_rs - u_gs
                        flow_gs2rs[v_gsi, u_gsi, 1] = v_rs - v_gs

        return flow_gs2rs
Example #8
0
    def __init__(self, gt_file, T_gt_target):
        gt_reader = csv.reader(open(gt_file), delimiter=",")
        next(gt_reader)  # skip the header line
        gt_ns_t_q = np.array(list(gt_reader))
        gt_ns = np.array(gt_ns_t_q[:, 0]).astype(np.long)
        gt_t_q = np.array(gt_ns_t_q[:, 1:]).astype(np.float)
        self.first_ns = gt_ns[1]
        self.last_ns = gt_ns[-2]

        # transfer to target coordinate
        ts, Rs = [], []
        for i in range(len(gt_ns)):
            T_w_gt = np.identity(4)
            T_w_gt[0:3, 3] = gt_t_q[i, :3]
            # convert to qx qy qz qw for from_quat
            gt_q = gt_t_q[i, np.ix_([4, 5, 6, 3])]
            T_w_gt[0:3, 0:3] = Rotation.from_quat(gt_q).as_matrix()
            T_w_target = np.matmul(T_w_gt, T_gt_target)
            ts.append(T_w_target[0:3, 3])
            Rs.append(T_w_target[0:3, 0:3])

        # splines
        self.t_spline = CubicSpline(gt_ns, ts)
        self.R_spline = RotationSpline(gt_ns, Rotation.from_matrix(Rs))
Example #9
0
def from_position(dt, lat, lon, alt, h, p, r):
    """Generate inertial readings given position and attitude.

    Parameters
    ----------
    dt : float
        Time step.
    lat, lon, alt : array_like, shape (n_points,)
        Time series of latitude, longitude and altitude.
    h, p, r : array_like, shape (n_points,)
        Time series of heading, pitch and roll angles.

    Returns
    -------
    traj : DataFrame
        Trajectory. Contains n_points rows.
    gyro : ndarray, shape (n_points - 1, 3)
        Gyro readings.
    accel : ndarray, shape (n_points - 1, 3)
        Accelerometer readings.
    """
    lat = np.asarray(lat, dtype=float)
    lon = np.asarray(lon, dtype=float)
    alt = np.asarray(alt, dtype=float)
    h = np.asarray(h, dtype=float)
    p = np.asarray(p, dtype=float)
    r = np.asarray(r, dtype=float)
    n_points = lat.shape[0]

    time = dt * np.arange(n_points)
    lat_inertial = lat.copy()
    lon_inertial = lon.copy()
    lon_inertial += np.rad2deg(earth.RATE) * time
    Cin = dcm.from_llw(lat_inertial, lon_inertial)

    R = transform.lla_to_ecef(lat_inertial, lon_inertial, alt)
    v_s = CubicSpline(time, R).derivative()
    v = v_s(time)

    V = v.copy()
    V[:, 0] += earth.RATE * R[:, 1]
    V[:, 1] -= earth.RATE * R[:, 0]
    V = util.mv_prod(Cin, V, at=True)

    Cnb = dcm.from_hpr(h, p, r)
    Cib = util.mm_prod(Cin, Cnb)

    Cib_spline = RotationSpline(time, Rotation.from_matrix(Cib))
    a = Cib_spline.interpolator.c[2]
    b = Cib_spline.interpolator.c[1]
    c = Cib_spline.interpolator.c[0]

    g = earth.gravitation_ecef(lat_inertial, lon_inertial, alt)
    a_s = v_s.derivative()
    d = a_s.c[1] - g[:-1]
    e = a_s.c[0] - np.diff(g, axis=0) / dt

    d = util.mv_prod(Cib[:-1], d, at=True)
    e = util.mv_prod(Cib[:-1], e, at=True)

    gyros, accels = _compute_readings(dt, a, b, c, d, e)

    traj = pd.DataFrame(index=np.arange(time.shape[0]))
    traj['lat'] = lat
    traj['lon'] = lon
    traj['alt'] = alt
    traj['VE'] = V[:, 0]
    traj['VN'] = V[:, 1]
    traj['VU'] = V[:, 2]
    traj['h'] = h
    traj['p'] = p
    traj['r'] = r

    return traj, gyros, accels
Example #10
0
def from_velocity(dt, lat0, lon0, alt0, VE, VN, VU, h, p, r):
    """Generate inertial readings given velocity and attitude.

    Parameters
    ----------
    dt : float
        Time step.
    lat0, lon0, alt0 : float
        Initial values of latitude, longitude and altitude.
    VE, VN, VU : array_like, shape (n_points,)
        Time series of East, North and vertical velocity components.
    h, p, r : array_like, shape (n_points,)
        Time series of heading, pitch and roll angles.

    Returns
    -------
    traj : DataFrame
        Trajectory. Contains n_points rows.
    gyro : ndarray, shape (n_points - 1, 3)
        Gyro readings.
    accel : ndarray, shape (n_points - 1, 3)
        Accelerometer readings.
    """
    MAX_ITER = 3
    ACCURACY = 0.01

    VE = np.asarray(VE, dtype=float)
    VN = np.asarray(VN, dtype=float)
    VU = np.asarray(VU, dtype=float)
    h = np.asarray(h, dtype=float)
    p = np.asarray(p, dtype=float)
    r = np.asarray(r, dtype=float)
    n_points = VE.shape[0]
    time = np.arange(n_points) * dt

    VU_spline = _QuadraticSpline(time, VU)
    alt_spline = VU_spline.antiderivative()
    alt = alt0 + alt_spline(time)

    lat0 = np.deg2rad(lat0)
    lon0 = np.deg2rad(lon0)
    lat = lat0

    for iteration in range(MAX_ITER):
        _, rn = earth.principal_radii(np.rad2deg(lat))
        rn += alt
        dlat_spline = _QuadraticSpline(time, VN / rn)
        lat_spline = dlat_spline.antiderivative()
        lat_new = lat_spline(time) + lat0
        delta = (lat - lat_new) * rn
        lat = lat_new
        if np.all(np.abs(delta) < ACCURACY):
            break

    re, _ = earth.principal_radii(np.rad2deg(lat))
    re += alt
    dlon_spline = _QuadraticSpline(time, VE / (re * np.cos(lat)))
    lon_spline = dlon_spline.antiderivative()
    lon = lon_spline(time) + lon0

    lat = np.rad2deg(lat)
    lon = np.rad2deg(lon)
    lon_inertial = lon + np.rad2deg(earth.RATE) * time
    Cin = dcm.from_llw(lat, lon_inertial)

    v = np.vstack((VE, VN, VU)).T
    v = util.mv_prod(Cin, v)
    R = transform.lla_to_ecef(lat, lon_inertial, alt)
    v[:, 0] -= earth.RATE * R[:, 1]
    v[:, 1] += earth.RATE * R[:, 0]
    v_s = _QuadraticSpline(time, v)

    Cnb = dcm.from_hpr(h, p, r)
    Cib = util.mm_prod(Cin, Cnb)

    Cib_spline = RotationSpline(time, Rotation.from_matrix(Cib))
    a = Cib_spline.interpolator.c[2]
    b = Cib_spline.interpolator.c[1]
    c = Cib_spline.interpolator.c[0]

    g = earth.gravitation_ecef(lat, lon_inertial, alt)
    a_s = v_s.derivative()
    d = a_s.c[1] - g[:-1]
    e = a_s.c[0] - np.diff(g, axis=0) / dt

    d = util.mv_prod(Cib[:-1], d, at=True)
    e = util.mv_prod(Cib[:-1], e, at=True)

    gyros, accels = _compute_readings(dt, a, b, c, d, e)

    traj = pd.DataFrame(index=np.arange(n_points))
    traj['lat'] = lat
    traj['lon'] = lon
    traj['alt'] = alt
    traj['VE'] = VE
    traj['VN'] = VN
    traj['VU'] = VU
    traj['h'] = h
    traj['p'] = p
    traj['r'] = r

    return traj, gyros, accels
Example #11
0
        p_t[:,i] = row[5:8]
        bq_t[:,i] = row[8:12]
        bp_t[:,i] = row[12:15]

# clean up temporary CSV
os.system('rm ./' + csvfn)

# assume orientation of plate is constant
Rot_board = Rotation.from_quat(bq_t.T)[0]

rot_t = Rot_board.inv() * Rotation.from_quat(q_t.T)
p_t =  Rot_board.inv().apply(p_t.T - bp_t.T).T

t = t-t[0]
# calculate derivatives with spline interpolation
rspline = RotationSpline(t, rot_t)
pspline = CubicSpline(t, p_t.T)

quat_t = rspline(t).as_quat().T

w_t = rspline(t, 1).T
dp_t = pspline(t, 1).T

# butterworth filter of order 2 to smooth velocity states

# sampling frequency
fs = 40.

# Cut-off frequency of angular velocity filter. < 20.0 (Nyquist)
fc_w = 5.
from scipy.spatial.transform import Rotation, RotationSpline

# Define the sequence of times and rotations from the Euler angles:

times = [0, 10, 20, 40]
angles = [[-10, 20, 30], [0, 15, 40], [-30, 45, 30], [20, 45, 90]]
rotations = Rotation.from_euler('XYZ', angles, degrees=True)

# Create the interpolator object:

spline = RotationSpline(times, rotations)

# Interpolate the Euler angles, angular rate and acceleration:

angular_rate = np.rad2deg(spline(times, 1))
angular_acceleration = np.rad2deg(spline(times, 2))
times_plot = np.linspace(times[0], times[-1], 100)
angles_plot = spline(times_plot).as_euler('XYZ', degrees=True)
angular_rate_plot = np.rad2deg(spline(times_plot, 1))
angular_acceleration_plot = np.rad2deg(spline(times_plot, 2))

# On this plot you see that Euler angles are continuous and smooth:

import matplotlib.pyplot as plt
plt.plot(times_plot, angles_plot)
plt.plot(times, angles, 'x')
plt.title("Euler angles")
plt.show()

# The angular rate is also smooth:
Example #13
0
def from_position(dt, lat, lon, alt, h, p, r, sensor_type='increment'):
    """Generate inertial readings given position and attitude.

    Parameters
    ----------
    dt : float
        Time step.
    lat, lon, alt : array_like, shape (n_points,)
        Time series of latitude, longitude and altitude.
    h, p, r : array_like, shape (n_points,)
        Time series of heading, pitch and roll angles.
    sensor_type: 'increment' or 'rate', optional
        Type of sensor to generate. If 'increment' (default), then integrals
        over sampling intervals are generated (in rad and m/s).
        If 'rate', then instantaneous rate values are generated
        (in rad/s and /m/s/s).

    Returns
    -------
    traj : DataFrame
        Trajectory. Contains n_points rows.
    gyro : ndarray, shape (n_points - 1, 3) or (n_points, 3)
        Gyro readings.
    accel : ndarray, shape (n_points - 1, 3) or (n_points, 3)
        Accelerometer readings.
    """
    if sensor_type not in ['rate', 'increment']:
        raise ValueError("`sensor_type` must be 'rate' or 'increment'.")

    lat = np.asarray(lat, dtype=float)
    lon = np.asarray(lon, dtype=float)
    alt = np.asarray(alt, dtype=float)
    h = np.asarray(h, dtype=float)
    p = np.asarray(p, dtype=float)
    r = np.asarray(r, dtype=float)
    n_points = lat.shape[0]

    time = dt * np.arange(n_points)
    lat_inertial = lat.copy()
    lon_inertial = lon.copy()
    lon_inertial += np.rad2deg(earth.RATE) * time
    Cin = dcm.from_llw(lat_inertial, lon_inertial)

    R = transform.lla_to_ecef(lat_inertial, lon_inertial, alt)
    v_s = CubicSpline(time, R).derivative()
    v = v_s(time)

    V = v.copy()
    V[:, 0] += earth.RATE * R[:, 1]
    V[:, 1] -= earth.RATE * R[:, 0]
    V = util.mv_prod(Cin, V, at=True)

    Cnb = dcm.from_hpr(h, p, r)
    Cib = util.mm_prod(Cin, Cnb)

    Cib_spline = RotationSpline(time, Rotation.from_matrix(Cib))
    g = earth.gravitation_ecef(lat_inertial, lon_inertial, alt)

    if sensor_type == 'rate':
        gyros = Cib_spline(time, 1)
        accels = util.mv_prod(Cib, v_s(time, 1) - g, at=True)
    elif sensor_type == 'increment':
        a = Cib_spline.interpolator.c[2]
        b = Cib_spline.interpolator.c[1]
        c = Cib_spline.interpolator.c[0]

        a_s = v_s.derivative()
        d = a_s.c[1] - g[:-1]
        e = a_s.c[0] - np.diff(g, axis=0) / dt

        d = util.mv_prod(Cib[:-1], d, at=True)
        e = util.mv_prod(Cib[:-1], e, at=True)

        gyros, accels = _compute_increment_readings(dt, a, b, c, d, e)
    else:
        assert False

    traj = pd.DataFrame(index=np.arange(time.shape[0]))
    traj['lat'] = lat
    traj['lon'] = lon
    traj['alt'] = alt
    traj['VE'] = V[:, 0]
    traj['VN'] = V[:, 1]
    traj['VU'] = V[:, 2]
    traj['h'] = h
    traj['p'] = p
    traj['r'] = r

    return traj, gyros, accels