2
Implementation of multi-resolution occupied voxel lists (MROL)
3
Author: Julian Ryde and Nick Hillier
5
from __future__ import division
17
import cython.fast as fast
19
#modalfractions = [0.7, 0.8, 0.9, 0.9, 0.9]
21
modalfractions = np.repeat(0.3, 5)
24
# TODO consider pulling in table class from icra2011 code, table.py
25
def printtable(table_array, width):
26
for row in table_array:
28
print '| ' + field.ljust(width),
31
def _cost_min_scipy(objective_func, initialpose, args):
32
import scipy.optimize as optimize
33
initialpose = initialpose.getTuple()
34
returns = optimize.fmin(objective_func, initialpose, args=args, disp=True, full_output=True)
35
#returns = optimize.fmin_powell(objective_func, initialpose, args=args, disp=True, full_output=True)
36
#returns = optimize.fmin_slsqp(objective_func, initialpose, args=args, full_output=True) # doesn't work?
37
#initpose = np.asarray(initialpose)
38
#delta = np.asarray((0.5, 0.5, 0.5, 0.2, 0.2, 0.2))
39
#lower = initpose - delta
40
#upper = initpose + delta
41
#returns = optimize.anneal(objective_func, initialpose, args=args, lower=lower, upper=upper)
42
bestpose = poseutil.Pose3D(X=returns[0])
48
Stores voxel indices at multiple resolution occupied voxel lists
49
and provides interfaces to add, remove and manipulate the data.
51
def __init__(self, resolution, levels=3):
52
# coarse to fine resolutions
53
self.factor = 2 # multiplier between the resolutions might have to be an integer?
54
resolutions = (pow(self.factor, np.arange(levels))*resolution)[::-1]
55
self.mapvoxels = [] # list of occupied voxel lists
57
for i, res in enumerate(resolutions):
58
ol = occupiedlist.OccupiedList(res)
59
self.mapvoxels.append(ol)
60
#self.maptimes.append([]) # TODO
63
self.feature_range = 8.0
64
# range to a good feature density for angular alignment purposes
72
resolutions = (str(m.resolution).rjust(width) for m in self.mapvoxels)
73
retstr = 'resolutions: ' + delim.join(resolutions) + '\nvoxel counts: '
74
voxel_counts = (str(len(m)).rjust(width) for m in self.mapvoxels)
75
retstr += delim.join(voxel_counts)
79
return len(self.mapvoxels)
81
def __eq__(self, mrol_map):
82
return self.__dict__ == mrol_map.__dict__
84
def getresolution(self, level=None):
85
if level == None: return self.getfinest().resolution
86
return self.mapvoxels[level].resolution
88
def getpoints(self, level=-1):
89
return self.mapvoxels[level].getpoints()
91
def getvoxels(self, level=-1):
92
return self.mapvoxels[level].getvoxels()
96
Returns the occupied list with the finest/best resolution it has.
98
return self.mapvoxels[-1]
100
def addpoints(self, xyzs, timestamp=None, userdata=None, increment=1):
101
# TODO do something with timestamps for re-journalling
102
# TODO allow userdata to be added as an information field to the voxels
104
# TODO work out how to get the userdata out again (perhaps in
105
# getpoints/getvoxels?)
107
for ol in self.mapvoxels:
110
## add to the finest occupiedlist
111
#modified_inds, increments = self.getfinest().addpoints(xyzs, return_modified=True, increment=increment)
112
##modified_inds_finest = modified_inds.copy()
113
## now propagate these modifications to coarser resolutions
114
#for ol in self.mapvoxels[-2::-1]:
115
# modified_inds /= self.factor
116
# # TODO slow but hopefully more likely to be correct way
117
# ol._update_voxels(modified_inds, increments)
118
##return modified_inds_finest
120
def removepoints(self, xyzs, return_removed=False):
121
return self.addpoints(xyzs, increment=-1)
123
# TODO Reconsider keyframe usage after trajectory class is implemented
124
#def setkeyframe(self, xyzs, pose):
125
# xyzs = poseutil.transformPointsTuple(xyzs,pose)
126
# voxel_array = occupiedlist.pointstovoxels(xyzs, self.getresolution(), unique=True)
127
# self.keyframe = set(occupiedlist.hashrows(voxel_array))
129
#def getkeyframeset(self):
130
# return self.keyframe
132
def optimise_alignment(self, xyzs, initialpose, full_data=False):
133
''' The format of this function allows us to use standard optimizers
134
and arbitrary cost functions from the scipy.optimize library.'''
135
dtheta_base = np.radians(0.5)
137
xyzs_sample = util.volumetricsample(xyzs, self.getfinest().resolution)
140
bestpose = initialpose
141
for level, ol in enumerate(self.mapvoxels):
142
objective_func = ol.cost_func
143
#objective_func = self.objectivefuncMROL
144
#ol.use_bloom = False
145
dtheta = dtheta_base*pow(self.factor,(len(self.mapvoxels)-level-1))
147
# This helps avoid local minima, but limits accuracy to voxel size
148
# at the finest resolution
149
bestpose, cost = optimization.cost_min(objective_func, bestpose, (xyzs_sample,), dx, dtheta, max_iterations=20, verbosity=0)
150
#import pydb; pydb.set_trace()
151
#bestpose, cost = _cost_min_scipy(objective_func, bestpose, (xyzs_sample,))
152
# run it again at the finest resolution with tighter steps to get
153
# sub-voxel accuracy.
154
dx = dx*pow(self.factor,-2)
155
dtheta = dtheta_base*pow(self.factor,-2)
157
# Sometimes get better results with the below resampling, but much faster.
158
xyzs = util.offsetvolsample(xyzs, self.getfinest().resolution)
159
bestpose, cost = optimization.cost_min(objective_func, bestpose, (xyzs,), dx, dtheta, max_iterations=100, verbosity=0)
160
return bestpose, cost
162
def objectivefuncMROL(self, pose, xyzs):
164
Objective function to be minimised.
165
For a list of xyzs, transforms by pose and calculations the cost
166
at the various resolutions. If level is specified, only
169
levels = len(self.mapvoxels)
170
hits = np.empty(levels)
171
resolutions = np.empty(levels)
172
for i, ol in enumerate(self.mapvoxels):
173
hits[i] = ol.calccollisions(pose, xyzs, query=False)
174
resolutions[i] = ol.resolution
175
# sort hits from coarse to fine
176
new = np.hstack((-np.diff(hits), hits[-1]))
177
#weights = np.ones(levels)
178
weights = resolutions[-1]/resolutions # same as ..., 1/8, 1/4, 1/2, 1
179
#cost = -np.dot(weights, hits)
180
cost = -np.dot(weights, new)
183
def calcray(self, ray):
184
ol = self.getfinest()
185
hits = ol.segment_ovl(None, ray)[0]
187
vox_hits = occupiedlist.pointstovoxels(hits,ol.resolution,ol.lattice)
188
return quantizer.uniquerows(vox_hits, 0)
192
def raytrace(self, pose, length, axis=0):
194
perform a ray-trace from the pose and return all map voxels at the
195
base resolution along the ray.
196
It traces from any point in the map frame (e.g. the sensor's
197
current position) along a ray oriented as per the pose's angular
200
res = self.getresolution()/2.
201
#import pdb;pdb.set_trace()
202
numcells = np.ceil(length/res)+2 # + 1 for 0th point + 1 for end point with poor aligned voxels
203
ray = np.zeros([numcells,3])
204
ray[:,axis] = np.hstack([0,np.cumsum(np.ones(numcells-1))*res])
205
ray = poseutil.check_and_transform_points(ray, pose)[0]
206
return self.calcray(ray)
208
def raytracepts(self, point1, point2, return_extras=False):
210
perform a ray-trace from point1 to point2 and return all map
211
voxels at the base resolution along the ray.
213
res = self.getresolution()#/3.
214
point2a = point2 - point1
215
range2 = np.sum(point2a**2)
216
length = np.sqrt(range2)
217
dir_unit = point2a/length
218
numcells = int(np.ceil(length/res)+2) # + 1 for 0th point + 1 for last point
219
ray = np.zeros([numcells,3])
220
## TODO make this a list comprehension as this loop slows the cast down lots
221
#for i in range(numcells):
222
# ray[i] = dir_unit*i*res
223
ray_res = length/(numcells-1)
224
ray = fast.fillray(numcells, dir_unit, ray_res, ray)
227
return self.calcray(ray), length, dir_unit, ray
229
return self.calcray(ray)
232
def add_dilate(self, xyzs):
233
self.dilated = occupiedlist.dilate(self.getvoxels())
235
def match(self, xyzs, centrepose, spreadpose):
236
"""Does a global search of the specified pose space.
237
Find the possible locations of the occupied list ol in the current
238
occupied list. Takes an occupied list to match to the current one and
242
# determines the orientation step size as it relates to occupancy list
245
bestpose = poseutil.Pose3D()
247
totaltime = time.time()
248
lasttime = time.time()
249
#for i in range(len(self.voxels)):
250
# volumetric sample and each resolution equivalent to matching two
251
# occupiedlists together but without the double quantisation error
252
# TODO think about this number
253
#xyzs = util.getsample(xyzs, 1000)
255
levels = len(self.mapvoxels)
256
table = np.zeros((levels+2, 9), dtype=np.dtype('S8'))
257
# header for the table of values of interest
258
table[0, :] = "Res modal map scan pose pose Top Time Best".split()
259
table[1, :] = "(m) Frac voxels points count count overlap Taken pose".split()
260
for i, mapol in enumerate(self.mapvoxels):
262
res = mapol.resolution
264
deltaorientation = res/self.feature_range # 8
266
poses = poseutil.uniformposes(centrepose, spreadpose, deltaposition, deltaorientation)
268
poses = poseutil.refineposes(poses, deltaposition, deltaorientation)
270
# TODO remove poses outside search space this needs tidying up/thinking about
271
#cparr = np.array(centrepose.get())
272
#sparr = np.array(spreadpose.get())
274
#D = np.array((deltaposition, deltaposition, deltaposition, deltaorientation, deltaorientation, deltaorientation))
275
#maxpose = cparr + sparr + D
276
#minpose = cparr - sparr - D
277
#select = np.logical_and(np.all(poses <= maxpose, 1), np.all(poses >= minpose, 1))
278
##print 'Culling poses: ', len(poses), sum(select)
279
#poses = poses[select]
281
xyzs_sample = util.volumetricsample(xyzs, res)
282
table[tablerow, :5] = (res, modalfractions[i], len(mapol), len(xyzs_sample), len(poses))
284
# TODO TIME consuming line
285
#overlaps = self.getoverlaps(ol, i, poses)
288
#overlap = np.sum(mapol.calccollisions(pose, xyzs, query=True))
289
overlap = mapol.calccollisions(pose, xyzs, query=False)
290
overlaps.append(overlap)
291
#overlaps.append(-self.objectivefuncMROL(pose, xyzs_sample))
292
overlaps = np.asarray(overlaps)
294
ind_max = np.argmax(overlaps)
295
bestpose.set(poses[ind_max])
296
bestoverlap = overlaps[ind_max]
297
#candidates = overlaps >= modalfractions[i] * bestoverlap
298
# maybe just take the top n?
299
if len(overlaps) > topn:
300
candidates = overlaps.argsort()[-topn:]
302
candidates = range(len(overlaps))
303
timetaken = time.time() - lasttime
304
lasttime = time.time()
305
table[tablerow, 5:] = (len(candidates), bestoverlap, timetaken, bestpose)
307
printtable(table, width=9)
309
#scansize = ol.voxels[-1].shape[0]
310
poses = poses[candidates]
312
print "Total time: ", time.time() - totaltime
313
overlaps = overlaps[candidates]
314
return bestpose, bestoverlap
316
def matchbest(self, xyzs, centrepose, spreadpose):
317
poses, overlaps = self.match(xyzs, centrepose, spreadpose)
318
bestind = np.argmax(overlaps)
319
bestpose = poseutil.Pose3D(poses[bestind, :])
320
cost = overlaps[bestind]
321
return bestpose, cost
323
def matchandadd(self, ol, centrepose, spreadpose):
324
""" matches the given occupiedlist ol by searching around the
325
centrepose a distance of spreadpose. If the match is successful the
326
occupied list is transfomred and combined with the current occupied
327
list. Returns the pose at which the occupied list ol was added."""
328
# TODO add a threshold for deciding whether to merge
329
# TODO add unit test for this
330
# TODO add a ray trace clear?
331
poses, overlaps = self.match(ol, centrepose, spreadpose)
332
bestpose = poseutil.Pose3D(poses[np.argmax(overlaps), :])
333
self.addpoints(bestpose.transformPoints(ol.getpoints()))
added
removed
mrol_mapping/mrol.py
