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