plotterYpr.addDataToSubplotMultiple(td_rovio, rovio_yprSpID, [1,2,3], ['r--','r--','r--'], ['','',''])
td_vicon.quaternionToYpr(vicon_attID,vicon_yprID)
plotterYpr.addDataToSubplotMultiple(td_vicon, vicon_yprID, [1,2,3], ['b','b','b'], ['','',''])
if doOkvis:
td_okvis.quaternionToYpr(okvis_attID,okvis_yprID)
plotterYpr.addDataToSubplotMultiple(td_okvis, okvis_yprID, [1,2,3], ['g','g','g'], ['','',''])
if plotFea: # Plotting rotational rate norm
if doRovio:
figure()
x = [[]]
y = [[]]
for j in np.arange(nFeatures):
newFea = td_rovio.cols(rovio_posID) + Quaternion.q_rotate(Quaternion.q_inverse(td_rovio.cols(rovio_attID)),td_rovio.cols(rovio_fea_posID[j]))
for i in np.arange(0,3):
td_rovio.setCol(newFea[:,i],rovio_fea_posID[j][i])
lastStart = 0.0
lastID = -1
startID = 0
for i in np.arange(td_rovio.length()):
if(td_rovio.d[i,td_rovio.timeID] > 25.0):
if(td_rovio.d[i,rovio_fea_idxID[j]] < 0.0 or td_rovio.d[i,rovio_fea_idxID[j]] != lastID):
if len(x[-1]) > 0:
x.append([])
y.append([])
if(td_rovio.d[i,rovio_fea_idxID[j]] >= 0.0):
if len(x[-1]) == 0:
lastStart = td_rovio.d[i,td_rovio.timeID]
lastID = td_rovio.d[i,rovio_fea_idxID[j]]
startID = i
CsvDataAcquisition.csvLoadTransform(okvisOutputFile, 0, 1, 4, td_okvis,
'pos', 'att')
elif (okvisOutputFile.endswith('.bag')):
RosDataAcquisition.rosBagLoadOdometry(okvisOutputFile,
okvisOutputTopic, td_okvis,
'pos', 'att', 'vel', 'ror')
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('att'[3])
q_im = -td_okvis.col('att'[0:3])
td_okvis.setCol(q_real, 'att'[0])
td_okvis.setCol(q_im, 'att'[1:4])
# td_okvis.computeRotationalRateFromAttitude('att','ror',10,10)
td_okvis.setCol(
Quaternion.q_rotate(td_okvis.col('att'), td_okvis.col('ror')), 'ror')
td_okvis.computeNormOfColumns('ror', 'ron')
td_okvis.applyTimeOffset(td_vicon.getFirstTime() - td_okvis.getFirstTime())
to = td_okvis.getTimeOffset('ron', td_vicon, 'ron')
td_okvis.applyTimeOffset(-to)
td_okvis.cropTimes(td_vicon.getFirstTime(), td_vicon.getLastTime())
td_okvis.setCol(
Quaternion.q_rotate(td_okvis.col('att'), td_okvis.col('vel')), 'vel')
# td_okvis.computeVelocitiesInBodyFrameFromPostionInWorldFrame('pos', 'vel', 'att',30,30)
if plotRon: # Plotting rotational rate norm
if doRovio:
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, 'pos', 'att')
elif(okvisOutputFile.endswith('.bag')):
RosDataAcquisition.rosBagLoadOdometry(okvisOutputFile,okvisOutputTopic,td_okvis,'pos','att','vel','ror')
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('att'[3])
q_im = -td_okvis.col('att'[0:3])
td_okvis.setCol(q_real,'att'[0])
td_okvis.setCol(q_im,'att'[1:4])
# td_okvis.computeRotationalRateFromAttitude('att','ror',10,10)
td_okvis.setCol(Quaternion.q_rotate(td_okvis.col('att'), td_okvis.col('ror')),'ror')
td_okvis.computeNormOfColumns('ror','ron')
td_okvis.applyTimeOffset(td_vicon.getFirstTime()-td_okvis.getFirstTime())
to = td_okvis.getTimeOffset('ron',td_vicon,'ron')
td_okvis.applyTimeOffset(-to)
td_okvis.cropTimes(td_vicon.getFirstTime(),td_vicon.getLastTime())
td_okvis.setCol(Quaternion.q_rotate(td_okvis.col('att'), td_okvis.col('vel')),'vel')
# td_okvis.computeVelocitiesInBodyFrameFromPostionInWorldFrame('pos', 'vel', 'att',30,30)
if plotRon: # Plotting rotational rate norm
if doRovio:
plotterRon.addDataToSubplot(td_rovio, 'ron', 1, 'r', 'rovio rotational rate norm')
plotterRon.addDataToSubplot(td_vicon, 'ron', 1, 'b', 'vicon rotational rate norm')
