python 画3维轨迹图并进行比较

最新推荐文章于 2024-08-16 11:45:44 发布

pj_find

最新推荐文章于 2024-08-16 11:45:44 发布

阅读量4.4k

点赞数

本文链接：https://blog.csdn.net/pj_find/article/details/88248613

版权

本文通过Python的matplotlib库展示了如何从数据集中加载3D坐标，并绘制两条不同的3D轨迹进行比较。代码示例中，数据分别来自'./dataset/poses/00.txt'和'../temp/kittiseq00_imu.txt'文件，提取了x、y、z坐标信息，并创建了红色和蓝色的3D曲线来表示不同数据集的轨迹。

摘要由CSDN通过智能技术生成

一.数据的格式

首先我们需要x,y,z三个数据进行画图。从本实验用到的数据集KITTI 00.txt中举例：

1.000000e+00 9.043680e-12 2.326809e-11 5.551115e-17 9.043683e-12 1.000000e+00 2.392370e-10 3.330669e-16 2.326810e-11 2.392370e-10 9.999999e-01 -4.440892e-16

一组有12个数据，相当于T={R，t}，R是3×3的矩阵，t是3×1的矩阵。我们需要的是t的数据。

有些groundtruth是8个数据，第一个是时间戳，在三个是x,y,z，后面四个是是四元数的数据。

代码如下：

# import necessary module
from mpl_toolkits.mplot3d import axes3d
import matplotlib.pyplot as plt
import numpy as np

# load data from file
# you can replace this using with open
data1 = np.loadtxt("./dataset/poses/00.txt")

first_2000 = data1[:, 3]
second_2000 = data1[:, 7]
third_2000 = data1[:, 11]
data2 = np.loadtxt("../temp/kittiseq00_imu.txt")
first_1000 = data2[:, 1]
second_1000 = data2[:, 2]
third_1000 = data2[:, 3]<