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
: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 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 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 principle_point(self):
f = self.VITAL_LIB['vital_camera_intrinsics_get_principle_point']
f.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
f.restype = EigenArray.c_ptr_type(2, 1, ctypes.c_double)
with VitalErrorHandle() as eh:
m_ptr = f(self, eh)
return EigenArray(2, 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))
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 map_2d(self, norm_pt):
"""
Map normalized image coordinates into actual image coordinates
This function applies both distortion and application of the
calibration matrix to map into actual image coordinates.
:param norm_pt: Normalized image coordinate to map to an image
coordinate (2-element sequence).
:type norm_pt: collections.Sequence[float]
:return: Mapped 2D image coordinate
:rtype: EigenArray[float]
"""
assert len(norm_pt) == 2, "Input sequence was not of length 2"
f = self.VITAL_LIB['vital_camera_intrinsics_map_2d']
f.argtypes = [self.C_TYPE_PTR,
EigenArray.c_ptr_type(2, 1, ctypes.c_double),
VitalErrorHandle.C_TYPE_PTR]
f.restype = EigenArray.c_ptr_type(2, 1, ctypes.c_double)
p = EigenArray(2)
p.T[:] = norm_pt
with VitalErrorHandle() as eh:
m_ptr = f(self, p, eh)
return EigenArray(2, 1, from_cptr=m_ptr, owns_data=True)
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 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_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_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 dist_coeffs(self):
""" Get the distortion coefficients array """
f = self.VITAL_LIB['vital_camera_intrinsics_get_dist_coeffs']
f.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
f.restype = EigenArray.c_ptr_type('X', 1, ctypes.c_double)
with VitalErrorHandle() as eh:
m_ptr = f(self, eh)
return EigenArray(dynamic_rows=1, from_cptr=m_ptr, owns_data=True)
def axis(self):
"""
:return: This rotation's axis of rotation.
:rtype: (vital.types.EigenArray, float)
"""
r2axis = self._get_c_function(self._spec, 'axis')
r2axis.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
r2axis.restype = EigenArray.c_ptr_type(3, 1, self._ctype)
with VitalErrorHandle() as eh:
mat_ptr = r2axis(self, eh)
return EigenArray(3, dtype=numpy.dtype(self._ctype),
from_cptr=mat_ptr, owns_data=True)
def rodrigues(self):
"""
:return: This rotation as a Rodrigues vector.
:rtype: vital.types.EigenArray
"""
r2rod = self._get_c_function(self._spec, "rodrigues")
r2rod.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
r2rod.restype = EigenArray.c_ptr_type(3, 1, self._ctype)
with VitalErrorHandle() as eh:
rod_ptr = r2rod(self, eh)
return EigenArray(3, dtype=numpy.dtype(self._ctype),
from_cptr=rod_ptr, owns_data=True)
def quaternion(self):
"""
:return: this rotation as a new quaternion (4x1 matrix).
:rtype: vital.types.EigenArray
"""
r_to_q = self._get_c_function(self._spec, 'quaternion')
r_to_q.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
r_to_q.restype = EigenArray.c_ptr_type(4, 1, self._ctype)
with VitalErrorHandle() as eh:
mat_ptr = r_to_q(self, eh)
return EigenArray(4, dtype=numpy.dtype(self._ctype),
from_cptr=mat_ptr, owns_data=True)
def matrix(self):
"""
:return: this rotation as a new 3x3 matrix.
:rtype: vital.types.EigenArray
"""
r_to_mat = self._get_c_function(self._spec, "to_matrix")
r_to_mat.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
r_to_mat.restype = EigenArray.c_ptr_type(3, 3, self._ctype)
with VitalErrorHandle() as eh:
mat_ptr = r_to_mat(self, eh)
return EigenArray(3, 3, dtype=numpy.dtype(self._ctype),
from_cptr=mat_ptr, owns_data=True)
def to_matrix(self):
c_to_mat = self._func_map['to_matrix']
c_to_mat.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
c_to_mat.restype = EigenArray.c_ptr_type(self._N, self._N, self._ctype)
with VitalErrorHandle() as eh:
m_ptr = c_to_mat(self, eh)
return EigenArray(self._N,
self._N,
dtype=numpy.dtype(self._ctype),
from_cptr=m_ptr,
owns_data=True)
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(2)
pp.T[:] = principle_point
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 principle_point(self):
f = self.VITAL_LIB['vital_camera_intrinsics_get_principle_point']
f.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
f.restype = EigenArray.c_ptr_type(2, 1, ctypes.c_double)
with VitalErrorHandle() as eh:
m_ptr = f(self, eh)
return EigenArray(2, from_cptr=m_ptr, owns_data=True)
def as_matrix(self):
"""
Access the intrinsics as an upper triangular matrix
**Note:** *This matrix includes the focal length, principal point,
aspect ratio, and skew, but does not model distortion.*
:return: 3x3 upper triangular matrix
"""
f = self.VITAL_LIB['vital_camera_intrinsics_as_matrix']
f.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
f.restype = EigenArray.c_ptr_type(3, 3, ctypes.c_double)
with VitalErrorHandle() as eh:
m_ptr = f(self, eh)
return EigenArray(3, 3, from_cptr=m_ptr, owns_data=True)
def dist_coeffs(self):
""" Get the distortion coefficients array """
f = self.VITAL_LIB['vital_camera_intrinsics_get_dist_coeffs']
f.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
f.restype = EigenArray.c_ptr_type('X', 1, ctypes.c_double)
with VitalErrorHandle() as eh:
m_ptr = f(self, eh)
return EigenArray(dynamic_rows=1, from_cptr=m_ptr, owns_data=True)
def to_matrix(self):
c_to_mat = self._func_map['to_matrix']
c_to_mat.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
c_to_mat.restype = EigenArray.c_ptr_type(self._N, self._N, self._ctype)
with VitalErrorHandle() as eh:
m_ptr = c_to_mat(self, eh)
return EigenArray(self._N, self._N, dtype=numpy.dtype(self._ctype),
from_cptr=m_ptr, owns_data=True)
def undistort_2d(self, dist_pt):
"""
Unmap distorted normalized coordinates into normalized coordinates
:param dist_pt: Distorted 2D coordinate to un-distort.
:return: Normalized 2D image coordinate.
"""
assert len(dist_pt) == 2, "Input sequence was not of length 2"
f = self.VITAL_LIB['vital_camera_intrinsics_undistort_2d']
f.argtypes = [self.C_TYPE_PTR,
EigenArray.c_ptr_type(2, 1, ctypes.c_double),
VitalErrorHandle.C_TYPE_PTR]
f.restype = EigenArray.c_ptr_type(2, 1, ctypes.c_double)
p = EigenArray(2)
p.T[:] = dist_pt
with VitalErrorHandle() as eh:
m_ptr = f(self, p, eh)
return EigenArray(2, 1, from_cptr=m_ptr, owns_data=True)
def axis(self):
"""
:return: This rotation's axis and angle.
:rtype: (vital.types.EigenArray, float)
"""
r2axis = self._get_c_function(self._spec, 'axis')
r2axis.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
r2axis.restype = EigenArray.c_ptr_type(3, 1, self._ctype)
with VitalErrorHandle() as eh:
mat_ptr = r2axis(self, eh)
return EigenArray(3, dtype=numpy.dtype(self._ctype),
from_cptr=mat_ptr, owns_data=True)
def quaternion(self):
"""
:return: this rotation as a new quaternion (4x1 matrix).
:rtype: vital.types.EigenArray
"""
r_to_q = self._get_c_function(self._spec, 'quaternion')
r_to_q.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
r_to_q.restype = EigenArray.c_ptr_type(4, 1, self._ctype)
with VitalErrorHandle() as eh:
mat_ptr = r_to_q(self, eh)
return EigenArray(4, dtype=numpy.dtype(self._ctype),
from_cptr=mat_ptr, owns_data=True)
def matrix(self):
"""
:return: this rotation as a new 3x3 matrix.
:rtype: vital.types.EigenArray
"""
r_to_mat = self._get_c_function(self._spec, "to_matrix")
r_to_mat.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
r_to_mat.restype = EigenArray.c_ptr_type(3, 3, self._ctype)
with VitalErrorHandle() as eh:
mat_ptr = r_to_mat(self, eh)
return EigenArray(3, 3, dtype=numpy.dtype(self._ctype),
from_cptr=mat_ptr, owns_data=True)
def rodrigues(self):
"""
:return: This rotation as a Rodrigues vector
:rtype: vital.types.EigenArray
"""
r2rod = self._get_c_function(self._spec, "rodrigues")
r2rod.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
r2rod.restype = EigenArray.c_ptr_type(3, 1, self._ctype)
with VitalErrorHandle() as eh:
rod_ptr = r2rod(self, eh)
return EigenArray(3, dtype=numpy.dtype(self._ctype),
from_cptr=rod_ptr, owns_data=True)
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(2)
pp.T[:] = principle_point
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 map_2d(self, norm_pt):
"""
Map normalized image coordinates into actual image coordinates
This function applies both distortion and application of the
calibration matrix to map into actual image coordinates.
:param norm_pt: Normalized image coordinate to map to an image
coordinate (2-element sequence).
:type norm_pt: collections.Sequence[float]
:return: Mapped 2D image coordinate
:rtype: EigenArray[float]
"""
assert len(norm_pt) == 2, "Input sequence was not of length 2"
f = self.VITAL_LIB['vital_camera_intrinsics_map_2d']
f.argtypes = [self.C_TYPE_PTR,
EigenArray.c_ptr_type(2, 1, ctypes.c_double),
VitalErrorHandle.C_TYPE_PTR]
f.restype = EigenArray.c_ptr_type(2, 1, ctypes.c_double)
p = EigenArray(2)
p.T[:] = norm_pt
with VitalErrorHandle() as eh:
m_ptr = f(self, p, eh)
return EigenArray(2, 1, from_cptr=m_ptr, owns_data=True)
def map_3d(self, norm_hpt):
"""
Map a 3D point in camera coordinates into actual image coordinates
:param norm_hpt: Normalized coordinate to map to an image coordinate
(3-element sequence)
:type norm_hpt: collections.Sequence[float]
:return: Mapped 2D image coordinate
:rtype: EigenArray[float]
"""
assert len(norm_hpt) == 3, "Input sequence was not of length 3"
f = self.VITAL_LIB['vital_camera_intrinsics_map_3d']
f.argtypes = [
self.C_TYPE_PTR,
EigenArray.c_ptr_type(3, 1, ctypes.c_double),
VitalErrorHandle.C_TYPE_PTR
]
f.restype = EigenArray.c_ptr_type(2, 1, ctypes.c_double)
p = EigenArray(3)
p.T[:] = norm_hpt
with VitalErrorHandle() as eh:
m_ptr = f(self, p, eh)
return EigenArray(2, 1, from_cptr=m_ptr, owns_data=True)
def unmap_2d(self, pt):
"""
Unmap actual image coordinates back into normalized image coordinates
This function applies both application of the inverse calibration matrix
and undistortion of the normalized coordinates
:param pt: Actual image 2D point to un-map.
:return: Un-mapped normalized image coordinate.
"""
assert len(pt) == 2, "Input sequence was not of length 2"
f = self.VITAL_LIB['vital_camera_intrinsics_unmap_2d']
f.argtypes = [
self.C_TYPE_PTR,
EigenArray.c_ptr_type(2, 1, ctypes.c_double),
VitalErrorHandle.C_TYPE_PTR
]
f.restype = EigenArray.c_ptr_type(2, 1, ctypes.c_double)
p = EigenArray(2)
p.T[:] = pt
with VitalErrorHandle() as eh:
m_ptr = f(self, p, eh)
return EigenArray(2, 1, from_cptr=m_ptr, owns_data=True)
def map_3d(self, norm_hpt):
"""
Map a 3D point in camera coordinates into actual image coordinates
:param norm_hpt: Normalized coordinate to map to an image coordinate
(3-element sequence)
:type norm_hpt: collections.Sequence[float]
:return: Mapped 2D image coordinate
:rtype: EigenArray[float]
"""
assert len(norm_hpt) == 3, "Input sequence was not of length 3"
f = self.VITAL_LIB['vital_camera_intrinsics_map_3d']
f.argtypes = [self.C_TYPE_PTR,
EigenArray.c_ptr_type(3, 1, ctypes.c_double),
VitalErrorHandle.C_TYPE_PTR]
f.restype = EigenArray.c_ptr_type(2, 1, ctypes.c_double)
p = EigenArray(3)
p.T[:] = norm_hpt
with VitalErrorHandle() as eh:
m_ptr = f(self, p, eh)
return EigenArray(2, 1, from_cptr=m_ptr, owns_data=True)
def unmap_2d(self, pt):
"""
Unmap actual image coordinates back into normalized image coordinates
This function applies both application of the inverse calibration matrix
and undistortion of the normalized coordinates
:param pt: Actual image 2D point to un-map.
:return: Un-mapped normalized image coordinate.
"""
assert len(pt) == 2, "Input sequence was not of length 2"
f = self.VITAL_LIB['vital_camera_intrinsics_unmap_2d']
f.argtypes = [self.C_TYPE_PTR,
EigenArray.c_ptr_type(2, 1, ctypes.c_double),
VitalErrorHandle.C_TYPE_PTR]
f.restype = EigenArray.c_ptr_type(2, 1, ctypes.c_double)
p = EigenArray(2)
p.T[:] = pt
with VitalErrorHandle() as eh:
m_ptr = f(self, p, eh)
return EigenArray(2, 1, from_cptr=m_ptr, owns_data=True)
def as_matrix(self):
"""
Access the intrinsics as an upper triangular matrix
**Note:** *This matrix includes the focal length, principal point,
aspect ratio, and skew, but does not model distortion.*
:return: 3x3 upper triangular matrix
"""
f = self.VITAL_LIB['vital_camera_intrinsics_as_matrix']
f.argtypes = [self.C_TYPE_PTR, VitalErrorHandle.C_TYPE_PTR]
f.restype = EigenArray.c_ptr_type(3, 3, ctypes.c_double)
with VitalErrorHandle() as eh:
m_ptr = f(self, eh)
return EigenArray(3, 3, from_cptr=m_ptr, owns_data=True)
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
def undistort_2d(self, dist_pt):
"""
Unmap distorted normalized coordinates into normalized coordinates
:param dist_pt: Distorted 2D coordinate to un-distort.
:return: Normalized 2D image coordinate.
"""
assert len(dist_pt) == 2, "Input sequence was not of length 2"
f = self.VITAL_LIB['vital_camera_intrinsics_undistort_2d']
f.argtypes = [self.C_TYPE_PTR,
EigenArray.c_ptr_type(2, 1, ctypes.c_double),
VitalErrorHandle.C_TYPE_PTR]
f.restype = EigenArray.c_ptr_type(2, 1, ctypes.c_double)
p = EigenArray(2)
p.T[:] = dist_pt
with VitalErrorHandle() as eh:
m_ptr = f(self, p, eh)
return EigenArray(2, 1, from_cptr=m_ptr, owns_data=True)
