~wecacuee/mrol/mrol-dev

« back to all changes in this revision

Viewing changes to mrol_mapping/mapper.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:
26
26
logger.setLevel(logging.INFO)
27
27
USE_BACKTRACING = True
28
28
 
29
 
 
30
29
_map_resolution_levels = 4 # number of different levels of resolution
31
30
 
32
31
def loadmap(fname):
33
32
    return pickle.load(open(fname))
34
33
 
 
34
class MapBuilder(object):
 
35
    def __init__(self, resolution,
 
36
                 visualise=True,
 
37
                 levels=_map_resolution_levels,
 
38
                 odometry_map=False,
 
39
                 rotate=False):
 
40
        self.vmap = VoxelMap(resolution, levels=levels,
 
41
                             rotate=rotate)
 
42
        self.odometry_map  = odometry_map
 
43
 
 
44
        self.bestpose = poseutil.Pose3D()
 
45
        self.scancount = 0
 
46
        if visualise:
 
47
            self.vmap.display()
 
48
 
 
49
    def add_pointcloud(self, pc):
 
50
        if self.scancount == 0:
 
51
            self.scancount = 1
 
52
            self.vmap.add_scan(pc, pc.pose)
 
53
            pc.pose = (poseutil.Pose3D() if pc.pose is None else pc.pose)
 
54
            return self.vmap.get_voxels(level=-1), pc.pose
 
55
 
 
56
        # reduce number of scan points to one per voxel
 
57
        # sample = pc.volumetricsample(self.vmap.resolution)
 
58
 
 
59
        logger.info('--------- Scan %d ---------' % self.scancount)
 
60
        self.scancount += 1
 
61
 
 
62
        if pc.pose is None: 
 
63
            guessPose = self.bestpose 
 
64
        else:
 
65
            guessPose = pc.pose
 
66
 
 
67
        start = time.time()
 
68
        if not self.odometry_map:
 
69
            self.bestpose, maxcost = self.vmap.align_points(pc, guessPose)
 
70
 
 
71
        alignment_time = time.time() - start
 
72
 
 
73
        start = time.time()
 
74
        if self.odometry_map: # create map without aligning
 
75
            self.vmap.add_scan(pc, guessPose) 
 
76
        else:
 
77
            self.vmap.add_scan(pc, self.bestpose)
 
78
 
 
79
        mapupdate_time = time.time() - start
 
80
        logger.info('Map voxel count:{0}'.format( len(self.vmap)))
 
81
 
 
82
        logger.info('Aligned pose:{0}'.format(self.bestpose))
 
83
        logger.info('Cost value: {0}'.format(maxcost))
 
84
 
 
85
        logger.info('Alignment time(s):  %.2f' % alignment_time)
 
86
        logger.info('Map update time(s): %.2f' % mapupdate_time)
 
87
        return self.vmap.get_voxels(level=-1), self.bestpose
 
88
 
35
89
def buildmap(pointclouds, resolution, visualise=True, odometry_map=False,
36
90
             rotate=False,
37
91
             bestpose_queue=None,
43
97
    optionally communicate back the bestpose got so far. This might be used to
44
98
    optimize the pc.pose i.e. the initial guesspose
45
99
    '''
46
 
 
47
100
    vmap = VoxelMap(resolution, levels=_map_resolution_levels, rotate=rotate)
48
101
    pc = pointclouds.next() 
49
102
    vmap.add_scan(pc, pc.pose)
223
276
        #else:
224
277
            #self._disp.se
225
278
 
226
 
    def add_scan(self, xyzs, pose, timestamp=None, userdata=None):
 
279
    def add_scan(self, pc, pose, timestamp=None):
227
280
        '''Adds a new scan to the existing map.
228
281
 
229
282
        Adds the occupied voxels and removes the freespace.
230
283
        '''
 
284
        userdata = pc.userdata
 
285
        xyzs = pc.points
231
286
        if USE_BACKTRACING:
232
287
            self.clear_freespace(xyzs, pose)
233
288
        self.add_points(xyzs, pose, timestamp, userdata)
239
294
            xyzs: The occupied PointCloud, with the sensor as origin.
240
295
            pose: The pose of `xyzs` PointCloud with respect to global map.
241
296
        '''
242
 
        xyzs = xyzs.points
243
297
        xyzs, return_pose = poseutil.transformPoints(xyzs, pose)
244
298
        sensor_loc = poseutil.transformPoints(np.array([[0, 0, 0]]), pose)
245
299
        self.mrol.clear_freespace(xyzs, sensor_loc[0])
248
302
        '''Adds the pointcloud xyzs to the map, transforming them by the given 
249
303
        pose if required. Returns the overlap of the new points against the 
250
304
        existing map.'''
251
 
        xyzs = xyzs.points
252
 
        if xyzs.shape[1] > 3: # handling for additional information that is part of the pointcloud object (e.g. colour)
253
 
            if userdata == None:
254
 
                userdata = xyzs[:,3:]
255
 
            else:
256
 
                userdata = np.hstack([xyzs[:,3:],userdata])
257
 
            xyzs = xyzs[:,:3]
 
305
        assert xyzs.shape[1] == 3, '3 dimensional points'
258
306
        xyzs, return_pose = poseutil.transformPoints(xyzs, pose)
259
307
        self.trajectory.append(return_pose)
260
308
        if self._rv is not None: # visualisation
264
312
            ol = finest
265
313
            #ol.add_points(xyzs)
266
314
            #ol = ol - finest
267
 
            self._rv.addmappts(ol.getpoints())
 
315
            vox = ol.getvoxeldata()
 
316
            self._rv.addmappts(vox)
268
317
            #self._rv.setminmax(np.min(self.get_points().points[:,2]),np.max(self.get_points().points[:,2]))
269
318
            self._rv.setrobotpose(return_pose.getMatrix())
270
319
            self._rv.addtrajectorypoint(return_pose.getTuple()[0:3])