Frank C. Park手眼標定方法

Frank C. Park與Tsai都是採用了兩步法，但它利用李代數求解AX=XB方法。
由$log A = log XBX^T$
有$[a]=X[b]X^T$,$[]$是so3形式的旋轉向量
所以$X=(M^TM)^{-\frac{1}{2}}M^T$,$M=\sum_{i=1}^kb_ia_i^T$
而平移向量求解方式與Tsai相同。
代碼實現：

def FCParkSolution(alpha,beta,ta,tb):
    # FCPark solution
    # RotX
    M = np.zeros(shape=(3,3))
    for j in range(len(alpha)):
        M = M + np.asmatrix(beta[j].reshape((3,1)))*np.asmatrix(alpha[j].reshape((3,1))).T
    eig_val,eig_vec = np.linalg.eig(M.T * M)
    FCPark_Rx =  np.asarray(eig_vec*np.diag(np.sqrt(1.0/eig_val))*np.linalg.inv(eig_vec)*M.T)
    # Estimate tx
    C = np.eye(3)-SE3.VecToRot(alpha[0])
    for i in range(1,len(alpha)):
        C = np.vstack((C,np.eye(3)-SE3.VecToRot(alpha[i])))
    g = ta[0] - np.dot(FCPark_Rx,tb[0])
    for i in range(1,len(alpha)):
        g = np.vstack((g, ta[i] - np.dot(FCPark_Rx,tb[i])))
    g = g.reshape(3*len(alpha),1)
    FCPark_tx = np.dot(np.linalg.pinv(C),g).reshape(3)
    return FCPark_Rx, FCPark_tx

地址：https://github.com/dinhhuy2109/python-cope