def matchandadd(self, ol, centrepose, spreadpose):
""" matches the given occupiedlist ol by searching around the
centrepose a distance of spreadpose. If the match is successful the
occupied list is transfomred and combined with the current occupied
list. Returns the pose at which the occupied list ol was added."""
# TODO add a threshold for deciding whether to merge
# TODO add unit test for this
# TODO add a ray trace clear?
poses, overlaps = self.match(ol, centrepose, spreadpose)
bestpose = poseutil.Pose3D(poses[np.argmax(overlaps), :])
self.addpoints(bestpose.transformPoints(ol.getpoints()))
return bestpose
def _cost_min_scipy(objective_func, initialpose, args):
import scipy.optimize as optimize
initialpose = initialpose.getTuple()
returns = optimize.fmin(objective_func,
initialpose,
args=args,
disp=True,
full_output=True)
#returns = optimize.fmin_powell(objective_func, initialpose, args=args, disp=True, full_output=True)
#returns = optimize.fmin_slsqp(objective_func, initialpose, args=args, full_output=True) # doesn't work?
#initpose = np.asarray(initialpose)
#delta = np.asarray((0.5, 0.5, 0.5, 0.2, 0.2, 0.2))
#lower = initpose - delta
#upper = initpose + delta
#returns = optimize.anneal(objective_func, initialpose, args=args, lower=lower, upper=upper)
bestpose = poseutil.Pose3D(X=returns[0])
cost = returns[1]
return bestpose, cost
def align_points(self, xyzs, guess_pose, two_D=False, full_data=False):
'''Align the point cloud xyzs with the data already contained in the
map, using the guess_pose as a seed to the optimisation. Returns the
pose that best fits the point cloud data to the map and the value of
the objective function at that pose.
If the map is a multi-resolution structure, then the alignment will use
the data at the various resolutions to perform the alignment, which is
more expensive, but more robust. To force alignment at just a single
resolution, specify the level parameter.
By default, the alignment is a 6DOF optimization. This can be
constrained to the XY plane by enabling the two_D parameter.'''
xyzs = xyzs.points
bestpose, cost = self.mrol.optimise_alignment(xyzs,
guess_pose,
full_data=full_data)
bestpose = poseutil.Pose3D(bestpose)
return bestpose, cost
def matchbest(self, xyzs, centrepose, spreadpose):
poses, overlaps = self.match(xyzs, centrepose, spreadpose)
bestind = np.argmax(overlaps)
bestpose = poseutil.Pose3D(poses[bestind, :])
cost = overlaps[bestind]
return bestpose, cost
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 cost_min(cost_func,
initialpose,
args,
dx,
dq,
max_iterations=100,
verbosity=0,
two_D=False):
if isinstance(initialpose, poseutil.Pose3D):
bestpose = np.asarray(initialpose.getTuple())
else:
bestpose = np.asarray(initialpose)
already_checked = {}
# TODO use manhatten moves so no diagonal moves, can still move
# diagonally but takes two steps however drastically cuts number of
# adjacent poses to test from 2^6 to 2*6
# each coord is plus or minus 1 individually all other are held the same
# this is equivalent to partial differentiation and should only need 12
# poses to check
X = np.array([1, 0, 0])
Y = np.array([0, 1, 0])
Z = np.array([0, 0, 1])
if two_D:
raise NotImplmented
# TODO fix this as per below
#mg = np.vstack((
#(1. , 0 , 0 , 0 , 0 , 0) ,
#(0 , 1. , 0 , 0 , 0 , 0) ,
#(0 , 0 , 0 , 0 , 0 , 1.) ,
#(-1. , 0 , 0 , 0 , 0 , 0) ,
#(0 , -1. , 0 , 0 , 0 , 0) ,
#(0 , 0 , 0 , 0 , 0 , -1.) ,
#))
else:
# TODO Try parallel testing of these poses.
#import pydb;pydb.set_trace()
mg = np.vstack((np.identity(6), -np.identity(6)))
x_ = np.hstack([np.vstack([X, X, X]), np.identity(3)])
x__ = np.hstack([np.vstack([X, X, X]), -np.identity(3)])
y_ = np.hstack([np.vstack([Y, Y, Y]), np.identity(3)])
y__ = np.hstack([np.vstack([Y, Y, Y]), -np.identity(3)])
z_ = np.hstack([np.vstack([Z, Z, Z]), np.identity(3)])
z__ = np.hstack([np.vstack([Z, Z, Z]), -np.identity(3)])
mg = np.vstack(
[mg, x_, -x_, x__, -x__, y_, -y_, y__, -y__, z_, -z_, z__, -z__])
mg[:, 0:3] *= dx
mg[:, 3:6] *= dq
initialoverlap = -cost_func(bestpose, args[0])
previousmax = initialoverlap
maxo = initialoverlap
p_tup = poseutil.tuplepose(bestpose)
already_checked[p_tup] = True
called_count = 1
iter_count = 0
if verbosity > 1:
print 'Scan voxel count, overlaps, max overlap'
for i in range(max_iterations):
iter_count += 1
poses = bestpose + mg
overlaps = []
# check poses surrounding bestpose
for p in poses:
p_tup = poseutil.tuplepose(p)
# optimization to save calculating the cost for poses which we have
# previously calculated the cost for
if already_checked.has_key(p_tup):
overlaps.append(0)
continue
else:
already_checked[p_tup] = True
cost = -cost_func(p, args[0])
# negative because this is a maximiser
called_count += 1
overlaps.append(cost)
if verbosity > 3:
print poseutil.Pose3D(p), cost
assert len(
overlaps
) != 0, "Already checked ALL of these poses: Some sort of circular minimisation error?"
overlaps = np.array(overlaps)
maxoi = np.argmax(overlaps)
maxo = overlaps[maxoi]
if sum(overlaps == maxo) > 1:
print 'WARNING: multiple maxima'
if verbosity > 2:
print i, ':', overlaps, maxo
#print 'Best pose:', poses[maxoi]
# break if current pose is maximum in the case when alternative pose is
# of equal overlap pick the previous pose
if maxo <= previousmax:
maxo = previousmax
break
else:
# re-assign for next loop
bestpose = poses[maxoi]
previousmax = maxo
if verbosity > 1:
print bestpose, maxo
if iter_count >= max_iterations:
print "WARNING: Maximum number of iterations reached. Solution did not reach convergence."
if verbosity > 0:
print 'cost function evaluated:', called_count, 'times over', iter_count, 'iterations'
print len(args[0]), 'cost increase:', initialoverlap, '->', maxo
return bestpose, -maxo # -ve again because it is standard for the calling function to assume a minimisation.