四元数与位姿矩阵的相互转换

最新推荐文章于 2022-09-19 16:34:44 发布
最新推荐文章于 2022-09-19 16:34:44 发布
阅读量1.7k 收藏 5
点赞数 1
文章标签： python
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/a13956621590/article/details/124406452
版权

1. 位姿矩阵转四元数

#coding:utf-8
#将位姿图由4*4矩阵转化为四元数表示
import numpy as np
import math
from pyquaternion import Quaternion

#读入loop矩阵       all_loop_matrix  inter_true_loop_matrix
########################################################
pose_txt = open("all_loop_matrix.txt","r") 
data = pose_txt.readlines() 
save_pose=data[:]     #存loop矩阵
number= data[:]        #存loop数字 如1 10

j=0
for i in save_pose:            #清空   save_pose 和  number
    i=[]
    save_pose[j]=i
    number[j] = i
    j=j+1

num=0
line_num=0
for i in data:
    if line_num%5==0:
        num=num+1
        data10,data11,data12=str(i).split() 
        print(number[num-1])
        number[num-1]=[]
        number[num-1].append(int(data10))
        number[num-1].append(int(data11)) 
    else:
        data0,data1,data2,data3=str(i).split()   #type of data0 is 'str'
        save_pose[num-1].append(float(data0))
        save_pose[num-1].append(float(data1))
        save_pose[num-1].append(float(data2))
        save_pose[num-1].append(float(data3))
    line_num=line_num+1
##########################################################
#写入顶点信息

#robot1 顶点数  robot2顶点数
vextex_1=169  ;vextex_2=324   # 从0开始  0-169  170-324
x=0 ; z=0
############写入0.g2o  f1
f1=open("0.g2o","w")
for  i in range(vextex_1+1):
    f1.write("VERTEX_SE3:QUAT "+str(i+6989586621679009792)+" "+str(x)+" 0 "+str(z)+ " 0 0 0 1\n")
    x=x+0.5
    z=z+0.5

trans=np.array([[1,0,0,1],[0.0,0.0,1.0, 0.0],[0.0,-1.0,0.0, 1.0], [0.0,0.0,0.0, 1.0]])
rotate=trans[0:3,0:3] 
RM = np.array(rotate)
q = Quaternion(matrix=RM)
x=170 ; z=170
############写入1.g2o  f2
f2=open("1.g2o","w")
for  i in range((vextex_1+1)-(vextex_1+1),(vextex_2+1)-(vextex_1+1),1):
    f2.write("VERTEX_SE3:QUAT "+str(i+7061644215716937728)+" "+str(x)+" 0 "+str(z)+" "+str(q.x)+" "+str(q.y)+" "+str(q.z)+" "+str(q.w)+" \n")
    x=x+0.5
    z=z+0.5
###########################################################
#写入odometry的位姿信息
#robot1 odmometry数 robot2 odometry数
#向0.g2o写入odometry
for  i in range(vextex_1+1):       #0.5 0 0.5 0 0 0 1            0.01 0 0.01
    if i == vextex_1:
        continue
    f1.write("EDGE_SE3:QUAT "+str(i+6989586621679009792)+" "+str(i+1+6989586621679009792)+" "+"0.00 0 0.00 0 0 0 1   100.000000 0.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000\n")
for  i in range((vextex_1+1)-(vextex_1+1),vextex_2-(vextex_1+1),1):    #0.5 -0.5 0 0 0 0 1         0.01 -0.01 0
    f2.write("EDGE_SE3:QUAT "+str(i+7061644215716937728)+" "+str(i+1+7061644215716937728)+" "+"0 0.00 0 0 0 0 1   100.000000 0.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000 0.000000   100.000000 0.000000 0.000000 0.000000   10000.000000 0.000000 0.000000   10000.000000 0.000000   10000.000000\n")


##########################################################
#写入loop四元数格式的位姿信息
trans=np.array([[0,0,0,1],[0.0,0.0,0.0, 1.0],[0.0,0.0,0.0, 1.0], [0.0,0.0,0.0, 1.0]])
first=0
for i in save_pose:
    if i==[]:
        break
    trans[0][0]=save_pose[first][0] ;trans[0][1]=save_pose[first][1] ;trans[0][2]=save_pose[first][2];    trans[0][3]=save_pose[first][3]
    trans[1][0]=save_pose[first][4];trans[1][1]=save_pose[first][5]; trans[1][2]=save_pose[first][6];    trans[1][3]=save_pose[first][7]
    trans[2][0]=save_pose[first][8];trans[2][1]=save_pose[first][9]; trans[2][2]=save_pose[first][10];   trans[2][3]=save_pose[first][11]
    rotate=trans[0:3,0:3]   
    translate=trans[0:3,3]
    RM = np.array(rotate)
    print(number[first]) # loop 序号
    print(translate)   #loop的位置
    print(f"x: {q.x}, y: {q.y}, z: {q.z}, w: {q.w}")    #loop的四元数
    # 旋转矩阵转换为四元数
    q = Quaternion(matrix=RM)
    if number[first][1]<vextex_1+1:    #robot1的intra-loop 写入f1  0.g2o
        f1.write("EDGE_SE3:QUAT "+str(number[first][0]+6989586621679009792)+" "+str(number[first][1]+6989586621679009792)+" "+str(translate[0])+" "+str(translate[1])+" "+str(translate[2])+" "+ str(q.x)+" "+str(q.y)+" "+str(q.z)+" "+str(q.w)+" 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000\n")
    if number[first][0]>vextex_1:   #robot2的intra-loop 写入f2 1.g2o
        f2.write("EDGE_SE3:QUAT "+str(number[first][0]+7061644215716937728-(vextex_1+1))+" "+str(number[first][1]+7061644215716937728-(vextex_1+1))+" "+str(translate[0])+" "+str(translate[1])+" "+str(translate[2])+" "+ str(q.x)+" "+str(q.y)+" "+str(q.z)+" "+str(q.w)+" 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000\n")
    if number[first][0]<(vextex_1+1) and number[first][1]>vextex_1:   #robot1-2的inter-loop 0.g2o和1.g2o都写入
        f1.write("EDGE_SE3:QUAT "+str(number[first][0]+6989586621679009792)+" "+str(number[first][1]+7061644215716937728-(vextex_1+1))+" "+str(translate[0])+" "+str(translate[1])+" "+str(translate[2])+" "+ str(q.x)+" "+str(q.y)+" "+str(q.z)+" "+str(q.w)+" 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000\n")
        f2.write("EDGE_SE3:QUAT "+str(number[first][0]+6989586621679009792)+" "+str(number[first][1]+7061644215716937728-(vextex_1+1))+" "+str(translate[0])+" "+str(translate[1])+" "+str(translate[2])+" "+ str(q.x)+" "+str(q.y)+" "+str(q.z)+" "+str(q.w)+" 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000\n")
    first=first+1

f1.close()
f2.close()

2. 四元数转位姿矩阵

#coding:utf-8
#代码用途： 将fullGraph_optimized.g2o中最终的位姿由四元数表示转化为4*4矩阵并输出， 以进行下一步的误差分析
import numpy as np
import math
from pyquaternion import Quaternion
from scipy.spatial.transform import Rotation as R

#读入fullGraph_optimized.g2o 读其中的position+四元数位姿，转化为4*4矩阵形式
########################################################
vertex_total = 325      #总顶点数

pose_txt = open("fullGraph_optimized.g2o","r")      #PCM输出的最终位姿文件(四元数形式)
f1=open('pose_final.txt','w')       #用于误差分析的位姿文件(矩阵形式)

#初始化 position和q
position=[0, 0, 0]
q=[0,0,0,1]
for  i in range(vertex_total):
    input=pose_txt.readline()
    name, num, position[0], position[1], position[2], q[0], q[1], q[2], q[3] = input.split("\n")[0].split(' ')  #['VERTEX_SE3:QUAT', '7061644215716937882', '24.2379', '-5.55041', '-154.986', '-0.604195', '-0.0597147', '0.256727', '0.75198']
    quaternion = R.from_quat(q)
    Rotation3_3=quaternion.as_matrix()    #得到的旋转矩阵3*3 Rotation3_3          位置向量3*1存储在position中

    f1.write(str(i)+" "+str(i)+" "+str(i+1)+"\n")
    f1.write(str(Rotation3_3[0][0])+" "+str(Rotation3_3[0][1])+" "+str(Rotation3_3[0][2])+" "+position[0]+"\n")
    f1.write(str(Rotation3_3[1][0])+" "+str(Rotation3_3[1][1])+" "+str(Rotation3_3[1][2])+" "+position[1]+"\n")
    f1.write(str(Rotation3_3[2][0])+" "+str(Rotation3_3[2][1])+" "+str(Rotation3_3[2][2])+" "+position[2]+"\n")
    f1.write("0 0 0 1\n")
    # print("原：",q," 现：", R.from_matrix(Rotation3_3).as_quat())

木key哇啦
关注
  • 1
    点赞
  • 踩
  • 5
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
学习四元数与姿态转换矩阵思考(1)
qq_37318438的博客
03-13 2335
学习四元数与姿态矩阵之间关系的时候，想到一个问题：为什么旋转轴的四元数实部不为零，而变换前后的两个坐标向量却是实部为零的两个四元数？即四元数的实部代表什么含义。 首先，动坐标系相对参考坐标系的方位可以等效为动坐标系绕某一固定轴 u 转动角度 θ。四元数可用来表示刚体的定点转动： q0是实部，R 是参考系，b 是动坐标系 。将向量 r^R 和 r^b 看做实部为 0 的四元数，其间的
matlab和python里实现旋转矩阵到四元数的变换
qq_31442381的博客
04-15 6545
在matlab和python中将旋转矩阵变换为四元数形式，但结果不同 四元数的形式代码实现1.matlab实现2.python实现总结 四元数的形式 首先需要知道四元数有两种形式，Hamilton 和 JPL 四元数的区别。 参考博客 Quaternion（四元数）是一种三维空间旋转的表示方法，四元数由一个实部和三个虚部构成： q=q0+q1i+q2j+q3k, q=q_0+q_1i+q_2j+q...
四元数和旋转矩阵相互转化的算法实现
11-27
讲述四元数和旋转矩阵之间相互转化的学术论文：首先介绍了四元数和旋转矩阵在实现向量旋转中的应用,在此基础上,根据游戏开发的实际需要,讨论了四元数和旋转矩阵之间的相互转化关系。最后,重点分析了由旋转矩阵到四元数的转换算法,并按照计算机程序设计的特殊要求设计了程序。
四元素与矩阵之间相互转换
weixin_30872867的博客
07-03 647
1.矩阵转换为四元素 public static Quaternion QuaternionFromMatrix(Matrix4x4 m) { Quaternion q = new Quaternion(); q.w = Mathf.Sqrt(Mathf.Max(0, 1 + m[0, 0] + m[1, 1] ...
四元数学习之四元数和矩阵的转换
架构设计
08-17 680
四元数学习之四元数和矩阵的转换 四元数是一种可以替代矩阵和欧拉角的数学工具。他最初是由William Rowan Hamilton发现的（参考维基百科），它的最大的特点是不满足交换率。也谈一下自己对这一点的体会。在离散数学中有讲到半群、群、环和域的概念，其中环的定义是具有交换率和分配率（详情参考环的数学定义），而域的概念则是在环的基础上加上了交换率。所以说四元数无法满足域的定义，它是除法环的一...
无人机姿态表示方法及相互转换（欧拉角、方向余弦矩阵、四元数）
星光
09-16 1万+
常用的姿态表示方法有欧拉角、方向余弦矩阵、四元素这几种 欧拉角表示方法采用来表示飞行器的姿态，其中为滚转角，为俯仰角和为航向角，表示飞行器首先航向偏转角度，再俯仰角度，然后机体滚转角度得到的姿态 ...
四元数，欧拉角和旋转矩阵相互转换
baidu_41706898的博客
09-19 1366
c++ 四元数、欧拉角和旋转矩阵相互转化
相机投影变换(位姿)
colorsky100的博客
04-18 5401
相机投影变换(位姿).md 针对SLAM新手中常见的对各种变换的迷惑，我这里答疑一下。 针孔相机模型 [外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-cPyc6VkK-1587194378847)(:/8acde09162b7499bb062e947abb2db78)] 其中: MextM_{ext}Mext​是等距变换，也就是欧式刚体变换 MprojM_{pro...
四元数、欧拉角及方向余弦矩阵的相互转换公式
stdEnable
02-11 3万+
四元数、欧拉角及方向余弦矩阵的相互转换公式一、欧拉角转四元数（常用来初始化四元数） 按Z-Y-X的旋转变换顺序有：二、四元数与旋转矩阵（常用来作坐标变换） 1.b系到R系的坐标变换矩阵 2.R系至b系的坐标变换矩阵公式 详情:http://blog.csdn.net/u012763833/article/details/52583350三、欧拉角转方向余弦矩阵 由以上两式可得，经
从四元数变换到四乘四位姿矩阵的程序源代码
07-01
编程实现了用四元数表示的位姿矩阵，将其变换到四行四列的位姿矩阵。
四元数法计算两个物体之间的相对位姿参数
07-03
四元素代码，基于MATLAB，计算两个物体相对姿态的四元素参数
PoseNet深度网络进行6D位姿估计的PyTorch实现推理权重
06-11
7. 后处理：由于模型可能输出的是旋转矩阵或四元数，需要进行适当的后处理，例如转换为欧拉角或解码为实际的3D位置和旋转。 8. 结果评估：可以使用标准的评估指标，如平均旋转误差（Angular Error）和平均平移误差...
05-3D位姿变换案例1
08-04
在上面的代码中，我们定义了matrix2euler函数，该函数将旋转矩阵转换为欧拉角。 3. 坐标系变换： 在3D空间中，我们可以使用坐标系变换来将一个对象从一个坐标系转换到另一个坐标系。坐标系变换可以用4x4矩阵表示，...
06第六课 手眼标定之机器人位姿设置.zip
10-24
在手眼标定中，我们需要找到一个转换矩阵，将摄像头坐标系下的点转换到机器人坐标系下。这通常涉及到旋转和平移矩阵的组合，即四元数或欧拉角表示的旋转，以及向量表示的平移。标定过程可能包括单应性矩阵的计算、...
transform_calculator:用于转换矩阵的基本计算器，以及用于机器人应用程序的更多计算器
04-19
3. **欧拉角和四元数转换**：除了转换矩阵，`transform_calculator`还可能支持欧拉角（Euler angles）和四元数（quaternions）之间的转换。欧拉角直观易懂，而四元数则在防止万向节死锁（gimbal lock）方面有优势。 ...
四元数和旋转矩阵
05-20 2万+
Quaternion(四元数) Quaternion 的定义 四元数一般定义如下：     q=w+xi+yj+zk 其中 w,x,y,z是实数。同时，有:     i*i=-1     j*j=-1     k*k=-1 四元数也可以表示为：     q=[w,v] 其中v=(x,y,z)是矢量，w是标量，虽然v是矢量，但不能简单的理解为3D空间的矢量，它是4维空间中的的
如何利用旋转矩阵得到四元数
tina的博客
04-29 5976
如何将给定的旋转矩阵转换为四元数？function q = vgg_quat_from_rotation_matrix( R ) % vgg_quat_from_rotation_matrix Generates quaternion from rotation matrix % q = vgg_quat_from_rotation_matrix(R)q = [ (1 +
刚体在三维空间的旋转(关于旋转矩阵、DCM、旋转向量、四元数、欧拉角)
MulinB的技术博客
04-24 5万+
三维空间的旋转(3D Rotation)是一个很神奇的东东：如果对某个刚体在三维空间进行任意次的旋转，只要旋转中心保持不变，无论多少次的旋转都可以合为绕三维空间中某一个轴的一次旋转来表示。表示三维空间的旋转有多种互相等价的方式，常见的有旋转矩阵、DCM、旋转向量、四元数、欧拉角等。本篇文章主要梳理一下这些表示方式及相互转换的方法。
彻底搞懂四元数
热门推荐
爱冒险的技术宅
09-03 10万+
提要旋转的表达方式有很多种，有欧拉角，旋转矩阵，轴角，四元素等等，今天要学习的就是游戏开发中最常用的四元素。从欧拉角和轴向角到四元数在讲四元素之前，我们先来看下简单的欧拉角和轴向角。欧拉角使用最简单的x,y,z值来分别表示在x，y，z轴上的旋转角度，其取值为0-360(或者0-2pi），一般使用roll，pitch，yaw来表示这些分量的旋转值。需要注意的是，这里的旋转是针对世界坐标系说的，这意味
列写位置描述、姿态描述和位姿描述的矩阵表达式？
最新发布
09-04
### 回答1： 位置描述：位置描述是指物体在三维空间中的位置，通常用一个三维向量表示。假设 $\vec{p}$ 表示物体的位置，则其矩阵表达式为： $$ \vec{p} = \begin{bmatrix} p_x \\ p_y \\ p_z \end{bmatrix} $$ 姿态描述：姿态描述是指物体在三维空间中的朝向，通常用旋转矩阵或四元数表示。假设 $R$ 表示物体的旋转矩阵，则其矩阵表达式为： $$ R = \begin{bmatrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{bmatrix} $$ 位姿描述：位姿描述是指物体在三维空间中的位置和朝向，通常用一个四元数或一个由位置向量和旋转矩阵组成的变换矩阵表示。假设 $T$ 表示物体的变换矩阵，则其矩阵表达式为： $$ T = \begin{bmatrix} r_{11} & r_{12} & r_{13} & p_x \\ r_{21} & r_{22} & r_{23} & p_y \\ r_{31} & r_{32} & r_{33} & p_z \\ 0 & 0 & 0 & 1 \end{bmatrix} $$ 其中 $r_{ij}$ 表示旋转矩阵中的元素，$p_x$、$p_y$、$p_z$ 表示位置向量中的元素。 ### 回答2： 位置描述、姿态描述和位姿描述都是对物体在空间中的位置和姿态的描述方式。 位置描述通常使用位置向量来表示，它表示物体的三维坐标。位置描述的矩阵表达式为： P = [x, y, z] 其中P为位置向量，x、y、z分别表示物体在x轴、y轴、z轴上的坐标。 姿态描述通常使用旋转矩阵来表示，它表示物体的旋转状态。姿态描述的矩阵表达式为： R = [r11, r12, r13; r21, r22, r23; r31, r32, r33] 其中R为旋转矩阵，r11、r12、r13等为矩阵元素，表示物体在三个坐标轴上的旋转角度。 位姿描述是位置描述和姿态描述的组合，可以使用齐次变换矩阵来表示，它表示物体在空间中的位置和姿态的综合描述。位姿描述的矩阵表达式为： T = [r11, r12, r13, x; r21, r22, r23, y; r31, r32, r33, z; 0, 0, 0, 1] 其中T为齐次变换矩阵，r11、r12、r13等为旋转矩阵的元素，x、y、z为位置向量的元素。这个矩阵描述了物体在三维空间中的位置和姿态信息。 ### 回答3： 位置描述、姿态描述和位姿描述都可以使用矩阵表达式来表示。 1. 位置描述矩阵：位置描述矩阵是一个3x1的矩阵，表示目标对象在三维空间中的位置坐标。通常用[x, y, z]表示，其中x、y、z分别代表目标对象在x、y、z轴上的位置。 2. 姿态描述矩阵：姿态描述矩阵是一个3x3的矩阵，表示目标对象在三维空间中的旋转情况。通常用R表示该矩阵，其每一行代表目标对象的坐标轴在全局坐标系下的方向向量。例如，R = [[r11, r12, r13], [r21, r22, r23], [r31, r32, r33]]，其中r11、r12、r13分别代表目标对象x轴在全局坐标系下的方向向量，r21、r22、r23分别代表目标对象y轴的方向向量，r31、r32、r33分别代表目标对象z轴的方向向量。 3. 位姿描述矩阵：位姿描述矩阵是一个4x4的矩阵，结合位置描述矩阵和姿态描述矩阵来表示目标对象在空间中的整体位置和旋转情况。通常用T表示该矩阵，其左上角3x3部分为姿态描述矩阵R，右侧是位置描述矩阵的3x1部分。例如，T = [[r11, r12, r13, x], [r21, r22, r23, y], [r31, r32, r33, z], [0, 0, 0, 1]]。 这样，通过位姿描述矩阵可以直观地同时描述目标对象的位置和姿态，方便进行坐标转换和运动规划等操作。

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值