← Back to branch summary

~wecacuee/mrol/mrol-dev

« back to all changes in this revision

Viewing changes to experimental/cup_mapper.py

  • Committer: Julian Ryde
  • Date: 2011-06-03 16:52:15 UTC
  • Revision ID: julian_ryde-20110603165215-oqr6yo9nwng3j9nb
Initial import from revision 206 of Julian Ryde's mrol_mapping repository.

Show diffs side-by-side

added added

removed removed

Lines of Context:
experimental/cup_mapper.py
 
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
 
7
 
 
8
 
 
9
import mrol_mapping.occupiedlist as occupiedlist
 
10
import mrol_mapping.quantizer as quantizer
 
11
 
 
12
import os
 
13
import numpy as np
 
14
import sys
 
15
 
 
16
sys.path.append('../test')
 
17
import depth_image
 
18
 
 
19
res = 0.01
 
20
visualise = True
 
21
make_free = False
 
22
load_pc = True
 
23
 
 
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)
 
29
    return pc
 
30
    
 
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])
 
40
    return free_pts
 
41
        
 
42
    
 
43
 
 
44
if __name__ == '__main__':
 
45
    try:
 
46
        datadir = sys.argv[1]
 
47
        outfname = sys.argv[2]+".map"
 
48
        fnames = os.listdir(datadir)
 
49
        fnames.sort()
 
50
        if load_pc:
 
51
            fnames = [datadir + '/' + fname for fname in fnames if fname.startswith('mrolpc')]
 
52
        else:
 
53
            fnames = [datadir + '/' + fname for fname in fnames if fname.endswith('.png')]
 
54
    except:
 
55
        print 'Need to specify a valid directory for input and file name for output'
 
56
        sys.exit()
 
57
 
 
58
    # variable initialisation
 
59
    iros_map = mapper.VoxelMap(res,levels=3)
 
60
    iros_free_map = mapper.VoxelMap(res,levels=1)
 
61
    bestpose = poseutil.Pose3D()
 
62
    
 
63
    if load_pc:
 
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.))
 
68
    else:
 
69
        pc = get_pointcloud(fnames.pop(0))
 
70
    iros_map.add_points(pc, bestpose)
 
71
    if visualise:
 
72
        iros_map.display(changes=False)
 
73
    if make_free:
 
74
        freepts = get_freespace(pc.points, bestpose, iros_map)
 
75
        pcfree = pointcloud.PointCloud(freepts)
 
76
        iros_free_map.add_points(pcfree,None)
 
77
        if visualise:
 
78
            iros_free_map.display(npts=1000000)
 
79
 
 
80
    for fname in fnames:
 
81
        print fname,
 
82
        if load_pc:
 
83
            pc = pointcloud.load(fname)
 
84
            pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
 
85
        else:
 
86
            pc = get_pointcloud(fname)
 
87
        bestpose = iros_map.align_points(pc, bestpose)[0]
 
88
        iros_map.add_points(pc, bestpose)
 
89
        if make_free:
 
90
            freepts = get_freespace(pc.points, bestpose, iros_map)
 
91
            pcfree = pointcloud.PointCloud(freepts)
 
92
            iros_free_map.add_points(pcfree,None)
 
93
        print bestpose
 
94
 
 
95
    iros_map.save(outfname)
 
96
    if make_free:
 
97
        iros_free_map.save(outfname+"free")
 
98
    print 'Map size:', len(iros_map), 'voxels.'
 
99
    print 'Map saved to', outfname