← Back to branch summary

~mrol-dev/mrol/trunk

« back to all changes in this revision

Viewing changes to mrol_mapping/occupiedlist.py

  • Committer: Vikas Dhiman
  • Date: 2012-11-16 19:59:46 UTC
  • Revision ID: vikasdhi@buffalo.edu-20121116195946-fwfdqevt3h3eqoyg
Manually added julians changes after merge failed. "Added function to save point cloud in the .ply format"; "Got speed_test working again after API changes.

Show diffs side-by-side

added added

removed removed

Lines of Context:
mrol_mapping/occupiedlist.py
55
55
    ol.add_points(xyzs[:, 0:3])
56
56
    return ol
57
57
 
58
 
def pointstovoxels(X, resolution=None, pose=None, latticetype='cubic', out=None):
 
58
def pointstovoxels(X, resolution, pose=None, latticetype='cubic', out=None):
59
59
    '''Converts a list of points to integer voxel indices. Optionally
60
60
    transforming by pose as well for speed.'''
61
61
    # Possible lattice types currently include cubic, bcc and fcc.
70
70
    # combines transformation and scaling into one operation for speed
71
71
    X, pose = poseutil.transformPoints(X, pose, scale=1.0/resolution)
72
72
    #out[:, :3] = np.round(X)
73
 
    out[:, :3] = X + 0.5
74
 
    #out[:, :3] = np.floor(X+0.5)
75
 
 
 
73
    # out[:, :3] = X + 0.5 # this does not work quite right
 
74
    # out[:, :3] = np.floor(X) # without multi-resolution offsets
 
75
    out[:, :3] = np.floor(X + 0.5) # with multi resolution offsets but they do not seem to help that much
76
76
    return out
77
77
 
78
78
def voxelstopoints(A, resolution):
79
79
    """ Converts an array of integers voxel indices to an array of points """
80
80
    # TODO make this work for alternative lattices
81
81
    return A*resolution
 
82
    #return (A + 0.5)*resolution # without multi-resolution offsets
82
83
 
83
84
def dilate(voxels):
84
85
    '''Add adjacent voxels, effectively blurring or smearing occupied voxel 
118
119
    ids.  the last column are zeros.'''
119
120
    assert inds.dtype == np.int16, 'inds needs to be np.int16'
120
121
    assert inds.shape[1] == 4 
 
122
    assert not np.isfortran(inds), 'Incorrect array memory layout'
121
123
 
122
124
    inds.dtype = np.int64
123
125
    inds.shape = -1
155
157
        return np.all(self.bloomtable == bf.bloomtable)
156
158
 
157
159
    def djbhash(self, i):
158
 
        """Hash function from D J Bernstein"""
 
160
        '''Hash function from D J Bernstein'''
159
161
        h = 5381L
160
162
        t = (h * 33) & 0xffffffffL
161
163
        h = t ^ i
162
164
        return h
163
165
 
164
166
    def fnvhash(self, i):
165
 
        """Fowler, Noll, Vo Hash function"""
 
167
        '''Fowler, Noll, Vo Hash function'''
166
168
        h = 2166136261
167
169
        t = (h * 16777619) & 0xffffffffL
168
170
        h = t ^ i
174
176
        # Need a hash function that that can generate multiple hashes for given 
175
177
        # input and over a specified range
176
178
        inds = np.empty((2, ids.shape[0]), np.int64)
 
179
        # use id values themselves as hash dangerous and quicker but when benchmarked seemed to make little difference
 
180
        # inds[0] = ids 
177
181
        inds[0] = self.fnvhash(ids)
178
182
        inds[1] = self.djbhash(ids)
179
183
        S = self.size