法线的夹角

import numpy as np
from scipy.spatial.transform import Rotation
# x + y + z + 2.6494 = 0
# 平面1的法向量
normal_plane1 = np.array([0.9941, -0.0218, -0.1060])

# 平面2的法向量
normal_plane2 = np.array([1,0,0])
# normal_plane2 = np.array([0, -0.03, -0.01])

# 计算法向量的长度（用于单位化）
length_plane1 = np.linalg.norm(normal_plane1)
length_plane2 = np.linalg.norm(normal_plane2)

# 单位化法向量
unit_normal_plane1 = normal_plane1 / length_plane1
unit_normal_plane2 = normal_plane2 / length_plane2

# 计算两法向量之间的夹角
cos_theta = np.dot(unit_normal_plane1, unit_normal_plane2)
theta = np.arccos(cos_theta)

# 计算旋转轴，使用叉积
rotation_axis = np.cross(unit_normal_plane1, unit_normal_plane2)
rotation_axis /= np.linalg.norm(rotation_axis)
print(rotation_axis)
# 创建旋转对象
r = Rotation.from_rotvec(theta * rotation_axis)

# 获取旋转矩阵
rotation_matrix = r.as_matrix()

# 计算平移矢量
translation_vector = np.array([0, 0, 0])  # 不需要平移，因为平面2是x=0

# 输出旋转矩阵和平移矢量
print("Rotation Matrix:")
print(rotation_matrix)
print("Translation Vector:")
print(translation_vector)

已知旋转前向量为P, 旋转后变为Q。由点积定义可知：

可推出P，Q之间的夹角为：

2.旋转轴

由1中可知，旋转角所在的平面为有P和Q所构成的平面，那么旋转轴必垂直该平面。

假定旋转前向量为a(a1, a2, a3)， 旋转后向量为b（b1, b2, b3)。由叉乘定义得：

所以旋转轴c(c1, c2, c3)为：

如何估计平面法线

import open3d as o3d
pcd = o3d.io.read_point_cloud(r"C:\Users\peog\Desktop\volume\plane\a.pcd")
plane_model, inliers = pcd.segment_plane(distance_threshold=0.01,
                                         ransac_n=50,
                                         num_iterations=1000)
[a, b, c, d] = plane_model
print(f"Plane equation: {a:.4f}x + {b:.4f}y + {c:.4f}z + {d:.4f} = 0")

inlier_cloud = pcd.select_by_index(inliers)
inlier_cloud.paint_uniform_color([1.0, 0, 0])
outlier_cloud = pcd.select_by_index(inliers, invert=True)
outlier_cloud.paint_uniform_color([0, 1, 0])
# o3d.visualization.draw_geometries([inlier_cloud, outlier_cloud])