def optimise_alignment(self, xyzs, initialpose, full_data=False):
''' The format of this function allows us to use standard optimizers
and arbitrary cost functions from the scipy.optimize library.'''
dtheta_base = np.radians(0.5)
if not full_data:
xyzs_sample = util.volumetricsample(xyzs, self.getfinest().resolution)
else:
xyzs_sample = xyzs
bestpose = initialpose
for level, ol in enumerate(self.mapvoxels):
objective_func = ol.cost_func
#objective_func = self.objectivefuncMROL
#ol.use_bloom = False
dtheta = dtheta_base*pow(self.factor,(len(self.mapvoxels)-level-1))
dx = ol.resolution
# This helps avoid local minima, but limits accuracy to voxel size
# at the finest resolution
bestpose, cost = optimization.cost_min(objective_func, bestpose, (xyzs_sample,), dx, dtheta, max_iterations=20, verbosity=0)
#import pydb; pydb.set_trace()
#bestpose, cost = _cost_min_scipy(objective_func, bestpose, (xyzs_sample,))
# run it again at the finest resolution with tighter steps to get
# sub-voxel accuracy.
dx = dx*pow(self.factor,-2)
dtheta = dtheta_base*pow(self.factor,-2)
if not full_data:
# Sometimes get better results with the below resampling, but much faster.
xyzs = util.offsetvolsample(xyzs, self.getfinest().resolution)
bestpose, cost = optimization.cost_min(objective_func, bestpose, (xyzs,), dx, dtheta, max_iterations=100, verbosity=0)
return bestpose, cost
def optimise_alignment(self, xyzs, initialpose, full_data=False):
''' The format of this function allows us to use standard optimizers
and arbitrary cost functions from the scipy.optimize library.'''
dtheta_base = np.radians(0.5)
if not full_data:
xyzs_sample = util.volumetricsample(xyzs,
self.getfinest().resolution)
else:
xyzs_sample = xyzs
bestpose = initialpose
for level, ol in enumerate(self.mapvoxels):
objective_func = ol.cost_func
#objective_func = self.objectivefuncMROL
#ol.use_bloom = False
dtheta = dtheta_base * pow(self.factor,
(len(self.mapvoxels) - level - 1))
dx = ol.resolution
# This helps avoid local minima, but limits accuracy to voxel size
# at the finest resolution
bestpose, cost = optimization.cost_min(objective_func,
bestpose, (xyzs_sample, ),
dx,
dtheta,
max_iterations=20,
verbosity=0)
#import pydb; pydb.set_trace()
#bestpose, cost = _cost_min_scipy(objective_func, bestpose, (xyzs_sample,))
# run it again at the finest resolution with tighter steps to get
# sub-voxel accuracy.
dx = dx * pow(self.factor, -2)
dtheta = dtheta_base * pow(self.factor, -2)
if not full_data:
# Sometimes get better results with the below resampling, but much faster.
xyzs = util.offsetvolsample(xyzs, self.getfinest().resolution)
bestpose, cost = optimization.cost_min(objective_func,
bestpose, (xyzs, ),
dx,
dtheta,
max_iterations=100,
verbosity=0)
return bestpose, cost
def volumetricsample(self, res):
'''Downsamples the point cloud volumetrically according to the
resolution'''
self.points = util.volumetricsample(self.points, res)
def match(self, xyzs, centrepose, spreadpose):
"""Does a global search of the specified pose space.
Find the possible locations of the occupied list ol in the current
occupied list. Takes an occupied list to match to the current one and
two poses."""
debug = False
# determines the orientation step size as it relates to occupancy list
# resolution
poses = None
bestpose = poseutil.Pose3D()
totaltime = time.time()
lasttime = time.time()
#for i in range(len(self.voxels)):
# volumetric sample and each resolution equivalent to matching two
# occupiedlists together but without the double quantisation error
# TODO think about this number
#xyzs = util.getsample(xyzs, 1000)
levels = len(self.mapvoxels)
table = np.zeros((levels + 2, 9), dtype=np.dtype('S8'))
# header for the table of values of interest
table[
0, :] = "Res modal map scan pose pose Top Time Best".split(
)
table[
1, :] = "(m) Frac voxels points count count overlap Taken pose".split(
)
for i, mapol in enumerate(self.mapvoxels):
tablerow = i + 2
res = mapol.resolution
deltaposition = res
deltaorientation = res / self.feature_range # 8
if poses == None:
poses = poseutil.uniformposes(centrepose, spreadpose,
deltaposition, deltaorientation)
else:
poses = poseutil.refineposes(poses, deltaposition,
deltaorientation)
# TODO remove poses outside search space this needs tidying up/thinking about
#cparr = np.array(centrepose.get())
#sparr = np.array(spreadpose.get())
#
#D = np.array((deltaposition, deltaposition, deltaposition, deltaorientation, deltaorientation, deltaorientation))
#maxpose = cparr + sparr + D
#minpose = cparr - sparr - D
#select = np.logical_and(np.all(poses <= maxpose, 1), np.all(poses >= minpose, 1))
##print 'Culling poses: ', len(poses), sum(select)
#poses = poses[select]
xyzs_sample = util.volumetricsample(xyzs, res)
table[tablerow, :5] = (res, modalfractions[i], len(mapol),
len(xyzs_sample), len(poses))
# TODO TIME consuming line
#overlaps = self.getoverlaps(ol, i, poses)
overlaps = []
for pose in poses:
#overlap = np.sum(mapol.calccollisions(pose, xyzs, query=True))
overlap = mapol.calccollisions(pose, xyzs, query=False)
overlaps.append(overlap)
#overlaps.append(-self.objectivefuncMROL(pose, xyzs_sample))
overlaps = np.asarray(overlaps)
ind_max = np.argmax(overlaps)
bestpose.set(poses[ind_max])
bestoverlap = overlaps[ind_max]
#candidates = overlaps >= modalfractions[i] * bestoverlap
# maybe just take the top n?
if len(overlaps) > topn:
candidates = overlaps.argsort()[-topn:]
else:
candidates = range(len(overlaps))
timetaken = time.time() - lasttime
lasttime = time.time()
table[tablerow,
5:] = (len(candidates), bestoverlap, timetaken, bestpose)
print
printtable(table, width=9)
print bestpose
#scansize = ol.voxels[-1].shape[0]
poses = poses[candidates]
print "Total time: ", time.time() - totaltime
overlaps = overlaps[candidates]
return bestpose, bestoverlap
def match(self, xyzs, centrepose, spreadpose):
"""Does a global search of the specified pose space.
Find the possible locations of the occupied list ol in the current
occupied list. Takes an occupied list to match to the current one and
two poses."""
debug = False
# determines the orientation step size as it relates to occupancy list
# resolution
poses = None
bestpose = poseutil.Pose3D()
totaltime = time.time()
lasttime = time.time()
#for i in range(len(self.voxels)):
# volumetric sample and each resolution equivalent to matching two
# occupiedlists together but without the double quantisation error
# TODO think about this number
#xyzs = util.getsample(xyzs, 1000)
levels = len(self.mapvoxels)
table = np.zeros((levels+2, 9), dtype=np.dtype('S8'))
# header for the table of values of interest
table[0, :] = "Res modal map scan pose pose Top Time Best".split()
table[1, :] = "(m) Frac voxels points count count overlap Taken pose".split()
for i, mapol in enumerate(self.mapvoxels):
tablerow = i + 2
res = mapol.resolution
deltaposition = res
deltaorientation = res/self.feature_range # 8
if poses == None:
poses = poseutil.uniformposes(centrepose, spreadpose, deltaposition, deltaorientation)
else:
poses = poseutil.refineposes(poses, deltaposition, deltaorientation)
# TODO remove poses outside search space this needs tidying up/thinking about
#cparr = np.array(centrepose.get())
#sparr = np.array(spreadpose.get())
#
#D = np.array((deltaposition, deltaposition, deltaposition, deltaorientation, deltaorientation, deltaorientation))
#maxpose = cparr + sparr + D
#minpose = cparr - sparr - D
#select = np.logical_and(np.all(poses <= maxpose, 1), np.all(poses >= minpose, 1))
##print 'Culling poses: ', len(poses), sum(select)
#poses = poses[select]
xyzs_sample = util.volumetricsample(xyzs, res)
table[tablerow, :5] = (res, modalfractions[i], len(mapol), len(xyzs_sample), len(poses))
# TODO TIME consuming line
#overlaps = self.getoverlaps(ol, i, poses)
overlaps = []
for pose in poses:
#overlap = np.sum(mapol.calccollisions(pose, xyzs, query=True))
overlap = mapol.calccollisions(pose, xyzs, query=False)
overlaps.append(overlap)
#overlaps.append(-self.objectivefuncMROL(pose, xyzs_sample))
overlaps = np.asarray(overlaps)
ind_max = np.argmax(overlaps)
bestpose.set(poses[ind_max])
bestoverlap = overlaps[ind_max]
#candidates = overlaps >= modalfractions[i] * bestoverlap
# maybe just take the top n?
if len(overlaps) > topn:
candidates = overlaps.argsort()[-topn:]
else:
candidates = range(len(overlaps))
timetaken = time.time() - lasttime
lasttime = time.time()
table[tablerow, 5:] = (len(candidates), bestoverlap, timetaken, bestpose)
print
printtable(table, width=9)
print bestpose
#scansize = ol.voxels[-1].shape[0]
poses = poses[candidates]
print "Total time: ", time.time() - totaltime
overlaps = overlaps[candidates]
return bestpose, bestoverlap
def volumetricsample(self, res):
'''Downsamples the point cloud volumetrically according to the
resolution'''
self.points = util.volumetricsample(self.points, res)
