用python实现上述问题， 不使用第三方库

直接在Python中实现三维点序列间的刚体变换比较复杂，因为涉及到一些线性代数和几何运算，尤其是对于本质矩阵的估计。然而，我们可以手动构建矩阵并处理数学部分。这里有一个简化版的示例，假设我们已经有两个三维点集，并已知一部分对应点对。

首先，我们需要手动计算单应性矩阵。给定两个点对（`point1` 和 `point2`），它们的坐标分别为 `p1[i], p2[i]`，我们可以这样计算：

def homography(point1, point2):
    n_points = len(point1)
    A = np.zeros((n_points*2, 9), dtype=np.float64)  # 初始化A矩阵

    for i in range(n_points):
        row1 = [point1[i][0]**2, point1[i][0]*point1[i][1], point1[i][0]*point1[i][2], point1[i][1]**2,
                point1[i][1]*point1[i][2], point1[i][2]**2, 2*point1[i][0], 2*point1[i][1], 2*point1[i][2]]
        row2 = [-point2[i][0]*point1[i][0], -point2[i][0]*point1[i][1], -point2[i][0]*point1[i][2],
                -point2[i][1]*point1[i][0], -point2[i][1]*point1[i][1], -point2[i][1]*point1[i][2],
                point2[i][0], point2[i][1], point2[i][2]]

        A[i*2:i*2+2] = np.array(row1).reshape(2, -1)
        A[i*2+2:i*2+4] = np.array(row2).reshape(2, -1)

    U, S, Vh = np.linalg.svd(A)
    H = Vh[-1].reshape((3, 3))

    return H

# 示例：
point1 = ...  # 第一组三维点集合
point2 = ...  # 第二组三维点集合
H = homography(point1, point2)

请注意，这个示例仅适用于已知对应点的情况，并且仅能处理平面内的变换。如果你想处理本质矩阵，会更复杂，通常需要专门的数学库，如NumPy或SciPy。