solve_RT1_RT2.py

def solve_RT1_RT2(A, B):
    ''' solve for B = x1.dot(A).dot(x2)
        A = [[[R1,T1],[0,1]]...] # shape: (n,4,4)
        B = [[[R2,T2],[0,1]]...] # shape: (n,4,4)
        minimize{np.einsum('jk,ikp,pq->ijq', x1, A, x2)-B}
        return: x1,x2: shape(4,4)
    '''
    def func(x,a,b):
        x1 = x[:16].reshape(4,4)
        x2 = x[16:].reshape(4,4)
        tmp = np.einsum('jk,ikp,pq->ijq', x1, a, x2)
        return (tmp - b).reshape(-1)
    #init = np.ones((32,), np.float)
    init = np.concatenate((np.identity(4,np.float).reshape(-1), np.identity(4, np.float).reshape(-1)))
    xx,_ = scipy.optimize.leastsq(func, init, args=(A,B))
    return xx[:16].reshape(4,4),xx[16:].reshape(4,4)