背景：
需要知坐标系a的原点、x轴正半轴上任一点和y轴正半轴上任一点在坐标系b下的坐标，求解坐标系a到坐标系b的旋转矩阵R和平移矩阵T；
输入：坐标系a的原点在坐标系b下的坐标:Oab(x1,y1,z1);
坐标系a的x轴正半轴上任一点在坐标系b下的坐标:Pxb(x2,y2,z2);
坐标系a的y轴正半轴上任一点在坐标系b下的坐标:Pyb(x3,y3,z3);
思路：
因为实际对应三点都是在坐标轴上，所以直接利用向量的思路来求解，利用旋转坐标的定义来计算。三维坐标旋转矩阵推导过程（包看懂）_三维旋转矩阵-CSDN博客
拓展：一般的对应点在不同坐标系下的计算需要用到Umeyama算法
python 有evo module 求解对应轨迹的工具，
C++ Eigen 库也是用了Umeyama，以及PCL库。【动手学MVG】ICP算法原理和代码实现_scale icp-CSDN博客
如下：有从数学的角度推导，到不同库的实例到测试的
Umeyama 算法之源码阅读与测试_eigen::umeyama 测试-CSDN博客；实际工作中调用对应的开源库即可。
代码
本文求解代码如下：
python
import numpy as np
import math
import pandas as pd
def calPoseFrom3Points(self, Oab, Pxb, Pyb):
'''
功能：已知坐标系a的原点、x轴正半轴上任一点和y轴正半轴上任一点在坐标系b下的坐标，
求解坐标系a到坐标系b的旋转矩阵R和平移矩阵T；
输入：坐标系a的原点在坐标系b下的坐标:Oab(x1,y1,z1);
坐标系a的x轴正半轴上任一点在坐标系b下的坐标:Pxb(x2,y2,z2);
坐标系a的y轴正半轴上任一点在坐标系b下的坐标:Pyb(x3,y3,z3);
输出：坐标系a到坐标系b的旋转矩阵Rab，输出格式为矩阵;平移矩阵Tab
'''
x = (Pxb - Oab) / np.linalg.norm(Pxb - Oab)
y = (Pyb - Oab) / np.linalg.norm(Pyb - Oab)
z = np.cross(x, y)
length = np.linalg.norm(z)
z = z / length
print("the np.matrix:",np.matrix([x, y, z]))
Rab = np.matrix([x, y, z]).transpose()
print(Rab)
Tab = np.matrix(Oab).transpose()
return Rab, Tab
def validate_transformation(Rab, Tab,Oab, Pxb, Pyb):
'''
功能：验证旋转矩阵Rab和平移矩阵Tab是否得到目标坐标；
输入：坐标系a上的一点，points_original ，b系下的点points_validate_expected
输出：输出Rab*points_original + Tab =？ points_validate_expected 结果;
'''
points_original = np.array([[0, 0, 0], # 点1
[1, 0, 0], # 点2
[0, 1, 0]]) # 点3
# 将原始坐标点矩阵扩展为4x3的齐次坐标矩阵
P_original_homogeneous = np.vstack([points_original.T, np.ones((1, 3))])
print("The P_original_homogeneous matrix is:", P_original_homogeneous)
points_validate_expected=np.vstack((Oab,Pxb,Pyb))
print("The points_validate_expected matrix is:", points_validate_expected)
T_matrix = np.vstack([
np.hstack([Rab, Tab.reshape((-1, 1))]),
[0, 0, 0, 1]
])
print("The T_matrix matrix is:", T_matrix)
# 应用变换矩阵得到新的坐标点矩阵
P_new_homogeneous = np.dot(T_matrix,P_original_homogeneous)
# 提取出变换后的三维坐标点（去掉齐次坐标）
P_new = P_new_homogeneous[:3, :]
Point_new=P_new.T
print("p-new=:", P_new)
print("points_validate_expected=:", points_validate_expected)
if np.allclose(Point_new, points_validate_expected,rtol=1e-03, atol=1e-05):
return True
else:
return False
# 主函数
def main(excel_file, sheet_name):
# 读取Excel文件
df = pd.read_excel(excel_file, sheet_name=sheet_name)
# 初始化结果DataFrame的列
result_columns = [
'Rotation_Matrix_11', 'Rotation_Matrix_12', 'Rotation_Matrix_13',
'Rotation_Matrix_21', 'Rotation_Matrix_22', 'Rotation_Matrix_23',
'Rotation_Matrix_31', 'Rotation_Matrix_32', 'Rotation_Matrix_33',
'Translation_X', 'Translation_Y', 'Translation_Z',
'Validation_Result'
]
# 初始化结果列表
result_rows = []
# 遍历DataFrame的每一行
for index, row in df.iterrows():
Oab = np.array([row['Oab_x'], row['Oab_y'], row['Oab_z']])
Pxb = np.array([row['Pxb_x'], row['Pxb_y'], row['Pxb_z']])
Pyb = np.array([row['Pyb_x'], row['Pyb_y'], row['Pyb_z']])
# 计算旋转矩阵和平移矩阵
Rab, Tab = calPoseFrom3Points(Oab, Pxb, Pyb)
# 验证计算结果
is_valid = validate_transformation(Rab, Tab, Oab, Pxb, Pyb)
# 将结果添加到结果DataFrame中
result_row = {
'Rotation_Matrix_11': Rab[0, 0],
'Rotation_Matrix_12': Rab[0, 1],
'Rotation_Matrix_13': Rab[0, 2],
'Rotation_Matrix_21': Rab[1, 0],
'Rotation_Matrix_22': Rab[1, 1],
'Rotation_Matrix_23': Rab[1, 2],
'Rotation_Matrix_31': Rab[2, 0],
'Rotation_Matrix_32': Rab[2, 1],
'Rotation_Matrix_33': Rab[2, 2],
'Translation_X': Tab[0],
'Translation_Y': Tab[1],
'Translation_Z': Tab[2],
'Validation_Result': is_valid
}
result_rows.append(result_row)
result_df = pd.DataFrame(result_rows)
# 显示或保存结果DataFrame
#print(result_df)
# 将结果写入新的Excel文件
output_excel_file = ' output_data.xlsx'
with pd.ExcelWriter(output_excel_file, engine='openpyxl') as writer:
df.to_excel(writer, sheet_name='Original_Data', index=False)
result_df.to_excel(writer, sheet_name='Transformation_Results', index=False)
print(f"Results have been saved to {output_excel_file}")
# 调用主函数
if __name__ == "__main__":
excel_file = 'D: ...data.xlsx' # 假设Excel文件名是data.xlsx
sheet_name = 'sheet1' # 假设数据在第一个工作表上
main(excel_file, sheet_name)