← Back to branch summary

~mrol-dev/mrol/trunk

« back to all changes in this revision

Viewing changes to mrol_mapping/occupiedlist.py

  • Committer: Julian Ryde
  • Date: 2012-03-04 02:43:54 UTC
  • mto: (16.1.12 trunk) (29.1.2 trunk)
  • mto: This revision was merged to the branch mainline in revision 25.
  • Revision ID: julian_ryde-20120304024354-yjf7woyr2bsd6vyn
Fixed mapper_test.  Updated the ground truth poses so that test passes aligning all simulated scans.

Show diffs side-by-side

added added

removed removed

Lines of Context:
mrol_mapping/occupiedlist.py
22
22
 
23
23
verbose = False
24
24
 
 
25
inds = np.indices((3,3,3)) - 1
 
26
inds.shape = (3, 27)
 
27
inds = inds.T
 
28
offset_inds = np.zeros((27, 4), dtype=np.int16)
 
29
offset_inds[:,:3] = inds
 
30
 
25
31
""" Dictionary of the currently implemented lattice quantizers """
26
32
latticefuncs = {'cubic':quantizer.quantize_cubic, 'bcc':quantizer.quantize_bcc, 'fcc':quantizer.quantize_fcc}
27
33
 
58
64
 
59
65
    # combines transformation and scaling into one operation for speed
60
66
    X, pose = poseutil.transformPoints(X, pose, scale=1.0/resolution)
61
 
    #import pdb; pdb.set_trace()
62
67
    #out[:, :3] = np.round(X)
63
68
    out[:, :3] = X + 0.5
64
69
    #out[:, :3] = np.floor(X+0.5)
510
515
        """ saves the finest occupied list to fname in x y z format """
511
516
        np.savetxt(fname, self.getpoints())
512
517
 
513
 
    def calccollisions(self, pose, xyzs, returntotal=True, query=False, out=None):
 
518
    def calccollisions(self, pose, xyzs, returntotal=True, query=False):
514
519
        '''The intersection of points with this occupied voxel list.
515
520
        If query then  return the density for each point after transformation 
516
 
        by the given pose, rather than just a count of hits.  out is an output 
517
 
        array for the result similar to numpy function.
 
521
        by the given pose, rather than just a count of hits.  
518
522
        '''
519
523
 
520
524
        # reuse transformed voxels to save initialising the fourth column it to
528
532
        _pack(self._transformedvoxels)
529
533
 
530
534
        if query:
531
 
            if out is None:
532
 
                return [self._voxels[ID] for ID in self._transformedvoxels]
533
 
            else:
534
 
                # TODO implement this
535
 
                raise NotImplemented
 
535
            return [self._voxels[ID] for ID in self._transformedvoxels]
536
536
            #return sum(self._voxels[ID] for ID in self._transformedvoxels)
537
537
 
538
538
        if self._use_bloom:
542
542
            else:
543
543
                return present
544
544
        else:
545
 
            # no range weighting TODO decide whether it is needed
546
 
            # Think the range weighting has been fixed by volumetric sampling
547
545
            if returntotal:
548
546
                return sum(ID in self._voxels for ID in self._transformedvoxels)
549
547
            else: