def test_inverse(self):
# quaternion calc from:
# https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3
r = Rotation.from_quaternion([[+2],
[-1],
[-3],
[+0]], ctype=ctypes.c_double)
r_inv = r.inverse()
numpy.testing.assert_equal(
r_inv.quaternion(),
[[-1/7.],
[+1/14.],
[+3/14.],
[0]]
)
r = Rotation.from_quaternion([[+2],
[-1],
[-3],
[+0]], ctype=ctypes.c_float)
r_inv = r.inverse()
numpy.testing.assert_equal(
r_inv.quaternion(),
[[numpy.float32(-1/7.)],
[numpy.float32(+1/14.)],
[numpy.float32(+3/14.)],
[0]]
)
def test_interpolation(self):
x_d = Rotation.from_axis_angle([1, 0, 0], 0, 'd')
y_d = Rotation.from_axis_angle([0, 1, 0], math.pi / 2, 'd')
r_d = Rotation.from_axis_angle([0, 1, 0], math.pi / 4, 'd')
x_f = Rotation.from_axis_angle([1, 0, 0], 0, 'f')
y_f = Rotation.from_axis_angle([0, 1, 0], math.pi / 2, 'f')
r_f = Rotation.from_axis_angle([0, 1, 0], math.pi / 4, 'f')
z_d = Rotation.interpolate(x_d, y_d, 0.5)
z_f = Rotation.interpolate(x_f, y_f, 0.5)
nose.tools.assert_almost_equal((z_d.inverse() * r_d).angle(), 0, 14)
nose.tools.assert_almost_equal((z_f.inverse() * r_f).angle(), 0, 6)
# Should auto-convert different y-type into x's type for computation.
# Return should be of x's type.
z_d = Rotation.interpolate(x_d, y_f, 0.5)
nose.tools.assert_is(z_d._ctype, x_d._ctype)
nose.tools.assert_is_not(z_d._ctype, y_f._ctype)
nose.tools.assert_almost_equal((z_d.inverse() * r_d).angle(), 0, 14)
z_f = Rotation.interpolate(x_f, y_d, 0.5)
nose.tools.assert_is(z_f._ctype, x_f._ctype)
nose.tools.assert_is_not(z_f._ctype, y_d._ctype)
nose.tools.assert_almost_equal((z_f.inverse() * r_f).angle(), 0, 6)
def test_interpolation(self):
x_d = Rotation.from_axis_angle([1, 0, 0], 0, 'd')
y_d = Rotation.from_axis_angle([0, 1, 0], math.pi / 2, 'd')
r_d = Rotation.from_axis_angle([0, 1, 0], math.pi / 4, 'd')
x_f = Rotation.from_axis_angle([1, 0, 0], 0, 'f')
y_f = Rotation.from_axis_angle([0, 1, 0], math.pi / 2, 'f')
r_f = Rotation.from_axis_angle([0, 1, 0], math.pi / 4, 'f')
z_d = Rotation.interpolate(x_d, y_d, 0.5)
z_f = Rotation.interpolate(x_f, y_f, 0.5)
nose.tools.assert_almost_equal((z_d.inverse() * r_d).angle(), 0, 14)
nose.tools.assert_almost_equal((z_f.inverse() * r_f).angle(), 0, 6)
# Should auto-convert different y-type into x's type for computation.
# Return should be of x's type.
z_d = Rotation.interpolate(x_d, y_f, 0.5)
nose.tools.assert_is(z_d._ctype, x_d._ctype)
nose.tools.assert_is_not(z_d._ctype, y_f._ctype)
nose.tools.assert_almost_equal((z_d.inverse() * r_d).angle(), 0, 14)
z_f = Rotation.interpolate(x_f, y_d, 0.5)
nose.tools.assert_is(z_f._ctype, x_f._ctype)
nose.tools.assert_is_not(z_f._ctype, y_d._ctype)
nose.tools.assert_almost_equal((z_f.inverse() * r_f).angle(), 0, 6)
def test_new_default(self):
# That these even construct
rot_d = Rotation('d')
nose.tools.assert_equal(rot_d._ctype, 'd')
rot_f = Rotation('f')
nose.tools.assert_equal(rot_f._ctype, 'f')
def _new(self, center, rotation, intrinsics):
cam_new = self.VITAL_LIB['vital_camera_new']
cam_new.argtypes = [
EigenArray.c_ptr_type(3, 1, ctypes.c_double),
Rotation.c_ptr_type(ctypes.c_double),
CameraIntrinsics.c_ptr_type(),
VitalErrorHandle.c_ptr_type()
]
cam_new.restype = self.c_ptr_type()
# Fill in parameter gaps
if center is None:
center = EigenArray(3)
center[:] = 0
else:
center = EigenArray.from_iterable(center)
if rotation is None:
rotation = Rotation()
if intrinsics is None:
intrinsics = CameraIntrinsics()
with VitalErrorHandle() as eh:
return cam_new(center, rotation, intrinsics, eh)
def test_inverse(self):
# quaternion calc from:
# https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3
r = Rotation.from_quaternion([[+2],
[-1],
[-3],
[+0]], ctype=ctypes.c_double)
r_inv = r.inverse()
e_inv = array_normalize([[-1/7.],
[+1/14.],
[+3/14.],
[0]])
numpy.testing.assert_allclose(
r_inv.quaternion(),
e_inv,
1e-15
)
r = Rotation.from_quaternion([[+2],
[-1],
[-3],
[+0]], ctype=ctypes.c_float)
r_inv = r.inverse()
numpy.testing.assert_allclose(
r_inv.quaternion(),
e_inv,
1e-7
)
def test_to_quaternion(self):
rot_d = Rotation('d')
numpy.testing.assert_array_equal(rot_d.quaternion(),
[0, 0, 0, 1])
rot_f = Rotation('f')
numpy.testing.assert_array_equal(rot_f.quaternion(),
[0, 0, 0, 1])
def test_from_matrix(self):
# Create a non-identity matrix from a different constructor that we
# assume works
# Create new rotation with that matrix.
# New rotation to_matrix method should produce the same matrix
pre_r = Rotation.from_quaternion([+2, -1, -3, +0])
mat = pre_r.matrix()
r = Rotation.from_matrix(mat)
numpy.testing.assert_allclose(mat, r.matrix(), 1e-15)
def test_new_default(self):
# That these even construct
rot_d = Rotation(ctypes.c_double)
nose.tools.assert_equal(rot_d._ctype, ctypes.c_double)
nose.tools.assert_equal(rot_d._spec, 'd')
rot_f = Rotation(ctypes.c_float)
nose.tools.assert_equal(rot_f._ctype, ctypes.c_float)
nose.tools.assert_equal(rot_f._spec, 'f')
def test_from_matrix(self):
# Create a non-identity matrix from a different constructor that we
# assume works
# Create new rotation with that matrix.
# New rotation to_matrix method should produce the same matrix
pre_r = Rotation.from_quaternion([[+2], [-1], [-3], [+0]])
mat = pre_r.matrix()
r = Rotation.from_matrix(mat)
numpy.testing.assert_equal(mat, r.matrix())
def test_from_matrix(self):
# Create a non-identity matrix from a different constructor that we
# assume works
# Create new rotation with that matrix.
# New rotation to_matrix method should produce the same matrix
pre_r = Rotation.from_quaternion([+2, -1, -3, +0])
mat = pre_r.matrix()
r = Rotation.from_matrix(mat)
numpy.testing.assert_allclose(mat, r.matrix(), 1e-15)
def rotation(self):
"""
Get the rotation of this similarity transformation
:rtype: Rotation
"""
rot_ptr = self._call_cfunc(
'vital_similarity_%s_rotation' % self._tchar, [self.C_TYPE_PTR],
[self], Rotation.c_ptr_type(self._ctype))
return Rotation(self._ctype, rot_ptr)
def rotation(self):
"""
:return: a copy of this camera's rotation
:rtype: Rotation
"""
cam_rot = self.VITAL_LIB['vital_camera_rotation']
cam_rot.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_rot.restype = Rotation.c_ptr_type()
with VitalErrorHandle() as eh:
c_ptr = cam_rot(self, eh)
return Rotation(from_cptr=c_ptr)
def test_to_matrix(self):
# Default value should be identity
rot_d = Rotation(ctypes.c_double)
numpy.testing.assert_array_equal(
rot_d.matrix(), numpy.eye(3)
)
rot_f = Rotation(ctypes.c_float)
numpy.testing.assert_array_equal(
rot_f.matrix(), numpy.eye(3)
)
def test_to_matrix(self):
# Default value should be identity
rot_d = Rotation('d')
numpy.testing.assert_array_equal(
rot_d.matrix(), numpy.eye(3)
)
rot_f = Rotation('f')
numpy.testing.assert_array_equal(
rot_f.matrix(), numpy.eye(3)
)
def test_inverse(self):
# quaternion calc from:
# https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3
r = Rotation.from_quaternion([+2, -1, -3, +0], ctype='d')
r_inv = r.inverse()
e_inv = array_normalize([-1 / 7., +1 / 14., +3 / 14., 0])
numpy.testing.assert_allclose(r_inv.quaternion(), e_inv, 1e-15)
r = Rotation.from_quaternion([+2, -1, -3, +0], ctype='f')
r_inv = r.inverse()
numpy.testing.assert_allclose(r_inv.quaternion(), e_inv, 1e-7)
def test_from_matrix(self):
# Create a non-identity matrix from a different constructor that we
# assume works
# Create new rotation with that matrix.
# New rotation to_matrix method should produce the same matrix
pre_r = Rotation.from_quaternion([[+2],
[-1],
[-3],
[+0]])
mat = pre_r.matrix()
r = Rotation.from_matrix(mat)
numpy.testing.assert_equal(mat, r.matrix())
def test_to_rodrigues(self):
# rodrigues identity: [0,0,0]
ident_rod = [0, 0, 0]
rot_d = Rotation('d')
rot_f = Rotation('f')
rod = rot_d.rodrigues()
numpy.testing.assert_equal(rod, ident_rod)
rod = rot_f.rodrigues()
numpy.testing.assert_equal(rod, ident_rod)
def test_compose(self):
expected_quat = [[+2], [-1], [-3], [+0]]
r_ident_d = Rotation(ctypes.c_double)
r_ident_f = Rotation(ctypes.c_float)
r_other_d = Rotation.from_quaternion(expected_quat, ctypes.c_double)
r_other_f = Rotation.from_quaternion(expected_quat, ctypes.c_float)
r_res_d = r_ident_d.compose(r_other_d)
nose.tools.assert_is_not(r_other_d, r_res_d)
numpy.testing.assert_equal(r_res_d, r_other_d)
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f.compose(r_other_f)
nose.tools.assert_is_not(r_other_f, r_res_f)
numpy.testing.assert_equal(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
# Should also work with multiply operator
r_res_d = r_ident_d * r_other_d
nose.tools.assert_is_not(r_other_d, r_res_d)
numpy.testing.assert_equal(r_res_d, r_other_d)
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f * r_other_f
nose.tools.assert_is_not(r_other_f, r_res_f)
numpy.testing.assert_equal(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
# Rotation of non-congruent types should be converted automatically
r_res_d = r_ident_d.compose(r_other_f)
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_equal(r_res_d.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f.compose(r_other_d)
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
r_res_d = r_ident_d * r_other_f
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_equal(r_res_d.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f * r_other_d
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
def test_from_ypr(self):
y = 1.2
p = 0.3
r = -1.0
# XXX
rot = Rotation.from_ypr(y, p, r)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(y, ry, 14)
nose.tools.assert_almost_equal(p, rp, 14)
nose.tools.assert_almost_equal(r, rr, 14)
# 0XX
rot = Rotation.from_ypr(0, p, r)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(0, ry, 14)
nose.tools.assert_almost_equal(p, rp, 14)
nose.tools.assert_almost_equal(r, rr, 14)
# X0X
rot = Rotation.from_ypr(y, 0, r)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(y, ry, 14)
nose.tools.assert_almost_equal(0, rp, 14)
nose.tools.assert_almost_equal(r, rr, 14)
# XX0
rot = Rotation.from_ypr(y, p, 0)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(y, ry, 14)
nose.tools.assert_almost_equal(p, rp, 14)
nose.tools.assert_almost_equal(0, rr, 14)
# 00X
rot = Rotation.from_ypr(0, 0, r)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(0, ry, 14)
nose.tools.assert_almost_equal(0, rp, 14)
nose.tools.assert_almost_equal(r, rr, 14)
# 0X0
rot = Rotation.from_ypr(0, p, 0)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(0, ry, 14)
nose.tools.assert_almost_equal(p, rp, 14)
nose.tools.assert_almost_equal(0, rr, 14)
# X00
rot = Rotation.from_ypr(y, 0, 0)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(y, ry, 14)
nose.tools.assert_almost_equal(0, rp, 14)
nose.tools.assert_almost_equal(0, rr, 14)
# 000
rot = Rotation.from_ypr(0, 0, 0)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(0, ry, 14)
nose.tools.assert_almost_equal(0, rp, 14)
nose.tools.assert_almost_equal(0, rr, 14)
def test_to_quaternion(self):
rot_d = Rotation(ctypes.c_double)
numpy.testing.assert_array_equal(rot_d.quaternion(),
[[0],
[0],
[0],
[1]])
rot_f = Rotation(ctypes.c_float)
numpy.testing.assert_array_equal(rot_f.quaternion(),
[[0],
[0],
[0],
[1]])
def test_to_rodrigues(self):
# rodrigues identity: [0,0,0]
ident_rod = [[0],
[0],
[0]]
rot_d = Rotation(ctypes.c_double)
rot_f = Rotation(ctypes.c_float)
rod = rot_d.rodrigues()
numpy.testing.assert_equal(rod, ident_rod)
rod = rot_f.rodrigues()
numpy.testing.assert_equal(rod, ident_rod)
def test_from_ypr(self):
y = 1.2
p = 0.3
r = -1.0
# XXX
rot = Rotation.from_ypr(y, p, r)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(y, ry, 14)
nose.tools.assert_almost_equal(p, rp, 14)
nose.tools.assert_almost_equal(r, rr, 14)
# 0XX
rot = Rotation.from_ypr(0, p, r)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(0, ry, 14)
nose.tools.assert_almost_equal(p, rp, 14)
nose.tools.assert_almost_equal(r, rr, 14)
# X0X
rot = Rotation.from_ypr(y, 0, r)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(y, ry, 14)
nose.tools.assert_almost_equal(0, rp, 14)
nose.tools.assert_almost_equal(r, rr, 14)
# XX0
rot = Rotation.from_ypr(y, p, 0)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(y, ry, 14)
nose.tools.assert_almost_equal(p, rp, 14)
nose.tools.assert_almost_equal(0, rr, 14)
# 00X
rot = Rotation.from_ypr(0, 0, r)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(0, ry, 14)
nose.tools.assert_almost_equal(0, rp, 14)
nose.tools.assert_almost_equal(r, rr, 14)
# 0X0
rot = Rotation.from_ypr(0, p, 0)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(0, ry, 14)
nose.tools.assert_almost_equal(p, rp, 14)
nose.tools.assert_almost_equal(0, rr, 14)
# X00
rot = Rotation.from_ypr(y, 0, 0)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(y, ry, 14)
nose.tools.assert_almost_equal(0, rp, 14)
nose.tools.assert_almost_equal(0, rr, 14)
# 000
rot = Rotation.from_ypr(0, 0, 0)
ry, rp, rr = rot.yaw_pitch_roll()
nose.tools.assert_almost_equal(0, ry, 14)
nose.tools.assert_almost_equal(0, rp, 14)
nose.tools.assert_almost_equal(0, rr, 14)
def test_read_write_krtd_file(self):
# Use a random string filename to avoid name collision.
fname = 'temp_camera_test_read_write_krtd_file.txt'
try:
for _ in range(100):
c = (rand(3)*2-1)*100
center = EigenArray.from_array([c])
rotation = Rotation.from_quaternion(numpy.random.rand(4)*2-1)
intrinsics = CameraIntrinsics(10, (5, 5), 1.2, 0.5, [4, 5, 6])
c1 = Camera(center, rotation,
intrinsics)
c1.write_krtd_file(fname)
c2 = Camera.from_krtd_file(fname)
err = numpy.linalg.norm(c1.center-c2.center)
assert err < 1e-9, ''.join(['Centers are different by ',
str(err)])
c1.rotation.angle_from(c2.rotation) < 1e-12
attr = ['focal_length','aspect_ratio','principle_point','skew',
'dist_coeffs']
for att in attr:
v1 = numpy.array(getattr(c1.intrinsics,att))
v2 = numpy.array(getattr(c2.intrinsics,att))
err = numpy.linalg.norm(v1-v2)
assert err < 1e-8, ''.join(['Difference ',str(err),
' for attribute: ',att])
finally:
if os.path.isfile(fname):
os.remove(fname)
def test_inverse(self):
# quaternion calc from:
# https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3
r = Rotation.from_quaternion([[+2], [-1], [-3], [+0]],
ctype=ctypes.c_double)
r_inv = r.inverse()
numpy.testing.assert_equal(r_inv.quaternion(),
[[-1 / 7.], [+1 / 14.], [+3 / 14.], [0]])
r = Rotation.from_quaternion([[+2], [-1], [-3], [+0]],
ctype=ctypes.c_float)
r_inv = r.inverse()
numpy.testing.assert_equal(
r_inv.quaternion(),
[[numpy.float32(-1 / 7.)], [numpy.float32(+1 / 14.)],
[numpy.float32(+3 / 14.)], [0]])
def test_to_ypr(self):
# ypr identity: (pi/2, 0, pi)
ident_ypr = (math.pi / 2, 0, -math.pi)
ident_ypr_float = map(lambda v: ctypes.c_float(v).value, ident_ypr)
rot_d = Rotation(ctypes.c_double)
rot_f = Rotation(ctypes.c_float)
numpy.testing.assert_equal(
rot_d.yaw_pitch_roll(),
ident_ypr
)
numpy.testing.assert_equal(
rot_f.yaw_pitch_roll(),
ident_ypr_float
)
def test_translation_initialized(self):
center = EigenArray.from_array([[1],[2],[3]])
rotation = Rotation.from_axis_angle([0, 1, 0], math.pi / 2.)
cam = Camera(center, rotation)
numpy.testing.assert_array_equal(
cam.translation,
-(rotation * center.get_matrix())
)
def test_to_ypr(self):
# ypr identity: (pi/2, 0, pi)
ident_ypr = (math.pi / 2, 0, -math.pi)
ident_ypr_float = [float(v) for v in ident_ypr]
rot_d = Rotation('d')
rot_f = Rotation('f')
numpy.testing.assert_almost_equal(
rot_d.yaw_pitch_roll(),
ident_ypr
)
numpy.testing.assert_almost_equal(
rot_f.yaw_pitch_roll(),
ident_ypr
)
def test_new(self):
# just seeing that basic construction doesn't blow up
cam = Camera()
c = EigenArray(3)
r = Rotation()
ci = CameraIntrinsics()
cam = Camera(c, r, ci)
def test_get_rotation(self):
s = Similarity()
numpy.testing.assert_array_almost_equal(s.rotation.matrix(),
Rotation().matrix())
s = Similarity(self.s, self.r, self.t)
numpy.testing.assert_array_almost_equal(s.rotation.matrix(),
self.r.matrix())
def test_compose(self):
expected_quat = [[+2],
[-1],
[-3],
[+0]]
r_ident_d = Rotation(ctypes.c_double)
r_ident_f = Rotation(ctypes.c_float)
r_other_d = Rotation.from_quaternion(expected_quat, ctypes.c_double)
r_other_f = Rotation.from_quaternion(expected_quat, ctypes.c_float)
r_res_d = r_ident_d.compose(r_other_d)
nose.tools.assert_is_not(r_other_d, r_res_d)
numpy.testing.assert_equal(r_res_d, r_other_d)
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f.compose(r_other_f)
nose.tools.assert_is_not(r_other_f, r_res_f)
numpy.testing.assert_equal(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
# Should also work with multiply operator
r_res_d = r_ident_d * r_other_d
nose.tools.assert_is_not(r_other_d, r_res_d)
numpy.testing.assert_equal(r_res_d, r_other_d)
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f * r_other_f
nose.tools.assert_is_not(r_other_f, r_res_f)
numpy.testing.assert_equal(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
# Rotation of non-congruent types should be converted automatically
r_res_d = r_ident_d.compose(r_other_f)
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_equal(r_res_d.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f.compose(r_other_d)
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
r_res_d = r_ident_d * r_other_f
nose.tools.assert_is_not(r_res_d, r_other_f)
numpy.testing.assert_equal(r_res_d.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat)
r_res_f = r_ident_f * r_other_d
nose.tools.assert_is_not(r_res_f, r_other_f)
numpy.testing.assert_equal(r_res_f.quaternion(),
r_other_f.quaternion())
numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
def test_from_quaternion(self):
q_list = [[+2],
[-1],
[-3],
[+0]]
r = Rotation.from_quaternion(q_list)
numpy.testing.assert_equal(
r.quaternion(), q_list
)
def test_from_aa(self):
# Axis should come out of rotation normalized
angle = 0.8
axis = numpy.array([[-3], [2], [1]])
axis_norm = axis / numpy.linalg.norm(axis)
r = Rotation.from_axis_angle(axis, angle)
nose.tools.assert_equal(angle, r.angle())
numpy.testing.assert_equal(axis_norm, r.axis())
def test_from_quaternion(self):
q = array_normalize([[+2],
[-1],
[-3],
[+0]], float)
r = Rotation.from_quaternion(q)
numpy.testing.assert_equal(
r.quaternion(), q
)
def test_from_aa(self):
# Axis should come out of rotation normalized
angle = 0.8
axis = [-3,2,1]
axis_norm = array_normalize(axis)
r = Rotation.from_axis_angle(axis, angle)
nose.tools.assert_equal(angle, r.angle())
numpy.testing.assert_equal(axis_norm, r.axis())
def test_from_aa(self):
# Axis should come out of rotation normalized
angle = 0.8
axis = [-3, 2, 1]
axis_norm = array_normalize(axis)
r = Rotation.from_axis_angle(axis, angle)
nose.tools.assert_equal(angle, r.angle())
numpy.testing.assert_equal(axis_norm, r.axis())
def test_from_rodrigues(self):
rod_list_1 = [0, 0, 0]
r1 = Rotation.from_rodrigues(rod_list_1)
numpy.testing.assert_equal(r1.rodrigues(), rod_list_1)
# This one will get normalized by magnitude in rotation instance
# This vector's is less than 2*pi, so we should expect this vector to be
# returned as is.
rod2 = numpy.array([2, -1, 0.5])
nod2_normed = array_normalize(rod2)
print('r2 2-norm:', numpy.linalg.norm(rod2))
print('r2-normed:', nod2_normed)
r2 = Rotation.from_rodrigues(rod2)
numpy.testing.assert_array_almost_equal(
r2.rodrigues(), rod2,
decimal=14, # 1e-14
)
def test_inverse(self):
# quaternion calc from:
# https://www.wolframalpha.com/input/?i=quaternion:+0%2B2i-j-3k&lk=3
r = Rotation.from_quaternion([+2, -1, -3, +0], ctype='d')
r_inv = r.inverse()
e_inv = array_normalize([-1/7., +1/14., +3/14., 0])
numpy.testing.assert_allclose(
r_inv.quaternion(),
e_inv,
1e-15
)
r = Rotation.from_quaternion([+2, -1, -3, +0], ctype='f')
r_inv = r.inverse()
numpy.testing.assert_allclose(
r_inv.quaternion(),
e_inv,
1e-7
)
def test_from_rodrigues(self):
rod_list_1 = [0, 0, 0]
r1 = Rotation.from_rodrigues(rod_list_1)
numpy.testing.assert_equal(r1.rodrigues(), rod_list_1)
# This one will get normalized by magnitude in rotation instance
# This vector's is less than 2*pi, so we should expect this vector to be
# returned as is.
rod2 = numpy.array([2, -1, 0.5])
nod2_normed = array_normalize(rod2)
print('r2 2-norm:', numpy.linalg.norm(rod2))
print('r2-normed:', nod2_normed)
r2 = Rotation.from_rodrigues(rod2)
numpy.testing.assert_array_almost_equal(
r2.rodrigues(),
rod2,
decimal=14, # 1e-14
)
def rotation(self):
"""
:return: a copy of this camera's rotation
:rtype: Rotation
"""
cam_rot = self.VITAL_LIB['vital_camera_rotation']
cam_rot.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_rot.restype = Rotation.c_ptr_type()
with VitalErrorHandle() as eh:
c_ptr = cam_rot(self, eh)
return Rotation(from_cptr=c_ptr)
def test_from_aa(self):
# Axis should come out of rotation normalized
angle = 0.8
axis = numpy.array([[-3],
[2],
[1]])
axis_norm = axis / numpy.linalg.norm(axis)
r = Rotation.from_axis_angle(axis, angle)
nose.tools.assert_equal(angle, r.angle())
numpy.testing.assert_equal(axis_norm, r.axis())
def test_to_axis_angle(self):
# expected identity: [0,0,1] and 0
ident_axis = [0, 0, 1]
ident_angle = 0
rot_d = Rotation('d')
rot_f = Rotation('f')
numpy.testing.assert_equal(rot_d.axis(), ident_axis)
nose.tools.assert_equal(rot_d.angle(), ident_angle)
numpy.testing.assert_equal(rot_f.axis(), ident_axis)
nose.tools.assert_equal(rot_f.angle(), ident_angle)
def test_to_axis_angle(self):
# expected identity: [0,0,1] and 0
ident_axis = [[0], [0], [1]]
ident_angle = 0
rot_d = Rotation(ctypes.c_double)
rot_f = Rotation(ctypes.c_float)
numpy.testing.assert_equal(rot_d.axis(), ident_axis)
nose.tools.assert_equal(rot_d.angle(), ident_angle)
numpy.testing.assert_equal(rot_f.axis(), ident_axis)
nose.tools.assert_equal(rot_f.angle(), ident_angle)
def test_equal(self):
cam1 = Camera()
cam2 = Camera()
nose.tools.assert_equal(cam1, cam1)
nose.tools.assert_equal(cam1, cam2)
center = EigenArray.from_iterable([[1], [2], [3]])
rotation = Rotation.from_axis_angle([[0], [1], [0]], math.pi / 2.)
cam1 = Camera(center, rotation)
cam2 = Camera(center, rotation)
nose.tools.assert_equal(cam1, cam1)
nose.tools.assert_equal(cam1, cam2)
def test_equal(self):
cam1 = Camera()
cam2 = Camera()
nose.tools.assert_equal(cam1, cam1)
nose.tools.assert_equal(cam1, cam2)
center = EigenArray.from_array([[1],[2],[3]])
rotation = Rotation.from_axis_angle([0, 1, 0], math.pi / 2.)
cam1 = Camera(center, rotation)
cam2 = Camera(center, rotation)
nose.tools.assert_equal(cam1, cam1)
nose.tools.assert_equal(cam1, cam2)
def test_compose(self):
s1 = Similarity(self.s, self.r, self.t)
s2 = Similarity(0.75, Rotation.from_rodrigues([-0.5, -0.5, 1.0]),
EigenArray.from_iterable([4, 6.5, 8]))
sim_comp = s1.compose(s2).as_matrix()
mat_comp = numpy.dot(s1.as_matrix(), s2.as_matrix())
print 'sim12 comp:\n', sim_comp
print 'mat comp:\n', mat_comp
print 'sim - mat:\n', sim_comp - mat_comp
nose.tools.assert_almost_equal(
numpy.linalg.norm(sim_comp - mat_comp, 2), 0., 14)
def test_to_matrix(self):
# Default value should be identity
rot_d = Rotation('d')
numpy.testing.assert_array_equal(rot_d.matrix(), numpy.eye(3))
rot_f = Rotation('f')
numpy.testing.assert_array_equal(rot_f.matrix(), numpy.eye(3))
def test_interpolated_rotations(self):
x = Rotation.from_axis_angle([1, 0, 0], 0)
a = math.pi / 2
y = Rotation.from_axis_angle([0, 1, 0], a)
i_list = Rotation.interpolated_rotations(x, y, 3)
nose.tools.assert_equal([i._ctype for i in i_list], ['d'] * 3)
i0_e_axis, i0_e_angle = [0, 1, 0], a * 0.25
i1_e_axis, i1_e_angle = [0, 1, 0], a * 0.50
i2_e_axis, i2_e_angle = [0, 1, 0], a * 0.75
numpy.testing.assert_almost_equal(i_list[0].axis(), i0_e_axis, 14)
numpy.testing.assert_almost_equal(i_list[0].angle(), i0_e_angle, 14)
numpy.testing.assert_almost_equal(i_list[1].axis(), i1_e_axis, 14)
numpy.testing.assert_almost_equal(i_list[1].angle(), i1_e_angle, 14)
numpy.testing.assert_almost_equal(i_list[2].axis(), i2_e_axis, 14)
numpy.testing.assert_almost_equal(i_list[2].angle(), i2_e_angle, 14)
# Mixed types
a = math.pi / 2
x = Rotation.from_axis_angle([1, 0, 0], 0, 'f')
y = Rotation.from_axis_angle([0, 1, 0], a)
i_list = Rotation.interpolated_rotations(x, y, 3)
nose.tools.assert_equal([i._ctype for i in i_list], ['f'] * 3)
numpy.testing.assert_almost_equal(i_list[0].axis(), i0_e_axis, 14)
numpy.testing.assert_almost_equal(i_list[0].angle(), i0_e_angle, 6)
numpy.testing.assert_almost_equal(i_list[1].axis(), i1_e_axis, 14)
numpy.testing.assert_almost_equal(i_list[1].angle(), i1_e_angle, 6)
numpy.testing.assert_almost_equal(i_list[2].axis(), i2_e_axis, 14)
numpy.testing.assert_almost_equal(i_list[2].angle(), i2_e_angle, 6)
def test_to_matrix(self):
# Default value should be identity
rot_d = Rotation(ctypes.c_double)
numpy.testing.assert_array_equal(rot_d.matrix(), numpy.eye(3))
rot_f = Rotation(ctypes.c_float)
numpy.testing.assert_array_equal(rot_f.matrix(), numpy.eye(3))
def test_compose_convert(self):
# Composing across types should work
s1 = Similarity(self.s, self.r, self.t)
s2 = Similarity(0.75, Rotation.from_rodrigues([-0.5, -0.5, 1.0]),
EigenArray.from_iterable([4, 6.5, 8]), ctypes.c_float)
sim_comp = s1.compose(s2).as_matrix()
mat_comp = numpy.dot(s1.as_matrix(), s2.as_matrix())
print('sim12 comp:\n', sim_comp)
print('mat comp:\n', mat_comp)
print('sim - mat:\n', sim_comp - mat_comp)
nose.tools.assert_almost_equal(
numpy.linalg.norm(sim_comp - mat_comp, 2), 0., 6)
def test_to_quaternion(self):
rot_d = Rotation(ctypes.c_double)
numpy.testing.assert_array_equal(rot_d.quaternion(),
[[0], [0], [0], [1]])
rot_f = Rotation(ctypes.c_float)
numpy.testing.assert_array_equal(rot_f.quaternion(),
[[0], [0], [0], [1]])
def _new(self, s, r, t):
"""
:type s: float
:type r: Rotation | None
:type t: collections.Iterable[float] | None
"""
# noinspection PyProtectedMember
if r is None:
r = Rotation(self._ctype)
elif r._ctype != self._ctype:
# Create new Rotation sharing our type
r = Rotation.from_quaternion(r.quaternion(), self._ctype)
if t is None:
t = EigenArray.from_iterable((0, 0, 0), self._ctype)
else:
t = EigenArray.from_iterable(t, self._ctype, (3, 1))
return self._call_cfunc('vital_similarity_%s_new' % self._tchar, [
self._ctype,
Rotation.c_ptr_type(self._ctype),
EigenArray.c_ptr_type(3, 1, self._ctype)
], [s, r, t], self.C_TYPE_PTR)
def test_rotation_vector(self):
vec = [1, 0, 0]
vec_expected = [0, 1, 0]
r_axis = [0, 0, 1]
r_angle = math.pi / 2.
r = Rotation.from_axis_angle(r_axis, r_angle)
vec_rotated = r.rotate_vector(vec)
numpy.testing.assert_array_almost_equal(vec_expected, vec_rotated)
# should be able to multiply a rotation as a left-hand side arg with a
# 3x1 vector as the right-hand side arg
vec_rotated = r * vec
numpy.testing.assert_array_almost_equal(vec_expected, vec_rotated)
def test_compose(self):
s1 = Similarity(self.s, self.r, self.t)
s2 = Similarity(0.75,
Rotation.from_rodrigues([-0.5, -0.5, 1.0]),
[4, 6.5, 8])
sim_comp = s1.compose(s2).as_matrix()
mat_comp = numpy.dot(s1.as_matrix(), s2.as_matrix())
print('sim12 comp:\n', sim_comp)
print('mat comp:\n', mat_comp)
print('sim - mat:\n', sim_comp - mat_comp)
nose.tools.assert_almost_equal(
numpy.linalg.norm(sim_comp - mat_comp, 2),
0., 12
)
def test_rotation_vector(self):
vec = [1, 0, 0]
vec_expected = [0, 1, 0]
r_axis = [0, 0, 1]
r_angle = math.pi / 2.
r = Rotation.from_axis_angle(r_axis, r_angle)
vec_rotated = r.rotate_vector(vec)
numpy.testing.assert_array_almost_equal(vec_expected, vec_rotated)
# should be able to multiply a rotation as a left-hand side arg with a
# 3x1 vector as the right-hand side arg
vec_rotated = r * vec
numpy.testing.assert_array_almost_equal(vec_expected, vec_rotated)
def noisy_cameras(cam_map, pos_stddev=1., rot_stddev=1.):
"""
Add positional and rotational gaussian noise to cameras
:type cam_map: CameraMap
:type pos_stddev: float
:type rot_stddev: float
:return: Camera map of new, noidy cameras'
"""
cmap = {}
for f, c in cam_map.as_dict().iteritems():
c2 = Camera(
c.center + random_point_3d(pos_stddev),
c.rotation * Rotation.from_rodrigues(random_point_3d(rot_stddev)),
c.intrinsics
)
cmap[f] = c2
return CameraMap(cmap)
def test_to_from_string(self):
cam = Camera()
cam_s = cam.as_string()
cam2 = Camera.from_string(cam_s)
print "Default camera string:\n%s" % cam_s
print "Default newcam string:\n%s" % cam2.as_string()
nose.tools.assert_equal(cam, cam2)
center = EigenArray.from_iterable([[1],
[2],
[3]])
rotation = Rotation.from_axis_angle([[0], [1], [0]], math.pi / 2.)
cam = Camera(center, rotation)
cam_s = cam.as_string()
cam2 = Camera.from_string(cam_s)
print "Custom camera string:\n%s" % cam_s
print "Custom newcam string:\n%s" % cam2.as_string()
nose.tools.assert_equal(cam, cam2)
def test_to_axis_angle(self):
# expected identity: [0,0,1] and 0
ident_axis = [0, 0, 1]
ident_angle = 0
rot_d = Rotation('d')
rot_f = Rotation('f')
numpy.testing.assert_equal(rot_d.axis(), ident_axis)
nose.tools.assert_equal(rot_d.angle(), ident_angle)
numpy.testing.assert_equal(rot_f.axis(), ident_axis)
nose.tools.assert_equal(rot_f.angle(), ident_angle)
def test_eq(self):
# Identities should equal
r1 = Rotation('d')
r2 = Rotation('d')
nose.tools.assert_equal(r1, r2)
r1 = Rotation('f')
r2 = Rotation('f')
nose.tools.assert_equal(r1, r2)
r1 = Rotation('d')
r2 = Rotation('f')
# r2 should get converted into a double instance for checking
nose.tools.assert_equal(r1, r2)
r1 = Rotation.from_quaternion([1,2,3,4], ctype='d')
r2 = Rotation.from_quaternion([1,2,3,4], ctype='d')
nose.tools.assert_equal(r1, r2)
r1 = Rotation.from_quaternion([1,2,3,4], ctype='d')
r2 = Rotation.from_quaternion([-1,-2,-3,-4], ctype='d')
assert r1.angle_from(r2) < 1e-12
def test_to_axis_angle(self):
# expected identity: [0,0,1] and 0
ident_axis = [[0],
[0],
[1]]
ident_angle = 0
rot_d = Rotation(ctypes.c_double)
rot_f = Rotation(ctypes.c_float)
numpy.testing.assert_equal(rot_d.axis(), ident_axis)
nose.tools.assert_equal(rot_d.angle(), ident_angle)
numpy.testing.assert_equal(rot_f.axis(), ident_axis)
nose.tools.assert_equal(rot_f.angle(), ident_angle)
