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