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...")