四元数转旋转矩阵

目录

[gsplat 四元数转旋转矩阵等同代码实现](#gsplat 四元数转旋转矩阵等同代码实现)

[scipy 四元数转旋转矩阵替换代码](#scipy 四元数转旋转矩阵替换代码)


gsplat 四元数转旋转矩阵等同代码实现

python 复制代码
import torch
import torch.nn.functional as F


def quat_act(x: torch.Tensor) -> torch.Tensor:
    return x / x.norm(dim=-1, keepdim=True)

def normalized_quat_to_rotmat(quat: torch.Tensor) -> torch.Tensor:
    # 源码来自: from gsplat.utils import normalized_quat_to_rotmat
    """Convert normalized quaternion to rotation matrix.

    Args:
        quat: Normalized quaternion in wxyz convension. (..., 4)

    Returns:
        Rotation matrix (..., 3, 3)
    """
    assert quat.shape[-1] == 4, quat.shape
    w, x, y, z = torch.unbind(quat, dim=-1)
    mat = torch.stack(
        [
            1 - 2 * (y**2 + z**2),
            2 * (x * y - w * z),
            2 * (x * z + w * y),
            2 * (x * y + w * z),
            1 - 2 * (x**2 + z**2),
            2 * (y * z - w * x),
            2 * (x * z - w * y),
            2 * (y * z + w * x),
            1 - 2 * (x**2 + y**2),
        ],
        dim=-1,
    )
    return mat.reshape(quat.shape[:-1] + (3, 3))

def quat2mat(quat):
    qw, qx, qy, qz = torch.unbind(quat, dim=-1)  # 原为wxyz
    quat_xyzw = torch.stack([qx, qy, qz, qw], dim=-1)  # 转为xyzw顺序
    # 后续代码保持原逻辑
    qx, qy, qz, qw = torch.unbind(quat_xyzw, dim=-1)

    # 计算旋转矩阵
    R00 = 1 - 2 * (qy ** 2 + qz ** 2)
    R01 = 2 * (qx * qy - qw * qz)
    R02 = 2 * (qx * qz + qw * qy)

    R10 = 2 * (qx * qy + qw * qz)
    R11 = 1 - 2 * (qx ** 2 + qz ** 2)
    R12 = 2 * (qy * qz - qw * qx)

    R20 = 2 * (qx * qz - qw * qy)
    R21 = 2 * (qy * qz + qw * qx)
    R22 = 1 - 2 * (qx ** 2 + qy ** 2)

    # 将旋转矩阵堆叠在一起
    matrix = torch.stack([R00, R01, R02, R10, R11, R12, R20, R21, R22], dim=-1)

    # 变换为 3x3 的矩阵
    return matrix.view(-1, 3, 3)

x=torch.range(0,3*4-1)

x=x.reshape(-1,4)

print(x)

# 调用 quat_act 函数进行归一化
normalized_x = quat_act(x)

aa=F.normalize(x, dim=-1)

print('diff',(normalized_x-aa).sum(dim=-1))
print("\nNormalized x:")
print(aa)  # 应该返回一个全为 1 的张量


if 1:
    mat= normalized_quat_to_rotmat(aa)
    print(mat)

    mat2=quat2mat(aa)

    print('diff2', (mat2 - mat).sum(dim=-1))

scipy 四元数转旋转矩阵替换代码

python 复制代码
import torch
from scipy.spatial.transform import Rotation as R
import torch.nn.functional as F
def quat2mat_scipy(quat):
    # 从四元数中提取 qx, qy, qz, qw
    qx, qy, qz, qw = torch.unbind(quat, dim=-1)

    # 计算旋转矩阵
    R00 = 1 - 2 * (qy ** 2 + qz ** 2)
    R01 = 2 * (qx * qy - qw * qz)
    R02 = 2 * (qx * qz + qw * qy)

    R10 = 2 * (qx * qy + qw * qz)
    R11 = 1 - 2 * (qx ** 2 + qz ** 2)
    R12 = 2 * (qy * qz - qw * qx)

    R20 = 2 * (qx * qz - qw * qy)
    R21 = 2 * (qy * qz + qw * qx)
    R22 = 1 - 2 * (qx ** 2 + qy ** 2)

    # 将旋转矩阵堆叠在一起
    matrix = torch.stack([R00, R01, R02, R10, R11, R12, R20, R21, R22], dim=-1)

    # 变换为 3x3 的矩阵
    return matrix.view(-1, 3, 3)

if 1:
    x = torch.range(0, 3 * 4 - 1)

    x = x.reshape(-1, 4)

    aa = F.normalize(x, dim=-1)
    r = R.from_quat(aa.numpy())
    mat3= r.as_matrix()

    mat4=quat2mat_scipy(aa)
    print('diff3', (mat4.numpy() - mat3).sum(axis=-1))
相关推荐
lingling00927 分钟前
艾利特机器人:光伏机器人如何重塑清洁能源制造新格局
大数据·人工智能·算法
新讯网27 分钟前
智合新材携先进陶瓷材料亮相2025西安先进制造及国防科技产业博览会
大数据·人工智能·制造
Monkey的自我迭代28 分钟前
python线性回归:从原理到实战应用
开发语言·python·机器学习
机器之心1 小时前
刚刚,OpenAI推出学习模式,AI教师真来了,系统提示词已泄露
人工智能
柠檬味拥抱1 小时前
AIOps在IT运维中的应用研究基于Isolation Forest的异常检测模型实证
人工智能
2202_756749691 小时前
05 OpenCV--图像预处理之图像轮廓、直方图均衡化、模板匹配、霍夫变化、图像亮度变化、形态学变化
图像处理·人工智能·python·opencv·计算机视觉
学Linux的语莫1 小时前
八大神经网络的区别
人工智能·深度学习·神经网络
码蜂工社AI智能体1 小时前
Coze开发平台开源了!超详细本地部署教程,解锁AI智能体开发无限可能
人工智能
柠檬味拥抱1 小时前
基于YOLOv8的有无戴安全帽检测识别项目|完整源码数据集+PyQt5界面+完整训练流程+开箱即用!
人工智能
吕永强1 小时前
人工智能与城市:城市生活的集成智能
人工智能·科普