~wecacuee/mrol/mrol-dev

« back to all changes in this revision

Viewing changes to mrol_mapping/occupiedlist.py

  • Committer: Vikas Dhiman
  • Date: 2012-05-22 15:26:38 UTC
  • Revision ID: wecacuee@gmail.com-20120522152638-j6srlkgrpdq4kdfe
Added support for userdata in terms of colored voxels. Added benchmark test and corresponding data

Show diffs side-by-side

added added

removed removed

Lines of Context:
9
9
#along with this program.  If not, see <http://www.gnu.org/licenses/>.
10
10
 
11
11
from __future__ import division
12
 
import collections
13
12
import numpy as np
14
13
import scipy.stats as stats
15
14
 
 
15
import itertools
16
16
import poseutil
17
17
import quantizer
18
18
import pointcloud
122
122
    inds.dtype = np.int64
123
123
    inds.shape = -1
124
124
    
125
 
def _fast_count(ints):
 
125
def _fast_count(ints, return_index=False):
126
126
    #counted_ids = collections.Counter(ids) # collections.Counter turns out to be slow
127
127
 
128
128
    # Fast counting of integers although might not scale to large N well
129
129
    # because of the sort done by unique?
130
 
    uniqs, inds = np.unique(ints, return_inverse=True)
131
 
    counts = np.bincount(inds)
132
 
    return uniqs, counts
 
130
    if return_index:
 
131
        uniqs, uniqinds, inverseinds = np.unique(ints, return_index=True,
 
132
                                          return_inverse=True)
 
133
        counts = np.bincount(inverseinds)
 
134
        return uniqs, counts, uniqinds
 
135
    else:
 
136
        uniqs, inds = np.unique(ints, return_inverse=True)
 
137
        counts = np.bincount(inds)
 
138
        return uniqs, counts
133
139
 
134
140
class BloomFilter:
135
141
    '''Special case counting bloom filter optimized for voxel indices'''
249
255
        # of packing 4 np.int16
250
256
        # the content of the dictionary is either an np.int16 holding
251
257
        # increments, or TODO a userdefined structure
252
 
        self._voxels = collections.Counter()
 
258
        self._voxels = dict()
253
259
        self.lattice = 'cubic' # should be one of the latticefuncs
254
260
        self._use_bloom = use_bloom
255
261
        self.use_KDE = False
350
356
        
351
357
    def getvoxeldata(self):
352
358
        '''Returns both voxel indices and assocated data'''
353
 
        voxels = self.getvoxels()
354
 
        vox_val = np.array(self._voxels.values())
355
 
        return np.hstack([voxels, vox_val.reshape(vox_val.shape[0], 1)])
 
359
        voxels = self.getvoxels() * self.resolution
 
360
        if not len(voxels):
 
361
            return voxels
 
362
        voxdtype = self._voxels.itervalues().next().dtype
 
363
        if ('userdata' in voxdtype.names):
 
364
            vox_val = np.array(self._voxels.values(), dtype=voxdtype)
 
365
            voxels = np.hstack([voxels, vox_val['userdata']])
 
366
        return voxels
356
367
 
357
368
    def getpoints(self):
358
369
        return voxelstopoints(self.getvoxels(), self.resolution)
403
414
        # merging multiple increments of the same voxel
404
415
        # count number of added points for each voxel
405
416
 
406
 
        uniqs, counts = _fast_count(increment_inds)
 
417
        uniqs, counts, uniqinds = _fast_count(increment_inds,
 
418
                                              return_index=True)
407
419
        # uniqs, counts are the unique values and their corresponding counts
408
420
 
409
 
        # TODO there is probably a bug if increment is 0 so remove increments of zero
410
 
        # query the map for occupancies of these voxels
411
 
        occupancies = np.array([self._voxels[ID] for ID in uniqs])
 
421
        if userdata is not None:
 
422
            # remove duplicates from userdata
 
423
            userdata = userdata[uniqinds]
 
424
            voxdtype = np.dtype([('count', int),
 
425
                                 ('userdata',
 
426
                                  userdata.dtype, userdata.shape[1])
 
427
                                ])
 
428
        else:
 
429
            if len(self._voxels):
 
430
                # make zeros consistent with existing self._voxels
 
431
                firstval = self._voxels.itervalues().next()
 
432
                voxdtype = firstval.dtype
 
433
            else:
 
434
                voxdtype = np.dtype([('count', int)])
 
435
        # perhaps a hack to get zero value
 
436
        zeros = np.zeros(1, dtype=voxdtype)[0] 
 
437
        newcounts = counts * increment
 
438
 
 
439
        value_uniqs = np.array([self._voxels.get(ID, zeros) for ID in uniqs],
 
440
                               dtype=voxdtype)
 
441
        occupancies = value_uniqs['count']
412
442
 
413
443
        # counter values are initialized with zero. Non-zero values indicate
414
444
        # pre-existance
415
 
        existing_ids = occupancies != 0
416
 
 
 
445
        existing_ids = (occupancies != zeros['count']).reshape(-1)
 
446
        
 
447
        # #############
 
448
        # actual update
 
449
        # #############
417
450
        # add map occupancies with collected voxel counts of points to be added
418
 
        occupancies += (counts * increment)
 
451
        if userdata is not None:
 
452
            value_uniqs['userdata'] = userdata
 
453
        value_uniqs['count'] = value_uniqs['count'] + newcounts
 
454
        occupancies = value_uniqs['count']
419
455
 
420
456
        # split into two groups, those that were changed and those that require
421
457
        # removal
422
458
 
423
459
        # delete any less than 1
424
 
        inds = occupancies < self.occupancy_threshold
 
460
        inds = (occupancies < self.occupancy_threshold)
425
461
        removed_ids = uniqs[inds]
426
462
        removed_existing = uniqs[inds & existing_ids]
 
463
 
427
464
        changed_ids = uniqs[np.logical_not(inds)]
428
 
        changed_occupancies = occupancies[np.logical_not(inds)]
 
465
        changed_values = value_uniqs[np.logical_not(inds)]
429
466
 
430
467
        logger.info("Removing:%d" % len(removed_existing))
431
 
        for ID in removed_ids:
 
468
        for ID in removed_existing:
432
469
            del self._voxels[ID]
433
470
 
 
471
 
434
472
        # set those with occupancy 1 or more 
435
473
        # set occupancies of voxels with their new values
436
 
        for i, ID in enumerate(changed_ids):
437
 
            self._voxels[ID] = changed_occupancies[i]
 
474
        self._voxels.update(itertools.izip(changed_ids, changed_values))
438
475
 
439
476
        #for ID in increment_inds:
440
477
        #    if ID not in self._voxels: