def test_unproject_normalized(self, device):
# this is for default normalize_points=False
depth = 2 * torch.tensor([[[
[1., 1., 1.],
[1., 1., 1.],
[1., 1., 1.],
[1., 1., 1.],
]]]).to(device)
camera_matrix = torch.tensor([[
[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.],
]]).to(device)
points3d_expected = torch.tensor([[[
[0., 2., 4.],
[0., 2., 4.],
[0., 2., 4.],
[0., 2., 4.],
], [
[0., 0., 0.],
[2., 2., 2.],
[4., 4., 4.],
[6., 6., 6.],
], [
[2., 2., 2.],
[2., 2., 2.],
[2., 2., 2.],
[2., 2., 2.],
]]]).to(device)
points3d = kornia.depth_to_3d(
depth, camera_matrix) # default is normalize_points=False
assert_allclose(points3d, points3d_expected)
def test_unproject(self, device):
depth = 2 * torch.tensor([[[
[1., 1., 1.],
[1., 1., 1.],
[1., 1., 1.],
[1., 1., 1.],
]]]).to(device)
camera_matrix = torch.tensor([[
[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.],
]]).to(device)
points3d_expected = torch.tensor([[[
[0.0000, 1.4142, 1.7889],
[0.0000, 1.1547, 1.6330],
[0.0000, 0.8165, 1.3333],
[0.0000, 0.6030, 1.0690],
],
[
[0.0000, 0.0000, 0.0000],
[1.4142, 1.1547, 0.8165],
[1.7889, 1.6330, 1.3333],
[1.8974, 1.8091, 1.6036],
],
[
[2.0000, 1.4142, 0.8944],
[1.4142, 1.1547, 0.8165],
[0.8944, 0.8165, 0.6667],
[0.6325, 0.6030, 0.5345],
]]]).to(device)
points3d = kornia.depth_to_3d(depth, camera_matrix)
assert_allclose(points3d, points3d_expected)
def test_shapes(self, batch_size, device, dtype):
depth = torch.rand(batch_size, 1, 3, 4, device=device, dtype=dtype)
camera_matrix = torch.rand(batch_size,
3,
3,
device=device,
dtype=dtype)
points3d = kornia.depth_to_3d(depth, camera_matrix)
assert points3d.shape == (batch_size, 3, 3, 4)
def test_unproject_and_project(self, device, dtype):
depth = 2 * torch.tensor(
[[[[1.0, 1.0, 1.0], [1.0, 1.0, 1.0], [1.0, 1.0, 1.0], [1.0, 1.0, 1.0]]]], device=device, dtype=dtype
)
camera_matrix = torch.tensor([[[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]], device=device, dtype=dtype)
points3d = kornia.depth_to_3d(depth, camera_matrix)
points2d = kornia.project_points(points3d.permute(0, 2, 3, 1), camera_matrix[:, None, None])
points2d_expected = kornia.create_meshgrid(4, 3, False, device=device).to(dtype=dtype)
assert_close(points2d, points2d_expected, atol=1e-4, rtol=1e-4)
def warp_frame_depth(
image_src: torch.Tensor,
depth_dst: torch.Tensor,
src_trans_dst: torch.Tensor,
camera_matrix: torch.Tensor,
normalize_points: bool = False,
sampling_mode='bilinear') -> torch.Tensor:
# TAKEN FROM KORNIA LIBRARY
if not isinstance(image_src, torch.Tensor):
raise TypeError(f"Input image_src type is not a torch.Tensor. Got {type(image_src)}.")
if not len(image_src.shape) == 4:
raise ValueError(f"Input image_src musth have a shape (B, D, H, W). Got: {image_src.shape}")
if not isinstance(depth_dst, torch.Tensor):
raise TypeError(f"Input depht_dst type is not a torch.Tensor. Got {type(depth_dst)}.")
if not len(depth_dst.shape) == 4 and depth_dst.shape[-3] == 1:
raise ValueError(f"Input depth_dst musth have a shape (B, 1, H, W). Got: {depth_dst.shape}")
if not isinstance(src_trans_dst, torch.Tensor):
raise TypeError(f"Input src_trans_dst type is not a torch.Tensor. "
f"Got {type(src_trans_dst)}.")
if not len(src_trans_dst.shape) == 3 and src_trans_dst.shape[-2:] == (3, 3):
raise ValueError(f"Input src_trans_dst must have a shape (B, 3, 3). "
f"Got: {src_trans_dst.shape}.")
if not isinstance(camera_matrix, torch.Tensor):
raise TypeError(f"Input camera_matrix type is not a torch.Tensor. "
f"Got {type(camera_matrix)}.")
if not len(camera_matrix.shape) == 3 and camera_matrix.shape[-2:] == (3, 3):
raise ValueError(f"Input camera_matrix must have a shape (B, 3, 3). "
f"Got: {camera_matrix.shape}.")
# unproject source points to camera frame
points_3d_dst: torch.Tensor = kornia.depth_to_3d(depth_dst, camera_matrix, normalize_points) # Bx3xHxW
# transform points from source to destination
points_3d_dst = points_3d_dst.permute(0, 2, 3, 1) # BxHxWx3
# apply transformation to the 3d points
points_3d_src = kornia.transform_points(src_trans_dst[:, None], points_3d_dst) # BxHxWx3
points_3d_src[:, :, :, 2] = torch.relu(points_3d_src[:, :, :, 2])
# project back to pixels
camera_matrix_tmp: torch.Tensor = camera_matrix[:, None, None] # Bx1x1xHxW
points_2d_src: torch.Tensor = kornia.project_points(points_3d_src, camera_matrix_tmp) # BxHxWx2
# normalize points between [-1 / 1]
height, width = depth_dst.shape[-2:]
points_2d_src_norm: torch.Tensor = kornia.normalize_pixel_coordinates(points_2d_src, height, width) # BxHxWx2
return torch.nn.functional.grid_sample(image_src, points_2d_src_norm, align_corners=True, mode=sampling_mode)
def get_differentiable_square_depth_estimation(reference_pose_torch,
measurement_pose_torch,
previous_depth_torch,
full_K_torch,
half_K_torch,
original_image_size,
device):
batch_size, _, _ = full_K_torch.size()
R_render = torch.eye(3, dtype=torch.float, device=device)
T_render = torch.zeros(3, dtype=torch.float, device=device)
R_render = torch.stack(batch_size * [R_render], dim=0)
T_render = torch.stack(batch_size * [T_render], dim=0)
R_render[:, 0, 0] *= -1
R_render[:, 1, 1] *= -1
trans = torch.bmm(torch.inverse(reference_pose_torch), measurement_pose_torch)
points_3d_src = kornia.depth_to_3d(previous_depth_torch, full_K_torch, normalize_points=False)
points_3d_src = points_3d_src.permute(0, 2, 3, 1)
points_3d_dst = kornia.transform_points(trans[:, None], points_3d_src).view(batch_size, -1, 3)
point_cloud_p3d = structures.Pointclouds(points=points_3d_dst, features=None)
width_normalizer = original_image_size / 4.0
height_normalizer = original_image_size / 4.0
px_ndc = (half_K_torch[:, 0, 2] - width_normalizer) / width_normalizer
py_ndc = (half_K_torch[:, 1, 2] - height_normalizer) / height_normalizer
fx_ndc = half_K_torch[:, 0, 0] / width_normalizer
fy_ndc = half_K_torch[:, 1, 1] / height_normalizer
principal_point = torch.stack([px_ndc, py_ndc], dim=-1)
focal_length = torch.stack([fx_ndc, fy_ndc], dim=-1)
cameras = renderer.SfMPerspectiveCameras(focal_length=focal_length,
principal_point=principal_point,
R=R_render,
T=T_render,
device=torch.device('cuda'))
raster_settings = renderer.PointsRasterizationSettings(
image_size=int(original_image_size / 2.0),
radius=0.02,
points_per_pixel=3)
depth_renderer = renderer.PointsRasterizer(cameras=cameras, raster_settings=raster_settings)
rendered_depth = torch.min(depth_renderer(point_cloud_p3d).zbuf, dim=-1)[0]
depth_hypothesis = torch.relu(rendered_depth).unsqueeze(1)
return depth_hypothesis
def get_non_differentiable_rectangle_depth_estimation(reference_pose_torch,
measurement_pose_torch,
previous_depth_torch,
full_K_torch,
half_K_torch,
original_width,
original_height):
batch_size, _, _ = reference_pose_torch.shape
half_width = int(original_width / 2)
half_height = int(original_height / 2)
trans = torch.bmm(torch.inverse(reference_pose_torch), measurement_pose_torch)
points_3d_src = kornia.depth_to_3d(previous_depth_torch, full_K_torch, normalize_points=False)
points_3d_src = points_3d_src.permute(0, 2, 3, 1)
points_3d_dst = kornia.transform_points(trans[:, None], points_3d_src)
points_3d_dst = points_3d_dst.view(batch_size, -1, 3)
z_values = points_3d_dst[:, :, -1]
z_values = torch.relu(z_values)
sorting_indices = torch.argsort(z_values, descending=True)
z_values = torch.gather(z_values, dim=1, index=sorting_indices)
sorting_indices_for_points = torch.stack([sorting_indices] * 3, dim=-1)
points_3d_dst = torch.gather(points_3d_dst, dim=1, index=sorting_indices_for_points)
projections = torch.round(kornia.project_points(points_3d_dst, half_K_torch.unsqueeze(1))).long()
is_valid_below = (projections[:, :, 0] >= 0) & (projections[:, :, 1] >= 0)
is_valid_above = (projections[:, :, 0] < half_width) & (projections[:, :, 1] < half_height)
is_valid = is_valid_below & is_valid_above
depth_hypothesis = torch.zeros(size=(batch_size, 1, half_height, half_width)).cuda()
for projection_index in range(0, batch_size):
valid_points_zs = z_values[projection_index][is_valid[projection_index]]
valid_projections = projections[projection_index][is_valid[projection_index]]
i_s = valid_projections[:, 1]
j_s = valid_projections[:, 0]
ij_combined = i_s * half_width + j_s
_, ij_combined_unique_indices = np.unique(ij_combined.cpu().numpy(), return_index=True)
ij_combined_unique_indices = torch.from_numpy(ij_combined_unique_indices).long().cuda()
i_s = i_s[ij_combined_unique_indices]
j_s = j_s[ij_combined_unique_indices]
valid_points_zs = valid_points_zs[ij_combined_unique_indices]
torch.index_put_(depth_hypothesis[projection_index, 0], (i_s, j_s), valid_points_zs)
return depth_hypothesis
def test_unproject_normalized(self, device, dtype):
# this is for normalize_points=True
depth = 2 * torch.tensor([[[
[1., 1., 1.],
[1., 1., 1.],
[1., 1., 1.],
[1., 1., 1.],
]]],
device=device,
dtype=dtype)
camera_matrix = torch.tensor([[
[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.],
]],
device=device,
dtype=dtype)
points3d_expected = torch.tensor([[[
[0.0000, 1.4142, 1.7889],
[0.0000, 1.1547, 1.6330],
[0.0000, 0.8165, 1.3333],
[0.0000, 0.6030, 1.0690],
],
[
[0.0000, 0.0000, 0.0000],
[1.4142, 1.1547, 0.8165],
[1.7889, 1.6330, 1.3333],
[1.8974, 1.8091, 1.6036],
],
[
[2.0000, 1.4142, 0.8944],
[1.4142, 1.1547, 0.8165],
[0.8944, 0.8165, 0.6667],
[0.6325, 0.6030, 0.5345],
]]],
device=device,
dtype=dtype)
points3d = kornia.depth_to_3d(depth,
camera_matrix,
normalize_points=True)
assert_allclose(points3d, points3d_expected, atol=1e-4, rtol=1e-4)
def test_unproject_and_project(self, device):
depth = 2 * torch.tensor([[[
[1., 1., 1.],
[1., 1., 1.],
[1., 1., 1.],
[1., 1., 1.],
]]]).to(device)
camera_matrix = torch.tensor([[
[1., 0., 0.],
[0., 1., 0.],
[0., 0., 1.],
]]).to(device)
points3d = kornia.depth_to_3d(depth, camera_matrix)
points2d = kornia.project_points(points3d.permute(0, 2, 3, 1),
camera_matrix[:, None, None])
points2d_expected = kornia.create_meshgrid(4, 3, False).to(device)
assert_allclose(points2d, points2d_expected)
def test_unproject_denormalized(self, device, dtype):
# this is for default normalize_points=False
depth = 2 * torch.tensor(
[[[[1.0, 1.0, 1.0], [1.0, 1.0, 1.0], [1.0, 1.0, 1.0], [1.0, 1.0, 1.0]]]], device=device, dtype=dtype
)
camera_matrix = torch.tensor([[[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]], device=device, dtype=dtype)
points3d_expected = torch.tensor(
[
[
[[0.0, 2.0, 4.0], [0.0, 2.0, 4.0], [0.0, 2.0, 4.0], [0.0, 2.0, 4.0]],
[[0.0, 0.0, 0.0], [2.0, 2.0, 2.0], [4.0, 4.0, 4.0], [6.0, 6.0, 6.0]],
[[2.0, 2.0, 2.0], [2.0, 2.0, 2.0], [2.0, 2.0, 2.0], [2.0, 2.0, 2.0]],
]
],
device=device,
dtype=dtype,
)
points3d = kornia.depth_to_3d(depth, camera_matrix) # default is normalize_points=False
assert_close(points3d, points3d_expected, atol=1e-4, rtol=1e-4)
def test_shapes_broadcast(self, device, batch_size):
depth = torch.rand(batch_size, 1, 3, 4).to(device)
camera_matrix = torch.rand(1, 3, 3).to(device)
points3d = kornia.depth_to_3d(depth, camera_matrix)
assert points3d.shape == (batch_size, 3, 3, 4)
def test_smoke(self, device):
depth = torch.rand(1, 1, 3, 4).to(device)
camera_matrix = torch.rand(1, 3, 3).to(device)
points3d = kornia.depth_to_3d(depth, camera_matrix)
assert points3d.shape == (1, 3, 3, 4)
