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)