← Back to branch summary

~mrol-dev/mrol/trunk

« back to all changes in this revision

Viewing changes to mrol_mapping/pointcloud.py

  • Committer: Vikas Dhiman
  • Date: 2012-05-14 16:28:10 UTC
  • Revision ID: wecacuee@gmail.com-20120514162810-gqqevqvisr4qyi5r
Backtracing must be working fine now.

Show diffs side-by-side

added added

removed removed

Lines of Context:
mrol_mapping/pointcloud.py
58
58
    return PointCloud(X[:,:X.shape[1]])
59
59
    
60
60
def save(fname, xyzs, text=False):
61
 
    '''Save a points in Nx3 arrays to disk. By default it is saved as a numpy 
 
61
    '''Save a point cloud to disk. By default it is saved as a numpy 
62
62
    binary array, but it can be saved as plain text by setting the 
63
63
    'text' flag'''
64
64
    if text:
66
66
    else:
67
67
        np.save(fname,xyzs)
68
68
 
69
 
def save_ply(fname, xyzs):
70
 
    ''' Save points in Nx3 arrays as .ply Polygon File Format.  This format is
71
 
    popular and can be loaded by programs such as blender, meshlab and the
72
 
    point cloud library. '''
73
 
 
74
 
    header = '''ply
75
 
format ascii 1.0
76
 
element vertex %d
77
 
property float x
78
 
property float y
79
 
property float z
80
 
end_header
81
 
''' % len(xyzs)
82
 
 
83
 
    # TODO new version of numpy.savetxt will support header option
84
 
    F = open(fname, 'w')
85
 
    F.write(header)
86
 
    for x, y, z in xyzs:
87
 
        F.write('%.03f %.03f %.03f\n' % (x, y, z))
88
 
    return
89
 
 
90
69
class PointCloud:
91
70
    # TODO point clouds should be stored as ints in millimetres?
92
71
 
97
76
        self.RGB = False # set if the point cloud has RGB data associated
98
77
        self.add_points(arr, timestamp)
99
78
        self.pose = None
100
 
        self.userdata = None
101
79
 
102
80
    def __eq__(self, pc, tol=0.00001):
103
81
        # TODO pick a proper value for this tol, like np.allclose
135
113
    def save(self, filename):
136
114
        save(filename, self.points)
137
115
 
138
 
    def save_ply(self, filename):
139
 
        save_ply(filename, self.points)
140
 
 
141
116
    # TODO: pose transform operator, may make addition/subtraction of 3x1 array redundant?
142
117
        
143
118
    def add_points(self, arr, timestamp=0):
151
126
            self.RGB = True                
152
127
        self.timestamp = timestamp
153
128
 
154
 
    def display(self, color=None, title='', scene=None):
155
 
        ''' Displays the point cloud in a visual window.  To display in an
156
 
        existing window supply visual scene object like the one returned by
157
 
        this function. '''
 
129
    def display(self, color=None, title=''):
158
130
        import visual # import here only if needed
159
131
        # import here to avoid circular dependency
160
132
        import mrol_mapping.visualiser.dispxyz as dispxyz
165
137
                color = self.points[:,3:6]/np.max(self.points[:,3:6])
166
138
        else:
167
139
            xyzs = self.points
168
 
 
169
 
        if scene is None:
170
 
            scene = visual.display(title=title + 'dispxyz')
 
140
        scene = visual.display(title=title + 'dispxyz')
171
141
 
172
142
        #scene.exit = False
173
143
        MT = dispxyz.EnableMouseThread(scene)