一. 資料的格式

首先我們需要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,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]
# print to check data
#print first_2000
#print second_2000
#print third_2000

# new a figure and set it into 3d
fig = plt.figure()
ax = fig.gca(projection='3d')

# set figure information
ax.set_title("3D_Curve")
ax.set_xlabel("x")
ax.set_ylabel("y")
ax.set_zlabel("z")

# draw the figure,the color is r = read
figure1 = ax.plot(first_2000,second_2000,third_2000,c='r')
figure2 = ax.plot(first_1000,second_1000,third_1000,c='b')
plt.show()

效果圖（電腦比較垃圾，後面的軌跡跟蹤的時候提取的特徵點太少）：

以上這篇python 畫3維軌跡圖並進行比較的例項就是小編分享給大家的全部內容了，希望能給大家一個參考，也希望大家多多支援我們。