def test_transform_voxelgrid_is_invertable(self):
"""
Note, this is not necessarily true for rotation or fractional translation, as converting to a voxelgrid
effectively truncates to an int so there is a loss of precision
@return:
"""
T = make_transform(thetas=[0.0, 0, np.pi / 2], translation=[1, 2, 3])
T_inv = np.linalg.inv(T)
scale = 0.5
self.assertTrue((np.dot(T, T_inv) == np.eye(4)).all())
vg_orig = load_test_files()[0]
vg_rot = transform_voxelgrid(vg_orig, T, scale=scale)
self.assertFalse((vg_orig == vg_rot).all())
vg_new = transform_voxelgrid(vg_rot, T_inv, scale=scale)
self.assertTrue((vg_orig == vg_new).all())
def icp(voxelgrid_to_transform,
template_voxelgrid,
scale,
max_iter=10,
downsample=1):
"""
Returns a voxelgrid fitted to the template using ICP
@param max_iter: maximum iterations of ICP
@param downsample: downsampling factor so fewer points are fit during ICP
@param scale:
@param voxelgrid_to_transform:
@param template_voxelgrid:
@return:
"""
vg_to_transform_downsampled = conversions.downsample(
voxelgrid_to_transform, downsample)
template_voxelgrid = conversions.downsample(template_voxelgrid, downsample)
pt_0 = conversions.voxelgrid_to_pointcloud(template_voxelgrid, scale=scale)
pt_1 = conversions.voxelgrid_to_pointcloud(vg_to_transform_downsampled,
scale=scale)
source = pcl.PointCloud(pt_0.astype(np.float32))
target = pcl.PointCloud(pt_1.astype(np.float32))
icp = source.make_IterativeClosestPoint()
converged, transf, estimate, fitness = icp.icp(source,
target,
max_iter=max_iter)
T = np.linalg.inv(transf)
vg_icp = conversions.transform_voxelgrid(voxelgrid_to_transform,
T,
scale=scale)
return vg_icp
def test_transform_voxelgrid_using_identity(self):
vg_orig = load_test_files()[0]
T = np.eye(4)
vg_new = transform_voxelgrid(vg_orig, T)
self.assertTrue((vg_new == vg_orig).all())
def test_transform_voxelgrid_using_identity_on_simple_data(self):
vg_orig = np.zeros([3, 3, 3])
vg_orig[(1, 2), (0, 2), (1, 0)] = 1.0
T = np.eye(4)
vg_new = transform_voxelgrid(vg_orig, T)
self.assertTrue((vg_new == vg_orig).all())
def test_transform_voxelgrid_on_empty(self):
vg_orig = np.zeros([3, 3, 3])
T = np.eye(4)
vg_new = transform_voxelgrid(vg_orig, T)
self.assertTrue((vg_new == vg_orig).all())
def icp(voxelgrid_to_transform, template_voxelgrid, scale, max_iter=10, downsample = 1):
T = icp_transform(voxelgrid_to_transform, template_voxelgrid, scale, max_iter, downsample)
vg_icp = conversions.transform_voxelgrid(voxelgrid_to_transform, T, scale=scale)
return vg_icp