0.025715529948,
-0.0257744366974,
0.00375618835797,
0.999660727178,
]
)
bodyTranslation = np.array([-0.0216401454975, -0.064676986768, 0.00981073058949])
bodyRotation = Quaternion.q_inverse(Quaternion.q_rotMatToQuat(R_BS))
inertialTranslation = None
inertialRotation = None
writeData = False
# Load Odometry Data
td.addLabelingIncremental("pos", 3)
td.addLabelingIncremental("att", 4)
td.reInit()
RosDataAcquisition.rosBagLoadOdometry(odometryBag, odometryTopic, td, "pos", "att")
# Load Timestamps into new td
td_aligned.addLabelingIncremental("pos", 3)
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)
td_rovio.addLabelingIncremental('posSp',3)
td_rovio.addLabelingIncremental('posSm',3)
td_rovio.addLabelingIncremental('ypr',3)
td_rovio.addLabelingIncremental('yprCov',9)
td_rovio.addLabelingIncremental('yprSp',3)
td_rovio.addLabelingIncremental('yprSm',3)
td_rovio.addLabelingIncremental('extPos',3)
td_rovio.addLabelingIncremental('extPosCov',9)
td_rovio.addLabelingIncremental('extPosSp',3)
td_rovio.addLabelingIncremental('extPosSm',3)
td_rovio.addLabelingIncremental('extAtt',4)
td_rovio.addLabelingIncremental('extAttCov',9)
td_rovio.addLabelingIncremental('feaPos',3,nFeatures)
td_rovio.addLabelingIncremental('feaCov',9,nFeatures)
td_rovio.addLabelingIncremental('feaIdx',1,nFeatures)
td_rovio.reInit()
if True: # Vicon Timed Data
td_vicon = TimedData()
td_vicon.addLabelingIncremental('pos',3)
td_vicon.addLabelingIncremental('att',4)
td_vicon.addLabelingIncremental('vel',3)
td_vicon.addLabelingIncremental('ror',3)
td_vicon.addLabelingIncremental('ron',1)
td_vicon.addLabelingIncremental('ypr',3)
td_vicon.reInit()
if doOkvis: # Okvis Timed Data
td_okvis = TimedData()
td_okvis.addLabelingIncremental('pos',3)
td_okvis.addLabelingIncremental('att',4)
td_rovio.addLabelingIncremental('posSp', 3)
td_rovio.addLabelingIncremental('posSm', 3)
td_rovio.addLabelingIncremental('ypr', 3)
td_rovio.addLabelingIncremental('yprCov', 9)
td_rovio.addLabelingIncremental('yprSp', 3)
td_rovio.addLabelingIncremental('yprSm', 3)
td_rovio.addLabelingIncremental('extPos', 3)
td_rovio.addLabelingIncremental('extPosCov', 9)
td_rovio.addLabelingIncremental('extPosSp', 3)
td_rovio.addLabelingIncremental('extPosSm', 3)
td_rovio.addLabelingIncremental('extAtt', 4)
td_rovio.addLabelingIncremental('extAttCov', 9)
td_rovio.addLabelingIncremental('feaPos', 3, nFeatures)
td_rovio.addLabelingIncremental('feaCov', 9, nFeatures)
td_rovio.addLabelingIncremental('feaIdx', 1, nFeatures)
td_rovio.reInit()
if True: # Vicon Timed Data
td_vicon = TimedData()
td_vicon.addLabelingIncremental('pos', 3)
td_vicon.addLabelingIncremental('att', 4)
td_vicon.addLabelingIncremental('vel', 3)
td_vicon.addLabelingIncremental('ror', 3)
td_vicon.addLabelingIncremental('ron', 1)
td_vicon.addLabelingIncremental('ypr', 3)
td_vicon.reInit()
if doOkvis: # Okvis Timed Data
td_okvis = TimedData()
td_okvis.addLabelingIncremental('pos', 3)
td_okvis.addLabelingIncremental('att', 4)
R_BS = np.array([
0.0148655429818, -0.999880929698, 0.00414029679422, 0.999557249008,
0.0149672133247, 0.025715529948, -0.0257744366974, 0.00375618835797,
0.999660727178
])
bodyTranslation = np.array(
[-0.0216401454975, -0.064676986768, 0.00981073058949])
bodyRotation = Quaternion.q_inverse(Quaternion.q_rotMatToQuat(R_BS))
inertialTranslation = None
inertialRotation = None
writeData = False
# Load Odometry Data
td.addLabelingIncremental('pos', 3)
td.addLabelingIncremental('att', 4)
td.reInit()
RosDataAcquisition.rosBagLoadOdometry(odometryBag, odometryTopic, td, 'pos',
'att')
# Load Timestamps into new td
td_aligned.addLabelingIncremental('pos', 3)
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
