Select overlapping keyframes to the current camera observation. Args: gt_depth (tensor): ground truth depth image of the current frame. w2c (tensor): world to camera matrix (4 x 4). keyframe_list (list): a list containing info for each keyframe.
(gt_depth, w2c, intrinsics, keyframe_list, k, pixels=1600)
| 38 | |
| 39 | |
| 40 | def keyframe_selection_overlap(gt_depth, w2c, intrinsics, keyframe_list, k, pixels=1600): |
| 41 | """ |
| 42 | Select overlapping keyframes to the current camera observation. |
| 43 | |
| 44 | Args: |
| 45 | gt_depth (tensor): ground truth depth image of the current frame. |
| 46 | w2c (tensor): world to camera matrix (4 x 4). |
| 47 | keyframe_list (list): a list containing info for each keyframe. |
| 48 | k (int): number of overlapping keyframes to select. |
| 49 | pixels (int, optional): number of pixels to sparsely sample |
| 50 | from the image of the current camera. Defaults to 1600. |
| 51 | Returns: |
| 52 | selected_keyframe_list (list): list of selected keyframe id. |
| 53 | """ |
| 54 | # Radomly Sample Pixel Indices from valid depth pixels |
| 55 | width, height = gt_depth.shape[2], gt_depth.shape[1] |
| 56 | valid_depth_indices = torch.where(gt_depth[0] > 0) |
| 57 | valid_depth_indices = torch.stack(valid_depth_indices, dim=1) |
| 58 | indices = torch.randint(valid_depth_indices.shape[0], (pixels,)) |
| 59 | sampled_indices = valid_depth_indices[indices] |
| 60 | |
| 61 | # Back Project the selected pixels to 3D Pointcloud |
| 62 | pts = get_pointcloud(gt_depth, intrinsics, w2c, sampled_indices) |
| 63 | |
| 64 | list_keyframe = [] |
| 65 | for keyframeid, keyframe in enumerate(keyframe_list): |
| 66 | # Get the estimated world2cam of the keyframe |
| 67 | est_w2c = keyframe['est_w2c'] |
| 68 | # Transform the 3D pointcloud to the keyframe's camera space |
| 69 | pts4 = torch.cat([pts, torch.ones_like(pts[:, :1])], dim=1) |
| 70 | transformed_pts = (est_w2c @ pts4.T).T[:, :3] |
| 71 | # Project the 3D pointcloud to the keyframe's image space |
| 72 | points_2d = torch.matmul(intrinsics, transformed_pts.transpose(0, 1)) |
| 73 | points_2d = points_2d.transpose(0, 1) |
| 74 | points_z = points_2d[:, 2:] + 1e-5 |
| 75 | points_2d = points_2d / points_z |
| 76 | projected_pts = points_2d[:, :2] |
| 77 | # Filter out the points that are outside the image |
| 78 | edge = 20 |
| 79 | mask = (projected_pts[:, 0] < width-edge)*(projected_pts[:, 0] > edge) * \ |
| 80 | (projected_pts[:, 1] < height-edge)*(projected_pts[:, 1] > edge) |
| 81 | mask = mask & (points_z[:, 0] > 0) |
| 82 | # Compute the percentage of points that are inside the image |
| 83 | percent_inside = mask.sum()/projected_pts.shape[0] |
| 84 | list_keyframe.append( |
| 85 | {'id': keyframeid, 'percent_inside': percent_inside}) |
| 86 | |
| 87 | # Sort the keyframes based on the percentage of points that are inside the image |
| 88 | list_keyframe = sorted( |
| 89 | list_keyframe, key=lambda i: i['percent_inside'], reverse=True) |
| 90 | # Select the keyframes with percentage of points inside the image > 0 |
| 91 | selected_keyframe_list = [keyframe_dict['id'] |
| 92 | for keyframe_dict in list_keyframe if keyframe_dict['percent_inside'] > 0.0] |
| 93 | selected_keyframe_list = list(np.random.permutation( |
| 94 | np.array(selected_keyframe_list))[:k]) |
| 95 | |
| 96 | return selected_keyframe_list |
no test coverage detected