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)
plt.tight_layout()
plt.savefig('comparison.pdf', bbox_inches = 'tight')
plt.savefig('comparison.png', bbox_inches = 'tight')
raw_input()
if plotYpr: # Yaw-pitch-roll plotting
plotterYpr = Plotter(-1, [3,1],'Yaw-Pitch-Roll Decomposition',['','','time[s]'],['roll[rad]','pitch[rad]','yaw[rad]'],10000)
if rovioEvaluator.doCov:
plotterYpr.addDataToSubplotMultiple(td_rovio, 'yprSm', [1,2,3], ['r--','r--','r--'], ['','',''])
plotterYpr.addDataToSubplotMultiple(td_rovio, 'yprSp', [1,2,3], ['r--','r--','r--'], ['','',''])
plotterYpr.addDataToSubplotMultiple(td_rovio, 'ypr', [1,2,3], ['r','r','r'], ['','',''])
plotterYpr.addDataToSubplotMultiple(td_vicon, 'ypr', [1,2,3], ['b','b','b'], ['','',''])
if plotRor: # Rotational rate plotting
plotterRor = Plotter(-1, [3,1],'Rotational Rate',['','','time[s]'],['$\omega_x$[rad/s]','$\omega_y$[rad/s]','$\omega_z$[rad/s]'],10000)
plotterRor.addDataToSubplotMultiple(td_rovio, 'ror', [1,2,3], ['r','r','r'], ['','',''])
plotterRor.addDataToSubplotMultiple(td_vicon, 'ror', [1,2,3], ['b','b','b'], ['','',''])
if plotRon: # Plotting rotational rate norm
plotterRon = Plotter(-1, [1,1],'Norm of Rotational Rate',['time [s]'],['Rotational Rate Norm [rad/s]'],10000)
plotterRon.addDataToSubplot(td_rovio, 'ron', 1, 'r', 'rovio rotational rate norm')
plotterRon.addDataToSubplot(td_vicon, 'ron', 1, 'b', 'vicon rotational rate norm')
if plotExt and rovioEvaluator.doExtrinsics: # Extrinsics Plotting
plotterExt = Plotter(-1, [3,1],'Extrinsics Translational Part',['','','time[s]'],['x[m]','y[m]','z[m]'],10000)
if rovioEvaluator.doCov:
plotterExt.addDataToSubplotMultiple(td_rovio, 'extPosSm', [1,2,3], ['r--','r--','r--'], ['','',''])
plotterExt.addDataToSubplotMultiple(td_rovio, 'extPosSp', [1,2,3], ['r--','r--','r--'], ['','',''])
plotterExt.addDataToSubplotMultiple(td_rovio, 'extPos', [1,2,3], ['r','r','r'], ['','',''])
rovioEvaluator.doLeutiEvaluation()
rovioEvaluator.doFeatureDepthEvaluation()
raw_input("Press Enter to continue...")
rorNID1 = 17
rorNID2 = 8
"""
First we will fill the two TimedData structures with the same StampedTransforms,
loaded from topic 'rovio/transform' in example.bag.
The indices denote the start column of the position(td1=1, td2=9) and attitude(td1=4, td2=1)
"""
RosDataAcquisition.rosBagLoadTransformStamped(
os.path.join(data_path, 'example.bag'), '/rovio/transform', td1,
posIDs1, attIDs1)
RosDataAcquisition.rosBagLoadTransformStamped(
os.path.join(data_path, 'example.bag'), '/rovio/transform', td2,
posIDs2, attIDs2)
# Add initial x to plot
plotter1.addDataToSubplot(td1, posIDs1[0], 1, 'r', 'td1In x')
plotter1.addDataToSubplot(td2, posIDs2[0], 1, 'b', 'td2In x')
"""
Apply body transform to the second data set. The column start IDs of position(9) and attitutde(1) have to be provided.
We define the rotation through a rotation vector vCB, the exponential represents the corresponding rotation quaternion qCB.
The translation is defined by the translation vector B_r_BC
"""
vCB = np.array([0.1, 0.2, 0.32])
qCB = Quaternion.q_exp(vCB)
B_r_BC = np.array([1.1, -0.2, 0.4])
td2.applyBodyTransform(posIDs2, attIDs2, B_r_BC, qCB)
print('Applying Body Transform:')
print('Rotation Vector ln(qCB):\tvx:' + str(vCB[0]) + '\tvy:' +
str(vCB[1]) + '\tvz:' + str(vCB[2]))
print('Translation Vector B_r_BC:\trx:' + str(B_r_BC[0]) + '\try:' +
str(B_r_BC[1]) + '\trz:' + str(B_r_BC[2]))
['roll[rad]', 'pitch[rad]', 'yaw[rad]'], 10000)
if rovioEvaluator.doCov:
plotterYpr.addDataToSubplotMultiple(td_visual, 'yprSm', [1, 2, 3],
['r--', 'r--', 'r--'],
['', '', ''])
plotterYpr.addDataToSubplotMultiple(td_visual, 'yprSp', [1, 2, 3],
['r--', 'r--', 'r--'],
['', '', ''])
plotterYpr.addDataToSubplotMultiple(td_visual, 'ypr', [1, 2, 3],
['r', 'r', 'r'], ['', '', ''])
plotterYpr.addDataToSubplotMultiple(td_vicon, 'ypr', [1, 2, 3],
['b', 'b', 'b'], ['', '', ''])
if plotRor: # Rotational rate plotting
plotterRor = Plotter(
-1, [3, 1], 'Rotational Rate', ['', '', 'time[s]'],
['$\omega_x$[rad/s]', '$\omega_y$[rad/s]', '$\omega_z$[rad/s]'], 10000)
plotterRor.addDataToSubplotMultiple(td_visual, 'ror', [1, 2, 3],
['r', 'r', 'r'], ['', '', ''])
plotterRor.addDataToSubplotMultiple(td_vicon, 'ror', [1, 2, 3],
['b', 'b', 'b'], ['', '', ''])
if plotRon: # Plotting rotational rate norm
plotterRon = Plotter(-1, [1, 1], 'Norm of Rotational Rate', ['time [s]'],
['Rotational Rate Norm [rad/s]'], 10000)
plotterRon.addDataToSubplot(td_visual, 'ron', 1, 'r',
'rovio rotational rate norm')
plotterRon.addDataToSubplot(td_vicon, 'ron', 1, 'b',
'vicon rotational rate norm')
raw_input("Press Enter to continue...")
