def test_isometry3(self):
# - Default constructor
transform = mut.Isometry3()
X = np.eye(4, 4)
self.assertTrue(np.allclose(transform.matrix(), X))
self.assertTrue(np.allclose(copy.copy(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())
if six.PY3:
self.assertTrue(np.allclose(
eval("transform.inverse() @ transform").matrix(), np.eye(4)))
self.assertTrue((
eval("transform @ [10, 20, 30]") == [21, -8, 33]).all())
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(SystemExit):
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(SystemExit):
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())
def test_isometry3(self, T):
Isometry3 = mut.Isometry3_[T]
# - Default constructor
transform = Isometry3()
self.assertEqual(numpy_compare.resolve_type(transform.matrix()), T)
X_I_np = np.eye(4, 4)
numpy_compare.assert_float_equal(transform.matrix(), X_I_np)
numpy_compare.assert_float_equal(copy.copy(transform).matrix(), X_I_np)
if T == float:
self.assertEqual(str(transform), str(X_I_np))
# - Constructor with (X_I_np)
transform = Isometry3(matrix=X_I_np)
numpy_compare.assert_float_equal(transform.matrix(), X_I_np)
# - Copy constructor.
cp = Isometry3(other=transform)
numpy_compare.assert_equal(transform.matrix(), cp.matrix())
# - Identity
transform = Isometry3.Identity()
numpy_compare.assert_float_equal(transform.matrix(), X_I_np)
# - Constructor with (R, p)
R_AB = np.array([[0., 1, 0], [-1, 0, 0], [0, 0, 1]])
p_AB = np.array([1., 2, 3])
X_AB_np = np.eye(4)
X_AB_np[:3, :3] = R_AB
X_AB_np[:3, 3] = p_AB
X_AB = Isometry3(rotation=R_AB, translation=p_AB)
numpy_compare.assert_float_equal(X_AB.matrix(), X_AB_np)
numpy_compare.assert_float_equal(X_AB.translation(), p_AB)
numpy_compare.assert_float_equal(X_AB.rotation(), R_AB)
# - Setters.
X_AB = Isometry3()
X_AB.set_translation(p_AB)
numpy_compare.assert_float_equal(X_AB.translation(), p_AB)
X_AB.set_rotation(R_AB)
numpy_compare.assert_float_equal(X_AB.rotation(), R_AB)
# - Cast
self.check_cast(mut.Isometry3_, T)
# - Check transactions for bad values.
if T != Expression:
X_temp = Isometry3(rotation=R_AB, translation=p_AB)
R_bad = np.copy(R_AB)
R_bad[0, 0] = 10.
with self.assertRaises(RuntimeError):
X_temp.set_rotation(R_bad)
numpy_compare.assert_float_equal(X_temp.rotation(), R_AB)
X_bad_np = np.copy(X_I_np)
X_bad_np[:3, :3] = R_bad
with self.assertRaises(RuntimeError):
X_temp.set_matrix(X_bad_np)
numpy_compare.assert_float_equal(X_temp.matrix(), X_AB_np)
# Test `type_caster`s.
if T == float:
value = test_util.create_isometry()
self.assertTrue(isinstance(value, mut.Isometry3))
test_util.check_isometry(value)
# Operations.
X_AB = Isometry3(rotation=R_AB, translation=p_AB)
X_I = X_AB.inverse().multiply(X_AB)
numpy_compare.assert_float_equal(X_I.matrix(), X_I_np)
p_BQ = [10, 20, 30]
p_AQ = [21., -8, 33]
numpy_compare.assert_float_equal(X_AB.multiply(position=p_BQ), p_AQ)
p_BQlist = np.array([p_BQ, p_BQ]).T
p_AQlist = np.array([p_AQ, p_AQ]).T
numpy_compare.assert_float_equal(X_AB.multiply(position=p_BQlist),
p_AQlist)
if six.PY3:
numpy_compare.assert_float_equal(
eval("X_AB.inverse() @ X_AB").matrix(), X_I_np)
numpy_compare.assert_float_equal(eval("X_AB @ p_BQ"), p_AQ)
def check_isometry3(self, T):
Isometry3 = mut.Isometry3_[T]
# - Default constructor
transform = Isometry3()
self.assertEqual(npc.resolve_type(transform.matrix()), T)
X = np.eye(4, 4)
npc.assert_float_equal(transform.matrix(), X)
npc.assert_float_equal(copy.copy(transform).matrix(), X)
if T == float:
self.assertEqual(str(transform), str(X))
# - Constructor with (X)
transform = Isometry3(matrix=X)
npc.assert_float_equal(transform.matrix(), X)
# - Copy constructor.
cp = Isometry3(other=transform)
npc.assert_equal(transform.matrix(), cp.matrix())
# - Identity
transform = Isometry3.Identity()
npc.assert_float_equal(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 = Isometry3(rotation=R, translation=p)
npc.assert_float_equal(transform.matrix(), X)
npc.assert_float_equal(transform.translation(), p)
transform.set_translation(-p)
npc.assert_float_equal(transform.translation(), -p)
npc.assert_float_equal(transform.rotation(), R)
transform.set_rotation(R.T)
npc.assert_float_equal(transform.rotation(), R.T)
# - Check transactions for bad values.
if T != Expression:
transform = Isometry3(rotation=R, translation=p)
R_bad = np.copy(R)
R_bad[0, 0] = 10.
with self.assertRaises(RuntimeError):
transform.set_rotation(R_bad)
npc.assert_float_equal(transform.rotation(), R)
X_bad = np.copy(X)
X_bad[:3, :3] = R_bad
with self.assertRaises(RuntimeError):
transform.set_matrix(X_bad)
npc.assert_float_equal(transform.matrix(), X)
# Test `type_caster`s.
if T == float:
value = test_util.create_isometry()
self.assertTrue(isinstance(value, mut.Isometry3))
test_util.check_isometry(value)
# Operations.
transform = Isometry3(rotation=R, translation=p)
transform_I = transform.inverse().multiply(transform)
npc.assert_float_equal(transform_I.matrix(), np.eye(4))
npc.assert_float_equal(
transform.multiply(position=[10, 20, 30]), [21., -8, 33])
if six.PY3:
npc.assert_float_equal(
eval("transform.inverse() @ transform").matrix(), np.eye(4))
npc.assert_float_equal(
eval("transform @ [10, 20, 30]"), [21., -8, 33])
def test_isometry3(self, T):
Isometry3 = mut.Isometry3_[T]
# - Default constructor
transform = Isometry3()
self.assertEqual(numpy_compare.resolve_type(transform.matrix()), T)
X = np.eye(4, 4)
numpy_compare.assert_float_equal(transform.matrix(), X)
numpy_compare.assert_float_equal(copy.copy(transform).matrix(), X)
if T == float:
self.assertEqual(str(transform), str(X))
# - Constructor with (X)
transform = Isometry3(matrix=X)
numpy_compare.assert_float_equal(transform.matrix(), X)
# - Copy constructor.
cp = Isometry3(other=transform)
numpy_compare.assert_equal(transform.matrix(), cp.matrix())
# - Identity
transform = Isometry3.Identity()
numpy_compare.assert_float_equal(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 = Isometry3(rotation=R, translation=p)
numpy_compare.assert_float_equal(transform.matrix(), X)
numpy_compare.assert_float_equal(transform.translation(), p)
transform.set_translation(-p)
numpy_compare.assert_float_equal(transform.translation(), -p)
numpy_compare.assert_float_equal(transform.rotation(), R)
transform.set_rotation(R.T)
numpy_compare.assert_float_equal(transform.rotation(), R.T)
# - Cast
self.check_cast(mut.Isometry3_, T)
# - Check transactions for bad values.
if T != Expression:
transform = Isometry3(rotation=R, translation=p)
R_bad = np.copy(R)
R_bad[0, 0] = 10.
with self.assertRaises(RuntimeError):
transform.set_rotation(R_bad)
numpy_compare.assert_float_equal(transform.rotation(), R)
X_bad = np.copy(X)
X_bad[:3, :3] = R_bad
with self.assertRaises(RuntimeError):
transform.set_matrix(X_bad)
numpy_compare.assert_float_equal(transform.matrix(), X)
# Test `type_caster`s.
if T == float:
value = test_util.create_isometry()
self.assertTrue(isinstance(value, mut.Isometry3))
test_util.check_isometry(value)
# Operations.
transform = Isometry3(rotation=R, translation=p)
transform_I = transform.inverse().multiply(transform)
numpy_compare.assert_float_equal(transform_I.matrix(), np.eye(4))
numpy_compare.assert_float_equal(
transform.multiply(position=[10, 20, 30]), [21., -8, 33])
if six.PY3:
numpy_compare.assert_float_equal(
eval("transform.inverse() @ transform").matrix(), np.eye(4))
numpy_compare.assert_float_equal(
eval("transform @ [10, 20, 30]"), [21., -8, 33])
