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')
if doOkvis:
plotterRon.addDataToSubplot(td_okvis, 'ron', 1, 'g',
'okvis rotational rate norm')
if True: # Transform body coordinate frame for better vizualization, TODO: add to config
if doRovio:
td_rovio.applyBodyTransform('pos', 'att',
bodyTransformForBetterPlotRangePos,
bodyTransformForBetterPlotRangeAtt)
td_rovio.applyBodyTransformToAttCov(
'attCov', bodyTransformForBetterPlotRangeAtt)
td_rovio.applyBodyTransformToTwist('vel', 'ror',
bodyTransformForBetterPlotRangePos,
bodyTransformForBetterPlotRangeAtt)
td_rovio.applyInertialTransform('pos', 'att', np.zeros(3),
np.array([1.0, 0, 0, 0]))
if doOkvis:
td_okvis.applyBodyTransform('pos', 'att',
bodyTransformForBetterPlotRangePos,
bodyTransformForBetterPlotRangeAtt)
td_okvis.applyBodyTransformToTwist('vel', 'ror',
bodyTransformForBetterPlotRangePos,
bodyTransformForBetterPlotRangeAtt)
td_okvis.applyInertialTransform('pos', 'att', np.zeros(3),
np.array([1.0, 0, 0, 0]))
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')
if True: # Transform body coordinate frame for better vizualization, TODO: add to config
if doRovio:
td_rovio.applyBodyTransform(rovio_posID, rovio_attID, bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
td_rovio.applyBodyTransformToAttCov(rovio_attCovID, bodyTransformForBetterPlotRangeAtt)
td_rovio.applyBodyTransformToTwist(rovio_velID, rovio_rorID, bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
td_rovio.applyInertialTransform(rovio_posID, rovio_attID, np.zeros(3), np.array([0,0,0,1]))
if doOkvis:
td_okvis.applyBodyTransform(okvis_posID, okvis_attID, bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
td_okvis.applyBodyTransformToTwist(okvis_velID, okvis_rorID, bodyTransformForBetterPlotRangePos, bodyTransformForBetterPlotRangeAtt)
td_okvis.applyInertialTransform(okvis_posID, okvis_attID, np.zeros(3), np.array([0,0,0,1]))
if True: # Align Vicon to Rovio using calibration (body frame)
B_r_BC_est = -Quaternion.q_rotate(qVM, MrMV) + Quaternion.q_rotate(Quaternion.q_inverse(qVM),bodyTransformForBetterPlotRangePos)
qCB_est = Quaternion.q_mult(bodyTransformForBetterPlotRangeAtt,Quaternion.q_inverse(qVM))
td_vicon.applyBodyTransform(vicon_posID, vicon_attID, B_r_BC_est, qCB_est)
td_vicon.applyBodyTransformToTwist(vicon_velID, vicon_rorID, B_r_BC_est, qCB_est)
if doRovio and bodyAlignViconToRovio: # Align Vicon to Rovio (body frame)
B_r_BC_est, qCB_est = td_vicon.calibrateBodyTransform(vicon_velID, vicon_rorID, td_rovio, rovio_velID,rovio_rorID)
