td_aligned.addLabelingIncremental("att", 4)
td_aligned.reInit()
RosDataAcquisition.rosBagLoadTimestampsOnly(timestampBag, timestampTopic, td_aligned)

# Interpolate Data
td.interpolateColumns(td_aligned, "pos", "pos")
td.interpolateQuaternion(td_aligned, "att", "att")

# Plotting
plotterPos = Plotter(-1, [3, 1], "Position", ["", "", "time[s]"], ["x[m]", "y[m]", "z[m]"], 10000)
plotterAtt = Plotter(-1, [4, 1], "Attitude", ["", "", "", "time[s]"], ["w", "x", "y", "z"], 10000)
plotterPos.addDataToSubplotMultiple(td, "pos", [1, 2, 3], ["b", "b", "b"], ["", "", ""])
plotterAtt.addDataToSubplotMultiple(td, "att", [1, 2, 3, 4], ["b", "b", "b", "b"], ["", "", "", ""])
plotterPos.addDataToSubplotMultiple(td_aligned, "pos", [1, 2, 3], ["g", "g", "g"], ["", "", ""])
plotterAtt.addDataToSubplotMultiple(td_aligned, "att", [1, 2, 3, 4], ["g", "g", "g", "g"], ["", "", "", ""])

# Apply body and inertial
if bodyTranslation != None and bodyRotation != None:
    td_aligned.applyBodyTransform("pos", "att", bodyTranslation, bodyRotation)
if inertialTranslation != None and inertialRotation != None:
    td_aligned.applyBodyTransform("pos", "att", inertialTranslation, inertialRotation)
plotterPos.addDataToSubplotMultiple(td_aligned, "pos", [1, 2, 3], ["r", "r", "r"], ["", "", ""])
plotterAtt.addDataToSubplotMultiple(td_aligned, "att", [1, 2, 3, 4], ["r", "r", "r", "r"], ["", "", "", ""])

if writeData:
    td_aligned.writeColsToSingleFiles(
        testFolder + "/poseOut", "pose", td_aligned.getColIDs("pos") + td_aligned.getColIDs("att"), " "
    )

raw_input("Press Enter to continue...")
Exemplo n.º 2
0
#     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)
Exemplo n.º 3
0
	RosDataAcquisition.rosBagLoadTransformStamped('example.bag','/rovio/transform',td2,posIDs2,attIDs2)
	
	# Add initial x to plot
	plotter1.addDataToSubplot(td1, posIDs1[0], 1, 'r', 'td1In x');
	plotter1.addDataToSubplot(td2, posIDs2[0], 1, 'b', 'td2In x');

	
	"""
		Apply body transform to the second data set. The column start IDs of position(9) and attitutde(1) have to be provided.
		We define the rotation through a rotation vector vCB, the exponential represents the corresponding rotation quaternion qCB.
		The translation is defined by the translation vector B_r_BC
	"""
	vCB = np.array([0.1,0.2,0.32])
	qCB = Quaternion.q_exp(vCB)
	B_r_BC = np.array([1.1,-0.2,0.4])
	td2.applyBodyTransform(posIDs2, attIDs2, B_r_BC, qCB)
	print('Applying Body Transform:')
	print('Rotation Vector ln(qCB):\tvx:' + str(vCB[0]) + '\tvy:' + str(vCB[1]) + '\tvz:' + str(vCB[2]))
	print('Translation Vector B_r_BC:\trx:' + str(B_r_BC[0]) + '\try:' + str(B_r_BC[1]) + '\trz:' + str(B_r_BC[2]))
	
	"""
		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]))
	
Exemplo n.º 4
0
    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')

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)
    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')
    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)
plotterPos = Plotter(-1, [3, 1], 'Position', ['', '', 'time[s]'],
                     ['x[m]', 'y[m]', 'z[m]'], 10000)
plotterAtt = Plotter(-1, [4, 1], 'Attitude', ['', '', '', 'time[s]'],
                     ['w', 'x', 'y', 'z'], 10000)
plotterPos.addDataToSubplotMultiple(td, 'pos', [1, 2, 3], ['b', 'b', 'b'],
                                    ['', '', ''])
plotterAtt.addDataToSubplotMultiple(td, 'att', [1, 2, 3, 4],
                                    ['b', 'b', 'b', 'b'], ['', '', '', ''])
plotterPos.addDataToSubplotMultiple(td_aligned, 'pos', [1, 2, 3],
                                    ['g', 'g', 'g'], ['', '', ''])
plotterAtt.addDataToSubplotMultiple(td_aligned, 'att', [1, 2, 3, 4],
                                    ['g', 'g', 'g', 'g'], ['', '', '', ''])

# Apply body and inertial
if bodyTranslation != None and bodyRotation != None:
    td_aligned.applyBodyTransform('pos', 'att', bodyTranslation, bodyRotation)
if inertialTranslation != None and inertialRotation != None:
    td_aligned.applyBodyTransform('pos', 'att', inertialTranslation,
                                  inertialRotation)
plotterPos.addDataToSubplotMultiple(td_aligned, 'pos', [1, 2, 3],
                                    ['r', 'r', 'r'], ['', '', ''])
plotterAtt.addDataToSubplotMultiple(td_aligned, 'att', [1, 2, 3, 4],
                                    ['r', 'r', 'r', 'r'], ['', '', '', ''])

if writeData:
    td_aligned.writeColsToSingleFiles(
        testFolder + '/poseOut', 'pose',
        td_aligned.getColIDs('pos') + td_aligned.getColIDs('att'), ' ')

raw_input("Press Enter to continue...")