def test_transform(self):
transform = mut.Isometry3()
X = np.eye(4, 4)
self.assertTrue(np.allclose(transform.matrix(), X))
self.assertEquals(str(transform), str(X))
R = np.array([[0., 1, 0], [-1, 0, 0], [0, 0, 1]])
p = np.array([1., 2, 3])
X = np.vstack((np.hstack((R, p.reshape((-1, 1)))), [0, 0, 0, 1]))
transform = mut.Isometry3(rotation=R, translation=p)
self.assertTrue(np.allclose(transform.matrix(), X))
self.assertTrue(np.allclose(transform.translation(), p))
transform.set_translation(-p)
self.assertTrue(np.allclose(transform.translation(), -p))
self.assertTrue(np.allclose(transform.rotation(), R))
transform.set_rotation(R.T)
self.assertTrue(np.allclose(transform.rotation(), R.T))
# - Check transactions for bad values.
transform = mut.Isometry3(rotation=R, translation=p)
R_bad = np.copy(R)
R_bad[0, 0] = 10.
with self.assertRaises(RuntimeError):
transform.set_rotation(R_bad)
self.assertTrue(np.allclose(R, transform.rotation()))
X_bad = np.copy(X)
X_bad[:3, :3] = R_bad
with self.assertRaises(RuntimeError):
transform.set_matrix(X_bad)
self.assertTrue(np.allclose(X, transform.matrix()))
# Test `type_caster`s.
value = test_util.create_isometry()
self.assertTrue(isinstance(value, mut.Isometry3))
test_util.check_isometry(value)
def test_transform(self):
# - Default constructor
transform = mut.Isometry3()
X = np.eye(4, 4)
self.assertTrue(np.allclose(transform.matrix(), X))
self.assertEqual(str(transform), str(X))
# - Constructor with (X)
transform = mut.Isometry3(matrix=X)
self.assertTrue(np.allclose(transform.matrix(), X))
# - Copy constructor.
cp = mut.Isometry3(other=transform)
self.assertTrue(np.allclose(transform.matrix(), cp.matrix()))
# - Identity
transform = mut.Isometry3.Identity()
self.assertTrue(np.allclose(transform.matrix(), X))
# - Constructor with (R, p)
R = np.array([
[0., 1, 0],
[-1, 0, 0],
[0, 0, 1]])
p = np.array([1., 2, 3])
X = np.vstack((np.hstack((R, p.reshape((-1, 1)))), [0, 0, 0, 1]))
transform = mut.Isometry3(rotation=R, translation=p)
self.assertTrue(np.allclose(transform.matrix(), X))
self.assertTrue(np.allclose(transform.translation(), p))
transform.set_translation(-p)
self.assertTrue(np.allclose(transform.translation(), -p))
self.assertTrue(np.allclose(transform.rotation(), R))
transform.set_rotation(R.T)
self.assertTrue(np.allclose(transform.rotation(), R.T))
# - Check transactions for bad values.
transform = mut.Isometry3(rotation=R, translation=p)
R_bad = np.copy(R)
R_bad[0, 0] = 10.
with self.assertRaises(RuntimeError):
transform.set_rotation(R_bad)
self.assertTrue(np.allclose(R, transform.rotation()))
X_bad = np.copy(X)
X_bad[:3, :3] = R_bad
with self.assertRaises(RuntimeError):
transform.set_matrix(X_bad)
self.assertTrue(np.allclose(X, transform.matrix()))
# Test `type_caster`s.
value = test_util.create_isometry()
self.assertTrue(isinstance(value, mut.Isometry3))
test_util.check_isometry(value)
# Operations.
transform = mut.Isometry3(rotation=R, translation=p)
transform_I = transform.inverse().multiply(transform)
self.assertTrue(np.allclose(transform_I.matrix(), np.eye(4)))
self.assertTrue((
transform.multiply(position=[10, 20, 30]) == [21, -8, 33]).all())