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')defgps_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

defcallback(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")defsave_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的文件。