1
'''File to build map from a directory of depth images and save to file'''
2
from __future__ import division
3
import mrol_mapping.poseutil as poseutil
4
import mrol_mapping.mapper as mapper
5
import mrol_mapping.pointcloud as pointcloud
6
import mrol_mapping.util as util
9
import mrol_mapping.occupiedlist as occupiedlist
10
import mrol_mapping.quantizer as quantizer
16
sys.path.append('../test')
24
def get_pointcloud(fname):
25
xyzs = depth_image.image_to_points(fname)
26
pc = pointcloud.PointCloud(xyzs)
27
# remove ceiling and floor
28
pc.boxfilter(zmin=-15, zmax=0.5)
31
def get_freespace(xyzs, pose, voxelmap):
32
# TODO consider using mapper._build_sensor_model() and mapper.generate_freespace() instead.
33
# resampling to align with grid. bad hack, but makes ray_cast easier.
34
res = voxelmap.get_resolution()
35
xyzs = poseutil.transformpoints(xyzs,pose)
36
voxs = occupiedlist.pointstovoxels(xyzs, res)
37
voxs = quantizer.uniquerows(voxs, 0)
38
X = occupiedlist.voxelstopoints(voxs, res)
39
free_pts = voxelmap.free_space_ray_trace(X,pose[:3])
44
if __name__ == '__main__':
47
outfname = sys.argv[2]+".map"
48
fnames = os.listdir(datadir)
51
fnames = [datadir + '/' + fname for fname in fnames if fname.startswith('mrolpc')]
53
fnames = [datadir + '/' + fname for fname in fnames if fname.endswith('.png')]
55
print 'Need to specify a valid directory for input and file name for output'
58
# variable initialisation
59
iros_map = mapper.VoxelMap(res,levels=3)
60
iros_free_map = mapper.VoxelMap(res,levels=1)
61
bestpose = poseutil.Pose3D()
64
pc_xform = poseutil.Pose3D(X=(0,0,0,0,0,0))
65
pc = pointcloud.load(fnames.pop(0))
66
pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
67
pc_xform = poseutil.Pose3D(X=(0,0,0,-np.pi/2.,0,-np.pi/2.))
69
pc = get_pointcloud(fnames.pop(0))
70
iros_map.add_points(pc, bestpose)
72
iros_map.display(changes=False)
74
freepts = get_freespace(pc.points, bestpose, iros_map)
75
pcfree = pointcloud.PointCloud(freepts)
76
iros_free_map.add_points(pcfree,None)
78
iros_free_map.display(npts=1000000)
83
pc = pointcloud.load(fname)
84
pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
86
pc = get_pointcloud(fname)
87
bestpose = iros_map.align_points(pc, bestpose)[0]
88
iros_map.add_points(pc, bestpose)
90
freepts = get_freespace(pc.points, bestpose, iros_map)
91
pcfree = pointcloud.PointCloud(freepts)
92
iros_free_map.add_points(pcfree,None)
95
iros_map.save(outfname)
97
iros_free_map.save(outfname+"free")
98
print 'Map size:', len(iros_map), 'voxels.'
99
print 'Map saved to', outfname