def projector_planes(proj):
"""
Function: projector_planes
Generates the planes representing each row and column of the projector
Parameters:
proj - *<DLPProjector>* The projector to generate the planes of
Returns:
*Tuple* in the form (vertical_planes, horizontal_planes). row_planes will have shape
(width, 4) and col_planes will have shape (height, 4). Each plane is defined
by the coefficients [a, b, c, d], where a*x + b*y + c*z = d
"""
# generate pixel rays in the projector frame
proj_pixels_proj_frame = pixel_rays(proj)
from nose.tools import set_trace; set_trace()
# translate and rotate the rays into the global frame
proj_pixels = to_global_frame(proj_pixels_proj_frame, proj)
# get projector location in the global frame
proj_pose = to_global_frame([0, 0, 0], proj)
# add projector location to the vertical points
vertical_shape = (proj.resolution[0], 1, 3)
proj_points_vertical = np.concatenate((proj_pixels, np.ones(vertical_shape) * proj_pose), axis=1)
# calculate the vertical planes
proj_planes_vertical = fit_plane(proj_points_vertical)
# add projector location to the horizontalumn points
horizontal_shape = (1, proj.resolution[1], 3)
proj_points_horizontal = np.concatenate((proj_pixels, np.ones(horizontal_shape) * proj_pose), axis=0)
# calculate the horizontalumn planes
proj_planes_horizontal = fit_plane(np.transpose(proj_points_horizontal, (1, 0, 2)))
return proj_planes_vertical, proj_planes_horizontal
def extract_point_cloud(initial_images, vertical_stripe_images, horizontal_stripe_images, min_contrast=0.2):
"""
Function: extract_point_cloud
Takes a series of images with gray codes projected on them and produces a point cloud.
Parameters:
initial_images - *[<Image>]* image of an all white projection followed by an all black projection
vertical_stripe_images - *[<Image>]* Images with vertical strip projections as described in <gray_code_estimates>
horizontal_stripe_images - *[<Image>]* Images with horizontal strip projections as described in <gray_code_estimates>
Returns:
*<PointCloud>* A PointCloud object with the scan data
"""
if len(initial_images) % 2 != 0:
raise Exception("For initial_images, must have an all white projection followed " +
"by an all black projection")
# alias projector and camera objects
proj = initial_images[0].patterns[0].projected_patterns[0][1]
cam = initial_images[0].camera
# get camera pixel rays
cam_rays = to_global_frame(pixel_rays(cam), cam)
# get cam location
cam_pose = to_global_frame([0, 0, 0], cam)
# get row projection indices from vertical stripe pattenrs
gray_code_row, pixel_mask_row = gray_code_estimates(vertical_stripe_images, min_contrast)
# get column projection indices from horizontal stripe patterns
gray_code_col, pixel_mask_col = gray_code_estimates(horizontal_stripe_images, min_contrast)
if gray_code_row == None and gray_code_col == None:
raise Exception("Must have images to scan from!")
# combine pixel masks from columns and rows (i.e., if a pixel was invalid
# from the column or row data, make it invalid for both)
if pixel_mask_row != None:
if pixel_mask_col != None:
pixel_mask = pixel_mask_row & pixel_mask_col
else:
pixel_mask = pixel_mask_row
else:
pixel_mask = pixel_mask_col
pixel_mask = pixel_mask_row
# also invalidate pixels if difference between original first and second
# image (black projection and whit projetion) doesn't exceed contrast
# ratio
gray_1 = cv2.cvtColor(initial_images[0].data, cv2.COLOR_RGB2GRAY)
gray_2 = cv2.cvtColor(initial_images[1].data, cv2.COLOR_RGB2GRAY)
pixel_mask[np.abs(gray_1.astype(np.int16) - gray_2.astype(np.int16)) <= 255 * min_contrast] = False
# also invalidate pixel if any the calculated gray code row is greater than
# the projection row, or if the calculated gray code column is greater than
# the gray code column
if gray_code_row != None:
pixel_mask[gray_code_row >= proj.resolution[0]] = False
if gray_code_col != None:
pixel_mask[gray_code_col >= proj.resolution[1]] = False
# get plane equations for every projector row and column
proj_planes_vert, proj_planes_horz = projector_planes(proj)
from nose.tools import set_trace; set_trace()
# TODO: vectorize this next part
# calculate all the points
points = []
for i in range(cam.resolution[0]):
for j in range(cam.resolution[1]):
if pixel_mask[i,j]:
if gray_code_row != None:
p_row = line_plane_intersection(cam_pose, cam_rays[i,j],
proj_planes_vert[gray_code_row[i,j]])
else:
p_row = None
if gray_code_col != None:
p_col = line_plane_intersection(cam_pose, cam_rays[i,j],
proj_planes_horz[gray_code_col[i,j]])
else:
p_col = None
if p_row != None:
points.append(p_row)
p = PointCloud()
p.from_array(np.array(points, dtype=np.float32))
return p