#!/usr/bin/python
# -*- coding: UTF-8 -*-
import numpy as np
import os
#==========================1坐标系转换函数API===============================
import math
a = 6378137
b = 6356752.3142
f = (a - b) / a
e_sq = f * (2-f)
pi = 3.14159265359
'''
功能： # gps转化到大地坐标系ECEF
输入：
等待转换的GPS 坐标 lat, lon, h
输出：
ecef 坐标 x, y, z
'''
def geodetic_to_ecef(lat, lon, h):
lat=float(lat)
lon=float(lon)
h=float(h)
# (lat, lon) in degrees
# h in meters
lamb = math.radians(lat)
phi = math.radians(lon)
s = math.sin(lamb)
N = a / math.sqrt(1 - e_sq * s * s)
sin_lambda = math.sin(lamb)
cos_lambda = math.cos(lamb)
sin_phi = math.sin(phi)
cos_phi = math.cos(phi)
x = (h + N) * cos_lambda * cos_phi
y = (h + N) * cos_lambda * sin_phi
z = (h + (1 - e_sq) * N) * sin_lambda
return x, y, z
'''
功能： # 大地坐标系 转化到GPS第一帧为原点的本地ENU坐标系
输入：
等待转换的ecef 坐标 x, y, z
作为原点的GPS第一帧 坐标lat0, lon0, h0
输出：
本地第一帧GPS为原点的 ENU 坐标系 xEast, yNorth, zUp
'''
def ecef_to_enu(x, y, z, lat0, lon0, h0):
x=float(x)
y=float(y)
z=float(z)
lat0=float(lat0)
lon0=float(lon0)
h0=float(h0)
lamb = math.radians(lat0)
phi = math.radians(lon0)
s = math.sin(lamb)
N = a / math.sqrt(1 - e_sq * s * s)
sin_lambda = math.sin(lamb)
cos_lambda = math.cos(lamb)
sin_phi = math.sin(phi)
cos_phi = math.cos(phi)
x0 = (h0 + N) * cos_lambda * cos_phi
y0 = (h0 + N) * cos_lambda * sin_phi
z0 = (h0 + (1 - e_sq) * N) * sin_lambda
xd = x - x0
yd = y - y0
zd = z - z0
t = -cos_phi * xd - sin_phi * yd
xEast = -sin_phi * xd + cos_phi * yd
yNorth = t * sin_lambda + cos_lambda * zd
zUp = cos_lambda * cos_phi * xd + cos_lambda * sin_phi * yd + sin_lambda * zd
return xEast, yNorth, zUp
'''
功能： enu坐标转化到ecef坐标
输入：
等待转换的ENU坐标 坐标 xEast, yNorth, zUp
GPS第一帧原点 坐标 lat0, lon0, h0
输出：
ecef 坐标 x, y, z
'''
def enu_to_ecef(xEast, yNorth, zUp, lat0, lon0, h0):
xEast=float(xEast)
yNorth=float(yNorth)
zUp=float(zUp)
lat0=float(lat0)
lon0=float(lon0)
h0=float(h0)
lamb = math.radians(lat0)
phi = math.radians(lon0)
s = math.sin(lamb)
N = a / math.sqrt(1 - e_sq * s * s)
sin_lambda = math.sin(lamb)
cos_lambda = math.cos(lamb)
sin_phi = math.sin(phi)
cos_phi = math.cos(phi)
x0 = (h0 + N) * cos_lambda * cos_phi
y0 = (h0 + N) * cos_lambda * sin_phi
z0 = (h0 + (1 - e_sq) * N) * sin_lambda
t = cos_lambda * zUp - sin_lambda * yNorth
zd = sin_lambda * zUp + cos_lambda * yNorth
xd = cos_phi * t - sin_phi * xEast
yd = sin_phi * t + cos_phi * xEast
x = xd + x0
y = yd + y0
z = zd + z0
return x, y, z
'''
功能： # 大地坐标系ECEF转化到gps
输入：
等待转换的ecef 坐标 x, y, z
输出：
GPS 坐标 lat, lon, h
'''
def ecef_to_geodetic(x, y, z):
x=float(x)
y=float(y)
z=float(z)
# Convert from ECEF cartesian coordinates to
# latitude, longitude and height. WGS-84
x2 = x ** 2
y2 = y ** 2
z2 = z ** 2
a = 6378137.0000 # earth radius in meters
b = 6356752.3142 # earth semiminor in meters
e = math.sqrt (1-(b/a)**2)
b2 = b*b
e2 = e ** 2
ep = e*(a/b)
r = math.sqrt(x2+y2)
r2 = r*r
E2 = a ** 2 - b ** 2
F = 54*b2*z2
G = r2 + (1-e2)*z2 - e2*E2
c = (e2*e2*F*r2)/(G*G*G)
s = ( 1 + c + math.sqrt(c*c + 2*c) )**(1/3)
P = F / (3 * (s+1/s+1)**2 * G*G)
Q = math.sqrt(1+2*e2*e2*P)
ro = -(P*e2*r)/(1+Q) + math.sqrt((a*a/2)*(1+1/Q) - (P*(1-e2)*z2)/(Q*(1+Q)) - P*r2/2)
tmp = (r - e2*ro) ** 2
U = math.sqrt( tmp + z2 )
V = math.sqrt( tmp + (1-e2)*z2 )
zo = (b2*z)/(a*V)
height = U*( 1 - b2/(a*V) )
lat = math.atan( (z + ep*ep*zo)/r )
temp = math.atan(y/x)
if x >=0 :
long = temp
elif (x < 0) & (y >= 0):
long = pi + temp
else :
long = temp - pi
lat0 = lat/(pi/180)
lon0 = long/(pi/180)
h0 = height
return lat0, lon0, h0
'''
功能： # gps直接转化到enu坐标系
相对于指定GPS_ref为原点（一般都是第一帧）的enu坐标系
输入：
等待转换的GPS 坐标 lat, lon, h
参考原点GPS 坐标 lat_ref, lon_ref, h_ref
输出：
enu坐标 x, y, z
'''
def geodetic_to_enu(lat, lon, h, lat_ref, lon_ref, h_ref):
lat=float(lat)
lon=float(lon)
h=float(h)
lat_ref=float(lat_ref)
lon_ref=float(lon_ref)
h_ref=float(h_ref)
x, y, z = geodetic_to_ecef(lat, lon, h)
return ecef_to_enu(x, y, z, lat_ref, lon_ref, h_ref)
'''
功能： # enu直接转化到gps坐标系
相对于指定GPS_ref为原点（一般都是第一帧）的enu坐标系
输入：
等待转换的enu 坐标 xEast, yNorth, zUp
参考原点GPS 坐标 lat_ref, lon_ref, h_ref
输出：
GPS 坐标 lat, lon, h
'''
def enu_to_geodetic(xEast, yNorth, zUp, lat_ref, lon_ref, h_ref):
xEast=float(xEast)
yNorth=float(yNorth)
zUp=float(zUp)
lat_ref=float(lat_ref)
lon_ref=float(lon_ref)
h_ref=float(h_ref)
x,y,z = enu_to_ecef(xEast, yNorth, zUp, lat_ref, lon_ref, h_ref)
return ecef_to_geodetic(x,y,z)
'''
#功能： 计算两个GPS之间的距离
输入
gps坐标 lat1,lng1,high1
gps坐标 lat2,lng2,high2
输出
直角坐标系下的 distance=sqrt(x^2+y^2+z^2)
'''
def geodistance(lat1,lng1,high1,lat2,lng2,high2):
lat1=float(lat1)
lng1=float(lng1)
high1=float(high1)
lat2=float(lat2)
lng2=float(lng2)
high2=float(high2)
xyz1=geodetic_to_ecef(lat1,lng1,high1)#转化为ecef坐标系
xyz2=geodetic_to_ecef(lat2,lng2,high2)#转化为ecef坐标系
#distance=sqrt((xyz1[0]-xyz2[0])**2+(xyz1[1]-xyz2[1])**2+(xyz1[2]-xyz2[2])**2)
distance=sqrt((xyz1[0]-xyz2[0])**2+(xyz1[1]-xyz2[1])**2)
#lng1,lat1,lng2,lat2 = (120.12802999999997,30.28708,115.86572000000001,28.7427)
#lng1, lat1, lng2, lat2 = map(radians, [float(lng1), float(lat1), float(lng2), float(lat2)]) # 经纬度转换成弧度
#dlon=lng2-lng1
#dlat=lat2-lat1
#a=sin(dlat/2)**2 + cos(lat1) * cos(lat2) * sin(dlon/2)**2
#distance=2*asin(sqrt(a))*6378137.0 # 地球平均半径，6371km C_EARTH=6378137.0
#distance=sqrt(distance*distance+(high2-high1)*(high2-high1))
distance=round(distance,3)
return distance
#=======================2ENU时间戳XYZ读取=============================
#函数功能 从TXT读取ENU
"""
输入 path_txt txt文件名字
数据格式 时间戳 x y z
497.12283301353455', '34.0344212093', '108.756020573', '400.164
输出
positionList x y z
"""
def readtxt_ENU(path_txt,fengefu=" "):
f = open(path_txt, mode='r+', encoding='utf-8') # 打开txt文件，以‘utf-8’编码读取
lines = f.readlines() # 以行的形式进行读取文件
positionList=[]
list_x=[]
list_y=[]
list_z=[]
for line in lines:
lines=line.strip().split(fengefu) # x.strip()#除去每行的换行符 按照：分割
#print(lines)
positionList_i=[0,0,0]
timesharp = lines[0:1]
timesharp="".join(timesharp).strip()
ENU_X_N = lines[1:2] # list--str
ENU_X_N="".join(ENU_X_N).strip()
ENU_Y_E = lines[2:3] # list--str
ENU_Y_E="".join(ENU_Y_E).strip()
ENU_Z_U = lines[3:4] # list--str
ENU_Z_U="".join(ENU_Z_U).strip()
#positionList_i[0]=float(timesharp)
positionList_i[0]=float(ENU_X_N)
positionList_i[1]=float(ENU_Y_E)
positionList_i[2]=float(ENU_Z_U)
positionList.append(positionList_i)
list_x.append(float(ENU_X_N))
list_y.append(float(ENU_Y_E))
list_z.append(float(ENU_Z_U))
#是否排序
#gps_list_1t2lon3lathigh = sorted(positionList,key=lambda x: float(x[0]))#以时间戳排序
#print(gps_list_1t2lon3lathigh)
f.close()
return positionList,list_x,list_y,list_z
#函数功能 从TXT读取ENU 各轴的误差
"""
输入 path_txt txt文件名字
数据格式 时间戳 x y z error_x error_y error_z
497.12283301353455', '34.0344212093', '108.756020573', '400.164 3.1 12 3.4
输出
positionList error_x error_y error_z
"""
def readtxt_ENU_Error(path_txt,fengefu=" "):
f = open(path_txt, mode='r+', encoding='utf-8') # 打开txt文件，以‘utf-8’编码读取
lines = f.readlines() # 以行的形式进行读取文件
positionList=[]
list_x=[]
list_y=[]
list_z=[]
for line in lines:
lines=line.strip().split(fengefu) # x.strip()#除去每行的换行符 按照：分割
#print(lines)
positionList_i=[0,0,0]
timesharp = lines[0:1]
timesharp="".join(timesharp).strip()
ENU_X_N = lines[4:5] # list--str
ENU_X_N="".join(ENU_X_N).strip()
ENU_Y_E = lines[5:6] # list--str
ENU_Y_E="".join(ENU_Y_E).strip()
ENU_Z_U = lines[6:7] # list--str
ENU_Z_U="".join(ENU_Z_U).strip()
try:
#positionList_i[0]=float(timesharp)
positionList_i[0]=float(ENU_X_N)
positionList_i[1]=float(ENU_Y_E)
positionList_i[2]=float(ENU_Z_U)
positionList.append(positionList_i)
list_x.append(float(ENU_X_N))
list_y.append(float(ENU_Y_E))
list_z.append(float(ENU_Z_U))
except ValueError :
print("转化失败",i)
#是否排序
#gps_list_1t2lon3lathigh = sorted(positionList,key=lambda x: float(x[0]))#以时间戳排序
#print(gps_list_1t2lon3lathigh)
f.close()
return positionList,list_x,list_y,list_z
#=======================2GPS时间戳读取=============================
#函数名 读取txt中指定位置参数内容
#函数输入:
# path_txt txt文件地址
# canshu 要从txt读取的内容
# fengefu 参数名字和值的分隔符号 默认 -
#gps数据格式
'''
gps.txt数据格式
编号 time time 状态 真实gps 经度 纬度 高度 定位gps 经度 纬度 高度
6.205903053283691 20230106172032520 1 108.75606035 34.0343725537 401.804 108.756070682 34.0343699087 401.72557778 1.14883264001 0.24373170974 0.0784222199544 -0.613050855174 -3.07510515832 62.2546435577 0.932564436131
1 6.407103061676025 20230106172032580 1 108.75606035 34.0343725537 402.142 108.756070552 34.0343699422 402.066939582 1.13442871038 0.240639960882 0.0750604180848 -0.737572900119 -3.47568977187 61.9155967908 0.937691115842
2 6.474128007888794 20230106172032621 1 -1 -1 -1 108.756070391 34.0343699572 402.139887106 0 0 0 -0.576328599814 -3.69308815477 61.8199687376 1
'''
#函数输出
# gps_list_1t2lon3lathigh 按照时间戳排序的位置列表
# 返回字符型结果
def readtxt_gps_havegps_we(path_txt,fengefu=" "):
f = open(path_txt, mode='r+', encoding='utf-8') # 打开txt文件，以‘utf-8’编码读取
lines = f.readlines() # 以行的形式进行读取文件
positionList=[]
first_line=0# 跳过第一行 有乱码
for line in lines:
if first_line==0:
first_line=1
continue
lines=line.strip().split(fengefu) # x.strip()#除去每行的换行符 按照：分割
#print(lines)
positionList_i=[0,0,0,0,0,0,0]
timesharp = lines[1:2]
timesharp="".join(timesharp).strip()
#state = lines[3:4] # list--str
#state="".join(state).strip()
real_gps_lon = lines[4:5] # list--str
real_gps_lon="".join(real_gps_lon).strip()
real_gps_lat = lines[5:6] # list--str
real_gps_lat="".join(real_gps_lat).strip()
real_gps_high = lines[6:7] # list--str
real_gps_high="".join(real_gps_high).strip()
slam_gps_lon = lines[7:8] # list--str
slam_gps_lon="".join(slam_gps_lon).strip()
slam_gps_lat = lines[8:9] # list--str
slam_gps_lat="".join(slam_gps_lat).strip()
slam_gps_high = lines[9:10] # list--str
slam_gps_high="".join(slam_gps_high).strip()
positionList_i[0]=timesharp
positionList_i[1]=slam_gps_lat
positionList_i[2]=slam_gps_lon
positionList_i[3]=slam_gps_high
positionList_i[4]=real_gps_lat
positionList_i[5]=real_gps_lon
positionList_i[6]=real_gps_high
#positionList_i[7]=state
positionList.append(positionList_i)
#是否排序
#gps_list_1t2lon3lathigh = sorted(positionList,key=lambda x: float(x[0]))#以时间戳排序
#print(gps_list_1t2lon3lathigh)
f.close()
return positionList
#从slam保存轨迹读取 时间戳是带图像名字
"""
输入 path_txt
格式
名字（时间戳） x z y
54.55198812484741.png -0 0 -0
54.7531681060791.png -0.0088182097 -0.0730422019 -0.00989559283
54.82018804550171.png -0.00635520236 -0.0877061512 -0.0110012228
54.954188108444214.png -0.00269816031 -0.118492937 -0.0137271836
"""
def readtxt_slamSaveTraceEnu(path_txt,fengefu=" "):
f = open(path_txt, mode='r+', encoding='utf-8') # 打开txt文件，以‘utf-8’编码读取
lines = f.readlines() # 以行的形式进行读取文件
positionList=[]
for line in lines:
lines=line.strip().split(fengefu) # x.strip()#除去每行的换行符 按照：分割
#print(lines)
positionList_i=[0,0,0,0]
timesharp = lines[0:1]
timesharp="".join(timesharp).strip()
timesharp=timesharp[:-4]
#timesharp.replace(".png","0")
#print(timesharp)
slam_x = lines[1:2] # list--str
slam_x="".join(slam_x).strip()
slam_y = lines[2:3] # list--str
slam_y="".join(slam_y).strip()
slam_z = lines[3:4] # list--str
slam_z="".join(slam_z).strip()
positionList_i[0]=float(timesharp)
positionList_i[1]=float(slam_x)
positionList_i[2]=float(slam_y)
positionList_i[3]=float(slam_z)
#positionList_i[7]=state
positionList.append(positionList_i)
#是否排序
#gps_list_1t2lon3lathigh = sorted(positionList,key=lambda x: float(x[0]))#以时间戳排序
#print(gps_list_1t2lon3lathigh)
f.close()
return positionList
#函数名 读取txt中指定位置参数内容
#函数输入:
# path_txt txt文件地址
# canshu 要从txt读取的内容
# fengefu 参数名字和值的分隔符号 默认 -
#gps数据格式
'''
gps.txt数据格式
编号 time time 状态 真实gps 经度 纬度 高度 定位gps 经度 纬度 高度
0.6401629447937012 34.0343737663 108.756061354 399.997
0.7071881294250488 34.0343737663 108.756061354 399.998
0.8412330150604248 34.0343737663 108.756061354 400.001
'''
#函数输出
# gps_list_1t2lon3lathigh 按照时间戳排序的位置列表
# 返回字符型结果
def readtxt_realgps(path_txt,fengefu=" "):
f = open(path_txt, mode='r+', encoding='utf-8') # 打开txt文件，以‘utf-8’编码读取
lines = f.readlines() # 以行的形式进行读取文件
positionList=[]
i=1
for line in lines:
#print(line)
lines=line.strip().split(fengefu) # x.strip()#除去每行的换行符 按照：分割
#print(lines)
positionList_i=[0,0,0,0]
timesharp = lines[0:1]
timesharp="".join(timesharp).strip()
real_gps_lat = lines[1:2] # list--str
real_gps_lat="".join(real_gps_lat).strip()
real_gps_lon = lines[2:3] # list--str
real_gps_lon="".join(real_gps_lon).strip()
real_gps_high = lines[3:4] # list--str
real_gps_high="".join(real_gps_high).strip()
if real_gps_lat == "-1" or real_gps_lon=="-1" or real_gps_high=="-1":
pass
elif real_gps_lat == " " or real_gps_lon==" " or real_gps_high==" ":
pass
else :
try:
positionList_i[0]=float(timesharp)
positionList_i[1]=float(real_gps_lat)
positionList_i[2]=float(real_gps_lon)
positionList_i[3]=float(real_gps_high)
except ValueError :
print("转化失败",i)
i=i+1
#if fixHigh>-1:
#positionList_i[3]=fixHigh
#print(positionList_i)
positionList.append(positionList_i)
#print(timesharp,real_gps_lat,real_gps_lon,real_gps_high)
#是否排序
#gps_list_1t2lon3lathigh = sorted(positionList,key=lambda x: float(x[0]))#以时间戳排序
#print(gps_list_1t2lon3lathigh)
f.close()
return positionList
# 将对其的GPS 按照图像的名字重新保存成TXT
def save_TXT_realGPS_slamGPS(real_slam_gps_time,save_name):
newgps_txtfile = open(save_name,'w')#若文件不存在，系统自动创建。'a'表示可连续写入到文件，保留原内容，在原内容之后写入。可修改该模式（'w+','w','wb'等）
for i in range(0,len(img_gps_sortlist)):
img_time_name=real_slam_gps_time[i][6]
gps_lat=(real_slam_gps_time[i][0])
gps_lon=(real_slam_gps_time[i][1])
gps_high=(real_slam_gps_time[i][2])
slam_lat=(real_slam_gps_time[i][3])
slam_lon=(real_slam_gps_time[i][4])
slam_high=(real_slam_gps_time[i][5])
msggps=str(img_time_name)+" "+str(gps_lat)+" "+str(gps_lon)+" "+str(gps_high)+"\n"
newgps_txtfile.write(msggps)
#print("写入",i,msggps)
newgps_txtfile.close()
print("txt保存完毕,总计行数",len(img_gps_sortlist))
# 根据真实gps轨迹 查找slam的gps轨迹 匹配 并计算ecef下误差m
# 输出 时间戳 真实gps 纬度 经度 高度 匹配的SLAM gps 纬度 经度 高度
# 775.079558134079 7747 10801 [775.079558134079, '34.0343986246', '108.756028006', '396.896', '34.0343943644', '108.75602437', '396.817510377']
#
def from_realGPSFindslamGPS(realGPSlist,slamGPSlist):
real_slam_list=[]
real_slam_list_fail_nomarl=[]#没有匹配的gps集合
real_slam_list_fail_error=[]#没有匹配的gps集合
slam_last=0
print(slamGPSlist[0][0],slamGPSlist[len(slamGPSlist)-1][0])
time_begin=float(slamGPSlist[0][0])
time_stop =float(slamGPSlist[len(slamGPSlist)-1][0])
for i in range(0,len(realGPSlist)):
realGPS_timesharp=float(realGPS_List[i][0])
realGPS_lat=realGPS_List[i][1]
realGPS_lon=realGPS_List[i][2]
realGPS_high=realGPS_List[i][3]
pipei=0
for j in range(slam_last,len(slamGPSlist)):
slamGPS_timesharp=float(slamGPSlist[j][0])
if realGPS_timesharp==slamGPS_timesharp:
slamGPS_lat =slamGPSlist[j][1]
slamGPS_lon =slamGPSlist[j][2]
slamGPS_high=slamGPSlist[j][3]
#计算单个误差
distance=geodistance(realGPS_lat,realGPS_lon,realGPS_high,slamGPS_lat,slamGPS_lon,slamGPS_high)
real_slam_list_i=[realGPS_timesharp,realGPS_lat,realGPS_lon,realGPS_high,slamGPS_lat,slamGPS_lon,slamGPS_high,distance]
real_slam_list.append(real_slam_list_i)
slam_last=j
pipei=1
#print(realGPS_timesharp,i,j,real_slam_list_i)
break
else:
pass
if pipei==0:
if realGPS_timesharp<time_begin or realGPS_timesharp>time_stop:# gps采集时间在slam外面
real_slam_list_fail_nomarl.append([realGPS_timesharp,realGPS_lat,realGPS_lon,realGPS_high,"正常失败"])
else:
real_slam_list_fail_error.append([realGPS_timesharp,realGPS_lat,realGPS_lon,realGPS_high,"异常失败"])
nomarl_fail=len(real_slam_list_fail_nomarl)
error_fail=len(real_slam_list_fail_error)
print("匹配成功数目",len(real_slam_list),"真值gps数目",len(realGPSlist),"slam定位gps数目",len(slamGPSlist),"正常失败",nomarl_fail,"异常失败",error_fail)
return real_slam_list
# 把匹配后的gps结果保存后才能txt
# time_realgps_slamgps_list 匹配的数组
# save_realgpsname 真值保存txt名字
# save_slamgpsname slam保存txt名字
# 保存格式 时间戳 经度 纬度 高度
#
def save_TXT_realGps_SLAMgps(time_realgps_slamgps_list,save_realgpsname,save_slamgpsname):
realgps_txtfile = open(save_realgpsname,'w')#若文件不存在，系统自动创建。'a'表示可连续写入到文件，保留原内容，在原内容之后写入。可修改该模式（'w+','w','wb'等）
slamgps_txtfile = open(save_slamgpsname,'w')
#time_realgps_slamgps_list=from_realGPSFindslamGPS(realGPS_List,slamGPS_List)
for i in range(0,len(time_realgps_slamgps_list)):
#print(i,time_realgps_slamgps_list[i])
timesharp=time_realgps_slamgps_list[i][0]
realGPS_lat=time_realgps_slamgps_list[i][1]
realGPS_lon=time_realgps_slamgps_list[i][2]
realGPS_high=time_realgps_slamgps_list[i][3]
slamGPS_lat=time_realgps_slamgps_list[i][4]
slamGPS_lon=time_realgps_slamgps_list[i][5]
slamGPS_high=time_realgps_slamgps_list[i][6]
#print("时间戳 真实gps 纬度 经度 高度 匹配的SLAM gps 纬度 经度 高度")
realGPS_lat_msg=str(timesharp)+" "+str(realGPS_lat)+" "+str(realGPS_lon)+" "+str(realGPS_high)+"\n"
realgps_txtfile.write(realGPS_lat_msg)
slamgps_txtfilet_msg=str(timesharp)+" "+str(slamGPS_lat)+" "+str(slamGPS_lon)+" "+str(slamGPS_high)+"\n"
slamgps_txtfile.write(slamgps_txtfilet_msg)
realgps_txtfile.close()
slamgps_txtfile.close()
print("txt保存完毕,总计行数",len(time_realgps_slamgps_list))
# 把匹配后的enu结果保存后才能txt
# time_realgps_slamgps_list 匹配的数组
# save_realgpsname 真值保存txt名字
# save_slamgpsname slam保存txt名字
# gps_ref[lat,lon,high] 参考原点
# threath 要剔除的阈值
# 保存格式 时间戳 x-n y-e z-u
#
def save_TXT_realEnu_SLAMEnu(time_realgps_slamgps_list,save_realgpsenuname,save_slamgpsenuname,gps_ref,threath=99999):
realgps_txtfile = open(save_realgpsenuname,'w')#若文件不存在，系统自动创建。'a'表示可连续写入到文件，保留原内容，在原内容之后写入。可修改该模式（'w+','w','wb'等）
slamgps_txtfile = open(save_slamgpsenuname,'w')
#time_realgps_slamgps_list=from_realGPSFindslamGPS(realGPS_List,slamGPS_List)
for i in range(0,len(time_realgps_slamgps_list)):
#print(i,time_realgps_slamgps_list[i])
timesharp=time_realgps_slamgps_list[i][0]
realGPS_lat =time_realgps_slamgps_list[i][1]
realGPS_lon =time_realgps_slamgps_list[i][2]
realGPS_high=time_realgps_slamgps_list[i][3]
xEast, yNorth, zUp=geodetic_to_enu(realGPS_lat,realGPS_lon,realGPS_high,gps_ref[0],gps_ref[1],gps_ref[2])
realGPS_lat_msg=str(timesharp)+" "+str(xEast)+" "+str(yNorth)+" "+str(zUp)+"\n"
realgps_txtfile.write(realGPS_lat_msg)
slamGPS_lat =time_realgps_slamgps_list[i][4]
slamGPS_lon =time_realgps_slamgps_list[i][5]
slamGPS_high=time_realgps_slamgps_list[i][6]
slamxEast, slamyNorth, slamzUp=geodetic_to_enu(slamGPS_lat,slamGPS_lon,slamGPS_high,gps_ref[0],gps_ref[1],gps_ref[2])
#计算各轴误差
error_xEast=slamxEast-xEast
error_yEast=slamyNorth-yNorth
error_zEast=slamzUp-zUp
#threath=10
if abs(error_xEast)>threath or abs(error_yEast)>threath or abs(error_zEast)>threath:
pass
else:
#pass
#打包信息保存一行
error_msg=str(error_xEast)+" "+ str(error_yEast)+" "+ str(error_zEast)
slamgps_txtfilet_msg=str(timesharp)+" "+str(slamxEast)+" "+str(slamyNorth)+" "+str(slamzUp)+" "+error_msg+"\n"
slamgps_txtfile.write(slamgps_txtfilet_msg)
#print("时间戳 真实gps 纬度 经度 高度 匹配的SLAM gps 纬度 经度 高度")
realgps_txtfile.close()
slamgps_txtfile.close()
print("txt保存完毕,总计行数",len(time_realgps_slamgps_list))
from math import radians, cos, sin, asin, sqrt
from pyecharts import Line3D
import math
from pyecharts import Line
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d
import numpy as np
# matplotlib画一条单独的3D轨迹
def Draw_trace1(x,y,z):
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.set_title("3D_Curve")
ax.set_xlabel("x(m)")
ax.set_ylabel("y(m)")
ax.set_zlabel("z(m)")
figure = ax.plot(x, y, z, c='r')
plt.show()
# matplotlib将多条3D轨迹画在同一个图里
def Draw3D_trace_more(tracelist):
fig = plt.figure()
ax = fig.gca(projection='3d')
#ax.set_title("3D_Curve")
ax.set_xlabel("x(m)")
ax.set_ylabel("y(m)")
ax.set_zlabel("z(m)")
ax.set_aspect("auto")#设置x,y z轴等比例
color=["blue","green","red","orange","purple","pink","yellow"]
linestyle=["-","--","-.","-"] #":"
for i in range(0,len(tracelist)):
xi_list=tracelist[i][0]
yi_list=tracelist[i][1]
zi_list=tracelist[i][2]
color_i=color[i]
figure = ax.plot(xi_list, yi_list, zi_list, c=color_i,linestyle=linestyle[0])
#plt.xlim(-1500,100)
#plt.ylim(-600,1600)
#plt.zlim(-2,15)
plt.grid()#网格线
plt.show()
#画单个2维平面轨迹
def Draw2D_trace_gpsvreal(x1,y1,x2,y2):
#绘画轨迹
print("画轨迹图")
plt.title('Error mapped onto trajectory')
plt.ylabel('Y(m)')
plt.xlabel('X(m)')
#plt.annotate('blue ', xy=(2,5), xytext=(2, 10),arrowprops=dict(facecolor='black', shrink=0.01),)
#x=[1, 2, 3, 4,3,2,1]
#y=[1, 4, 9, 16,3,5,6]
plt.plot(x1,y1,color='b',linestyle='dashed')
'''
color：线条颜色，值r表示红色（red）
marker：点的形状，值o表示点为圆圈标记（circle marker）
linestyle：线条的形状，值dashed表示用虚线连接各点
'''
#x=[3, 4, 5, 6]
#y=[1, 4, 9, 16]
#plt.plot(x2, y2, color='r',marker='o',linestyle='dashed')
plt.plot(x2, y2, color='r')
'''
axis：坐标轴范围
语法为axis[xmin, xmax, ymin, ymax]，
也就是axis[x轴最小值, x轴最大值, y轴最小值, y轴最大值]
'''
#plt.axis([0, 6, 0, 20])
plt.grid()#网格线
#plt.grid(axis='x',color = 'r', linestyle = '--', linewidth = 0.5)## 设置 y 就在轴方向显示网格线
plt.show()
#画3个对对比 2维平面轨迹
"""
输入 data_list 多个轨迹的enu列表
要画的x轴 xlabel='X(m)' "Y(m)" "Z(m)"
要画的y轴 ylabeln='X(m)' "Y(m)" "Z(m)"
"""
def Draw2D_trace_gpsvreal_list(data_list,xlabel='X(m)',ylabeln='Y(m)'):
color=["blue","green","red","orange","purple","pink","yellow"]
'''
'-' solid line style
'--' dashed line style
'-.' dash-dot line style
':' dotted line style
'''
linestyle=["-","--","-.","-"] #:
xmax=0
xmin=0
ymax=0
ymin=0
for i in range(0,len(data_list)):
linexylist=data_list[i]
linexlist=[]
lineylist=[]
#根据轴 决定画的是侧视图还是俯视图
if xlabel=="X(m)":
linexlist=linexylist[0]
elif xlabel=="Y(m)":
linexlist=linexylist[1]
elif xlabel=="Z(m)":
linexlist=linexylist[2]
if ylabeln=="X(m)":
lineylist=linexylist[0]
elif ylabeln=="Y(m)":
lineylist=linexylist[1]
elif ylabeln=="Z(m)":
lineylist=linexylist[2]
threah=0
#if xlabel=="X(m)" and ylabeln=="Y(m)":
#threah=100
# xmax=int(max(linexlist)+threah)
# xmin=int(min(linexlist)-threah)
# ymax=int(max(lineylist)+threah)
# ymin=int(min(lineylist)-threah)
#绘画轨迹
#print("画轨迹图")
#plt.title('Error mapped onto trajectory')
plt.xlabel(xlabel)
plt.ylabel(ylabeln)
#plt.annotate('blue ', xy=(2,5), xytext=(2, 10),arrowprops=dict(facecolor='black', shrink=0.01),)
#x=[1, 2, 3, 4,3,2,1]
#y=[1, 4, 9, 16,3,5,6]
color_i=color[i]
line_i=linestyle[i]
plt.plot(linexlist,lineylist,color=color_i,linestyle=line_i)
#plt.xlim(xmin,xmax)
#plt.ylim(ymin,ymax)
plt.grid()#网格线
plt.show()
'''
color：线条颜色，值r表示红色（red）
cnames = {
'aliceblue': '#F0F8FF',
'antiquewhite': '#FAEBD7',
'aqua': '#00FFFF',
'aquamarine': '#7FFFD4',
'azure': '#F0FFFF',
'beige': '#F5F5DC',
'bisque': '#FFE4C4',
'black': '#000000',
'blanchedalmond': '#FFEBCD',
'blue': '#0000FF',
'blueviolet': '#8A2BE2',
'brown': '#A52A2A',
'burlywood': '#DEB887',
'cadetblue': '#5F9EA0',
'chartreuse': '#7FFF00',
'chocolate': '#D2691E',
'coral': '#FF7F50',
'cornflowerblue': '#6495ED',
'cornsilk': '#FFF8DC',
'crimson': '#DC143C',
'cyan': '#00FFFF',
'darkblue': '#00008B',
'darkcyan': '#008B8B',
'darkgoldenrod': '#B8860B',
'darkgray': '#A9A9A9',
'darkgreen': '#006400',
'darkkhaki': '#BDB76B',
'darkmagenta': '#8B008B',
'darkolivegreen': '#556B2F',
'darkorange': '#FF8C00',
'darkorchid': '#9932CC',
'darkred': '#8B0000',
'darksalmon': '#E9967A',
'darkseagreen': '#8FBC8F',
'darkslateblue': '#483D8B',
'darkslategray': '#2F4F4F',
'darkturquoise': '#00CED1',
'darkviolet': '#9400D3',
'deeppink': '#FF1493',
'deepskyblue': '#00BFFF',
'dimgray': '#696969',
'dodgerblue': '#1E90FF',
'firebrick': '#B22222',
'floralwhite': '#FFFAF0',
'forestgreen': '#228B22',
'fuchsia': '#FF00FF',
'gainsboro': '#DCDCDC',
'ghostwhite': '#F8F8FF',
'gold': '#FFD700',
'goldenrod': '#DAA520',
'gray': '#808080',
'green': '#008000',
'greenyellow': '#ADFF2F',
'honeydew': '#F0FFF0',
'hotpink': '#FF69B4',
'indianred': '#CD5C5C',
'indigo': '#4B0082',
'ivory': '#FFFFF0',
'khaki': '#F0E68C',
'lavender': '#E6E6FA',
'lavenderblush': '#FFF0F5',
'lawngreen': '#7CFC00',
'lemonchiffon': '#FFFACD',
'lightblue': '#ADD8E6',
'lightcoral': '#F08080',
'lightcyan': '#E0FFFF',
'lightgoldenrodyellow': '#FAFAD2',
'lightgreen': '#90EE90',
'lightgray': '#D3D3D3',
'lightpink': '#FFB6C1',
'lightsalmon': '#FFA07A',
'lightseagreen': '#20B2AA',
'lightskyblue': '#87CEFA',
'lightslategray': '#778899',
'lightsteelblue': '#B0C4DE',
'lightyellow': '#FFFFE0',
'lime': '#00FF00',
'limegreen': '#32CD32',
'linen': '#FAF0E6',
'magenta': '#FF00FF',
'maroon': '#800000',
'mediumaquamarine': '#66CDAA',
'mediumblue': '#0000CD',
'mediumorchid': '#BA55D3',
'mediumpurple': '#9370DB',
'mediumseagreen': '#3CB371',
'mediumslateblue': '#7B68EE',
'mediumspringgreen': '#00FA9A',
'mediumturquoise': '#48D1CC',
'mediumvioletred': '#C71585',
'midnightblue': '#191970',
'mintcream': '#F5FFFA',
'mistyrose': '#FFE4E1',
'moccasin': '#FFE4B5',
'navajowhite': '#FFDEAD',
'navy': '#000080',
'oldlace': '#FDF5E6',
'olive': '#808000',
'olivedrab': '#6B8E23',
'orange': '#FFA500',
'orangered': '#FF4500',
'orchid': '#DA70D6',
'palegoldenrod': '#EEE8AA',
'palegreen': '#98FB98',
'paleturquoise': '#AFEEEE',
'palevioletred': '#DB7093',
'papayawhip': '#FFEFD5',
'peachpuff': '#FFDAB9',
'peru': '#CD853F',
'pink': '#FFC0CB',
'plum': '#DDA0DD',
'powderblue': '#B0E0E6',
'purple': '#800080',
'red': '#FF0000',
'rosybrown': '#BC8F8F',
'royalblue': '#4169E1',
'saddlebrown': '#8B4513',
'salmon': '#FA8072',
'sandybrown': '#FAA460',
'seagreen': '#2E8B57',
'seashell': '#FFF5EE',
'sienna': '#A0522D',
'silver': '#C0C0C0',
'skyblue': '#87CEEB',
'slateblue': '#6A5ACD',
'slategray': '#708090',
'snow': '#FFFAFA',
'springgreen': '#00FF7F',
'steelblue': '#4682B4',
'tan': '#D2B48C',
'teal': '#008080',
'thistle': '#D8BFD8',
'tomato': '#FF6347',
'turquoise': '#40E0D0',
'violet': '#EE82EE',
'wheat': '#F5DEB3',
'white': '#FFFFFF',
'whitesmoke': '#F5F5F5',
'yellow': '#FFFF00',
'yellowgreen': '#9ACD32'}
marker：点的形状，值o表示点为圆圈标记（circle marker）
'.' point marker
',' pixel marker
'o' circle marker
'v' triangle_down marker
'^' triangle_up marker
'<' triangle_left marker
'>' triangle_right marker
'1' tri_down marker
'2' tri_up marker
'3' tri_left marker
'4' tri_right marker
's' square marker
'p' pentagon marker
'*' star marker
'h' hexagon1 marker
'H' hexagon2 marker
'+' plus marker
'x' x marker
'D' diamond marker
'd' thin_diamond marker
'|' vline marker
'_' hline marker
linestyle：线条的形状，值dashed表示用虚线连接各点
'-' solid line style
'--' dashed line style
'-.' dash-dot line style
':' dotted line style
'''
#画单个折线图 水平误差折线
def Draw2D(realgps_enu_listx,realgps_enu_listy,slamgps_enu_listx,slamgps_enu_listy):
line = Line("真值和定位轨迹")
v_x = [5, 20, 36, 10, 10, 100]
v_y = [55, 60, 16, 20, 15, 80]
line.add("真值",
realgps_enu_listx,
realgps_enu_listy,
#mark_point=["average"]
)
line.add(
"SLAM",
slamgps_enu_listx,
slamgps_enu_listy,
#mark_line=[“average”] 表示补充一条平均值线
mark_line=["average", "max", "min"],
#mark_point=["average", "max", "min"],
#mark_point_symbol="diamond",
#mark_point_textcolor="#40ff27",
is_smooth=True,
)
line.render()
#画多个折线图 在同一个图里 水平误差折线
def Draw2D_error_onePic(slam_enu_error_list_xyz):
# 第一个为真值
# len(gps_data_list)和 len(slam_data_list)数量应该是对等的
error_list=[]
hengzuobiao=[]
fps=20.0
time_interbval=1.0/fps
for i in range(0,len(slam_enu_error_list_xyz)):
slam_enu_error_i=slam_enu_error_list_xyz[i]
#line = Line("Positioning Error (m) "+ str(i))
error_x_list=slam_enu_error_i[0]
error_y_list=slam_enu_error_i[1]
error_z_list=slam_enu_error_i[2]
for j in range(0,len(error_x_list)):
hengzuobiao.append((j*time_interbval))
plt.figure(figsize=(20, 10), dpi=100)
plt.plot(hengzuobiao, error_x_list, c='red', label="x")
plt.plot(hengzuobiao, error_y_list, c='green', linestyle='--', label="y")
plt.plot(hengzuobiao, error_z_list, c='blue', linestyle='-.', label="z")
plt.scatter(hengzuobiao, error_x_list, c='red')
plt.scatter(hengzuobiao, error_y_list, c='green')
plt.scatter(hengzuobiao, error_z_list, c='blue')
plt.legend(loc='best')
#plt.yticks(range(0, 50, 5))
plt.grid(True, linestyle='--', alpha=0.5)
plt.xlabel("Time (s)", fontdict={'size': 16})
plt.ylabel("Error (m)", fontdict={'size': 16})
plt.title("Position Error", fontdict={'size': 20})
plt.show()
#画单组 3个折线图 各自单独一个图里 水平误差折线
def Draw2D_error_MorePic(slam_enu_error_list_xyz):
# 第一个为真值
# len(gps_data_list)和 len(slam_data_list)数量应该是对等的
fps=10.0
time_interbval=1.0/fps
plt.figure()
'''
num = None, # 设定figure名称。系统默认按数字升序命名的figure_num（透视表输出窗口）e.g. “figure1”。可自行设定figure名称，名称或是INT，或是str类型；
figsize=None, # 设定figure尺寸。系统默认命令是rcParams["figure.fig.size"] = [6.4, 4.8]，即figure长宽为6.4 * 4.8；
dpi=None, # 设定figure像素密度。系统默命令是rcParams["sigure.dpi"] = 100；
facecolor=None, # 设定figure背景色。系统默认命令是rcParams["figure.facecolor"] = 'w'，即白色white；
edgecolor=None, frameon=True, # 设定要不要绘制轮廓&轮廓颜色。系统默认绘制轮廓，轮廓染色rcParams["figure.edgecolor"]='w',即白色white；
FigureClass=<class 'matplotlib.figure.Figure'>, # 设定使不使用一个figure模板。系统默认不使用；
'''
#fig, axs = plt.subplots(nrows=3, ncols=1)#3行1列 figsize=(20, 6) dpi=100 像素密度
colors = ['red', 'green', 'blue']
line_style = ['-', '--', '-.']
y_labels = ["x Error (m)", "y Error (m)", "z Error (m)"] #Error (m)
slam_enu_error_i=slam_enu_error_list_xyz
error_x_list=slam_enu_error_i[0]
error_y_list=slam_enu_error_i[1]
error_z_list=slam_enu_error_i[2]
x_i=[]
for j in range(0,len(error_x_list)):
x_i.append(j)
x_data=x_i
y_data = [error_x_list, error_y_list, error_z_list]
# xmin=int(min(x_data))
# xmax=int(max(x_data))
# ymin=int(min(y_data))
# ymax=int(max(y_data))
# yth=2
for i in range(1,4):
plt.subplot(3,1,i)
#plt.plot([0,1],[0,1])
i=i-1
# axs[i].plot(x_data, y_data[i], c=colors[i], label=y_labels[i], linestyle=line_style[i],linewidth=1,marker='.'
# ,markeredgecolor=colors[i],markersize='1',markeredgewidth=1) #label='total' alpha=0.5
plt.plot(x_data, y_data[i], c=colors[i], label=y_labels[i], linestyle=line_style[i],linewidth=1,marker='.',markersize=0.1)
#plt.scatter(x_data, y_data[i], c=colors[i])
plt.xlabel("ID")
plt.ylabel(y_labels[i])
#axs[i].legend(loc='best') #默认: 对于Axes， ‘best’, 对于Figure, 'upper right'
#axs[i].set_aspect(aspect=0.1)#y轴的单位刻度显示长度 与 x轴的单位刻度显示长度 的比例
#axs[i].axis('equal')#设置y轴和x轴的单位刻度显示长度相同
#axs[i].set_yticks(range(ymin,ymax, 2)) # y轴的间距和显示范围
plt.grid(True, linestyle='--', alpha=0.5)#背景网格线
#plt.set_xlabel("Time (s)", fontdict={'size': 10})#x轴坐标名字 和 字号
#plt.set_ylabel(y_labels[i], fontdict={'size': 10}, rotation=90) #y轴坐标名字 和 字号
#plt.set_title("Position {}".format(y_labels[i]), fontdict={'size': 12}) #标题
#fig.autofmt_xdate()
plt.show()
#画单组 3个折线图 各自单独一个图里 水平误差折线
def Draw2D_error_MorePicv2(slam_enu_error_list_xyz):
# 第一个为真值
# len(gps_data_list)和 len(slam_data_list)数量应该是对等的
fps=10.0
time_interbval=1.0/fps
'''
num = None, # 设定figure名称。系统默认按数字升序命名的figure_num（透视表输出窗口）e.g. “figure1”。可自行设定figure名称，名称或是INT，或是str类型；
figsize=None, # 设定figure尺寸。系统默认命令是rcParams["figure.fig.size"] = [6.4, 4.8]，即figure长宽为6.4 * 4.8；
dpi=None, # 设定figure像素密度。系统默命令是rcParams["sigure.dpi"] = 100；
facecolor=None, # 设定figure背景色。系统默认命令是rcParams["figure.facecolor"] = 'w'，即白色white；
edgecolor=None, frameon=True, # 设定要不要绘制轮廓&轮廓颜色。系统默认绘制轮廓，轮廓染色rcParams["figure.edgecolor"]='w',即白色white；
FigureClass=<class 'matplotlib.figure.Figure'>, # 设定使不使用一个figure模板。系统默认不使用；
'''
fig, axs = plt.subplots(nrows=3, ncols=1)#3行1列 figsize=(20, 6) dpi=100 像素密度
colors = ['red', 'green', 'blue']
line_style = ['-', '--', '-.']
y_labels = ["x Error (m)", "y Error (m)", "z Error (m)"] #Error (m)
slam_enu_error_i=slam_enu_error_list_xyz
error_x_list=slam_enu_error_i[0]
error_y_list=slam_enu_error_i[1]
error_z_list=slam_enu_error_i[2]
x_i=[]
for j in range(0,len(error_x_list)):
x_i.append((j*time_interbval))
x_data=x_i
y_data = [error_x_list, error_y_list, error_z_list]
# xmin=int(min(x_data))
# xmax=int(max(x_data))
# ymin=int(min(y_data))
# ymax=int(max(y_data))
# yth=2
for i in range(3):
# axs[i].plot(x_data, y_data[i], c=colors[i], label=y_labels[i], linestyle=line_style[i],linewidth=1,marker='.'
# ,markeredgecolor=colors[i],markersize='1',markeredgewidth=1) #label='total' alpha=0.5
axs[i].plot(x_data, y_data[i], c=colors[i], label=y_labels[i], linestyle=line_style[i],linewidth=1,marker='.',markersize=0.1)
#axs[i].scatter(x_data, y_data[i], c=colors[i])
#axs[i].legend(loc='best') #默认: 对于Axes， ‘best’, 对于Figure, 'upper right'
#axs[i].set_aspect(aspect=0.1)#y轴的单位刻度显示长度 与 x轴的单位刻度显示长度 的比例
#axs[i].axis('equal')#设置y轴和x轴的单位刻度显示长度相同
#axs[i].set_yticks(range(ymin,ymax, 2)) # y轴的间距和显示范围
axs[i].grid(True, linestyle='--', alpha=0.5)#背景网格线
axs[i].set_xlabel("Time (s)", fontdict={'size': 10})#x轴坐标名字 和 字号
axs[i].set_ylabel(y_labels[i], fontdict={'size': 10}, rotation=90) #y轴坐标名字 和 字号
axs[i].set_title("Position {}".format(y_labels[i]), fontdict={'size': 12}) #标题
#fig.autofmt_xdate()
plt.show()
#画多组 多个折线图 各自单独一个图里 水平误差折线
def Draw2D_error_More33Pic(slam_enu_error_list_xyz):
fps=10.0
time_interbval=1.0/fps
'''
num = None, # 设定figure名称。系统默认按数字升序命名的figure_num（透视表输出窗口）e.g. “figure1”。可自行设定figure名称，名称或是INT，或是str类型；
figsize=None, # 设定figure尺寸。系统默认命令是rcParams["figure.fig.size"] = [6.4, 4.8]，即figure长宽为6.4 * 4.8；
dpi=None, # 设定figure像素密度。系统默命令是rcParams["sigure.dpi"] = 100；
facecolor=None, # 设定figure背景色。系统默认命令是rcParams["figure.facecolor"] = 'w'，即白色white；
edgecolor=None, frameon=True, # 设定要不要绘制轮廓&轮廓颜色。系统默认绘制轮廓，轮廓染色rcParams["figure.edgecolor"]='w',即白色white；
FigureClass=<class 'matplotlib.figure.Figure'>, # 设定使不使用一个figure模板。系统默认不使用；
'''
fig, axs = plt.subplots(nrows=3, ncols=1,dpi=100)#3行1列 figsize=(20, 6) dpi=100 像素密度
#colors = ['blue','green','red']
colors = ['blue','green','red']
line_style = ['-', '--', '-.']
#datalabls=["nogps-nogps","gps-nogps","gps-gps"]
datalabls=["true","vio + gps","vio"]
y_labels = ["x Error (m)", "y Error (m)", "z Error (m)"] #Error (m)
for i in range(0,len(slam_enu_error_list_xyz)):
slam_enu_error_i=slam_enu_error_list_xyz[i]
error_x_list=slam_enu_error_i[0]
error_y_list=slam_enu_error_i[1]
error_z_list=slam_enu_error_i[2]
x_i=[]
for j in range(0,len(error_x_list)):
x_i.append((j*time_interbval))
x_data=x_i
y_data = [error_x_list, error_y_list, error_z_list]
for ii in range(0,3):
# axs[i].plot(x_data, y_data[i], c=colors[i], label=y_labels[i], linestyle=line_style[i],linewidth=1,marker='.'
# ,markeredgecolor=colors[i],markersize='1',markeredgewidth=1) #label='total' alpha=0.5
#添加lable标签显示
#axs[ii].plot(x_data, y_data[ii], c=colors[i], label=datalabls[i], linestyle=line_style[i],linewidth=1,marker='.',markersize=0.1)
axs[ii].plot(x_data, y_data[ii], c=colors[i], linestyle=line_style[i],linewidth=1,marker='.',markersize=0.1)
#axs[ii].scatter(x_data, y_data[ii], c=colors[i])
axs[ii].legend(loc='best')
#axs[ii].gca().set_box_aspect((3, 5, 2)) # 当x、y、z轴范围之比为3:5:2时。
#axs[ii].set_yticks(range(-4, 4, 2))
#if ii==2:
#axs[ii].set_yticks(range(-4, 8, 2))
# if ii==0:
# axs[ii].set_yticks(range(-60, 30, 10))
# elif ii==1:
# axs[ii].set_yticks(range(-30, 70, 20)) # y轴的间距和显示范围
# else:
# axs[ii].set_yticks(range(-20, 30, 10))
axs[ii].grid(True, linestyle='--', alpha=0.5)#背景网格线
axs[ii].set_xlabel("Time (s)", fontdict={'size': 10})#x轴坐标名字 和 字号
axs[ii].set_ylabel(y_labels[ii], fontdict={'size': 10}, rotation=90) #y轴坐标名字 和 字号 Y轴说明是否旋转90度
axs[ii].set_title("Position {}".format(y_labels[ii]), fontdict={'size': 12}) #标题
fig.autofmt_xdate()
plt.show()
#==========================================================
pipei_sourec_value=0#读取元数据匹配 对其GPS
read_enu_darw=1 #读取处理过的enu 对比画图
if __name__ == "__main__":
#root_path="C:/Users/dongdong/Desktop/slam代码/测试数据/芝加哥_ehang实验结果/ehang/ehang2建图数据测试地图精度结果/"
root_path="C:/Users/dongdong/Desktop/slam代码/测试数据/芝加哥_ehang实验结果/ehang/ehang4/"
#root_path="C:/Users/dongdong/Desktop/slam代码/测试数据/芝加哥_ehang实验结果/ehang/ehang3/"
#root_path="C:/Users/dongdong/Desktop/slam代码/测试数据/芝加哥_ehang实验结果/芝加哥/测试数据/"
#root_path="C:/Users/dongdong/Desktop/slam代码/测试数据/芝加哥_ehang实验结果/芝加哥/建图数据/"
#root_path="C:/Users/dongdong/Desktop/slam代码/测试数据/芝加哥_ehang实验结果/科技路数据/测试数据1/"
#root_path="C:/Users/dongdong/Desktop/slam代码/测试数据/芝加哥_ehang实验结果/科技路数据/建图数据/"
slamgps_txt_name="结果1.txt" #slam定位结果txt 名字 結果1
#slamgps_txt_name="frame_trajectory.txt"
realgps_txt_name="真实gps.txt" #真实GPS txt 名字 gps真值 真实gps
# 见图用到的
#data_name="带gps建图数据定位"
#data_name="不带gps建图数据定位"
#定位用到的
# eh3 eh4
data_name="不带gps建图不带gps定位" # 12.857068502706884 10.504316957115515
data_name="带gps建图不带gps定位" # 7.131057084533781 3.9117045628559612
data_name="带gps建图带gps定位" # 1.87419478565561 2.394471786497714
#ENU坐标系参考GPS原点
#gps_ref=[47.6289700414 ,-122.167686472 ,111.5] #仿真芝加哥测试测试数据
#gps_ref=[47.6304101698, -122.165953479, 360.334594727] #仿真芝加哥建图测试数据
#gps_ref=[34.2371703354, 108.890706449, 400] #科技路建图数据
#gps_ref=[34.0343737663, 108.756061354, 400.0] #ehang4
gps_ref=[34.0343737663 , 108.756061354 ,400.0] #ehang3-4
#gps_ref=[34.0343829843 , 108.756072317 ,400.0] #ehang2
slamgps_txt=root_path+data_name+"/"+slamgps_txt_name #slam定位结果txt 路径
print(slamgps_txt)
realgps_txt=root_path+realgps_txt_name #真实gpstxt 路径
print(realgps_txt)
gps_slamgps_txt_savename=root_path+data_name+"/匹配gps_slam"+slamgps_txt_name #匹配后要保存的
gps_realgps_txt_savename=root_path+data_name+"/匹配gps_real"+slamgps_txt_name
enu_slamgps_txt_savename=root_path+data_name+"/匹配enu_slam"+slamgps_txt_name #匹配后要保存的
enu_realgps_txt_savename=root_path+data_name+"/匹配enu_real"+slamgps_txt_name
if pipei_sourec_value==1:
#8810 8381 8638 15880 15863
#1-1从原始数据中获取slam gps和对应真实GPS（可能没有 -1）
slamGPS_List=readtxt_gps_havegps_we(slamgps_txt) # 从自己的SLam输出读取数据
#slamGPS_List=readtxt_slamSaveTraceEnu(slamgps_txt) # 从slam保存轨迹读取 时间戳是带图像名字
'''
for i in range(0,len(slamGPS_List)):
print(i,slamGPS_List[i])
print("\n",len(slamGPS_List))
'''
#print("时间戳 定位gps 纬度 经度 高度 真实gps 纬度 经度 高度 ",len(GPS_List))
#1-2 读取真实GPS realgps_txt 文件名 fixhigh 高度是否固定 -1 否 其他
realGPS_List=readtxt_realgps(realgps_txt)
'''
for i in range(0,len(realGPS_List)):
print(i,realGPS_List[i])
'''
#print("真实gps 时间戳 纬度 经度 高度 ",len(realGPS_List))
#2 匹配真实gps和slam gps帧
time_realgps_slamgps_list=from_realGPSFindslamGPS(realGPS_List,slamGPS_List)
# for i in range(0,len(time_realgps_slamgps_list)):
# print(i,time_realgps_slamgps_list[i])
# print("时间戳 真实gps 纬度 经度 高度 匹配的SLAM gps 纬度 经度 高度 误差")
# 3-1保存GPS 画轨迹图用
save_TXT_realGps_SLAMgps(time_realgps_slamgps_list,gps_realgps_txt_savename,gps_slamgps_txt_savename)
# 3-2保存enu坐标系（指定gps为原点） 画轨迹图用
#gps_ref=[] #ENU坐标系参考GPS原点
if data_name=="带gps建图不带gps定位" :
#剔除enu大于误差10m的点
save_TXT_realEnu_SLAMEnu(time_realgps_slamgps_list,enu_realgps_txt_savename,enu_slamgps_txt_savename,gps_ref,10)
else:
save_TXT_realEnu_SLAMEnu(time_realgps_slamgps_list,enu_realgps_txt_savename,enu_slamgps_txt_savename,gps_ref,999999)
#3-3单个图和真值对比可视化
# gps用于画真实轨迹图 enu用于画简略图
data_list=[] #文件名
data_list.append(enu_realgps_txt_savename)
data_list.append(enu_slamgps_txt_savename)
draw_tracelist=[]# enu数据列表
for i in range(0,len(data_list)):
list_txyz0,list_x0,list_y0,list_z0 = readtxt_ENU(data_list[i])
draw_tracelist.append([list_x0,list_y0,list_z0])
#折线误差slam值
errorlist_txyz0_error,list_x0_error,list_y0_error,list_z0_error = readtxt_ENU_Error(enu_slamgps_txt_savename)
draw_slamEnutraceError_list=[list_x0_error,list_y0_error,list_z0_error]
# 0 累计误差 计算总体转化均方根误差
error_all=0
for i in range(0,len(errorlist_txyz0_error)):
#print(errorlist_txyz0_error[i])
distance=sqrt((errorlist_txyz0_error[i][1])**2+(errorlist_txyz0_error[i][2])**2)
#print(i,time_realgps_slamgps_list[i])
error_all=error_all+time_realgps_slamgps_list[i][7]**2
error_averrange=sqrt(error_all/len(errorlist_txyz0_error))
print("时间戳 真实gps 纬度 经度 高度 匹配的SLAM gps 纬度 经度 高度 误差")
print("总误差",data_name,error_averrange)
#=============1 单组折线图画在多个行里
Draw2D_error_MorePic(draw_slamEnutraceError_list)
#=============2 画真值和单个slam轨迹 3D轨迹在同一个3D图 对比轨迹
Draw3D_trace_more(draw_tracelist)
#=============3 画多个3D轨迹的2D侧视图
Draw2D_trace_gpsvreal_list(draw_tracelist,"X(m)","Y(m)") #-俯视图xy
Draw2D_trace_gpsvreal_list(draw_tracelist,"X(m)","Z(m)") #-左视图xz
Draw2D_trace_gpsvreal_list(draw_tracelist,"Y(m)","Z(m)") #-右视图yz
#=======================2对ENU画图操作==========================
if read_enu_darw==1:
#4-1 读取enu数据
# 时间戳 x y z
#enu 坐标路径
# #1 gps真值
# enu_realgps_txt_savename=root_path+"带gps建图数据定位"+"/匹配enu_real"+slamgps_txt_name
# #2 不同状态的定位
# enu_slamgps_txt_savename1=root_path+"带gps建图数据定位"+"/匹配enu_slam"+slamgps_txt_name
# enu_slamgps_txt_savename2=root_path+"不带gps建图数据定位"+"/匹配enu_slam"+slamgps_txt_name
# data_list=[] #文件名
# data_list.append(enu_realgps_txt_savename)
# data_list.append(enu_slamgps_txt_savename1)
# data_list.append(enu_slamgps_txt_savename2)
#1 gps真值
#enu_realgps_txt_savename0=root_path+"真实gps完整.txt"
enu_realgps_txt_savename0=root_path+"带gps建图带gps定位"+"/匹配enu_real"+slamgps_txt_name
#2 不同状态的定位
enu_slamgps_txt_savename2=root_path+"不带gps建图不带gps定位"+"/匹配enu_slam"+slamgps_txt_name
enu_slamgps_txt_savename1=root_path+"带gps建图不带gps定位"+"/匹配enu_slam"+slamgps_txt_name
enu_slamgps_txt_savename3=root_path+"带gps建图带gps定位"+"/匹配enu_slam"+slamgps_txt_name
data_list=[] #文件名
data_list.append(enu_realgps_txt_savename0)
data_list.append(enu_slamgps_txt_savename1)
data_list.append(enu_slamgps_txt_savename2)
data_list.append(enu_slamgps_txt_savename3)
draw_tracelist=[]# enu数据列表
for i in range(0,len(data_list)):
list_txyz0,list_x0,list_y0,list_z0 = readtxt_ENU(data_list[i])
draw_tracelist.append([list_x0,list_y0,list_z0])
#==========1 画多个3D轨迹在同一个3D图 对比轨迹
Draw3D_trace_more(draw_tracelist)
#==========2 画多个3D轨迹的2D侧视图
Draw2D_trace_gpsvreal_list(draw_tracelist,"X(m)","Y(m)") #-俯视图xy
Draw2D_trace_gpsvreal_list(draw_tracelist,"X(m)","Z(m)") #-左视图xz
Draw2D_trace_gpsvreal_list(draw_tracelist,"Y(m)","Z(m)") #-右视图yz
#============3 画2D折线图误差
#3-1 加载数据 折线误差slam值
SlamEnuError_txtNamelist=[] #保存slam enu error轨迹 文件名
#SlamEnuError_txtNamelist.append(enu_realgps_txt_savename0)
SlamEnuError_txtNamelist.append(enu_slamgps_txt_savename3)
SlamEnuError_txtNamelist.append(enu_slamgps_txt_savename1)
SlamEnuError_txtNamelist.append(enu_slamgps_txt_savename2)
draw_slamEnutraceError_list=[] #保存slam enu轨迹 数据
for i in range(0,len(SlamEnuError_txtNamelist)):
errorlist_txyz0_error,list_x0_error,list_y0_error,list_z0_error = readtxt_ENU_Error(SlamEnuError_txtNamelist[i])
errorlist_txyx0_error_skip=[]
errorlist_txyy0_error_skip=[]
errorlist_txyz0_error_skip=[]
# #步长 3
# for i in range(0,len(errorlist_txyz0_error),100):
# errorlist_txyx0_error_skip.append(list_x0_error[i])
# errorlist_txyy0_error_skip.append(list_y0_error[i])
# errorlist_txyz0_error_skip.append(list_z0_error[i])
draw_slamEnutraceError_list.append([list_x0_error,list_y0_error,list_z0_error])
#draw_slamEnutraceError_list.append([errorlist_txyx0_error_skip,errorlist_txyy0_error_skip,errorlist_txyz0_error_skip])
#=============3-2 画数据 2D折线图画在多个行里
Draw2D_error_More33Pic(draw_slamEnutraceError_list)
标签：误差,txt,list,方根,slam,enu,error,GPS,gps From： https://www.cnblogs.com/gooutlook/p/17067377.html