Exemplo n.º 1
0
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
Exemplo n.º 2
0
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