def test_transform_bad_input():
"""
Test the plane transform for bad inputs.
"""
transform = Transform()
with pytest.raises(TypeError):
transform.transform_point(5)
def test_bad_initialization():
"""
bad initializers.
"""
with pytest.raises(TypeError):
Transform (rotation = 5)
with pytest.raises(TypeError):
Transform (translation=[1, 2])
def test_default_initialization():
"""
Test default initialization (Null transform)
"""
transform = Transform()
np.testing.assert_array_equal([0, 0, 0], transform._rotation)
np.testing.assert_array_equal([0, 0, 0], transform._translation)
assert np.array_equal(np.identity(4), transform._matrix)
assert np.array_equal([1, 2, 3], transform.transform_point([1, 2, 3]))
def test_transform_plane():
"""
Test transfroming a simple plane.
"""
rot_90_x = [np.pi/2, 0, 0]
trans = [1, 2, -1]
transform = Transform(translation = trans, rotation = rot_90_x)
plane = Plane()
new_plane = transform.transform_plane(plane)
np.testing.assert_array_almost_equal(trans, new_plane.point)
np.testing.assert_array_almost_equal([0, -1, 0], new_plane.normal)
def test_simple_transforms():
"""
test translation without rotation.
"""
rot_90_x = [np.pi/2, 0, 0]
trans = [1, 2, -1]
point = [1, 1, 0]
trans_tran_only = Transform(translation = trans)
trans_rot_only = Transform(rotation = rot_90_x)
trans_full = Transform(translation = trans, rotation = rot_90_x)
assert np.array_equal([2, 3, -1], trans_tran_only.transform_point(point))
np.testing.assert_array_almost_equal([1, 0, 1], trans_rot_only.transform_point(point))
np.testing.assert_array_almost_equal([2, 2, 0], trans_full.transform_point(point))
def point_residuals(params, points_3d, points_2d):
"""
Get residuals of projected 3d points, compared to the imaged points (two points for each points)
"""
cam_matrix, distortion, rvec, tvec = unpack_params(params)
undistort = Undistort(cam_matrix, distortion)
transform = Transform(rotation=rvec, translation=tvec)
residuals = np.array([
get_projected(point_3d, undistort, transform) - point_2d
for point_3d, point_2d in zip(points_3d, points_2d)
])
return residuals.ravel()
def test_get_projected():
"""
Test the projection function again opencv
"""
rvec=np.array(initial_R, dtype="float32")
tvec=np.array(initial_t, dtype="float32")
undistorter=Undistort(initial_camera_matrix, initial_distortion)
transformer=Transform(rvec, tvec)
_, translations, points_2d = generate_dataset()
for point_2d, point_3d in zip (points_2d, translations):
point = sc.get_projected(point_3d, undistorter, transformer)
np.testing.assert_array_almost_equal( point_2d, point, decimal=4 )
def test_combine_transform():
"""
Test combining transforms
"""
point = np.array([1, 2, 3])
rot_90_x = np.array([np.pi/2, 0, 0])
trans = np.array([1, 2, -1])
transform1 = Transform(translation = trans, rotation = rot_90_x)
transform2 = Transform(translation = trans+1, rotation = rot_90_x)
trans_combine = transform1.combine(transform2)
p1 = transform1.transform_point(transform2.transform_point(point))
p2 = trans_combine.transform_point(point)
np.testing.assert_array_almost_equal(p1, p2)
def generate_dataset():
"""
Generate the 3d points, translations of the stage to get there and 2d point correspondences.
"""
initial_point = np.array([0, 0, 0])
initial_U = Undistort(initial_camera_matrix, initial_distortion)
initial_T = Transform (rotation=initial_R, translation=initial_t)
#distance: 1.5 cm, FOV Width: (0.17920477879410118, 0.10080268807168191) cm
#distance: 2 cm, FOV Width: (0.23893970505880158, 0.13440358409557587) cm
#distance: 2.5 cm, FOV Width: (0.29867463132350197, 0.16800448011946983) cm
distances = np.array([1.5, 2.0, 2.5]) - initial_t[2]
width_x = [0.175, 0.23, 0.29]
width_y = [0.10, 0.13, 0.16]
translations = []
for d, w, h in zip(distances, width_x, width_y):
for x in np.arange(-w, w, w/10):
for y in np.arange(-h, h, h/10):
translations.append(np.array([x, y, d]))
points_3d = []
points_2d = []
trans_out = []
for t in translations:
T = Transform(translation=t)
point_3d = initial_T.transform_point(T.transform_point(initial_point))
point_2d = initial_U.project_point_with_distortion(point_3d)
if np.abs(point_2d[0]) > initial_camera_matrix[0, 2] or np.abs(point_2d[1]) > initial_camera_matrix[1, 2]:
continue
points_3d.append(point_3d)
points_2d.append(point_2d)
trans_out.append(t)
return (points_3d, trans_out, points_2d)