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: #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())
