def test_point_to_plane_gradICP_transform1(self):
device = torch.device("cuda")
channels_first = False
colors, depths, intrinsics, poses = load_test_data(channels_first,
batch_size=1)
rgbdimages = RGBDImages(
colors.to(device),
depths.to(device),
intrinsics.to(device),
poses.to(device),
channels_first=channels_first,
)
sigma = 0.6
src_pointclouds = pointclouds_from_rgbdimages(rgbdimages[:,
0]).to(device)
rad = 0.2
transform = torch.tensor(
[
[1.0, 0.0, 0.0, 0.05],
[0.0, np.cos(rad), -np.sin(rad), 0.03],
[0.0, np.sin(rad), np.cos(rad), 0.01],
[0.0, 0.0, 0.0, 1.0],
],
device=device,
dtype=colors.dtype,
)
# transform = torch.tensor(
# [
# [np.cos(rad), -np.sin(rad), 0.0, 0.05],
# [np.sin(rad), np.cos(rad), 0.0, 0.03],
# [0.0, 0.0, 1.0, 0.01],
# [0.0, 0.0, 0.0, 1.0],
# ],
# device=device,
# dtype=colors.dtype,
# )
tgt_pointclouds = src_pointclouds.transform(transform)
src_pc = src_pointclouds.points_padded
tgt_pc = tgt_pointclouds.points_padded
tgt_normals = tgt_pointclouds.normals_padded
initial_transform = torch.eye(4, device=device)
numiters = 100
damp = 1e-8
dist_thresh = None
t, idx = point_to_plane_gradICP(
src_pc,
tgt_pc,
tgt_normals,
initial_transform,
numiters,
damp,
dist_thresh,
)
assert t.shape == transform.shape
assert_allclose(t, transform)
def test_gradICP_provide(self):
device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
channels_first = False
colors, depths, intrinsics, poses = load_test_data(channels_first,
batch_size=1)
rgbdimages = RGBDImages(
colors.to(device),
depths.to(device),
intrinsics.to(device),
poses.to(device),
channels_first=channels_first,
)
sigma = 0.6
src_pointclouds = pointclouds_from_rgbdimages(rgbdimages[:,
0]).to(device)
rad = 0.1
transform = torch.tensor(
[
[np.cos(rad), -np.sin(rad), 0.0, 0.05],
[np.sin(rad), np.cos(rad), 0.0, 0.03],
[0.0, 0.0, 1.0, 0.01],
[0.0, 0.0, 0.0, 1.0],
],
device=device,
dtype=colors.dtype,
)
tgt_pointclouds = src_pointclouds.transform(transform)
numiters = 30
damp = 1e-8
dist_thresh = 0.2
lambda_max = 2.0
B = 1.0
B2 = 1.0
nu = 200.0
odom = GradICPOdometryProvider(
numiters=numiters,
damp=damp,
dist_thresh=dist_thresh,
lambda_max=lambda_max,
B=B,
B2=B2,
nu=nu,
)
odom_transform = odom.provide(tgt_pointclouds, src_pointclouds)
odom_transform = odom_transform.squeeze(1).squeeze(0)
assert odom_transform.shape == transform.shape
assert_allclose(odom_transform, transform)