如何将点云旋转至XY平面？拟合平面后旋转角计算方案问询

我来帮你梳理下把拟合平面旋转到XY平面的核心思路，结合你现有的代码一步步拆解问题并修正细节：

要让拟合平面与XY平面对齐，本质是让拟合平面的单位法向量旋转到XY平面的法向量（Z轴方向：(0,0,1)）——因为平面的方向由法向量唯一确定，且你的点云已经平移到原点，只需要旋转操作即可。

第一步：修正法向量计算（你的代码里有个小坑） #

先看你的fitPlaneLTSQ函数，你把df['Y']赋值给了变量Z，拟合的是Y = a*X + b*Z + c，对应的平面标准方程是aX - Y + bZ + c = 0，所以法向量应该是(a, -1, b)，但你原代码里写的是(a,b,-1)，这会导致后续旋转方向完全错误！我先修正这个关键问题：

import numpy as np
import pandas as pd

def fitPlaneLTSQ(df):
    (rows, cols) = df.shape
    G = np.ones((rows, 3))
    G[:, 0] = df['X']
    G[:, 1] = df['Z']
    Y = df['Y']  # 变量名和列名统一，避免混淆
    (a, b, c), resid, rank, s = np.linalg.lstsq(G, Y, rcond=None)
    # 平面标准式Ax+By+Cz+D=0的法向量是(A,B,C)，对应这里的aX - Y + bZ + c = 0
    normal = (a, -1, b)
    nn = np.linalg.norm(normal)
    normal = normal / nn  # 单位化法向量
    return (c, normal)

第二步：计算旋转矩阵（推荐用Rodrigues公式） #

绕X/Y轴分步旋转容易出现万向锁问题（旋转顺序不同结果不同），更可靠的方式是用Rodrigues旋转公式直接生成旋转矩阵，把当前法向量转到Z轴方向：

# 加载并预处理数据（你的这部分代码没问题）
data = pd.read_csv('data.csv', sep=';')
meanx, meany, meanz = np.mean(data['X']), np.mean(data['Y']), np.mean(data['Z'])
data['X'] -= meanx
data['Y'] -= meany
data['Z'] -= meanz

# 拟合平面得到单位法向量
c, normal = fitPlaneLTSQ(data)
target_normal = np.array([0, 0, 1])  # XY平面的目标法向量

# 计算旋转轴和旋转角
axis = np.cross(normal, target_normal)
axis_norm = np.linalg.norm(axis)

# 如果法向量已经和Z轴平行，直接跳过旋转
if axis_norm < 1e-6:
    rotated_data = data.copy()
else:
    axis = axis / axis_norm  # 单位化旋转轴
    # 计算两个法向量的夹角（用clip避免数值误差导致超出[-1,1]范围）
    theta = np.arccos(np.clip(np.dot(normal, target_normal), -1.0, 1.0))
    
    # 用Rodrigues公式构建旋转矩阵
    cos_theta = np.cos(theta)
    sin_theta = np.sin(theta)
    # 构造反对称矩阵
    skew_matrix = np.array([
        [0, -axis[2], axis[1]],
        [axis[2], 0, -axis[0]],
        [-axis[1], axis[0], 0]
    ])
    rotation_matrix = cos_theta * np.eye(3) + sin_theta * skew_matrix + (1 - cos_theta) * np.outer(axis, axis)
    
    # 将点云转换为矩阵并应用旋转
    points = data[['X', 'Y', 'Z']].values.T
    rotated_points = rotation_matrix @ points
    # 转回DataFrame格式
    rotated_data = pd.DataFrame({
        'X': rotated_points[0, :],
        'Y': rotated_points[1, :],
        'Z': rotated_points[2, :]
    })

# 验证：旋转后的点云Z值均值应该接近0（拟合残差范围内）
print("旋转后点云Z值均值：", np.mean(rotated_data['Z']))

如果你一定要分解为绕X/Y轴的角度 #

如果业务场景必须得到绕X、Y轴的旋转角度，需要先确定旋转顺序（比如先绕X轴转α，再绕Y轴转β），然后从上面的旋转矩阵中推导角度：

假设旋转顺序是先绕X轴转α，再绕Y轴转β，对应的旋转矩阵是R = R_y(β) * R_x(α)，其中：

R_x(α) = [[1, 0, 0],
          [0, cosα, -sinα],
          [0, sinα, cosα]]
R_y(β) = [[cosβ, 0, sinβ],
          [0, 1, 0],
          [-sinβ, 0, cosβ]]

从我们生成的rotation_matrix中提取元素计算角度：

# 从旋转矩阵中提取绕X、Y轴的角度（先X后Y的顺序）
beta = np.arcsin(-rotation_matrix[2, 0])
cos_beta = np.cos(beta)
if abs(cos_beta) > 1e-6:
    alpha = np.arcsin(rotation_matrix[2, 1] / cos_beta)
else:
    # 当cosβ接近0时，万向锁出现，α可以取任意值，这里取0
    alpha = 0.0

print(f"绕X轴旋转角度（弧度）：{alpha}，绕Y轴旋转角度（弧度）：{beta}")

注意：这种分解方式依赖旋转顺序，不同顺序得到的角度完全不同，且可能出现万向锁（无法唯一确定角度），所以优先推荐直接使用旋转矩阵的方案。

内容的提问来源于stack exchange，提问作者qwertz