def optimize(self):
n = len(self.P)
if not n == len(self.Q):
print("P and Q must be of the same size")
return
P = np.array(self.P).transpose()
Q = np.array(self.Q).transpose()
centroidP = P.mean(axis=1).reshape((3, 1)).repeat(n, axis=1)
centroidQ = Q.mean(axis=1).reshape((3, 1)).repeat(n, axis=1)
H = (P - centroidP).dot((Q - centroidQ).transpose())
svd = np.linalg.svd(H)
R = svd[2].dot(svd[0].transpose())
if np.linalg.det(R) < 0:
R[:, 2] *= -1
t = centroidQ[:, 0] - R.dot(centroidP[:, 0])
T = [0, 0, 0, 1, 0, 0, 0]
T[0:3] = t.tolist()
T[3:7] = Quaternion(R).toTuple()
print(self.names[0], T)
self.instructions.text = "Current transform of\n" + self.names[
0] + "\nis\n" + str(T) + "\n\nSelect a fixed point."
self.plugin.gui.gui.applyConfiguration(self.names[0], T)
self.plugin.gui.gui.refresh()
for i in range(n):
self.setLineTransform(i)
self.plugin.gui.gui.refresh()
def setLineTransform(self, index, new=False):
name = self.groupName + "/pointPair_" + str(index)
if not new:
self.plugin.gui.gui.deleteNode(name, False)
LFname = self.names[index]
P = np.array(self.P[index]) # Moving
Q = np.array(self.Q[index]) # Fixed
T = self.plugin.gui.gui.getNodeGlobalTransform(LFname)
R = Quaternion(T[3:7]).toRotationMatrix()
t = np.array(T[0:3])
Pwf = R.dot(P) + t
self.plugin.gui.gui.addLine(name, Pwf.tolist(), Q.tolist(),
Color.yellow)
def selected(self, name, posInWorldFrame):
v = (posInWorldFrame.x(), posInWorldFrame.y(), posInWorldFrame.z())
if len(self.P) == len(self.Q):
self.Q.append(v)
inst = "Select a moving point."
else:
self.names.append(str(name))
# Compute local coordinate
T = self.plugin.gui.gui.getNodeGlobalTransform(str(name))
R = Quaternion(T[3:7]).toRotationMatrix()
t = np.array(T[0:3])
self.P.append(R.transpose().dot(np.array(v) - t))
index = len(self.P) - 1
self.setLineTransform(index, True)
self.plugin.gui.gui.refresh()
inst = "Select a fixed point."
text = inst + "\nNb selected points: " + str(len(self.P))
self.instructions.text = text
cl.gui.addBox ('fcl/b1_0', 2, 2, 2, [0,0 ,1,0.5])
cl.gui.addBox ('fcl/b1_1', 2, 2, 2, [0,0.5,1,0.5])
cl.gui.addSphere ("fcl/p0", 0.01, [1, 0, 1, 1])
cl.gui.addSphere ("fcl/p1", 0.01, [0, 1, 1, 1])
cl.gui.addArrow ("fcl/n0", 0.01, 1., [1, 0, 1, 1])
cl.gui.addArrow ("fcl/n1", 0.01, 1., [0, 1, 1, 1])
eps = 0.
d0 = 1.5183589910964868 + eps
n0 = [0.0310588, 0.942603, -0.332467]
d1 = 1.7485932899646754 + eps
n1 = [0.132426, -0.0219519, -0.99095]
qn0 = Quaternion()
qn1 = Quaternion()
qn0.fromTwoVector([1,0,0], n0)
qn1.fromTwoVector([1,0,0], n1)
pb1 = [ 0.135584, 0.933659, 0.290395, 0.119895, 0.977832, -0.164725, 0.0483272 ]
pb1_0 = translate (pb1, n0, d0)
pb1_1 = translate (pb1, n1, -d1)
cl.gui.applyConfiguration ("fcl/b1", pb1)
cl.gui.applyConfiguration ("fcl/b1_0", pb1_0)
cl.gui.applyConfiguration ("fcl/b1_1", pb1_1)
cl.gui.applyConfigurations(["fcl/p0","fcl/p1"], [
[0.832569, 0.259513, -0.239598, 0,0,0,1],
[-0.879579, 0.719545, 0.171906, 0,0,0,1] ])
cl.gui.applyConfigurations(["fcl/n0","fcl/n1"], [