9
9
#along with this program. If not, see <http://www.gnu.org/licenses/>.
11
11
from __future__ import division
13
14
import scipy.stats as stats
19
from bresenhamline import bresenhamline
23
20
#import numexpr as ne
24
21
#ne.set_num_threads(4)
26
logger = log.getLogger('occupiedlist')
27
logger.setLevel(logging.INFO)
28
NBACKTRACE_VOXELS = 10
30
25
inds = np.indices((3,3,3)) - 1
31
26
inds.shape = (3, 27)
55
50
ol.add_points(xyzs[:, 0:3])
58
def pointstovoxels(X, resolution, pose=None, latticetype='cubic', out=None):
53
def pointstovoxels(X, resolution=None, pose=None, latticetype='cubic', out=None):
59
54
'''Converts a list of points to integer voxel indices. Optionally
60
55
transforming by pose as well for speed.'''
61
56
# Possible lattice types currently include cubic, bcc and fcc.
70
65
# combines transformation and scaling into one operation for speed
71
66
X, pose = poseutil.transformPoints(X, pose, scale=1.0/resolution)
72
67
#out[:, :3] = np.round(X)
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
69
#out[:, :3] = np.floor(X+0.5)
78
73
def voxelstopoints(A, resolution):
79
74
""" Converts an array of integers voxel indices to an array of points """
80
75
# TODO make this work for alternative lattices
81
76
return A*resolution
82
#return (A + 0.5)*resolution # without multi-resolution offsets
84
78
def dilate(voxels):
85
79
'''Add adjacent voxels, effectively blurring or smearing occupied voxel
119
113
ids. the last column are zeros.'''
120
114
assert inds.dtype == np.int16, 'inds needs to be np.int16'
121
115
assert inds.shape[1] == 4
122
assert not np.isfortran(inds), 'Incorrect array memory layout'
124
117
inds.dtype = np.int64
127
def _fast_count(ints, return_index=False):
120
def _fast_count(ints):
128
121
#counted_ids = collections.Counter(ids) # collections.Counter turns out to be slow
130
123
# Fast counting of integers although might not scale to large N well
131
124
# because of the sort done by unique?
133
uniqs, uniqinds, inverseinds = np.unique(ints, return_index=True,
135
counts = np.bincount(inverseinds)
136
return uniqs, counts, uniqinds
138
uniqs, inds = np.unique(ints, return_inverse=True)
139
counts = np.bincount(inds)
125
uniqs, inds = np.unique(ints, return_inverse=True)
126
counts = np.bincount(inds)
142
129
class BloomFilter:
143
130
'''Special case counting bloom filter optimized for voxel indices'''
157
144
return np.all(self.bloomtable == bf.bloomtable)
159
146
def djbhash(self, i):
160
'''Hash function from D J Bernstein'''
147
"""Hash function from D J Bernstein"""
162
149
t = (h * 33) & 0xffffffffL
166
153
def fnvhash(self, i):
167
'''Fowler, Noll, Vo Hash function'''
154
"""Fowler, Noll, Vo Hash function"""
169
156
t = (h * 16777619) & 0xffffffffL
176
163
# Need a hash function that that can generate multiple hashes for given
177
164
# input and over a specified range
178
165
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
181
166
inds[0] = self.fnvhash(ids)
182
167
inds[1] = self.djbhash(ids)
254
239
def __init__(self, resolution, maxvoxels=1e6, use_bloom=True):
255
240
self.resolution = resolution
256
241
self._transformedvoxels = None
259
243
# voxels is a dictionary with keys of np.int64 IDs that are the result
260
244
# of packing 4 np.int16
261
245
# the content of the dictionary is either an np.int16 holding
262
246
# increments, or TODO a userdefined structure
263
self._voxels = dict()
247
self._voxels = collections.Counter()
264
248
self.lattice = 'cubic' # should be one of the latticefuncs
265
249
self._use_bloom = use_bloom
266
250
self.use_KDE = False
346
329
Getting empty voxels should not change ol
348
331
>>> ol = OccupiedList(0.1)
349
>>> ol.get_occupancies( np.array(((1,2,3),)) )
332
>>> ol.get_occupancies( ((1,2,3),) )
354
337
ids = _4_column_int16(voxel_inds)
356
return np.array([self._voxels.get(ID, 0) for ID in ids])
339
return np.array([self._voxels[ID] for ID in ids])
358
341
def getvoxels(self):
359
342
ids = np.asarray(self._voxels.keys())
363
346
def getvoxeldata(self):
364
347
'''Returns both voxel indices and assocated data'''
365
voxels = self.getvoxels() * self.resolution
368
voxdtype = self._voxels.itervalues().next().dtype
369
if ('userdata' in voxdtype.names):
370
vox_val = np.array(self._voxels.values(), dtype=voxdtype)
371
voxels = np.hstack([voxels, vox_val['userdata']])
348
return self.getvoxels(), np.array(self._voxels.values())
374
350
def getpoints(self):
375
351
return voxelstopoints(self.getvoxels(), self.resolution)
405
381
if self._voxels[ID][0] < self.occupancy_threshold:
406
382
del self._voxels[ID]
407
383
self.removed_ids.append(ID)
409
def _extract_voxdtype(self, userdata):
410
if userdata is not None:
411
# remove duplicates from userdata
412
voxdtype = np.dtype([('count', int),
414
userdata.dtype, userdata.shape[1])
417
if len(self._voxels):
418
# make zeros consistent with existing self._voxels
419
firstval = self._voxels.itervalues().next()
420
voxdtype = firstval.dtype
422
voxdtype = np.dtype([('count', int)])
425
def _check_voxdtype(self, userdata):
426
voxdtype = self._extract_voxdtype(userdata)
427
if self.voxdtype is None:
428
self.voxdtype = voxdtype
430
assert voxdtype == self.voxdtype
433
return np.zeros(1, dtype=self.voxdtype)[0]
435
385
def _update_voxels(self, increment_inds, increment, userdata=None):
436
386
#ids = _4_column_int16(increment_inds)
443
393
# Generate a list of newly added voxels and removed voxels for the
444
394
# bloom table update, and do any voxel merging operations
445
logger.info('Number of points to add: %d' % len(increment_inds))
396
print 'Number of points to add: ', len(increment_inds)
446
397
# merging multiple increments of the same voxel
447
398
# count number of added points for each voxel
449
uniqs, counts, uniqinds = _fast_count(increment_inds,
451
if userdata is not None:
452
userdata = userdata[uniqinds]
400
uniqs, counts = _fast_count(increment_inds)
453
401
# uniqs, counts are the unique values and their corresponding counts
454
self._check_voxdtype(userdata)
456
# perhaps a hack to get zero value
457
zeros = self._zeros()#np.zeros(1, dtype=voxdtype)[0]
458
newcounts = counts * increment
459
value_uniqs = np.array([self._voxels.get(ID, zeros) for ID in uniqs],
461
occupancies = value_uniqs['count']
463
# counter values are initialized with zero. Non-zero values indicate
465
existing_ids = (occupancies != zeros['count']).reshape(-1)
403
# TODO there is probably a bug if increment is 0 so remove increments of zero
405
# query the map for occupancies of these voxels
406
occupancies = np.array([self._voxels[ID] for ID in uniqs])
470
407
# add map occupancies with collected voxel counts of points to be added
471
if userdata is not None:
472
value_uniqs['userdata'] = userdata
473
value_uniqs['count'] = value_uniqs['count'] + newcounts
474
occupancies = value_uniqs['count']
408
occupancies += counts
476
410
# split into two groups, those that were changed and those that require
479
413
# delete any less than 1
480
inds = (occupancies < self.occupancy_threshold)
414
inds = occupancies < self.occupancy_threshold
481
415
removed_ids = uniqs[inds]
482
removed_existing = uniqs[inds & existing_ids]
484
416
changed_ids = uniqs[np.logical_not(inds)]
485
changed_values = value_uniqs[np.logical_not(inds)]
487
logger.info("Removing:%d" % len(removed_existing))
488
for ID in removed_existing:
417
changed_occupancies = occupancies[np.logical_not(inds)]
418
for ID in removed_ids:
489
419
del self._voxels[ID]
492
421
# set those with occupancy 1 or more
493
422
# set occupancies of voxels with their new values
494
self._voxels.update(itertools.izip(changed_ids, changed_values))
423
for i, ID in enumerate(changed_ids):
424
self._voxels[ID] = changed_occupancies[i]
496
426
#for ID in increment_inds:
497
427
# if ID not in self._voxels:
503
433
#new_ids = np.asarray(new_ids)
504
434
if self._use_bloom:
505
435
self.bloomfilter.add_voxel_ids(changed_ids)
506
self.bloomfilter.remove_voxel_ids(removed_existing)
508
def clear_freespace(self, xyzs, sensor_location):
510
Clear the voxels in global map that we are quite certain about not
514
xyzs : points with sensor at origin. The points have not been
515
transformed to global map coordinates. But the `pose` provides the
516
exact transformation required for converting to global map.
517
pose : The pose that will map the points `xyzs` to global map.
519
if not len(self._voxels):
521
# TODO should really take a point cloud object rather than xyzs
524
xyzs = np.atleast_2d(xyzs) # for single points
525
assert xyzs.shape[1] == 3, 'Points should be in a n by 3 array format'
527
inds = pointstovoxels(xyzs, resolution=self.resolution, latticetype=self.lattice)
528
sensor_voxel = pointstovoxels(sensor_location,
529
resolution=self.resolution, latticetype=self.lattice)
531
free_voxels = bresenhamline(inds, sensor_voxel,
532
max_iter=NBACKTRACE_VOXELS)
534
# equivalent of remove points
535
self._update_voxels(free_voxels, -1)
536
assert len(self._voxels) < self.maxvoxels, 'Max voxels exceeded for bloom table'
436
self.bloomfilter.remove_voxel_ids(removed_ids)
539
438
# TODO do add_points and removepoints properly by apportioning the point
540
439
# probability amongst surrounding voxels, like kernel density estimation.
added
removed
mrol_mapping/occupiedlist.py
