#!/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转化 计算均方根误差和单帧误差的更多相关文章

  1. GPS数据包格式及数据包解析

    GPS数据包解析 GPS数据包解析 目的 GPS数据类型及格式 数据格式 数据解释 解析代码 结构体定义 GPRMC解析函数 GPGGA解析函数 测试样例输出 gps数据包格式 gps数据解析 车联网 ...

  2. 字符输入流Reader类和FileReader和字符输入流读取字符数据

    java.io.Reader:字符输入流,是字符输入流的最顶层的父类,定义了一些共性的成员方法,是一个抽象类 共性成员方法: int read();读取单个字符并返回 int read(char[] ...

  3. GPS数据读取与处理

    GPS数据读取与处理 GPS模块简介 SiRF芯片在2004年发布的最新的第三代芯片SiRFstar III(GSW 3.0/3.1),使得民用GPS芯片在性能方面登上了一个顶峰,灵敏度比以前的产品大 ...

  4. 采集的GPS数据如何正确显示在arcgis和cad中

    利用GPS定位卫星,在全球范围内实时进行定位.导航的系统,称为全球卫星定位系统,简称GPS.GPS是由美国国防部研制建立的一种具有全方位.全天候.全时段.高精度的卫星导航系统,能为全球用户提供低成本. ...

  5. 使用Socket通信实现Silverlight客户端实时数据的获取(模拟GPS数据，地图实时位置)

    原文:使用Socket通信实现Silverlight客户端实时数据的获取(模拟GPS数据,地图实时位置) 在上一篇中说到了Silverlight下的Socket通信,在最后的时候说到本篇将会结合地图. ...

  6. 基于GPS数据建立隐式马尔可夫模型预测目的地

    <Trip destination prediction based on multi-day GPS data>是一篇在2019年,由吉林交通大学团队发表在elsevier期刊上的一篇论 ...

  7. python操作txt文件中数据教程[3]-python读取文件夹中所有txt文件并将数据转为csv文件

    python操作txt文件中数据教程[3]-python读取文件夹中所有txt文件并将数据转为csv文件 觉得有用的话,欢迎一起讨论相互学习~Follow Me 参考文献 python操作txt文件中 ...

  8. GPS数据解析

    1.摘要 GPS模块使用串口通信,那么它的的数据处理本质上还是串口通信处理,只是GPS模块的输出的有其特定的格式,需要字符串处理逻辑来解析其含义.如何高效的处理从GPS模块接收到的数据帧,是GPS驱动 ...

  9. 利用大数据技术处理海量GPS数据

    我秀中国物联网地图服务平台目前接入的监控车辆近百万辆,每天采集GPS数据7亿多条,产生日志文件70GB,使用传统的数据处理方式非常耗时. 比如,仅仅对GPS做一些简单的统计分析,程序就需要几个小时才能 ...

  10. 2.4 使用ARDUINO控制MC20进行GPS数据的获取和解析

    需要准备的硬件 MC20开发板 1个 https://item.taobao.com/item.htm?id=562661881042 GSM/GPRS天线 1根 https://item.taoba ...

随机推荐

  1. 读后笔记 -- Python 全栈测试开发 Chapter9：Postman + Newman 实现接口自动化

    9.1 Postman 工具 9.1.4 Postman 基本操作 1. Get 请求 GET 请求的参数通过 Params 设置,最后出现在 url 地址栏上,拼接在 API 后面.  2. Pos ...

  2. Linux CentOS下搭建golang 1.17 开发环境

    1. 下载软件包并安装 cd ~ wget https://storage.googleapis.com/golang/go1.17.2.linux-amd64.tar.gz tar zxvf go1 ...

  3. vue 点击元素滚动到指定位置（滑动到指定位置对应标签自动选中）

    一:各个模块不相同情况 1.内容部分<div class="anchor"> <div v-for="(item,index) in anchors&q ...

  4. 配置Centos8网络绑定

    配置Centos8网络绑定 原理: 在物理网卡两块之上创建一块虚拟主卡, 逻辑上是一主双从, 按不同的模式负载运行,常用模式如主备或并行提供双倍带宽等.模式:   可选参数"mode=act ...

  5. OpenCV Mat类数据存储方式

    参考BiliBili 于仕琪老师 avoid-memory-copy-in-opencv class CV_EXPORTS Mat { public: // some members int rows ...

  6. android studio 导出 release aar

  7. 关于 echarts 使用 geo 制作地图 tooltip 不显示问题（转）

    原文地址 我之前遇到过这问题,单独设置 tooltip 没效果,geo 下面也有 tooltip 属性,但是也不管用,网上查了一下说 geo 不支持 tooltip 提示框显示,就自己根据 echar ...

  8. OceanBase使用OBLOADER、OBDUMPER进行导入导出

    需求背景 需要定时给OB进行数据备份,并且在需要时可以全量导入,所以只能通过脚本来减少手动操作的繁琐. 脚本示例 导出脚本 #!/bin/bash # 这一步不能省,如果不设置定时运行时可能会有问题 ...

  9. C#windows 服务 《转载》

    转自:https://blog.csdn.net/Code_May/article/details/123909870 c#应用Windows服务 背景 一.创建windows服务 1.创建windo ...

  10. centos7 python设置虚拟环境

    virtualenv 是一个可以在同一计算机中隔离多个python版本的工具.有时,两个不同的项目可能需要不同版本的python,如 python2.6.6 / python3.0 ,但是如果都装到一 ...