使用truth.nav或者编译之后得到的KF_GINS_Navresult.nav文件都可以,通过查看readme_cn,得知每一列的物理意义,如下图:
第二列为时间,第6~8列为该时刻下的速度,使用这四列信息就可以画出来
需要用到两个公式:
加速度计算公式a=(v2-v1)/(t2-t1)
位移公式x=v0 * t + 1/2 * a * t^2
将初始点规定为(0,0,0),使用上两个公式就可以计算出相邻两行数据的位移,再加上上一时刻的位置就是当前的位置,这样子就可以得到许多的位置信息,在平面上画出即可.
import numpy as np
import pyqtgraph as pg
from pyqtgraph.Qt import QtCore, QtGui
import pyqtgraph.opengl as gl
from PyQt5.Qt import QApplication
from pyqtgraph.opengl.GLGraphicsItem import GLGraphicsItem
from OpenGL.GL import *
with open("dataset/ADIS16460/truth.nav") as f:
# with open("dataset/KF_GINS_Navresult.nav") as f:
# with open("dataset/KF_GINS_Navresult.nav") as f:
files = f.readlines()
time = []
vx = []
vy = []
vz = []
for file in files:
t = float(file.split()[1])
v_x = float(file.split()[5])
v_y = float(file.split()[6])
v_z = float(file.split()[7])
time.append(t)
vx.append(v_x)
vy.append(v_y)
vz.append(v_z)
def a(v1,v2,t):
if v2-v1 !=0:
a_ = (v2-v1)/t
else:
a_=0
return a_
def x(v0,t,a):
s_ = v0*t+(1/2)*a*t**2
return s_
s_i = []
for i in range(len(vx)-1):
t = time[i+1]-time[i]
a_x = a(vx[i],vx[i+1],t)
a_y = a(vy[i],vy[i+1],t)
a_z = a(vz[i],vz[i+1],t)
s_x = x(vx[i],t,a_x)
s_y = x(vy[i],t,a_y)
s_z = x(vz[i],t,a_z)
s_i.append([s_x,s_y,s_z])
points_add = np.array(s_i)
points = np.zeros((len(vx)-1,3))
for i in range(len(vx)-2):
points[i+1,:] = points[i,:]+points_add[i,:]
app = pg.mkQApp("InfiniteLine Example")
w = gl.GLViewWidget()
w.show()
w.setWindowTitle('pyqtgraph example: GLLinePlotItem')
w.setCameraPosition(distance=100,azimuth=180)
lp = gl.GLLinePlotItem()
w.addItem(lp)
gx = gl.GLGridItem()
gx.setSize(x=2500, y=2500, z=0)
gx.rotate(90, 0, 1, 0)
gx.translate(-10, 0, 0)
w.addItem(gx)
gy = gl.GLGridItem()
gy.setSize(x=2500, y=2500, z=0)
gy.rotate(90, 1, 0, 0)
gy.translate(0, -10, 0)
w.addItem(gy)
gz = gl.GLGridItem()
gz.setSize(x=2500, y=2500, z=0)
gz.translate(0, 0, -10)
w.addItem(gz)
lp.setData(pos=points )
QApplication.exec()
我这里使用pyqtgraph画出,也可以用opencv,但是要把z轴的位置信息给去除,具体效果如下,上面一幅图是数据集里面给出的图,下面是具体画出来的图