← 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-05-14 16:28:10 UTC
  • Revision ID: wecacuee@gmail.com-20120514162810-gqqevqvisr4qyi5r
Backtracing must be working fine now.

Show diffs side-by-side

added added

removed removed

Lines of Context:
mrol_mapping/mapper.py
26
26
logger.setLevel(logging.INFO)
27
27
USE_BACKTRACING = True
28
28
 
 
29
 
29
30
_map_resolution_levels = 4 # number of different levels of resolution
30
31
 
31
32
def loadmap(fname):
32
33
    return pickle.load(open(fname))
33
34
 
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
35
def buildmap(pointclouds, resolution, visualise=True, odometry_map=False,
90
36
             rotate=False,
91
37
             bestpose_queue=None,
97
43
    optionally communicate back the bestpose got so far. This might be used to
98
44
    optimize the pc.pose i.e. the initial guesspose
99
45
    '''
 
46
 
100
47
    vmap = VoxelMap(resolution, levels=_map_resolution_levels, rotate=rotate)
101
48
    pc = pointclouds.next() 
102
49
    vmap.add_scan(pc, pc.pose)
133
80
        if not odometry_map:
134
81
            bestpose, maxcost = vmap.align_points(sample, guessPose)
135
82
            if bestpose_queue:
136
 
                logger.info("Best pose {0}".format(bestpose))
137
83
                bestpose_queue.put_force(bestpose)
138
84
 
139
85
        alignment_time = time.time() - start
262
208
 
263
209
    def display(self,npts=1e6,changes=False):
264
210
        '''Shows a representation of the map in a gui'''
265
 
        from visualiser.robotvisualiser import Visualiser
 
211
        from visualiser.rviz import Visualiser
266
212
        if self._rv is None:
267
213
            self._rv_changes = changes
268
214
            self._rv = Visualiser(npts=npts, Rotate=self._rotate)
277
223
        #else:
278
224
            #self._disp.se
279
225
 
280
 
    def add_scan(self, pc, pose, timestamp=None):
 
226
    def add_scan(self, xyzs, pose, timestamp=None, userdata=None):
281
227
        '''Adds a new scan to the existing map.
282
228
 
283
229
        Adds the occupied voxels and removes the freespace.
284
230
        '''
285
 
        userdata = pc.userdata
286
 
        xyzs = pc.points
287
231
        if USE_BACKTRACING:
288
232
            self.clear_freespace(xyzs, pose)
289
233
        self.add_points(xyzs, pose, timestamp, userdata)
295
239
            xyzs: The occupied PointCloud, with the sensor as origin.
296
240
            pose: The pose of `xyzs` PointCloud with respect to global map.
297
241
        '''
 
242
        xyzs = xyzs.points
298
243
        xyzs, return_pose = poseutil.transformPoints(xyzs, pose)
299
244
        sensor_loc = poseutil.transformPoints(np.array([[0, 0, 0]]), pose)
300
245
        self.mrol.clear_freespace(xyzs, sensor_loc[0])
303
248
        '''Adds the pointcloud xyzs to the map, transforming them by the given 
304
249
        pose if required. Returns the overlap of the new points against the 
305
250
        existing map.'''
306
 
        if isinstance(xyzs, pointcloud.PointCloud):
307
 
            xyzs = xyzs.points
308
 
        assert xyzs.shape[1] == 3, '3 dimensional points'
 
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]
309
258
        xyzs, return_pose = poseutil.transformPoints(xyzs, pose)
310
259
        self.trajectory.append(return_pose)
311
260
        if self._rv is not None: # visualisation
315
264
            ol = finest
316
265
            #ol.add_points(xyzs)
317
266
            #ol = ol - finest
318
 
            vox = ol.getvoxeldata()
319
 
            self._rv.addmappts(vox)
 
267
            self._rv.addmappts(ol.getpoints())
320
268
            #self._rv.setminmax(np.min(self.get_points().points[:,2]),np.max(self.get_points().points[:,2]))
321
269
            self._rv.setrobotpose(return_pose.getMatrix())
322
270
            self._rv.addtrajectorypoint(return_pose.getTuple()[0:3])