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'], ['','','',''])