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)