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...")
td_rovio.computeSigmaBounds('extPos','extPosCov','extPosSp','extPosSm',3)
plotterExt.addDataToSubplotMultiple(td_rovio, 'extPosSm', [1,2,3], ['r--','r--','r--'], ['','',''])
plotterExt.addDataToSubplotMultiple(td_rovio, 'extPosSp', [1,2,3], ['r--','r--','r--'], ['','',''])
plotterExt.addDataToSubplotMultiple(td_rovio, 'extPos', [1,2,3], ['r','r','r'], ['','',''])
if plotFea: # Plotting features height, TODO: zoom in & plot vs traveled distance
if doRovio:
plotFeaTimeEnd = 3.0
startTime = 25.0
startAverage = 1.0
frequency = 20.0
figure()
x = [[]]
y = [[]]
cov = [[]]
feaIdxID = td_rovio.getColIDs('feaIdx')
feaPosID = td_rovio.getColIDs('feaPos')
feaCovID = td_rovio.getColIDs('feaCov')
for j in np.arange(nFeatures):
newFea = td_rovio.col('pos') + Quaternion.q_rotate(Quaternion.q_inverse(td_rovio.col('att')),td_rovio.col(feaPosID[j]))
td_rovio.applyRotationToCov(feaCovID[j], 'att', True)
for i in np.arange(0,3):
td_rovio.setCol(newFea[:,i],feaPosID[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] > startTime):
if(td_rovio.d[i,feaIdxID[j]] < 0.0 or td_rovio.d[i,feaIdxID[j]] != lastID):
if len(x[-1]) > 0:
['r--', 'r--', 'r--'],
['', '', ''])
plotterExt.addDataToSubplotMultiple(td_rovio, 'extPos', [1, 2, 3],
['r', 'r', 'r'], ['', '', ''])
if plotFea: # Plotting features height, TODO: zoom in & plot vs traveled distance
if doRovio:
plotFeaTimeEnd = 3.0
startTime = 25.0
startAverage = 1.0
frequency = 20.0
figure()
x = [[]]
y = [[]]
cov = [[]]
feaIdxID = td_rovio.getColIDs('feaIdx')
feaPosID = td_rovio.getColIDs('feaPos')
feaCovID = td_rovio.getColIDs('feaCov')
for j in np.arange(nFeatures):
newFea = td_rovio.col('pos') + Quaternion.q_rotate(
Quaternion.q_inverse(td_rovio.col('att')),
td_rovio.col(feaPosID[j]))
td_rovio.applyRotationToCov(feaCovID[j], 'att', True)
for i in np.arange(0, 3):
td_rovio.setCol(newFea[:, i], feaPosID[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] > startTime):
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...")
