def __new__(cls, size=128, ctype=ctypes.c_double, from_cptr=None):
"""
Create a descriptor instance
:param size: Size of the descriptor (number of elements). Default of 128
(arbitrary).
:param ctype: Data type that this data is represented as under the hood.
:param from_cptr: Existing Descriptor instance to wrap.
"""
if from_cptr is None:
d_type = ctype
# noinspection PyProtectedMember
d_type_char = ctype._type_
d_new = cls.VITAL_LIB['vital_descriptor_new_{}'.format(
d_type_char)]
d_new.argtypes = [ctypes.c_size_t, VitalErrorHandle.c_ptr_type()]
d_new.restype = cls.c_ptr_type()
with VitalErrorHandle() as eh:
inst_ptr = d_new(size, eh)
else:
if not isinstance(from_cptr, cls.c_ptr_type()):
raise ValueError("Invalid ``from_cptr`` value (given %s" %
type(from_cptr))
inst_ptr = from_cptr
# Get type char from generic data type introspection function
# ASSUMING typename from c++ is the same as ctypes _type_ values,
# which is at least currently true for float/double types, which
# is all that we care about / is implemented in C/C++.
d_type = TYPE_NAME_MAP[cls._call_cfunc(
'vital_descriptor_type_name', [cls.c_ptr_type()], [inst_ptr],
ctypes.c_char_p)]
# noinspection PyProtectedMember
d_type_char = d_type._type_
# Extract existing instance size information
size = cls._call_cfunc('vital_descriptor_size', [cls.c_ptr_type()],
[inst_ptr], ctypes.c_size_t)
# Get the raw-data pointer from inst to wrap array around
d_raw_data = cls.VITAL_LIB['vital_descriptor_get_{}_raw_data'.format(
d_type_char)]
d_raw_data.argtypes = [cls.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
d_raw_data.restype = ctypes.POINTER(d_type)
# TODO: We could recover from an exception here by parsing the type
# expected in the error message and changing the construction type
with VitalErrorHandle() as eh:
eh.set_exception_map({1: VitalDynamicCastException})
data_ptr = d_raw_data(inst_ptr, eh)
b = numpy.ctypeslib.as_array(data_ptr, (size, ))
npy_type = numpy.dtype(d_type)
obj = numpy.ndarray.__new__(cls, size, npy_type, b)
obj._inst_ptr = inst_ptr
obj._owns_data = True # This is the owning instance
return obj
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 _call_cfunc(cls, func_name, argtypes, restype, *args):
"""
Extract function from vital library and call it with a VitalErrorHandle.
This assumes that the C function takes an additional parameter than what
is given to this function that is the error handle.
:param func_name: C function name to pull from library
:type func_name: str
:param argtypes: Ctypes argument type array
:type argtypes: list | tuple
:param restype: Ctypes return type
:param args: iterable of positional arguments to the C function
:param args: tuple
:return: Result of the c function call
"""
# local import to prevent circular import
from vital.util import VitalErrorHandle
f = cls.VITAL_LIB[func_name]
if argtypes:
f.argtypes = list(argtypes) + [VitalErrorHandle.c_ptr_type()]
f.restype = restype
with VitalErrorHandle() as eh:
return f(*(args + (eh,)))
def to_dict(self):
"""
:return: Internal frame-number to cameras mapping
:rtype: dict[int, Camera]
"""
cm_get_map = self.VITAL_LIB['vital_camera_map_get_map']
cm_get_map.argtypes = [
self.c_ptr_type(),
ctypes.POINTER(ctypes.c_size_t),
ctypes.POINTER(ctypes.POINTER(ctypes.c_int64)),
ctypes.POINTER(ctypes.POINTER(Camera.c_ptr_type())),
VitalErrorHandle.c_ptr_type()
]
length = ctypes.c_size_t()
frame_numbers = ctypes.POINTER(ctypes.c_int64)()
cameras = ctypes.POINTER(Camera.c_ptr_type())()
with VitalErrorHandle() as eh:
cm_get_map(self, ctypes.byref(length), ctypes.byref(frame_numbers),
ctypes.byref(cameras), eh)
m = {}
for i in xrange(length.value):
# copy camera cptr so we don't
cptr = Camera.c_ptr_type()(cameras[i].contents)
m[frame_numbers[i]] = Camera(from_cptr=cptr)
# Free frame number and camera pointer arrays
free_ptr = self.VITAL_LIB['vital_free_pointer']
free_ptr(frame_numbers)
free_ptr(cameras)
return m
def covariance(self):
"""
:return: a copy of this camera's center covariance
:rtype: Covariance
"""
cam_covar = self.VITAL_LIB['vital_camera_center_covar']
cam_covar.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_covar.restype = Covariance.c_ptr_type(3)
with VitalErrorHandle() as eh:
c_ptr = cam_covar(self, eh)
return Covariance(3, from_cptr=c_ptr)
def intrinsics(self):
"""
:return: a reference to this camera's intrinsics object
:rtype: CameraIntrinsics
"""
cam_int = self.VITAL_LIB['vital_camera_intrinsics']
cam_int.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_int.restype = CameraIntrinsics.c_ptr_type()
with VitalErrorHandle() as eh:
c_ptr = cam_int(self, eh)
return CameraIntrinsics(from_cptr=c_ptr)
def intrinsics(self):
"""
:return: a reference to this camera's intrinsics object
:rtype: CameraIntrinsics
"""
cam_int = self.VITAL_LIB['vital_camera_intrinsics']
cam_int.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_int.restype = CameraIntrinsics.c_ptr_type()
with VitalErrorHandle() as eh:
c_ptr = cam_int(self, eh)
return CameraIntrinsics(from_cptr=c_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 covariance(self):
"""
:return: a copy of this camera's center covariance
:rtype: Covariance
"""
cam_covar = self.VITAL_LIB['vital_camera_center_covar']
cam_covar.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_covar.restype = Covariance.c_ptr_type(3)
with VitalErrorHandle() as eh:
c_ptr = cam_covar(self, eh)
return Covariance(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 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 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 __new__(cls, size=128, ctype=ctypes.c_double, from_cptr=None):
"""
Create a descriptor instance
:param size: Size of the descriptor (number of elements). Default of 128
(arbitrary).
:param ctype: Data type that this data is represented as under the hood.
:param from_cptr: Existing Descriptor instance to wrap.
"""
# noinspection PyProtectedMember
type_char = ctype._type_
if from_cptr is None:
d_new = cls.VITAL_LIB['vital_descriptor_new_{}'.format(type_char)]
d_new.argtypes = [ctypes.c_size_t, VitalErrorHandle.c_ptr_type()]
d_new.restype = cls.c_ptr_type()
with VitalErrorHandle() as eh:
inst_ptr = d_new(size, eh)
else:
if not isinstance(from_cptr, cls.c_ptr_type()):
raise ValueError("Invalid ``from_cptr`` value (given %s" %
type(from_cptr))
inst_ptr = from_cptr
# Get the raw-data pointer from inst to wrap array around
d_raw_data = cls.VITAL_LIB['vital_descriptor_get_{}_raw_data'.format(
type_char)]
d_raw_data.argtypes = [cls.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
d_raw_data.restype = ctypes.POINTER(ctype)
# TODO: We could recover from an exception here by parsing the type
# expected in the error message and changing the construction type
with VitalErrorHandle() as eh:
eh.set_exception_map({1: VitalDynamicCastException})
data_ptr = d_raw_data(inst_ptr, eh)
b = numpy.ctypeslib.as_array(data_ptr, (size, ))
dtype = numpy.dtype(ctype)
obj = numpy.ndarray.__new__(cls, size, dtype, b)
obj._inst_ptr = inst_ptr
obj._owns_data = True # This is the owning instance
return obj
def __new__(cls, size=128, ctype=ctypes.c_double, from_cptr=None):
"""
Create a descriptor instance
:param size: Size of the descriptor (number of elements). Default of 128
(arbitrary).
:param ctype: Data type that this data is represented as under the hood.
:param from_cptr: Existing Descriptor instance to wrap.
"""
# noinspection PyProtectedMember
type_char = ctype._type_
if from_cptr is None:
d_new = cls.VITAL_LIB['vital_descriptor_new_{}'.format(type_char)]
d_new.argtypes = [ctypes.c_size_t, VitalErrorHandle.c_ptr_type()]
d_new.restype = cls.c_ptr_type()
with VitalErrorHandle() as eh:
inst_ptr = d_new(size, eh)
else:
if not isinstance(from_cptr, cls.c_ptr_type()):
raise ValueError("Invalid ``from_cptr`` value (given %s"
% type(from_cptr))
inst_ptr = from_cptr
# Get the raw-data pointer from inst to wrap array around
d_raw_data = cls.VITAL_LIB['vital_descriptor_get_{}_raw_data'
.format(type_char)]
d_raw_data.argtypes = [cls.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
d_raw_data.restype = ctypes.POINTER(ctype)
# TODO: We could recover from an exception here by parsing the type
# expected in the error message and changing the construction type
with VitalErrorHandle() as eh:
eh.set_exception_map({1: VitalDynamicCastException})
data_ptr = d_raw_data(inst_ptr, eh)
b = numpy.ctypeslib.as_array(data_ptr, (size,))
dtype = numpy.dtype(ctype)
obj = numpy.ndarray.__new__(cls, size, dtype, b)
obj._inst_ptr = inst_ptr
obj._owns_data = True # This is the owning instance
return obj
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 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 tobytearray(self):
"""
:return: a copy of the descriptor vector as a numpy.ndarray. Copied data
will be of bytes regardless of stored data type.
:rtype: vital.util.array_wrapping.CArrayWrapper
"""
d_as_bytes = self.VITAL_LIB['vital_descriptor_as_bytes']
d_as_bytes.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
d_as_bytes.restype = ctypes.POINTER(ctypes.c_uint8)
with VitalErrorHandle() as eh:
cptr = d_as_bytes(self, eh)
return CArrayWrapper(cptr, self.nbytes, ctypes.c_uint8, free_void_ptr)
def from_string(cls, s):
"""
:param s: String camera representation. This must be in the same form
that would be generated by this class' ``as_string`` method.
:type s: str
:return: New camera instance from a string sequence.
:rtype: Camera
"""
cam_from_str = cls.VITAL_LIB['vital_camera_new_from_string']
cam_from_str.argtypes = [ctypes.c_char_p, VitalErrorHandle.c_ptr_type()]
cam_from_str.restype = cls.c_ptr_type()
with VitalErrorHandle() as eh:
cptr = cam_from_str(s, eh)
return Camera(from_cptr=cptr)
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 tobytearray(self):
"""
:return: a copy of the descriptor vector as a numpy.ndarray. Copied data
will be of bytes regardless of stored data type.
:rtype: vital.util.array_wrapping.CArrayWrapper
"""
d_as_bytes = self.VITAL_LIB['vital_descriptor_as_bytes']
d_as_bytes.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
d_as_bytes.restype = ctypes.POINTER(ctypes.c_uint8)
with VitalErrorHandle() as eh:
cptr = d_as_bytes(self, eh)
return CArrayWrapper(cptr, self.nbytes, ctypes.c_uint8, free_void_ptr)
def from_string(cls, s):
"""
:param s: String camera representation. This must be in the same form
that would be generated by this class' ``as_string`` method.
:type s: str
:return: New camera instance from a string sequence.
:rtype: Camera
"""
cam_from_str = cls.VITAL_LIB['vital_camera_new_from_string']
cam_from_str.argtypes = [
ctypes.c_char_p, VitalErrorHandle.c_ptr_type()
]
cam_from_str.restype = cls.c_ptr_type()
with VitalErrorHandle() as eh:
cptr = cam_from_str(s, eh)
return Camera(from_cptr=cptr)
def _call_cfunc(cls,
func_name,
argtypes=(),
args=(),
restype=None,
exception_map=None):
"""
Extract function from vital library and call it with a VitalErrorHandle.
This assumes that the C function takes an additional parameter than what
is given to this function that is the error handle instance.
This function may raise an exception when the C function call's error
handle picked something up. If an exception map is provided, it should
be a dictionary mapping integer error codes to the python exception
type that should be thrown.
:param func_name: C function name to pull from library
:type func_name: str
:param argtypes: Ctypes argument type array
:type argtypes: list | tuple
:param args: sequence of positional arguments to the C function
:type args: list | tuple
:param restype: optional Ctypes return type
:param exception_map: Return code to exception mapping to give to the
error handler.
:type exception_map: dict[int, BaseException | types.FunctionType]
:return: Result of the c function call
"""
# local to prevent circular import
from vital.util import VitalErrorHandle
f = cls.VITAL_LIB[func_name]
if argtypes:
f.argtypes = list(argtypes) + [VitalErrorHandle.c_ptr_type()]
f.restype = restype
with VitalErrorHandle() as eh:
if exception_map:
eh.set_exception_map(exception_map)
return f(*(list(args) + [eh]))
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 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 as_string(self):
"""
Convert the camera to a string representation
:return: String representation of this camera
:rtype: str
"""
cam_tostr = self.VITAL_LIB['vital_camera_to_string']
cam_tostr.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_tostr.restype = ctypes.c_void_p
with VitalErrorHandle() as eh:
v_ptr = cam_tostr(self, eh)
s = ctypes.c_char_p(v_ptr).value
ptr_free = self.VITAL_LIB['vital_free_pointer']
ptr_free.argtypes = [ctypes.c_void_p]
ptr_free(v_ptr)
return s
def as_string(self):
"""
Convert the camera to a string representation
:return: String representation of this camera
:rtype: str
"""
cam_tostr = self.VITAL_LIB['vital_camera_to_string']
cam_tostr.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
cam_tostr.restype = ctypes.c_void_p
with VitalErrorHandle() as eh:
v_ptr = cam_tostr(self, eh)
s = ctypes.c_char_p(v_ptr).value
ptr_free = self.VITAL_LIB['vital_free_pointer']
ptr_free.argtypes = [ctypes.c_void_p]
ptr_free(v_ptr)
return s
def _call_cfunc(cls, func_name, argtypes=(), args=(), restype=None,
exception_map=None):
"""
Extract function from vital library and call it with a VitalErrorHandle.
This assumes that the C function takes an additional parameter than what
is given to this function that is the error handle instance.
This function may raise an exception when the C function call's error
handle picked something up. If an exception map is provided, it should
be a dictionary mapping integer error codes to the python exception
type that should be thrown.
:param func_name: C function name to pull from library
:type func_name: str
:param argtypes: Ctypes argument type array
:type argtypes: list | tuple
:param args: sequence of positional arguments to the C function
:type args: list | tuple
:param restype: optional Ctypes return type
:param exception_map: Return code to exception mapping to give to the
error handler.
:type exception_map: dict[int, BaseException | types.FunctionType]
:return: Result of the c function call
"""
# local to prevent circular import
from vital.util import VitalErrorHandle
f = cls.VITAL_LIB[func_name]
if argtypes:
f.argtypes = list(argtypes) + [VitalErrorHandle.c_ptr_type()]
f.restype = restype
with VitalErrorHandle() as eh:
if exception_map:
eh.set_exception_map(exception_map)
return f(*(list(args) + [eh]))
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 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 _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 _destroy(self):
if hasattr(self, '_owns_data') and self._owns_data:
d_del = self.VITAL_LIB['vital_descriptor_destroy']
d_del.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
with VitalErrorHandle() as eh:
d_del(self, eh)
def _destroy(self):
if hasattr(self, '_owns_data') and self._owns_data:
d_del = self.VITAL_LIB['vital_descriptor_destroy']
d_del.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
with VitalErrorHandle() as eh:
d_del(self, eh)
