记录几个slam 常用的PYTHON脚本

最新推荐文章于 2024-05-10 11:40:47 发布

2401_84584535

最新推荐文章于 2024-05-10 11:40:47 发布

阅读量228

点赞数 5

分类专栏：程序员文章标签： python 开发语言

本文链接：https://blog.csdn.net/2401_84584535/article/details/138327970

版权

本文分享了SLAM（Simultaneous Localization and Mapping）领域使用Python进行开发的相关资源，包括学习路线、开发工具、视频教程、实战案例和面试题，帮助读者系统地学习和实践SLAM技术。

摘要由CSDN通过智能技术生成

#coding:utf-8
import numpy as np

def euler_to_rotation_matrix(roll, pitch, yaw):
    # 将欧拉角转换为弧度
    roll = np.radians(roll)
    pitch = np.radians(pitch)
    yaw = np.radians(yaw)

    # 计算旋转矩阵的元素
    cos_roll = np.cos(roll)
    sin_roll = np.sin(roll)
    cos_pitch = np.cos(pitch)
    sin_pitch = np.sin(pitch)
    cos_yaw = np.cos(yaw)
    sin_yaw = np.sin(yaw)

    # 构建旋转矩阵
    rotation_matrix = np.array([
        [cos_pitch * cos_yaw, cos_yaw * sin_pitch * sin_roll - cos_roll * sin_yaw, cos_roll * cos_yaw * sin_pitch + sin_roll * sin_yaw],
        [cos_pitch * sin_yaw, cos_roll * cos_yaw + sin_pitch * sin_roll * sin_yaw, -cos_yaw * sin_roll + cos_roll * sin_pitch * sin_yaw],
        [-sin_pitch, cos_pitch * sin_roll, cos_pitch * cos_roll]
    ])

    return rotation_matrix

# 示例使用
roll =  0.314218  # 沿着 x 轴旋转的角度
pitch = 0.1246  # 沿着 y 轴旋转的角度
yaw =  -177.198  # 沿着 z 轴旋转的角度

rotation_matrix = euler_to_rot