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)