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 test():
landmarks = np.random.uniform(-100, 100, (3, 10))
truth = []
position = np.zeros((3, 1))
orientation = Quaternion.Identity()
for i in range(1000):
position += np.random.normal(0.0, 0.025, (3, 1))
orientation += np.random.normal(0.0, 0.025, (3, 1))
truth.append(
np.hstack(np.array([[i]]), position.T, orientation.elements.T))
truth = np.array(truth).squeeze()
feature_time, zetas, depths, ids = add_landmark(truth, landmarks)
def run_tests():
# run some math helper tests
# Test vectorized quat from two unit vectors
v1 = np.random.uniform(-1, 1, (3, 100))
v2 = np.random.uniform(-1, 1, (3, 1))
v3 = np.random.uniform(-1, 1, (3, 1))
v1 /= norm(v1, axis=0)
v2 /= norm(v2)
v3 /= norm(v3)
# On a single vector
assert norm(
Quaternion(q_array_from_two_unit_vectors(v3, v2)).rot(v3) - v2) < 1e-8
# on a bunch of vectors
quat_array = q_array_from_two_unit_vectors(v2, v1)
for q, v in zip(quat_array.T, v1.T):
Quaternion(q[:, None]).rot(v2) - v[:, None]
# Test T_zeta
q2 = q_array_from_two_unit_vectors(e_z, v2)
assert norm(T_zeta(Quaternion(q2)).T.dot(v2)) < 1e-8
# Check derivative of T_zeta - This was giving me trouble
d_dTdq = np.zeros((2, 2))
q = Quaternion(np.random.uniform(-1, 1, (4, 1)))
q.arr[3] = 0.0
q.normalize()
x0 = T_zeta(q).T.dot(v2)
epsilon = 1e-6
I = np.eye(2) * epsilon
for i in range(2):
qplus = q_feat_boxplus(q, I[:, i, None])
xprime = T_zeta(qplus).T.dot(v2)
d_dTdq[i, :, None] = (xprime - x0) / epsilon
a_dTdq = -T_zeta(q).T.dot(skew(v2).dot(T_zeta(q)))
assert (abs(a_dTdq - d_dTdq) < 1e-6).all()
# Check Derivative dqzeta/dqzeta <- this was also giving me trouble
for j in range(1000):
d_dqdq = np.zeros((2, 2))
if j == 0:
q = Quaternion.Identity()
else:
q = Quaternion(np.random.uniform(-1, 1, (4, 1)))
q.arr[3] = 0.0
q.normalize()
for i in range(2):
d_dqdq[i, :, None] = q_feat_boxminus(
q_feat_boxplus(q, I[:, i, None]), q) / epsilon
a_dqdq = T_zeta(q).T.dot(T_zeta(q))
assert (abs(a_dqdq - d_dqdq) < 1e-1).all()
# Check Manifold Consistency
for i in range(1000):
omega = np.random.uniform(-1, 1, (3, 1))
omega2 = np.random.uniform(-1, 1, (3, 1))
omega[2] = 0.0
omega2[2] = 0.0
x = Quaternion.exp(omega)
y = Quaternion.exp(omega2)
dx = np.random.normal(0.0, 0.5, (2, 1))
# Check x [+] 0 == x
assert norm(q_feat_boxplus(x, np.zeros((2, 1))) - x) < 1e-8
# Check x [+] (y [-] x) == y (compare the rotated zetas, because there are infinitely
# many quaternions which return the same zeta.) We don't have the framework to handle
# forcing the quaternion to actually be the same
assert norm((q_feat_boxplus(x, q_feat_boxminus(y, x))).rot(e_z) -
y.rot(e_z)) < 1e-8
# Check (x [+] dx) [-] x == dx
assert norm(q_feat_boxminus(q_feat_boxplus(x, dx), x) - dx) < 1e-8
# assert norm( q_feat_boxminus(q_feat_boxplus(qzeta, dqzeta), qzeta) - dqzeta) < 1e-8
print "math helper test: [PASSED]"