def acquireDataGT(self):
if(self.gtFile.endswith('.csv')):
CsvDataAcquisition.csvLoadTransform(self.gtFile, 0, 1, 4, self.tdgt, 'pos', 'att')
elif(self.gtFile.endswith('.bag')):
RosDataAcquisition.rosBagLoadTransformStamped(self.gtFile,self.gtTopic,self.tdgt,'pos','att')
if(self.gtFile.endswith('data.csv')):
self.tdgt.d[:,0] = self.tdgt.d[:,0]*1e-9
self.tdgt.cropTimes(self.tdgt.getFirstTime()+self.startcut,self.tdgt.getLastTime()-self.endcut)
self.tdgt.applyTimeOffset(-self.tdgt.getFirstTime())
def acquireDataGT(self):
if self.gtFile.endswith(".csv"):
CsvDataAcquisition.csvLoadTransform(self.gtFile, 0, 1, 4, self.tdgt, "pos", "att")
elif self.gtFile.endswith(".bag"):
RosDataAcquisition.rosBagLoadTransformStamped(self.gtFile, self.gtTopic, self.tdgt, "pos", "att")
if self.gtFile.endswith("data.csv"):
self.tdgt.d[:, 0] = self.tdgt.d[:, 0] * 1e-9
self.tdgt.cropTimes(self.tdgt.getFirstTime() + self.startcut, self.tdgt.getLastTime() - self.endcut)
self.tdgt.applyTimeOffset(-self.tdgt.getFirstTime())
def acquireDataGT(self,
csvTimeCol=0,
csvPosCol=range(1, 4),
csvAttCol=range(4, 8),
timescale=1.):
if (self.gtFile.endswith('.csv')):
CsvDataAcquisition.csvLoadTransform(self.gtFile, csvTimeCol,
csvPosCol, csvAttCol,
self.tdgt, 'pos', 'att')
self.tdgt.d[:, csvTimeCol] = self.tdgt.d[:, csvTimeCol] * timescale
elif (self.gtFile.endswith('.bag')):
RosDataAcquisition.rosBagLoadTransformStamped(
self.gtFile, self.gtTopic, self.tdgt, 'pos', 'att')
if (self.gtFile.endswith('data.csv')):
self.tdgt.d[:, 0] = self.tdgt.d[:, 0] * timescale
self.tdgt.cropTimes(self.tdgt.getFirstTime() + self.startcut,
self.tdgt.getLastTime() - self.endcut)
self.tdgt.applyTimeOffset(-self.tdgt.getFirstTime())
def acquireData(self):
if self.bag.endswith(".bag"):
if self.doCov:
RosDataAcquisition.rosBagLoadOdometry(
self.bag,
self.odomTopic,
self.td,
"pos",
"att",
"vel",
"ror",
"posCov",
"attCov",
"velCov",
"rorCov",
)
else:
RosDataAcquisition.rosBagLoadOdometry(self.bag, self.odomTopic, self.td, "pos", "att", "vel", "ror")
if self.pclTopic != "" and self.doNFeatures > 0:
RosDataAcquisition.rosBagLoadRobocentricPointCloud(
self.bag, self.pclTopic, self.td, "feaIdx", "feaPos", "feaCov", "feaDis", "feaDisCov"
)
if self.extrinsicsTopic != "" and self.doExtrinsics:
if self.doCov:
RosDataAcquisition.rosBagLoadPoseWithCovariance(
self.bag, self.extrinsicsTopic, self.td, "extPos", "extAtt", "extPosCov", "extAttCov"
)
else:
RosDataAcquisition.rosBagLoadPoseWithCovariance(
self.bag, self.extrinsicsTopic, self.td, "extPos", "extAtt"
)
if self.biasesTopic != "" and self.doBiases:
if self.doCov:
RosDataAcquisition.rosBagLoadImuWithCovariance(
self.bag, self.biasesTopic, self.td, "gyb", "acb", "gybCov", "acbCov"
)
else:
RosDataAcquisition.rosBagLoadImuWithCovariance(self.bag, self.biasesTopic, self.td, "gyb", "acb")
if self.bag.endswith("data.csv"):
CsvDataAcquisition.csvLoadTransform(self.bag, 0, 1, 4, self.td, "pos", "att")
self.td.d[:, 0] = self.td.d[:, 0] * 1e-9
attID = self.td.getColIDs("att")
q_real = self.td.col(attID[3])
q_im = -self.td.col(attID[0:3])
self.td.setCol(q_real, attID[0])
self.td.setCol(q_im, attID[1:4])
self.td.applyTimeOffset(-self.td.getFirstTime())
-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)
plotterPos.addDataToSubplotMultiple(td, "pos", [1, 2, 3], ["b", "b", "b"], ["", "", ""])
def acquireData(self,
csvTimeCol=0,
csvPosCol=range(1, 4),
csvAttCol=range(4, 8),
csvVelCol=None,
csvRorCol=None,
timescale=1.,
delimiter=',',
start=0):
if (self.bag.endswith('.bag')):
if self.doCov:
RosDataAcquisition.rosBagLoadOdometry(self.bag,
self.odomTopic,
self.td,
'pos',
'att',
'vel',
'ror',
'posCov',
'attCov',
'velCov',
'rorCov',
start=start)
else:
RosDataAcquisition.rosBagLoadOdometry(self.bag,
self.odomTopic,
self.td,
'pos',
'att',
'vel',
'ror',
start=start)
if self.pclTopic != '' and self.doNFeatures > 0:
RosDataAcquisition.rosBagLoadRobocentricPointCloud(
self.bag, self.pclTopic, self.td, 'feaIdx', 'feaPos',
'feaCov', 'feaDis', 'feaDisCov')
if self.extrinsicsTopic != '' and self.doExtrinsics:
if self.doCov:
RosDataAcquisition.rosBagLoadPoseWithCovariance(
self.bag, self.extrinsicsTopic, self.td, 'extPos',
'extAtt', 'extPosCov', 'extAttCov')
else:
RosDataAcquisition.rosBagLoadPoseWithCovariance(
self.bag, self.extrinsicsTopic, self.td, 'extPos',
'extAtt')
if self.biasesTopic != '' and self.doBiases:
if self.doCov:
RosDataAcquisition.rosBagLoadImuWithCovariance(
self.bag, self.biasesTopic, self.td, 'gyb', 'acb',
'gybCov', 'acbCov')
else:
RosDataAcquisition.rosBagLoadImuWithCovariance(
self.bag, self.biasesTopic, self.td, 'gyb', 'acb')
if (self.bag.endswith('.csv')):
CsvDataAcquisition.csvLoadTransform(self.bag,
csvTimeCol,
csvPosCol,
csvAttCol,
self.td,
'pos',
'att',
csvVelCol,
csvRorCol,
'vel',
'ror',
delimiter=delimiter,
start=start)
self.td.d[:, csvTimeCol] = self.td.d[:, csvTimeCol] * timescale
# attID = self.td.getColIDs('att')
# q_real = self.td.col(attID[3])
# q_im = -self.td.col(attID[0:3])
# self.td.setCol(q_real,attID[0])
# self.td.setCol(q_im,attID[1:4])
# self.td.d = self.td.d[start:, :]
# self.td.last = self.td.d.shape[0] - 1
self.td.applyTimeOffset(-self.td.getFirstTime())
td_okvis = TimedData()
td_okvis.addLabelingIncremental('pos', 3)
td_okvis.addLabelingIncremental('att', 4)
td_okvis.addLabelingIncremental('vel', 3)
td_okvis.addLabelingIncremental('ror', 3)
td_okvis.addLabelingIncremental('ron', 1)
td_okvis.addLabelingIncremental('ypr', 3)
td_okvis.reInit()
if True: # Vicon data acquisition and pre-processing
if (viconGroundtruthFile.endswith('.csv')):
CsvDataAcquisition.csvLoadTransform(viconGroundtruthFile, 0, 1, 4,
td_vicon, 'pos', 'att')
elif (viconGroundtruthFile.endswith('.bag')):
RosDataAcquisition.rosBagLoadTransformStamped(viconGroundtruthFile,
viconGroundtruthTopic,
td_vicon, 'pos', 'att')
if (viconGroundtruthFile.endswith('data.csv')):
td_vicon.d[:, 0] = td_vicon.d[:, 0] * 1e-9
td_vicon.cropTimes(td_vicon.getFirstTime() + startcut,
td_vicon.getLastTime() - endcut)
td_vicon.applyTimeOffset(-td_vicon.getFirstTime())
td_vicon.computeRotationalRateFromAttitude('att', 'ror', 3, 3)
td_vicon.computeNormOfColumns('ror', 'ron')
td_vicon.computeVelocitiesInBodyFrameFromPostionInWorldFrame(
'pos', 'vel', 'att', 3, 3)
if doRovio: # Rovio data acquisition and pre-processing
RosDataAcquisition.rosBagLoadOdometry(rovioOutputBag, rovioOutputTopic,
td_rovio, 'pos', 'att', 'vel', 'ror',
'posCov', 'attCov')
attIDs2 = [1,2,3,4]
velIDs1 = [8,9,10]
velIDs2 = [12,13,14]
velBIDs1 = [11,12,13]
velBIDs2 = [15,16,17]
rorIDs1 = [14,15,16]
rorIDs2 = [5,6,7]
rorNID1 = 17
rorNID2 = 8
"""
First we will fill the two TimedData structures with the same StampedTransforms,
loaded from topic 'rovio/transform' in example.bag.
The indices denote the start column of the position(td1=1, td2=9) and attitude(td1=4, td2=1)
"""
RosDataAcquisition.rosBagLoadTransformStamped('example.bag','/rovio/transform',td1,posIDs1,attIDs1)
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])
vicon_yprID = [15,16,17]
if doOkvis: # Okvis Timed Data
td_okvis = TimedData(18)
okvis_posID = [1,2,3]
okvis_attID = [4,5,6,7]
okvis_velID = [8,9,10]
okvis_rorID = [11,12,13]
okvis_ronID = 14
okvis_yprID = [15,16,17]
if True: # Vicon data acquisition and pre-processing
if(viconGroundtruthFile.endswith('.csv')):
CsvDataAcquisition.csvLoadTransform(viconGroundtruthFile, 0, 1, 4, td_vicon, vicon_posID, vicon_attID)
elif(viconGroundtruthFile.endswith('.bag')):
RosDataAcquisition.rosBagLoadTransformStamped(viconGroundtruthFile,viconGroundtruthTopic,td_vicon,vicon_posID,vicon_attID)
if(viconGroundtruthFile.endswith('data.csv')):
td_vicon.d[:,0] = td_vicon.d[:,0]*1e-9
td_vicon.cropTimes(td_vicon.getFirstTime()+startcut,td_vicon.getLastTime()-endcut)
td_vicon.applyTimeOffset(-td_vicon.getFirstTime())
td_vicon.computeRotationalRateFromAttitude(vicon_attID,vicon_rorID)
td_vicon.computeNormOfColumns(vicon_rorID,vicon_ronID)
td_vicon.computeVelocitiesInBodyFrameFromPostionInWorldFrame(vicon_posID, vicon_velID, vicon_attID)
if doRovio: # Rovio data acquisition and pre-processing
RosDataAcquisition.rosBagLoadOdometry(rovioOutputBag, rovioOutputTopic ,td_rovio,rovio_posID,rovio_attID,rovio_velID,rovio_rorID,rovio_posCovID,rovio_attCovID)
RosDataAcquisition.rosBagLoadRobocentricPointCloud(rovioOutputBag,'/rovio/pcl',td_rovio,rovio_fea_idxID,rovio_fea_posID)
td_rovio.computeNormOfColumns(rovio_rorID,rovio_ronID)
td_rovio.applyTimeOffset(td_vicon.getFirstTime()-td_rovio.getFirstTime())
to = td_rovio.getTimeOffset(rovio_ronID,td_vicon,vicon_ronID)
td_rovio.applyTimeOffset(-to)
if doOkvis: # Okvis Timed Data
td_okvis = TimedData()
td_okvis.addLabelingIncremental('pos',3)
td_okvis.addLabelingIncremental('att',4)
td_okvis.addLabelingIncremental('vel',3)
td_okvis.addLabelingIncremental('ror',3)
td_okvis.addLabelingIncremental('ron',1)
td_okvis.addLabelingIncremental('ypr',3)
td_okvis.reInit()
if True: # Vicon data acquisition and pre-processing
if(viconGroundtruthFile.endswith('.csv')):
CsvDataAcquisition.csvLoadTransform(viconGroundtruthFile, 0, 1, 4, td_vicon, 'pos', 'att')
elif(viconGroundtruthFile.endswith('.bag')):
RosDataAcquisition.rosBagLoadTransformStamped(viconGroundtruthFile,viconGroundtruthTopic,td_vicon,'pos','att')
if(viconGroundtruthFile.endswith('data.csv')):
td_vicon.d[:,0] = td_vicon.d[:,0]*1e-9
td_vicon.cropTimes(td_vicon.getFirstTime()+startcut,td_vicon.getLastTime()-endcut)
td_vicon.applyTimeOffset(-td_vicon.getFirstTime())
td_vicon.computeRotationalRateFromAttitude('att','ror',3,3)
td_vicon.computeNormOfColumns('ror','ron')
td_vicon.computeVelocitiesInBodyFrameFromPostionInWorldFrame('pos', 'vel', 'att',3,3)
if doRovio: # Rovio data acquisition and pre-processing
RosDataAcquisition.rosBagLoadOdometry(rovioOutputBag, rovioOutputTopic ,td_rovio,'pos','att','vel','ror','posCov','attCov')
if rovioPclTopic != '':
RosDataAcquisition.rosBagLoadRobocentricPointCloud(rovioOutputBag,rovioPclTopic,td_rovio,'feaIdx','feaPos','feaCov')
if rovioExtrinsicTopic != '':
RosDataAcquisition.rosBagLoadPoseWithCovariance(rovioOutputBag, rovioExtrinsicTopic ,td_rovio,'extPos','extAtt','extPosCov','extAttCov')
td_rovio.computeNormOfColumns('ror','ron')
def acquireData(self):
if(self.bag.endswith('.bag')):
if self.doCov:
RosDataAcquisition.rosBagLoadOdometry(self.bag, self.odomTopic ,self.td,'pos','att','vel','ror','posCov','attCov','velCov','rorCov')
else:
RosDataAcquisition.rosBagLoadOdometry(self.bag, self.odomTopic ,self.td,'pos','att','vel','ror')
if self.pclTopic != '' and self.doNFeatures > 0:
RosDataAcquisition.rosBagLoadRobocentricPointCloud(self.bag,self.pclTopic,self.td,'feaIdx','feaPos','feaCov','feaDis','feaDisCov')
if self.extrinsicsTopic != '' and self.doExtrinsics:
if self.doCov:
RosDataAcquisition.rosBagLoadPoseWithCovariance(self.bag, self.extrinsicsTopic ,self.td,'extPos','extAtt','extPosCov','extAttCov')
else:
RosDataAcquisition.rosBagLoadPoseWithCovariance(self.bag, self.extrinsicsTopic ,self.td,'extPos','extAtt')
if self.biasesTopic != '' and self.doBiases:
if self.doCov:
RosDataAcquisition.rosBagLoadImuWithCovariance(self.bag, self.biasesTopic ,self.td,'gyb','acb','gybCov','acbCov')
else:
RosDataAcquisition.rosBagLoadImuWithCovariance(self.bag, self.biasesTopic ,self.td,'gyb','acb')
if(self.bag.endswith('data.csv')):
CsvDataAcquisition.csvLoadTransform(self.bag, 0, 1, 4, self.td, 'pos', 'att')
self.td.d[:,0] = self.td.d[:,0]*1e-9
attID = self.td.getColIDs('att')
q_real = self.td.col(attID[3])
q_im = -self.td.col(attID[0:3])
self.td.setCol(q_real,attID[0])
self.td.setCol(q_im,attID[1:4])
self.td.applyTimeOffset(-self.td.getFirstTime())
def acquireData(self):
if(self.bag.endswith('.bag')):
if self.doCov:
RosDataAcquisition.rosBagLoadOdometry(self.bag, self.odomTopic ,self.td,'pos','att','vel','ror','posCov','attCov','velCov','rorCov')
else:
RosDataAcquisition.rosBagLoadOdometry(self.bag, self.odomTopic ,self.td,'pos','att','vel','ror')
if self.pclTopic != '' and self.doNFeatures > 0:
RosDataAcquisition.rosBagLoadRobocentricPointCloud(self.bag,self.pclTopic,self.td,'feaIdx','feaPos','feaCov','feaDis','feaDisCov')
if self.extrinsicsTopic != '' and self.doExtrinsics:
if self.doCov:
RosDataAcquisition.rosBagLoadPoseWithCovariance(self.bag, self.extrinsicsTopic ,self.td,'extPos','extAtt','extPosCov','extAttCov')
else:
RosDataAcquisition.rosBagLoadPoseWithCovariance(self.bag, self.extrinsicsTopic ,self.td,'extPos','extAtt')
if self.biasesTopic != '' and self.doBiases:
if self.doCov:
RosDataAcquisition.rosBagLoadImuWithCovariance(self.bag, self.biasesTopic ,self.td,'gyb','acb','gybCov','acbCov')
else:
RosDataAcquisition.rosBagLoadImuWithCovariance(self.bag, self.biasesTopic ,self.td,'gyb','acb')
if(self.bag.endswith('data.csv')):
CsvDataAcquisition.csvLoadTransform(self.bag, 0, 1, 4, self.td, 'pos', 'att')
self.td.d[:,0] = self.td.d[:,0]*1e-9
attID = self.td.getColIDs('att')
q_real = self.td.col(attID[3])
q_im = -self.td.col(attID[0:3])
self.td.setCol(q_real,attID[0])
self.td.setCol(q_im,attID[1:4])
self.td.applyTimeOffset(-self.td.getFirstTime())
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
plotterPos = Plotter(-1, [3, 1], 'Position', ['', '', 'time[s]'],
['x[m]', 'y[m]', 'z[m]'], 10000)