I am trying to align a step file with a 3D model to a point of clouds of the object 3D scan. I am doing the following steps and using the open3d module.
- Convert the step file to stl and then to a point of clouds.
- Calculate the center of the scan and 3d model. Then aligned the two on the same point.
- Get the PCA components of the two.
- Compute a rotation matrix so that the same PCA components are aligned. Repeat this step for the new points of the scan
I have been trying two ways to calculate the rotation matrix
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
and dst_norm
are the PCA components can be either PC1, PC2 or PC3. source_array
is the array of the 3D scan
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 and v2 are the PCA components can be either PC1, PC2 or PC3. source_array
is the array of the 3D scan
The image below is the initial PCA components' orientation, where red, green and blue are PC1, PC2, PC3 of the 3D model and the darker colors are from the scan.
I am new to point cloud computation and perhaps there is another way to align the two point of clouds but I do not know how. I tried ICP but it did not work. The two point of clouds have different sizes but I think that is not the problem.