def test_transform_not_added():
"""Test request for transforms that have not been added."""
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
C2D = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
tm.add_transform("C", "D", C2D)
assert_raises_regexp(KeyError, "Unknown frame", tm.get_transform, "A", "G")
assert_raises_regexp(KeyError, "Unknown frame", tm.get_transform, "G", "D")
assert_raises_regexp(KeyError, "Cannot compute path", tm.get_transform,
"A", "D")
def test_update_transform():
"""Update an existing transform."""
random_state = np.random.RandomState(0)
A2B1 = random_transform(random_state)
A2B2 = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B1)
tm.add_transform("A", "B", A2B2)
A2B = tm.get_transform("A", "B")
# Hack: test depends on internal member
assert_array_almost_equal(A2B, A2B2)
assert_equal(len(tm.i), 1)
assert_equal(len(tm.j), 1)
def test_request_added_transform():
"""Request an added transform from the transform manager."""
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
A2B_2 = tm.get_transform("A", "B")
assert_array_almost_equal(A2B, A2B_2)
def test_whitelist():
"""Test correct handling of whitelists for plotting."""
random_state = np.random.RandomState(2)
A2B = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
nodes = tm._whitelisted_nodes(None)
assert_equal(set(["A", "B"]), nodes)
nodes = tm._whitelisted_nodes("A")
assert_equal(set(["A"]), nodes)
assert_raises_regexp(KeyError, "unknown nodes", tm._whitelisted_nodes, "C")
def test_check_consistency():
"""Test correct detection of inconsistent graphs."""
random_state = np.random.RandomState(2)
tm = TransformManager()
A2B = random_transform(random_state)
tm.add_transform("A", "B", A2B)
B2A = random_transform(random_state)
tm.add_transform("B", "A", B2A)
assert_false(tm.check_consistency())
tm = TransformManager()
A2B = random_transform(random_state)
tm.add_transform("A", "B", A2B)
assert_true(tm.check_consistency())
C2D = random_transform(random_state)
tm.add_transform("C", "D", C2D)
assert_true(tm.check_consistency())
B2C = random_transform(random_state)
tm.add_transform("B", "C", B2C)
assert_true(tm.check_consistency())
A2D_over_path = tm.get_transform("A", "D")
A2D = random_transform(random_state)
tm.add_transform("A", "D", A2D)
assert_false(tm.check_consistency())
tm.add_transform("A", "D", A2D_over_path)
assert_true(tm.check_consistency())
def test_request_concatenated_transform():
"""Request a concatenated transform from the transform manager."""
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
B2C = random_transform(random_state)
F2A = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
tm.add_transform("B", "C", B2C)
tm.add_transform("D", "E", np.eye(4))
tm.add_transform("F", "A", F2A)
A2C = tm.get_transform("A", "C")
assert_array_almost_equal(A2C, concat(A2B, B2C))
C2A = tm.get_transform("C", "A")
assert_array_almost_equal(C2A, concat(invert_transform(B2C),
invert_transform(A2B)))
F2B = tm.get_transform("F", "B")
assert_array_almost_equal(F2B, concat(F2A, A2B))
def test_concat():
"""Test concatenation of transforms."""
random_state = np.random.RandomState(0)
for _ in range(5):
A2B = random_transform(random_state)
assert_transform(A2B)
B2C = random_transform(random_state)
assert_transform(B2C)
A2C = concat(A2B, B2C)
assert_transform(A2C)
p_A = np.array([0.3, -0.2, 0.9, 1.0])
p_C = transform(A2C, p_A)
C2A = invert_transform(A2C)
p_A2 = transform(C2A, p_C)
assert_array_almost_equal(p_A, p_A2)
C2A2 = concat(invert_transform(B2C), invert_transform(A2B))
p_A3 = transform(C2A2, p_C)
assert_array_almost_equal(p_A, p_A3)
def test_transform():
"""Test transformation of points."""
PA = np.array([[1, 2, 3, 1], [2, 3, 4, 1]])
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
PB = transform(A2B, PA)
p0B = transform(A2B, PA[0])
p1B = transform(A2B, PA[1])
assert_array_almost_equal(PB, np.array([p0B, p1B]))
assert_raises_regexp(ValueError,
"Cannot transform array with more than 2 dimensions",
transform, A2B, np.zeros((2, 2, 4)))
def test_pickle():
"""Test if a transform manager can be pickled."""
random_state = np.random.RandomState(1)
A2B = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "B", A2B)
_, filename = tempfile.mkstemp(".pickle")
try:
pickle.dump(tm, open(filename, "wb"))
tm2 = pickle.load(open(filename, "rb"))
finally:
if os.path.exists(filename):
try:
os.remove(filename)
except WindowsError:
pass # workaround for permission problem on Windows
A2B2 = tm2.get_transform("A", "B")
assert_array_almost_equal(A2B, A2B2)
def test_check_transform():
"""Test input validation for transformation matrix."""
A2B = np.eye(3)
assert_raises_regexp(
ValueError, "Expected homogeneous transformation matrix with shape",
check_transform, A2B)
A2B = np.eye(4, dtype=int)
A2B = check_transform(A2B)
assert_equal(type(A2B), np.ndarray)
assert_equal(A2B.dtype, np.float)
A2B[:3, :3] = np.array([[1, 1, 1], [0, 0, 0], [2, 2, 2]])
assert_raises_regexp(ValueError, "rotation matrix", check_transform, A2B)
A2B = np.eye(4)
A2B[3, :] = np.array([0.1, 0.0, 0.0, 1.0])
assert_raises_regexp(ValueError, "homogeneous transformation matrix",
check_transform, A2B)
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
A2B2 = check_transform(A2B)
assert_array_almost_equal(A2B, A2B2)
def test_scale_transform():
"""Test scaling of transforms."""
random_state = np.random.RandomState(0)
A2B = random_transform(random_state)
A2B_scaled1 = scale_transform(A2B, s_xt=0.5, s_yt=0.5, s_zt=0.5)
A2B_scaled2 = scale_transform(A2B, s_t=0.5)
assert_array_almost_equal(A2B_scaled1, A2B_scaled2)
A2B_scaled1 = scale_transform(A2B, s_xt=0.5, s_yt=0.5, s_zt=0.5, s_r=0.5)
A2B_scaled2 = scale_transform(A2B, s_t=0.5, s_r=0.5)
A2B_scaled3 = scale_transform(A2B, s_d=0.5)
assert_array_almost_equal(A2B_scaled1, A2B_scaled2)
assert_array_almost_equal(A2B_scaled1, A2B_scaled3)
A2B_scaled = scale_transform(A2B, s_xr=0.0)
a_scaled = axis_angle_from_matrix(A2B_scaled[:3, :3])
assert_array_almost_equal(a_scaled[0], 0.0)
A2B_scaled = scale_transform(A2B, s_yr=0.0)
a_scaled = axis_angle_from_matrix(A2B_scaled[:3, :3])
assert_array_almost_equal(a_scaled[1], 0.0)
A2B_scaled = scale_transform(A2B, s_zr=0.0)
a_scaled = axis_angle_from_matrix(A2B_scaled[:3, :3])
assert_array_almost_equal(a_scaled[2], 0.0)
======
In this example, we will demonstrate how to use the TransformManager.
We will add several transforms to the manager and plot all frames in
two reference frames ('world' and 'A').
"""
print(__doc__)
import numpy as np
import matplotlib.pyplot as plt
from pytransform.plot_utils import make_3d_axis
from pytransform.transformations import random_transform
from pytransform.transform_manager import TransformManager
random_state = np.random.RandomState(0)
A2world = random_transform(random_state)
B2world = random_transform(random_state)
A2C = random_transform(random_state)
D2B = random_transform(random_state)
tm = TransformManager()
tm.add_transform("A", "world", A2world)
tm.add_transform("B", "world", B2world)
tm.add_transform("A", "C", A2C)
tm.add_transform("D", "B", D2B)
plt.figure(figsize=(10, 5))
ax = make_3d_axis(3, 121)
ax = tm.plot_frames_in("world", ax=ax, alpha=0.6)
ax.view_init(30, 20)