将步骤文件对齐到云点 python

Question

我正在尝试将带有 3D 模型的步骤文件与对象 3D 扫描的云点对齐。我正在执行以下步骤并使用 open3d 模块。

1. 将step文件转成stl再转成点云
2. 计算扫描和 3d 模型的中心。然后将两者对齐在同一点上。
3. 得到两者的PCA分量
4. 计算旋转矩阵，使相同的 PCA 组件对齐。对扫描的新点重复此步骤

一直在尝试两种计算旋转矩阵的方法

def pca_rotate(self, src_norm, dst_norm, source_array):

        # Calculate the rotation quaternion
        rot_axis = np.cross(src_norm, dst_norm)
        if np.linalg.norm(rot_axis) < 1e-6:
            # The two vectors are already aligned
            rot_quat = np.array([1, 0, 0, 0])
        else:
            rot_angle = np.arccos(np.dot(src_norm, dst_norm))
            rot_axis_norm = rot_axis / np.linalg.norm(rot_axis)
            rot_quat = R.from_rotvec(rot_angle * rot_axis_norm).as_quat()

        # Apply the rotation quaternion to the source coordinates
        src_aligned = R.from_quat(rot_quat).apply(src_norm)
        source_rotate_array = R.from_quat(rot_quat).apply(source_array)
        print(src_aligned, dst_norm)
        
        return source_rotate_array

src_norm 和 dst_norm 是 PCA 组件，可以是 PC1、PC2 或 PC3。 source_array 是 3D 扫描的数组

def pca_rotate(self, v1, v2, source_array):
        theta = np.arccos(np.dot(v1, v2) / np.linalg.norm(v1)*np.linalg.norm(v2))
        k = np.cross(v1, v2) / np.linalg.norm(np.cross(v1, v2))
        K = np.array([[0, -k[2], k[1]],[k[2], 0, -k[0]],[-k[1], k[0], 0]])
        I = np.eye(3)
        R = I + np.sin(theta)*K + (1-np.cos(theta))*(np.matmul(K,K))

        source_rotate_array = R @ source_array.T
        
        return source_rotate_array.T

v1 和 v2 是 PCA 组件，可以是 PC1、PC2 或 PC3。 source_array 是 3D 扫描的数组 是初始 PCA 组件方向的方向，其中红色、绿色和蓝色是 3D 模型的 PC1、PC2、PC3，较暗的颜色来自扫描。 我是点云计算的新手，也许还有另一种方法可以对齐云的两个点，但我不知道如何。我尝试了 ICP，但没有用。 云的两个点有不同的大小，但我认为这不是问题。