用旋转矩阵和平移向量求tum格式下的轨迹

小海盗haner

于 2021-07-21 19:12:56 发布

阅读量555

点赞数

本文链接：https://blog.youkuaiyun.com/haner27/article/details/118973842

版权

本文详细介绍了如何使用四元数和旋转矩阵将TUM格式的轨迹进行旋转和平移变换，包括从旋转矩阵到四元数，逆操作，以及在kitum和gt格式之间的转换步骤。通过实例代码演示了旋转前后的矩阵处理，适合理解并实践轨迹数据处理的工程师阅读。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

用旋转矩阵和平移向量求tum格式下的轨迹

以下代码都是自己查找的函数
我用的是第二部分，第一部分没有进行验证
如果有问题还望指出，共同进步，谢谢！

已知：旋转矩阵R和平移向量T

已知：旋转前的矩阵kitum（tum标准格式）
求：旋转后的gt（tum格式）
请添加图片描述

q =dcm2quat(R);   %旋转矩阵转四元数q
q_inv=quatinv(q);   %q求逆

gt(1:n,1)=kitum(1:n,1);

for i=1:n
    temp=quatmultiply(kitum(i,5:8)，q) ;  %四元数乘法
    gt(i,5:8) =quatmultiply(temp,q_inv);
    gt(i,2:4)=(R*(kitti00tum(i,2:4)'+T')';    
end

已知旋转后的矩阵kitum（tum标准格式）
求初始矩阵gt（tum格式）

注意！这里的R、T是gt转kitum的矩阵！是反着的

q =dcm2quat(R);   %旋转矩阵转四元数q
q_inv=quatinv(q);   %q求逆

gt(1:n,1)=kitum(1:n,1);

for i=1:n
    temp=quatmultiply(q_inv,kitum(i,5:8)) ;  %四元数乘法
    gt(i,5:8) =quatmultiply(temp,q);
    gt(i,2:4)=(inv(R)*(kitti00tum(i,2:4)'-T'))';    
end