#!/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
#=======================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
#函数名 读取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=[]
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()
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()
positionList_i[0]=timesharp
positionList_i[1]=real_gps_lat
positionList_i[2]=real_gps_lon
positionList_i[3]=real_gps_high
positionList.append(positionList_i)
#是否排序
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
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] 参考原点
# 保存格式 时间戳 x-n y-e z-u
#
def save_TXT_realEnu_SLAMEnu(time_realgps_slamgps_list,save_realgpsname,save_slamgpsname,gps_ref):
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]
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)
xEast, yNorth, zUp=geodetic_to_enu(realGPS_lat,realGPS_lon,realGPS_high,gps_ref[0],gps_ref[1],gps_ref[2])
slamGPS_lat=time_realgps_slamgps_list[i][4]
slamGPS_lon=time_realgps_slamgps_list[i][5]
slamGPS_high=time_realgps_slamgps_list[i][6]
slamgps_txtfilet_msg=str(timesharp)+" "+str(xEast)+" "+str(yNorth)+" "+str(zUp)+"\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
#==========================================================
if __name__ == "__main__":
#root_path="C:/Users/dongdong/Desktop/slam代码/测试数据/芝加哥_ehang实验结果/ehang/ehang3/"
root_path="C:/Users/dongdong/Desktop/slam代码/测试数据/芝加哥_ehang实验结果/芝加哥/测试数据/"
slamgps_txt_name="結果1.txt" #slam定位结果txt 名字
realgps_txt_name="gps真值.txt" #真实GPStxt 名字 gps真值 真实gps
#data_name="不带gps建图不带gps定位"
#data_name="带gps建图不带gps定位"
#data_name="带gps建图带gps定位"
data_name="不帶gps不帶gps定位結果"
#data_name="帶gps建圖不帶gps定位结果"
#data_name="帶gps地圖帶gps定位結果"
#ENU坐标系参考GPS原点
#gps_ref=[34.0343737663, 108.756061354, 400.0] #ehang3
#gps_ref=[47.626127888 ,-122.165953479 ,358.800964355] #ehang4
gps_ref=[47.6289700414 ,-122.167686472 ,111.5] #仿真测试测试数据
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
#1-1从原始数据中获取slam gps和对应真实GPS（可能没有 -1）
slamGPS_List=readtxt_gps_havegps_we(slamgps_txt)
'''
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_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原点
save_TXT_realEnu_SLAMEnu(time_realgps_slamgps_list,enu_realgps_txt_savename,enu_slamgps_txt_savename,gps_ref)
#3计算差值
# 3-2 累计误差 计算总体转化均方根误差
error_all=0
for i in range(0,len(time_realgps_slamgps_list)):
print(i,time_realgps_slamgps_list[i])
error_all=error_all+time_realgps_slamgps_list[i][7]**2
error_averrange=sqrt(error_all/len(time_realgps_slamgps_list))
print("时间戳 真实gps 纬度 经度 高度 匹配的SLAM gps 纬度 经度 高度 误差")
print("总误差",data_name,error_averrange)
# gps用于画真实轨迹图 enu用于画简略图
（1）从txt读取GPS数据 真实GPS和slam定位GPS匹配 坐标系ecef和enu转化 计算均方根误差和单帧误差的相关教程结束。
