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_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_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_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_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 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 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 setUpClass(cls):
cls.s = 2.4
cls.r = Rotation.from_rodrigues([0.1, -1.5, 2.0])
cls.t = EigenArray.from_iterable([1, -2, 5])
def setUpClass(cls):
cls.s = 2.4
cls.r = Rotation.from_rodrigues([0.1, -1.5, 2.0])
cls.r_f = Rotation.from_rodrigues([0.1, -1.5, 2.0], 'f')
cls.t = [1, -2, 5]
def setUpClass(cls):
cls.s = 2.4
cls.r = Rotation.from_rodrigues([0.1, -1.5, 2.0])
cls.r_f = Rotation.from_rodrigues([0.1, -1.5, 2.0], 'f')
cls.t = [1, -2, 5]
