【点】两个坐标系之间的转换

任意的两个笛卡尔坐标系之间的转换都可以用一个矩阵来表示。只要能算出转换矩阵,就可以算出A坐标系的一个点P在坐标系B里的对应点坐标。
在二维坐标系下至少给定2个点,便能计算出转换矩阵,而在三维坐标系下需要至少3个点。一般来说点越多,计算出来的转换就越精确。

# -*- coding: utf-8 -*-
import numpy as np

def get_transform(A, B):
    assert len(A) == len(B)

    # 点的个数
    N = A.shape[0] 

    # 中心点
    centroid_A = np.mean(A, axis=0)
    centroid_B = np.mean(B, axis=0)

    AA = A - np.tile(centroid_A, (N, 1))
    BB = B - np.tile(centroid_B, (N, 1))

    H = np.matmul(np.transpose(AA), BB)

    U, S, Vt = np.linalg.svd(H)

    R = np.matmul(Vt.T, U.T)

    # 特殊情况
    if np.linalg.det(R) < 0:
        print("Reflection detected")
        Vt[2, :] *= -1
        R = np.matmul(Vt.T,U.T)

    t = -np.matmul(R, centroid_A) + centroid_B
    # err = B - np.matmul(A,R.T) - t.reshape([1, 3])
    return R, t

if __name__=='__main__':

    a = np.array([[0.126901, -0.054710, 0.938],
                  [0.076113, -0.057638, 0.942],
                  [0.074728, -0.081546, 0.895],
                  [0.125624, -0.081282, 0.893],
                  [0.156072, -0.285685, 0.827],
                  [0.019842, -0.280429, 0.851],
                  [0.092248, -0.321462, 0.763],
                  [-0.043618, -0.312796, 0.788]])

    b = np.array([[0.46022323, 0.50710499, 0.28645349],
                  [0.42473236, 0.47370705, 0.28595987],
                  [0.38551146, 0.51143277, 0.28599533],
                  [0.42059597, 0.54657292, 0.28665495],
                  [0.34020177, 0.67224169, 0.13511288],
                  [0.25803548, 0.56310284, 0.13381004],
                  [0.24375798, 0.68313318, 0.13381931],
                  [0.16232316, 0.57071841, 0.13304782]])

    r, t = get_transform(a, b)

    b2 = np.matmul(a, r.T) + t.reshape([1, 3])
    print('b2:', b2)
0%