def testuniquerows(self):
#self.X[5, :] = [0, 1, 2]
rows = 10
X = np.random.random((rows, 3))
X = np.tile(X, (3, 1))
np.random.shuffle(X)
UR, freqs = quantizer.uniquerows(X, 3, return_frequencies=True)
self.assertEquals(len(UR), rows)
self.isequal(np.tile(3, 10), freqs)
# TODO occupances currently do not correspond to rows?
# test this
# repeat to make sure nothing changes
UR = quantizer.uniquerows(UR, 3)
self.assertEquals(len(UR), rows)
def testuniquerows(self):
#self.X[5, :] = [0, 1, 2]
rows = 10
X = np.random.random((rows, 3))
X = np.tile(X, (3, 1))
np.random.shuffle(X)
UR, freqs = quantizer.uniquerows(X, 3, return_frequencies=True)
self.assertEquals(len(UR), rows)
self.isequal(np.tile(3, 10), freqs)
# TODO occupances currently do not correspond to rows?
# test this
# repeat to make sure nothing changes
UR = quantizer.uniquerows(UR, 3)
self.assertEquals(len(UR), rows)
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
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.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:
pc_xform = poseutil.Pose3D(X=(0, 0, 0, -np.pi / 2., 0, -np.pi / 2.))
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()
