"""
Apply inertial transform to the second data set. Same procedure as with the body transform.
"""
vIJ = np.array([0.2,-0.2,-0.4])
qIJ = Quaternion.q_exp(vIJ)
J_r_JI = np.array([-0.1,0.5,0.1])
td2.applyInertialTransform(posIDs2, attIDs2,J_r_JI,qIJ)
print('Applying Inertial Transform:')
print('Rotation Vector ln(qIJ):\tvx:' + str(vIJ[0]) + '\tvy:' + str(vIJ[1]) + '\tvz:' + str(vIJ[2]))
print('Translation Vector J_r_JI:\trx:' + str(J_r_JI[0]) + '\try:' + str(J_r_JI[1]) + '\trz:' + str(J_r_JI[2]))
"""
Apply time delay to the second data set.
"""
timeOffset = 0.2;
td2.applyTimeOffset(timeOffset)
print('Applying Time Offset:')
print('Time Offset: ' + str(timeOffset) + 's')
# Add transformed x to plot
plotter1.addDataToSubplot(td1, posIDs1[0], 2, 'r', 'td1In x');
plotter1.addDataToSubplot(td2, posIDs2[0], 2, 'b', 'td2Trans x');
"""
Now we are ready to calculate the other TimedData properties.
"""
# Calculate the velocity in the world frame provide pos(td1=[1,2,3], td2=[9,10,11]) and vel(td1=[8,9,10], td2=[12,13,14]) column IDs.
td1.computeVectorNDerivative(posIDs1, velIDs1)
td2.computeVectorNDerivative(posIDs2, velIDs2)
# Calculate the velocity in the body frame provide pos(td1=[1,2,3], td2=[9,10,11]) and velInBodyFrame(td1=[11,12,13], td2=[15,16,17]) column IDs.
# Additionally the rotation Quaternion qBI rps qCJ has to be provided.
td_okvis.addLabelingIncremental('ypr', 3)
td_okvis.reInit()
if True: # Vicon data acquisition and pre-processing
if (viconGroundtruthFile.endswith('.csv')):
CsvDataAcquisition.csvLoadTransform(viconGroundtruthFile, 0, 1, 4,
td_vicon, 'pos', 'att')
elif (viconGroundtruthFile.endswith('.bag')):
RosDataAcquisition.rosBagLoadTransformStamped(viconGroundtruthFile,
viconGroundtruthTopic,
td_vicon, 'pos', 'att')
if (viconGroundtruthFile.endswith('data.csv')):
td_vicon.d[:, 0] = td_vicon.d[:, 0] * 1e-9
td_vicon.cropTimes(td_vicon.getFirstTime() + startcut,
td_vicon.getLastTime() - endcut)
td_vicon.applyTimeOffset(-td_vicon.getFirstTime())
td_vicon.computeRotationalRateFromAttitude('att', 'ror', 3, 3)
td_vicon.computeNormOfColumns('ror', 'ron')
td_vicon.computeVelocitiesInBodyFrameFromPostionInWorldFrame(
'pos', 'vel', 'att', 3, 3)
if doRovio: # Rovio data acquisition and pre-processing
RosDataAcquisition.rosBagLoadOdometry(rovioOutputBag, rovioOutputTopic,
td_rovio, 'pos', 'att', 'vel', 'ror',
'posCov', 'attCov')
if rovioPclTopic != '':
RosDataAcquisition.rosBagLoadRobocentricPointCloud(
rovioOutputBag, rovioPclTopic, td_rovio, 'feaIdx', 'feaPos',
'feaCov')
if rovioExtrinsicTopic != '':
RosDataAcquisition.rosBagLoadPoseWithCovariance(
okvis_posID = [1,2,3]
okvis_attID = [4,5,6,7]
okvis_velID = [8,9,10]
okvis_rorID = [11,12,13]
okvis_ronID = 14
okvis_yprID = [15,16,17]
if True: # Vicon data acquisition and pre-processing
if(viconGroundtruthFile.endswith('.csv')):
CsvDataAcquisition.csvLoadTransform(viconGroundtruthFile, 0, 1, 4, td_vicon, vicon_posID, vicon_attID)
elif(viconGroundtruthFile.endswith('.bag')):
RosDataAcquisition.rosBagLoadTransformStamped(viconGroundtruthFile,viconGroundtruthTopic,td_vicon,vicon_posID,vicon_attID)
if(viconGroundtruthFile.endswith('data.csv')):
td_vicon.d[:,0] = td_vicon.d[:,0]*1e-9
td_vicon.cropTimes(td_vicon.getFirstTime()+startcut,td_vicon.getLastTime()-endcut)
td_vicon.applyTimeOffset(-td_vicon.getFirstTime())
td_vicon.computeRotationalRateFromAttitude(vicon_attID,vicon_rorID)
td_vicon.computeNormOfColumns(vicon_rorID,vicon_ronID)
td_vicon.computeVelocitiesInBodyFrameFromPostionInWorldFrame(vicon_posID, vicon_velID, vicon_attID)
if doRovio: # Rovio data acquisition and pre-processing
RosDataAcquisition.rosBagLoadOdometry(rovioOutputBag, rovioOutputTopic ,td_rovio,rovio_posID,rovio_attID,rovio_velID,rovio_rorID,rovio_posCovID,rovio_attCovID)
RosDataAcquisition.rosBagLoadRobocentricPointCloud(rovioOutputBag,'/rovio/pcl',td_rovio,rovio_fea_idxID,rovio_fea_posID)
td_rovio.computeNormOfColumns(rovio_rorID,rovio_ronID)
td_rovio.applyTimeOffset(td_vicon.getFirstTime()-td_rovio.getFirstTime())
to = td_rovio.getTimeOffset(rovio_ronID,td_vicon,vicon_ronID)
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')):
