# 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]'],
['$v_x$[m/s]', '$v_y$[m/s]', '$v_z$[m/s]'], 10000)
plotterVel.addDataToSubplotMultiple(td_rovio, 'vel', [1, 2, 3],
['r', 'r', 'r'], ['', '', ''])
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'], ['','',''])
plotterAtt.addDataToSubplotMultiple(td_aligned, 'att', [1,2,3,4], ['r','r','r','r'], ['','','',''])
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'], ['','','',''])
if plotYpr: # Yaw-pitch-roll plotting
