def test_form_near_clipping_plane() -> None:
"""Test form_near_clipping_plane(). Use 4 points to fit the near clipping plane."""
img_width = 10.0
img_height = 15.0
near_clip_dist = 30.0
near_plane = form_near_clipping_plane(near_clip_dist)
near = np.array([
[img_width / 2, 0, near_clip_dist],
[-img_width / 2, 0, near_clip_dist],
[img_width / 2, -img_height / 2.0, near_clip_dist],
[img_width / 2, img_height / 2.0, near_clip_dist],
])
a, b, c, d = fit_plane_to_point_cloud(near)
near_plane_gt = np.array([a, b, c, d])
assert np.allclose(near_plane, near_plane_gt)
def test_form_left_clipping_plane() -> None:
"""Test form_left_clipping_plane(). Use 4 points to fit the left clipping plane."""
fx = 10.0
img_width = 30.0
left_plane = form_left_clipping_plane(fx, img_width)
Y_OFFSET = 10
left = np.array([[0, 0, 0], [-img_width / 2.0, 0, fx], [0, Y_OFFSET, 0],
[-img_width / 2.0, Y_OFFSET, fx]])
a, b, c, d = fit_plane_to_point_cloud(left)
left_plane_gt = -1 * np.array([a, b, c, d])
# enforce that plane normal points into the frustum
if left_plane_gt[0] < 0:
left_plane_gt *= -1
assert np.allclose(left_plane, left_plane_gt)
def test_form_top_clipping_plane() -> None:
"""Test form_top_clipping_plane(). Use 3 points to fit the TOP clipping plane."""
fx = 10.0
img_height = 45.0
top_plane = form_top_clipping_plane(fx, img_height)
img_width = 1000.0
top_pts = np.array([[0, 0, 0], [-img_width / 2, -img_height / 2, fx],
[img_width / 2, -img_height / 2, fx]])
a, b, c, d = fit_plane_to_point_cloud(top_pts)
top_plane_gt = np.array([a, b, c, d])
# enforce that plane normal points into the frustum
if top_plane_gt[1] < 0:
# y-coord of normal should point in pos y-axis dir(down) on top-clipping plane
top_plane_gt *= -1
assert top_plane_gt[1] > 0 and top_plane_gt[2] > 0
assert np.allclose(top_plane, top_plane_gt)
def test_form_right_clipping_plane() -> None:
"""Test form_right_clipping_plane(). Use 4 points to fit the right clipping plane."""
fx = 10.0
img_width = 30.0
right_plane = form_right_clipping_plane(fx, img_width)
Y_OFFSET = 10 # arbitrary extent down the imager
right = np.array([[0, 0, 0], [img_width / 2.0, 0, fx], [0, Y_OFFSET, 0],
[img_width / 2.0, Y_OFFSET, fx]])
a, b, c, d = fit_plane_to_point_cloud(right)
right_plane_gt = np.array([a, b, c, d])
# enforce that plane normal points into the frustum
# x-component of normal should point in negative direction.
if right_plane_gt[0] > 0:
right_plane_gt *= -1
assert np.allclose(right_plane, right_plane_gt)
def test_form_low_clipping_plane() -> None:
"""Test form_low_clipping_plane()."""
fx = 12.0
img_height = 35.0
low_plane = form_low_clipping_plane(fx, img_height)
img_width = 10000
low_pts = np.array([[0, 0, 0], [-img_width / 2, img_height / 2, fx],
[img_width / 2, img_height / 2, fx]])
a, b, c, d = fit_plane_to_point_cloud(low_pts)
low_plane_gt = np.array([a, b, c, d])
# enforce that plane normal points into the frustum
# y-coord of normal should point in neg y-axis dir(up) on low-clipping plane
# z-coord should point in positive z-axis direction (away from camera)
if low_plane_gt[1] > 0:
low_plane_gt *= -1
assert low_plane_gt[1] < 0 and low_plane_gt[2] > 0
assert np.allclose(low_plane, low_plane_gt)
def test_fit_plane_to_point_cloud() -> None:
"""Given a plane with slope +2/1 for +y/+z, find slowly tilting normal away from the plane.
+y
/|
/ |
/ |
------ + z
"""
pc = np.array([[0, 2, 1], [1, 2, 1], [0, 0, 0]])
a, b, c, d = fit_plane_to_point_cloud(pc)
assert d == 0 # touching origin
normal = np.array([a, b, c])
normal /= np.linalg.norm(normal)
gt_normal = np.array([0.0, -1.0, 2.0])
gt_normal /= np.linalg.norm(gt_normal)
# correct y sign if needed, slope is all what matters
# (ratio between +y/+z should be -1/2)
if normal[1] > 0:
normal *= -1
assert np.allclose(gt_normal, normal)
