Exemplo n.º 1
0
    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)
Exemplo n.º 2
0
    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)
Exemplo n.º 3
0
    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)
Exemplo n.º 4
0
    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)
Exemplo n.º 5
0
 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)
Exemplo n.º 6
0
    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)
Exemplo n.º 7
0
    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)
Exemplo n.º 8
0
    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)
Exemplo n.º 9
0
    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)
Exemplo n.º 10
0
    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)
Exemplo n.º 11
0
    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)
Exemplo n.º 12
0
    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)
Exemplo n.º 13
0
 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)
Exemplo n.º 14
0
 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)
Exemplo n.º 15
0
 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)
Exemplo n.º 16
0
 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)
Exemplo n.º 17
0
 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)
Exemplo n.º 18
0
    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)
Exemplo n.º 19
0
    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)
Exemplo n.º 20
0
 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)
Exemplo n.º 21
0
    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)
Exemplo n.º 22
0
 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)
Exemplo n.º 23
0
    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)
Exemplo n.º 24
0
    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)
Exemplo n.º 25
0
 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)
Exemplo n.º 26
0
 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)
Exemplo n.º 27
0
 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)
Exemplo n.º 28
0
 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)
Exemplo n.º 29
0
    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)
Exemplo n.º 30
0
    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)
Exemplo n.º 31
0
    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)
Exemplo n.º 32
0
    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)
Exemplo n.º 33
0
    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)
Exemplo n.º 34
0
    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)
Exemplo n.º 35
0
    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)
Exemplo n.º 36
0
    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
Exemplo n.º 37
0
    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
Exemplo n.º 38
0
    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)