def map(self, point):
"""
Map a 3D point using this homography
If the data type of the point given, if an EigenArray, is not the same
as this homography's ``datatype``, it will be automatically converted.
:param point: 2D point to transform
:type point: collections.Iterable[float] | EigenArray
:return: Transformed 2D point as an EigenArray
:rtype: EigenArray
"""
point_t = EigenArray.c_ptr_type(2, ctype=self._datatype)
point = EigenArray.from_iterable(point, self._datatype, (2, 1))
p_cptr = self._call_cfunc(
'vital_homography_%s_map_point' % self._tchar,
[self.C_TYPE_PTR, point_t],
[self, point],
point_t,
{
1: PointMapsToInfinityException
}
)
return EigenArray(2, dtype=self._datatype, from_cptr=p_cptr)
def normal(self, n):
n = EigenArray.from_iterable(n, self._datatype, (3, 1))
self._call_cfunc(
'vital_landmark_{}_set_normal'.format(self._tchar),
[self.C_TYPE_PTR,
EigenArray.c_ptr_type(3, ctype=self._datatype)], [self, n],
exception_map={1: VitalDynamicCastException})
def from_matrix(cls, m, datatype=ctypes.c_double):
"""
Create a homography from an existing 3x3 matrix.
If the data type of the matrix given is not the same as ``datatype``,
it will be automatically converted.
:param m: Matrix to base the new homography on. This should be a 3x3
matrix.
:type m: collections.Iterable[collections.Iterable[float]] | vital.types.EigenArray
:param datatype: Type to store data in the homography.
:type datatype: ctypes._SimpleCData
:return: New homography instance whose transform is equal to the given
matrix.
:rtype: Homography
"""
# noinspection PyProtectedMember
tchar = datatype._type_
m = EigenArray.from_iterable(m, datatype, (3, 3))
cptr = cls._call_cfunc(
'vital_homography_%s_new_from_matrix' % tchar,
[EigenArray.c_ptr_type(3, 3, datatype)], [m],
Homography.c_ptr_type()
)
return Homography(from_cptr=cptr)
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_center_initialized(self):
expected_c = EigenArray(3)
expected_c.get_matrix()[:] = [[1], [2], [3]]
cam = Camera(expected_c)
numpy.testing.assert_array_equal(
numpy.array([cam.center]).T, expected_c.get_matrix())
numpy.testing.assert_array_equal(cam.center, [1, 2, 3])
def clone_look_at(self, stare_point, up_direction=(0, 0, 1)):
"""
Create a clone of this camera that is rotated to look at the given point
The camera should also be rotated about its principal axis such that
the vertical image direction is closest to \c up_direction in the world.
:param stare_point: the location at which the camera is oriented to
point
:type stare_point: collections.Iterable | numpy.ndarray | EigenArray
:param up_direction: the vector which is "up" in the world (defaults to
Z-axis)
:type up_direction: collections.Iterable | numpy.ndarray | EigenArray
:return: New camera instance that is the clone of the one given, but set
to look at the given point.
:rtype: Camera
"""
stare_point = EigenArray.from_iterable(stare_point,
target_shape=(3, 1))
up_direction = EigenArray.from_iterable(up_direction,
target_shape=(3, 1))
cptr = self._call_cfunc('vital_camera_clone_look_at', [
self.C_TYPE_PTR,
EigenArray.c_ptr_type(3),
EigenArray.c_ptr_type(3)
], [self, stare_point, up_direction], self.C_TYPE_PTR)
return Camera(from_cptr=cptr)
def test_point_map(self):
h_f = Homography('f')
h_d = Homography('d')
p_af = EigenArray.from_array([[2.2, 3.3]], 'f')
p_f = p_af.get_matrix()[0]
p_ad = EigenArray.from_array([[5.5, 6.6]], 'd')
p_d = p_ad.get_matrix()[0]
# float-float
numpy.testing.assert_almost_equal(
h_f.map(p_f), p_f
)
# float-double
numpy.testing.assert_almost_equal(
h_f.map(p_d), p_d
)
# double-float
numpy.testing.assert_almost_equal(
h_d.map(p_f), p_f
)
# double-double
numpy.testing.assert_almost_equal(
h_d.map(p_d), p_d
)
# Code to generate truth
h = numpy.random.rand(3,3)
h = h/numpy.linalg.norm(h)
p0 = numpy.random.rand(3); p0[2] = 1
p1 = numpy.dot(h, p0)
p1 = p1[:2]/p1[2]
h_d = Homography.from_matrix(h, 'd')
# map from Numpy array.
numpy.testing.assert_almost_equal(
h_d.map(p0[:2]).ravel(), p1
)
# map from EigenArray
p0 = EigenArray.from_array([p0[:2]])
numpy.testing.assert_almost_equal(
h_d.map(p0.get_matrix()[0]).ravel(), p1
)
# Another explicit case.
p0 = numpy.array([1923.47,645.676,1])
h = numpy.array([[5.491496261770000276e-01,-1.125428185150000038e-01,
1.358427031619999923e+02],
[-1.429513389049999993e-02 ,6.035527375529999849e-01,
5.923971959490000216e+01],
[-2.042570000000000164e-06,-2.871670000000000197e-07,
1]])
p1 = numpy.dot(h, p0); p1 = p1[:2]/p1[2]
H = Homography.from_matrix(h)
P = EigenArray.from_array([p0[:2]])
numpy.testing.assert_almost_equal(
H.map(P.get_matrix()[0]).ravel(), p1
)
def test_point_map(self):
h_f = Homography(ctypes.c_float)
h_d = Homography(ctypes.c_double)
p_f = EigenArray.from_iterable([2.2, 3.3], ctypes.c_float)
p_d = EigenArray.from_iterable([5.5, 6.6], ctypes.c_double)
# float-float
numpy.testing.assert_almost_equal(
h_f.map(p_f), p_f
)
# float-double
numpy.testing.assert_almost_equal(
h_f.map(p_d), p_d
)
# double-float
numpy.testing.assert_almost_equal(
h_d.map(p_f), p_f
)
# double-double
numpy.testing.assert_almost_equal(
h_d.map(p_d), p_d
)
# Code to generate truth
h = numpy.random.rand(3,3)
h = h/numpy.linalg.norm(h)
p0 = numpy.random.rand(3); p0[2] = 1
p1 = numpy.dot(h, p0)
p1 = p1[:2]/p1[2]
h_d = Homography.from_matrix(h, ctypes.c_double)
# map from Numpy array.
numpy.testing.assert_almost_equal(
h_d.map(p0[:2]).ravel(), p1
)
# map from EigenArray
p0 = EigenArray.from_iterable(p0[:2])
numpy.testing.assert_almost_equal(
h_d.map(p0).ravel(), p1
)
# Another explicit case.
p0 = numpy.array([1923.47,645.676,1])
h = numpy.array([[5.491496261770000276e-01,-1.125428185150000038e-01,
1.358427031619999923e+02],
[-1.429513389049999993e-02 ,6.035527375529999849e-01,
5.923971959490000216e+01],
[-2.042570000000000164e-06,-2.871670000000000197e-07,
1]])
p1 = numpy.dot(h, p0); p1 = p1[:2]/p1[2]
H = Homography.from_matrix(h)
P = EigenArray.from_iterable(p0[:2])
numpy.testing.assert_almost_equal(
H.map(P).ravel(), p1
)
def loc(self):
"""
Get the 3D location of this landmark
:return: 3D location of this landmark
:rtype: EigenArray
"""
cptr = self._call_cfunc('vital_landmark_loc', [self.C_TYPE_PTR],
[self], EigenArray.c_ptr_type(3))
return EigenArray(3, from_cptr=cptr)
def location(self):
""" Get feature location """
cptr = self._call_cfunc(
'vital_feature_loc',
[self.C_TYPE_PTR],
[self],
EigenArray.c_ptr_type(2),
)
return EigenArray(2, from_cptr=cptr)
def test_dynamic_size_init(self):
a = EigenArray(2, dynamic_rows=True)
ntools.assert_equal(a.shape, (2, 1))
a = EigenArray(300, dynamic_rows=True)
ntools.assert_equal(a.shape, (300, 1))
a = EigenArray(1234, 256, dynamic_rows=True, dynamic_cols=True)
ntools.assert_equal(a.shape, (1234, 256))
def translation(self):
"""
Get the translation of this similarity transformation
:rtype: EigenArray
"""
trans_ptr = self._call_cfunc(
'vital_similarity_%s_translation' % self._tchar, [self.C_TYPE_PTR],
[self], EigenArray.c_ptr_type(3, 1, self._ctype))
return EigenArray(3, 1, dtype=self._ctype, from_cptr=trans_ptr)
def as_matrix(self):
"""
:return: Get this homography as a 3x3 matrix (same data type as this
homography).
:rtype: EigenArray
"""
m_cptr = self._call_cfunc(
'vital_homography_%s_as_matrix' % self._tchar, [self.C_TYPE_PTR],
[self], EigenArray.c_ptr_type(3, 3, self._datatype))
return EigenArray(3, 3, dtype=self._datatype, from_cptr=m_cptr)
def test_from_iterable_1D(self):
# 1-dim iterables/vectors are treated as column vectors
input = [1, 2, 3, 4]
expected = [[1], [2], [3], [4]]
e = EigenArray.from_iterable(input)
numpy.testing.assert_equal(e, expected)
e2 = EigenArray.from_iterable(e)
numpy.testing.assert_equal(e, e2)
def as_matrix(self):
"""
:return: similarity transformation as a 4x4 matrix
:rtype: EigenArray
"""
cptr = self._call_cfunc(
'vital_similarity_%s_to_matrix4x4' % self._tchar,
[self.C_TYPE_PTR], [self],
EigenArray.c_ptr_type(4, 4, self._ctype))
return EigenArray(4, 4, dtype=self._ctype, from_cptr=cptr)
def test_mutability(self):
a = EigenArray(2, 3)
d = a.get_matrix() # The data pointer
for i in range(2):
for j in range(3):
d[i][j] = 0
numpy.testing.assert_array_equal(a.get_matrix(),
[[0, 0, 0], [0, 0, 0]])
d[:] = 1
numpy.testing.assert_array_equal(a.get_matrix(),
[[1, 1, 1], [1, 1, 1]])
d[1, 0] = 2
numpy.testing.assert_array_equal(a.get_matrix(),
[[1, 1, 1], [2, 1, 1]])
d[:, 2] = 3
numpy.testing.assert_array_equal(a.get_matrix(),
[[1, 1, 3], [2, 1, 3]])
d += 1
numpy.testing.assert_array_equal(a.get_matrix(),
[[2, 2, 4], [3, 2, 4]])
b = d * 0
numpy.testing.assert_array_equal(a.get_matrix(),
[[2, 2, 4], [3, 2, 4]])
numpy.testing.assert_array_equal(b, [[0, 0, 0], [0, 0, 0]])
def loc(self, new_loc):
"""
Set the 3D location of this landmark
:param new_loc: New 3D location
:type new_loc: collections.Iterable[float]
"""
new_loc = EigenArray.from_iterable(new_loc, self._datatype, (3, 1))
self._call_cfunc(
'vital_landmark_{}_set_loc'.format(self._tchar),
[self.C_TYPE_PTR,
EigenArray.c_ptr_type(3, ctype=self._datatype)], [self, new_loc])
def translation(self):
"""
:return: a copy of this camera's translation vector
:rtype: EigenArray
"""
cam_trans = self.VITAL_LIB['vital_camera_translation']
cam_trans.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_trans.restype = EigenArray.c_ptr_type(3)
with VitalErrorHandle() as eh:
c_ptr = cam_trans(self, eh)
return EigenArray(3, from_cptr=c_ptr)
def test_from_array_1D(self):
# 1-dim iterables/vectors are treated as column vectors
input = [[1], [2], [3], [4]]
expected = [[1], [2], [3], [4]]
e = EigenArray.from_array(input)
em = e.get_matrix()
numpy.testing.assert_equal(em, expected)
e2 = EigenArray.from_array(em)
em2 = e2.get_matrix()
numpy.testing.assert_equal(em, em2)
def test_center_initialized(self):
expected_c = EigenArray(3)
expected_c.get_matrix()[:] = [[1],[2],[3]]
cam = Camera(expected_c)
numpy.testing.assert_array_equal(
numpy.array([cam.center]).T,
expected_c.get_matrix()
)
numpy.testing.assert_array_equal(
cam.center,
[1,2,3]
)
def test_from_iterable_1D(self):
# 1-dim iterables/vectors are treated as column vectors
input = [1, 2, 3, 4]
expected = [[1],
[2],
[3],
[4]]
e = EigenArray.from_iterable(input)
numpy.testing.assert_equal(e, expected)
e2 = EigenArray.from_iterable(e)
numpy.testing.assert_equal(e, e2)
def test_matrix_init(self):
# Test that construction does not fail when passing valid matrices as
# initializer
m1 = [[0, 1, 3], [0.3, 0.1, 10], [-1, 8.1, 4.7]]
m2_d = EigenArray.from_array(m1, 'd')
m2_f = EigenArray.from_array(m1, 'f')
Homography.from_matrix(m1, 'd')
Homography.from_matrix(m1, 'f')
Homography.from_matrix(m2_d.get_matrix(), 'd')
Homography.from_matrix(m2_d.get_matrix(), 'f')
Homography.from_matrix(m2_f.get_matrix(), 'd')
Homography.from_matrix(m2_f.get_matrix(), 'f')
def as_matrix(self):
"""
Convert camera into a new 3x4 homogeneous projection matrix.
:return: new 3x4 homogeneous projection matrix
:rtype: EigenArray
"""
cam_asmat = self.VITAL_LIB['vital_camera_as_matrix']
cam_asmat.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_asmat.restype = EigenArray.c_ptr_type(3, 4)
with VitalErrorHandle() as eh:
cptr = cam_asmat(self, eh)
return EigenArray(3, 4, from_cptr=cptr)
def test_matrix_init(self):
# Test that construction does not fail when passing valid matrices as
# initializer
m1 = [[0, 1, 3], [0.3, 0.1, 10], [-1, 8.1, 4.7]]
m2_d = EigenArray.from_iterable(m1, ctypes.c_double, (3, 3))
m2_f = EigenArray.from_iterable(m1, ctypes.c_float, (3, 3))
Homography.from_matrix(m1, ctypes.c_double)
Homography.from_matrix(m1, ctypes.c_float)
Homography.from_matrix(m2_d, ctypes.c_double)
Homography.from_matrix(m2_d, ctypes.c_float)
Homography.from_matrix(m2_f, ctypes.c_double)
Homography.from_matrix(m2_f, ctypes.c_float)
def _new(self, loc, mag, scale, angle, rgb_color):
loc = EigenArray.from_iterable(loc,
target_ctype=self._datatype,
target_shape=(2, 1))
if rgb_color is None:
rgb_color = RGBColor()
# noinspection PyProtectedMember
return self._call_cfunc('vital_feature_{}_new'.format(self._tchar), [
EigenArray.c_ptr_type(2, 1, self._datatype), self._datatype,
self._datatype, self._datatype,
RGBColor.c_ptr_type()
], [loc, mag, scale, angle, rgb_color], self.C_TYPE_PTR)
def test_mutability(self):
a = EigenArray(2, 3)
d = a.get_matrix() # The data pointer
for i in range(2):
for j in range(3):
d[i][j] = 0
numpy.testing.assert_array_equal(a.get_matrix(), [[0, 0, 0],
[0, 0, 0]])
d[:] = 1
numpy.testing.assert_array_equal(a.get_matrix(), [[1, 1, 1],
[1, 1, 1]])
d[1, 0] = 2
numpy.testing.assert_array_equal(a.get_matrix(), [[1, 1, 1],
[2, 1, 1]])
d[:, 2] = 3
numpy.testing.assert_array_equal(a.get_matrix(), [[1, 1, 3],
[2, 1, 3]])
d += 1
numpy.testing.assert_array_equal(a.get_matrix(), [[2, 2, 4],
[3, 2, 4]])
b = d*0
numpy.testing.assert_array_equal(a.get_matrix(), [[2, 2, 4],
[3, 2, 4]])
numpy.testing.assert_array_equal(b, [[0, 0, 0],
[0, 0, 0]])
def center(self):
"""
:return: a copy of this camera's center coordinate.
:rtype: EigenArray
"""
cam_center = self.VITAL_LIB['vital_camera_center']
cam_center.argtypes = [
self.c_ptr_type(),
VitalErrorHandle.c_ptr_type()
]
cam_center.restype = EigenArray.c_ptr_type(3)
with VitalErrorHandle() as eh:
c_ptr = cam_center(self, eh)
return EigenArray(3, from_cptr=c_ptr)
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 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_valid_static_size_init(self):
"""
Test construction of some of the static sizes.
"""
a = EigenArray() # default shape
ntools.assert_equal(a.shape, (2, 1))
a = EigenArray(2, 1)
ntools.assert_equal(a.shape, (2, 1))
a = EigenArray(2, 2)
ntools.assert_equal(a.shape, (2, 2))
a = EigenArray(4, 4)
ntools.assert_equal(a.shape, (4, 4))
def test_point_map(self):
h_f = Homography(ctypes.c_float)
h_d = Homography(ctypes.c_double)
p_f = EigenArray.from_iterable([2.2, 3.3], ctypes.c_float)
p_d = EigenArray.from_iterable([5.5, 6.6], ctypes.c_double)
# float-float
numpy.testing.assert_almost_equal(h_f.map(p_f), p_f)
# float-double
numpy.testing.assert_almost_equal(h_f.map(p_d), p_d)
# double-float
numpy.testing.assert_almost_equal(h_d.map(p_f), p_f)
# double-double
numpy.testing.assert_almost_equal(h_d.map(p_d), p_d)
def test_from_array_1D(self):
# 1-dim iterables/vectors are treated as column vectors
input = [[1], [2], [3], [4]]
expected = [[1],
[2],
[3],
[4]]
e = EigenArray.from_array(input)
em = e.get_matrix()
numpy.testing.assert_equal(em, expected)
e2 = EigenArray.from_array(em)
em2 = e2.get_matrix()
numpy.testing.assert_equal(em, em2)
def test_matrix_init(self):
# Test that construction does not fail when passing valid matrices as
# initializer
m1 = [[0, 1, 3],
[0.3, 0.1, 10],
[-1, 8.1, 4.7]]
m2_d = EigenArray.from_array(m1, 'd')
m2_f = EigenArray.from_array(m1, 'f')
Homography.from_matrix(m1, 'd')
Homography.from_matrix(m1, 'f')
Homography.from_matrix(m2_d.get_matrix(), 'd')
Homography.from_matrix(m2_d.get_matrix(), 'f')
Homography.from_matrix(m2_f.get_matrix(), 'd')
Homography.from_matrix(m2_f.get_matrix(), 'f')
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_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_order_transform(self):
a = EigenArray(2, 3)
d = a.base.base # The data pointer
# column-major 2x3 matrix [[ 1 2 3 ] (Eigen format)
# [ 4 5 6 ]]
d[0] = 1; d[2] = 2; d[4] = 3
d[1] = 4; d[3] = 5; d[5] = 6
numpy.testing.assert_array_equal(a, [[1., 2., 3.],
[4., 5., 6.]])
ntools.assert_equal(a.at_eigen_base_index(0, 0), 1)
ntools.assert_equal(a.at_eigen_base_index(0, 1), 2)
ntools.assert_equal(a.at_eigen_base_index(0, 2), 3)
ntools.assert_equal(a.at_eigen_base_index(1, 0), 4)
ntools.assert_equal(a.at_eigen_base_index(1, 1), 5)
ntools.assert_equal(a.at_eigen_base_index(1, 2), 6)
def _new(self, loc, mag, scale, angle, rgb_color):
loc = EigenArray.from_iterable(loc, target_ctype=self._datatype,
target_shape=(2, 1))
if rgb_color is None:
rgb_color = RGBColor()
# noinspection PyProtectedMember
return self._call_cfunc(
'vital_feature_{}_new'.format(self._tchar),
[
EigenArray.c_ptr_type(2, 1, self._datatype),
self._datatype, self._datatype, self._datatype,
RGBColor.c_ptr_type()
],
self.C_TYPE_PTR,
loc, mag, scale, angle, rgb_color
)
def test_clone_look_at(self):
pp = EigenArray.from_iterable([300, 400])
k = CameraIntrinsics(1000, pp)
focus = EigenArray.from_iterable([0, 1, -2])
base = Camera([3, -4, 7], Rotation(), k)
cam = base.clone_look_at(focus)
nose.tools.assert_not_equal(base, cam)
ifocus = cam.project(focus)
nose.tools.assert_almost_equal(numpy.linalg.norm(ifocus - pp, 2), 0.,
12)
ifocus_up = cam.project(focus + EigenArray.from_iterable([0, 0, 2]))
tmp = ifocus_up - pp
nose.tools.assert_almost_equal(tmp[0], 0., 12)
nose.tools.assert_true(tmp[1] < 0.)
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 project(self, point):
"""
Project a 3D point into a new 2D image point (eigen array) via this
camera model.
:param point: 3D point to transform into the 2D image plane.
:return: New 2D eigen array that is the transformed point.
"""
point = EigenArray.from_iterable(point, target_shape=(3, 1))
cam_project = self.VITAL_LIB['vital_camera_project']
cam_project.argtypes = [self.c_ptr_type(),
EigenArray.c_ptr_type(3),
VitalErrorHandle.c_ptr_type()]
cam_project.restype = EigenArray.c_ptr_type(2)
with VitalErrorHandle() as eh:
cptr = cam_project(self, point, eh)
return EigenArray(2, from_cptr=cptr)
def from_matrix(cls, m, ctype=DFLT_CTYPE):
"""
Create a similarity transform instance from a 4x4 matrix
:param m: 4x4 transformation matrix
:type m: EigenArray | collections.Iterable[float]
:return: new Similarity instance
:rtype: Similarity
"""
m = EigenArray.from_iterable(m, ctype, target_shape=(4, 4))
# noinspection PyProtectedMember
tchar = ctype._type_
cptr = cls._call_cfunc('vital_similarity_%s_from_matrix4x4' % tchar,
[EigenArray.c_ptr_type(4, 4, ctype)], [m],
Similarity.c_ptr_type(ctype))
return Similarity(ctype=ctype, from_cptr=cptr)
def location(self):
""" Get feature location """
cptr = self._call_cfunc(
'vital_feature_loc',
[self.C_TYPE_PTR],
EigenArray.c_ptr_type(2),
self
)
return EigenArray(2, from_cptr=cptr)
def transform_vector(self, vec):
"""
Transform a 3D vector with this similarity transformation
:param vec: 3D Vector to transform
:type vec: collections.Iterable[float]
:return: Transformed 3D vector
:rtype: EigenArray
"""
vec = EigenArray.from_iterable(vec, self._ctype, (3, 1))
t_ptr = self._call_cfunc(
'vital_similarity_%s_vector_transform' % self._tchar,
[self.C_TYPE_PTR,
EigenArray.c_ptr_type(3, 1, self._ctype)], [self, vec],
EigenArray.c_ptr_type(3, 1, self._ctype))
return EigenArray(3, 1, dtype=self._ctype, from_cptr=t_ptr)
def test_clone_look_at(self):
pp = EigenArray.from_array([[300], [400]])
k = CameraIntrinsics(1000, [300, 400])
focus = EigenArray.from_array([[0], [1], [-2]])
center = EigenArray.from_array([[3],[-4],[7]])
base = Camera(center, Rotation(), k)
cam = base.clone_look_at(numpy.array([0,1,2]))
nose.tools.assert_not_equal(base, cam)
ifocus = cam.project([0,1,2])
nose.tools.assert_almost_equal(numpy.linalg.norm(ifocus - pp.get_matrix().T, 2),
0., 12)
ifocus_up = cam.project([0,1,4])
tmp = (ifocus_up - pp.get_matrix().T)[0]
nose.tools.assert_almost_equal(tmp[0], 0., 12)
nose.tools.assert_true(tmp[1] < 0.)
def test_from_iterable(self):
# from list
expected_list = [[0.4, 0],
[1, 1.123],
[2.253, 4.768124]]
ea = EigenArray.from_iterable(expected_list, target_shape=(3, 2))
numpy.testing.assert_array_equal(ea, expected_list)
# from ndarray
expected_ndar = numpy.array(expected_list)
ea = EigenArray.from_iterable(expected_ndar, target_shape=(3, 2))
numpy.testing.assert_array_equal(ea, expected_ndar)
# from EigenArray, which should return the input object
ea = EigenArray(3, 2)
ea[:] = expected_list
ea2 = EigenArray.from_iterable(ea, target_shape=(3, 2))
numpy.testing.assert_array_equal(ea2, ea)
ntools.assert_is(ea, ea2)
ntools.assert_true(ea is ea2)
def init_landmarks(num_lm, c=None):
"""
construct map of landmarks will all locations at ``c`` with IDs in range
``[0, num_lm]``.
"""
if c is None:
c = EigenArray.from_iterable([0, 0, 0])
d = {}
for i in range(num_lm):
d[i] = Landmark(loc=c)
return LandmarkMap.from_dict(d)
def center(self):
"""
:return: a copy of this camera's center coordinate.
:rtype: EigenArray
"""
cam_center = self.VITAL_LIB['vital_camera_center']
cam_center.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_center.restype = EigenArray.c_ptr_type(3)
with VitalErrorHandle() as eh:
c_ptr = cam_center(self, eh)
return EigenArray(3, from_cptr=c_ptr)
def translation(self):
"""
:return: a copy of this camera's translation vector
:rtype: EigenArray
"""
cam_trans = self.VITAL_LIB['vital_camera_translation']
cam_trans.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_trans.restype = EigenArray.c_ptr_type(3)
with VitalErrorHandle() as eh:
c_ptr = cam_trans(self, eh)
return EigenArray(3, from_cptr=c_ptr)
def test_new_matrix(self):
a = EigenArray(2, 2, type='d')
m = a.get_matrix()
m[:] = 1.
c = Covariance.from_matrix(2, 'd', m)
m_out = c.to_matrix()
print('input matrix:\n', m)
print('output matrix:\n', m_out)
numpy.testing.assert_array_equal(m_out, m)
# Type casting should be handled
a = EigenArray(2, 2, type='f')
m = a.get_matrix()
m[:] = 1.
c = Covariance.from_matrix(2, 'd', m)
m_out = c.to_matrix()
print('input matrix:\n', m)
print('output matrix:\n', m_out)
numpy.testing.assert_array_equal(m_out, m)
# Any other numpy array of the correct shape should be acceptable
m = numpy.ndarray((2, 2))
m[:] = 3.
c = Covariance.from_matrix(2, 'f', init=m)
m_out = c.to_matrix()
print('input matrix:\n', m)
print('output matrix:\n', m_out)
numpy.testing.assert_array_equal(m_out, m)
# Diagonally congruent values should be averages when initializing with
# matrix
m = numpy.eye(3, dtype=numpy.double)
m[0,2] = 2.
m_expected = m.copy()
m_expected[0,2] = 1.
m_expected[2,0] = 1.
c = Covariance.from_matrix(3, init=m)
m_out = c.to_matrix()
print('input matrix:\n', m)
print('output matrix:\n', m_out)
numpy.testing.assert_array_equal(m_out, m_expected)
def location(self, loc):
""" Set new feature location
:param loc: New locations. May be any iterable of 2 elements.
:type loc: collections.Iterable
"""
loc = EigenArray.from_iterable(loc, self._datatype, (2, 1))
self._call_cfunc(
'vital_feature_{}_set_loc'.format(self._tchar),
[self.C_TYPE_PTR, loc.C_TYPE_PTR],
None,
self, loc
)
def depth(self, point):
"""
Compute the distance of the 3d point to the image plane.
Points with negative depth are behind the camera.
:param point: 3D point to find the depth of.
:return: Depth value
:rtype: float
"""
point = EigenArray.from_iterable(point)
cam_depth = self.VITAL_LIB['vital_camera_depth']
cam_depth.argtypes = [self.c_ptr_type(),
EigenArray.c_ptr_type(3),
VitalErrorHandle.c_ptr_type()]
cam_depth.restype = ctypes.c_double
with VitalErrorHandle() as eh:
return cam_depth(self, point, eh)
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 as_matrix(self):
"""
Convert camera into a new 3x4 homogeneous projection matrix.
:return: new 3x4 homogeneous projection matrix
:rtype: EigenArray
"""
cam_asmat = self.VITAL_LIB['vital_camera_as_matrix']
cam_asmat.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_asmat.restype = EigenArray.c_ptr_type(3, 4)
with VitalErrorHandle() as eh:
cptr = cam_asmat(self, eh)
return EigenArray(3, 4, from_cptr=cptr)
def location(self, loc):
""" Set new feature location
:param loc: New locations. May be any iterable of 2 elements.
:type loc: collections.Iterable
"""
if self._datatype is None:
raise VitalNoTypeInfoException("Type info required but not present")
loc = EigenArray.from_iterable(loc, self._datatype, (2, 1))
self._call_cfunc(
'vital_feature_{}_set_loc'.format(self._tchar),
[self.C_TYPE_PTR, loc.C_TYPE_PTR],
[self, loc],
)
def test_from_array(self):
# from list
expected_list = [[0.4, 0],
[1, 1.123],
[2.253, 4.768124]]
ea = EigenArray.from_array(expected_list)
em = ea.get_matrix()
numpy.testing.assert_array_equal(em, expected_list)
# from ndarray
expected_ndar = numpy.array(expected_list)
ea = EigenArray.from_array(expected_ndar)
em = ea.get_matrix()
numpy.testing.assert_array_equal(em, expected_ndar)
# from EigenArray, which should return the input object
ea = EigenArray(3, 2)
em = ea.get_matrix()
em[:] = expected_list
ea2 = EigenArray.from_array(em)
em2 = ea2.get_matrix()
numpy.testing.assert_array_equal(em2, em)
def test_new_matrix(self):
m = EigenArray(2, 2, dtype=numpy.double)
m[:] = 1.
c = Covariance(2, ctypes.c_double, m)
m_out = c.to_matrix()
print 'input matrix:\n', m
print 'output matrix:\n', m_out
numpy.testing.assert_array_equal(m_out, m)
# Type casting should be handled
m = EigenArray(2, 2, dtype=numpy.float32)
m[:] = 1.
c = Covariance(2, ctypes.c_double, m)
m_out = c.to_matrix()
print 'input matrix:\n', m
print 'output matrix:\n', m_out
numpy.testing.assert_array_equal(m_out, m)
# Any other numpy array of the correct shape should be acceptable
m = numpy.ndarray((2, 2))
m[:] = 3.
c = Covariance(2, ctypes.c_float, init_scalar_or_matrix=m)
m_out = c.to_matrix()
print 'input matrix:\n', m
print 'output matrix:\n', m_out
numpy.testing.assert_array_equal(m_out, m)
# Diagonally congruent values should be averages when initializing with
# matrix
m = numpy.eye(3, dtype=numpy.double)
m[0,2] = 2.
m_expected = m.copy()
m_expected[0,2] = 1.
m_expected[2,0] = 1.
c = Covariance(3, init_scalar_or_matrix=m)
m_out = c.to_matrix()
print 'input matrix:\n', m
print 'output matrix:\n', m_out
numpy.testing.assert_array_equal(m_out, m_expected)
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 cube_corners(s, c=None):
"""
Construct map of landmarks at the corners of a cube centered on ``c`` with a
side length of ``s``.
:rtype: LandmarkMap
"""
if c is None:
c = EigenArray.from_iterable([0, 0, 0])
s /= 2.
# Can add lists to numpy.ndarray types
d = {
0: Landmark(c + EigenArray.from_iterable([-s, -s, -s])),
1: Landmark(c + EigenArray.from_iterable([-s, -s, s])),
2: Landmark(c + EigenArray.from_iterable([-s, s, -s])),
3: Landmark(c + EigenArray.from_iterable([-s, s, s])),
4: Landmark(c + EigenArray.from_iterable([ s, -s, -s])),
5: Landmark(c + EigenArray.from_iterable([ s, -s, s])),
6: Landmark(c + EigenArray.from_iterable([ s, s, -s])),
7: Landmark(c + EigenArray.from_iterable([ s, s, s])),
}
return LandmarkMap.from_dict(d)
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 init_cameras(num_cams=20, intrinsics=None):
"""
Initialize camera sequence with all cameras at the same location (0, 0, 1)
and looking at origin.
:param num_cams: Number of cameras to create, default 20.
:param intrinsics: Intrinsics to use for all cameras.
:return: Camera map of initialize cameras
"""
if intrinsics is None:
intrinsics = CameraIntrinsics(1000, (640, 480))
r = Rotation()
c = EigenArray.from_iterable((0, 0, 1))
d = {}
for i in range(num_cams):
cam = Camera(c, r, intrinsics).clone_look_at([0, 0, 0],
[0, 1, 0])
d[i] = cam
return CameraMap(d)