def test_to_matrix(self): # Default value should be identity rot_d = Rotation('d') numpy.testing.assert_array_equal(rot_d.matrix(), numpy.eye(3)) rot_f = Rotation('f') numpy.testing.assert_array_equal(rot_f.matrix(), numpy.eye(3))
def test_new_default(self): # That these even construct rot_d = Rotation('d') nose.tools.assert_equal(rot_d._ctype, 'd') rot_f = Rotation('f') nose.tools.assert_equal(rot_f._ctype, 'f')
def test_to_matrix(self): # Default value should be identity rot_d = Rotation(ctypes.c_double) numpy.testing.assert_array_equal(rot_d.matrix(), numpy.eye(3)) rot_f = Rotation(ctypes.c_float) numpy.testing.assert_array_equal(rot_f.matrix(), numpy.eye(3))
def test_to_quaternion(self): rot_d = Rotation(ctypes.c_double) numpy.testing.assert_array_equal(rot_d.quaternion(), [[0], [0], [0], [1]]) rot_f = Rotation(ctypes.c_float) numpy.testing.assert_array_equal(rot_f.quaternion(), [[0], [0], [0], [1]])
def test_new_default(self): # That these even construct rot_d = Rotation(ctypes.c_double) nose.tools.assert_equal(rot_d._ctype, ctypes.c_double) nose.tools.assert_equal(rot_d._spec, 'd') rot_f = Rotation(ctypes.c_float) nose.tools.assert_equal(rot_f._ctype, ctypes.c_float) nose.tools.assert_equal(rot_f._spec, 'f')
def test_to_ypr(self): # ypr identity: (pi/2, 0, pi) ident_ypr = (math.pi / 2, 0, -math.pi) ident_ypr_float = [ctypes.c_float(v).value for v in ident_ypr] rot_d = Rotation(ctypes.c_double) rot_f = Rotation(ctypes.c_float) numpy.testing.assert_equal(rot_d.yaw_pitch_roll(), ident_ypr) numpy.testing.assert_equal(rot_f.yaw_pitch_roll(), ident_ypr_float)
def test_to_ypr(self): # ypr identity: (pi/2, 0, pi) ident_ypr = (math.pi / 2, 0, -math.pi) ident_ypr_float = [float(v) for v in ident_ypr] rot_d = Rotation('d') rot_f = Rotation('f') numpy.testing.assert_almost_equal(rot_d.yaw_pitch_roll(), ident_ypr) numpy.testing.assert_almost_equal(rot_f.yaw_pitch_roll(), ident_ypr)
def test_to_rodrigues(self): # rodrigues identity: [0,0,0] ident_rod = [[0], [0], [0]] rot_d = Rotation(ctypes.c_double) rot_f = Rotation(ctypes.c_float) rod = rot_d.rodrigues() numpy.testing.assert_equal(rod, ident_rod) rod = rot_f.rodrigues() numpy.testing.assert_equal(rod, ident_rod)
def test_to_rodrigues(self): # rodrigues identity: [0,0,0] ident_rod = [0, 0, 0] rot_d = Rotation('d') rot_f = Rotation('f') rod = rot_d.rodrigues() numpy.testing.assert_equal(rod, ident_rod) rod = rot_f.rodrigues() numpy.testing.assert_equal(rod, ident_rod)
def test_compose(self): expected_quat = [[+2], [-1], [-3], [+0]] r_ident_d = Rotation(ctypes.c_double) r_ident_f = Rotation(ctypes.c_float) r_other_d = Rotation.from_quaternion(expected_quat, ctypes.c_double) r_other_f = Rotation.from_quaternion(expected_quat, ctypes.c_float) r_res_d = r_ident_d.compose(r_other_d) nose.tools.assert_is_not(r_other_d, r_res_d) numpy.testing.assert_equal(r_res_d, r_other_d) numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat) r_res_f = r_ident_f.compose(r_other_f) nose.tools.assert_is_not(r_other_f, r_res_f) numpy.testing.assert_equal(r_res_f, r_other_f) numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat) # Should also work with multiply operator r_res_d = r_ident_d * r_other_d nose.tools.assert_is_not(r_other_d, r_res_d) numpy.testing.assert_equal(r_res_d, r_other_d) numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat) r_res_f = r_ident_f * r_other_f nose.tools.assert_is_not(r_other_f, r_res_f) numpy.testing.assert_equal(r_res_f, r_other_f) numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat) # Rotation of non-congruent types should be converted automatically r_res_d = r_ident_d.compose(r_other_f) nose.tools.assert_is_not(r_res_d, r_other_f) numpy.testing.assert_equal(r_res_d.quaternion(), r_other_f.quaternion()) numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat) r_res_f = r_ident_f.compose(r_other_d) nose.tools.assert_is_not(r_res_f, r_other_f) numpy.testing.assert_equal(r_res_f.quaternion(), r_other_f.quaternion()) numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat) r_res_d = r_ident_d * r_other_f nose.tools.assert_is_not(r_res_d, r_other_f) numpy.testing.assert_equal(r_res_d.quaternion(), r_other_f.quaternion()) numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat) r_res_f = r_ident_f * r_other_d nose.tools.assert_is_not(r_res_f, r_other_f) numpy.testing.assert_equal(r_res_f.quaternion(), r_other_f.quaternion()) numpy.testing.assert_equal(r_res_f.quaternion(), expected_quat)
def test_to_axis_angle(self): # expected identity: [0,0,1] and 0 ident_axis = [0, 0, 1] ident_angle = 0 rot_d = Rotation('d') rot_f = Rotation('f') numpy.testing.assert_equal(rot_d.axis(), ident_axis) nose.tools.assert_equal(rot_d.angle(), ident_angle) numpy.testing.assert_equal(rot_f.axis(), ident_axis) nose.tools.assert_equal(rot_f.angle(), ident_angle)
def test_to_axis_angle(self): # expected identity: [0,0,1] and 0 ident_axis = [[0], [0], [1]] ident_angle = 0 rot_d = Rotation(ctypes.c_double) rot_f = Rotation(ctypes.c_float) numpy.testing.assert_equal(rot_d.axis(), ident_axis) nose.tools.assert_equal(rot_d.angle(), ident_angle) numpy.testing.assert_equal(rot_f.axis(), ident_axis) nose.tools.assert_equal(rot_f.angle(), ident_angle)
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)
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)
def test_get_rotation(self): s = Similarity() numpy.testing.assert_array_almost_equal(s.rotation.matrix(), Rotation().matrix()) s = Similarity(self.s, self.r, self.t) numpy.testing.assert_array_almost_equal(s.rotation.matrix(), self.r.matrix())
def rotation(self): """ Get the rotation of this similarity transformation :rtype: Rotation """ rot_ptr = self._call_cfunc( 'vital_similarity_%s_rotation' % self._tchar, [self.C_TYPE_PTR], [self], Rotation.c_ptr_type(self._ctype)) return Rotation(self._ctype, rot_ptr)
def rotation(self): """ :return: a copy of this camera's rotation :rtype: Rotation """ cam_rot = self.VITAL_LIB['vital_camera_rotation'] cam_rot.argtypes = [self.c_ptr_type(), VitalErrorHandle.c_ptr_type()] cam_rot.restype = Rotation.c_ptr_type() with VitalErrorHandle() as eh: c_ptr = cam_rot(self, eh) return Rotation(from_cptr=c_ptr)
def test_clone(self): # clone should have the same values as original but be a different # memory address. R = Rotation() K = CameraIntrinsics(1000, (640, 480)) c1 = Camera((0, 3, 5), R, K) c2 = c1.clone() # Different mem address nose.tools.assert_not_equal(ctypes.addressof(c1.c_pointer.contents), ctypes.addressof(c2.c_pointer.contents)) numpy.testing.assert_equal(c1.center, c2.center) nose.tools.assert_equal(c1.rotation, c2.rotation) nose.tools.assert_equal(c1.intrinsics, c2.intrinsics)
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.)
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.)
def camera_seq(num_cams=20, k=None): """ Create a camera sequence (elliptical path) :param num_cams: Number of cameras. Default is 20 :param k: Camera intrinsics to use for all created cameras. Default has focal length = 1000 and principle point of (640, 480). :return: """ if k is None: k = CameraIntrinsics(1000, [640, 480]) d = {} r = Rotation() # identity for i in xrange(num_cams): frac = float(i) / num_cams x = 4 * math.cos(2 * frac) y = 3 * math.sin(2 * frac) d[i] = Camera([x, y, 2 + frac], r, k).clone_look_at([0, 0, 0]) return CameraMap(d)
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)
def test_eq(self): # Identities should equal r1 = Rotation(ctypes.c_double) r2 = Rotation(ctypes.c_double) nose.tools.assert_equal(r1, r2) r1 = Rotation(ctypes.c_float) r2 = Rotation(ctypes.c_float) nose.tools.assert_equal(r1, r2) r1 = Rotation(ctypes.c_double) r2 = Rotation(ctypes.c_float) # r2 should get converted into a double instance for checking nose.tools.assert_equal(r1, r2)
def _new(self, s, r, t): """ :type s: float :type r: Rotation | None :type t: collections.Iterable[float] | None """ # noinspection PyProtectedMember if r is None: r = Rotation(self._ctype) elif r._ctype != self._ctype: # Create new Rotation sharing our type r = Rotation.from_quaternion(r.quaternion(), self._ctype) if t is None: t = EigenArray.from_iterable((0, 0, 0), self._ctype) else: t = EigenArray.from_iterable(t, self._ctype, (3, 1)) return self._call_cfunc('vital_similarity_%s_new' % self._tchar, [ self._ctype, Rotation.c_ptr_type(self._ctype), EigenArray.c_ptr_type(3, 1, self._ctype) ], [s, r, t], self.C_TYPE_PTR)
def test_eq(self): # Identities should equal r1 = Rotation('d') r2 = Rotation('d') nose.tools.assert_equal(r1, r2) r1 = Rotation('f') r2 = Rotation('f') nose.tools.assert_equal(r1, r2) r1 = Rotation('d') r2 = Rotation('f') # r2 should get converted into a double instance for checking nose.tools.assert_equal(r1, r2) r1 = Rotation.from_quaternion([1, 2, 3, 4], ctype='d') r2 = Rotation.from_quaternion([1, 2, 3, 4], ctype='d') nose.tools.assert_equal(r1, r2) r1 = Rotation.from_quaternion([1, 2, 3, 4], ctype='d') r2 = Rotation.from_quaternion([-1, -2, -3, -4], ctype='d') assert r1.angle_from(r2) < 1e-12
def test_get_rotation(self): s = Similarity() nose.tools.assert_equal(s.rotation, Rotation()) s = Similarity(self.s, self.r, self.t) nose.tools.assert_equal(s.rotation, self.r)
def test_new_default(self): s = Similarity() nose.tools.assert_equal(s.scale, 1) nose.tools.assert_equal(s.rotation, Rotation()) numpy.testing.assert_array_equal(s.translation, EigenArray.from_iterable((0, 0, 0)))
def test_new_default(self): s = Similarity() nose.tools.assert_equal(s.scale, 1) numpy.testing.assert_array_almost_equal(s.rotation.matrix(), Rotation().matrix()) numpy.testing.assert_array_equal(s.translation, [0, 0, 0])
def test_to_quaternion(self): rot_d = Rotation('d') numpy.testing.assert_array_equal(rot_d.quaternion(), [0, 0, 0, 1]) rot_f = Rotation('f') numpy.testing.assert_array_equal(rot_f.quaternion(), [0, 0, 0, 1])
def test_compose(self): # Normalize quaternaion vector. expected_quat = array_normalize([+2., -1., -3., +0.]) r_ident_d = Rotation('d') r_ident_f = Rotation('f') r_other_d = Rotation.from_quaternion(expected_quat, 'd') r_other_f = Rotation.from_quaternion(expected_quat, 'f') r_res_d = r_ident_d.compose(r_other_d) nose.tools.assert_is_not(r_other_d, r_res_d) numpy.testing.assert_equal(r_res_d, r_other_d) numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat) r_res_f = r_ident_f.compose(r_other_f) nose.tools.assert_is_not(r_other_f, r_res_f) numpy.testing.assert_equal(r_res_f, r_other_f) numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat, 1e-7) # Should also work with multiply operator r_res_d = r_ident_d * r_other_d nose.tools.assert_is_not(r_other_d, r_res_d) numpy.testing.assert_equal(r_res_d, r_other_d) numpy.testing.assert_equal(r_res_d.quaternion(), expected_quat) r_res_f = r_ident_f * r_other_f nose.tools.assert_is_not(r_other_f, r_res_f) numpy.testing.assert_equal(r_res_f, r_other_f) numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat, 1e-7) # Rotation of non-congruent types should be converted automatically r_res_d = r_ident_d.compose(r_other_f) nose.tools.assert_is_not(r_res_d, r_other_f) numpy.testing.assert_allclose(r_res_d.quaternion(), r_other_f.quaternion(), 1e-7) numpy.testing.assert_allclose(r_res_d.quaternion(), expected_quat, 1e-7) r_res_f = r_ident_f.compose(r_other_d) nose.tools.assert_is_not(r_res_f, r_other_f) numpy.testing.assert_allclose(r_res_f.quaternion(), r_other_f.quaternion(), 1e-7) numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat, 1e-7) # Equality check between types should pass due to integrety resolution # inside function. r_res_d = r_ident_d * r_other_f nose.tools.assert_is_not(r_res_d, r_other_f) numpy.testing.assert_allclose(r_res_d.quaternion(), r_other_f.quaternion(), 1e-7) numpy.testing.assert_allclose(r_res_d.quaternion(), expected_quat, 1e-7) r_res_f = r_ident_f * r_other_d nose.tools.assert_is_not(r_res_f, r_other_f) numpy.testing.assert_equal(r_res_f.quaternion(), r_other_f.quaternion()) numpy.testing.assert_allclose(r_res_f.quaternion(), expected_quat, 1e-7)