1
from __future__ import division
5
def cost_min(cost_func, initialpose, args, dx, dq, max_iterations=100, verbosity=0, two_D=False):
6
if isinstance(initialpose, poseutil.Pose3D):
7
bestpose = np.asarray(initialpose.getTuple())
9
bestpose = np.asarray(initialpose)
11
# TODO use manhatten moves so no diagonal moves, can still move
12
# diagonally but takes two steps however drastically cuts number of
13
# adjacent poses to test from 2^6 to 2*6
14
# each coord is plus or minus 1 individually all other are held the same
15
# this is equivalent to partial differentiation and should only need 12
22
# TODO fix this as per below
24
#(1. , 0 , 0 , 0 , 0 , 0) ,
25
#(0 , 1. , 0 , 0 , 0 , 0) ,
26
#(0 , 0 , 0 , 0 , 0 , 1.) ,
27
#(-1. , 0 , 0 , 0 , 0 , 0) ,
28
#(0 , -1. , 0 , 0 , 0 , 0) ,
29
#(0 , 0 , 0 , 0 , 0 , -1.) ,
32
# TODO Try parallel testing of these poses.
33
#import pydb;pydb.set_trace()
34
mg = np.vstack((np.identity(6), -np.identity(6)))
35
x_ = np.hstack([np.vstack([X,X,X]),np.identity(3)])
36
x__ = np.hstack([np.vstack([X,X,X]),-np.identity(3)])
37
y_ = np.hstack([np.vstack([Y,Y,Y]),np.identity(3)])
38
y__ = np.hstack([np.vstack([Y,Y,Y]),-np.identity(3)])
39
z_ = np.hstack([np.vstack([Z,Z,Z]),np.identity(3)])
40
z__ = np.hstack([np.vstack([Z,Z,Z]),-np.identity(3)])
41
mg = np.vstack([mg,x_,-x_,x__,-x__,y_,-y_,y__,-y__,z_,-z_,z__,-z__])
45
initialoverlap = -cost_func(bestpose, args[0])
46
previousmax = initialoverlap
48
p_tup = poseutil.tuplepose(bestpose)
49
already_checked[p_tup] = True
53
print 'Scan voxel count, overlaps, max overlap'
54
for i in range(max_iterations):
58
# check poses surrounding bestpose
60
p_tup = poseutil.tuplepose(p)
61
# optimization to save calculating the cost for poses which we have
62
# previously calculated the cost for
63
if already_checked.has_key(p_tup):
67
already_checked[p_tup] = True
68
cost = -cost_func(p, args[0])
69
# negative because this is a maximiser
73
print poseutil.Pose3D(p), cost
74
assert len(overlaps) != 0, "Already checked ALL of these poses: Some sort of circular minimisation error?"
75
overlaps = np.array(overlaps)
76
maxoi = np.argmax(overlaps)
77
maxo = overlaps[maxoi]
78
if sum(overlaps == maxo) > 1:
79
print 'WARNING: multiple maxima'
81
print i, ':', overlaps, maxo
82
#print 'Best pose:', poses[maxoi]
83
# break if current pose is maximum in the case when alternative pose is
84
# of equal overlap pick the previous pose
85
if maxo <= previousmax:
89
# re-assign for next loop
90
bestpose = poses[maxoi]
94
if iter_count >= max_iterations:
95
print "WARNING: Maximum number of iterations reached. Solution did not reach convergence."
98
print 'cost function evaluated:', called_count, 'times over', iter_count, 'iterations'
99
print len(args[0]), 'cost increase:', initialoverlap, '->', maxo
100
return bestpose, -maxo # -ve again because it is standard for the calling function to assume a minimisation.
103
def plotobjective(cost_func, initialpose, xyzs, plot_range=(-0.2, 0.2, np.radians(-5), np.radians(5)), dx=None, dq=None):
105
Plots the cost function for various poses, centered about the initialpose.
107
dofs = {0:'x', 1:'y', 2:'z', 3:'rotx', 4:'roty', 5:'rotz'}
108
xmin, xmax = plot_range[:2]
109
qmin, qmax = plot_range[2:]
111
dx = (xmax-xmin)/100.0
113
dq = (qmax-qmin)/100.0
115
[xmin, xmin, xmin, qmin, qmin, qmin],
116
[xmax, xmax, xmax, qmax, qmax, qmax],
117
[dx, dx, dx, dq, dq, dq]])
122
pylab.subplot(2, 1, 1)
124
X = np.arange(ranges[i, 0], ranges[i, 1], ranges[i, 2]) + initialpose[i]
125
pose = np.array(initialpose, dtype=float)
129
Y.append(cost_func(pose, xyzs))
130
pylab.plot(X - initialpose[i], Y, 'x-', label=str(dofs[i]))
134
pylab.ylabel('objective function value')
135
pylab.subplot(2, 1, 2)
136
for i in range(3, 6):
137
X = np.arange(ranges[i, 0], ranges[i, 1], ranges[i, 2]) + initialpose[i]
138
pose = np.array(initialpose, dtype=float)
142
Y.append(cost_func(pose, xyzs))
143
pylab.plot(X - initialpose[i], Y, 'x-', label=str(dofs[i]))
147
pylab.ylabel('objective function value')
added
removed
mrol_mapping/optimization.py
