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 chamfer_distance(vg1, vg2, scale, downsample=1):
"""
Returns the chamfer distance between two voxelgrids
"""
vg1 = conversions.downsample(vg1, downsample)
vg2 = conversions.downsample(vg2, downsample)
pt1 = conversions.voxelgrid_to_pointcloud(vg1, scale=scale)
pt2 = conversions.voxelgrid_to_pointcloud(vg2, scale=scale)
return chamfer_distance_pointcloud(pt1, pt2)
def test_downsample_with_extra_dims(self):
vg_orig = np.zeros((1, 4, 4, 4))
vg_orig[0, 0, 0, 0] = 1.0
self.assertEqual((1, 2, 2, 2), downsample(vg_orig).shape)
vg_orig = np.zeros((1, 4, 4, 4, 1))
vg_orig[0, 0, 0, 0] = 1.0
self.assertEqual((1, 2, 2, 2, 1), downsample(vg_orig).shape)
vg_orig = np.zeros((4, 4, 4, 1))
vg_orig[0, 0, 0, 0] = 1.0
self.assertEqual((2, 2, 2, 1), downsample(vg_orig).shape)
def test_downsample_without_extra_dims(self):
vg_orig = np.zeros((4, 4, 4))
vg_orig[0, 0, 0] = 1.0
vg_orig = tf.cast(vg_orig, tf.float32)
vg_downsampled = downsample(vg_orig)
self.assertEqual((2, 2, 2), vg_downsampled.shape)
vg_expected = [[[1, 0], [0, 0]], [[0, 0], [0, 0]]]
self.assertTrue(tf.reduce_all(vg_expected == vg_downsampled))