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) print('Calibrate Body Transform for Rovio:')
'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])) if True: # Align Vicon to Rovio using calibration (body frame) B_r_BC_est = -Quaternion.q_rotate(qVM, MrMV) + Quaternion.q_rotate(
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') 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])) 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('pos', 'att', B_r_BC_est, qCB_est) td_vicon.applyBodyTransformToTwist('vel', 'ror', 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('vel', 'ror', td_rovio, 'vel','ror') print('Calibrate Body Transform for Rovio:')