KF-gins输出结果绘图

使用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轴的位置信息给去除,具体效果如下,上面一幅图是数据集里面给出的图,下面是具体画出来的图