def pos_to_mat(data):
if 'pose' in dir(data):
data = data.pose
if 'position' in dir(data):
translation = [data.position.x, data.position.y, data.position.z]
rotation = [data.orientation.w, data.orientation.x, data.orientation.y,
data.orientation.z, ]
else:
translation = [data.translation.x, data.translation.y,
data.translation.z]
rotation = [ data.rotation.w, data.rotation.x, data.rotation.y,
data.rotation.z,]
res = transformations.quaternion_matrix(rotation)
res[:3,3] = translation
return res
def regress_Hs(Hs):
"""
>>> Hx = numpy.array([ \
[-0.88405797,-0.40579710, -0.23188406, 11.00000000], \
[-0.40579710, 0.42028986, 0.81159420, 21.00000000], \
[-0.23188406, 0.81159420, -0.53623188,-18.00000000], \
[ 0 , 0 , 0 , 1] \
])
>>> Hl1 = numpy.array([ \
[-0.87179487, 0.48717949, -0.05128205, 5.00000000], \
[ 0.33333333, 0.66666667, 0.66666667, -4.00000000], \
[ 0.35897436, 0.56410256, -0.74358974, 3.00000000], \
[ 0 , 0 , 0 , 1] \
])
>>> Hl2 = numpy.array([ \
[-0.70114943, 0.02298850, -0.71264368, 2.00000000], \
[ 0.66666667,-0.33333333, -0.66666667, -3.00000000], \
[-0.25287356,-0.94252874, 0.21839080, 9.00000000], \
[ 0 , 0 , 0 , 1] \
])
>>> Hc1 = numpy.array([ \
[-0.13831397,-0.61660716, -0.77502572, 0.131178 ], \
[-0.84328869,-0.33704404, 0.41864724, 34.399851 ], \
[-0.51935868, 0.71147518, -0.47335997,-41.570048 ], \
[ 0 , 0 , 0 , 1] \
])
>>> Hc2 = numpy.array([ \
[-0.69307617, 0.66439727, -0.27968142, 7.6275196 ], \
[ 0.01005777,-0.37903028, -0.92532961, -3.1216059 ], \
[-0.72079419,-0.64413687, 0.25601450, -8.9446943 ], \
[ 0 , 0 , 0 , 1] \
])
>>> numpy.linalg.norm(numpy.dot(Hl1, Hx) - numpy.dot(Hx, Hc1)) < 1e-6
True
>>> numpy.linalg.norm(numpy.dot(Hl2, Hx) - numpy.dot(Hx, Hc2)) < 1e-6
True
>>> exp_ul1 = -numpy.array([ 0.22798115, 0.91168461, 0.34188173])
>>> exp_ul2 = numpy.array([-0.32929278,-0.54882130, 0.76834982])
>>> exp_uc1 = -numpy.array([-0.65073141, 0.56815128, 0.50373878])
>>> exp_uc2 = numpy.array([ 0.33565393, 0.52655029,-0.78107611])
>>> exp_th1 = 360-193.002824
>>> exp_th2 = 155.239701
>>> thl1, ul1, pl1 = rotation_from_matrix(Hl1)
>>> thl2, ul2, pl2 = rotation_from_matrix(Hl2)
>>> thc1, uc1, pc1 = rotation_from_matrix(Hc1)
>>> thc2, uc2, pc2 = rotation_from_matrix(Hc2)
>>> thl1 *= 180./math.pi
>>> thl2 *= 180./math.pi
>>> thc1 *= 180./math.pi
>>> thc2 *= 180./math.pi
>>> if not numpy.linalg.norm(ul1 - exp_ul1) < 1e-4: print ul1
>>> if not numpy.linalg.norm(ul2 - exp_ul2) < 1e-4: print ul2
>>> if not numpy.linalg.norm(uc1 - exp_uc1) < 1e-4: print uc1
>>> if not numpy.linalg.norm(uc2 - exp_uc2) < 1e-4: print uc2
>>> if not abs(thl1 - exp_th1) < 1e-2: print thl1
>>> if not abs(thl2 - exp_th2) < 1e-2: print thl2
>>> if not abs(thc1 - exp_th1) < 1e-2: print thc1
>>> if not abs(thc2 - exp_th2) < 1e-2: print thc2
>>> Hx_reg = regress_Hs([[Hl1, Hc1], [Hl2, Hc2]])[0]
>>> numpy.allclose(Hx_reg[:3,:3], Hx[:3,:3])
True
>>> numpy.allclose(Hx_reg[:3,3], Hx[:3,3])
True
"""
E = None
A2 = numpy.zeros((4,4))
for (Hl, Hc) in Hs:
pl = transformations.quaternion_from_matrix(Hl)
pc = transformations.quaternion_from_matrix(Hc)
angle, direc, point = rotation_from_matrix(Hl)
weight = abs(angle) + numpy.linalg.norm(Hl[:3,3])
lhA = quat_plus(pl) - quat_bar(pc)
#lhA*= weight
if E == None:
E = lhA
else:
E = numpy.append(E, lhA, axis = 0)
Et = numpy.transpose(E)
A = numpy.dot(Et, E)
lambdas, vs = eig(A)
qx = vs[0]
Ax = transformations.quaternion_matrix(qx)
# Optimization of translation
A = None
b = []
for Hl, Hc in Hs:
rl = Hl[:3,3]
rc = Hc[:3,3]
Al = Hl[:3,:3]
angle, direc, point = rotation_from_matrix(Hl)
weight = abs(angle) + numpy.linalg.norm(Hl[:3,3])
Ai = Al - numpy.eye(3)
#Ai *= weight
if A == None:
A = Ai
else:
A = numpy.append(A, Ai, axis = 0)
bi = numpy.dot(Ax[:3,:3], rc) - rl
#bi *= weight
b = numpy.append(b, bi, axis = 0)
# print rc
# print Ax
# print numpy.dot(Ax[:3,3], rc)
# print "rl:",rl
# print Al
# print "---"
# leastquare A*rx = b
# print A
# print b
rx, res, rank, s = linalg.lstsq(A, b, 1e-6)
#print rx, res, rank, s
Ax[:3,3] = rx
return Ax, E, lambdas, res, len(Hs)
