← Back to branch summary

~mrol-dev/mrol/trunk

« back to all changes in this revision

Viewing changes to mrol_mapping/mapper.py

  • Committer: Vikas Dhiman
  • Date: 2012-03-12 19:10:56 UTC
  • Revision ID: wecacuee@gmail.com-20120312191056-31xyar8xpyhwgxwb
Fixed doc test in occupancy_list.py.

Show diffs side-by-side

added added

removed removed

Lines of Context:
mrol_mapping/mapper.py
19
19
import cPickle as pickle
20
20
import quantizer
21
21
import time
22
 
import log
23
 
import logging
24
 
 
25
 
logger = log.getLogger('mapper')
26
 
logger.setLevel(logging.INFO)
27
 
USE_BACKTRACING = True
28
 
 
 
22
 
 
23
debug = True
29
24
_map_resolution_levels = 4 # number of different levels of resolution
30
25
 
31
26
def loadmap(fname):
32
27
    return pickle.load(open(fname))
33
28
 
34
 
class MapBuilder(object):
35
 
    def __init__(self, resolution,
36
 
                 visualise=False,
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
 
 
89
29
def buildmap(pointclouds, resolution, visualise=True, odometry_map=False,
90
 
             rotate=False,
91
30
             bestpose_queue=None,
92
31
             voxel_queue=None):
93
32
    '''Takes an iterable of point clouds which can have guess poses and produce 
97
36
    optionally communicate back the bestpose got so far. This might be used to
98
37
    optimize the pc.pose i.e. the initial guesspose
99
38
    '''
100
 
    vmap = VoxelMap(resolution, levels=_map_resolution_levels, rotate=rotate)
 
39
 
 
40
    vmap = VoxelMap(resolution, levels=_map_resolution_levels)
 
41
 
101
42
    pc = pointclouds.next() 
102
 
    vmap.add_scan(pc, pc.pose)
103
 
 
104
 
    logger.info('--------- Scan 0 ---------')
105
 
    logger.info('Map voxel count:{0}'.format(len(vmap)))
 
43
    vmap.add_points(pc, pc.pose)
 
44
    if debug:
 
45
        print '--------- Scan 0 ---------'
 
46
        print 'Map voxel count:', len(vmap)
106
47
 
107
48
    if visualise:
108
49
        vmap.display()
122
63
        # reduce number of scan points to one per voxel
123
64
        sample = pc.volumetricsample(resolution)
124
65
 
125
 
        logger.info('--------- Scan %d ---------' % i)
 
66
        if debug:
 
67
            print '--------- Scan %d ---------' % i
126
68
 
127
69
        if pc.pose is None: 
128
70
            guessPose = bestpose 
133
75
        if not odometry_map:
134
76
            bestpose, maxcost = vmap.align_points(sample, guessPose)
135
77
            if bestpose_queue:
136
 
                logger.info("Best pose {0}".format(bestpose))
137
78
                bestpose_queue.put_force(bestpose)
138
79
 
139
80
        alignment_time = time.time() - start
140
81
 
141
82
        start = time.time()
142
83
        if odometry_map: # create map without aligning
143
 
            vmap.add_scan(pc, guessPose) 
 
84
            vmap.add_points(pc, guessPose) 
144
85
        else:
145
 
            vmap.add_scan(pc, bestpose)
 
86
            vmap.add_points(pc, bestpose)
146
87
 
147
88
        if voxel_queue:
148
89
            (vdata, vres) = vmap.get_voxels(level=-1)
149
90
            voxel_queue.put_force((vdata, vres))
150
91
 
151
92
        mapupdate_time = time.time() - start
152
 
        logger.info('Map voxel count:{0}'.format( len(vmap)))
153
 
 
154
 
        logger.info('Aligned pose:{0}'.format(bestpose))
155
 
        logger.info('Cost value: {0}'.format(maxcost))
156
 
 
157
 
        #logger.info('Execution times (s):  %.2f\t%.2f' % (alignment_time, mapupdate_time))
158
 
        logger.info('Alignment time(s):  %.2f' % alignment_time)
159
 
        logger.info('Map update time(s): %.2f' % mapupdate_time)
 
93
        print 'Map voxel count:', len(vmap)
 
94
 
 
95
        if debug: 
 
96
            print 'Aligned pose:', bestpose
 
97
            print 'Cost value: ', maxcost
 
98
 
 
99
        if debug:
 
100
            #print 'Execution times (s):  %.2f\t%.2f' % (alignment_time, mapupdate_time)
 
101
            print 'Alignment time(s):  %.2f' % alignment_time
 
102
            print 'Map update time(s): %.2f' % mapupdate_time
160
103
    return vmap
161
104
 
162
105
def _build_Kinect_sensor_model(resolution, minR=0.5, maxR=5):
203
146
 
204
147
    # Special functions #############################################
205
148
    # optional also include an estimate of the pose error
206
 
    def __init__(self, resolution, levels=1, rotate=False):
 
149
    def __init__(self, resolution, levels=1):
207
150
        # lattice_type='cubic', #alignment_method='OVL','Pt2Pt_ICP','Pt2Pl_ICP'
208
151
        self.mrol = mrol.MROL(resolution, levels)
209
152
        self.verbosity = 0
217
160
        # gauss_siedel optimisation
218
161
        self._sensor_pts = None # the sensor model as voxel points
219
162
        self._rv = None # the robot visualiser
220
 
        self._rotate = rotate # if visualising, autorotate
221
163
 
222
164
    def __len__(self):
223
165
        ''' return the number of voxels at the finest resolution '''
234
176
    def get_voxels(self, level=-1):
235
177
        '''Returns the (almost) raw voxel information together
236
178
        with the resolution of the voxelisation.'''
 
179
        #import pdb; pdb.set_trace()
237
180
        voxels = self.mrol.getvoxels(level=level)
238
181
        resolution = self.mrol.getresolution(level=level)
239
 
        return voxels, resolution
 
182
        return voxels[0], voxels[1], resolution
240
183
            
241
184
    def get_points(self, level=-1):
242
185
        '''Returns the map as a point cloud of the voxel centers. If the
260
203
        self._rv = None # remove visualiser
261
204
        pickle.dump(self, outfile, protocol=2)
262
205
 
263
 
    def display(self,npts=1e6,changes=False):
 
206
    def display(self,npts=4e5,changes=False):
264
207
        '''Shows a representation of the map in a gui'''
265
 
        from visualiser.robotvisualiser import Visualiser
 
208
        import visualiser.robotvisualiser
266
209
        if self._rv is None:
267
210
            self._rv_changes = changes
268
 
            self._rv = Visualiser(npts=npts, Rotate=self._rotate)
 
211
            self._rv = visualiser.robotvisualiser.Visualiser(npts=npts)
269
212
            self._rv.set_orthographic(Bool=True)
 
213
            #self._rv.setminmax(np.min(self.get_points().points[:,2]),np.max(self.get_points().points[:,2]))
 
214
            self._rv.setautominmax()
 
215
 
 
216
            #if len(inliers) > 0:
 
217
            #    vis.setrightpts(inliers)
 
218
            #    vis.setleftpts(outliers)
 
219
            #vis.setspinpts(FreeMapper.get_points())
270
220
 
271
221
            # TODO visualiser should take a point cloud object
272
222
            self._rv.addmappts(self.get_points().points)
277
227
        #else:
278
228
            #self._disp.se
279
229
 
280
 
    def add_scan(self, pc, pose, timestamp=None):
281
 
        '''Adds a new scan to the existing map.
282
 
 
283
 
        Adds the occupied voxels and removes the freespace.
284
 
        '''
285
 
        userdata = pc.userdata
286
 
        xyzs = pc.points
287
 
        if USE_BACKTRACING:
288
 
            self.clear_freespace(xyzs, pose)
289
 
        self.add_points(xyzs, pose, timestamp, userdata)
290
 
 
291
 
    def clear_freespace(self, xyzs, pose):
292
 
        '''Removes the free space voxels from the occupancy list.
293
 
 
294
 
        Parameters:
295
 
            xyzs: The occupied PointCloud, with the sensor as origin.
296
 
            pose: The pose of `xyzs` PointCloud with respect to global map.
297
 
        '''
298
 
        xyzs, return_pose = poseutil.transformPoints(xyzs, pose)
299
 
        sensor_loc = poseutil.transformPoints(np.array([[0, 0, 0]]), pose)
300
 
        self.mrol.clear_freespace(xyzs, sensor_loc[0])
301
 
 
302
230
    def add_points(self, xyzs, pose, timestamp=None, userdata=None):
303
231
        '''Adds the pointcloud xyzs to the map, transforming them by the given 
304
232
        pose if required. Returns the overlap of the new points against the 
305
233
        existing map.'''
306
 
        if isinstance(xyzs, pointcloud.PointCloud):
307
 
            xyzs = xyzs.points
308
 
        assert xyzs.shape[1] == 3, '3 dimensional points'
 
234
        xyzs = xyzs.points
 
235
        if xyzs.shape[1] > 3: # handling for additional information that is part of the pointcloud object (e.g. colour)
 
236
            if userdata == None:
 
237
                userdata = xyzs[:,3:]
 
238
            else:
 
239
                userdata = np.hstack([xyzs[:,3:],userdata])
 
240
            xyzs = xyzs[:,:3]
309
241
        xyzs, return_pose = poseutil.transformPoints(xyzs, pose)
310
242
        self.trajectory.append(return_pose)
311
243
        if self._rv is not None: # visualisation
312
244
            finest = self.mrol.getfinest()
313
245
            # determine unique new voxels convert to points and add to the map
314
 
            #ol = occupiedlist.OccupiedList(finest.resolution)
315
 
            ol = finest
316
 
            #ol.add_points(xyzs)
317
 
            #ol = ol - finest
318
 
            vox = ol.getvoxeldata()
319
 
            self._rv.addmappts(vox)
 
246
            ol = occupiedlist.OccupiedList(finest.resolution)
 
247
            ol.add_points(xyzs)
 
248
            ol = ol - finest
 
249
            self._rv.addmappts(ol.getpoints())
320
250
            #self._rv.setminmax(np.min(self.get_points().points[:,2]),np.max(self.get_points().points[:,2]))
321
251
            self._rv.setrobotpose(return_pose.getMatrix())
322
252
            self._rv.addtrajectorypoint(return_pose.getTuple()[0:3])
405
335
        if self._sensor_pts is None:
406
336
            sensor_model_ol = _build_Kinect_sensor_model(resolution, minR=minR, maxR=maxR) # for simulated data
407
337
            self._sensor_pts = sensor_model_ol.getpoints()
408
 
            logger.info("Sensor model points:", len(self._sensor_pts))
 
338
            print "Sensor model points:", len(self._sensor_pts)
409
339
            #vis = visualiser.robotvisualiser.Visualiser(npts=1e7)
410
340
            #vis.addmappts(self._sensor_pts)
411
341
        
415
345
        # ray trace from each pose and add voxels to the free space occupied voxel list
416
346
        if pose == None:
417
347
            for pose in self.trajectory:
418
 
                logger.info("Pose {0}".format(pose))
 
348
                print pose
419
349
                inliers, outliers = base_ol.segment_ovl(pose, self._sensor_pts) 
420
350
                # TODO some means of not adding points that are further away than the intersection (doesn't work very well without this!)
421
351
                # Julian suggests a masking operation whilst stepping through the layers of the model in X
427
357
            freespace_ol.add_points(outliers)
428
358
            freespace_ol.removepoints(inliers)
429
359
        return freespace_ol
 
360
            
 
361
    def generate_freespace2(self,pc=None, pose=None, resolution=None, minR=0.5, maxR=5):
 
362
        '''
 
363
        Uses existing ray-trace method to get 
 
364
        around the current limitations of above implementation.
 
365
        
 
366
        Generates an occuplied list consisting of the observed free space at 
 
367
        the specified resolution.  This is useful for detecting object removal 
 
368
        from a map.
 
369
        '''
 
370
        if resolution == None:
 
371
            res = self.get_resolution()
 
372
        else:
 
373
            res = resolution
 
374
        assert res >= self.get_resolution()
 
375
        free_space_ol = occupiedlist.OccupiedList(res, maxvoxels=2e6)
 
376
        if pc==None and pose==None:
 
377
            # generate from map using pose list
 
378
            if self._sensor_pts is None:
 
379
                sensor_model_ol = _build_Kinect_sensor_model(res, minR=minR, maxR=maxR)
 
380
                self._sensor_pts = pointcloud.PointCloud(sensor_model_ol.getpoints())
 
381
                print "Sensor model points:", len(self._sensor_pts)
 
382
            base_ol = self.mrol.getfinest()
 
383
 
 
384
            for pose in self.trajectory:
 
385
                print pose
 
386
                inliers, outliers = base_ol.segment_ovl(pose, self._sensor_pts.points)
 
387
                inlier_pc = pointcloud.PointCloud(inliers)
 
388
                free_pts = self.free_space_ray_trace(inliers,pose.getTuple()[:3],self.get_resolution())
 
389
                free_space_ol.add_points(free_pts.points)
 
390
        else:
 
391
            assert xyzs != None and pose != None, "Both xyzs and pose must be specified"
 
392
            # resampling to align with grid. bad hack, but makes ray_cast easier.
 
393
            voxs = occupiedlist.pointstovoxels(xyzs, res)
 
394
            voxs = quantizer.uniquerows(voxs, 0)
 
395
            X = occupiedlist.voxelstopoints(voxs, res)
 
396
            X_pc = pointcloud.PointCloud(X)
 
397
            free_pts = self.free_space_ray_trace(X_pc,pose.getTuple()[:3],self.get_resolution())
 
398
            free_space_ol.add_points(free_pts.points)
 
399
        return free_space_ol
 
400
 
 
401
    def free_space_ray_trace(self,pc,origin,resolution,voxel_offset=0):
 
402
        # This is essentially independant of the VoxelMap; put elsewhere?
 
403
        free_pts = []
 
404
        res = resolution
 
405
        v_off = np.ceil(voxel_offset*(res/self.get_resolution()))
 
406
        free_pts_OL = occupiedlist.OccupiedList(res,maxvoxels=3e6)
 
407
        count = 0
 
408
        for point in pc.points:
 
409
            ray_vox_hits, range2, dir_unit, all_ray_pts = self.ray_trace_pts(origin,point,return_extras=True)
 
410
            all_ray_pts = all_ray_pts.points[:-(v_off+1)]
 
411
            all_ray_vox = occupiedlist.pointstovoxels(all_ray_pts, res)
 
412
 
 
413
            all_ray_vox_ol = occupiedlist.OccupiedList(1)
 
414
            all_ray_vox_ol.add_points(all_ray_vox)
 
415
 
 
416
            ray_vox_hits_ol = occupiedlist.OccupiedList(1) 
 
417
            ray_vox_hits_ol.add_points(ray_vox_hits.points)
 
418
 
 
419
            free_ray_vox_ol = all_ray_vox_ol - ray_vox_hits_ol
 
420
            free_pts_OL.add_points(free_ray_vox_ol.getpoints()*res)
 
421
            count +=1
 
422
            #if np.remainder(count / len(pc) * 100,1) == 0:
 
423
            #    print count / len(pc) * 100 , '%'
 
424
        return pointcloud.PointCloud(free_pts_OL.getpoints())
 
425
            
 
426
            
 
427
 
 
428
    def ray_trace(self, pose, length, axis=0):
 
429
        '''perform a ray-trace from the pose and return all map voxels at the
 
430
        base resolution along the ray as a pointcloud object.
 
431
        '''
 
432
        return pointcloud.PointCloud(self.mrol.raytrace(pose, length, axis=axis))
 
433
        
 
434
    def ray_trace_pts(self, point1, point2, return_extras=False):
 
435
        '''
 
436
        perform a ray-trace from point1 to point2 and return all map 
 
437
        voxels at the base resolution along the ray as a pointcloud object.
 
438
        '''
 
439
        if not return_extras:
 
440
            ray_vox_hits = self.mrol.raytracepts(point1, point2, return_extras=False)
 
441
            return pointcloud.PointCloud(ray_vox_hits)
 
442
        else:
 
443
            ray_vox_hits, range2, dir_unit, all_ray_pts = self.mrol.raytracepts(point1, point2, return_extras=True)
 
444
            return pointcloud.PointCloud(ray_vox_hits), range2, dir_unit, pointcloud.PointCloud(all_ray_pts)
430
445
        
431
446
    def _trajectory_objective(self, guess_pose, pts, foo):           
432
447
        return -occupiedlist.calccollisions(guess_pose, self._traj_alignment_voxel_set, self.res, pts)