def _new(self, focal_length, principle_point, aspect_ratio, skew,
dist_coeffs):
"""
Construct a new vital::camera_intrinsics instance
:type focal_length: float
:type principle_point: collections.Sequence[float]
:type aspect_ratio: float
:type skew: float
:type dist_coeffs: collections.Sequence[float]
"""
ci_new = self.VITAL_LIB['vital_camera_intrinsics_new']
ci_new.argtypes = [
ctypes.c_double,
EigenArray.c_ptr_type(2, 1, ctypes.c_double),
ctypes.c_double,
ctypes.c_double,
EigenArray.c_ptr_type('X', 1, ctypes.c_double),
VitalErrorHandle.C_TYPE_PTR,
]
ci_new.restype = self.C_TYPE_PTR
# Make "vectors"
pp = EigenArray.from_iterable(principle_point, target_shape=(2, 1))
dc = EigenArray(len(dist_coeffs), dynamic_rows=True)
if len(dist_coeffs):
dc.T[:] = dist_coeffs
with VitalErrorHandle() as eh:
return ci_new(focal_length, pp, aspect_ratio, skew, dc, eh)
def rotate_vector(self, vec):
"""
Rotate a given 3x1 vector about this rotation, returning a new 3x1
vector.
Returned vector will have the same data type as this rotation.
:param vec: 3x1 array-like to rotate
:type vec: collections.Iterable
:return: New 3x1 rotated vector
:rtype: vital.types.EigenArray
:raises ValueError: The input array-like data did not conform the
expected 3x1 shape (column vector).
"""
vec = EigenArray.from_iterable(vec, self._ctype, (3, 1))
# make EigenArray out of input array if its not already
r_rv = self._get_c_function(self._spec, "rotate_vector")
r_rv.argtypes = [self.C_TYPE_PTR, vec.C_TYPE_PTR,
VitalErrorHandle.C_TYPE_PTR]
r_rv.restype = EigenArray.c_ptr_type(3, 1, self._ctype)
with VitalErrorHandle() as eh:
m_ptr = r_rv(self, vec, eh)
return EigenArray(3, dtype=numpy.dtype(self._ctype), from_cptr=m_ptr,
owns_data=True)
def from_quaternion(cls, q_vec, ctype=ctypes.c_double):
"""
Create rotation based on the given 4x1 (column-vector) quaternion
representation whose format, `[x, y, z, w]`, represents the
`w+xi+yj+zk` formula (see Eigen's `Quaternion` class).
Input data is copied.
:param q_vec: Quaternion column-vector array-like to initialize to.
:type q_vec: collections.Iterable
:param ctype: C data type to store rotation data in.
:type ctype: _ctypes._SimpleCData
:return: New rotation instance with the initialized rotation.
:rtype: vital.types.Rotation
:raises ValueError: The input array-like data did not conform to the
specified target shape.
"""
q_vec = EigenArray.from_iterable(q_vec, ctype, (4, 1))
q_vec /= q_vec.norm()
s = cls._gen_spec(ctype)
r_from_q = cls._get_c_function(s, 'new_from_quaternion')
r_from_q.argtypes = [EigenArray.c_ptr_type(4, 1, ctype),
VitalErrorHandle.C_TYPE_PTR]
r_from_q.restype = cls.C_TYPE_PTR[cls.TYPE_SPEC.format(type=s)]
with VitalErrorHandle() as eh:
r_ptr = r_from_q(q_vec, eh)
return Rotation(ctype, r_ptr)
def from_axis_angle(cls, axis, angle, ctype=ctypes.c_double):
"""
Create rotation based on the given angle and axis vector (3x1
column-vector). The axis vector will be normalized .
:param axis: Axis column vector (3x1)
:type axis: collections.Iterable
:param angle: Angle of rotation about axis (radians)
:type angle: float
:param ctype: C data type to store rotation data in.
:type ctype: _ctypes._SimpleCData
:return: New rotation instance with the initialized rotation.
:rtype: vital.types.Rotation
:raises ValueError: The input array-like data did not conform to the
specified target shape.
"""
axis = EigenArray.from_iterable(axis, ctype, (3, 1))
s = cls._gen_spec(ctype)
r_from_aa = cls._get_c_function(s, 'new_from_axis_angle')
r_from_aa.argtypes = [ctype,
EigenArray.c_ptr_type(3, 1, ctype),
VitalErrorHandle.C_TYPE_PTR]
r_from_aa.restype = Rotation.C_TYPE_PTR[s]
with VitalErrorHandle() as eh:
r_ptr = r_from_aa(angle, axis, eh)
return Rotation(ctype, r_ptr)
def from_matrix(cls, mat, ctype=ctypes.c_double):
"""
Create rotation based on the given 3x3 rotation matrix.
:param mat: Input rotation matrix.
:type mat: collections.Iterable
:param ctype: C data type to store rotation data in.
:type ctype: _ctypes._SimpleCData
:return: New rotation instance with the initialized rotation
:rtype: vital.types.Rotation
:raises ValueError: The input array-like data did not conform to the
specified target shape.
"""
mat = EigenArray.from_iterable(mat, ctype, (3, 3))
s = cls._gen_spec(ctype)
r_from_mat = cls._get_c_function(s, 'new_from_matrix')
r_from_mat.argtypes = [EigenArray.c_ptr_type(3, 3, ctype),
VitalErrorHandle.C_TYPE_PTR]
r_from_mat.restype = cls.C_TYPE_PTR[s]
with VitalErrorHandle() as eh:
r_ptr = r_from_mat(mat, eh)
return Rotation(ctype, r_ptr)
def from_quaternion(cls, q_vec, ctype=ctypes.c_double):
"""
Create rotation based on the given 4x1 (column-vector) quaternion
representation whose format, `[x, y, z, w]`, represents the `w+xi+yj+zk`
formula (see Eigen's `Quaternion` class).
Input data is copied.
:param q_vec: Quaternion column-vector array-like to initialize to.
:type q_vec: collections.Iterable
:param ctype: C data type to store rotation data in.
:type ctype: _ctypes._SimpleCData
:return: New rotation instance with the initialized rotation.
:rtype: vital.types.Rotation
:raises ValueError: The input array-like data did not conform to the
specified target shape.
"""
q_vec = EigenArray.from_iterable(q_vec, ctype, (4, 1))
s = cls._gen_spec(ctype)
r_from_q = cls._get_c_function(s, 'new_from_quaternion')
r_from_q.argtypes = [EigenArray.c_ptr_type(4, 1, ctype),
VitalErrorHandle.C_TYPE_PTR]
r_from_q.restype = cls.C_TYPE_PTR[cls.TYPE_SPEC.format(type=s)]
with VitalErrorHandle() as eh:
r_ptr = r_from_q(q_vec, eh)
return Rotation(ctype, r_ptr)
def from_rodrigues(cls, r_vec, ctype=ctypes.c_double):
"""
Create rotation based on the given 3x1 (column-vector) rodrigues
representation.
:param r_vec: Rodrigues 3x1 column-vector
:type r_vec: collections.Iterable
:param ctype: C data type to store rotation data in.
:type ctype: _ctypes._SimpleCData
:return: New rotation instance with the initialized rotation
:rtype: vital.types.Rotation
:raises ValueError: The input array-like data did not conform to the
specified target shape.
"""
r_vec = EigenArray.from_iterable(r_vec, ctype, (3, 1))
s = cls._gen_spec(ctype)
r_from_rod = cls._get_c_function(s, 'new_from_rodrigues')
r_from_rod.argtypes = [EigenArray.c_ptr_type(3, 1, ctype),
VitalErrorHandle.C_TYPE_PTR]
r_from_rod.restype = cls.C_TYPE_PTR[s]
with VitalErrorHandle() as eh:
r_ptr = r_from_rod(r_vec, eh)
return Rotation(ctype, r_ptr)
def rotate_vector(self, vec):
"""
Rotate a given 3x1 vector about this rotation, returning a new 3x1
vector.
Returned vector will have the same data type as this rotation.
:param vec: 3x1 array-like to rotate
:type vec: collections.Iterable
:return: New 3x1 rotated vector
:rtype: vital.types.EigenArray
:raises ValueError: The input array-like data did not conform the
expected 3x1 shape (column vector).
"""
vec = EigenArray.from_iterable(vec, self._ctype, (3, 1))
# make EigenArray out of input array if its not already
r_rv = self._get_c_function(self._spec, "rotate_vector")
r_rv.argtypes = [self.C_TYPE_PTR, vec.C_TYPE_PTR,
VitalErrorHandle.C_TYPE_PTR]
r_rv.restype = EigenArray.c_ptr_type(3, 1, self._ctype)
with VitalErrorHandle() as eh:
m_ptr = r_rv(self, vec, eh)
return EigenArray(3, dtype=numpy.dtype(self._ctype), from_cptr=m_ptr,
owns_data=True)
def from_matrix(cls, mat, ctype=ctypes.c_double):
"""
Create rotation based on the given 3x3 rotation matrix.
:param mat: Input rotation matrix.
:type mat: collections.Iterable
:param ctype: C data type to store rotation data in.
:type ctype: _ctypes._SimpleCData
:return: New rotation instance with the initialized rotation
:rtype: vital.types.Rotation
:raises ValueError: The input array-like data did not conform to the
specified target shape.
"""
mat = EigenArray.from_iterable(mat, ctype, (3, 3))
s = cls._gen_spec(ctype)
r_from_mat = cls._get_c_function(s, 'new_from_matrix')
r_from_mat.argtypes = [EigenArray.c_ptr_type(3, 3, ctype),
VitalErrorHandle.C_TYPE_PTR]
r_from_mat.restype = cls.C_TYPE_PTR[s]
with VitalErrorHandle() as eh:
r_ptr = r_from_mat(mat, eh)
return Rotation(ctype, r_ptr)
def from_axis_angle(cls, axis, angle, ctype=ctypes.c_double):
"""
Create rotation based on the given angle and axis vector (3x1
column-vector). The axis vector will be normalized .
:param axis: Axis column vector (3x1)
:type axis: collections.Iterable
:param angle: Angle of rotation about axis
:type angle: float
:param ctype: C data type to store rotation data in.
:type ctype: _ctypes._SimpleCData
:return: New rotation instance with the initialized rotation.
:rtype: vital.types.Rotation
:raises ValueError: The input array-like data did not conform to the
specified target shape.
"""
axis = EigenArray.from_iterable(axis, ctype, (3, 1))
s = cls._gen_spec(ctype)
r_from_aa = cls._get_c_function(s, 'new_from_axis_angle')
r_from_aa.argtypes = [ctype,
EigenArray.c_ptr_type(3, 1, ctype),
VitalErrorHandle.C_TYPE_PTR]
r_from_aa.restype = Rotation.C_TYPE_PTR[s]
with VitalErrorHandle() as eh:
r_ptr = r_from_aa(angle, axis, eh)
return Rotation(ctype, r_ptr)
def from_rodrigues(cls, r_vec, ctype=ctypes.c_double):
"""
Create rotation based on the given 3x1 (column-vector) rodrigues
representation.
:param r_vec: Rodrigues 3x1 column-vector
:type r_vec: collections.Iterable
:param ctype: C data type to store rotation data in.
:type ctype: _ctypes._SimpleCData
:return: New rotation instance with the initialized rotation
:rtype: vital.types.Rotation
:raises ValueError: The input array-like data did not conform to the
specified target shape.
"""
r_vec = EigenArray.from_iterable(r_vec, ctype, (3, 1))
s = cls._gen_spec(ctype)
r_from_rod = cls._get_c_function(s, 'new_from_rodrigues')
r_from_rod.argtypes = [EigenArray.c_ptr_type(3, 1, ctype),
VitalErrorHandle.C_TYPE_PTR]
r_from_rod.restype = cls.C_TYPE_PTR[s]
with VitalErrorHandle() as eh:
r_ptr = r_from_rod(r_vec, eh)
return Rotation(ctype, r_ptr)
def _new(self, init_scalar_or_matrix):
"""
Construct a new instance, returning new instance opaque C pointer and
initializing any other necessary object properties
:returns: New C opaque structure pointer.
"""
N = self._N
c_type = self._ctype
# Choose relevant constructor
if init_scalar_or_matrix is None:
self._log.debug("Initializing identity")
c_new = self._func_map['new_identity']
c_new.argtypes = [VitalErrorHandle.C_TYPE_PTR]
c_new.restype = self.C_TYPE_PTR
args = ()
elif isinstance(init_scalar_or_matrix, (collections.Iterable,
numpy.ndarray)):
self._log.debug("Initializing with matrix")
# Should be a NxN square matrix
mat = EigenArray.from_iterable(init_scalar_or_matrix, c_type,
(N, N))
c_new = self._func_map['new_matrix']
c_new.argtypes = [mat.C_TYPE_PTR,
VitalErrorHandle.C_TYPE_PTR]
args = (mat,)
else:
self._log.debug("Initializing with scalar")
c_new = self._func_map['new_scalar']
c_new.argtypes = [self._ctype, VitalErrorHandle.C_TYPE_PTR]
args = (init_scalar_or_matrix,)
c_new.restype = self.C_TYPE_PTR
with VitalErrorHandle() as eh:
c_args = args + (eh,)
# self._log.debug("Construction args: %s", c_args)
c_ptr = c_new(*c_args)
if not bool(c_ptr):
raise RuntimeError("C++ Construction failed (null pointer)")
return c_ptr
def _new(self, init_scalar_or_matrix):
"""
Construct a new instance, returning new instance opaque C pointer and
initializing any other necessary object properties
:returns: New C opaque structure pointer.
"""
N = self._N
c_type = self._ctype
# Choose relevant constructor
if init_scalar_or_matrix is None:
self._log.debug("Initializing identity")
c_new = self._func_map['new_identity']
c_new.argtypes = [VitalErrorHandle.C_TYPE_PTR]
c_new.restype = self.C_TYPE_PTR
args = ()
elif isinstance(init_scalar_or_matrix,
(collections.Iterable, numpy.ndarray)):
self._log.debug("Initializing with matrix")
# Should be a NxN square matrix
mat = EigenArray.from_iterable(init_scalar_or_matrix, c_type,
(N, N))
c_new = self._func_map['new_matrix']
c_new.argtypes = [mat.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
args = (mat, )
else:
self._log.debug("Initializing with scalar")
c_new = self._func_map['new_scalar']
c_new.argtypes = [self._ctype, VitalErrorHandle.C_TYPE_PTR]
args = (init_scalar_or_matrix, )
c_new.restype = self.C_TYPE_PTR
with VitalErrorHandle() as eh:
c_args = args + (eh, )
# self._log.debug("Construction args: %s", c_args)
c_ptr = c_new(*c_args)
if not bool(c_ptr):
raise RuntimeError("C++ Construction failed (null pointer)")
return c_ptr
