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