if load_pc:
fnames = [datadir + '/' + fname for fname in fnames if fname.startswith('mrolpc')]
else:
fnames = [datadir + '/' + fname for fname in fnames if fname.endswith('.png')]
except:
print 'Need to specify a valid directory for input and file name for output'
sys.exit()
# variable initialisation
iros_map = mapper.VoxelMap(res,levels=3)
iros_free_map = mapper.VoxelMap(res,levels=1)
bestpose = poseutil.Pose3D()
if load_pc:
pc_xform = poseutil.Pose3D(X=(0,0,0,0,0,0))
pc = pointcloud.load(fnames.pop(0))
pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
pc_xform = poseutil.Pose3D(X=(0,0,0,-np.pi/2.,0,-np.pi/2.))
else:
pc = get_pointcloud(fnames.pop(0))
iros_map.add_points(pc, bestpose)
if visualise:
iros_map.display(changes=False)
if make_free:
freepts = get_freespace(pc.points, bestpose, iros_map)
pcfree = pointcloud.PointCloud(freepts)
iros_free_map.add_points(pcfree,None)
if visualise:
iros_free_map.display(npts=1000000)
for fname in fnames:
# TODO merge the rotating code below with the rotating function
if __name__ == '__main__':
if len(sys.argv) == 1:
print __doc__
sys.exit()
fname = sys.argv[1]
rotate = '--rotating' in sys.argv[1:]
grid = '--grid' in sys.argv[1:]
az = 0
daz = np.radians(1)
visual.scene.select()
visual.scene.up = (0, 0, 1)
visual.scene.forward = (np.sin(az), np.cos(az), -1)
M = pointcloud.load(fname)
# approximately center the view on the point cloud
visual.scene.center = M.mean(axis=0)
if not rotate:
MT = EnableMouseThread(visual.scene)
MT.start()
while True:
M = pointcloud.load(fname)
if grid:
vis_pts = plotgrid(M)
else:
vis_pts = showpts(M)
# wait for file to change
]
else:
fnames = [
datadir + '/' + fname for fname in fnames if fname.endswith('.png')
]
except:
print 'Need to specify a valid directory for input'
sys.exit()
visualise = True
long_term = False
bestpose = poseutil.Pose3D(X=(0, 0, 0, 0, 0, 0))
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),
return vis_pts
# TODO merge the rotating code below with the rotating function
if __name__ == '__main__':
if len(sys.argv) == 1:
print __doc__
sys.exit()
fname = sys.argv[1]
rotate = '--rotating' in sys.argv[1:]
grid = '--grid' in sys.argv[1:]
az = 0
daz = np.radians(1)
visual.scene.select()
visual.scene.up = (0, 0, 1)
visual.scene.forward = (np.sin(az), np.cos(az), -1)
M = pointcloud.load(fname)
# approximately center the view on the point cloud
visual.scene.center = M.mean(axis=0)
if not rotate:
MT = EnableMouseThread(visual.scene)
MT.start()
while True:
M = pointcloud.load(fname)
if grid:
vis_pts = plotgrid(M)
else:
vis_pts = showpts(M)
# wait for file to change
