td_rovio.applyTimeOffset(-to)
td_rovio.cropTimes(td_vicon.getFirstTime(),td_vicon.getLastTime())
if doOkvis: # Okvis data acquisition and pre-processing
if(okvisOutputFile.endswith('.csv')):
CsvDataAcquisition.csvLoadTransform(okvisOutputFile, 0, 1, 4, td_okvis, okvis_posID, okvis_attID)
elif(okvisOutputFile.endswith('.bag')):
RosDataAcquisition.rosBagLoadTransformStamped(okvisOutputFile,okvisOutputTopic,td_okvis,okvis_posID,okvis_attID)
if(okvisOutputFile.endswith('data.csv')):
td_okvis.d[:,0] = td_okvis.d[:,0]*1e-9
if(okvisOutputFile.endswith('0.5speed.bag')):
td_okvis.d[:,0] = 0.5*td_okvis.d[:,0]
if(okvisOutputFile == '/home/michael/datasets/long_trajectory_leutiLynen/aslam/aslam_estimator_output_data.csv'):
q_real = td_okvis.col(okvis_attID[3])
q_im = -td_okvis.cols(okvis_attID[0:3])
td_okvis.setCols(q_real,okvis_attID[0])
td_okvis.setCols(q_im,okvis_attID[1:4])
td_okvis.computeRotationalRateFromAttitude(okvis_attID,okvis_rorID,2,2)
td_okvis.computeNormOfColumns(okvis_rorID,okvis_ronID)
td_okvis.applyTimeOffset(td_vicon.getFirstTime()-td_okvis.getFirstTime())
to = td_okvis.getTimeOffset(okvis_ronID,td_vicon,vicon_ronID)
td_okvis.applyTimeOffset(-to)
td_okvis.cropTimes(td_vicon.getFirstTime(),td_vicon.getLastTime())
td_okvis.computeVelocitiesInBodyFrameFromPostionInWorldFrame(okvis_posID, okvis_velID, okvis_attID)
if plotRon: # Plotting rotational rate norm
if doRovio:
plotterRon.addDataToSubplot(td_rovio, rovio_ronID, 1, 'r', 'rovio rotational rate norm')
plotterRon.addDataToSubplot(td_vicon, vicon_ronID, 1, 'b', 'vicon rotational rate norm')
if doOkvis:
plotterRon.addDataToSubplot(td_okvis, okvis_ronID, 1, 'g', 'okvis rotational rate norm')
