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