SLAM 轨迹评估方法 evo（包括GPS坐标转换成TUM）

安装 evo

pip install evo --upgrade --no-binary evo 

SLAM轨迹

运行ORBSLAM

rosrun ORB_SLAM3 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/RealSense_T265.yaml false 

之后会生成一个TUM格式的轨迹FramTrajectory_TUM_Format.txt

GPS轨迹

我们也需要将获取的GPS轨迹转化成笛卡尔坐标下TUM格式坐标

import math 
import numpy as np 
import rospy
from std_msgs.msg import std_msgs
from plot_py.msg import INS_Data
import time
from matplotlib.animation import FuncAnimation
import os
from math import sin,cos,radians
a = 6378137. #长轴
f = 1 / 298.257223563 #扁率
fle = open("/home/jiangz/catkin_ws/src/plot_py/data/GPS_301_short.txt",mode = 'w')
def gps_to_cartesion(lat,lon,alt):
    lat = radians(lat)
    lon = radians(lon)
    print (lat,lon,alt)
    b = a * (1 - f)
    e = (a ** 2 - b ** 2) ** 0.5 / a
    N = a / (1 - e ** 2 * sin(lat) ** 2) ** 0.5

    # 计算XYZ坐标
    X = (N + alt) * cos(lat) * cos(lon)
    Y = (N + alt) * cos(lat) * sin(lon)
    Z = (N * (1 - e ** 2) + alt) * sin(lat)

    return X,Y,Z

def callback(data):
    
    x,y,z = gps_to_cartesion(data.latitude,data.longitude,data.altitude)
    stamp = data.header.stamp
    time = float(stamp.secs) + (stamp.nsecs / 1e+9 ) 
    fle.write(str(time) +" "+str(x) +" "+str(y) + " " + str(z))
    fle.write(" 0. 0. 0. 0.\n")
    # print(x,y,z)
    print(str(time) +" "+str(x) +" "+str(y) + " " + str(z))
    print("\n")

def save_as_txt():
    rospy.init_node('GPS',anonymous=True) 
    rospy.Subscriber("/ins_data",INS_Data,callback)
    rospy.spin() 
    fle.close()
    
save_as_txt()

使用此代码，将GPS的经纬度海拔信息转换成笛卡尔坐标下的坐标，并且保存成TUM格式

画出图像

轨迹对比图像

链接：https://www.guyuehome.com/18717
里详细介绍了evo方法

原谅我的GPS测量误差大得离谱。。。。

标题单一图像

在ORBSLAM里面自带了一个画图的工具
/evaluation/evaluate_ate_scale.py

python evaluation/evaluate_ate_scale.py FrameTrajectory_TUM_Format.txt FrameTrajectory_TUM_Format.txt --plot PLOT 

其中第一个txt文件是真实轨迹，后一个是测量轨迹，正常来说会对两个轨迹进行对比，但是犹豫GPS轨迹在转换成笛卡尔坐标系后，还需要求出R和T之后，才能得到SLAM轨迹相同Scale 的轨迹图，所以使用这个方法比对会比较麻烦。

执行完成后，会在当前目录下生成一个名为PLOT的文件。