def OFFtest_Zmapbuild_6DOF_feature(self): # "Z" prepended to run this last cos it is slow print "test_mapbuild_6DOF_feature" #vmap = self.vmap vmap = mapper.VoxelMap(self.res, levels=5) pc, di = get_PC(0) di_disconts = depth_image.depth_discont_sobel(di) pc_disconts = depth_image.image_to_points(di_disconts) vmap.add_points(pc_disconts, pc.pose) guessPose = vmap.trajectory[0] #guessPose = get_PC(0)[0].pose if visualise: import mrol_mapping.visualiser.robotvisualiser as robotvisualiser vmap.display() import pylab pylab.ion() pylab.imshow(di_disconts) pylab.draw() PC_vis = robotvisualiser.Visualiser(title='points') PC_vis.setautominmax() PC_vis.addmappts(pc_disconts.points) #import pdb;pdb.set_trace() start = time.time() for i in range(1, 16): print 'Aligning scan', i pc, di = get_PC(i) print "extracting feature set" di_disconts = depth_image.depth_discont_sobel(di) pc_disconts = depth_image.image_to_points(di_disconts) if visualise: pylab.imshow(di_disconts) pylab.draw() PC_vis.clear() PC_vis.addmappts(pc_disconts.points) #import pdb;pdb.set_trace() print "performing alignment" bestpose, maxcost = vmap.align_points(pc_disconts, guessPose, full_data=True) #bestpose = pc.pose # use the true value print 'Bestpose:', bestpose #vmap.add_points(pc, bestpose) vmap.add_points(pc_disconts, bestpose) self.check_localise(bestpose, pc.pose) guessPose = bestpose #import pdb;pdb.set_trace() taken = time.time() - start print 'Mapbuild time:', taken, 'seconds' print 'Map size (voxels):', len(vmap) import pdb pdb.set_trace() print 'Perfect map has 19812 voxels, press "c" to continue.'
def OFFtest_Zmapbuild_6DOF_feature(self): # "Z" prepended to run this last cos it is slow print "test_mapbuild_6DOF_feature" #vmap = self.vmap vmap = mapper.VoxelMap(self.res, levels=5) pc, di = get_PC(0) di_disconts = depth_image.depth_discont_sobel(di) pc_disconts = depth_image.image_to_points(di_disconts) vmap.add_points(pc_disconts, pc.pose) guessPose = vmap.trajectory[0] #guessPose = get_PC(0)[0].pose if visualise: import mrol_mapping.visualiser.robotvisualiser as robotvisualiser vmap.display() import pylab pylab.ion() pylab.imshow(di_disconts) pylab.draw() PC_vis = robotvisualiser.Visualiser(title='points') PC_vis.setautominmax() PC_vis.addmappts(pc_disconts.points) #import pdb;pdb.set_trace() start = time.time() for i in range(1, 16): print 'Aligning scan', i pc, di = get_PC(i) print "extracting feature set" di_disconts = depth_image.depth_discont_sobel(di) pc_disconts = depth_image.image_to_points(di_disconts) if visualise: pylab.imshow(di_disconts) pylab.draw() PC_vis.clear() PC_vis.addmappts(pc_disconts.points) #import pdb;pdb.set_trace() print "performing alignment" bestpose, maxcost = vmap.align_points(pc_disconts, guessPose, full_data=True) #bestpose = pc.pose # use the true value print 'Bestpose:',bestpose #vmap.add_points(pc, bestpose) vmap.add_points(pc_disconts, bestpose) self.check_localise(bestpose, pc.pose) guessPose = bestpose #import pdb;pdb.set_trace() taken = time.time() - start print 'Mapbuild time:', taken, 'seconds' print 'Map size (voxels):', len(vmap) import pdb; pdb.set_trace() print 'Perfect map has 19812 voxels, press "c" to continue.'
def get_PC(i): '''Loads the i-th point cloud from test data and attach the true pose to it''' fname = str(i).rjust(4, '0') + '.png' di = depth_image.load_image(basedir + fname) pc = depth_image.image_to_points(di, max_range=di.max()) #pc = pointcloud.PointCloud(xyzs) # remove ceiling and floor # this could be done automatically with RANSAC # large horizontal plane cause problems when contraining pose in # horizontal plane? # in noisy outdoor environments this is less of a problem? pc.boxfilter(zmin=-1.5, zmax=0.5) # Attach ground truth pose to the pointcloud try: pc.pose = true_poses[fname] except KeyError as ke: print ke return pc, di
def get_pointcloud(fname): xyzs = depth_image.image_to_points(fname) pc = pointcloud.PointCloud(xyzs) # remove ceiling and floor pc.boxfilter(zmin=-15, zmax=0.5) return pc