4
from __future__ import division
9
import mrol_mapping.mapper as mapper
10
import mrol_mapping.poseutil as poseutil
11
import mrol_mapping.mrol as mrol
12
import mrol_mapping.occupiedlist as occupiedlist
13
import mrol_mapping.pointcloud as pointcloud
20
basedir = 'data/simulated/'
22
def load_poses(fname):
26
for line in open(fname):
27
if line.strip().startswith('#'):
29
fields = line.split(None, 1)
31
mat = np.asarray(eval(fields[1]))
34
# need to transpose from blender?
39
true_poses = load_poses(basedir + 'poses.txt')
42
'''Loads the i-th point cloud from test data and attach the true pose to
44
fname = str(i).rjust(4, '0') + '.png'
45
di = depth_image.load_image(basedir + fname)
46
pc = depth_image.image_to_points(di, max_range=di.max())
47
#pc = pointcloud.PointCloud(xyzs)
48
# remove ceiling and floor
49
# this could be done automatically with RANSAC
50
# large horizontal plane cause problems when contraining pose in
52
# in noisy outdoor environments this is less of a problem?
53
pc.boxfilter(zmin=-1.5, zmax=0.5)
55
# Attach ground truth pose to the pointcloud
57
pc.pose = true_poses[fname]
58
except KeyError as ke:
62
# TODO add benchmark test
64
class Testposeutil(unittest.TestCase):
70
self.vmap = mapper.VoxelMap(self.res, levels=3)
72
self.vmap.add_points(pc, pc.pose)
74
def check_localise(self, bestpose, truepose):
75
#print 'Best:', bestpose
76
#print 'True:', truepose
77
poseerror = truepose.getCombined(bestpose.inverse())
78
print 'Pose error:', poseerror
79
pos_error = np.sum(np.abs(poseerror.get()[:3]))
80
ang_error = np.sum(np.abs(poseerror.get()[3:]))
81
print '\nPosition error (m):', pos_error
82
print 'Angular error (degs):', np.degrees(ang_error)
84
position_tol = 0.04 # total position error tolerance
85
angular_tol = np.radians(0.55)
86
#self.assertTrue(pos_error < position_tol)
87
#self.assertTrue(ang_error < angular_tol)
88
#self.assertTrue(bestpose.is_close(truepose, position_tol, angular_tol))
90
def test_Ascanmatch_6DOF(self):
91
# "A" prepended to run this first cos it is quick
92
print "test_scanmatch_6DOF"
94
guessPose = get_PC(0)[0].pose
98
bestpose, maxcost = vmap.align_points(pc1, guessPose)
99
taken = time.time() - start
100
print 'Scanmatch time:', taken, 'seconds'
101
self.check_localise(bestpose, truepose)
103
def test_Ymapbuild_6DOF(self):
104
# "Z" prepended to run this last cos it is slow
105
print "test_mapbuild_6DOF"
107
guessPose = vmap.trajectory[0]
108
#guessPose = get_PC(0)[0].pose
112
for i in range(1, 16):
113
print 'Aligning scan', i
115
bestpose, maxcost = vmap.align_points(pc, guessPose)
116
#bestpose = pc.pose # use the true value
117
print 'Bestpose:',bestpose
118
vmap.add_points(pc, bestpose)
119
self.check_localise(bestpose, pc.pose)
121
taken = time.time() - start
122
print 'Mapbuild time:', taken, 'seconds'
123
print 'Map size (voxels):', len(vmap)
124
print 'Perfect map has 19812 voxels, press "c" to continue.'
125
# TODO possibly interesting to calculate the ideal compression with the
126
# true poses, might be possible to get better than that?
128
def OFFtest_Zmapbuild_6DOF_feature(self):
129
# "Z" prepended to run this last cos it is slow
130
print "test_mapbuild_6DOF_feature"
132
vmap = mapper.VoxelMap(self.res, levels=5)
134
di_disconts = depth_image.depth_discont_sobel(di)
135
pc_disconts = depth_image.image_to_points(di_disconts)
136
vmap.add_points(pc_disconts, pc.pose)
137
guessPose = vmap.trajectory[0]
138
#guessPose = get_PC(0)[0].pose
140
import mrol_mapping.visualiser.robotvisualiser as robotvisualiser
144
pylab.imshow(di_disconts)
146
PC_vis = robotvisualiser.Visualiser(title='points')
147
PC_vis.setautominmax()
148
PC_vis.addmappts(pc_disconts.points)
150
for i in range(1, 16):
151
print 'Aligning scan', i
153
print "extracting feature set"
154
di_disconts = depth_image.depth_discont_sobel(di)
155
pc_disconts = depth_image.image_to_points(di_disconts)
157
pylab.imshow(di_disconts)
160
PC_vis.addmappts(pc_disconts.points)
161
print "performing alignment"
162
bestpose, maxcost = vmap.align_points(pc_disconts, guessPose, full_data=True)
163
#bestpose = pc.pose # use the true value
164
print 'Bestpose:',bestpose
165
#vmap.add_points(pc, bestpose)
166
vmap.add_points(pc_disconts, bestpose)
167
self.check_localise(bestpose, pc.pose)
169
taken = time.time() - start
170
print 'Mapbuild time:', taken, 'seconds'
171
print 'Map size (voxels):', len(vmap)
172
print 'Perfect map has 19812 voxels, press "c" to continue.'
173
# TODO possibly interesting to calculate the ideal compression with the
174
# true poses, might be possible to get better than that?
177
def OFFtest_saveload(self): # turned off because I run out of RAM and have to start paging.
178
print "test_saveload"
179
# TODO use tempfile module?
181
fname = '/tmp/vmap.map'
183
loaded = mapper.loadmap(fname)
184
# TODO assert equals needs __eq__ method on VoxelMap
186
def OFFtest_mapper_global_localisation(self):
189
spread_pose = poseutil.Pose3D((dx, dx, 0, 0, 0, dq))
191
map_mrol = mrol.MROL(res, levels=4)
192
#map_mrol.set_feature_range(8)
193
self.P1.volumetricsample(self.res+0.01111)
194
self.P2.volumetricsample(0.08)
195
map_mrol.addpoints(self.P1)
199
bestpose, maxo = map_mrol.match(pc1, guesspose, spread_pose)
200
truepose = self.P2.pose
203
self.P1.display(color=(1,0,0))
204
self.P2.pose = bestpose
205
self.P2.display(color=(0,1,0))
207
print 'Best:', bestpose
208
print 'True:', truepose
209
poseerror = truepose.getCombined(bestpose.inverse())
210
pos_error = np.sum(np.abs(poseerror.get()[:3]))
211
ang_error = np.sum(np.abs(poseerror.get()[3:]))
213
print 'Total Position error (m): ', pos_error
214
print 'Total Angular error (degs):', np.degrees(ang_error)
216
angular_tol = np.radians(2)
218
self.assertTrue(bestpose.is_close(truepose, position_tol, angular_tol))
220
def test_Amapper_raytrace(self): # "A" prepended to run this first cos it is quick
221
print "test_Amapper_raytrace"
223
vm = mapper.VoxelMap(res, levels=3)
224
xyzs = np.zeros([4, 3])
225
xyzs[:, 1] = np.cumsum(np.ones(4))*res
226
xyzs[3, :] = [0.5, 0.5, 0.5] # this one isn't ray traced out
227
pc = pointcloud.PointCloud(xyzs)
228
vm.add_points(pc, None)
229
pose = (0, 0, 0, 0, 0, np.pi/2.)
230
ray_occs_pc = vm.ray_trace(pose, 50*res)
231
self.assertEqual(ray_occs_pc, pointcloud.PointCloud(occupiedlist.pointstovoxels(xyzs[:3], res, vm.lattice)))
232
ray_occs_pc = vm.ray_trace_pts(xyzs[0,:],xyzs[2,:])
233
self.assertEqual(ray_occs_pc, pointcloud.PointCloud(occupiedlist.pointstovoxels(xyzs[:3], res, vm.lattice)))
234
ray_occs_pc = vm.ray_trace_pts(np.array([0,0,0]),xyzs[3,:])
235
self.assertEqual(ray_occs_pc, pointcloud.PointCloud(occupiedlist.pointstovoxels(xyzs[3,:], res, vm.lattice)))
236
ray_occs_pc = vm.ray_trace_pts(xyzs[2,:],xyzs[0,:])
237
self.assertEqual(ray_occs_pc, pointcloud.PointCloud(occupiedlist.pointstovoxels(xyzs[:3], res, vm.lattice)))
238
# TODO ray check perpendicular to a plane
240
# test the free-space tracing
241
pc_555 = pointcloud.PointCloud(xyzs[3,:])
242
free_555_pc = vm.free_space_ray_trace(pc_555,np.asarray([0,0,0]),resolution=res)
243
xyzs_free_555 = np.array( [[ 0. , 0. , 0. ],
254
[ 0.44, 0.44, 0.44]] )
255
expected_555_pc = pointcloud.PointCloud(xyzs_free_555)
256
self.assertEqual(expected_555_pc, free_555_pc)
258
if __name__ == '__main__':
added
removed
test/mapper_test.py
