def test_project_points_invalid_as_input_null():
with pytest.raises(ValueError):
pu.project_points(None, None, None, None)
with pytest.raises(ValueError):
pu.project_points(np.zeros([4, 3]), None, None, None)
with pytest.raises(ValueError):
pu.project_points(np.zeros([4, 3]), np.zeros([4, 4]), None, None)
def test_camera_projection(setup_vtk_overlay_window):
vtk_overlay, factory, vtk_std_err, app = setup_vtk_overlay_window
# See data:
# chessboard_14_10_3_no_ID.txt - 3D chessboard coordinates
# left-1095.png - image taken of chessboard
# left-1095.png.points.txt - detected 2D image points
# calib.intrinsic.txt - top 3x3 matrix are intrinsic parameters
# left-1095.extrinsic.txt - model to camera matrix, a.k.a camera extrinsics, a.k.a pose
# Load 3D points
model_points_file = 'tests/data/calibration/chessboard_14_10_3_no_ID.txt'
model_points = np.loadtxt(model_points_file)
number_model_points = model_points.shape[0]
model_polydata = [sm.VTKSurfaceModel('tests/data/calibration/chessboard_14_10_3.vtk', (1.0, 1.0, 0.0))]
# Load images
left_image = cv2.imread('tests/data/calibration/left-1095-undistorted.png')
left_mask = cv2.imread('tests/data/calibration/left-1095-undistorted-mask.png')
left_mask = cv2.cvtColor(left_mask, cv2.COLOR_RGB2GRAY)
# Load 2D points
image_points_file ='tests/data/calibration/left-1095-undistorted.png.points.txt'
image_points = np.loadtxt(image_points_file)
number_image_points = image_points.shape[0]
# Load intrinsics for projection matrix.
intrinsics_file = 'tests/data/calibration/calib.left.intrinsic.txt'
intrinsics = np.loadtxt(intrinsics_file)
# Load extrinsics for camera pose (position, orientation).
extrinsics_file = 'tests/data/calibration/left-1095.extrinsic.txt'
extrinsics = np.loadtxt(extrinsics_file)
# OpenCV maps from chessboard to camera.
# Assume chessboard == world, so the input matrix is world_to_camera.
# We need camera_to_world to position the camera in world coordinates.
# So, invert it.
camera_to_world = np.linalg.inv(extrinsics)
assert number_model_points == 140
assert number_image_points == 140
assert len(image_points) == 140
assert len(model_points) == 140
screen = app.primaryScreen()
width = left_image.shape[1]
height = left_image.shape[0]
while width >= screen.geometry().width() or height >= screen.geometry().height():
width //= 2
height //= 2
vtk_overlay.add_vtk_models(model_polydata)
vtk_overlay.set_video_image(left_image)
vtk_overlay.set_camera_pose(camera_to_world)
vtk_overlay.resize(width, height)
vtk_overlay.show()
vtk_renderer = vtk_overlay.get_foreground_renderer()
vtk_camera = vtk_overlay.get_foreground_camera()
vtk_renderwindow_size = vtk_overlay.GetRenderWindow().GetSize()
# Wierdly, vtkRenderWindow, sometimes seems to use the wrong resolution,
# like its trying to render at high resolution, maybe for anti-aliasing or averaging?
scale_x = left_image.shape[1] / vtk_renderwindow_size[0]
scale_y = left_image.shape[0] / vtk_renderwindow_size[1]
# Print out some debug. On some displays, the widget size and the size of the vtkRenderWindow don't match.
six.print_('Left image = (' + str(left_image.shape[1]) + ', ' + str(left_image.shape[0]) + ')')
six.print_('Chosen size = (' + str(width) + ', ' + str(height) + ')')
six.print_('Render window = ' + str(vtk_overlay.GetRenderWindow().GetSize()))
six.print_('Widget = (' + str(vtk_overlay.width()) + ', ' + str(vtk_overlay.height()) + ')')
six.print_('Viewport = ' + str(vtk_renderer.GetViewport()))
six.print_('Scale = ' + str(scale_x) + ', ' + str(scale_y))
# Set intrisic parameters, which internally sets vtkCamera vars.
vtk_overlay.set_camera_matrix(intrinsics)
# Compute the rms error, using a vtkCoordinate loop, which is slow.
rms = pu.compute_rms_error(model_points,
image_points,
vtk_renderer,
scale_x,
scale_y,
left_image.shape[0]
)
six.print_('rms using VTK =' + str(rms))
# Now check the rms error, using an OpenCV projection, which should be faster.
projected_points = pu.project_points(model_points,
camera_to_world,
intrinsics
)
counter = 0
rms_opencv = 0
for counter in range(0, number_model_points):
i_c = image_points[counter]
dx = projected_points[counter][0][0] - float(i_c[0])
dy = projected_points[counter][0][1] - float(i_c[1])
rms_opencv += (dx * dx + dy * dy)
counter += 1
rms_opencv /= float(counter)
rms_opencv = np.sqrt(rms_opencv)
six.print_('rms using OpenCV =' + str(rms_opencv))
assert rms < 1.2
assert rms_opencv < 0.7
model_polydata_points = model_polydata[0].get_points_as_numpy()
model_polydata_normals = model_polydata[0].get_normals_as_numpy()
six.print_('model_points=' + str(model_polydata_points))
projected_facing_points = pu.project_facing_points(model_polydata_points,
model_polydata_normals,
camera_to_world,
intrinsics
)
assert projected_facing_points.shape[0] == 4
assert projected_facing_points.shape[2] == 2
# Can't think how to do this more efficiently yet.
masked = []
for point_index in range(projected_facing_points.shape[0]):
x = projected_facing_points[point_index][0][0]
y = projected_facing_points[point_index][0][1]
val = left_mask[int(y), int(x)]
six.print_('p=' + str(x) + ', ' + str(y))
if int(x) >= 0 \
and int(x) < left_mask.shape[1] \
and int(y) >= 0 \
and int(y) < left_mask.shape[0] \
and val > 0:
masked.append((x, y))
assert len(masked) == 2
def test_stereo_overlay_window(vtk_interlaced_stereo_window):
widget, _, app = vtk_interlaced_stereo_window
model_points_file = 'tests/data/calibration/chessboard_14_10_3_no_ID.txt'
model_points = np.loadtxt(model_points_file)
number_model_points = model_points.shape[0]
assert number_model_points == 140
# Load images
left_image = cv2.imread('tests/data/calibration/left-1095-undistorted.png')
right_image = cv2.imread('tests/data/calibration/right-1095-undistorted.png')
# Load left intrinsics for projection matrix.
left_intrinsics_file = 'tests/data/calibration/calib.left.intrinsic.txt'
left_intrinsics = np.loadtxt(left_intrinsics_file)
# Load right intrinsics for projection matrix.
right_intrinsics_file = 'tests/data/calibration/calib.right.intrinsic.txt'
right_intrinsics = np.loadtxt(right_intrinsics_file)
# Load 2D points
image_points_file ='tests/data/calibration/right-1095-undistorted.png.points.txt'
image_points = np.loadtxt(image_points_file)
number_image_points = image_points.shape[0]
assert number_model_points == number_image_points
# Load extrinsics for camera pose (position, orientation).
extrinsics_file = 'tests/data/calibration/left-1095.extrinsic.txt'
extrinsics = np.loadtxt(extrinsics_file)
left_camera_to_world = np.linalg.inv(extrinsics)
# Load extrinsics for stereo.
stereo_extrinsics_file = 'tests/data/calibration/calib.l2r.4x4'
stereo_extrinsics = np.loadtxt(stereo_extrinsics_file)
screen = app.primaryScreen()
width = left_image.shape[1]
height = left_image.shape[0]
while width >= screen.geometry().width() or height >= screen.geometry().height():
width /= 2
height /= 2
# Create a vtk point model.
vtk_points = vtk_point_model.VTKPointModel(model_points.astype(np.float),
model_points.astype(np.byte))
widget.add_vtk_actor(vtk_points.actor)
widget.set_video_images(left_image, right_image)
widget.set_camera_matrices(left_intrinsics, right_intrinsics)
widget.set_left_to_right(stereo_extrinsics)
widget.set_camera_poses(left_camera_to_world)
widget.resize(width, height)
widget.show()
widget.set_current_viewer_index(0)
widget.set_current_viewer_index(1)
widget.set_current_viewer_index(2)
widget.render()
six.print_('Chosen size = (' + str(width) + 'x' + str(height) + ')')
six.print_('Left image = :' + str(left_image.shape))
six.print_('Right image = :' + str(right_image.shape))
six.print_('Widget = (' + str(widget.width()) + ', ' + str(widget.height()) + ')')
# Project points using OpenCV.
right_camera_to_world = cam.compute_right_camera_pose(left_camera_to_world, stereo_extrinsics)
right_points = pu.project_points(model_points,
right_camera_to_world,
right_intrinsics
)
rms_opencv = 0
for counter in range(0, number_model_points):
i_c = image_points[counter]
dx = right_points[counter][0][0] - i_c[0]
dy = right_points[counter][0][1] - i_c[1]
rms_opencv += (dx * dx + dy * dy)
rms_opencv /= number_model_points
rms_opencv = np.sqrt(rms_opencv)
assert rms_opencv < 1
widget.save_scene_to_file('tests/output/test_interlaced_stereo_window.png')
#app.exec_()
def test_project_points_invalid_as_distortion_wrong_number_rows():
with pytest.raises(ValueError):
pu.project_points(np.zeros([4, 3]), np.eye(4), np.eye(3),
np.zeros([2, 4]))
def test_project_points_invalid_as_intrinsics_wrong_size():
with pytest.raises(ValueError):
pu.project_points(np.zeros([4, 3]), np.zeros([4, 4]), np.eye(2),
np.zeros([1, 4]))
def test_project_points_invalid_as_extrinsics_wrong_number_columns():
with pytest.raises(ValueError):
pu.project_points(np.zeros([4, 3]), np.zeros([4, 5]), np.eye(3), None)
def test_project_points_invalid_as_extrinsics_not_2d():
with pytest.raises(ValueError):
pu.project_points(np.zeros([4, 3]), np.zeros([4, 4, 4]), np.eye(3),
None)
def test_project_points_invalid_as_extrinsics_not_numpy_array():
with pytest.raises(TypeError):
pu.project_points(np.zeros([4, 3]), "invalid", np.eye(3), None)
def test_project_points_invalid_as_points_not_3_columns():
with pytest.raises(ValueError):
pu.project_points(np.zeros([4, 4]), np.zeros([4, 4]), np.eye(3), None)