def plot_3d_trajectory(ground_truth,
feat_time=None,
qzetas=None,
depths=None,
ids=None,
p_b_c=np.zeros((3, 1)),
q_b_c=Quaternion.Identity(),
fighandle=None):
pos_time = ground_truth[:, 0]
position = ground_truth[:, 1:4]
orientation = ground_truth[:, 4:8]
if fighandle is not None:
ax = fighandle
else:
plt.figure(2, figsize=(14, 10))
ax = plt.subplot(111, projection='3d')
plt.plot(position[:1, 0], position[:1, 1], position[:1, 2], 'kx')
plt.plot(position[:, 0], position[:, 1], position[:, 2], 'c-')
ax.set_xlabel('X axis')
ax.set_ylabel('Y axis')
ax.set_zlabel('Z axis')
e_x = 0.1 * np.array([[1., 0, 0]]).T
e_y = 0.1 * np.array([[0, 1., 0]]).T
e_z = 0.1 * np.array([[0, 0, 1.]]).T
if qzetas is not None:
id_indices = np.unique(np.hstack(ids))
colors = get_colors(len(id_indices), plt.cm.jet)
for i in range(len(position) / 25):
j = i * 25
q_i_b = Quaternion(orientation[j, :, None])
body_pos = position[j, :, None]
x_end = body_pos + q_i_b.rot(e_x)
y_end = body_pos + q_i_b.rot(e_y)
z_end = body_pos + q_i_b.rot(e_z)
plt.plot([body_pos[0, 0], x_end[0, 0]], [body_pos[1, 0], x_end[1, 0]],
[body_pos[2, 0], x_end[2, 0]], 'r-')
plt.plot([body_pos[0, 0], y_end[0, 0]], [body_pos[1, 0], y_end[1, 0]],
[body_pos[2, 0], y_end[2, 0]], 'g-')
plt.plot([body_pos[0, 0], z_end[0, 0]], [body_pos[1, 0], z_end[1, 0]],
[body_pos[2, 0], z_end[2, 0]], 'b-')
if feat_time is not None:
feat_idx = np.argmin(feat_time < pos_time[j])
camera_pos = body_pos + q_i_b.invrot(p_b_c)
for qz, d, id in zip(qzetas[feat_idx], depths[feat_idx],
ids[feat_idx]):
if np.isfinite(qz).all() and np.isfinite(d):
q_c_z = Quaternion(qz[:, None])
zeta_end = camera_pos + q_i_b.rot(
q_b_c.rot(q_c_z.rot(10. * e_z * d)))
plt.plot([camera_pos[0, 0], zeta_end[0, 0]],
[camera_pos[1, 0], zeta_end[1, 0]],
[camera_pos[2, 0], zeta_end[2, 0]],
'--',
color=colors[np.where(id_indices == id)[0][0]],
lineWidth='1.0')
def generate_data():
dt = 0.0033
t = np.arange(dt, 120.1, dt)
g = np.array([[0, 0, 9.80665]]).T
q0 = Quaternion(np.array([[1, 0.0, 0, 0]]).T)
q0.normalize()
q = np.zeros((len(t), 4))
q[0, :, None] = q0.elements
frequencies = np.array([[1.0, 1.5, -1.0]]).T
amplitudes = np.array([[1.0, 1.0, 1.0]]).T
omega = amplitudes * np.sin(frequencies * t)
acc = np.zeros([3, len(t)])
for i in tqdm(range(len(t))):
if i == 0.0:
continue
quat = Quaternion(q[i - 1, :, None])
q[i, :, None] = (quat + omega[:, i, None] * dt).elements
acc[:, i, None] = -quat.rot(g)
data = dict()
data['truth_NED'] = dict()
data['truth_NED']['pos'] = np.zeros((len(t), 3))
data['truth_NED']['vel'] = np.zeros((len(t), 3))
data['truth_NED']['att'] = q
data['truth_NED']['t'] = t
data['imu_data'] = dict()
data['imu_data']['t'] = t
data['imu_data']['acc'] = acc.T
data['imu_data']['gyro'] = omega.T
landmarks = np.array([[0, 0, 1], [0, 0, 1], [1, 0, 1], [1, 1, 1]])
landmarks = np.random.uniform(-25, 25, (2, 3))
#[0, 9, 1],
#[2, 3, 5]
data['features'] = dict()
data['features']['t'] = data['truth_NED']['t']
data['features']['zeta'], data['features']['depth'] = add_landmark(
data['truth_NED']['pos'], data['truth_NED']['att'], landmarks)
cPickle.dump(data, open('generated_data.pkl', 'wb'))
def calculate_velocity_from_position(t, position, orientation):
# Calculate body-fixed velocity by differentiating position and rotating
# into the body frame
b, a = scipy.signal.butter(8, 0.03) # Create a Butterworth Filter
# differentiate Position
delta_x = np.diff(position, axis=0)
delta_t = np.diff(t)
unfiltered_inertial_velocity = np.vstack((np.zeros((1, 3)), delta_x / delta_t[:, None]))
# Filter
v_inertial = scipy.signal.filtfilt(b, a, unfiltered_inertial_velocity, axis=0)
# Rotate into Body Frame
vel_data = []
for i in range(len(t)):
q_I_b = Quaternion(orientation[i, :, None])
vel_data.append(q_I_b.rot(v_inertial[i, None].T).T)
vel_data = np.array(vel_data).squeeze()
return vel_data
def h_feat(self, x, **kwargs):
# assert x.shape == (xZ + 5 * self.len_features, 1) and isinstance(i, int)
i = self.global_to_local_feature_id[kwargs['i']]
dxZETA_i = dxZ + i * 3
q_zeta = Quaternion(x[xZ + i * 5:xZ + i * 5 + 4])
zeta = q_zeta.rot(self.khat)
sk_zeta = skew(zeta)
ezT_zeta = (self.khat.T.dot(zeta)).squeeze()
T_z = T_zeta(q_zeta)
h = self.cam_F.dot(zeta) / ezT_zeta + self.cam_center
dhdx = np.zeros((2, dxZ + 3 * self.len_features))
dhdx[:, dxZETA_i:dxZETA_i +
2] = -self.cam_F.dot((sk_zeta.dot(T_z)) / ezT_zeta -
zeta.dot(self.khat.T).dot(sk_zeta).dot(T_z) /
(ezT_zeta * ezT_zeta))
return h, dhdx
def h_pixel_vel(self, x, i, u):
# assert x.shape == (xZ + 5 * self.len_features, 1) and isinstance(i, int) and u.shape == (6, 1)
vel = x[xVEL:xVEL + 3]
omega = u[uG:uG + 3] - x[xB_G:xB_G + 3]
# Camera Dynamics
vel_c_i = self.q_b_c.invrot(vel + skew(omega).dot(self.p_b_c))
omega_c_i = self.q_b_c.invrot(omega)
q_c_z = Quaternion(x[xZ + i * 5:xZ + i * 5 + 4])
rho = x[xZ + i * 5 + 4]
zeta = q_c_z.rot(self.khat)
sk_vel = skew(vel_c_i)
sk_ez = skew(self.khat)
sk_zeta = skew(zeta)
R_b_c = self.q_b_c.R
# TODO: Need to convert to camera dynamics
h = -self.focal_len * I_2x3.dot(sk_ez).dot(rho *
(sk_zeta.dot(vel_c_i)) +
omega_c_i)
ZETA_i = dxZ + 3 * i
RHO_i = dxZ + 3 * i + 2
dhdx = np.zeros((2, dxZ + 3 * self.len_features))
dhdx[:, dxVEL:dxVEL +
3] = -self.focal_len * rho * I_2x3.dot(sk_ez).dot(sk_zeta)
dhdx[:, ZETA_i:ZETA_i + 2] = -self.focal_len * rho * I_2x3.dot(
sk_ez).dot(sk_vel).dot(sk_zeta).dot(T_zeta(q_c_z))
dhdx[:, RHO_i, None] = -self.focal_len * I_2x3.dot(sk_ez).dot(
sk_zeta).dot(vel_c_i)
dhdx[:, dxB_G:dxB_G + 3] = self.focal_len * I_2x3.dot(sk_ez).dot(
R_b_c - rho * sk_zeta.dot(R_b_c).dot(skew(self.p_b_c)))
# dhdx[:, dxB_G:dxB_G + 3] = self.focal_len * I_2x3.dot(sk_ez).dot(I_zz)
return h, dhdx
def dynamics(self, x, u):
# assert x.shape == (xZ+5*self.len_features, 1) and u.shape == (6,1)
# Reset Matrix Workspace
self.dx.fill(0.0)
self.A.fill(0.0)
self.G.fill(0.0)
vel = x[xVEL:xVEL + 3]
q_I_b = Quaternion(x[xATT:xATT + 4])
omega = u[uG:uG + 3] - x[xB_G:xB_G + 3]
acc = u[uA:uA + 3] - x[xB_A:xB_A + 3]
acc_z = np.array([[0, 0, acc[2, 0]]]).T
mu = x[xMU, 0]
gravity_B = q_I_b.invrot(self.gravity)
vel_I = q_I_b.invrot(vel)
vel_xy = I_2x3.T.dot(I_2x3).dot(vel)
# CALCULATE STATE DYNAMICS
self.dx[dxPOS:dxPOS + 3] = vel_I
if self.use_drag_term:
self.dx[dxVEL:dxVEL + 3] = acc_z + gravity_B - mu * vel_xy
else:
self.dx[dxVEL:dxVEL + 3] = acc + gravity_B
self.dx[dxATT:dxATT + 3] = omega
###################################
# STATE JACOBIAN
self.A[dxPOS:dxPOS + 3, dxVEL:dxVEL + 3] = q_I_b.R
self.A[dxPOS:dxPOS + 3, dxATT:dxATT + 3] = skew(vel_I)
if self.use_drag_term:
self.A[dxVEL:dxVEL + 3, dxVEL:dxVEL + 3] = -mu * I_2x3.T.dot(I_2x3)
self.A[dxVEL:dxVEL + 3,
dxB_A:dxB_A + 3] = -self.khat.dot(self.khat.T)
self.A[dxVEL:dxVEL + 3, dxMU, None] = -vel_xy
else:
self.A[dxVEL:dxVEL + 3, dxB_A:dxB_A + 3] = -I_3x3
self.A[dxVEL:dxVEL + 3, dxATT:dxATT + 3] = skew(gravity_B)
self.A[dxATT:dxATT + 3, dxB_G:dxB_G + 3] = -I_3x3
#################################
## INPUT JACOBIAN
if self.use_drag_term:
self.G[dxVEL:dxVEL + 3, uA:uA + 3] = self.khat.dot(self.khat.T)
else:
self.G[dxVEL:dxVEL + 3, uA:uA + 3] = I_3x3
self.G[dxATT:dxATT + 3, uG:uG + 3] = I_3x3
# Camera Dynamics
omega_c_i = self.q_b_c.invrot(omega)
vel_c_i = self.q_b_c.invrot(vel - skew(omega).dot(self.p_b_c))
for i in range(self.len_features):
xZETA_i = xZ + i * 5
xRHO_i = xZ + 5 * i + 4
dxZETA_i = dxZ + i * 3
dxRHO_i = dxZ + i * 3 + 2
q_zeta = Quaternion(x[xZETA_i:xZETA_i + 4, :])
rho = x[xRHO_i, 0]
zeta = q_zeta.rot(self.khat)
T_z = T_zeta(q_zeta)
skew_zeta = skew(zeta)
skew_vel_c = skew(vel_c_i)
skew_p_b_c = skew(self.p_b_c)
R_b_c = self.q_b_c.R
rho2 = rho * rho
#################################
## FEATURE DYNAMICS
self.dx[dxZETA_i:dxZETA_i +
2, :] = -T_z.T.dot(omega_c_i +
rho * skew_zeta.dot(vel_c_i))
self.dx[dxRHO_i, :] = rho2 * zeta.T.dot(vel_c_i)
#################################
## FEATURE STATE JACOBIAN
self.A[dxZETA_i:dxZETA_i + 2,
dxVEL:dxVEL + 3] = -rho * T_z.T.dot(skew_zeta).dot(R_b_c)
self.A[dxZETA_i:dxZETA_i + 2, dxB_G:dxB_G +
3] = T_z.T.dot(rho * skew_zeta.dot(R_b_c).dot(skew_p_b_c) +
R_b_c)
self.A[dxZETA_i:dxZETA_i + 2, dxZETA_i:dxZETA_i + 2] = -T_z.T.dot(
skew(rho * skew_zeta.dot(vel_c_i) + omega_c_i) +
(rho * skew_vel_c.dot(skew_zeta))).dot(T_z)
self.A[dxZETA_i:dxZETA_i + 2, dxRHO_i,
None] = -T_z.T.dot(skew_zeta).dot(vel_c_i)
self.A[dxRHO_i, dxVEL:dxVEL + 3] = rho2 * zeta.T.dot(R_b_c)
self.A[dxRHO_i,
dxB_G:dxB_G + 3] = -rho2 * zeta.T.dot(R_b_c).dot(skew_p_b_c)
self.A[dxRHO_i, dxZETA_i:dxZETA_i +
2] = -rho2 * vel_c_i.T.dot(skew_zeta).dot(T_z)
self.A[dxRHO_i, dxRHO_i] = 2 * rho * zeta.T.dot(vel_c_i).squeeze()
#################################
## FEATURE INPUT JACOBIAN
self.G[dxZETA_i:dxZETA_i + 2, uG:uG +
3] = -T_z.T.dot(R_b_c +
rho * skew_zeta.dot(R_b_c).dot(skew_p_b_c))
self.G[dxRHO_i, uG:] = rho2 * zeta.T.dot(R_b_c).dot(skew_p_b_c)
return self.dx, self.A, self.G