def test_angles(self):
q_sp = quaternions.qeye()
q_state = quaternions.qeye()
result = quar_axis_error(q_sp, q_state)
self.assertEqual(result[0], 0.)
self.assertEqual(result[1], 0.)
self.assertEqual(result[2], 0.)
# Roll
q_sp = euler.euler2quat(0.1,0.0,0)
result = quar_axis_error(q_sp, q_state)
self.assertAlmostEqual(result[0], 0.1, delta=1.e-8)
self.assertAlmostEqual(result[1], 0.0, delta= 1.e-8)
self.assertAlmostEqual(result[2], 0., delta=1.e-8)
# Pitch
q_sp = euler.euler2quat(0,0.1,0)
result = quar_axis_error(q_sp, q_state)
self.assertAlmostEqual(result[0], 0.0, delta = 1.e-8)
self.assertAlmostEqual(result[1], 0.1, delta = 1.e-8)
self.assertAlmostEqual(result[2], 0.0, delta = 1.e-8)
# Yaw
q_sp = euler.euler2quat(0,0,0.1)
result = quar_axis_error(q_sp, q_state)
self.assertAlmostEqual(result[0], 0.0, delta = 1.e-8)
self.assertAlmostEqual(result[1], 0.0, delta = 1.e-8)
self.assertAlmostEqual(result[2], 0.1, delta = 1.e-8)
def test_qnorm():
qi = tq.qeye()
assert tq.qnorm(qi) == 1
assert tq.qisunit(qi)
qi[1] = 0.2
assert not tq.qisunit(qi)
# Test norm is sqrt of scalar for multiplication with conjugate.
# https://en.wikipedia.org/wiki/Quaternion#Conjugation,_the_norm,_and_reciprocal
for q in quats:
q_c = tq.qconjugate(q)
exp_norm = np.sqrt(tq.qmult(q, q_c)[0])
assert np.allclose(tq.qnorm(q), exp_norm)
def test_qnorm():
qi = tq.qeye()
assert tq.qnorm(qi) == 1
assert tq.qisunit(qi)
qi[1] = 0.2
assert not tq.qisunit(qi)
# Test norm is sqrt of scalar for multiplication with conjugate.
# https://en.wikipedia.org/wiki/Quaternion#Conjugation,_the_norm,_and_reciprocal
for q in quats:
q_c = tq.qconjugate(q)
exp_norm = np.sqrt(tq.qmult(q, q_c)[0])
assert np.allclose(tq.qnorm(q), exp_norm)
def quat_v1v2(v1, v2):
"""
Return the minimum-angle quaternion that rotates v1 to v2.
Non-SIMD version for clarity of maths.
"""
th = acos(np.dot(v1, v2))
ax = np.cross(v1, v2)
if all(np.isnan(ax)):
# = = = This happens when the two vectors are identical
return qops.qeye()
else:
# = = = Do normalisation within the next function
return qops.axangle2quat(ax, th)
def compute(self, sparse_model_flag=False):
"""
Function to compute the sparse model and the relative scene transformations
through optimization. Output directory will be created if not exists.
Returns a success boolean.
"""
# create output dir if not exists
if not os.path.isdir(self.output_dir):
os.makedirs(self.output_dir)
#populate selection matrix from select_vec
total_kpt_count = len(self.select_vec)
found_kpt_count = len(np.nonzero(self.select_vec)[0])
selection_matrix = np.zeros((found_kpt_count * 3, total_kpt_count * 3))
for idx, nz_idx in enumerate(np.nonzero(self.select_vec)[0]):
selection_matrix[(idx * 3):(idx * 3) + 3,
(nz_idx * 3):(nz_idx * 3) + 3] = np.eye(3)
computed_vector = []
success_flag = False
if sparse_model_flag:
object_model = SparseModel().reader(self.sparse_model_file)
success_flag, res = app.geo_constrain.predict(
object_model, self.scene_kpts.transpose(0, 2, 1),
self.select_vec)
scene_t = np.asarray([np.array(i[:3, 3]) for i in res])
scene_q = np.asarray(
[tfq.mat2quat(np.array(i[:3, :3])) for i in res])
computed_vector = np.concatenate(
(scene_t.flatten(), scene_q.flatten()))
else:
#initialize quaternions and translations for each scene
scene_t_ini = np.array([[0, 0,
0]]).repeat(self.scene_kpts.shape[0],
axis=0)
scene_q_ini = np.array([[1, 0, 0,
0]]).repeat(self.scene_kpts.shape[0],
axis=0)
scene_P_ini = np.array([[[0, 0,
0]]]).repeat(self.scene_kpts.shape[2],
axis=0)
#initialize with known keypoints
scene_P_ini = self.scene_kpts[0].transpose()[:, np.newaxis, :]
#main optimization step
res = app.optimize.predict(self.scene_kpts, scene_t_ini,
scene_q_ini, scene_P_ini,
selection_matrix)
#extract generated sparse object model optimization output
len_ts = scene_t_ini[1:].size
len_qs = scene_q_ini[1:].size
object_model = res.x[len_ts + len_qs:].reshape(scene_P_ini.shape)
object_model = object_model.squeeze()
#save the generated sparse object model
SparseModel().writer(
object_model, os.path.join(self.output_dir,
"sparse_model.txt"), True)
computed_vector = np.concatenate([
np.zeros(3), res.x[:len_ts],
tfq.qeye(), res.x[len_ts:len_ts + len_qs]
])
success_flag = res.success
#save the input and the output from optimization step
out_fn = os.path.join(self.output_dir, 'saved_meta_data')
np.savez(out_fn,
model=object_model,
scenes=computed_vector,
ref=self.scene_kpts,
sm=selection_matrix)
if success_flag:
print("--------\n--------\n--------")
print("Computed results saved at {}".format(out_fn))
print("--------\n--------\n--------")
return success_flag, object_model
def test_qeye():
qi = tq.qeye()
assert qi.dtype.kind == 'f'
assert np.all([1, 0, 0, 0] == qi)
assert np.allclose(tq.quat2mat(qi), np.eye(3))
def test_qnorm():
qi = tq.qeye()
assert tq.qnorm(qi) == 1
assert tq.qisunit(qi)
qi[1] = 0.2
assert not tq.qisunit(qi)
def test_qeye():
qi = tq.qeye()
assert qi.dtype.kind == 'f'
assert np.all([1,0,0,0]==qi)
assert np.allclose(tq.quat2mat(qi), np.eye(3))
