def get_freespace(xyzs, pose, voxelmap):
# TODO consider using mapper._build_sensor_model() and mapper.generate_freespace() instead.
# resampling to align with grid. bad hack, but makes ray_cast easier.
res = voxelmap.get_resolution()
xyzs = poseutil.transformpoints(xyzs,pose)
voxs = occupiedlist.pointstovoxels(xyzs, res)
voxs = quantizer.uniquerows(voxs, 0)
X = occupiedlist.voxelstopoints(voxs, res)
free_pts = voxelmap.free_space_ray_trace(X,pose[:3])
return free_pts
def get_freespace(xyzs, pose, voxelmap):
# TODO consider using mapper._build_sensor_model() and mapper.generate_freespace() instead.
# resampling to align with grid. bad hack, but makes ray_cast easier.
res = voxelmap.get_resolution()
xyzs = poseutil.transformpoints(xyzs,pose)
voxs = occupiedlist.pointstovoxels(xyzs, res)
voxs = quantizer.uniquerows(voxs, 0)
X = occupiedlist.voxelstopoints(voxs, res)
free_pts = voxelmap.free_space_ray_trace(X,pose[:3])
return free_pts
def testpointstoandfromvoxels(self):
res = 0.5 # is only exact for 0.5
# test for all lattice quantisation functions
for funcstr in occupiedlist.latticefuncs:
# test reciprocal nature of conversion
A = occupiedlist.pointstovoxels(self.X, res, latticetype=funcstr)
Y = occupiedlist.voxelstopoints(A, res)
self.assertTrue(np.allclose(self.X, Y))
X = np.array([1, 2, 3]).reshape(1, 3)
# TODO currently does not handle single points
A = occupiedlist.pointstovoxels(X, res, latticetype=funcstr)
self.assertTrue(np.allclose(X/res, A))
segment_map = mapper.VoxelMap(res, levels=3)
pc = pointcloud.load(fnames.pop(0))
# pc = pointcloud.PointCloud(pc)
pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
segment_map.add_points(pc, bestpose)
object_map = mapper.VoxelMap(res / 4.0, levels=1)
# freespace_ol = segment_map.generate_freespace2(resolution = res*2, minR=0.4, maxR=3)
# freespace_ol = segment_map.generate_freespace(res, minR=0.25, maxR=0.75) # *4 for speed during testing, should just be res
free_space_res = res
pc_vox = occupiedlist.pointstovoxels(pc.points, free_space_res)
pc_vox = quantizer.uniquerows(pc_vox, 0)
pc_pts = occupiedlist.voxelstopoints(pc_vox, free_space_res)
pc_regular = pointcloud.PointCloud(pc_pts)
freespace = segment_map.free_space_ray_trace(pc_regular, (0, 0, 0), free_space_res, voxel_offset=2.5)
freespace_ol = occupiedlist.OccupiedList(free_space_res)
freespace_ol.addpoints(freespace.points)
# import pdb;pdb.set_trace()
# vis = robotvisualiser.Visualiser(npts=2e6)
# vis.addmappts(segment_map.get_points().points)
# vis.addmappts(freespace_ol.getpoints())
# free_pc = pointcloud.PointCloud(freespace_ol.getpoints())
# free_pc.display()
if visualise:
vis = robotvisualiser.Visualiser(npts=2e6)
res = 0.01
segment_map = mapper.VoxelMap(res, levels=3)
pc = pointcloud.load(fnames.pop(0))
#pc = pointcloud.PointCloud(pc)
pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
segment_map.add_points(pc, bestpose)
object_map = mapper.VoxelMap(res / 4., levels=1)
#freespace_ol = segment_map.generate_freespace2(resolution = res*2, minR=0.4, maxR=3)
#freespace_ol = segment_map.generate_freespace(res, minR=0.25, maxR=0.75) # *4 for speed during testing, should just be res
free_space_res = res
pc_vox = occupiedlist.pointstovoxels(pc.points, free_space_res)
pc_vox = quantizer.uniquerows(pc_vox, 0)
pc_pts = occupiedlist.voxelstopoints(pc_vox, free_space_res)
pc_regular = pointcloud.PointCloud(pc_pts)
freespace = segment_map.free_space_ray_trace(pc_regular, (0, 0, 0),
free_space_res,
voxel_offset=2.5)
freespace_ol = occupiedlist.OccupiedList(free_space_res)
freespace_ol.addpoints(freespace.points)
#import pdb;pdb.set_trace()
#vis = robotvisualiser.Visualiser(npts=2e6)
#vis.addmappts(segment_map.get_points().points)
#vis.addmappts(freespace_ol.getpoints())
#free_pc = pointcloud.PointCloud(freespace_ol.getpoints())
#free_pc.display()
if visualise: