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)
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)
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
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)
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)
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))
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
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))
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
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
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:
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