def run_iterative_closest_point():
points_alignment.iterative_closest_point(
X,
Y,
estimate_scale=estimate_scale,
allow_reflection=False,
verbose=False,
max_iterations=100,
relative_rmse_thr=1e-4,
)
torch.cuda.synchronize()
def _compare_with_trimesh(self,
n_points_X=100,
n_points_Y=100,
estimate_scale=False):
"""
Executes a single test for `iterative_closest_point` for a
specific setting of the inputs / outputs. Compares the result with
the result of the trimesh package on the same input data.
"""
device = torch.device("cuda:0")
# load the trimesh results and the initial point clouds for icp
key = (int(n_points_X), int(n_points_Y), int(estimate_scale))
X, Y, R_trimesh, T_trimesh, s_trimesh = [
x.to(device) for x in self.trimesh_results[key]
]
# run the icp algorithm
(
converged,
_,
_,
(R_ours, T_ours, s_ours),
_,
) = points_alignment.iterative_closest_point(
X,
Y,
estimate_scale=estimate_scale,
allow_reflection=False,
verbose=False,
max_iterations=100,
)
# check that we have the same transformation
# and that the icp converged
atol = 1e-5
self.assertClose(R_ours, R_trimesh, atol=atol)
self.assertClose(T_ours, T_trimesh, atol=atol)
self.assertClose(s_ours, s_trimesh, atol=atol)
self.assertTrue(converged)
def test_init_transformation(self, batch_size=10):
"""
First runs a full ICP on a random problem. Then takes a given point
in the history of ICP iteration transformations, initializes
a second run of ICP with this transformation and checks whether
both runs ended with the same solution.
"""
device = torch.device("cuda:0")
for dim in (2, 3, 11):
for n_points_X in (30, 100):
for n_points_Y in (30, 100):
# initialize ground truth point clouds
X, Y = [
TestCorrespondingPointsAlignment.init_point_cloud(
batch_size=batch_size,
n_points=n_points,
dim=dim,
device=device,
use_pointclouds=False,
random_pcl_size=True,
)
for n_points in (n_points_X, n_points_Y)
]
# run full icp
(
converged,
_,
Xt,
(R, T, s),
t_hist,
) = points_alignment.iterative_closest_point(
X,
Y,
estimate_scale=False,
allow_reflection=False,
verbose=False,
max_iterations=100,
)
# start from the solution after the third
# iteration of the previous ICP
t_init = t_hist[min(2, len(t_hist) - 1)]
# rerun the ICP
(
converged_init,
_,
Xt_init,
(R_init, T_init, s_init),
t_hist_init,
) = points_alignment.iterative_closest_point(
X,
Y,
init_transform=t_init,
estimate_scale=False,
allow_reflection=False,
verbose=False,
max_iterations=100,
)
# compare transformations and obtained clouds
# check that both sets of transforms are the same
atol = 3e-5
self.assertClose(R_init, R, atol=atol)
self.assertClose(T_init, T, atol=atol)
self.assertClose(s_init, s, atol=atol)
self.assertClose(Xt_init, Xt, atol=atol)
def test_heterogenous_inputs(self, batch_size=10):
"""
Tests whether we get the same result when running ICP on
a set of randomly-sized Pointclouds and on their padded versions.
"""
device = torch.device("cuda:0")
for estimate_scale in (True, False):
for max_n_points in (10, 30, 100):
# initialize ground truth point clouds
X_pcl, Y_pcl = [
TestCorrespondingPointsAlignment.init_point_cloud(
batch_size=batch_size,
n_points=max_n_points,
dim=3,
device=device,
use_pointclouds=True,
random_pcl_size=True,
)
for _ in range(2)
]
# get the padded versions and their num of points
X_padded = X_pcl.points_padded()
Y_padded = Y_pcl.points_padded()
n_points_X = X_pcl.num_points_per_cloud()
n_points_Y = Y_pcl.num_points_per_cloud()
# run icp with Pointlouds inputs
(
_,
_,
Xt_pcl,
(R_pcl, T_pcl, s_pcl),
_,
) = points_alignment.iterative_closest_point(
X_pcl,
Y_pcl,
estimate_scale=estimate_scale,
allow_reflection=False,
verbose=False,
max_iterations=100,
)
Xt_pcl = Xt_pcl.points_padded()
# run icp with tensor inputs on each element
# of the batch separately
icp_results = [
points_alignment.iterative_closest_point(
X_[None, :n_X, :],
Y_[None, :n_Y, :],
estimate_scale=estimate_scale,
allow_reflection=False,
verbose=False,
max_iterations=100,
)
for X_, Y_, n_X, n_Y in zip(
X_padded, Y_padded, n_points_X, n_points_Y
)
]
# parse out the transformation results
R, T, s = [
torch.cat([x.RTs[i] for x in icp_results], dim=0) for i in range(3)
]
# check that both sets of transforms are the same
atol = 1e-5
self.assertClose(R_pcl, R, atol=atol)
self.assertClose(T_pcl, T, atol=atol)
self.assertClose(s_pcl, s, atol=atol)
# compare the transformed point clouds
for pcli in range(batch_size):
nX = n_points_X[pcli]
Xt_ = icp_results[pcli].Xt[0, :nX]
Xt_pcl_ = Xt_pcl[pcli][:nX]
self.assertClose(Xt_pcl_, Xt_, atol=atol)
