def translate(self, coord, absolute=True):
'''
Arguements {Default value}
coord
array like object with dimensions 1x3 in format [x,y,z]
absolute
boolean that tells whether coord is the point to set joint to,
or the step by with to move the joint from its current location
'''
coord = numpy.array(coord)
if coord.shape != (3, ):
raise Exception("Incorrect input parameters")
if absolute:
self.translateMat = numpyTransform.translation(coord)
else:
self.translateMat *= numpyTransform.translation(coord)
def translate(self, coord, absolute=True):
'''
Arguements {Default value}
coord
array like object with dimensions 1x3 in format [x,y,z]
absolute
boolean that tells whether coord is the point to set joint to,
or the step by with to move the joint from its current location
'''
coord = numpy.array(coord)
if coord.shape != (3,):
raise Exception("Incorrect input parameters")
if absolute:
self.translateMat = numpyTransform.translation(coord)
else:
self.translateMat *= numpyTransform.translation(coord)
def minimizeCustom(self, p, q, **kwargs):
S = numpy.matrix(numpy.identity(4))
# TODO: try using functions from the nlopt module
def objectiveFunc(*args, **kwargs):
d = p
m = q
params = args[0]
if args[1].size > 0: # gradient
args[1][:] = numpy.array([pi / 100, pi / 100, pi / 100, 0.01, 0.01, 0.01]) # arbitrary gradient
# transform = numpy.matrix(numpy.identity(4))
translate = numpyTransform.translation(params[3:6])
rotx = numpyTransform.rotation(params[0], [1, 0, 0], N=4)
roty = numpyTransform.rotation(params[1], [0, 1, 0], N=4)
rotz = numpyTransform.rotation(params[2], [0, 0, 1], N=4)
transform = translate * rotx * roty * rotz
Dicp = numpyTransform.transformPoints(transform, d)
# err = self.rms_error(m, Dicp)
err = numpy.mean(numpy.sqrt(numpy.sum((m - Dicp) ** 2, axis=1)))
# err = numpy.sqrt(numpy.sum((m - Dicp) ** 2, axis=1))
return err
x0 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
if 'optAlg' in kwargs:
opt = nlopt.opt(kwargs['optAlg'], 6)
else:
opt = nlopt.opt(nlopt.GN_CRS2_LM, 6)
opt.set_min_objective(objectiveFunc)
opt.set_lower_bounds([-pi, -pi, -pi, -3.0, -3.0, -3.0])
opt.set_upper_bounds([pi, pi, pi, 3.0, 3.0, 3.0])
opt.set_maxeval(1500)
params = opt.optimize(x0)
# output = scipy.optimize.leastsq(objectiveFunc, x0, args=funcArgs)
# params = output[0]
# params = scipy.optimize.fmin(objectiveFunc, x0, args=funcArgs)
# constraints = []
# varBounds = [(-pi, pi), (-pi, pi), (-pi, pi), (-3.0, 3.0), (-3.0, 3.0), (-3.0, 3.0)]
# params = scipy.optimize.fmin_slsqp(objectiveFunc, x0, eqcons=constraints, bounds=varBounds, args=funcArgs)
# output = scipy.optimize.fmin_l_bfgs_b(objectiveFunc, x0, bounds=varBounds, args=funcArgs, approx_grad=True)
# params = output[0]
# print 'Min error:', output[1]
# params = scipy.optimize.fmin_tnc(objectiveFunc, x0, bounds=varBounds, args=funcArgs, approx_grad=True)
# params = scipy.optimize.fmin_slsqp(objectiveFunc, x0, eqcons=constraints, bounds=varBounds, args=funcArgs)
# params = scipy.optimize.fmin_slsqp(objectiveFunc, x0, eqcons=constraints, bounds=varBounds, args=funcArgs)
translate = numpyTransform.translation(params[3:6])
rotx = numpyTransform.rotation(params[0], [1, 0, 0], N=4)
roty = numpyTransform.rotation(params[1], [0, 1, 0], N=4)
rotz = numpyTransform.rotation(params[2], [0, 0, 1], N=4)
transform = translate * rotx * roty * rotz
return rotx * roty * rotz, S
def minimizePoint(self, q, p, **kwargs):
R = numpy.matrix(numpy.identity(4))
T = numpy.matrix(numpy.identity(4))
S = numpy.matrix(numpy.identity(4))
if 'weights' in kwargs:
weights = kwargs['weights']
else:
raise Warning('weights argument not supplied')
return R, T
# function [R,T] = eq_point(q,p,weights)
m = p.shape[0]
n = q.shape[0]
# normalize weights
weights = weights / weights.sum()
# find data centroid and deviations from centroid
q_bar = (numpy.mat(q.T) * numpy.mat(weights[:, numpy.newaxis])).getA().squeeze()
q_mark = q - numpy.tile(q_bar, n).reshape((n, 3))
# Apply weights
q_mark = q_mark * numpy.repeat(weights, 3).reshape((weights.shape[0], 3))
# find data centroid and deviations from centroid
p_bar = (numpy.mat(p.T) * numpy.mat(weights[:, numpy.newaxis])).getA().squeeze()
p_mark = p - numpy.tile(p_bar, m).reshape((m, 3))
# Apply weights
# p_mark = p_mark * numpy.repeat(weights, 3).reshape((weights.shape[0],3))
N = (numpy.mat(p_mark).T * numpy.mat(q_mark)).getA() # taking points of q in matched order
[U, Ss, V] = numpy.linalg.svd(N); # singular value decomposition
V = (numpy.mat(V).H).getA()
RMattemp = numpy.mat(V) * numpy.mat(U).T
Ttemp = (numpy.mat(q_bar).T - RMattemp * numpy.mat(p_bar).T).getA().squeeze()
R[:3, :3] = RMattemp.getA()
T = numpyTransform.translation(Ttemp)
return R, T, S
def objectiveFunc(*args, **kwargs):
d = p
m = q
params = args[0]
if args[1].size > 0: # gradient
args[1][:] = numpy.array([pi / 100, pi / 100, pi / 100, 0.01, 0.01, 0.01]) # arbitrary gradient
# transform = numpy.matrix(numpy.identity(4))
translate = numpyTransform.translation(params[3:6])
rotx = numpyTransform.rotation(params[0], [1, 0, 0], N=4)
roty = numpyTransform.rotation(params[1], [0, 1, 0], N=4)
rotz = numpyTransform.rotation(params[2], [0, 0, 1], N=4)
transform = translate * rotx * roty * rotz
Dicp = numpyTransform.transformPoints(transform, d)
# err = self.rms_error(m, Dicp)
err = numpy.mean(numpy.sqrt(numpy.sum((m - Dicp) ** 2, axis=1)))
# err = numpy.sqrt(numpy.sum((m - Dicp) ** 2, axis=1))
return err
def demo(*args, **kwargs):
import math
m = 80 # width of grid
n = m ** 2 # number of points
minVal = -2.0
maxVal = 2.0
delta = (maxVal - minVal) / (m - 1)
X, Y = numpy.mgrid[minVal:maxVal + delta:delta, minVal:maxVal + delta:delta]
X = X.flatten()
Y = Y.flatten()
Z = numpy.sin(X) * numpy.cos(Y)
# Create the data point-matrix
M = numpy.array([X, Y, Z]).T
# Translation values (a.u.):
Tx = 0.5
Ty = -0.3
Tz = 0.2
# Translation vector
T = numpyTransform.translation(Tx, Ty, Tz)
S = numpyTransform.scaling(1.0, N=4)
# Rotation values (rad.):
rx = 0.3
ry = -0.2
rz = 0.05
Rx = numpy.matrix([[1, 0, 0, 0],
[0, math.cos(rx), -math.sin(rx), 0],
[0, math.sin(rx), math.cos(rx), 0],
[0, 0, 0, 1]])
Ry = numpy.matrix([[math.cos(ry), 0, math.sin(ry), 0],
[0, 1, 0, 0],
[-math.sin(ry), 0, math.cos(ry), 0],
[0, 0, 0, 1]])
Rz = numpy.matrix([[math.cos(rz), -math.sin(rz), 0, 0],
[math.sin(rz), math.cos(rz), 0, 0],
[0, 0, 1, 0],
[0, 0, 0, 1]])
# Rotation matrix
R = Rx * Ry * Rz
transformMat = numpy.matrix(numpy.identity(4))
transformMat *= T
transformMat *= R
transformMat *= S
# Transform data-matrix plus noise into model-matrix
D = numpyTransform.transformPoints(transformMat, M)
# Add noise to model and data
M = M + 0.01 * numpy.random.randn(n, 3)
D = D + 0.01 * numpy.random.randn(n, 3)
# Run ICP (standard settings)
initialGuess = numpy.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
lowerBounds = numpy.array([-pi, -pi, -pi, -100.0, -100.0, -100.0])
upperBounds = numpy.array([pi, pi, pi, 100.0, 100.0, 100.0])
icp = ICP(M, D, maxIterations=15, dataDownsampleFactor=1, minimizeMethod='fmincon', **kwargs)
# icp = ICP(M, D, maxIterations=15, dataDownsampleFactor=1, minimizeMethod='point', **kwargs)
transform, err, t = icp.runICP(x0=initialGuess, lb=lowerBounds, ub=upperBounds)
# Transform data-matrix using ICP result
Dicp = numpyTransform.transformPoints(transform[-1], D)
# Plot model points blue and transformed points red
if False:
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
fig = plt.figure()
ax = fig.add_subplot(2, 2, 1, projection='3d')
ax.scatter(M[:, 0], M[:, 1], M[:, 2], c='r', marker='o')
ax.scatter(D[:, 0], D[:, 1], D[:, 2], c='b', marker='^')
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
ax = fig.add_subplot(2, 2, 2, projection='3d')
ax.scatter(M[:, 0], M[:, 1], M[:, 2], c='r', marker='o')
ax.scatter(Dicp[:, 0], Dicp[:, 1], Dicp[:, 2], c='b', marker='^')
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
ax.set_zlabel('Z Label')
ax = fig.add_subplot(2, 2, 3)
ax.plot(t, err, 'x--')
ax.set_xlabel('X Label')
ax.set_ylabel('Y Label')
plt.show()
else:
import visvis as vv
app = vv.use()
vv.figure()
vv.subplot(2, 2, 1)
vv.plot(M[:, 0], M[:, 1], M[:, 2], lc='b', ls='', ms='o')
vv.plot(D[:, 0], D[:, 1], D[:, 2], lc='r', ls='', ms='x')
vv.xlabel('[0,0,1] axis')
vv.ylabel('[0,1,0] axis')
vv.zlabel('[1,0,0] axis')
vv.title('Red: z=sin(x)*cos(y), blue: transformed point cloud')
# Plot the results
vv.subplot(2, 2, 2)
vv.plot(M[:, 0], M[:, 1], M[:, 2], lc='b', ls='', ms='o')
vv.plot(Dicp[:, 0], Dicp[:, 1], Dicp[:, 2], lc='r', ls='', ms='x')
vv.xlabel('[0,0,1] axis')
vv.ylabel('[0,1,0] axis')
vv.zlabel('[1,0,0] axis')
vv.title('ICP result')
# Plot RMS curve
vv.subplot(2, 2, 3)
vv.plot(t, err, ls='--', ms='x')
vv.xlabel('time [s]')
vv.ylabel('d_{RMS}')
vv.title('KD-Tree matching')
if 'optAlg' in kwargs:
opt2 = nlopt.opt(kwargs['optAlg'], 2)
vv.title(opt2.get_algorithm_name())
del opt2
else:
vv.title('KD-Tree matching')
app.Run()