def test_proximity(self):
# point on voxel faces should be shared equally between two adjacent
# voxels
res = 0.1
for i in range(3):
ol = occupiedlist.OccupiedList(res)
expected = np.zeros((2,3), dtype=int)
expected[1, i] = 1
X = [0, 0, 0]
X[i] = res/2
ol.addpoints(X)
voxels = ol.getvoxels()
self.assertTrue(pointcloud.equal(expected, voxels))
occs = ol.getoccupancies()
self.assertTrue(np.allclose(occs, occs[0], atol=0.001))
# point on corner should be shared equally between 8 adjacent voxels
ol = occupiedlist.OccupiedList(res)
ol.addpoints((res/2, res/2, res/2))
occs = ol.getoccupancies()
self.assertTrue(np.allclose(occs, occs[0], atol=0.01))
# point in middle of voxels with small sigma should only update one voxel
ol = occupiedlist.OccupiedList(res)
ol.addpoints((0,0,0), sigma=0.1)
occs = ol.getoccupancies()
self.assertTrue(occs[0] > 0.99)
voxels = ol.getvoxels()
self.assertTrue(np.allclose(voxels, (0,0,0)))
def testequals(self):
X = np.random.random((10, 3))*10
A = occupiedlist.OccupiedList(0.05)
A.addpoints(X)
B = occupiedlist.OccupiedList(0.05)
B.addpoints(X)
self.assertEquals(A, B)
Z = np.asarray(((0,0,0),))
B.addpoints(Z)
self.assertNotEquals(A, B)
def test_fastenough(self):
limit = 3
res = 0.1
cpu_speed = benchmark()
# test update rate, number of points that can be added per second
olmap = occupiedlist.OccupiedList(res)
map_pts = np.random.random((3000, 3)) * limit
start = time.time()
olmap.addpoints(map_pts)
taken = time.time() - start
update_rate = len(map_pts)/taken
print 'Map update speed:', update_rate, 'points/s'
self.assertTrue(update_rate > 2000, 'OccupiedList update rate too slow')
# TODO normalise this assertion for current cpu speed?
# test query rate
xyzs = map_pts[:100,:]
pose = np.array([ -2., 42., 80., 0.2, 0.1, 2.6])
for P in [None, pose]:
print 'Transformations and lookups per second, with pose,', P
start = time.time()
collision_list = olmap.calccollisions(P, xyzs, returntotal=False)
taken = time.time() - start
print 1e-6 * len(xyzs)/taken, 'million/s'
normalised = taken * cpu_speed
print 'Normalised: ', normalised
#self.assertEqual(np.sum(collision_list), overlap)
self.assertTrue(normalised < 250, 'Too slow')
def testcalccollisions(self):
limit = 3
res = 0.1
olmap = occupiedlist.OccupiedList(res)
map_pts = np.random.random((3000, 3)) * limit
olmap.addpoints(map_pts)
overlap = olmap.calccollisions(None, map_pts)
# To check a possible bug where calccollisions returns the number
# of voxels instead of the number of points
self.assertEqual(map_pts.shape[0], overlap)
self.assertNotEqual(len(olmap), overlap)
xyzs = map_pts[:100,:]
overlap = olmap.calccollisions(None, xyzs)
self.assertEqual(100, overlap)
olmap.removepoints(xyzs)
overlap = olmap.calccollisions(None, map_pts[100:,:])
self.assertEqual(map_pts.shape[0]-100, overlap)
# test with query option
#occupancies = olmap.calccollisions(None, map_pts[100:,:], query=True)
np.random.seed(6)
npts = 4000
xyzs = 2*limit*np.random.random((npts, 3)) - limit
pose = None
def test_volumetricsample(self):
sample = util.volumetricsample(self.pc.points, self.res)
self.assertEqual(len(sample), 2)
#xyzs = np.arange(1500).reshape(500, 3)
#xyzs = np.random.randint(50,size=(1500,3))
#xyzs = xyzs*0.01
xyzs = np.random.randn(1500, 3)
import mrol_mapping.occupiedlist as occupiedlist
ol = occupiedlist.OccupiedList(0.1)
ol.addpoints(util.volumetricsample(xyzs, 0.1))
inpts, outpts = ol.segment_ovl(None, xyzs)
self.assertTrue(len(outpts) == 0)
def setUp(self):
#np.random.seed(6)
self.mpts = 100000 #Adding a zero here changes the cython results dramatically
self.npts = 10000
self.high = 50
self.M = np.random.randint(0, self.high,
(self.mpts, 3)).astype(np.int16)
self.P = np.random.randint(0, self.high,
(self.npts, 3)).astype(np.int16)
self.testpose = (-2., 42., 80., 0.2, 0.1, 2.6)
self.testposeinv = poseutil.inverse(self.testpose)
self.Pxformed = poseutil.check_and_transform_points(
self.P, self.testpose)[0]
self.ol = occupiedlist.OccupiedList(1)
self.ol.addpoints(self.M)
self.Mkdt = spatial.cKDTree(self.M)
self.maparray = np.zeros((self.high, self.high, self.high),
dtype=np.bool)
#self.maparray = ndsparse.ndsparse((self.high, self.high, self.high))
self.maparray[self.M[:, 0], self.M[:, 1], self.M[:, 2]] = True
# set up packed arrays map
ids = occupiedlist._pack(self.M)
self.mapvoxels_int16 = dict.fromkeys(ids, 1)
self.mapset = set(ids)
#self.mapvoxels_int16 = collections.defaultdict(int)
#for ID in ids:
# self.mapvoxels_int16[ID.tostring()] += 1
# bloom map
self.bloom = occupiedlist.BloomFilter(self.mpts)
self.bloom.add_voxel_ids(occupiedlist._pack(self.M))
# dictionary of dictionaries
#D = dict.fromkeys(self.mpts[:,0], dict())
self.Pint = self.P.astype(int)
D = dict()
# first initialise
for a, b, c in self.M:
D.setdefault(a, dict())
D[a].setdefault(b, dict())
D[a][b][c] = 0
for a, b, c in self.M:
D[a][b][c] += 1
self.nestedDict = D
def setUp(self):
self.X = np.arange(30).reshape(10, 3)
self.ol = occupiedlist.OccupiedList(0.2)
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()
if visualise:
vis = robotvisualiser.Visualiser(npts=2e6)
vis.set_orthographic(Bool=True)
vis.setminmax(-1.5, 1.5)
#objvis = robotvisualiser.Visualiser()
#objvis.set_orthographic(Bool=True)
bestpose = poseutil.Pose3D(X=(0.5,-0.5,0,0,0,np.pi/4))
for fname in fnames:
pc = iros_mapper.get_pointcloud(fname)
print fname,
bestpose = segment_map.align_points(pc, bestpose)[0]
print bestpose
hits, new_points = segment_map.mrol.getfinest().segment_ovl(bestpose,pc.points)
inliers, outliers = freespace_ol.segment_ovl(None, new_points)
segment_map.add_points(pointcloud.PointCloud(outliers), None)
if freespace2:
if len(inliers) > 0:
tmp_freespace_ol = segment_map.generate_freespace2(inliers,pose=bestpose)
else:
# nothing to segment?
import pdb;pdb.set_trace()
tmp_freespace_ol = occupiedlist.OccupiedList(1)
else:
tmp_freespace_ol = segment_map.generate_freespace(res,pose=bestpose)
freespace_ol.addpoints(tmp_freespace_ol.getpoints(), None)
if visualise:
print "Visualising"
vis.clear()
vis.addmappts(segment_map.get_points().points)
vis.setrobotpose(bestpose.getMatrix())
vis.addtrajectorypoint(bestpose.getTuple()[0:3])
if len(inliers) > 0:
vis.setrightpts(outliers) # new map points
vis.setleftpts(inliers) # segmented points
# pointcloud.save(fname+"_seg",inliers)
# np.savetxt(fname+"_pose",bestpose)