def testpointstoandfromvoxels(self):
     res = 0.5 # is only exact for 0.5
     # test for all lattice quantisation functions
     for funcstr in occupiedlist.latticefuncs:
         # test reciprocal nature of conversion
         A = occupiedlist.pointstovoxels(self.X, res, latticetype=funcstr)
         Y = occupiedlist.voxelstopoints(A, res)
         self.assertTrue(np.allclose(self.X, Y)) 
         X = np.array([1, 2, 3]).reshape(1, 3)
         # TODO currently does not handle single points
         A = occupiedlist.pointstovoxels(X, res, latticetype=funcstr) 
         self.assertTrue(np.allclose(X/res, A)) 
Exemple #2
0
 def setUp(self):
     self.datadir = 'data/stillscans/'
     self.Pall, self.Q, self.guesspose, self.res = generate_tst_data_from_file(self.datadir + 'empty-map.txt', self.datadir + '1271124950-scan.txt', self.datadir + '1271124950-pose.txt')
     # override guess pose to make it harder for the algorithms
     self.guesspose = np.array([-2, 42, 80, 0.2, 0.1, 2.58])
     # sampling
     self.P = util.volumetricsample(self.Pall, self.res) # shuffles P as well
     # setup the mrol for the alignment
     self.mapmrol = mrol.MROL(self.res, levels=5)
     self.mapmrol.addpoints(self.Q)
     # setup the occupiedlist for the segmentation
     mv = occupiedlist.pointstovoxels(self.Q, self.res)
     self.mapvoxels = {} 
     occupiedlist.increment(self.mapvoxels, mv)
     # setup a dilated occupiedlist for segmentation
     self.dilated = {} 
     mv = occupiedlist.dilate(self.mapmrol.getvoxels().keys())
     occupiedlist.increment(self.dilated, mv)
     # setup a mapset for fast alignment
     self.mapset = set(occupiedlist.hashrows(self.mapvoxels.keys()))
     # Ground truths and tolerances
     self.lin_tol = 1e-1
     self.ang_tol = np.deg2rad(2.0)
     self.truepose = np.array([-1.8572, 41.7913, 79.3341, np.deg2rad(12.4607), np.deg2rad(4.3733), np.deg2rad(147.2011)]) # Overlap: 6094, Pose (m, degrees): -1.8572 41.7913 79.3341 12.4607 4.3733 147.2011 
     self.truePoints = poseutil.transformPoints(self.Pall, poseutil.mat(self.truepose))
     # list the aligners to be tested
     self.aligners = [ 
                       self.align_mrol_optimize_00001,
                       self.align_mrol_optimize_11111,
                     ]     
     # list the segmenters to be tested
     self.segmenters = [ self.segment_ovl,
                         self.segment_ovl_dilated
                       ]
    def test_Amapper_raytrace(
            self):  # "A" prepended to run this first cos it is quick
        print "test_Amapper_raytrace"
        res = 0.04
        vm = mapper.VoxelMap(res, levels=3)
        xyzs = np.zeros([4, 3])
        xyzs[:, 1] = np.cumsum(np.ones(4)) * res
        xyzs[3, :] = [0.5, 0.5, 0.5]  # this one isn't ray traced out
        pc = pointcloud.PointCloud(xyzs)
        vm.add_points(pc, None)
        pose = (0, 0, 0, 0, 0, np.pi / 2.)
        ray_occs_pc = vm.ray_trace(pose, 50 * res)
        self.assertEqual(
            ray_occs_pc,
            pointcloud.PointCloud(
                occupiedlist.pointstovoxels(xyzs[:3], res, vm.lattice)))
        ray_occs_pc = vm.ray_trace_pts(xyzs[0, :], xyzs[2, :])
        self.assertEqual(
            ray_occs_pc,
            pointcloud.PointCloud(
                occupiedlist.pointstovoxels(xyzs[:3], res, vm.lattice)))
        ray_occs_pc = vm.ray_trace_pts(np.array([0, 0, 0]), xyzs[3, :])
        self.assertEqual(
            ray_occs_pc,
            pointcloud.PointCloud(
                occupiedlist.pointstovoxels(xyzs[3, :], res, vm.lattice)))
        ray_occs_pc = vm.ray_trace_pts(xyzs[2, :], xyzs[0, :])
        self.assertEqual(
            ray_occs_pc,
            pointcloud.PointCloud(
                occupiedlist.pointstovoxels(xyzs[:3], res, vm.lattice)))
        # TODO ray check perpendicular to a plane

        # test the free-space tracing
        pc_555 = pointcloud.PointCloud(xyzs[3, :])
        free_555_pc = vm.free_space_ray_trace(pc_555,
                                              np.asarray([0, 0, 0]),
                                              resolution=res)
        xyzs_free_555 = np.array([[0., 0., 0.], [0.04, 0.04, 0.04],
                                  [0.08, 0.08, 0.08], [0.12, 0.12, 0.12],
                                  [0.16, 0.16, 0.16], [0.2, 0.2, 0.2],
                                  [0.24, 0.24, 0.24], [0.28, 0.28, 0.28],
                                  [0.32, 0.32, 0.32], [0.36, 0.36, 0.36],
                                  [0.4, 0.4, 0.4], [0.44, 0.44, 0.44]])
        expected_555_pc = pointcloud.PointCloud(xyzs_free_555)
        self.assertEqual(expected_555_pc, free_555_pc)
def get_freespace(xyzs, pose, voxelmap):
    # TODO consider using mapper._build_sensor_model() and mapper.generate_freespace() instead.
    # resampling to align with grid. bad hack, but makes ray_cast easier.
    res = voxelmap.get_resolution()
    xyzs = poseutil.transformpoints(xyzs,pose)
    voxs = occupiedlist.pointstovoxels(xyzs, res)
    voxs = quantizer.uniquerows(voxs, 0)
    X = occupiedlist.voxelstopoints(voxs, res)
    free_pts = voxelmap.free_space_ray_trace(X,pose[:3])
    return free_pts
Exemple #5
0
def get_freespace(xyzs, pose, voxelmap):
    # TODO consider using mapper._build_sensor_model() and mapper.generate_freespace() instead.
    # resampling to align with grid. bad hack, but makes ray_cast easier.
    res = voxelmap.get_resolution()
    xyzs = poseutil.transformpoints(xyzs,pose)
    voxs = occupiedlist.pointstovoxels(xyzs, res)
    voxs = quantizer.uniquerows(voxs, 0)
    X = occupiedlist.voxelstopoints(voxs, res)
    free_pts = voxelmap.free_space_ray_trace(X,pose[:3])
    return free_pts
 def test_Amapper_raytrace(self): # "A" prepended to run this first cos it is quick
     print "test_Amapper_raytrace"
     res = 0.04
     vm = mapper.VoxelMap(res, levels=3)
     xyzs = np.zeros([4, 3])
     xyzs[:, 1] = np.cumsum(np.ones(4))*res
     xyzs[3, :] = [0.5, 0.5, 0.5] # this one isn't ray traced out
     pc = pointcloud.PointCloud(xyzs)
     vm.add_points(pc, None)
     pose = (0, 0, 0, 0, 0, np.pi/2.)
     ray_occs_pc = vm.ray_trace(pose, 50*res)
     self.assertEqual(ray_occs_pc, pointcloud.PointCloud(occupiedlist.pointstovoxels(xyzs[:3], res, vm.lattice)))
     ray_occs_pc = vm.ray_trace_pts(xyzs[0,:],xyzs[2,:])
     self.assertEqual(ray_occs_pc, pointcloud.PointCloud(occupiedlist.pointstovoxels(xyzs[:3], res, vm.lattice)))
     ray_occs_pc = vm.ray_trace_pts(np.array([0,0,0]),xyzs[3,:])
     self.assertEqual(ray_occs_pc, pointcloud.PointCloud(occupiedlist.pointstovoxels(xyzs[3,:], res, vm.lattice)))
     ray_occs_pc = vm.ray_trace_pts(xyzs[2,:],xyzs[0,:])
     self.assertEqual(ray_occs_pc, pointcloud.PointCloud(occupiedlist.pointstovoxels(xyzs[:3], res, vm.lattice)))
     # TODO ray check perpendicular to a plane
     
     # test the free-space tracing
     pc_555 = pointcloud.PointCloud(xyzs[3,:])
     free_555_pc = vm.free_space_ray_trace(pc_555,np.asarray([0,0,0]),resolution=res)
     xyzs_free_555 = np.array( [[ 0.  ,  0.  ,  0.  ],
                                [ 0.04,  0.04,  0.04],
                                [ 0.08,  0.08,  0.08],
                                [ 0.12,  0.12,  0.12],
                                [ 0.16,  0.16,  0.16],
                                [ 0.2 ,  0.2 ,  0.2 ],
                                [ 0.24,  0.24,  0.24],
                                [ 0.28,  0.28,  0.28],
                                [ 0.32,  0.32,  0.32],
                                [ 0.36,  0.36,  0.36],
                                [ 0.4 ,  0.4 ,  0.4 ],
                                [ 0.44,  0.44,  0.44]] )
     expected_555_pc = pointcloud.PointCloud(xyzs_free_555)
     self.assertEqual(expected_555_pc, free_555_pc)
pc_xform = poseutil.Pose3D(X=(0, 0, 0, -np.pi / 2.0, 0, -np.pi / 2.0))
res = 0.01
segment_map = mapper.VoxelMap(res, levels=3)
pc = pointcloud.load(fnames.pop(0))
# pc = pointcloud.PointCloud(pc)
pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
segment_map.add_points(pc, bestpose)
object_map = mapper.VoxelMap(res / 4.0, levels=1)


# freespace_ol = segment_map.generate_freespace2(resolution = res*2, minR=0.4, maxR=3)
# freespace_ol = segment_map.generate_freespace(res, minR=0.25, maxR=0.75) # *4 for speed during testing, should just be res

free_space_res = res

pc_vox = occupiedlist.pointstovoxels(pc.points, free_space_res)
pc_vox = quantizer.uniquerows(pc_vox, 0)
pc_pts = occupiedlist.voxelstopoints(pc_vox, free_space_res)
pc_regular = pointcloud.PointCloud(pc_pts)
freespace = segment_map.free_space_ray_trace(pc_regular, (0, 0, 0), free_space_res, voxel_offset=2.5)
freespace_ol = occupiedlist.OccupiedList(free_space_res)
freespace_ol.addpoints(freespace.points)

# import pdb;pdb.set_trace()
# vis = robotvisualiser.Visualiser(npts=2e6)
# vis.addmappts(segment_map.get_points().points)
# vis.addmappts(freespace_ol.getpoints())
# free_pc = pointcloud.PointCloud(freespace_ol.getpoints())
# free_pc.display()

Exemple #8
0
bestpose = poseutil.Pose3D(X=(0, 0, 0, 0, 0, 0))
pc_xform = poseutil.Pose3D(X=(0, 0, 0, -np.pi / 2., 0, -np.pi / 2.))
res = 0.01
segment_map = mapper.VoxelMap(res, levels=3)
pc = pointcloud.load(fnames.pop(0))
#pc = pointcloud.PointCloud(pc)
pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
segment_map.add_points(pc, bestpose)
object_map = mapper.VoxelMap(res / 4., levels=1)

#freespace_ol = segment_map.generate_freespace2(resolution = res*2, minR=0.4, maxR=3)
#freespace_ol = segment_map.generate_freespace(res, minR=0.25, maxR=0.75) # *4 for speed during testing, should just be res

free_space_res = res

pc_vox = occupiedlist.pointstovoxels(pc.points, free_space_res)
pc_vox = quantizer.uniquerows(pc_vox, 0)
pc_pts = occupiedlist.voxelstopoints(pc_vox, free_space_res)
pc_regular = pointcloud.PointCloud(pc_pts)
freespace = segment_map.free_space_ray_trace(pc_regular, (0, 0, 0),
                                             free_space_res,
                                             voxel_offset=2.5)
freespace_ol = occupiedlist.OccupiedList(free_space_res)
freespace_ol.addpoints(freespace.points)

#import pdb;pdb.set_trace()
#vis = robotvisualiser.Visualiser(npts=2e6)
#vis.addmappts(segment_map.get_points().points)
#vis.addmappts(freespace_ol.getpoints())
#free_pc = pointcloud.PointCloud(freespace_ol.getpoints())
#free_pc.display()