def estimate_from_landmark_maps(self, from_lm, to_lm):
"""
Estimate the similarity transform between two corresponding landmark
maps.
Landmarks with corresponding frame IDs in the two maps are paired for
transform estimation. Landmarks with no corresponding frame ID in the
other map are ignored. An exception is set if there are no shared frame
IDs between the two provided maps (nothing to pair).
:param from_lm: Map of original landmarks, sharing N frames with the
transformed landmarks, where N > 0.
:type from_lm: LandmarkMap
:param to_lm: Map of transformed landmarks, sharing N frames with the
original landmarks, where N > 0.
:type to_lm: LandmarkMap
:return: An estimated similarity transform mapping landmark centers in
the ``from`` space to camera centers in the ``to`` space.
:rtype: Similarity
"""
cptr = self._call_cfunc(
'vital_algorithm_estimate_similarity_transform_estimate_landmark_map',
[
self.C_TYPE_PTR,
LandmarkMap.c_ptr_type(),
LandmarkMap.c_ptr_type()
], [self, from_lm, to_lm], Similarity.c_ptr_type(ctypes.c_double),
{1: VitalAlgorithmException})
return Similarity(ctype=ctypes.c_double, from_cptr=cptr)
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_equals(self):
s1 = Similarity()
s2 = Similarity()
nose.tools.assert_equal(s1, s2)
s1 = Similarity(self.s, self.r, self.t)
s2 = Similarity(self.s, self.r, self.t)
nose.tools.assert_equal(s1, s2)
def test_notequal(self):
s1 = Similarity()
s2 = Similarity(self.s, self.r, self.t)
nose.tools.assert_not_equal(s1, s2)
nose.tools.assert_not_equal(s1, 0)
nose.tools.assert_not_equal(s1, 'foo')
nose.tools.assert_not_equal(s1, [1, 2, 3])
def test_inverse(self):
# Inverse of identity is itself
s = Similarity()
nose.tools.assert_equal(s, s.inverse())
s = Similarity(self.s, self.r, self.t)
s_i = s.inverse()
i = s * s_i
# Similarity composed with inverse should be identity
nose.tools.assert_almost_equal(i.scale, 1., 14)
nose.tools.assert_almost_equal(i.rotation.angle(), 0., 14)
nose.tools.assert_almost_equal(numpy.linalg.norm(i.translation, 2),
0., 12)
def test_transform_vector(self):
s = Similarity(self.s, self.r, self.t)
v1 = [4, 2.1, 9.125]
v2 = s.transform_vector(v1)
v3 = s.inverse().transform_vector(v2)
nose.tools.assert_false(numpy.allclose(v1, v2))
nose.tools.assert_true(numpy.allclose(v1, v3))
# This should also work with mult syntax
v4 = s * v1
v5 = s.inverse() * v4
nose.tools.assert_false(numpy.allclose(v1, v4))
nose.tools.assert_true(numpy.allclose(v1, v5))
def test_transform_vector(self):
s = Similarity(self.s, self.r, self.t)
v1 = EigenArray.from_iterable([4, 2.1, 9.125])
v2 = s.transform_vector(v1)
v3 = s.inverse().transform_vector(v2)
nose.tools.assert_false(numpy.allclose(v1, v2))
nose.tools.assert_true(numpy.allclose(v1, v3))
# This should also work with mult syntax
v4 = s * v1
v5 = s.inverse() * v4
nose.tools.assert_false(numpy.allclose(v1, v4))
nose.tools.assert_true(numpy.allclose(v1, v5))
def estimate_from_points(self, from_pts, to_pts):
"""
Estimate the similarity transform between two corresponding point sets
:raises VitalAlgorithmException: from and to point sets are misaligned,
insufficient or degenerate
:param from_pts: Iterable of 3D points in the ``from`` space.
:type from_pts: collections.Iterable[EigenArray | collections.Sequence[float]]
:param to_pts: Iterable of 3D points in the ``to`` space.
:type to_pts: collections.Iterable[EigenArray | collections.Sequence[float]]
:return: New similarity instance
:rtype: Similarity
"""
ea_type = EigenArray.c_ptr_type(3, 1, ctypes.c_double)
# make C arrays from input points
from_list = [
EigenArray.from_iterable(p, target_shape=(3, 1)) for p in from_pts
]
to_list = [
EigenArray.from_iterable(p, target_shape=(3, 1)) for p in to_pts
]
if len(from_list) != len(to_list):
raise VitalAlgorithmException(
"From and to iterables not the same length: %d != %d" %
(len(from_list), len(to_list)))
n = len(from_list)
from_ptr_arr = (ea_type * n)()
for i, e in enumerate(from_list):
from_ptr_arr[i] = e.c_pointer
to_ptr_arr = (ea_type * n)()
for i, e in enumerate(to_list):
to_ptr_arr[i] = e.c_pointer
sim_ptr = self._call_cfunc(
'vital_algorithm_estimate_similarity_transform_estimate_transform_points',
[
self.C_TYPE_PTR, ctypes.c_size_t,
ctypes.POINTER(ea_type),
ctypes.POINTER(ea_type)
], [self, n, from_ptr_arr, to_ptr_arr],
Similarity.c_ptr_type(ctypes.c_double),
{1: VitalAlgorithmException})
return Similarity(ctype=ctypes.c_double, from_cptr=sim_ptr)
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_new(self):
sim = Similarity(self.s, self.r, self.t)
nose.tools.assert_equal(sim.scale, self.s)
numpy.testing.assert_array_almost_equal(sim.rotation.matrix(),
self.r.matrix())
numpy.testing.assert_array_equal(sim.translation, self.t)
def estimate(self, cameras, landmarks):
"""
Estimate a canonical similarity transform for cameras and points
:param cameras: The camera map containing all the cameras
:type cameras: CameraMap
:param landmarks: The landmark map containing all the 3D landmarks
:type landmarks: LandmarkMap
:return: New estimated similarity transformation mapping the data to the
canonical space.
:rtype: Similarity
"""
cptr = self._call_cfunc(
'vital_algorithm_estimate_canonical_transform_estimate',
[self.C_TYPE_PTR, CameraMap.c_ptr_type(), LandmarkMap.c_ptr_type()],
[self, cameras, landmarks],
Similarity.c_ptr_type(ctypes.c_double),
{
1: VitalAlgorithmException
}
)
return Similarity(ctype=ctypes.c_double, from_cptr=cptr)
def estimate_from_landmark_maps(self, from_lm, to_lm):
"""
Estimate the similarity transform between two corresponding landmark
maps.
Landmarks with corresponding frame IDs in the two maps are paired for
transform estimation. Landmarks with no corresponding frame ID in the
other map are ignored. An exception is set if there are no shared frame
IDs between the two provided maps (nothing to pair).
:param from_lm: Map of original landmarks, sharing N frames with the
transformed landmarks, where N > 0.
:type from_lm: LandmarkMap
:param to_lm: Map of transformed landmarks, sharing N frames with the
original landmarks, where N > 0.
:type to_lm: LandmarkMap
:return: An estimated similarity transform mapping landmark centers in
the ``from`` space to camera centers in the ``to`` space.
:rtype: Similarity
"""
cptr = self._call_cfunc(
'vital_algorithm_estimate_similarity_transform_estimate_landmark_map',
[self.C_TYPE_PTR, LandmarkMap.c_ptr_type(), LandmarkMap.c_ptr_type()],
[self, from_lm, to_lm],
Similarity.c_ptr_type(ctypes.c_double),
{
1: VitalAlgorithmException
}
)
return Similarity(ctype=ctypes.c_double, from_cptr=cptr)
def test_new_mixed_types(self):
# r and t are in double format, so try to use them to construct float
# similarity inst
sim = Similarity(self.s, self.r, self.t, ctypes.c_float)
nose.tools.assert_almost_equal(sim.scale, self.s, 6)
nose.tools.assert_equal(sim.rotation, self.r)
numpy.testing.assert_array_equal(sim.translation, self.t)
def test_convert_matrix(self):
sim = Similarity()
numpy.testing.assert_array_equal(sim.as_matrix(), numpy.eye(4))
sim1 = Similarity(self.s, self.r, self.t)
mat1 = sim1.as_matrix()
sim2 = Similarity.from_matrix(mat1)
mat2 = sim2.as_matrix()
print "Sim1:", sim1.as_matrix()
print "Sim2:", sim2.as_matrix()
numpy.testing.assert_almost_equal(mat1, mat2, decimal=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 estimate_from_points(self, from_pts, to_pts):
"""
Estimate the similarity transform between two corresponding point sets
:raises VitalAlgorithmException: from and to point sets are misaligned,
insufficient or degenerate
:param from_pts: Iterable of 3D points in the ``from`` space.
:type from_pts: collections.Iterable[EigenArray | collections.Sequence[float]]
:param to_pts: Iterable of 3D points in the ``to`` space.
:type to_pts: collections.Iterable[EigenArray | collections.Sequence[float]]
:return: New similarity instance
:rtype: Similarity
"""
ea_type = EigenArray.c_ptr_type(3, 1, ctypes.c_double)
# make C arrays from input points
from_list = [EigenArray.from_iterable(p, target_shape=(3, 1))
for p in from_pts]
to_list = [EigenArray.from_iterable(p, target_shape=(3, 1))
for p in to_pts]
if len(from_list) != len(to_list):
raise VitalAlgorithmException(
"From and to iterables not the same length: %d != %d"
% (len(from_list), len(to_list))
)
n = len(from_list)
from_ptr_arr = (ea_type * n)()
for i, e in enumerate(from_list):
from_ptr_arr[i] = e.c_pointer
to_ptr_arr = (ea_type * n)()
for i, e in enumerate(to_list):
to_ptr_arr[i] = e.c_pointer
sim_ptr = self._call_cfunc(
'vital_algorithm_estimate_similarity_transform_estimate_transform_points',
[self.C_TYPE_PTR, ctypes.c_size_t, ctypes.POINTER(ea_type),
ctypes.POINTER(ea_type)],
[self, n, from_ptr_arr, to_ptr_arr],
Similarity.c_ptr_type(ctypes.c_double),
{
1: VitalAlgorithmException
}
)
return Similarity(ctype=ctypes.c_double, from_cptr=sim_ptr)
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 estimate(self, cameras, landmarks):
"""
Estimate a canonical similarity transform for cameras and points
:param cameras: The camera map containing all the cameras
:type cameras: CameraMap
:param landmarks: The landmark map containing all the 3D landmarks
:type landmarks: LandmarkMap
:return: New estimated similarity transformation mapping the data to the
canonical space.
:rtype: Similarity
"""
cptr = self._call_cfunc(
'vital_algorithm_estimate_canonical_transform_estimate', [
self.C_TYPE_PTR,
CameraMap.c_ptr_type(),
LandmarkMap.c_ptr_type()
], [self, cameras, landmarks],
Similarity.c_ptr_type(ctypes.c_double),
{1: VitalAlgorithmException})
return Similarity(ctype=ctypes.c_double, from_cptr=cptr)
def test_convert_matrix(self):
sim = Similarity()
numpy.testing.assert_array_equal(sim.as_matrix(), numpy.eye(4))
sim1 = Similarity(self.s, self.r, self.t)
mat1 = sim1.as_matrix()
sim2 = Similarity.from_matrix(mat1)
mat2 = sim2.as_matrix()
print("Sim1:", sim1.as_matrix())
print("Sim2:", sim2.as_matrix())
numpy.testing.assert_almost_equal(mat1, mat2, decimal=14)
def test_inverse(self):
# Inverse of identity is itself
s = Similarity()
nose.tools.assert_equal(s, s.inverse())
s = Similarity(self.s, self.r, self.t)
s_i = s.inverse()
i = s * s_i
# Similarity composed with inverse should be identity
nose.tools.assert_almost_equal(i.scale, 1., 14)
nose.tools.assert_almost_equal(i.rotation.angle(), 0., 14)
nose.tools.assert_almost_equal(numpy.linalg.norm(i.translation, 2), 0.,
14)
def test_get_rotation(self):
s = Similarity()
nose.tools.assert_equal(s.rotation, Rotation())
s = Similarity(self.s, self.r, self.t)
nose.tools.assert_equal(s.rotation, self.r)
def test_get_scale(self):
s = Similarity()
nose.tools.assert_equal(s.scale, 1.0)
s = Similarity(self.s, self.r, self.t)
nose.tools.assert_equal(s.scale, self.s)
def test_new_default(self):
s = Similarity()
nose.tools.assert_equal(s.scale, 1)
nose.tools.assert_equal(s.rotation, Rotation())
numpy.testing.assert_array_equal(s.translation,
EigenArray.from_iterable((0, 0, 0)))
def test_compose_fail(self):
s = Similarity(self.s, self.r, self.t)
nose.tools.assert_raises(ValueError, s.compose, 0)
nose.tools.assert_raises(ValueError, s.compose, 'foo')
nose.tools.assert_raises(ValueError, s.compose, [1, 2, 3])
def test_get_translation(self):
s = Similarity()
numpy.testing.assert_equal(s.translation, [[0], [0], [0]])
s = Similarity(self.s, self.r, self.t)
numpy.testing.assert_equal(s.translation, self.t)
def test_new_default(self):
s = Similarity()
nose.tools.assert_equal(s.scale, 1)
numpy.testing.assert_array_almost_equal(s.rotation.matrix(),
Rotation().matrix())
numpy.testing.assert_array_equal(s.translation, [0, 0, 0])
