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))
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
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()
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()