计算两坐标系轴上有对应三点的 旋转变换矩阵(Python)

背景:

需要知坐标系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)
相关推荐
wuyk5554 小时前
Python办公自动化-第01讲-环境搭建与自动化认知
python·自动化·pyqt
高洁014 小时前
数字孪生与计算机视觉:让机器看懂现场
人工智能·python·机器学习·transformer·迁移学习
ComPDFKit4 小时前
RAG 回答只显示 PDF 文件名?用 Python 保留引用页码
开发语言·python·pdf·rag
yz_aiks6 小时前
Termux 使用教程:把安卓手机变成随身 Linux(装 Python、SSH 远程、跑自动化脚本)
android·linux·python·智能手机·ssh·termux
2601_967980118 小时前
Flask Web 应用
python·flask·web开发·环境变量·虚拟环境
aifannihao8 小时前
ß 大写变 SS:用 Python 算清 Ruby2Shell 的缓冲区账本
python·安全·标准库·缓冲区溢出·字符串编码
liwulin05068 小时前
【OLLAMA】调用SKILL的通用ReAct
python
郝学胜-神的一滴9 小时前
Effective Python 条款36:优雅拼装迭代器与生成器
开发语言·数据结构·人工智能·python·numpy·scikit-learn
Norna10 小时前
Git 给路径加了引号,这个文件就从 review 里消失了
python
wuyk55511 小时前
Python办公自动化-第04讲-批量新建Excel
开发语言·python·excel