td_aligned.addLabelingIncremental('pos',3)
td_aligned.addLabelingIncremental('att',4)
td_aligned.reInit()
RosDataAcquisition.rosBagLoadTimestampsOnly(timestampBag,timestampTopic,td_aligned)
# Interpolate Data
td.interpolateColumns(td_aligned, 'pos', 'pos')
td.interpolateQuaternion(td_aligned, 'att', 'att')
ind, = np.nonzero(np.fabs(np.diff(td.col(1))) < 1e-10)
print(ind)
ind, = np.nonzero(np.fabs(np.diff(td_aligned.col(1))) < 1e-10)
print(ind)
# Plotting
plotterPos = Plotter(-1, [3,1],'Position',['','','time[s]'],['x[m]','y[m]','z[m]'],150000)
plotterAtt = Plotter(-1, [4,1],'Attitude',['','','','time[s]'],['w','x','y','z'],150000)
plotterPos.addDataToSubplotMultiple(td, 'pos', [1,2,3], ['b','b','b'], ['','',''])
plotterAtt.addDataToSubplotMultiple(td, 'att', [1,2,3,4], ['b','b','b','b'], ['','','',''])
plotterPos.addDataToSubplotMultiple(td_aligned, 'pos', [1,2,3], ['g','g','g'], ['','',''])
plotterAtt.addDataToSubplotMultiple(td_aligned, 'att', [1,2,3,4], ['g','g','g','g'], ['','','',''])
plotterVel = Plotter(-1, [3,1],'Velocity',['','','time[s]'],['x[m]','y[m]','z[m]'],150000)
plotterVel.addDataToSubplotMultiple(td, 'vel', [1,2,3], ['b','b','b'], ['','',''])
# Apply body and inertial
if bodyTranslation != None and bodyRotation != None:
td_aligned.applyBodyTransform('pos', 'att', bodyTranslation, bodyRotation)
if inertialTranslation != None and inertialRotation != None:
td_aligned.applyBodyTransform('pos', 'att', inertialTranslation, inertialRotation)
plotterPos.addDataToSubplotMultiple(td_aligned, 'pos', [1,2,3], ['r','r','r'], ['','',''])
import os, sys, inspect
import seaborn as sns
import numpy as np
import matplotlib.pyplot as plt
from IPython import embed
from trajectory_toolkit.TimedData import TimedData
from trajectory_toolkit.Plotter import Plotter
from trajectory_toolkit.VIEvaluator import VIEvaluator
from trajectory_toolkit import Quaternion
from trajectory_toolkit import Utils
from trajectory_toolkit import RosDataAcquisition
plotterPos = Plotter(-1, [1,1],'',['time[s]'],['x[m]'],10000)
plotterPos.legendLoc = 'center'
for i, (label, bag_file_name, odometry_topic_name) in enumerate(comparisons):
td_visual, td_vicon = load_one_comparison(bag_file_name, odometry_topic_name)
#plotterPos.addDataToSubplotMultiple(td_visual, 'pos', [1, 3], 2*[''], 2*[label])
plotterPos.addDataToSubplot(td_visual, 1, 1, '', label)
#plotterPos.addDataToSubplot(td_visual, 3, 3, '', label)
if i == 0:
# Only plot ground truth once
plotterPos.addDataToSubplot(td_vicon, 1, 1, '', 'Truth')
#plotterPos.addDataToSubplot(td_vicon, 3, 3, '', 'Truth')
if True:
plotterPos.setFigureSize(9, 5)
1. A bagfile is loaded into two TimedData structures. The second TimedData is transformed,
we transform the body frame as well as the inertial frame, additionally a time delay is introduced.
We then try to align the first TimedData to the altered TimedData.
The results are plotted. In a first subplot one can observe the inputs, which are identical.
The second subplot shows the altered second TimedData compared to the input.
And the third plot shows the aligned first TimedData compared with the altered second TimedData.
2. A second functionality is live plotting. This is illustrated by simply plotting the entries of a
TransformStamped message. Set isNode to true for that feature.
"""
isNode = False
if isNode is not True:
""" Initialize Plotters
plotter1: figureID=1, three subplots
"""
plotter1 = Plotter(1, [3, 1])
""" Initialize TimedData
td1: TimedData with 18 columns: 0:t, 1-3:pos, 4-7:att, 8-10:vel, 11-13:velInBodyFrame, 14-16:ror, 17:rorNorm
td2: TimedData with 18 columns: 0:t, 1-4:att, 5-7:ror, 8:rorNorm, 9-11:pos, 12-14:vel, 15-17:velInBodyFrame
"""
td1 = TimedData(18)
td2 = TimedData(18)
posIDs1 = [1, 2, 3]
posIDs2 = [9, 10, 11]
attIDs1 = [4, 5, 6, 7]
attIDs2 = [1, 2, 3, 4]
velIDs1 = [8, 9, 10]
velIDs2 = [12, 13, 14]
velBIDs1 = [11, 12, 13]
velBIDs2 = [15, 16, 17]
rorIDs1 = [14, 15, 16]
# attIds = td_vicon.getColIDs('att')
# td_vicon.d[:, attIds[1:]] *= -1
# attIds = td_rovio.getColIDs('att')
# td_rovio.d[:, attIds[1:]] *= -1
# print(td_rovio.d.shape)
rovioEvaluator.getAllDerivatives()
rovioEvaluator.alignTime()
rovioEvaluator.alignBodyFrame()
rovioEvaluator.alignInertialFrame()
rovioEvaluator.getYpr()
rovioEvaluator.evaluateSigmaBounds()
td_rovio.computeRMS('pos', 'att', td_vicon, 'pos', 'att', 0.0)
if plotPos and draw_fig: # Position plotting
plotterPos = Plotter(-1, [3, 1], 'Position', ['', '', 'time[s]'],
['x[m]', 'y[m]', 'z[m]'], 10000)
if rovioEvaluator.doCov:
plotterPos.addDataToSubplotMultiple(td_rovio, 'posSm', [1, 2, 3],
['r--', 'r--', 'r--'],
['', '', ''])
plotterPos.addDataToSubplotMultiple(td_rovio, 'posSp', [1, 2, 3],
['r--', 'r--', 'r--'],
['', '', ''])
plotterPos.addDataToSubplotMultiple(td_rovio, 'pos', [1, 2, 3],
['r', 'r', 'r'], ['', '', ''])
plotterPos.addDataToSubplotMultiple(td_vicon, 'pos', [1, 2, 3],
['b', 'b', 'b'], ['', '', ''])
if plotVel and draw_fig: # Velocity plotting
plotterVel = Plotter(-1, [3, 1], 'Robocentric Velocity',
['', '', 'time[s]'],
rovioEvaluator.alignMode = 1
rovioEvaluator.plotLeutiDistances = []
rovioEvaluator.initTimedData(td_rovio)
rovioEvaluator.initTimedDataGT(td_vicon)
rovioEvaluator.acquireData()
rovioEvaluator.acquireDataGT()
rovioEvaluator.getAllDerivatives()
rovioEvaluator.alignTime()
rovioEvaluator.alignBodyFrame()
rovioEvaluator.alignInertialFrame()
rovioEvaluator.getYpr()
rovioEvaluator.evaluateSigmaBounds()
if plotPos: # Position plotting
plotterPos = Plotter(-1, [3,1],'Position',['','','time[s]'],['x[m]','y[m]','z[m]'],10000)
if rovioEvaluator.doCov:
plotterPos.addDataToSubplotMultiple(td_rovio, 'posSm', [1,2,3], ['r--','r--','r--'], ['','',''])
plotterPos.addDataToSubplotMultiple(td_rovio, 'posSp', [1,2,3], ['r--','r--','r--'], ['','',''])
plotterPos.addDataToSubplotMultiple(td_rovio, 'pos', [1,2,3], ['r','r','r'], ['','',''])
plotterPos.addDataToSubplotMultiple(td_vicon, 'pos', [1,2,3], ['b','b','b'], ['','',''])
if plotVel: # Velocity plotting
plotterVel = Plotter(-1, [3,1],'Robocentric Velocity',['','','time[s]'],['$v_x$[m/s]','$v_y$[m/s]','$v_z$[m/s]'],10000)
plotterVel.addDataToSubplotMultiple(td_rovio, 'vel', [1,2,3], ['r','r','r'], ['','',''])
plotterVel.addDataToSubplotMultiple(td_vicon, 'vel', [1,2,3], ['b','b','b'], ['','',''])
if plotAtt: # Attitude plotting
plotterAtt = Plotter(-1, [4,1],'Attitude Quaternion',['','','','time[s]'],['w[1]','x[1]','y[1]','z[1]'],10000)
plotterAtt.addDataToSubplotMultiple(td_rovio, 'att', [1,2,3,4], ['r','r','r','r'], ['','','',''])
plotterAtt.addDataToSubplotMultiple(td_vicon, 'att', [1,2,3,4], ['b','b','b','b'], ['','','',''])
