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 _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 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 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 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 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 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 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 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 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 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 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)
