Пример #1
0
    def map(self, point):
        """
        Map a 3D point using this homography

        If the data type of the point given, if an EigenArray, is not the same
        as this homography's ``datatype``, it will be automatically converted.

        :param point: 2D point to transform
        :type point: collections.Iterable[float] | EigenArray

        :return: Transformed 2D point as an EigenArray
        :rtype: EigenArray

        """
        point_t = EigenArray.c_ptr_type(2, ctype=self._datatype)
        point = EigenArray.from_iterable(point, self._datatype, (2, 1))
        p_cptr = self._call_cfunc(
            'vital_homography_%s_map_point' % self._tchar,
            [self.C_TYPE_PTR, point_t],
            [self, point],
            point_t,
            {
                1: PointMapsToInfinityException
            }
        )
        return EigenArray(2, dtype=self._datatype, from_cptr=p_cptr)
Пример #2
0
 def normal(self, n):
     n = EigenArray.from_iterable(n, self._datatype, (3, 1))
     self._call_cfunc(
         'vital_landmark_{}_set_normal'.format(self._tchar),
         [self.C_TYPE_PTR,
          EigenArray.c_ptr_type(3, ctype=self._datatype)], [self, n],
         exception_map={1: VitalDynamicCastException})
Пример #3
0
    def from_matrix(cls, m, datatype=ctypes.c_double):
        """
        Create a homography from an existing 3x3 matrix.

        If the data type of the matrix given is not the same as ``datatype``,
        it will be automatically converted.

        :param m: Matrix to base the new homography on. This should be a 3x3
            matrix.
        :type m: collections.Iterable[collections.Iterable[float]] | vital.types.EigenArray

        :param datatype: Type to store data in the homography.
        :type datatype: ctypes._SimpleCData

        :return: New homography instance whose transform is equal to the given
            matrix.
        :rtype: Homography

        """
        # noinspection PyProtectedMember
        tchar = datatype._type_
        m = EigenArray.from_iterable(m, datatype, (3, 3))
        cptr = cls._call_cfunc(
            'vital_homography_%s_new_from_matrix' % tchar,
            [EigenArray.c_ptr_type(3, 3, datatype)], [m],
            Homography.c_ptr_type()
        )
        return Homography(from_cptr=cptr)
Пример #4
0
    def _new(self, center, rotation, intrinsics):
        cam_new = self.VITAL_LIB['vital_camera_new']
        cam_new.argtypes = [
            EigenArray.c_ptr_type(3, 1, ctypes.c_double),
            Rotation.c_ptr_type(ctypes.c_double),
            CameraIntrinsics.c_ptr_type(),
            VitalErrorHandle.c_ptr_type()
        ]
        cam_new.restype = self.c_ptr_type()

        # Fill in parameter gaps
        if center is None:
            center = EigenArray(3)
            center[:] = 0
        else:
            center = EigenArray.from_iterable(center)

        if rotation is None:
            rotation = Rotation()

        if intrinsics is None:
            intrinsics = CameraIntrinsics()

        with VitalErrorHandle() as eh:
            return cam_new(center, rotation, intrinsics, eh)
Пример #5
0
 def test_center_initialized(self):
     expected_c = EigenArray(3)
     expected_c.get_matrix()[:] = [[1], [2], [3]]
     cam = Camera(expected_c)
     numpy.testing.assert_array_equal(
         numpy.array([cam.center]).T, expected_c.get_matrix())
     numpy.testing.assert_array_equal(cam.center, [1, 2, 3])
Пример #6
0
    def clone_look_at(self, stare_point, up_direction=(0, 0, 1)):
        """
        Create a clone of this camera that is rotated to look at the given point

        The camera should also be rotated about its principal axis such that
        the vertical image direction is closest to \c up_direction in the world.

        :param stare_point: the location at which the camera is oriented to
            point
        :type stare_point: collections.Iterable | numpy.ndarray | EigenArray

        :param up_direction: the vector which is "up" in the world (defaults to
            Z-axis)
        :type up_direction: collections.Iterable | numpy.ndarray | EigenArray

        :return: New camera instance that is the clone of the one given, but set
            to look at the given point.
        :rtype: Camera

        """
        stare_point = EigenArray.from_iterable(stare_point,
                                               target_shape=(3, 1))
        up_direction = EigenArray.from_iterable(up_direction,
                                                target_shape=(3, 1))
        cptr = self._call_cfunc('vital_camera_clone_look_at', [
            self.C_TYPE_PTR,
            EigenArray.c_ptr_type(3),
            EigenArray.c_ptr_type(3)
        ], [self, stare_point, up_direction], self.C_TYPE_PTR)
        return Camera(from_cptr=cptr)
Пример #7
0
    def test_point_map(self):
        h_f = Homography('f')
        h_d = Homography('d')

        p_af = EigenArray.from_array([[2.2, 3.3]], 'f')
        p_f = p_af.get_matrix()[0]
        p_ad = EigenArray.from_array([[5.5, 6.6]], 'd')
        p_d = p_ad.get_matrix()[0]

        # float-float
        numpy.testing.assert_almost_equal(
            h_f.map(p_f), p_f
        )
        # float-double
        numpy.testing.assert_almost_equal(
            h_f.map(p_d), p_d
        )
        # double-float
        numpy.testing.assert_almost_equal(
            h_d.map(p_f), p_f
        )
        # double-double
        numpy.testing.assert_almost_equal(
            h_d.map(p_d), p_d
        )

        # Code to generate truth
        h = numpy.random.rand(3,3)
        h = h/numpy.linalg.norm(h)
        p0 = numpy.random.rand(3); p0[2] = 1
        p1 = numpy.dot(h, p0)
        p1 = p1[:2]/p1[2]
        h_d = Homography.from_matrix(h, 'd')

        # map from Numpy array.
        numpy.testing.assert_almost_equal(
            h_d.map(p0[:2]).ravel(), p1
        )

        # map from EigenArray
        p0 = EigenArray.from_array([p0[:2]])
        numpy.testing.assert_almost_equal(
            h_d.map(p0.get_matrix()[0]).ravel(), p1
        )

        # Another explicit case.
        p0 = numpy.array([1923.47,645.676,1])
        h = numpy.array([[5.491496261770000276e-01,-1.125428185150000038e-01,
                          1.358427031619999923e+02],
                         [-1.429513389049999993e-02	,6.035527375529999849e-01,
                          5.923971959490000216e+01],
                         [-2.042570000000000164e-06,-2.871670000000000197e-07,
                          1]])
        p1 = numpy.dot(h, p0);      p1 = p1[:2]/p1[2]
        H = Homography.from_matrix(h)
        P = EigenArray.from_array([p0[:2]])
        numpy.testing.assert_almost_equal(
            H.map(P.get_matrix()[0]).ravel(), p1
        )
Пример #8
0
    def test_point_map(self):
        h_f = Homography(ctypes.c_float)
        h_d = Homography(ctypes.c_double)

        p_f = EigenArray.from_iterable([2.2, 3.3], ctypes.c_float)
        p_d = EigenArray.from_iterable([5.5, 6.6], ctypes.c_double)

        # float-float
        numpy.testing.assert_almost_equal(
            h_f.map(p_f), p_f
        )
        # float-double
        numpy.testing.assert_almost_equal(
            h_f.map(p_d), p_d
        )
        # double-float
        numpy.testing.assert_almost_equal(
            h_d.map(p_f), p_f
        )
        # double-double
        numpy.testing.assert_almost_equal(
            h_d.map(p_d), p_d
        )
        
        # Code to generate truth
        h = numpy.random.rand(3,3)
        h = h/numpy.linalg.norm(h)
        p0 = numpy.random.rand(3); p0[2] = 1
        p1 = numpy.dot(h, p0)
        p1 = p1[:2]/p1[2]
        h_d = Homography.from_matrix(h, ctypes.c_double)
        
        # map from Numpy array.
        numpy.testing.assert_almost_equal(
            h_d.map(p0[:2]).ravel(), p1
        )
        
        # map from EigenArray
        p0 = EigenArray.from_iterable(p0[:2])
        numpy.testing.assert_almost_equal(
            h_d.map(p0).ravel(), p1
        )
        
        # Another explicit case.
        p0 = numpy.array([1923.47,645.676,1])
        h = numpy.array([[5.491496261770000276e-01,-1.125428185150000038e-01,
                          1.358427031619999923e+02],
                         [-1.429513389049999993e-02	,6.035527375529999849e-01,
                          5.923971959490000216e+01],
                         [-2.042570000000000164e-06,-2.871670000000000197e-07,
                          1]])
        p1 = numpy.dot(h, p0);      p1 = p1[:2]/p1[2]
        H = Homography.from_matrix(h)
        P = EigenArray.from_iterable(p0[:2])
        numpy.testing.assert_almost_equal(
            H.map(P).ravel(), p1
        )
Пример #9
0
 def loc(self):
     """
     Get the 3D location of this landmark
     :return: 3D location of this landmark
     :rtype: EigenArray
     """
     cptr = self._call_cfunc('vital_landmark_loc', [self.C_TYPE_PTR],
                             [self], EigenArray.c_ptr_type(3))
     return EigenArray(3, from_cptr=cptr)
Пример #10
0
 def location(self):
     """ Get feature location """
     cptr = self._call_cfunc(
         'vital_feature_loc',
         [self.C_TYPE_PTR],
         [self],
         EigenArray.c_ptr_type(2),
     )
     return EigenArray(2, from_cptr=cptr)
Пример #11
0
    def test_dynamic_size_init(self):
        a = EigenArray(2, dynamic_rows=True)
        ntools.assert_equal(a.shape, (2, 1))

        a = EigenArray(300, dynamic_rows=True)
        ntools.assert_equal(a.shape, (300, 1))

        a = EigenArray(1234, 256, dynamic_rows=True, dynamic_cols=True)
        ntools.assert_equal(a.shape, (1234, 256))
Пример #12
0
 def translation(self):
     """
     Get the translation of this similarity transformation
     :rtype: EigenArray
     """
     trans_ptr = self._call_cfunc(
         'vital_similarity_%s_translation' % self._tchar, [self.C_TYPE_PTR],
         [self], EigenArray.c_ptr_type(3, 1, self._ctype))
     return EigenArray(3, 1, dtype=self._ctype, from_cptr=trans_ptr)
Пример #13
0
 def as_matrix(self):
     """
     :return: Get this homography as a 3x3 matrix (same data type as this
         homography).
     :rtype: EigenArray
     """
     m_cptr = self._call_cfunc(
         'vital_homography_%s_as_matrix' % self._tchar, [self.C_TYPE_PTR],
         [self], EigenArray.c_ptr_type(3, 3, self._datatype))
     return EigenArray(3, 3, dtype=self._datatype, from_cptr=m_cptr)
Пример #14
0
    def test_from_iterable_1D(self):
        # 1-dim iterables/vectors are treated as column vectors
        input = [1, 2, 3, 4]
        expected = [[1], [2], [3], [4]]

        e = EigenArray.from_iterable(input)
        numpy.testing.assert_equal(e, expected)

        e2 = EigenArray.from_iterable(e)
        numpy.testing.assert_equal(e, e2)
Пример #15
0
 def as_matrix(self):
     """
     :return: similarity transformation as a 4x4 matrix
     :rtype: EigenArray
     """
     cptr = self._call_cfunc(
         'vital_similarity_%s_to_matrix4x4' % self._tchar,
         [self.C_TYPE_PTR], [self],
         EigenArray.c_ptr_type(4, 4, self._ctype))
     return EigenArray(4, 4, dtype=self._ctype, from_cptr=cptr)
Пример #16
0
    def test_mutability(self):
        a = EigenArray(2, 3)
        d = a.get_matrix()  # The data pointer
        for i in range(2):
            for j in range(3):
                d[i][j] = 0
        numpy.testing.assert_array_equal(a.get_matrix(),
                                         [[0, 0, 0], [0, 0, 0]])

        d[:] = 1
        numpy.testing.assert_array_equal(a.get_matrix(),
                                         [[1, 1, 1], [1, 1, 1]])

        d[1, 0] = 2
        numpy.testing.assert_array_equal(a.get_matrix(),
                                         [[1, 1, 1], [2, 1, 1]])

        d[:, 2] = 3
        numpy.testing.assert_array_equal(a.get_matrix(),
                                         [[1, 1, 3], [2, 1, 3]])

        d += 1
        numpy.testing.assert_array_equal(a.get_matrix(),
                                         [[2, 2, 4], [3, 2, 4]])

        b = d * 0
        numpy.testing.assert_array_equal(a.get_matrix(),
                                         [[2, 2, 4], [3, 2, 4]])
        numpy.testing.assert_array_equal(b, [[0, 0, 0], [0, 0, 0]])
Пример #17
0
 def loc(self, new_loc):
     """
     Set the 3D location of this landmark
     :param new_loc: New 3D location
     :type new_loc: collections.Iterable[float]
     """
     new_loc = EigenArray.from_iterable(new_loc, self._datatype, (3, 1))
     self._call_cfunc(
         'vital_landmark_{}_set_loc'.format(self._tchar),
         [self.C_TYPE_PTR,
          EigenArray.c_ptr_type(3, ctype=self._datatype)], [self, new_loc])
Пример #18
0
 def translation(self):
     """
     :return: a copy of this camera's translation vector
     :rtype: EigenArray
     """
     cam_trans = self.VITAL_LIB['vital_camera_translation']
     cam_trans.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
     cam_trans.restype = EigenArray.c_ptr_type(3)
     with VitalErrorHandle() as eh:
         c_ptr = cam_trans(self, eh)
     return EigenArray(3, from_cptr=c_ptr)
Пример #19
0
    def test_from_array_1D(self):
        # 1-dim iterables/vectors are treated as column vectors
        input = [[1], [2], [3], [4]]
        expected = [[1], [2], [3], [4]]

        e = EigenArray.from_array(input)
        em = e.get_matrix()
        numpy.testing.assert_equal(em, expected)

        e2 = EigenArray.from_array(em)
        em2 = e2.get_matrix()
        numpy.testing.assert_equal(em, em2)
Пример #20
0
 def test_center_initialized(self):
     expected_c = EigenArray(3)
     expected_c.get_matrix()[:] = [[1],[2],[3]]
     cam = Camera(expected_c)
     numpy.testing.assert_array_equal(
         numpy.array([cam.center]).T,
         expected_c.get_matrix()
     )
     numpy.testing.assert_array_equal(
         cam.center,
         [1,2,3]
     )
Пример #21
0
    def test_from_iterable_1D(self):
        # 1-dim iterables/vectors are treated as column vectors
        input = [1, 2, 3, 4]
        expected = [[1],
                    [2],
                    [3],
                    [4]]

        e = EigenArray.from_iterable(input)
        numpy.testing.assert_equal(e, expected)

        e2 = EigenArray.from_iterable(e)
        numpy.testing.assert_equal(e, e2)
Пример #22
0
    def test_matrix_init(self):
        # Test that construction does not fail when passing valid matrices as
        # initializer
        m1 = [[0, 1, 3], [0.3, 0.1, 10], [-1, 8.1, 4.7]]
        m2_d = EigenArray.from_array(m1, 'd')
        m2_f = EigenArray.from_array(m1, 'f')

        Homography.from_matrix(m1, 'd')
        Homography.from_matrix(m1, 'f')
        Homography.from_matrix(m2_d.get_matrix(), 'd')
        Homography.from_matrix(m2_d.get_matrix(), 'f')
        Homography.from_matrix(m2_f.get_matrix(), 'd')
        Homography.from_matrix(m2_f.get_matrix(), 'f')
Пример #23
0
    def as_matrix(self):
        """
        Convert camera into a new 3x4 homogeneous projection matrix.

        :return: new 3x4 homogeneous projection matrix
        :rtype: EigenArray
        """
        cam_asmat = self.VITAL_LIB['vital_camera_as_matrix']
        cam_asmat.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
        cam_asmat.restype = EigenArray.c_ptr_type(3, 4)
        with VitalErrorHandle() as eh:
            cptr = cam_asmat(self, eh)
        return EigenArray(3, 4, from_cptr=cptr)
Пример #24
0
    def test_matrix_init(self):
        # Test that construction does not fail when passing valid matrices as
        # initializer
        m1 = [[0, 1, 3], [0.3, 0.1, 10], [-1, 8.1, 4.7]]
        m2_d = EigenArray.from_iterable(m1, ctypes.c_double, (3, 3))
        m2_f = EigenArray.from_iterable(m1, ctypes.c_float, (3, 3))

        Homography.from_matrix(m1, ctypes.c_double)
        Homography.from_matrix(m1, ctypes.c_float)
        Homography.from_matrix(m2_d, ctypes.c_double)
        Homography.from_matrix(m2_d, ctypes.c_float)
        Homography.from_matrix(m2_f, ctypes.c_double)
        Homography.from_matrix(m2_f, ctypes.c_float)
Пример #25
0
    def _new(self, loc, mag, scale, angle, rgb_color):
        loc = EigenArray.from_iterable(loc,
                                       target_ctype=self._datatype,
                                       target_shape=(2, 1))
        if rgb_color is None:
            rgb_color = RGBColor()

        # noinspection PyProtectedMember
        return self._call_cfunc('vital_feature_{}_new'.format(self._tchar), [
            EigenArray.c_ptr_type(2, 1, self._datatype), self._datatype,
            self._datatype, self._datatype,
            RGBColor.c_ptr_type()
        ], [loc, mag, scale, angle, rgb_color], self.C_TYPE_PTR)
Пример #26
0
    def test_mutability(self):
        a = EigenArray(2, 3)
        d = a.get_matrix()  # The data pointer
        for i in range(2):
          for j in range(3):
            d[i][j] = 0
        numpy.testing.assert_array_equal(a.get_matrix(), [[0, 0, 0],
                                             [0, 0, 0]])

        d[:] = 1
        numpy.testing.assert_array_equal(a.get_matrix(), [[1, 1, 1],
                                             [1, 1, 1]])

        d[1, 0] = 2
        numpy.testing.assert_array_equal(a.get_matrix(), [[1, 1, 1],
                                             [2, 1, 1]])

        d[:, 2] = 3
        numpy.testing.assert_array_equal(a.get_matrix(), [[1, 1, 3],
                                             [2, 1, 3]])

        d += 1
        numpy.testing.assert_array_equal(a.get_matrix(), [[2, 2, 4],
                                             [3, 2, 4]])

        b = d*0
        numpy.testing.assert_array_equal(a.get_matrix(), [[2, 2, 4],
                                             [3, 2, 4]])
        numpy.testing.assert_array_equal(b, [[0, 0, 0],
                                             [0, 0, 0]])
Пример #27
0
 def center(self):
     """
     :return: a copy of this camera's center coordinate.
     :rtype: EigenArray
     """
     cam_center = self.VITAL_LIB['vital_camera_center']
     cam_center.argtypes = [
         self.c_ptr_type(),
         VitalErrorHandle.c_ptr_type()
     ]
     cam_center.restype = EigenArray.c_ptr_type(3)
     with VitalErrorHandle() as eh:
         c_ptr = cam_center(self, eh)
     return EigenArray(3, from_cptr=c_ptr)
    def estimate_from_points(self, from_pts, to_pts):
        """
        Estimate the similarity transform between two corresponding point sets

        :raises VitalAlgorithmException: from and to point sets are misaligned,
            insufficient or degenerate

        :param from_pts: Iterable of 3D points in the ``from`` space.
        :type from_pts: collections.Iterable[EigenArray | collections.Sequence[float]]

        :param to_pts: Iterable of 3D points in the ``to`` space.
        :type to_pts: collections.Iterable[EigenArray | collections.Sequence[float]]

        :return: New similarity instance
        :rtype: Similarity

        """
        ea_type = EigenArray.c_ptr_type(3, 1, ctypes.c_double)

        # make C arrays from input points
        from_list = [EigenArray.from_iterable(p, target_shape=(3, 1))
                     for p in from_pts]
        to_list = [EigenArray.from_iterable(p, target_shape=(3, 1))
                   for p in to_pts]
        if len(from_list) != len(to_list):
            raise VitalAlgorithmException(
                "From and to iterables not the same length: %d != %d"
                % (len(from_list), len(to_list))
            )
        n = len(from_list)
        from_ptr_arr = (ea_type * n)()
        for i, e in enumerate(from_list):
            from_ptr_arr[i] = e.c_pointer

        to_ptr_arr = (ea_type * n)()
        for i, e in enumerate(to_list):
            to_ptr_arr[i] = e.c_pointer

        sim_ptr = self._call_cfunc(
            'vital_algorithm_estimate_similarity_transform_estimate_transform_points',
            [self.C_TYPE_PTR, ctypes.c_size_t, ctypes.POINTER(ea_type),
             ctypes.POINTER(ea_type)],
            [self, n, from_ptr_arr, to_ptr_arr],
            Similarity.c_ptr_type(ctypes.c_double),
            {
                1: VitalAlgorithmException
            }
        )
        return Similarity(ctype=ctypes.c_double, from_cptr=sim_ptr)
    def estimate_from_points(self, from_pts, to_pts):
        """
        Estimate the similarity transform between two corresponding point sets

        :raises VitalAlgorithmException: from and to point sets are misaligned,
            insufficient or degenerate

        :param from_pts: Iterable of 3D points in the ``from`` space.
        :type from_pts: collections.Iterable[EigenArray | collections.Sequence[float]]

        :param to_pts: Iterable of 3D points in the ``to`` space.
        :type to_pts: collections.Iterable[EigenArray | collections.Sequence[float]]

        :return: New similarity instance
        :rtype: Similarity

        """
        ea_type = EigenArray.c_ptr_type(3, 1, ctypes.c_double)

        # make C arrays from input points
        from_list = [
            EigenArray.from_iterable(p, target_shape=(3, 1)) for p in from_pts
        ]
        to_list = [
            EigenArray.from_iterable(p, target_shape=(3, 1)) for p in to_pts
        ]
        if len(from_list) != len(to_list):
            raise VitalAlgorithmException(
                "From and to iterables not the same length: %d != %d" %
                (len(from_list), len(to_list)))
        n = len(from_list)
        from_ptr_arr = (ea_type * n)()
        for i, e in enumerate(from_list):
            from_ptr_arr[i] = e.c_pointer

        to_ptr_arr = (ea_type * n)()
        for i, e in enumerate(to_list):
            to_ptr_arr[i] = e.c_pointer

        sim_ptr = self._call_cfunc(
            'vital_algorithm_estimate_similarity_transform_estimate_transform_points',
            [
                self.C_TYPE_PTR, ctypes.c_size_t,
                ctypes.POINTER(ea_type),
                ctypes.POINTER(ea_type)
            ], [self, n, from_ptr_arr, to_ptr_arr],
            Similarity.c_ptr_type(ctypes.c_double),
            {1: VitalAlgorithmException})
        return Similarity(ctype=ctypes.c_double, from_cptr=sim_ptr)
Пример #30
0
    def test_valid_static_size_init(self):
        """
        Test construction of some of the static sizes.
        """
        a = EigenArray()  # default shape
        ntools.assert_equal(a.shape, (2, 1))

        a = EigenArray(2, 1)
        ntools.assert_equal(a.shape, (2, 1))

        a = EigenArray(2, 2)
        ntools.assert_equal(a.shape, (2, 2))

        a = EigenArray(4, 4)
        ntools.assert_equal(a.shape, (4, 4))
Пример #31
0
    def test_point_map(self):
        h_f = Homography(ctypes.c_float)
        h_d = Homography(ctypes.c_double)

        p_f = EigenArray.from_iterable([2.2, 3.3], ctypes.c_float)
        p_d = EigenArray.from_iterable([5.5, 6.6], ctypes.c_double)

        # float-float
        numpy.testing.assert_almost_equal(h_f.map(p_f), p_f)
        # float-double
        numpy.testing.assert_almost_equal(h_f.map(p_d), p_d)
        # double-float
        numpy.testing.assert_almost_equal(h_d.map(p_f), p_f)
        # double-double
        numpy.testing.assert_almost_equal(h_d.map(p_d), p_d)
Пример #32
0
    def test_from_array_1D(self):
        # 1-dim iterables/vectors are treated as column vectors
        input = [[1], [2], [3], [4]]
        expected = [[1],
                    [2],
                    [3],
                    [4]]

        e = EigenArray.from_array(input)
        em = e.get_matrix()
        numpy.testing.assert_equal(em, expected)

        e2 = EigenArray.from_array(em)
        em2 = e2.get_matrix()
        numpy.testing.assert_equal(em, em2)
Пример #33
0
    def test_matrix_init(self):
        # Test that construction does not fail when passing valid matrices as
        # initializer
        m1 = [[0,     1,   3],
              [0.3, 0.1,  10],
              [-1,  8.1, 4.7]]
        m2_d = EigenArray.from_array(m1, 'd')
        m2_f = EigenArray.from_array(m1, 'f')

        Homography.from_matrix(m1, 'd')
        Homography.from_matrix(m1, 'f')
        Homography.from_matrix(m2_d.get_matrix(), 'd')
        Homography.from_matrix(m2_d.get_matrix(), 'f')
        Homography.from_matrix(m2_f.get_matrix(), 'd')
        Homography.from_matrix(m2_f.get_matrix(), 'f')
Пример #34
0
    def test_read_write_krtd_file(self):
        # Use a random string filename to avoid name collision.
        fname = 'temp_camera_test_read_write_krtd_file.txt'

        try:
            for _ in range(100):
                c = (rand(3)*2-1)*100
                center = EigenArray.from_array([c])
                rotation = Rotation.from_quaternion(numpy.random.rand(4)*2-1)
                intrinsics = CameraIntrinsics(10, (5, 5), 1.2, 0.5, [4, 5, 6])
                c1 = Camera(center, rotation,
                            intrinsics)

                c1.write_krtd_file(fname)
                c2 = Camera.from_krtd_file(fname)

                err = numpy.linalg.norm(c1.center-c2.center)
                assert err < 1e-9, ''.join(['Centers are different by ',
                                            str(err)])

                c1.rotation.angle_from(c2.rotation) < 1e-12

                attr = ['focal_length','aspect_ratio','principle_point','skew',
                        'dist_coeffs']
                for att in attr:
                    v1 = numpy.array(getattr(c1.intrinsics,att))
                    v2 = numpy.array(getattr(c2.intrinsics,att))
                    err = numpy.linalg.norm(v1-v2)
                    assert err < 1e-8, ''.join(['Difference ',str(err),
                                                 ' for attribute: ',att])
        finally:
            if os.path.isfile(fname):
                os.remove(fname)
Пример #35
0
 def test_translation_initialized(self):
     center = EigenArray.from_array([[1],[2],[3]])
     rotation = Rotation.from_axis_angle([0, 1, 0], math.pi / 2.)
     cam = Camera(center, rotation)
     numpy.testing.assert_array_equal(
         cam.translation,
         -(rotation * center.get_matrix())
     )
Пример #36
0
    def test_order_transform(self):
        a = EigenArray(2, 3)
        d = a.base.base  # The data pointer
        # column-major 2x3 matrix [[ 1 2 3 ]  (Eigen format)
        #                          [ 4 5 6 ]]
        d[0] = 1; d[2] = 2; d[4] = 3
        d[1] = 4; d[3] = 5; d[5] = 6

        numpy.testing.assert_array_equal(a, [[1., 2., 3.],
                                             [4., 5., 6.]])

        ntools.assert_equal(a.at_eigen_base_index(0, 0), 1)
        ntools.assert_equal(a.at_eigen_base_index(0, 1), 2)
        ntools.assert_equal(a.at_eigen_base_index(0, 2), 3)
        ntools.assert_equal(a.at_eigen_base_index(1, 0), 4)
        ntools.assert_equal(a.at_eigen_base_index(1, 1), 5)
        ntools.assert_equal(a.at_eigen_base_index(1, 2), 6)
Пример #37
0
    def _new(self, loc, mag, scale, angle, rgb_color):
        loc = EigenArray.from_iterable(loc, target_ctype=self._datatype,
                                       target_shape=(2, 1))
        if rgb_color is None:
            rgb_color = RGBColor()

        # noinspection PyProtectedMember
        return self._call_cfunc(
            'vital_feature_{}_new'.format(self._tchar),
            [
                EigenArray.c_ptr_type(2, 1, self._datatype),
                self._datatype, self._datatype, self._datatype,
                RGBColor.c_ptr_type()
            ],
            self.C_TYPE_PTR,
            loc, mag, scale, angle, rgb_color
        )
Пример #38
0
    def test_clone_look_at(self):
        pp = EigenArray.from_iterable([300, 400])
        k = CameraIntrinsics(1000, pp)
        focus = EigenArray.from_iterable([0, 1, -2])

        base = Camera([3, -4, 7], Rotation(), k)
        cam = base.clone_look_at(focus)
        nose.tools.assert_not_equal(base, cam)

        ifocus = cam.project(focus)
        nose.tools.assert_almost_equal(numpy.linalg.norm(ifocus - pp, 2), 0.,
                                       12)

        ifocus_up = cam.project(focus + EigenArray.from_iterable([0, 0, 2]))
        tmp = ifocus_up - pp
        nose.tools.assert_almost_equal(tmp[0], 0., 12)
        nose.tools.assert_true(tmp[1] < 0.)
Пример #39
0
    def test_new(self):
        # just seeing that basic construction doesn't blow up
        cam = Camera()

        c = EigenArray(3)
        r = Rotation()
        ci = CameraIntrinsics()
        cam = Camera(c, r, ci)
Пример #40
0
    def project(self, point):
        """
        Project a 3D point into a new 2D image point (eigen array) via this
        camera model.

        :param point: 3D point to transform into the 2D image plane.
        :return: New 2D eigen array that is the transformed point.
        """
        point = EigenArray.from_iterable(point, target_shape=(3, 1))

        cam_project = self.VITAL_LIB['vital_camera_project']
        cam_project.argtypes = [self.c_ptr_type(),
                                EigenArray.c_ptr_type(3),
                                VitalErrorHandle.c_ptr_type()]
        cam_project.restype = EigenArray.c_ptr_type(2)
        with VitalErrorHandle() as eh:
            cptr = cam_project(self, point, eh)
        return EigenArray(2, from_cptr=cptr)
Пример #41
0
    def from_matrix(cls, m, ctype=DFLT_CTYPE):
        """
        Create a similarity transform instance from a 4x4 matrix

        :param m: 4x4 transformation matrix
        :type m: EigenArray | collections.Iterable[float]

        :return: new Similarity instance
        :rtype: Similarity

        """
        m = EigenArray.from_iterable(m, ctype, target_shape=(4, 4))
        # noinspection PyProtectedMember
        tchar = ctype._type_
        cptr = cls._call_cfunc('vital_similarity_%s_from_matrix4x4' % tchar,
                               [EigenArray.c_ptr_type(4, 4, ctype)], [m],
                               Similarity.c_ptr_type(ctype))
        return Similarity(ctype=ctype, from_cptr=cptr)
Пример #42
0
 def location(self):
     """ Get feature location """
     cptr = self._call_cfunc(
         'vital_feature_loc',
         [self.C_TYPE_PTR],
         EigenArray.c_ptr_type(2),
         self
     )
     return EigenArray(2, from_cptr=cptr)
Пример #43
0
    def transform_vector(self, vec):
        """
        Transform a 3D vector with this similarity transformation

        :param vec: 3D Vector to transform
        :type vec: collections.Iterable[float]

        :return: Transformed 3D vector
        :rtype: EigenArray

        """
        vec = EigenArray.from_iterable(vec, self._ctype, (3, 1))
        t_ptr = self._call_cfunc(
            'vital_similarity_%s_vector_transform' % self._tchar,
            [self.C_TYPE_PTR,
             EigenArray.c_ptr_type(3, 1, self._ctype)], [self, vec],
            EigenArray.c_ptr_type(3, 1, self._ctype))
        return EigenArray(3, 1, dtype=self._ctype, from_cptr=t_ptr)
Пример #44
0
    def test_clone_look_at(self):
        pp = EigenArray.from_array([[300], [400]])
        k = CameraIntrinsics(1000, [300, 400])
        focus = EigenArray.from_array([[0], [1], [-2]])
        center = EigenArray.from_array([[3],[-4],[7]])

        base = Camera(center, Rotation(), k)
        cam = base.clone_look_at(numpy.array([0,1,2]))
        nose.tools.assert_not_equal(base, cam)

        ifocus = cam.project([0,1,2])
        nose.tools.assert_almost_equal(numpy.linalg.norm(ifocus - pp.get_matrix().T, 2),
                                       0., 12)

        ifocus_up = cam.project([0,1,4])
        tmp = (ifocus_up - pp.get_matrix().T)[0]
        nose.tools.assert_almost_equal(tmp[0], 0., 12)
        nose.tools.assert_true(tmp[1] < 0.)
Пример #45
0
    def test_from_iterable(self):
        # from list
        expected_list = [[0.4, 0],
                         [1, 1.123],
                         [2.253, 4.768124]]
        ea = EigenArray.from_iterable(expected_list, target_shape=(3, 2))
        numpy.testing.assert_array_equal(ea, expected_list)

        # from ndarray
        expected_ndar = numpy.array(expected_list)
        ea = EigenArray.from_iterable(expected_ndar, target_shape=(3, 2))
        numpy.testing.assert_array_equal(ea, expected_ndar)

        # from EigenArray, which should return the input object
        ea = EigenArray(3, 2)
        ea[:] = expected_list
        ea2 = EigenArray.from_iterable(ea, target_shape=(3, 2))
        numpy.testing.assert_array_equal(ea2, ea)
        ntools.assert_is(ea, ea2)
        ntools.assert_true(ea is ea2)
Пример #46
0
def init_landmarks(num_lm, c=None):
    """
    construct map of landmarks will all locations at ``c`` with IDs in range
    ``[0, num_lm]``.
    """
    if c is None:
        c = EigenArray.from_iterable([0, 0, 0])
    d = {}
    for i in range(num_lm):
        d[i] = Landmark(loc=c)
    return LandmarkMap.from_dict(d)
Пример #47
0
 def center(self):
     """
     :return: a copy of this camera's center coordinate.
     :rtype: EigenArray
     """
     cam_center = self.VITAL_LIB['vital_camera_center']
     cam_center.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
     cam_center.restype = EigenArray.c_ptr_type(3)
     with VitalErrorHandle() as eh:
         c_ptr = cam_center(self, eh)
     return EigenArray(3, from_cptr=c_ptr)
Пример #48
0
 def translation(self):
     """
     :return: a copy of this camera's translation vector
     :rtype: EigenArray
     """
     cam_trans = self.VITAL_LIB['vital_camera_translation']
     cam_trans.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
     cam_trans.restype = EigenArray.c_ptr_type(3)
     with VitalErrorHandle() as eh:
         c_ptr = cam_trans(self, eh)
     return EigenArray(3, from_cptr=c_ptr)
Пример #49
0
    def test_new_matrix(self):
        a = EigenArray(2, 2, type='d')
        m = a.get_matrix()
        m[:] = 1.
        c = Covariance.from_matrix(2, 'd', m)
        m_out = c.to_matrix()
        print('input matrix:\n', m)
        print('output matrix:\n', m_out)
        numpy.testing.assert_array_equal(m_out, m)

        # Type casting should be handled
        a = EigenArray(2, 2, type='f')
        m = a.get_matrix()
        m[:] = 1.
        c = Covariance.from_matrix(2, 'd', m)
        m_out = c.to_matrix()
        print('input matrix:\n', m)
        print('output matrix:\n', m_out)
        numpy.testing.assert_array_equal(m_out, m)

        # Any other numpy array of the correct shape should be acceptable
        m = numpy.ndarray((2, 2))
        m[:] = 3.
        c = Covariance.from_matrix(2, 'f', init=m)
        m_out = c.to_matrix()
        print('input matrix:\n', m)
        print('output matrix:\n', m_out)
        numpy.testing.assert_array_equal(m_out, m)

        # Diagonally congruent values should be averages when initializing with
        # matrix
        m = numpy.eye(3, dtype=numpy.double)
        m[0,2] = 2.
        m_expected = m.copy()
        m_expected[0,2] = 1.
        m_expected[2,0] = 1.
        c = Covariance.from_matrix(3, init=m)
        m_out = c.to_matrix()
        print('input matrix:\n', m)
        print('output matrix:\n', m_out)
        numpy.testing.assert_array_equal(m_out, m_expected)
Пример #50
0
 def location(self, loc):
     """ Set new feature location
     :param loc: New locations. May be any iterable of 2 elements.
     :type loc: collections.Iterable
     """
     loc = EigenArray.from_iterable(loc, self._datatype, (2, 1))
     self._call_cfunc(
         'vital_feature_{}_set_loc'.format(self._tchar),
         [self.C_TYPE_PTR, loc.C_TYPE_PTR],
         None,
         self, loc
     )
Пример #51
0
    def depth(self, point):
        """
        Compute the distance of the 3d point to the image plane.

        Points with negative depth are behind the camera.

        :param point: 3D point to find the depth of.

        :return: Depth value
        :rtype: float

        """
        point = EigenArray.from_iterable(point)

        cam_depth = self.VITAL_LIB['vital_camera_depth']
        cam_depth.argtypes = [self.c_ptr_type(),
                              EigenArray.c_ptr_type(3),
                              VitalErrorHandle.c_ptr_type()]
        cam_depth.restype = ctypes.c_double
        with VitalErrorHandle() as eh:
            return cam_depth(self, point, eh)
Пример #52
0
    def test_equal(self):
        cam1 = Camera()
        cam2 = Camera()
        nose.tools.assert_equal(cam1, cam1)
        nose.tools.assert_equal(cam1, cam2)

        center = EigenArray.from_array([[1],[2],[3]])
        rotation = Rotation.from_axis_angle([0, 1, 0], math.pi / 2.)
        cam1 = Camera(center, rotation)
        cam2 = Camera(center, rotation)
        nose.tools.assert_equal(cam1, cam1)
        nose.tools.assert_equal(cam1, cam2)
Пример #53
0
    def as_matrix(self):
        """
        Convert camera into a new 3x4 homogeneous projection matrix.

        :return: new 3x4 homogeneous projection matrix
        :rtype: EigenArray
        """
        cam_asmat = self.VITAL_LIB['vital_camera_as_matrix']
        cam_asmat.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()]
        cam_asmat.restype = EigenArray.c_ptr_type(3, 4)
        with VitalErrorHandle() as eh:
            cptr = cam_asmat(self, eh)
        return EigenArray(3, 4, from_cptr=cptr)
Пример #54
0
 def location(self, loc):
     """ Set new feature location
     :param loc: New locations. May be any iterable of 2 elements.
     :type loc: collections.Iterable
     """
     if self._datatype is None:
         raise VitalNoTypeInfoException("Type info required but not present")
     loc = EigenArray.from_iterable(loc, self._datatype, (2, 1))
     self._call_cfunc(
         'vital_feature_{}_set_loc'.format(self._tchar),
         [self.C_TYPE_PTR, loc.C_TYPE_PTR],
         [self, loc],
     )
Пример #55
0
    def test_from_array(self):
        # from list
        expected_list = [[0.4, 0],
                         [1, 1.123],
                         [2.253, 4.768124]]
        ea = EigenArray.from_array(expected_list)
        em = ea.get_matrix()
        numpy.testing.assert_array_equal(em, expected_list)

        # from ndarray
        expected_ndar = numpy.array(expected_list)
        ea = EigenArray.from_array(expected_ndar)
        em = ea.get_matrix()
        numpy.testing.assert_array_equal(em, expected_ndar)

        # from EigenArray, which should return the input object
        ea = EigenArray(3, 2)
        em = ea.get_matrix()
        em[:] = expected_list
        ea2 = EigenArray.from_array(em)
        em2 = ea2.get_matrix()
        numpy.testing.assert_array_equal(em2, em)
Пример #56
0
    def test_new_matrix(self):
        m = EigenArray(2, 2, dtype=numpy.double)
        m[:] = 1.
        c = Covariance(2, ctypes.c_double, m)
        m_out = c.to_matrix()
        print 'input matrix:\n', m
        print 'output matrix:\n', m_out
        numpy.testing.assert_array_equal(m_out, m)

        # Type casting should be handled
        m = EigenArray(2, 2, dtype=numpy.float32)
        m[:] = 1.
        c = Covariance(2, ctypes.c_double, m)
        m_out = c.to_matrix()
        print 'input matrix:\n', m
        print 'output matrix:\n', m_out
        numpy.testing.assert_array_equal(m_out, m)

        # Any other numpy array of the correct shape should be acceptable
        m = numpy.ndarray((2, 2))
        m[:] = 3.
        c = Covariance(2, ctypes.c_float, init_scalar_or_matrix=m)
        m_out = c.to_matrix()
        print 'input matrix:\n', m
        print 'output matrix:\n', m_out
        numpy.testing.assert_array_equal(m_out, m)

        # Diagonally congruent values should be averages when initializing with
        # matrix
        m = numpy.eye(3, dtype=numpy.double)
        m[0,2] = 2.
        m_expected = m.copy()
        m_expected[0,2] = 1.
        m_expected[2,0] = 1.
        c = Covariance(3, init_scalar_or_matrix=m)
        m_out = c.to_matrix()
        print 'input matrix:\n', m
        print 'output matrix:\n', m_out
        numpy.testing.assert_array_equal(m_out, m_expected)
Пример #57
0
    def _new(self, center, rotation, intrinsics):
        cam_new = self.VITAL_LIB['vital_camera_new']
        cam_new.argtypes = [EigenArray.c_ptr_type(3, 1, ctypes.c_double),
                            Rotation.c_ptr_type(ctypes.c_double),
                            CameraIntrinsics.c_ptr_type(),
                            VitalErrorHandle.c_ptr_type()]
        cam_new.restype = self.c_ptr_type()

        # Fill in parameter gaps
        if center is None:
            center = EigenArray(3)
            center[:] = 0
        else:
            center = EigenArray.from_iterable(center)

        if rotation is None:
            rotation = Rotation()

        if intrinsics is None:
            intrinsics = CameraIntrinsics()

        with VitalErrorHandle() as eh:
            return cam_new(center, rotation, intrinsics, eh)
Пример #58
0
def cube_corners(s, c=None):
    """
    Construct map of landmarks at the corners of a cube centered on ``c`` with a
    side length of ``s``.
    :rtype: LandmarkMap
    """
    if c is None:
        c = EigenArray.from_iterable([0, 0, 0])
    s /= 2.
    # Can add lists to numpy.ndarray types
    d = {
        0: Landmark(c + EigenArray.from_iterable([-s, -s, -s])),
        1: Landmark(c + EigenArray.from_iterable([-s, -s,  s])),
        2: Landmark(c + EigenArray.from_iterable([-s,  s, -s])),
        3: Landmark(c + EigenArray.from_iterable([-s,  s,  s])),
        4: Landmark(c + EigenArray.from_iterable([ s, -s, -s])),
        5: Landmark(c + EigenArray.from_iterable([ s, -s,  s])),
        6: Landmark(c + EigenArray.from_iterable([ s,  s, -s])),
        7: Landmark(c + EigenArray.from_iterable([ s,  s,  s])),
    }
    return LandmarkMap.from_dict(d)
Пример #59
0
    def test_to_from_string(self):
        cam = Camera()
        cam_s = cam.as_string()
        cam2 = Camera.from_string(cam_s)
        print "Default camera string:\n%s" % cam_s
        print "Default newcam string:\n%s" % cam2.as_string()
        nose.tools.assert_equal(cam, cam2)

        center = EigenArray.from_iterable([[1],
                                           [2],
                                           [3]])
        rotation = Rotation.from_axis_angle([[0], [1], [0]], math.pi / 2.)
        cam = Camera(center, rotation)
        cam_s = cam.as_string()
        cam2 = Camera.from_string(cam_s)
        print "Custom camera string:\n%s" % cam_s
        print "Custom newcam string:\n%s" % cam2.as_string()
        nose.tools.assert_equal(cam, cam2)
Пример #60
0
def init_cameras(num_cams=20, intrinsics=None):
    """
    Initialize camera sequence with all cameras at the same location (0, 0, 1)
    and looking at origin.

    :param num_cams: Number of cameras to create, default 20.
    :param intrinsics: Intrinsics to use for all cameras.
    :return: Camera map of initialize cameras

    """
    if intrinsics is None:
        intrinsics = CameraIntrinsics(1000, (640, 480))
    r = Rotation()
    c = EigenArray.from_iterable((0, 0, 1))
    d = {}
    for i in range(num_cams):
        cam = Camera(c, r, intrinsics).clone_look_at([0, 0, 0],
                                                     [0, 1, 0])
        d[i] = cam
    return CameraMap(d)