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