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 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]"