安装 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的文件。文章来源：https://www.toymoban.com/news/detail-812387.html
文章来源地址https://www.toymoban.com/news/detail-812387.html
到了这里，关于SLAM 轨迹评估方法 evo（包括GPS坐标转换成TUM）的文章就介绍完了。如果您还想了解更多内容，请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章，希望大家以后多多支持TOY模板网！
