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